summaryrefslogtreecommitdiff
path: root/ldra.c
blob: 5d2a6bc617ee4b05bf51e4a05374c6264c7e6284 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
/*
  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");
}

u_int16_t csum_ipv6udp(struct ip6_hdr *ip6, const u_int16_t *data,  size_t datalen) {
	int csum = datalen + IPPROTO_UDP;
	int x, i;
	u_int16_t *p;
	char buf[INET6_ADDRSTRLEN];

	fprintf(stderr,"chksum: src=%s", inet_ntop(AF_INET6, &ip6->ip6_src, buf, INET6_ADDRSTRLEN));
	fprintf(stderr,", dst=%s, datalen=%d\n", inet_ntop(AF_INET6, &ip6->ip6_dst, buf, INET6_ADDRSTRLEN),datalen);

	/* sum up "pseudo header": ip6_src, ip6_dst, udplen, padding, nxtheader */
	p = (u_int16_t *)&ip6->ip6_src;
	for (i = 0; i < 8; i++)
		csum += ntohs(p[i]);

	p = (u_int16_t *)&ip6->ip6_dst;
	for (i = 0; i < 8; i++)
		csum += ntohs(p[i]);

	x = datalen/2;
	for (i = 0; i < x; i++)
		csum += ntohs(data[i]);

	if (x * 2 < datalen)	/* odd length? */
		csum += data[x+1] & 0xff00;

	while (csum >> 16)
		csum = (csum & 0xffff) + (csum >> 16);

	return ~csum;
}

/* from http://gitorious.org/freebsd/freebsd/blobs/dd19fe2fbd4de4e113ffb8ff8893726d03e38cf5/sbin/dhclient/packet.c

   FIXME: attribute it with license text!

 */

int newalgo = 1;

u_int32_t wrapsum(u_int32_t sum) {
       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));
}

u_int32_t checksum(unsigned char *buf, unsigned nbytes, u_int32_t sum) {
	int i;
	
	/* Checksum all the pairs of bytes first... */
	for (i = 0; i < (nbytes & ~1U); i += 2) {
		sum += (u_int16_t)ntohs(*((u_int16_t *)(buf + i)));
		if (sum > 0xFFFF)
			sum -= 0xFFFF;
	}
	
	/*
	 * If there's a single byte left over, checksum it, too.
	 * Network byte order is big-endian, so the remaining byte is
	 * the high byte.
	 */
	if (i < nbytes) {
		sum += buf[i] << 8;
		if (sum > 0xFFFF)
			sum -= 0xFFFF;
	}
	
	return (sum);
}



/* calulate a possibly partial checksum */
u_int32_t chksum(u_int8_t *buf, size_t n) {
	u_int32_t sum = 0;
	int i;

	for (i = 0; i <(n & ~1U); 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];
	struct sockaddr_ll ll;
	char *buf, *txbuf;
	int val, fromlen, len = 0;
	int on = 1;
	int vlan;

	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;

	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) {
		       fprintf(stderr, "got tpacket_auxdata with tp_vlan_tci=%hd\n", aux->tp_vlan_tci);
		       vlan = aux->tp_vlan_tci;
	       }
	}


	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;

		/* 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: may already by 547 if we received the packet from another relay */
		if (ntohs(udp->source) == 546)
			udp->source = htons(547);

		csum = (u_int16_t *)dhcpv6;
		for (j = 0; j < extra / 2; j++)  /* FIXME: assumes extra % 2 == 0 */
			new += htons(csum[j]);
		
		while (new >> 16)
			new = (new & 0xffff) + (new >> 16);

		if (new)
			udp->check = ~(ntohs(new));
		else 
			udp->check = 0xffff;

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

		/* calculate UDP checksum */
		udp->check = 0; /* reset before calculating new checksum */

		if (!newalgo) { /* FIMXE: testing */
			u_int16_t oldchksum, newchksum;

			oldchksum = 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 */
			newchksum = 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 */

			fprintf(stderr, "old chksum code: %#06hx, new checksum code: %#06hx\n",oldchksum,newchksum);
			udp->check = oldchksum;

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

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