Fix port reservation, checksumming.

3 files changed, 230 insertions, 67 deletions

diff --git a/src/checksum.c b/src/checksum.c
new file mode 100644
index 0000000..300237d
--- /dev/null
+++ b/src/checksum.c
@@ -0,0 +1,124 @@
+/*
+ *
+ * INET		An implementation of the TCP/IP protocol suite for the LINUX
+ *		operating system.  INET is implemented using the  BSD Socket
+ *		interface as the means of communication with the user level.
+ *
+ *		IP/TCP/UDP checksumming routines
+ *
+ * Authors:	Jorge Cwik, <jorge@laser.satlink.net>
+ *		Arnt Gulbrandsen, <agulbra@nvg.unit.no>
+ *		Tom May, <ftom@netcom.com>
+ *		Andreas Schwab, <schwab@issan.informatik.uni-dortmund.de>
+ *		Lots of code moved from tcp.c and ip.c; see those files
+ *		for more names.
+ *
+ * 03/02/96	Jes Sorensen, Andreas Schwab, Roman Hodek:
+ *		Fixed some nasty bugs, causing some horrible crashes.
+ *		A: At some points, the sum (%0) was used as
+ *		length-counter instead of the length counter
+ *		(%1). Thanks to Roman Hodek for pointing this out.
+ *		B: GCC seems to mess up if one uses too many
+ *		data-registers to hold input values and one tries to
+ *		specify d0 and d1 as scratch registers. Letting gcc
+ *		choose these registers itself solves the problem.
+ *
+ *		This program is free software; you can redistribute it and/or
+ *		modify it under the terms of the GNU General Public License
+ *		as published by the Free Software Foundation; either version
+ *		2 of the License, or (at your option) any later version.
+ */
+
+/* Based on code from the Linux kernel. */
+
+#include <stdint.h>
+
+/* Revised by Kenneth Albanowski for m68knommu. Basic problem: unaligned access
+ kills, so most of the assembly has to go. */
+
+static inline unsigned short from32to16(unsigned int x)
+{
+	/* add up 16-bit and 16-bit for 16+c bit */
+	x = (x & 0xffff) + (x >> 16);
+	/* add up carry.. */
+	x = (x & 0xffff) + (x >> 16);
+	return x;
+}
+
+static unsigned int do_csum(const unsigned char *buff, int len)
+{
+	int odd;
+	unsigned int result = 0;
+
+	if (len <= 0)
+		goto out;
+	odd = 1 & (unsigned long) buff;
+	if (odd) {
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+		result += (*buff << 8);
+#else
+		result = *buff;
+#endif
+		len--;
+		buff++;
+	}
+	if (len >= 2) {
+		if (2 & (unsigned long) buff) {
+			result += *(unsigned short *) buff;
+			len -= 2;
+			buff += 2;
+		}
+		if (len >= 4) {
+			const unsigned char *end = buff + ((unsigned)len & ~3);
+			unsigned int carry = 0;
+			do {
+				unsigned int w = *(unsigned int *) buff;
+				buff += 4;
+				result += carry;
+				result += w;
+				carry = (w > result);
+			} while (buff < end);
+			result += carry;
+			result = (result & 0xffff) + (result >> 16);
+		}
+		if (len & 2) {
+			result += *(unsigned short *) buff;
+			buff += 2;
+		}
+	}
+	if (len & 1)
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+		result += *buff;
+#else
+		result += (*buff << 8);
+#endif
+	result = from32to16(result);
+	if (odd)
+		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
+out:
+	return result;
+}
+
+/*
+ * computes the checksum of a memory block at buff, length len,
+ * and adds in "sum" (32-bit)
+ *
+ * returns a 32-bit number suitable for feeding into itself
+ * or csum_tcpudp_magic
+ *
+ * this function must be called with even lengths, except
+ * for the last fragment, which may be odd
+ *
+ * it's best to have buff aligned on a 32-bit boundary
+ */
+uint16_t csum_partial(const void *buff, int len, uint16_t wsum)
+{
+	uint16_t sum = wsum;
+	uint16_t result = do_csum(buff, len);
+
+	/* add in old sum, and carry.. */
+	result += sum;
+	if (sum > result)
+		result += 1;
+	return result;
+}
diff --git a/src/checksum.h b/src/checksum.h
index d9e6bda..f788ff1 100644
--- a/src/checksum.h
+++ b/src/checksum.h
@@ -1,27 +1,45 @@
+#include <stdarg.h>
 #include <stdint.h>
+#include <stdlib.h>
 
-static inline uint16_t do_csum(const char *buf, unsigned size) {
-    unsigned int sum = 0;
-    unsigned int i;
+// based on code from RFCs 1071 and 1624
 
-    for (i = 0; i < size - 1; i += 2)
-        sum += *(uint16_t *)&buf[i];
-
-    if (size & 1)
-        sum += (uint8_t)buf[i];
+/*
+static inline uint16_t csum_update(const void *ptr, uint16_t new_value, uint16_t wsum) {
+    uint32_t sum = *(uint16_t *)ptr + (~ntohs(*(uint16_t *)&new_value) & 0xffff) + ntohs(wsum);
+    sum = (sum & 0xffff) + (sum >> 16);
+    return htons(sum + (sum >> 16));
+}
 
+static inline uint16_t fold_sum(uint32_t sum) {
     while (sum >> 16)
-        sum = (sum & 0xFFFF) + (sum >> 16);
-
-    return ~sum;
+        sum = (sum & 0xffff) + (sum >> 16);
+    return sum;
 }
 
-static inline uint16_t csum_partial(const void *buff, int len, uint16_t wsum) {
-    unsigned int sum = (unsigned int)wsum;
-    unsigned int result = do_csum(buff, len);
+static inline uint16_t do_csum(const void *ptr, size_t len) {
+    uint32_t sum = 0;
+
+    while (len > 1) {
+        sum += *(uint16_t *)ptr++;
+        len -= 2;
+    }
 
-    result += sum;
-    if (sum > result)
-        result += 1;
-    return result;
+    if (len > 0)
+        sum += *(uint8_t *)ptr;
+
+    return ~fold_sum(sum);
+}
+
+static inline uint16_t csum_partial(uint16_t sum, const void *ptr, size_t len, ...) {
+    va_list ap;
+    va_start(ap, len);
+    do {
+        sum = ~fold_sum(~sum + ~do_csum(ptr, len));
+    } while ((ptr = va_arg(ap, const void *)) && (len = va_arg(ap, size_t)));
+    va_end(ap);
+    return sum;
 }
+*/
+
+uint16_t csum_partial(const void *buff, int len, uint16_t wsum);
diff --git a/src/client.c b/src/client.c
index 21f6217..23d5eac 100644
--- a/src/client.c
+++ b/src/client.c
@@ -1,3 +1,4 @@
+#include <assert.h>
 #include <errno.h>
 #include <ev.h>
 #include <fcntl.h>
@@ -17,7 +18,19 @@
 #include "client.h"
 #include "uthash.h"
 
-#define PORTS_IN_INT sizeof(int) * CHAR_BIT
+#define PORTS_IN_INT (sizeof(int) * CHAR_BIT)
+
+struct c_data {
+    const char *r_host;
+    const char *r_port;
+    struct o_c_sock *o_socks_by_caddr;
+    struct o_c_rsock *o_rsocks;
+    struct sockaddr_storage pkt_addr;
+    int s_sock;
+    int i_sock;
+    socklen_t s_addrlen;
+    uint16_t csum_p;
+};
 
 struct o_c_rsock {
     struct sockaddr *r_addr;
@@ -27,8 +40,8 @@ struct o_c_rsock {
     ev_io io_w;
     UT_hash_handle hh;
     int fd;
-    uint16_t csum_a;
     socklen_t r_addrlen;
+    uint16_t csum_p;
 };
 
 struct o_c_sock {
@@ -39,38 +52,27 @@ struct o_c_sock {
     ev_timer tm_w;
     UT_hash_handle hh_lp;
     UT_hash_handle hh_ca;
+    uint16_t csum_p;
     uint16_t seq_num;
     in_port_t l_port;
     uint8_t status;
     int8_t syn_retries;
 };
 
-struct c_data {
-    const char *r_host;
-    const char *r_port;
-    struct o_c_sock *o_socks_by_caddr;
-    struct o_c_rsock *o_rsocks;
-    struct sockaddr_storage pkt_addr;
-    socklen_t s_addrlen;
-    int s_sock;
-    int i_sock;
-};
-
 static struct c_data *global_c_data;
 
 static const uint8_t tcp_syn_retry_timeouts[] = { 3, 6, 12, 24, 0 };
 
 static inline int check_resv_poff(unsigned int *used_ports, uint16_t poff) {
-    if (used_ports[poff / PORTS_IN_INT] & (1 << poff % PORTS_IN_INT)) {
-        used_ports[poff / PORTS_IN_INT] |= 1 << poff % PORTS_IN_INT;
-        return poff;
-    }
-    return 0;
+    if (used_ports[poff / PORTS_IN_INT] & (1 << (poff % PORTS_IN_INT)))
+        return 0;
+    used_ports[poff / PORTS_IN_INT] |= 1 << (poff % PORTS_IN_INT);
+    return poff;
 }
 
 /* reserve a local TCP port (local addr, remote addr, remote port are usually
  * fixed in the tuple) */
-static uint16_t reserve_port(unsigned int *used_ports) {
+static inline uint16_t reserve_port(unsigned int *used_ports) {
     long r;
 
     // randomly try 16 places
@@ -78,10 +80,10 @@ static uint16_t reserve_port(unsigned int *used_ports) {
         r = random();
 
         if (check_resv_poff(used_ports, r % 32768))
-            return 32768 + r;
+            return 32768 + (r % 32768);
 
         if (check_resv_poff(used_ports, (r >> 16) % 32768))
-            return 32768 + (r >> 16);
+            return 32768 + ((r >> 16) % 32768);
     }
 
     // give up and go sequentially
@@ -285,50 +287,63 @@ static void cc_cb(struct ev_loop *loop, ev_io *w, int revents __attribute__((unu
     }
 }
 
-#define SIX_OR_FOUR(sa, six, four, neither) \
-    (((struct sockaddr *)(sa))->sa_family == AF_INET6 ? (six) : ((struct sockaddr *)(sa))->sa_family == AF_INET ? (four) : abort(), neither)
-
-#define EXTRACT_IN_ADDR(sa) \
-    SIX_OR_FOUR((struct sockaddr *)(sa), &(((struct sockaddr_in6 *)(sa))->sin6_addr), &(((struct sockaddr_in *)(sa))->sin_addr), NULL), \
-    SIX_OR_FOUR((struct sockaddr *)(sa), sizeof(struct in6_addr), sizeof(in_addr_t), 0)
-
-static int c_rsock_init(struct o_c_sock *sock, struct addrinfo *res) {
-    sock->rsock = malloc(sizeof(*sock->rsock));
-    memset(&sock->rsock->used_ports, 0, sizeof(sock->rsock->used_ports));
-    sock->rsock->r_addr = malloc(res->ai_addrlen);
+static inline struct o_c_rsock * c_rsock_init(struct addrinfo *res) {
+    struct o_c_rsock *rsock;
+    rsock = malloc(sizeof(*rsock));
+    memset(&rsock->used_ports, 0, sizeof(rsock->used_ports));
+    rsock->r_addr = malloc(res->ai_addrlen);
 
-    memcpy(sock->rsock->r_addr, res->ai_addr, res->ai_addrlen);
-    sock->rsock->r_addrlen = res->ai_addrlen;
+    memcpy(rsock->r_addr, res->ai_addr, res->ai_addrlen);
+    rsock->r_addrlen = res->ai_addrlen;
     freeaddrinfo(res);
-    sock->rsock->o_socks_by_lport = NULL;
+    rsock->o_socks_by_lport = NULL;
 
-    sock->rsock->fd = socket(sock->rsock->r_addr->sa_family, SOCK_RAW, IPPROTO_TCP);
-    if (!sock->rsock->fd) {
+    rsock->fd = socket(rsock->r_addr->sa_family, SOCK_RAW, IPPROTO_TCP);
+    if (!rsock->fd) {
         perror("socket");
-        return 0;
+        return NULL;
     }
 
-    if (connect(sock->rsock->fd, sock->rsock->r_addr, sock->rsock->r_addrlen) == -1) {
+    if (connect(rsock->fd, rsock->r_addr, rsock->r_addrlen) == -1) {
         perror("connect");
-        return 0;
+        return NULL;
     }
 
-    if (fcntl(sock->rsock->fd, F_SETFL, O_NONBLOCK) == -1) {
+    if (fcntl(rsock->fd, F_SETFL, O_NONBLOCK) == -1) {
         perror("fcntl");
-        return 0;
+        return NULL;
     }
 
     struct sockaddr_storage our_addr;
     socklen_t our_addr_len = sizeof(our_addr);
-    int r = getsockname(sock->rsock->fd, (struct sockaddr *)&our_addr, &our_addr_len);
+    int r = getsockname(rsock->fd, (struct sockaddr *)&our_addr, &our_addr_len);
     if (r == -1) {
         perror("getsockname");
-        return 0;
+        return NULL;
     }
 
-    //sock->rsock->csum_a = csum_partial(EXTRACT_IN_ADDR(sock->rsock->r_addr), csum_partial(EXTRACT_IN_ADDR(&our_addr), 0));
+    char proto[] = { 0, IPPROTO_TCP };
+
+    if (((struct sockaddr *)rsock->r_addr)->sa_family != our_addr.ss_family)
+        abort();
+
+    switch (our_addr.ss_family) {
+    case AF_INET:
+        rsock->csum_p = csum_partial(&((struct sockaddr_in *)&our_addr)->sin_addr, sizeof(in_addr_t),
+                csum_partial(&((struct sockaddr_in *)rsock->r_addr)->sin_addr, sizeof(in_addr_t), 0));
+        break;
+    case AF_INET6:
+        rsock->csum_p = csum_partial(&((struct sockaddr_in6 *)&our_addr)->sin6_addr, sizeof(struct in6_addr),
+                csum_partial(&((struct sockaddr_in6 *)rsock->r_addr)->sin6_addr, sizeof(struct in6_addr), 0));
+        break;
+    default:
+        abort();
+    }
 
-    return 1;
+    rsock->csum_p = csum_partial(&((struct sockaddr_in *)rsock->r_addr)->sin_port, sizeof(in_port_t),
+            csum_partial(proto, sizeof(proto), rsock->csum_p));
+
+    return rsock;
 }
 
 static void cs_cb(EV_P_ ev_io *w, int revents __attribute__((unused))) {
@@ -368,7 +383,8 @@ static void cs_cb(EV_P_ ev_io *w, int revents __attribute__((unused))) {
 
             if (!sock->rsock) {
                 DBG("could not locate remote socket to host, initializing new raw socket");
-                if (!c_rsock_init(sock, res)) {
+                sock->rsock = c_rsock_init(res);
+                if (!sock->rsock) {
                     ev_break(EV_A_ EVBREAK_ONE);
                     return;
                 }
@@ -382,6 +398,7 @@ static void cs_cb(EV_P_ ev_io *w, int revents __attribute__((unused))) {
             }
 
             uint16_t l_port = reserve_port(sock->rsock->used_ports);
+            assert(l_port >= 32768);
             DBG("using port %hu", l_port);
             if (!l_port) {
                 fputs("we ran out of ports?\n", stderr);
@@ -390,6 +407,8 @@ static void cs_cb(EV_P_ ev_io *w, int revents __attribute__((unused))) {
             }
             sock->l_port = htons(l_port);
 
+            sock->csum_p = csum_partial(&sock->l_port, sizeof(in_port_t), sock->rsock->csum_p);
+
             HASH_ADD_KEYPTR(hh_ca, c_data->o_socks_by_caddr, sock->c_address, addresslen, sock);
             HASH_ADD(hh_lp, sock->rsock->o_socks_by_lport, l_port, sizeof(in_port_t), sock);
 
@@ -398,10 +417,12 @@ static void cs_cb(EV_P_ ev_io *w, int revents __attribute__((unused))) {
             struct tcphdr buf = {
                 .th_sport = sock->l_port,
                 .th_dport = ((struct sockaddr_in *)sock->rsock->r_addr)->sin_port,
-                .th_seq = htonl(sock->seq_num++),
+                .th_seq = htonl(sock->seq_num),
                 .th_flags = TH_SYN,
-                .th_off = 5
+                .th_off = 5,
             };
+            uint16_t tsz = htons(sizeof(buf));
+            buf.th_sum = ~csum_partial(&buf.th_seq, 16, csum_partial(&tsz, sizeof(tsz), sock->csum_p));
 
             sock->pending_data = malloc(sz);
             memcpy(sock->pending_data, rbuf, sz);