From 5c72289e845695a7513f214f3d26c3f2ce78d0d8 Mon Sep 17 00:00:00 2001 From: Bjørn Mork Date: Wed, 9 Nov 2011 16:07:33 +0100 Subject: Working relay both ways - Still need a lot of cleanup! MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Bjørn Mork --- Makefile | 2 +- dhcpv6.h | 34 ++--- ldra.c | 456 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 461 insertions(+), 31 deletions(-) diff --git a/Makefile b/Makefile index 46569a8..f4adeff 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ all: ldra -ldra: ldra.c +ldra: ldra.c dhcpv6.h gcc -o ldra ldra.c diff --git a/dhcpv6.h b/dhcpv6.h index 1596034..79d9a40 100644 --- a/dhcpv6.h +++ b/dhcpv6.h @@ -1,3 +1,12 @@ + +/* these structures are word aligned */ +#pragma pack(push) +#pragma pack(2) +struct vlan_tag { + u_int16_t tpid; + u_int16_t tci; +}; + struct dhcpv6_msg { /* [RFC3315] ch 6 */ u_int8_t msg_type; /* Message type */ u_int8_t xid[3]; /* transaction-id */ @@ -19,24 +28,17 @@ struct dhcpv6_option { /* [RFC3315] generic option */ char data[]; /* any */ }; -struct dhcpv6_interface_id { /* [RFC3315] ch 22.18 */ - u_int16_t code; /* option-code (18) */ - u_int16_t len; /* option-len */ - char id[]; /* interface-id */ -}; +#pragma pack(pop) -struct dhcpv6_remote_id { /* [RFC4649] */ - u_int16_t code; /* option-code (37) */ - u_int16_t len; /* option-len */ - u_int32_t enterprise; /* enterprise-number */ - char id[]; /* remote-id */ -}; +static struct dhcpv6_option *nextopt(const struct dhcpv6_option *opt) { + return (struct dhcpv6_option *)((char *)opt + sizeof(struct dhcpv6_option) + ntohs(opt->len)); +} -struct dhcpv6_subscriber_id { /* [RFC4580] */ - u_int16_t code; /* option-code (38) */ - u_int16_t len; /* option-len */ - char id[]; /* subscriber-id */ -}; + +#define NEXTOPT(x) \ + x = ((struct dhcpv6_option *)x)->len ? \ + (struct dhcpv6_option *)((char *)x + (ntohs(((struct dhcpv6_option *)x)->len) + sizeof(struct dhcpv6_option))) : \ + NULL /* all the following are ref http://www.iana.org/assignments/dhcpv6-parameters/dhcpv6-parameters.xml */ diff --git a/ldra.c b/ldra.c index 2062c28..7f3cd14 100644 --- a/ldra.c +++ b/ldra.c @@ -47,16 +47,25 @@ http://aschauf.landshut.org/fh/linux/udp_vs_raw/ch01s03.html #include #include #include +#include #include #include -#include -#include +/* FIXME: colliding with if_packet.h #include */ +#include +#include +#include /* SIOCGIFINDEX */ #include +#include /* for PACKET_ORIGDEV */ #include #include #include #include "dhcpv6.h" + +/* 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 + /* 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 @@ -83,9 +92,11 @@ static struct sock_filter ipv6udp547[] = { /* TODO: create an alternate VLAN matching filter to allow us to use VLAN tags as interface IDs? */ -int print_mac(const char *mac) { - fprintf(stderr, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", +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; } int print_hex(const char *buf, size_t len) { @@ -103,25 +114,169 @@ int print_hex(const char *buf, size_t len) { 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! + + */ + +u_int32_t wrapsum(u_int32_t sum) { + 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); +} + + int main(int argc, char *argv[]) { int domain, s, i; char str[INET6_ADDRSTRLEN]; struct sockaddr_ll ll; char *buf; - int fromlen, len = 0; + 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_msg *dhcpv6; - + 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 @@ -156,20 +311,288 @@ 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); + + for (i = 0; i<20; i++) { fromlen = sizeof(ll); - buf = malloc(ETH_FRAME_LEN); - len = recvfrom(s, buf, ETH_FRAME_LEN, 0, (struct sockaddr *)&ll, &fromlen); + + /* get the VLAN tag etc */ + memset(&msg, 0, sizeof(msg)); + msg.msg_name = ≪ + 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 + RELAYOPT_LEN; + +/* must use recvmsg() interface to get the aux data */ +#if 0 + len = recvfrom(s, buf + RELAYOPT_LEN, ETH_FRAME_LEN, 0, (struct sockaddr *)&ll, &fromlen); +#endif + len = recvmsg(s, &msg, MSG_TRUNC); + if (len == -1) { fprintf(stderr, "%s(): recvfrom failed: %m\n", __FUNCTION__); return(0); } - fprintf(stderr, "%s(): received %d bytes from address with len=%d\n", __FUNCTION__, len, ll.sll_halen ); + + /* 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 = sizeof(struct dhcpv6_relay_msg) + 2 * sizeof(struct dhcpv6_option) + sizeof(int); + char *head; + struct dhcpv6_option *opt; + int *val; + int new, j; + u_int16_t *csum; + char *p; + + 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); + + + /* move ethernet + IPv6 + udp header */ + head = memcpy(buf + RELAYOPT_LEN - extra, buf + RELAYOPT_LEN, sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr)); + + /* 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)); + + /* fill inn DHCPv6 values */ + 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 */ + 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)); + + fprintf(stderr, "length of INTERFACE-ID attr is %d\n", (char *)opt - (char *)(&dhcpv6->options)); + + /* correct 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: port is not part of the pseudo header used for checksumming */ + 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, ð->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(head, len + extra); + + if (sendto(s, head, 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; + struct vlan_tag *vtag; + struct tpacket_auxdata *aux; + + 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); + + /* 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)); + + + /* 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; + } + + 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 */ + 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); + 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); + + break; + default: + fprintf(stderr, "ignoring unknown relay option\n"); + } + opt = nextopt(opt); + } while (opt < max); + + + /* verify that head was moved */ + if (head == (buf + RELAYOPT_LEN)) { + fprintf(stderr, "couldn't find any OPTION_RELAY_MSG in packet - ignoring\n"); + continue; + } + if (interfaceid == 0) { + fprintf(stderr, "couldn't find any 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 */ + 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 */ + + 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); + 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, ð->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(head, packetlen); + + if (sendto(s, head, 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, @@ -208,14 +631,19 @@ Steven McCanne and Van Jacobson of Lawrence Berkeley Laboratory. */ +/* 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_msg *)(buf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr)); + dhcpv6 = (struct dhcpv6_relay_msg *)(buf + sizeof(struct ethhdr) + sizeof(struct ip6_hdr) + sizeof(struct udphdr)); - fprintf(stderr,"ip6.ip6_plen=%hd, udp.len=%hd, dhcpv6.msg_type=%hhd\n", - ntohs(ip6->ip6_plen), ntohs(udp->len), dhcpv6->msg_type); + 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); } -- cgit v1.2.3