working version using old checksum code

Signed-off-by: Bjørn Mork <bjorn@mork.no>

1 files changed, 61 insertions, 22 deletions

diff --git a/ldra.c b/ldra.c
index 9c3196a..ea2c683 100644
--- a/ldra.c
+++ b/ldra.c
@@ -91,15 +91,18 @@ static struct sock_filter ipv6udp547[] = {
         BPF_STMT(BPF_RET+BPF_K, 0),
 };
 
-/* TODO:  create an alternate VLAN matching filter to allow us to use VLAN tags as interface IDs? */
-
-char *macstr(const char *mac) {
+const char *macstr(const char *mac) {
 	static char buf[] = "00:00:00:00:00:00";
 	sprintf(buf, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", 
 		mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 	return buf;
 }
 
+const char *ip6str(const struct in6_addr *ip6) {
+	static char buf[INET6_ADDRSTRLEN];
+	return inet_ntop(AF_INET6, ip6, buf, INET6_ADDRSTRLEN);
+}
+
 int print_hex(const char *buf, size_t len) {
 	char *p;
 	unsigned int i = 0;
@@ -152,8 +155,20 @@ u_int16_t csum_ipv6udp(struct ip6_hdr *ip6, const u_int16_t *data,  size_t datal
 
  */
 
+int newalgo = 0; // does not work...
+
 u_int32_t wrapsum(u_int32_t sum) {
-	sum = ~sum & 0xFFFF;
+       sum = ~sum & 0xFFFF;
+       return (htons(sum));
+
+}
+
+u_int32_t complement(u_int32_t sum) {
+	/* carry adjustment  */
+	while (sum >> 16)
+		sum = (sum & 0xffff) + (sum >> 16);
+
+	sum = ~sum & 0xffff;
 	return (htons(sum));
 }
 
@@ -182,6 +197,23 @@ u_int32_t checksum(unsigned char *buf, unsigned nbytes, u_int32_t sum) {
 }
 
 
+
+/* calulate a possibly partial checksum */
+u_int32_t chksum(u_int8_t *buf, size_t n) {
+	u_int32_t sum;
+	int i;
+
+	for (i = 0; i < n/2; i += 2)
+		sum += (u_int16_t)ntohs(*(u_int16_t *)(buf + i));
+
+	/* odd number of bytes? */
+	if (i < n)
+		sum += buf[i] << 8;
+
+	return sum;
+}
+
+
 int main(int argc, char *argv[]) {
         int domain, s, i;
         char str[INET6_ADDRSTRLEN];
@@ -406,6 +438,9 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		ip6 = (struct ip6_hdr *)(txbuf + sizeof(struct ethhdr));
 		udp = (struct udphdr *)(txbuf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
 
+		fprintf(stderr, "sending from %s", ip6str(&ip6->ip6_src));
+		fprintf(stderr, " to %s\n", ip6str(&ip6->ip6_dst));
+
 		/* 2. fill inn DHCP relay header */
 		dhcpv6 = (struct dhcpv6_relay_msg *)(txbuf + PKTHDRZ);
 		dhcpv6->msg_type = DHCPV6_RELAY_FORW;
@@ -454,8 +489,7 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		/* fixup UDP checksum - why is the 2* and +2 correct? */
 		new = ~(ntohs(udp->check)) + 2 * extra + 2; /* initialize to old sum + added length */
 
-		/* maybe correct UDP source port?
-		   NOTE: port is not part of the pseudo header used for checksumming */
+		/* maybe correct UDP source port? NOTE: may already by 547 if we received the packet from another relay */
 		if (ntohs(udp->source) == 546)
 			udp->source = htons(547);
 
@@ -492,12 +526,11 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 
 	} else if (ll.sll_ifindex == idx_up) {
 		int j;
-		struct dhcpv6_option *opt, *max;
+		struct dhcpv6_option *opt;
 		int interfaceid = 0;
 		int datalen = 0;
 		int packetlen;
 		struct vlan_tag *vtag;
-		struct tpacket_auxdata *aux;
 
 		fprintf(stderr, "up => down\n");
 
@@ -514,14 +547,20 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 			continue;
 		}
 
+		/* FIXME: parse options here, to allow us to decide
+		 * whether or not to add a VLAN tag based on the
+		 * interfaceid 
+		 */
+
+
 		/* create outgoing frame */
+
 		/* 1. copy ethernet header */
 		eth = memcpy(txbuf, buf, sizeof(struct ethhdr));
 
-
 		/* 2. insert VLAN tag */
 		vtag = (struct vlan_tag *)&eth->h_proto;
-		vtag->h_proto = eth->h_proto;             /* copy protocol */
+		vtag->h_proto = eth->h_proto;             /* copy original protocol */
 		vtag->v_tpid = htons(ETH_P_8021Q);
 
 		/* 3. copy IPv6 and UDP headers and set up structure pointers */
@@ -529,17 +568,10 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		udp = (struct udphdr *)(txbuf + sizeof(struct ethhdr) + VLANTAGZ + sizeof(struct ip6_hdr));
 		
 
-/*		fprintf(stderr, "dhcpv6->options=%p, (void *)udp + ntohs(udp->len)=%p, udp=%p, udp->len=%hd\n",
-			&dhcpv6->options, (void *)udp + ntohs(udp->len), udp, ntohs(udp->len));
-*/
-
-		/* initialize to first relay option in received packet */
+		/* 4. parse RELAY options */
 		opt = (struct dhcpv6_option *)&dhcpv6->options; 
-
-
 		do {
-			fprintf(stderr, "opt=%p, max=%p, opt->code=%#06hx, opt->len=%hd\n", 
-				opt, max, ntohs(opt->code), ntohs(opt->len));
+			fprintf(stderr, "opt=%p, opt->code=%#06hx, opt->len=%hd\n", opt, ntohs(opt->code), ntohs(opt->len));
 
 			switch (ntohs(opt->code)) {
 			case OPTION_INTERFACE_ID:
@@ -569,12 +601,12 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 			opt = nextopt(opt);
 		} while ((char *)opt < (buf + len));
 
-		/* verify that we found a OPTION_RELAY_MSG */
+		/* verify that we found an OPTION_RELAY_MSG */
 		if (datalen == 0) {
 			fprintf(stderr, "couldn't find mandatory option OPTION_RELAY_MSG in packet - ignoring\n");
 			continue;
 		}
-		/* verify that we found a OPTION_INTERFACE_ID */
+		/* verify that we found an OPTION_INTERFACE_ID */
 		if (interfaceid == 0) {
 			fprintf(stderr, "couldn't find mandatory option OPTION_INTERFACE_ID in packet - ignoring\n");
 			continue;
@@ -587,10 +619,17 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 
 		/* calculate UDP checksum */
 		udp->check = 0; /* reset before calculating new checksum */
+
+		if (!newalgo) { /* FIMXE: testing */
 		udp->check = wrapsum(checksum((unsigned char *)udp, sizeof(struct udphdr) + datalen, /* udp header + packet data */
 				     checksum((unsigned char *)&ip6->ip6_src, 2 * sizeof(ip6->ip6_src), /* src + dst address */
 				     IPPROTO_UDP + (u_int32_t)ntohs(udp->len))));                       /* final part of pseudo header */
 
+		} else {
+		udp->check = complement(chksum((u_int8_t *)udp, sizeof(struct udphdr) + datalen) +     		/* udp header + packet data */
+					chksum((u_int8_t *)&ip6->ip6_src, 2 * sizeof(ip6->ip6_src)) +	/* src + dst address */
+					IPPROTO_UDP + (u_int32_t)ntohs(udp->len));			/* final part of pseudo header */
+		}
 
 		packetlen = sizeof(struct ethhdr) + VLANTAGZ +  sizeof(struct ip6_hdr) + ntohs(udp->len);
 		fprintf(stderr, "packetlen=%d bytes\n", packetlen);
@@ -604,7 +643,7 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		ll.sll_ifindex = idx_down;
 
 		memcpy(&ll.sll_addr, &eth->h_dest, ll.sll_halen);
-		ll.sll_protocol = 0;
+ 		ll.sll_protocol = 0;
 		ll.sll_hatype = 0;
 		ll.sll_pkttype = 0;