path: root/ldra.c

/*
  client facing interfaces:
   ifid, remoteid, subscriberid , trusted
 

   intercept traffic to ff02::1:2 udp port 547

   drop these message types:

   o  ADVERTISE (2)

   o  REPLY (7)

   o  RECONFIGURE (10)

   o  RELAY-REPL (13)

   o  RELAY-FORW (12) unless trusted


  copy ll and ip source and destination
  create RELAY-FORW


  network facing interfaces:

   intercept traffic from link local to link local udp dest port 547

   drop any message except

   o  RELAY-REPL (13)


  generic sanity:

   If a Relay-Message would exceed the MTU of the outgoing interface, it
   MUST be discarded, and an error condition SHOULD be logged.



  code sample:
http://aschauf.landshut.org/fh/linux/udp_vs_raw/ch01s03.html

 */

#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
/* FIXME: colliding with if_packet.h #include <netpacket/packet.h> */
#include <net/ethernet.h>
#include <net/if.h>
#include <sys/ioctl.h>  /* SIOCGIFINDEX */
#include <linux/filter.h>
#include <linux/if_packet.h> /* for PACKET_ORIGDEV */
#include <netinet/in.h>
#include <netinet/ip6.h>
#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))

/* 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
  the interesting packets.  The packets need further sanity checking
  after matching this.  Addresses, source ports and DHCPv6 message
  types should be verified later.
 */
static struct sock_filter ipv6udp547[] = {
	/* verify IPv6 */
        BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 12),  /* 6 byte dst + 6 byte src */
        BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, ETH_P_IPV6, 0, 5), /* 86dd */

	/* verify next header = udp (NOTE: ingoring any additional headers!) */
        BPF_STMT(BPF_LD+BPF_B+BPF_ABS, 20),  /* 14 byte ethernet + 4 byte ipver/class/flow + 2byte payload len */ 
        BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, IPPROTO_UDP, 0, 3), /* udp = 17 */

	/* src IPv6 addr = 22, dst IPv6 addr = 38 - do not check these here */
	/* verify destination port */
        BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 56), /* 14 byte ethernet + 40 byte IPv6 + 2 byte src port */
        BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, 547, 0, 1),

        BPF_STMT(BPF_RET+BPF_K, (u_int)-1),
        BPF_STMT(BPF_RET+BPF_K, 0),
};

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;

	for (p = (char *)buf; p < (buf + len); p++) {
		if (i % 16 == 0) 
			fprintf(stderr, "\n%04x", i);
		if (i % 8 == 0) 
			fprintf(stderr, " ");
		fprintf(stderr, " %02hhx", *p);
		i++;
	}
	fprintf(stderr, "\n\n");
}


/* calulate a possibly partial checksum from a stream of 16bit 
   numbers in network byte order.

   Just sum them up, leaving the carry for later adjustments 
*/
u_int32_t chksum(char *buf, size_t n)
{
	u_int32_t sum = 0;
	int i;

	/* this will limit the max buf length, but we cannot handle that large packets anyway */
	for (i = 0; i < (n & 0xfffe); i += 2)
		sum += ntohs(*(u_int16_t *)(buf + i));

	/* odd number of bytes? */
	if (i < n)
		sum += buf[i] << 8;

	return sum;
}

/* calculate IPv6 udp chksum, blindly assuming that there are no other headers in the packet */
u_int16_t udp6chksum(struct ip6_hdr *ip6)
{
	int len = ntohs(ip6->ip6_plen);
	struct udphdr *udp = (struct udphdr *)((char *)ip6 + sizeof(struct ip6_hdr));
	u_int32_t sum = chksum((char *)udp, len);	/* udp header + packet data */

	/* add the pseudo header: src + dst + nxtheader + length */
	sum += chksum((char *)&ip6->ip6_src, 2 * sizeof(ip6->ip6_src));
	sum += IPPROTO_UDP + len;

	/* subtract the old checksum instead of resetting it to 0 */
	sum -= ntohs(udp->check);

	/* adjust for carry */
	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);

	/* return the complement sum */
	return ~sum;
}


/* decodes the relay attributes, returning the interfaceid and the OPTION_RELAY_MSG option */
int parse_relay_packet(struct dhcpv6_option **relay_msg, size_t len)
{
	int interfaceid = -1;
	int n = 0;
	struct dhcpv6_option *opt;
	char *buf = (char *)*relay_msg;

	/* default to not found */
	*relay_msg = NULL;

	/* parse the RELAY options before copying anything, so that we know e.g. if we need a VLAN tag */
	while (n < len) {
		opt = (struct dhcpv6_option *)(buf + n);
		n += sizeof(struct dhcpv6_option) + 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:
			if (interfaceid > 0) { /* sanity: never accept more than one of these! */
				fprintf(stderr, "multiple OPTION_INTERFACE_ID is not allowed!\n");
				return -1;
			}
			interfaceid = ntohl(*(int *)&opt->data);
			break;
		case OPTION_RELAY_MSG:
			if (*relay_msg) { /* sanity: never accept more than one of these! */
				fprintf(stderr, "multiple OPTION_RELAY_MSG is not allowed!\n"); 
				return -1;
			}
			/* save a pointer to this option to preserve both lenth and data for later  */
			*relay_msg = opt;
			break;
		default:
			fprintf(stderr, "ignoring unknown relay option: %hd\n", ntohs(opt->code));
		}
	}

	/* verify that we found all the required options */
	if ((*relay_msg == NULL) || (interfaceid < 0)) {
		fprintf(stderr, "couldn't find all mandatory options - ignoring packet\n");
		return -1;
	}

	/* return the index of the interface where this frame should be sent */
	return interfaceid;
}

/* set both IPv6 and UDP length fields and return pointer to UDP header */
struct udphdr *setlen(struct ip6_hdr *ip6, int datalen)
{
	struct udphdr *udp;
	int len = htons(datalen + sizeof(struct udphdr));

	ip6->ip6_plen = len;
	udp = (struct udphdr *)((char *)ip6 + sizeof(struct ip6_hdr));
	udp->len = len;
	return udp;
}

/* copy packet headers, possibly inserting vlan tag */
int copyheader(char *txbuf, char *buf, int tci)
{
	struct ethhdr *eth;
	struct vlan_tag *vtag;
	int size;

	/* 1. copy ethernet header */
	eth = memcpy(txbuf, buf, sizeof(struct ethhdr));
	size = sizeof(struct ethhdr);

	/* 2. insert VLAN tag */
	if (tci > 0) {
		memcpy((char *)&eth->h_proto + sizeof(struct vlan_tag), &eth->h_proto, sizeof(eth->h_proto)); /* copy original protocol */
		vtag = (struct vlan_tag *)&eth->h_proto;
		vtag->v_tpid = htons(ETH_P_8021Q);
		vtag->v_tci = htons(tci);
		size += sizeof(struct vlan_tag);
	}

	/* 3. copy IPv6 header and update length */
	memcpy(txbuf + size, buf + sizeof(struct ethhdr), sizeof(struct ip6_hdr));
	size += sizeof(struct ip6_hdr);

	return size;
}

int sendit(int s, char *txbuf, int len, int ifidx)
{
	struct sockaddr_ll ll;
	struct ethhdr *eth = (struct ethhdr *)txbuf;
	int ret;

	memset(&ll, 0, sizeof(ll));
	ll.sll_family = AF_PACKET;
	ll.sll_ifindex = ifidx;
	ll.sll_halen = sizeof(eth->h_dest);
	memcpy(&ll.sll_addr, &eth->h_dest, ll.sll_halen);
	
	fprintf(stderr, "sending %d bytes to %s on ifid=%d:\n", len, macstr((char *)ll.sll_addr), ll.sll_ifindex);

	if (len < 1000)
		print_hex(txbuf, len);
	
	if (ret = sendto(s, txbuf, len, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
		fprintf(stderr, "sendto() failed: %m\n");
	
	return ret;
}

int main(int argc, char *argv[]) {
        int domain, s, i;
        char str[INET6_ADDRSTRLEN];
	struct sockaddr_ll ll;
	char *buf, *txbuf;
	int fromlen, len = 0;
	int on = 1;
	int vlan;
	int size;

	struct sock_fprog fprog;
	
	struct ethhdr *eth;
	struct ip6_hdr *ip6;
	struct udphdr *udp;
	struct dhcpv6_relay_msg *dhcpv6;

	struct iovec		iov;
	struct msghdr           msg;
	struct cmsghdr          *cmsg;
	union {
		struct cmsghdr  cmsg;
		char            buf[CMSG_SPACE(sizeof(struct tpacket_auxdata))];
	} cmsg_buf;


	struct ifreq ifr;

	const char upstream[] = "eth2.602";
	const char downstream[] = "eth2";  /* Note: may include upstream! */

	int idx_up, idx_down;

	/* FIXME: When using VLANs, we receive the *same* packet on both
	   main interface and VLAN interface, with no difference except
	   for the ifindex */	
	s = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); /* use ETH_P_IPV6 ? */
	if (s == -1) {  
		fprintf(stderr, "%s(): socket failed: %m\n", __FUNCTION__);
		return(0);
	}

	/* FIXME: run SIOCGIFINDEX repeatedly to get the list of interesting ifindexes */
	strncpy((char *)&ifr.ifr_name, upstream, IFNAMSIZ);
	if (ioctl(s, SIOCGIFINDEX, &ifr, sizeof(ifr)))
		fprintf(stderr, "%s(): SIOCGIFINDEX failed: %m\n", __FUNCTION__);
		
	idx_up = ifr.ifr_ifindex;

	strncpy((char *)&ifr.ifr_name, downstream, IFNAMSIZ);
	if (ioctl(s, SIOCGIFINDEX, &ifr, sizeof(ifr)))
		fprintf(stderr, "%s(): SIOCGIFINDEX failed: %m\n", __FUNCTION__);
		
	idx_down = ifr.ifr_ifindex;

	fprintf(stderr,"idx_down=%d, idx_up=%d\n", idx_down, idx_up);

	/* Maybe bind to exactly two interfaces - one upstream and one downstream,
	   using a specifically crated filter for each socket? */


	/* maybe use setsockopt(3, SOL_PACKET, PACKET_RESERVE, ...) to 
	   reserve extra room for our inserted options?


	   Only relevant if using mmapped ring buffers?
	*/

	/* Probe whether kernel supports TPACKET_V2 
	   taken from http://seclists.org/tcpdump/2008/q3/36 
	val = TPACKET_V2;
	len = sizeof(val);
	if (getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &val, &len) < 0) {
		if (errno == ENOPROTOOPT)
			fprintf(stderr, "TPACKET_V2 unsupported!\n");
		else 
			fprintf(stderr, "can't get TPACKET_V2 header len: %m\n");
	}
	fprintf(stderr, "hdrlen=%d\n", val);

	val = TPACKET_V2;
	if (setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)) < 0) {
		fprintf(stderr, "can't activate TPACKET_V2 on socket %m\n");
	}

	/* ref http://gteissier.wordpress.com/2010/02/02/packet-capture-on-recent-linux-systems/ */


	/* attempt to get VLAN tags
	   Must be reconstructed after receiving the packet.
	   ref: http://seclists.org/tcpdump/2008/q3/35

	   NOTE:  this just enable us to call recvmsg() later
	*/
	if (setsockopt(s, SOL_PACKET, PACKET_AUXDATA, &on, sizeof(on)) == -1)
		fprintf(stderr, "%s(): setsockopt failed: %m\n", __FUNCTION__);

/* from packet(7):

     By default all packets of the specified protocol type are passed to a packet
       socket.  To only get packets from a specific interface use bind(2) specifying
       an address in a struct sockaddr_ll to bind the packet socket to an interface.
       Only the sll_protocol and the sll_ifindex address fields are used for purposes
       of binding.

 from socket(7):

BUGS
       The CONFIG_FILTER socket options SO_ATTACH_FILTER and SO_DETACH_FILTER are not documented.  The  suggested
       interface to use them is via the libpcap library.

 from linux/Documentation/networking/filter.txt :

setsockopt(sockfd, SOL_SOCKET, SO_ATTACH_FILTER, &Filter, sizeof(Filter));
setsockopt(sockfd, SOL_SOCKET, SO_DETACH_FILTER, &value, sizeof(value));

See the BSD bpf.4 manpage and the BSD Packet Filter paper written by
Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory.



  see also linux/net/core/filter.c

 and /usr/include/linux/filter.h
*/

	fprog.len = sizeof(ipv6udp547)/sizeof(struct sock_filter);
	fprog.filter = ipv6udp547;
	
	setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));

	/* single frame RX buffer */
	buf = malloc(ETH_FRAME_LEN);

	/* single frame RX buffer */
	txbuf = malloc(ETH_FRAME_LEN);


	for (i = 0; i<20; i++) {

	/* get the VLAN tag etc */
	memset(&msg, 0, sizeof(msg));
	msg.msg_name		= &ll;
	msg.msg_namelen		= sizeof(ll);
	msg.msg_iov		= &iov;
	msg.msg_iovlen		= 1;
	msg.msg_control		= &cmsg_buf;
	msg.msg_controllen	= sizeof(cmsg_buf);
	msg.msg_flags		= 0;

	iov.iov_len		= ETH_FRAME_LEN;
	iov.iov_base		= buf;

	len = recvmsg(s, &msg, MSG_TRUNC);

	if (len == -1) {
		fprintf(stderr, "%s(): recvfrom failed: %m\n", __FUNCTION__);
		return(0);
	}


	/* ignore outgoing packets */
	if (ll.sll_pkttype == PACKET_OUTGOING)
		continue;

	/* reset before continuing */
	vlan=0;
	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
		struct tpacket_auxdata *aux;

		/* only looking for the PACKET_AUXDATA msg */
		if (cmsg->cmsg_len < CMSG_LEN(sizeof(struct tpacket_auxdata)) ||
			cmsg->cmsg_level != SOL_PACKET ||
			cmsg->cmsg_type != PACKET_AUXDATA)
			continue;

               aux = (struct tpacket_auxdata *)CMSG_DATA(cmsg);
	       if (aux->tp_status & TP_STATUS_VLAN_VALID)
		       vlan = aux->tp_vlan_tci;
	}

	/* do some common checks before knowing whether this packet goes up or down */

	/* NOTE: Not necessarily a relay message, but we only look at msg_type until we know for sure */
	dhcpv6 = (struct dhcpv6_relay_msg *)(buf + PKTHDRZ);	

	fprintf(stderr, "received %d bytes from %s on interface %d (vlan=%d) with DHCPv6 type=%hhd\n",
		len, macstr((char *)ll.sll_addr), ll.sll_ifindex, vlan, dhcpv6->msg_type);

		
	/* packets from upstream are simple to discard */
	if (ll.sll_ifindex == idx_up) {
		struct dhcpv6_option *opt;
		int txintf;
		int datalen;

		/* only DHCPV6_RELAY_REPL messages are to be processed */
		if (dhcpv6->msg_type != DHCPV6_RELAY_REPL) {
			fprintf(stderr, "ignoring message type %hhu on interface %d\n", dhcpv6->msg_type, ll.sll_ifindex);
			continue;
		}

		/* FIMXE:
		   verify that destination and source address is link local
		   verify that source port is 547
		*/

		/* parse DHCP RELAY message */
		opt = (struct dhcpv6_option *)&dhcpv6->options;
		txintf = parse_relay_packet(&opt, buf + len - (char *)&dhcpv6->options);

		/* parse error? */
		if (txintf < 0)
			continue;

		/* create tx packet header */
		size = copyheader(txbuf, buf, txintf & 0xfff);

		/* we change most of the UDP header, so create a new instead of copying */
		ip6 = (struct ip6_hdr *)(txbuf + size - sizeof(struct ip6_hdr));
		udp = setlen(ip6, ntohs(opt->len));
		udp->dest = htons(546);
		udp->source = htons(547);
		size += sizeof(struct udphdr);
		
		/* copy in the DHCP message */
		memcpy(txbuf + size, &opt->data, ntohs(opt->len));
		size += ntohs(opt->len);

		/* update the checksum after copying all the data */
		udp->check = htons(udp6chksum(ip6));

		/* ready to send message */
		sendit(s, txbuf, size, txintf >> 16);

	} else { /* most likely downstream, but could still be a VLAN tagged upstream on a common port */
		struct dhcpv6_option *opt;
		int extra;
		int datalen;
		int *val;
		int hopcount = 0;
		int trusted = 1;	/* FIMXE: allow setting this per interface */
		int mtu = 1500;		/* FIMXE: get this from the upstream interface? */


		/* FIMXE: We receive three(!) copies of the same frame in the common case:
		   tagged on port, untagged in VLAN interface, untagged on bridge interface
		*/
		
		/* require tag */
		if (!vlan)
			continue;

		switch (dhcpv6->msg_type) {
		case DHCPV6_RELAY_REPL:
			/* silently ignore this, as the reason might be that upstream is a VLAN on a common port */
			continue;
		
		case DHCPV6_ADVERTISE:
		case DHCPV6_REPLY:
		case DHCPV6_RECONFIGURE:
			fprintf(stderr, "ignoring message type %hhu on interface %d\n", dhcpv6->msg_type, ll.sll_ifindex);
			continue;

		case DHCPV6_RELAY_FORW:
			/* ignore these on untrusted interfaces */
			if (!trusted) {
				fprintf(stderr, "ignoring message type %hhu on untrused interface %d\n", dhcpv6->msg_type, ll.sll_ifindex);
				continue;
			}
			hopcount = dhcpv6->hop_count + 1; /* propagate */
		}

		/* FIMXE:
	   verify that destination address is ff02::1:2
	   verify that source address is link local
	   verify that source port is 546 (unless hopcount > 0)
	   verify that message type is not one of the forbidden ones

		 */
	
		/* create tx packet header */
		size = copyheader(txbuf, buf, 0); /* no VLAN */
		
		/* save a pointer to the IPv6 header */
		ip6 = (struct ip6_hdr *)(txbuf + size - sizeof(struct ip6_hdr));
		/* abusing the fact that we haven't updated this field yet */
		datalen = ntohs(ip6->ip6_plen) - sizeof(struct udphdr);
		
		/* just reserve space for the UDP header for now */
		size += sizeof(struct udphdr);
		
		/* 2. fill inn DHCP relay header */
		dhcpv6 = (struct dhcpv6_relay_msg *)(txbuf + size);
		dhcpv6->msg_type = DHCPV6_RELAY_FORW;
		dhcpv6->hop_count = hopcount;
		memset(&dhcpv6->link_addr, 0, sizeof(dhcpv6->link_addr));
		memcpy(&dhcpv6->peer_addr, &ip6->ip6_src, sizeof(dhcpv6->peer_addr));
	
		/* fill in INTERFACE-ID */
		opt = (struct dhcpv6_option *)&dhcpv6->options;
		opt->code = htons(OPTION_INTERFACE_ID);
		opt->len = htons(sizeof(int));
		val = (int *)&opt->data;
		*val = htonl((ll.sll_ifindex) << 16 | (vlan & 0xfff));

		/* fill in OPTION_RELAY_MSG */
		opt = nextopt(opt);
		opt->code = htons(OPTION_RELAY_MSG);
		opt->len = htons(datalen); 

		/* how many bytes did we insert? */
		extra = (char *)&opt->data - (char *)dhcpv6;

		/* account for total packet length */
		size += extra + datalen;

		/* verify that we have enough space lefte for the original DHCP packet */
		if (size > mtu) {
			fprintf(stderr, "ERROR: relay packet exceeds MTU (%d > %d)\n", size, mtu);
			continue;
		}
	
		/* copy the original DHCP packet into the OPTION_RELAY_MSG */
		memcpy(&opt->data, buf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr), datalen);
			
		/* update the UDP header now that we have the length and data in place */
		udp = setlen(ip6, datalen + extra); /* orig + inserted DHCP relay options */
		udp->dest = htons(547);
		udp->source = htons(547);
		udp->check = htons(udp6chksum(ip6));

		/* ready to send message */
		sendit(s, txbuf, size, idx_up);

	
        }
	}

	close(s);
	return(0);
}

#if 0

	if (vlan && (vlan != 602) && (ll.sll_ifindex == idx_down)) {
		/* insert RELAY-MSG + INTERFACE-ID + OPTION_RELAY_MSG header */
		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 */
		int mtu = 1500;		/* FIMXE: get this from the upstream 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, 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;
			}
		}

		/* create outgoing frame */
		/* 1. copy ethernet + IPv6 + udp header */
		eth = memcpy(txbuf, buf, PKTHDRZ);

		/* set up structure for easy data manipulation */
		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;
		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);
		opt->len = htons(sizeof(int));
		val = (int *)&opt->data;
		*val = htonl(vlan);

		/* fill in OPTION_RELAY_MSG */
		opt = nextopt(opt);
		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;

		/* verify that we have enough space lefte for the original DHCP packet */
		if ((extra + len) > mtu) {
			fprintf(stderr, "ERROR: relay packet exceeds MTU (%d > %d)\n", (extra + len), mtu);
			continue;
		}


		/* 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));

		/* update UDP and IPv6 header lengths */
		new = htons(ntohs(udp->len) + extra);
		udp->len = new;
		ip6->ip6_plen = new;

		/* 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);

		/* update UDP checksum - we could have based this on the old sum, but... */
		udp->check = htons(udp6chksum(ip6));

		/* finally: transmit the packet on the upstream interface */

		/* packet(7): When  you  send packets it is enough to specify 
		   sll_family, sll_addr, sll_halen, sll_ifindex.  The other 
		   fields should be 0. */
		ll.sll_ifindex = idx_up;
		memcpy(&ll.sll_addr, &eth->h_dest, ll.sll_halen);
		ll.sll_protocol = 0;
		ll.sll_hatype = 0;
		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(txbuf, len + extra);
	
		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;
		struct dhcpv6_option *opt;
		int interfaceid = 0;
		int datalen = 0;
		int packetlen;
		struct vlan_tag *vtag;

		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, 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;
		}

		/* 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 original protocol */
		vtag->v_tpid = htons(ETH_P_8021Q);

		/* 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));
		

		/* 4. parse RELAY options */
		opt = (struct dhcpv6_option *)&dhcpv6->options; 
		do {
			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:
				/* 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:
				/* 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 ((char *)opt < (buf + len));

		/* 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 an OPTION_INTERFACE_ID */
		if (interfaceid == 0) {
			fprintf(stderr, "couldn't find mandatory option OPTION_INTERFACE_ID in packet - ignoring\n");
			continue;
		}

		/* fixup lengths and destination port */
		ip6->ip6_plen = htons(datalen + sizeof(struct udphdr));
		udp->len = ip6->ip6_plen;
		udp->dest = htons(546);

		/* update UDP checksum */
		udp->check = htons(udp6chksum(ip6));

		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 */

		/* packet(7): When  you  send packets it is enough to specify 
		   sll_family, sll_addr, sll_halen, sll_ifindex.  The other 
		   fields should be 0. */

		ll.sll_ifindex = idx_down;

		memcpy(&ll.sll_addr, &eth->h_dest, ll.sll_halen);
 		ll.sll_protocol = 0;
		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(txbuf, packetlen);

		if (sendto(s, txbuf, packetlen, 0, (struct sockaddr *)&ll, sizeof(ll)) == -1)
			fprintf(stderr, "sendto() failed: %m\n");



	} else {
		fprintf(stderr, "ignoring unknown interface=%d\n", ll.sll_ifindex);
		continue;
	}


/*
	fprintf(stderr, "%s(): received %d bytes from address with len=%d on ifid=%d\n",
		__FUNCTION__, len, ll.sll_halen, ll.sll_ifindex );
	print_mac((char *)ll.sll_addr);
	fprintf(stderr,"\n");

	print_hex(buf, len);
*/

	/* TODO:
	   verify that packet arrived on one of the interfaces we're wathcing,
           and decide whether it is going up or down based on interface.

	   up:
	   verify that destination address is ff02::1:2
	   verify that source address is link local
	   verify that source port is 546
	   verify that message type is not one of the forbidden ones

	   get hop count from original packet if type is RELAY-FORW
	   and interface is trusted
	 
	   insert new options, including DHCPPACKET attribute between
	   udp header and original DHCP packet

	   outgoing interface is the predefined upstream interface


	   down:
	   verify that destination and source address is link local
	   verify that source port is 547
	   verify that message type is RELAY-REPL (13)

	   strip away the outer RELAY envelope, using the interface-id to select outgoing interface

	   common code:
	   
	   fixup both IPv6 payload length and UDP length
	   verify that the new length doesn't exceed outgoing interface MTU (or log error)
	   fixup UDP checksum
	   transmit on the selected outgoing interface (FIMXE: support multiple upstream interfaces?)

	   

	 */

/*
	eth = (struct ethhdr *)buf;
	ip6 = (struct ip6_hdr *)(buf + sizeof(struct ethhdr));
	udp = (struct udphdr *)(buf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr));
	dhcpv6 = (struct dhcpv6_relay_msg *)(buf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr));

	fprintf(stderr,"eth.h_proto=%#06x, ll.sll_protocol=%#06x, ip6.ip6_plen=%hd, udp.len=%hd, dhcpv6.msg_type=%hhd\n", 
		ntohs(eth->h_proto), ntohs(ll.sll_protocol), ntohs(ip6->ip6_plen), ntohs(udp->len), dhcpv6->msg_type);

*/


}

	close(s);
	return(0);
}

#endif