cleanups. Splitting in separate RX and TX buffers

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

2 files changed, 113 insertions, 79 deletions

diff --git a/dhcpv6.h b/dhcpv6.h
index 79d9a40..fc73030 100644
--- a/dhcpv6.h
+++ b/dhcpv6.h
@@ -3,8 +3,9 @@
 #pragma pack(push)
 #pragma pack(2) 
 struct vlan_tag {
-        u_int16_t tpid;
-	u_int16_t tci;
+        u_int16_t v_tpid;
+	u_int16_t v_tci;
+	u_int16_t h_proto;
 };
 
 struct dhcpv6_msg {	/* [RFC3315] ch 6 */
diff --git a/ldra.c b/ldra.c
index 7f3cd14..4e01803 100644
--- a/ldra.c
+++ b/ldra.c
@@ -61,10 +61,11 @@ http://aschauf.landshut.org/fh/linux/udp_vs_raw/ch01s03.html
 #include <netinet/udp.h>
 #include "dhcpv6.h"
 
+/* total size of packet headers - include VLAN tag as well? */
+#define PKTHDRZ  (sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr))
 
-/* allow for upto 34 byte RELAY-MSG + 8 byte INTERFACE-ID + 4 byte DHCP-OPT-HEADER +
-   82 bytes shared between REMOTE-ID and SUBSCRIBER-ID */
-#define RELAYOPT_LEN 128
+/* 802.1q tag size */
+#define VLANTAGZ 4
 
 /* attempting to filter out IPv6 to udp port 547
   NOTE: This filter is created as short as possible for sorting out
@@ -185,7 +186,7 @@ int main(int argc, char *argv[]) {
         int domain, s, i;
         char str[INET6_ADDRSTRLEN];
 	struct sockaddr_ll ll;
-	char *buf;
+	char *buf, *txbuf;
 	int val, fromlen, len = 0;
 	int on = 1;
 	int vlan;
@@ -311,9 +312,12 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 	
 	setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
 
-	/* reserve a bit of extra room for inserting our RELAY options in
-	   front of the received DHCPv6 packet */
-	buf = malloc(ETH_FRAME_LEN + RELAYOPT_LEN);
+	/* single frame RX buffer */
+	buf = malloc(ETH_FRAME_LEN);
+
+	/* single frame RX buffer */
+	txbuf = malloc(ETH_FRAME_LEN);
+
 
 	for (i = 0; i<20; i++) {
 	fromlen = sizeof(ll);
@@ -329,7 +333,7 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 	msg.msg_flags		= 0;
 
 	iov.iov_len		= ETH_FRAME_LEN;
-	iov.iov_base		= buf + RELAYOPT_LEN;
+	iov.iov_base		= buf;
 
 /* must use recvmsg() interface to get the aux data */
 #if 0
@@ -363,41 +367,59 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 	       }
 	}
 
+
 	if (vlan && (vlan != 602) && (ll.sll_ifindex == idx_down)) {
 		/* insert RELAY-MSG + INTERFACE-ID + OPTION_RELAY_MSG header */
-		int extra = sizeof(struct dhcpv6_relay_msg) + 2 * sizeof(struct dhcpv6_option) + sizeof(int);
-		char *head;
+		int extra;
 		struct dhcpv6_option *opt;
 		int *val;
 		int new, j;
 		u_int16_t *csum;
 		char *p;
+		int hopcount = 0;
+		int trusted = 1;  /* FIMXE: allow setting this per interface */
 
 		fprintf(stderr, "down => up\n");
 
 		fprintf(stderr, "received %d bytes from %s on ifid=%d:\n", len, macstr((char *)ll.sll_addr), ll.sll_ifindex);
-		print_hex(buf + RELAYOPT_LEN, len);
-
+		print_hex(buf, len);
+
+		/* verify that incoming packet is supposed to be relayed */
+		dhcpv6 = (struct dhcpv6_relay_msg *)(buf + PKTHDRZ);	/* NOTE: Not necessarily a relay message */
+		switch (dhcpv6->msg_type) {
+		case DHCPV6_ADVERTISE:
+		case DHCPV6_REPLY:
+		case DHCPV6_RECONFIGURE:
+		case DHCPV6_RELAY_REPL:
+			fprintf(stderr, "ignoring message type %hhu on downstream interface\n", dhcpv6->msg_type);
+			continue;
+		case DHCPV6_RELAY_FORW:
+			if (trusted)
+				hopcount = dhcpv6->hop_count + 1; /* propagate */
+			else {
+				fprintf(stderr, "ignoring message type DHCPV6_RELAY_FORW on untrusted downstream interface\n");
+				continue;
+			}
+		}
 
-		/* move ethernet + IPv6 + udp header */
-		head = memcpy(buf + RELAYOPT_LEN - extra, buf + RELAYOPT_LEN, sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr));
+		/* create outgoing frame */
+		/* 1. copy ethernet + IPv6 + udp header */
+		eth = memcpy(txbuf, buf, PKTHDRZ);
 
 		/* set up structure for easy data manipulation */
-		eth = (struct ethhdr *)head;
-		ip6 = (struct ip6_hdr *)(head + sizeof(struct ethhdr));
-		udp = (struct udphdr *)(head + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
-		dhcpv6 = (struct dhcpv6_relay_msg *)(head + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr));
+		ip6 = (struct ip6_hdr *)(txbuf + sizeof(struct ethhdr));
+		udp = (struct udphdr *)(txbuf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
 
-		/* fill inn DHCPv6 values */
+		/* 2. fill inn DHCP relay header */
+		dhcpv6 = (struct dhcpv6_relay_msg *)(txbuf + PKTHDRZ);
 		dhcpv6->msg_type = DHCPV6_RELAY_FORW;
-		dhcpv6->hop_count = 0;	/* FIXME: check if original packet type is DHCPV6_RELAY_FORW and if so: copy hopcount and inc */
+		dhcpv6->hop_count = hopcount;
 		memset(&dhcpv6->link_addr, 0, sizeof(dhcpv6->link_addr));
 		memcpy(&dhcpv6->peer_addr, &ip6->ip6_src, sizeof(dhcpv6->peer_addr));
 
 		fprintf(stderr, "sizeof dhcpv6_relay_msg = %d, sizeof(dhcpv6->link_addr)=%d, extra=%d\n", 
 			sizeof(struct dhcpv6_relay_msg), sizeof(dhcpv6->link_addr), extra);
 
-
 		/* fill in INTERFACE-ID */
 		opt = (struct dhcpv6_option *)&dhcpv6->options;
 		opt->code = htons(OPTION_INTERFACE_ID);
@@ -410,9 +432,18 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		opt->code = htons(OPTION_RELAY_MSG);
 		opt->len = htons(ntohs(udp->len) - sizeof(struct udphdr)); 
 
+		/* get the totals size of the added DHCP relay message with options */
+		extra = (char *)&opt->data - (char *)dhcpv6;
+
+		/* copy the original DHCP packet into the OPTION_RELAY_MSG */
+		memcpy(&opt->data, (buf + PKTHDRZ), ntohs(opt->len));
+		
+		fprintf(stderr, "verify that extra=%d is equal to %d\n", extra,
+			sizeof(struct dhcpv6_relay_msg) + 2 * sizeof(struct dhcpv6_option) + sizeof(int));
+
 		fprintf(stderr, "length of INTERFACE-ID attr is %d\n", (char *)opt - (char *)(&dhcpv6->options));
 
-		/* correct UDP and IPv6 header lengths */
+		/* update UDP and IPv6 header lengths */
 		new = htons(ntohs(udp->len) + extra);
 		udp->len = new;
 		ip6->ip6_plen = new;
@@ -449,18 +480,16 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		ll.sll_pkttype = 0;
 		
 		fprintf(stderr, "sending %d bytes to %s on ifid=%d:\n", len + extra, macstr((char *)ll.sll_addr), ll.sll_ifindex);
-		print_hex(head, len + extra);
+		print_hex(txbuf, len + extra);
 	
-		if (sendto(s, head, len + extra, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
+		if (sendto(s, txbuf, len + extra, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
 			fprintf(stderr, "sendto() failed: %m\n");
 
 		
 
 	} else if (ll.sll_ifindex == idx_up) {
 		int j;
-		char *head;
 		struct dhcpv6_option *opt, *max;
-		int headerlen = sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr);
 		int interfaceid = 0;
 		int datalen = 0;
 		int packetlen;
@@ -470,92 +499,97 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		fprintf(stderr, "up => down\n");
 
 		fprintf(stderr, "received %d bytes from %s on ifid=%d:\n", len, macstr((char *)ll.sll_addr), ll.sll_ifindex);
-		print_hex(buf + RELAYOPT_LEN, len);
+		print_hex(buf, len);
+
+		/* verify that incoming packet is supposed to be relayed */
+		dhcpv6 = (struct dhcpv6_relay_msg *)(buf + PKTHDRZ);	/* NOTE: Not necessarily a relay message */
+		switch (dhcpv6->msg_type) {
+		case DHCPV6_RELAY_REPL:
+			break;
+		default:
+			fprintf(stderr, "ignoring msg_type=%d on upstream interface\n", dhcpv6->msg_type);
+			continue;
+		}
 
-		/* set up structure for easy data manipulation */
-		head = buf + RELAYOPT_LEN;
-		udp = (struct udphdr *)(head + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
-		dhcpv6 = (struct dhcpv6_relay_msg *)(head + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr));
+		/* create outgoing frame */
+		/* 1. copy ethernet header */
+		eth = memcpy(txbuf, buf, sizeof(struct ethhdr));
 
 
-		/* verify that this is a packet we handle */
-		if (dhcpv6->msg_type != DHCPV6_RELAY_REPL) {
-			fprintf(stderr, "ignoring msg_type=%d\n", dhcpv6->msg_type);
-			continue;
-		}
+		/* 2. insert VLAN tag */
+		vtag = (struct vlan_tag *)&eth->h_proto;
+		vtag->h_proto = eth->h_proto;             /* copy protocol */
+		vtag->v_tpid = htons(ETH_P_8021Q);
 
-		fprintf(stderr, "dhcpv6->options=%p, (void *)udp + ntohs(udp->len)=%p, udp=%p, udp->len=%hd\n",
+		/* 3. copy IPv6 and UDP headers and set up structure pointers */
+		ip6 = memcpy(txbuf + sizeof(struct ethhdr) + VLANTAGZ, buf + sizeof(struct ethhdr), sizeof(struct ip6_hdr) + sizeof(struct udphdr));
+		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 */
+		/* initialize to first relay option in received packet */
 		opt = (struct dhcpv6_option *)&dhcpv6->options; 
 
-		/* save end pointer now, as we'll invalidate these structures inside the loop */
-		max = (struct dhcpv6_option *)((char *)udp + ntohs(udp->len));
 
 		do {
 			fprintf(stderr, "opt=%p, max=%p, opt->code=%#06hx, opt->len=%hd\n", 
 				opt, max, ntohs(opt->code), ntohs(opt->len));
 
-			/* FIXME: failsafing... */
-			if (opt->code == 0)
-				break;
-
 			switch (ntohs(opt->code)) {
-
 			case OPTION_INTERFACE_ID:
-				interfaceid = ntohl(*(int *)&opt->data);
+				/* sanity: never accept more than one of these! */
+				if (interfaceid > 0) {
+					fprintf(stderr, "multiple OPTION_INTERFACE_ID is not allowed!\n"); 
+				} else {
+					interfaceid = ntohl(*(int *)&opt->data);
+					vtag->v_tci = htons(interfaceid & 0xfff); /* use directly as VLAN */
+				}
 				break;
 			case OPTION_RELAY_MSG:
-				/* save the lengh for later header fixups - this will be destroyed by the memcpy! */
-				datalen = ntohs(opt->len);
-
-				/* this will destroy parts of the message we've already parsed */
-				head = memmove((char *)opt + sizeof(struct dhcpv6_option) - headerlen , head, headerlen);
-
+				/* sanity: never accept more than one of these! */
+				if (datalen > 0) {
+					fprintf(stderr, "multiple OPTION_RELAY_MSG is not allowed!\n"); 
+				} else {
+					/* save the lengh for later header fixups  */
+					datalen = ntohs(opt->len);
+
+					/* copy the option data to our outgoing packet */
+					memcpy(txbuf + PKTHDRZ + VLANTAGZ, &opt->data, datalen);
+				}
 				break;
 			default:
 				fprintf(stderr, "ignoring unknown relay option\n");
 			}
 			opt = nextopt(opt);
-		} while (opt < max);
+		} while ((char *)opt < (buf + len));
 
-
-		/* verify that head was moved */
-		if (head == (buf + RELAYOPT_LEN)) {
-			fprintf(stderr, "couldn't find any OPTION_RELAY_MSG in packet - ignoring\n");
+		/* verify that we found a 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 */
 		if (interfaceid == 0) {
-			fprintf(stderr, "couldn't find any OPTION_INTERFACE_ID in packet - ignoring\n");
+			fprintf(stderr, "couldn't find mandatory option OPTION_INTERFACE_ID in packet - ignoring\n");
 			continue;
 		}
 
-		/* set up pointers relative to our new packet head */
-		eth = (struct ethhdr *)(head - sizeof(struct vlan_tag));
-		ip6 = (struct ip6_hdr *)(head + sizeof(struct ethhdr));
-		udp = (struct udphdr *)(head + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
-
-
-		/* insert VLAN tag */
-		head = memcpy(eth, head, sizeof(struct ethhdr) - sizeof(eth->h_proto));
-		vtag = (struct vlan_tag *)(head + sizeof(struct ethhdr) - sizeof(eth->h_proto));
-		vtag->tpid = htons(ETH_P_8021Q);
-		vtag->tci = htons(interfaceid & 0xfff);
-
-		/* and fixup */
+		/* fixup lengths and sestination port */
 		ip6->ip6_plen = htons(datalen + sizeof(struct udphdr));
 		udp->len = ip6->ip6_plen;
 		udp->dest = htons(546);
-		udp->check = 0; /* reset before calculating new checksum */
 
+		/* calculate UDP checksum */
+		udp->check = 0; /* reset before calculating new checksum */
 		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 */
 
 
-		packetlen = sizeof(struct ethhdr) +  sizeof(struct ip6_hdr) + ntohs(udp->len);
-		packetlen += sizeof(struct vlan_tag);
+		packetlen = sizeof(struct ethhdr) + VLANTAGZ +  sizeof(struct ip6_hdr) + ntohs(udp->len);
 		fprintf(stderr, "packetlen=%d bytes\n", packetlen);
 
 		/* finally: transmit the packet on the correct downstream interface */
@@ -571,11 +605,10 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.
 		ll.sll_hatype = 0;
 		ll.sll_pkttype = 0;
 
-
 		fprintf(stderr, "sending %d bytes to %s on ifid=%d:\n", packetlen, macstr((char *)ll.sll_addr), ll.sll_ifindex);
-		print_hex(head, packetlen);
+		print_hex(txbuf, packetlen);
 
-		if (sendto(s, head, packetlen, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
+		if (sendto(s, txbuf, packetlen, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
 			fprintf(stderr, "sendto() failed: %m\n");