src/inet6.c

// Copyright 2016 Google Inc. All Rights Reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files
// (the "Software"), to deal in the Software without restriction,
// including without limitation the rights to use, copy, modify, merge,
// publish, distribute, sublicense, and/or sell copies of the Software,
// and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
// IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
// TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include <string.h>
#include <stdint.h>
#include <stdio.h>

#include <inet6.h>

#if 1
#define BAD(n) do { printf("error: %s\n", n); return; } while (0)
#else
#define BAD(n) do { return; } while (0)
#endif


// useful addresses
const ip6_addr ip6_ll_all_nodes = {
	.x = { 0xFF, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
};
const ip6_addr ip6_ll_all_routers = {
	.x = { 0xFF, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2 },
};

// Convert MAC Address to IPv6 Link Local Address
// aa:bb:cc:dd:ee:ff => FF80::aabb:ccFF:FEdd:eeff
// bit 2 (U/L) of the mac is inverted
void ll6addr_from_mac(ip6_addr *_ip, const mac_addr *_mac) {
	uint8_t *ip = _ip->x;
	const uint8_t *mac = _mac->x;
	memset(ip, 0, IP6_ADDR_LEN);
	ip[0] = 0xFE;
	ip[1] = 0x80;
	memset(ip + 2, 0, 6);
	ip[8] = mac[0] ^ 2;
	ip[9] = mac[1];
	ip[10] = mac[2];
	ip[11] = 0xFF;
	ip[12] = 0xFE;
	ip[13] = mac[3];
	ip[14] = mac[4];
	ip[15] = mac[5];
}

// Convert MAC Address to IPv6 Solicit Neighbor Multicast Address
// aa:bb:cc:dd:ee:ff -> FF02::1:FFdd:eeff
void snmaddr_from_mac(ip6_addr *_ip, const mac_addr *_mac) {
	uint8_t *ip = _ip->x;
	const uint8_t *mac = _mac->x;
	ip[0] = 0xFF;
	ip[1] = 0x02;
	memset(ip + 2, 0, 9);
	ip[11] = 0x01;
	ip[12] = 0xFF;
	ip[13] = mac[3];
	ip[14] = mac[4];
	ip[15] = mac[5];
}

// Convert IPv6 Multicast Address to Ethernet Multicast Address
void multicast_from_ip6(mac_addr *_mac, const ip6_addr *_ip6) {
	const uint8_t *ip = _ip6->x;
	uint8_t *mac = _mac->x;
	mac[0] = 0x33;
	mac[1] = 0x33;
	mac[2] = ip[12];
	mac[3] = ip[13];
	mac[4] = ip[14];
	mac[5] = ip[15];
}

// ip6 stack configuration
mac_addr ll_mac_addr;
ip6_addr ll_ip6_addr;
mac_addr snm_mac_addr;
ip6_addr snm_ip6_addr;

// cache for the last source addresses we've seen
static mac_addr rx_mac_addr;
static ip6_addr rx_ip6_addr;

void ip6_init(void *macaddr) {
	char tmp[IP6TOAMAX];
	mac_addr all;

	// save our ethernet MAC and synthesize link layer addresses
	memcpy(&ll_mac_addr, macaddr, 6);
	ll6addr_from_mac(&ll_ip6_addr, &ll_mac_addr);
	snmaddr_from_mac(&snm_ip6_addr, &ll_mac_addr);
	multicast_from_ip6(&snm_mac_addr, &snm_ip6_addr);

	eth_add_mcast_filter(&snm_mac_addr);

	multicast_from_ip6(&all, &ip6_ll_all_nodes);
	eth_add_mcast_filter(&all);

	printf("macaddr: %02x:%02x:%02x:%02x:%02x:%02x\n",
		ll_mac_addr.x[0], ll_mac_addr.x[1], ll_mac_addr.x[2],
		ll_mac_addr.x[3], ll_mac_addr.x[4], ll_mac_addr.x[5]);
	printf("ip6addr: %s\n", ip6toa(tmp, &ll_ip6_addr));
	printf("snmaddr: %s\n", ip6toa(tmp, &snm_ip6_addr));
}

static int resolve_ip6(mac_addr *_mac, const ip6_addr *_ip) {
	const uint8_t *ip = _ip->x;

	// Multicast addresses are a simple transform
	if (ip[0] == 0xFF) {
		multicast_from_ip6(_mac, _ip);
		return 0;
	}

	// Trying to send to the IP that we last received a packet from?
	// Assume their mac address has not changed
	if (memcmp(_ip, &rx_ip6_addr, sizeof(rx_ip6_addr)) == 0) {
		memcpy(_mac, &rx_mac_addr, sizeof(rx_mac_addr));
		return 0;
	}

	// We don't know how to find peers or routers yet, so give up...
	return -1;
}

static uint16_t checksum(const void *_data, size_t len, uint16_t _sum) {
	uint32_t sum = _sum;
	const uint16_t *data = _data;
	while (len > 1) {
		sum += *data++;
		len -= 2;
	}
	if (len) {
		sum += (*data & 0xFF);
	}
	return sum + (sum >> 16);
}

typedef struct {
	uint8_t eth[16];
	ip6_hdr ip6;
	uint8_t data[0];
} ip6_pkt;

typedef struct {
	uint8_t eth[16];
	ip6_hdr ip6;
	udp_hdr udp;
	uint8_t data[0];
} udp_pkt;

static unsigned ip6_checksum(ip6_hdr *ip, unsigned type, size_t length) {
	uint16_t sum;

	// length and protocol field for pseudo-header
	sum = checksum(&ip->length, 2, htons(type));
	// src/dst for pseudo-header + payload
	sum = checksum(ip->src, 32 + length, sum);

	// 0 is illegal, so 0xffff remains 0xffff
	if (sum != 0xffff) {
		return ~sum;
	} else {
		return sum;
	}
}

static int ip6_setup(ip6_pkt *p, const ip6_addr *daddr, size_t length, uint8_t type) {
	mac_addr dmac;

	if (resolve_ip6(&dmac, daddr)) return -1;

	// ethernet header
	memcpy(p->eth + 2, &dmac, ETH_ADDR_LEN);
	memcpy(p->eth + 8, &ll_mac_addr, ETH_ADDR_LEN);
	p->eth[14] = (ETH_IP6 >> 8) & 0xFF;
	p->eth[15] = ETH_IP6 & 0xFF;

	// ip6 header
	p->ip6.ver_tc_flow = 0x60; // v=6, tc=0, flow=0
	p->ip6.length = htons(length);
	p->ip6.next_header = type;
	p->ip6.hop_limit = 255;
	memcpy(p->ip6.src, &ll_ip6_addr, sizeof(ip6_addr));
	memcpy(p->ip6.dst, daddr, sizeof(ip6_addr));

	return 0;
}

#define UDP6_MAX_PAYLOAD (ETH_MTU - ETH_HDR_LEN - IP6_HDR_LEN - UDP_HDR_LEN)

int udp6_send(const void *data, size_t dlen, const ip6_addr *daddr, uint16_t dport, uint16_t sport) {
	size_t length = dlen + UDP_HDR_LEN;
	udp_pkt *p = eth_get_buffer(ETH_MTU + 2);

	if (p == 0) return -1;
	if (dlen > UDP6_MAX_PAYLOAD) goto fail;
	if (ip6_setup((void*) p, daddr, length, HDR_UDP)) goto fail;

	// udp header
	p->udp.src_port = htons(sport);
	p->udp.dst_port = htons(dport);
	p->udp.length = htons(length);
	p->udp.checksum = 0;

	memcpy(p->data, data, dlen);
	p->udp.checksum = ip6_checksum(&p->ip6, HDR_UDP, length);
	return eth_send(p->eth + 2, ETH_HDR_LEN + IP6_HDR_LEN + length);

fail:
	eth_put_buffer(p);
	return -1;
}

#define ICMP6_MAX_PAYLOAD (ETH_MTU - ETH_HDR_LEN - IP6_HDR_LEN)

static int icmp6_send(const void *data, size_t length, const ip6_addr *daddr) {
	ip6_pkt *p;
	icmp6_hdr *icmp;

	p = eth_get_buffer(ETH_MTU + 2);
	if (p == 0) return -1;
	if (length > ICMP6_MAX_PAYLOAD) goto fail;
	if (ip6_setup(p, daddr, length, HDR_ICMP6)) goto fail;

	icmp = (void*) p->data;
	memcpy(icmp, data, length);
	icmp->checksum = ip6_checksum(&p->ip6, HDR_ICMP6, length);
	return eth_send(p->eth + 2, ETH_HDR_LEN + IP6_HDR_LEN + length);

fail:
	eth_put_buffer(p);
	return -1;
}

void _udp6_recv(ip6_hdr *ip, void *_data, size_t len) {
	udp_hdr *udp = _data;
	uint16_t sum, n;

	if (len < UDP_HDR_LEN) BAD("Bogus Header Len");
	if (udp->checksum == 0) BAD("Checksum Invalid");
	if (udp->checksum == 0xFFFF) udp->checksum = 0;

	sum = checksum(&ip->length, 2, htons(HDR_UDP));
	sum = checksum(ip->src, 32 + len, sum);
	if (sum != 0xFFFF) BAD("Checksum Incorrect");

	n = ntohs(udp->length);
	if (n < UDP_HDR_LEN) BAD("Bogus Header Len");
	if (n > len) BAD("Packet Too Short");
	len = n - UDP_HDR_LEN;

	udp6_recv((uint8_t*) _data + UDP_HDR_LEN, len,
		(void*) ip->dst, ntohs(udp->dst_port),
		(void*) ip->src, ntohs(udp->src_port));
}

void icmp6_recv(ip6_hdr *ip, void *_data, size_t len) {
	icmp6_hdr *icmp = _data;
	uint16_t sum;

	if (icmp->checksum == 0) BAD("Checksum Invalid");
	if (icmp->checksum == 0xFFFF) icmp->checksum = 0;

	sum = checksum(&ip->length, 2, htons(HDR_ICMP6));
	sum = checksum(ip->src, 32 + len, sum);
	if (sum != 0xFFFF) BAD("Checksum Incorrect");

	if (icmp->type == ICMP6_NDP_N_SOLICIT) {
		ndp_n_hdr *ndp = _data;
		struct {
			ndp_n_hdr hdr;
			uint8_t opt[8];
		} msg;

		if (len < sizeof(ndp_n_hdr)) BAD("Bogus NDP Message");
		if (ndp->code != 0) BAD("Bogus NDP Code");
		if (memcmp(ndp->target, &ll_ip6_addr, IP6_ADDR_LEN)) BAD("NDP Not For Me");

		msg.hdr.type = ICMP6_NDP_N_ADVERTISE;
		msg.hdr.code = 0;
		msg.hdr.checksum = 0;
		msg.hdr.flags = 0x60; // (S)olicited and (O)verride flags
		memcpy(msg.hdr.target, &ll_ip6_addr, IP6_ADDR_LEN);
		msg.opt[0] = NDP_N_TGT_LL_ADDR;
		msg.opt[1] = 1;
		memcpy(msg.opt + 2, &ll_mac_addr, ETH_ADDR_LEN);

		icmp6_send(&msg, sizeof(msg), (void*) ip->src);
		return;
	}

	if (icmp->type == ICMP6_ECHO_REQUEST) {
		icmp->checksum = 0;
		icmp->type = ICMP6_ECHO_REPLY;
		icmp6_send(_data, len, (void*) ip->src);
		return;
	}

	BAD("ICMP6 Unhandled");
}

void eth_recv(void *_data, size_t len) {
	uint8_t *data = _data;
	ip6_hdr *ip;
	uint32_t n;

	if (len < (ETH_HDR_LEN + IP6_HDR_LEN)) BAD("Bogus Header Len");
	if (data[12] != (ETH_IP6 >> 8)) return;
	if (data[13] != (ETH_IP6 & 0xFF)) return;

	ip = (void*) (data + ETH_HDR_LEN);
	data += (ETH_HDR_LEN + IP6_HDR_LEN);
	len -= (ETH_HDR_LEN + IP6_HDR_LEN);

	// require v6
	if ((ip->ver_tc_flow & 0xF0) != 0x60) BAD("Unknown IP6 Version");

	// ensure length is sane
	n = ntohs(ip->length);
	if (n > len) BAD("IP6 Length Mismatch");

	// ignore any trailing data in the ethernet frame
	len = n;

	// require that we are the destination
	if (memcmp(&ll_ip6_addr, ip->dst, IP6_ADDR_LEN) &&
		memcmp(&snm_ip6_addr, ip->dst, IP6_ADDR_LEN)) {
		return;
	}

	// stash the sender's info to simplify replies
	memcpy(&rx_mac_addr, (uint8_t*) _data + 6, ETH_ADDR_LEN);
	memcpy(&rx_ip6_addr, ip->src, IP6_ADDR_LEN);

	if (ip->next_header == HDR_ICMP6) {
		icmp6_recv(ip, data, len);
		return;
	}

	if (ip->next_header == HDR_UDP) {
		_udp6_recv(ip, data, len);
		return;
	}

	BAD("Unhandled IP6");
}

char *ip6toa(char *_out, void *ip6addr) {
	const uint8_t *x = ip6addr;
	const uint8_t *end = x + 16;
	char *out = _out;
	uint16_t n;

	n = (x[0] << 8) | x[1];
	while ((n == 0) && (x < end)) {
		x += 2;
		n = (x[0] << 8) | x[1];
	}

	if ((end - x) < 16) {
		if (end == x) {
			// all 0s - special case
			sprintf(out, "::");
			return _out;
		}
		// we consumed some number of leading 0s
		out += sprintf(out, ":");
		while (x < end) {
			out += sprintf(out, ":%x", n);
			x += 2;
			n = (x[0] << 8) | x[1];
		}
		return _out;
	}

	while (x < (end - 2)) {
		out += sprintf(out, "%x:", n);
		x += 2;
		n = (x[0] << 8) | x[1];
		if (n == 0) goto middle_zeros;
	}
	out += sprintf(out, "%x", n);
	return _out;

middle_zeros:
	while ((n == 0) && (x < end)) {
		x += 2;
		n = (x[0] << 8) | x[1];
	}
	if (x == end) {
		out += sprintf(out, ":");
		return _out;
	}
	while (x < end) {
		out += sprintf(out, ":%x", n);
		x += 2;
		n = (x[0] << 8) | x[1];
	}
	return _out;
}