Fix and cleanup gt_{from,to}hex

src/common.c

@@ -41,17 +41,20 @@ void gt_na (const char *name)
 
 int gt_tohex (char *dst, size_t dst_size, const uint8_t *src, size_t src_size)
 {
-    if _0_(dst_size<2*src_size+1)
+    if _0_(!dst_size)
         return -1;
 
-    const char tbl[] = "0123456789ABCDEF";
+    if _0_(((dst_size-1)/2)<src_size)
+        return -1;
+
+    static const char tbl[] = "0123456789ABCDEF";
 
     for (size_t i=0; i<src_size; i++) {
-        dst[(i<<1)+0] = tbl[0xF&(src[i]>>4)];
-        dst[(i<<1)+1] = tbl[0xF&(src[i])];
+        *dst++ = tbl[0xF&(src[i]>>4)];
+        *dst++ = tbl[0xF&(src[i])];
     }
 
-    dst[2*src_size] = 0;
+    *dst = 0;
 
     return 0;
 }
@@ -63,7 +66,7 @@ static inline int fromhex (const char c)
         return c-'0';
 
     if (c>='A' && c<='F')
-        return c-'a'+10;
+        return c-'A'+10;
 
     return -1;
 }
@@ -73,7 +76,7 @@ int gt_fromhex (uint8_t *dst, size_t dst_size, const char *src, size_t src_size)
     if _0_(src_size&1)
         return -1;
 
-    if _0_(src_size>2*dst_size)
+    if _0_(dst_size<(src_size/2))
         return -1;
 
     for (size_t i=0; i<src_size; i+=2) {
@@ -83,7 +86,7 @@ int gt_fromhex (uint8_t *dst, size_t dst_size, const char *src, size_t src_size)
         if _0_(a==-1 || b==-1)
             return -1;
 
-        dst[i>>1] = (a<<4)|b;
+        *dst++ = (a<<4)|b;
     }
 
     return 0;

src/main.c

@@ -5,9 +5,15 @@
 #include <poll.h>
 #include <sys/time.h>
 #include <sys/fcntl.h>
-#include <sys/socket.h>
+
+#ifndef __FAVOR_BSD
+#define __FAVOR_BSD 1
+#endif
+
 #include <netinet/in.h>
 #include <netinet/tcp.h>
+#include <netinet/udp.h>
+
 #include <arpa/inet.h>
 #include <netdb.h>
 
@@ -362,15 +368,10 @@ static ssize_t fd_write (int fd, const void *data, size_t size)
     return ret;
 }
 
-static ssize_t fd_read_all (int fd, void *data, size_t size)
+static size_t fd_read_all (int fd, void *data, size_t size)
 {
     size_t done = 0;
 
-    struct pollfd pollfd = {
-        .fd = fd,
-        .events = POLLIN,
-    };
-
     while (done<size) {
         ssize_t ret = fd_read(fd, (uint8_t *)data+done, size-done);
 
@@ -378,8 +379,14 @@ static ssize_t fd_read_all (int fd, void *data, size_t size)
             break;
 
         if (ret<0) {
+            struct pollfd pollfd = {
+                .fd = fd,
+                .events = POLLIN,
+            };
+
             if (!poll(&pollfd, 1, GT_TIMEOUT))
                 break;
+
             continue;
         }
 
@@ -389,15 +396,10 @@ static ssize_t fd_read_all (int fd, void *data, size_t size)
     return done;
 }
 
-static ssize_t fd_write_all (int fd, const void *data, size_t size)
+static size_t fd_write_all (int fd, const void *data, size_t size)
 {
     size_t done = 0;
 
-    struct pollfd pollfd = {
-        .fd = fd,
-        .events = POLLOUT,
-    };
-
     while (done<size) {
         ssize_t ret = fd_write(fd, (const uint8_t *)data+done, size-done);
 
@@ -405,8 +407,14 @@ static ssize_t fd_write_all (int fd, const void *data, size_t size)
             break;
 
         if (ret<0) {
+            struct pollfd pollfd = {
+                .fd = fd,
+                .events = POLLOUT,
+            };
+
             if (!poll(&pollfd, 1, GT_TIMEOUT))
                 break;
+
             continue;
         }
 
@@ -505,23 +513,42 @@ static void gt_print_hdr (uint8_t *data, size_t ip_size, const char *sockname)
 
     gt_log("%s: version=%i size=%zi proto=%zi src=%s dst=%s\n", sockname, ip_version, ip_size, ip_proto, ip_src, ip_dst);
 
-    if (ip_hdr_size<=0 || ip_proto!=6)
+    if (ip_hdr_size<=0)
         return;
 
-    struct tcphdr tcp;
+    if (ip_proto==6) {
+        struct tcphdr tcp;
 
-    byte_cpy(&tcp, &data[ip_hdr_size], sizeof(tcp));
+        byte_cpy(&tcp, &data[ip_hdr_size], sizeof(tcp));
 
-    tcp.source = ntohs(tcp.source);
-    tcp.dest = ntohs(tcp.dest);
-    tcp.seq = ntohl(tcp.seq);
-    tcp.ack_seq = ntohl(tcp.ack_seq);
-    tcp.window = ntohs(tcp.window);
+        tcp.th_sport = ntohs(tcp.th_sport);
+        tcp.th_dport = ntohs(tcp.th_dport);
+        tcp.th_seq = ntohl(tcp.th_seq);
+        tcp.th_ack = ntohl(tcp.th_ack);
+        tcp.th_win = ntohs(tcp.th_win);
 
-    gt_log("%s: tcp src=%i dst=%i seq=%u ack=%u win=%u %s%s%s%s%s%s\n",
-            sockname, tcp.source, tcp.dest, tcp.seq, tcp.ack_seq, tcp.window,
-            tcp.fin?"FIN ":"", tcp.syn?"SYN ":"", tcp.rst?"RST ":"",
-            tcp.psh?"PSH ":"", tcp.ack?"ACK ":"", tcp.urg?"URG ":"");
+        gt_log("%s: tcp src=%u dst=%u seq=%u ack=%u win=%u %c%c%c%c%c%c\n",
+                sockname, tcp.th_sport, tcp.th_dport, tcp.th_seq, tcp.th_ack, tcp.th_win,
+                (tcp.th_flags&TH_FIN) ?'F':'.',
+                (tcp.th_flags&TH_SYN) ?'S':'.',
+                (tcp.th_flags&TH_RST) ?'R':'.',
+                (tcp.th_flags&TH_PUSH)?'P':'.',
+                (tcp.th_flags&TH_ACK) ?'A':'.',
+                (tcp.th_flags&TH_URG) ?'U':'.');
+    }
+
+    if (ip_proto==17) {
+        struct udphdr udp;
+
+        byte_cpy(&udp, &data[ip_hdr_size], sizeof(udp));
+
+        udp.uh_sport = ntohs(udp.uh_sport);
+        udp.uh_dport = ntohs(udp.uh_dport);
+        udp.uh_ulen = ntohs(udp.uh_ulen);
+
+        gt_log("%s: udp src=%u dst=%u len=%u\n",
+                sockname, udp.uh_sport, udp.uh_dport, udp.uh_ulen);
+    }
 }
 
 static int gt_setup_secretkey (struct crypto_ctx *ctx, char *keyfile)
@@ -931,7 +958,6 @@ int main (int argc, char **argv)
                     if _0_(debug)
                         gt_print_hdr(data, ip_size, sockname);
 
-
                     blks[blk_write++].size = r;
                     blk_count++;
                 }