IP6 support

1 files changed, 309 insertions, 14 deletions

diff --git a/src/net/ipv6.c b/src/net/ipv6.c
index 38705aa0..d00562a0 100644
--- a/src/net/ipv6.c
+++ b/src/net/ipv6.c
@@ -1,42 +1,337 @@
 #include <errno.h>
-#include <stdlib.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdlib.h>
+#include <malloc.h>
+#include <vsprintf.h>
 #include <byteswap.h>
+#include <gpxe/in.h>
+#include <gpxe/ip6.h>
+#include <gpxe/ndp.h>
+#include <gpxe/list.h>
+#include <gpxe/icmp6.h>
+#include <gpxe/tcpip.h>
+#include <gpxe/socket.h>
 #include <gpxe/pkbuff.h>
 #include <gpxe/netdevice.h>
-#include <gpxe/in.h>
 #include <gpxe/if_ether.h>
-#include <gpxe/tcpip.h>
+
+struct net_protocol ipv6_protocol;
+
+/* Unspecified IP6 address */
+static struct in6_addr ip6_none = {
+        .in6_u.u6_addr32[0] = 0,
+        .in6_u.u6_addr32[1] = 0,
+        .in6_u.u6_addr32[2] = 0,
+        .in6_u.u6_addr32[3] = 0,
+};
+
+/** An IPv6 routing table entry */
+struct ipv6_miniroute {
+	/* List of miniroutes */
+	struct list_head list;
+	/* Network device */
+	struct net_device *netdev;
+	/* Destination prefix */
+	struct in6_addr prefix;
+	/* Prefix length */
+	int prefix_len;
+	/* IPv6 address of interface */
+	struct in6_addr address;
+	/* Gateway address */
+	struct in6_addr gateway;
+};
+
+/** List of IPv6 miniroutes */
+static LIST_HEAD ( miniroutes );
 
 /**
- * Transmit IP6 packets
+ * Add IPv6 interface
+ *
+ * @v netdev	Network device
+ * @v prefix	Destination prefix
+ * @v address	Address of the interface
+ * @v gateway	Gateway address (or ::0 for no gateway)
+ */
+int add_ipv6_address ( struct net_device *netdev, struct in6_addr prefix,
+		       int prefix_len, struct in6_addr address,
+		       struct in6_addr gateway ) {
+	struct ipv6_miniroute *miniroute;
+
+	miniroute = malloc ( sizeof ( *miniroute ) );
+	if ( !miniroute ) {
+		DBG ( "Not enough memory\n" );
+		return -ENOMEM;
+	}
+	miniroute->netdev = netdev;
+	miniroute->prefix = prefix;
+	miniroute->prefix_len = prefix_len;
+	miniroute->address = address;
+	miniroute->gateway = gateway;
+
+	/* Add miniroute to list of miniroutes */
+	if ( !IP6_EQUAL ( gateway, ip6_none ) ) {
+		list_add_tail ( &miniroute->list, &miniroutes );
+	} else {
+		list_add ( &miniroute->list, &miniroutes );
+	}
+	return 0;
+}
+
+/**
+ * Remove IPv6 interface
+ *
+ * @v netdev	Network device
+ */
+void del_ipv6_address ( struct net_device *netdev ) {
+	struct ipv6_miniroute *miniroute;
+
+	list_for_each_entry ( miniroute, &miniroutes, list ) {
+		if ( miniroute->netdev == netdev ) {
+			list_del ( &miniroute->list );
+			break;
+		}
+	}
+}
+
+/**
+ * Calculate TCPIP checksum
+ *
+ * @v pkb	Packet buffer
+ * @v tcpip	TCP/IP protocol
+ *
+ * This function constructs the pseudo header and completes the checksum in the
+ * upper layer header.
+ */
+static void ipv6_tx_csum ( struct pk_buff *pkb, struct tcpip_protocol *tcpip ) {
+	struct ip6_header *ip6hdr = pkb->data;
+	struct ipv6_pseudo_header pshdr;
+	uint16_t *csum = ( ( ( void * ) ip6hdr ) + sizeof ( *ip6hdr ) +
+			tcpip->csum_offset );
+
+	/* Calculate pseudo header */
+	memset ( &pshdr, 0, sizeof ( pshdr ) );
+	pshdr.src = ip6hdr->src;
+	pshdr.dest = ip6hdr->dest;
+	pshdr.len = htons ( pkb_len ( pkb ) - sizeof ( *ip6hdr ) );
+	pshdr.nxt_hdr = ip6hdr->nxt_hdr;
+
+	/* Update checksum value */
+	*csum = tcpip_continue_chksum ( *csum, &pshdr, sizeof ( pshdr ) );
+}
+
+/**
+ * Dump IP6 header for debugging
+ *
+ * ip6hdr	IPv6 header
+ */
+void ipv6_dump ( struct ip6_header *ip6hdr ) {
+	DBG ( "IP6 %p src %s dest %s nxt_hdr %d len %d\n", ip6hdr,
+	      inet6_ntoa ( ip6hdr->src ), inet6_ntoa ( ip6hdr->dest ),
+	      ip6hdr->nxt_hdr, ntohs ( ip6hdr->payload_len ) );
+}
+
+/**
+ * Transmit IP6 packet
+ *
+ * pkb		Packet buffer
+ * tcpip	TCP/IP protocol
+ * st_dest	Destination socket address
+ *
+ * This function prepends the IPv6 headers to the payload an transmits it.
  */
 static int ipv6_tx ( struct pk_buff *pkb,
-		     struct tcpip_protocol *tcpip_protocol,
+		     struct tcpip_protocol *tcpip,
 		     struct sockaddr_tcpip *st_dest ) {
-	return -ENOSYS;
+	struct sockaddr_in6 *dest = ( struct sockaddr_in6* ) st_dest;
+	struct in6_addr next_hop;
+	struct ipv6_miniroute *miniroute;
+	struct net_device *netdev = NULL;
+	uint8_t ll_dest_buf[MAX_LL_ADDR_LEN];
+	const uint8_t *ll_dest = ll_dest_buf;
+	int rc;
+
+	/* Construct the IPv6 packet */
+	struct ip6_header *ip6hdr = pkb_push ( pkb, sizeof ( *ip6hdr ) );
+	memset ( ip6hdr, 0, sizeof ( *ip6hdr) );
+	ip6hdr->ver_traffic_class_flow_label = htonl ( 0x60000000 );//IP6_VERSION;
+	ip6hdr->payload_len = htons ( pkb_len ( pkb ) - sizeof ( *ip6hdr ) );
+	ip6hdr->nxt_hdr = tcpip->tcpip_proto;
+	ip6hdr->hop_limit = IP6_HOP_LIMIT; // 255
+
+	/* Determine the next hop address and interface
+	 *
+	 * TODO: Implement the routing table.
+	 */
+	next_hop = dest->sin6_addr;
+	list_for_each_entry ( miniroute, &miniroutes, list ) {
+		if ( ( strncmp ( &ip6hdr->dest, &miniroute->prefix,
+					miniroute->prefix_len ) == 0 ) ||
+		     ( IP6_EQUAL ( miniroute->gateway, ip6_none ) ) ) {
+			netdev = miniroute->netdev;
+			ip6hdr->src = miniroute->address;
+			if ( ! ( IS_UNSPECIFIED ( miniroute->gateway ) ) ) {
+				next_hop = miniroute->gateway;
+			}
+			break;
+		}
+	}
+	/* No network interface identified */
+	if ( !netdev ) {
+		DBG ( "No route to host %s\n", inet6_ntoa ( ip6hdr->dest ) );
+		rc = -EHOSTUNREACH;
+		goto err;
+	}
+
+	/* Complete the transport layer checksum */
+	if ( tcpip->csum_offset > 0 ) {
+		ipv6_tx_csum ( pkb, tcpip );
+	}
+
+	/* Print IPv6 header */
+	ipv6_dump ( ip6hdr );
+	
+	/* Resolve link layer address */
+	if ( next_hop.in6_u.u6_addr8[0] == 0xff ) {
+		ll_dest_buf[0] = 0x33;
+		ll_dest_buf[1] = 0x33;
+		ll_dest_buf[2] = next_hop.in6_u.u6_addr8[12];
+		ll_dest_buf[3] = next_hop.in6_u.u6_addr8[13];
+		ll_dest_buf[4] = next_hop.in6_u.u6_addr8[14];
+		ll_dest_buf[5] = next_hop.in6_u.u6_addr8[15];
+	} else {
+		/* Unicast address needs to be resolved by NDP */
+		if ( ( rc = ndp_resolve ( netdev, &next_hop, &ip6hdr->src,
+					  ll_dest_buf ) ) != 0 ) {
+			DBG ( "No entry for %s\n", inet6_ntoa ( next_hop ) );
+			goto err;
+		}
+	}
+
+	/* Transmit packet */
+	return net_tx ( pkb, netdev, &ipv6_protocol, ll_dest );
+
+  err:
+	free_pkb ( pkb );
+	return rc;
+}
+
+/**
+ * Process next IP6 header
+ *
+ * @v pkb	Packet buffer
+ * @v nxt_hdr	Next header number
+ * @v src	Source socket address
+ * @v dest	Destination socket address
+ *
+ * Refer http://www.iana.org/assignments/ipv6-parameters for the numbers
+ */
+static int ipv6_process_nxt_hdr ( struct pk_buff *pkb, uint8_t nxt_hdr,
+		struct sockaddr_tcpip *src, struct sockaddr_tcpip *dest ) {
+	switch ( nxt_hdr ) {
+	case IP6_HOPBYHOP: 
+	case IP6_ROUTING: 
+	case IP6_FRAGMENT: 
+	case IP6_AUTHENTICATION: 
+	case IP6_DEST_OPTS: 
+	case IP6_ESP: 
+		DBG ( "Function not implemented for header %d\n", nxt_hdr );
+		return -ENOSYS;
+	case IP6_ICMP6: 
+		break;
+	case IP6_NO_HEADER: 
+		DBG ( "No next header\n" );
+		return 0;
+	}
+	/* Next header is not a IPv6 extension header */
+	return tcpip_rx ( pkb, nxt_hdr, src, dest );
 }
 
 /**
  * Process incoming IP6 packets
  *
- * Placeholder function. Should rewrite in net/ipv6.c
+ * @v pkb		Packet buffer
+ * @v netdev		Network device
+ * @v ll_source		Link-layer source address
+ *
+ * This function processes a IPv6 packet
  */
-static int ipv6_rx ( struct pk_buff *pkb __unused,
-		     struct net_device *netdev __unused,
-		     const void *ll_source __unused ) {
-	return -ENOSYS;
+static int ipv6_rx ( struct pk_buff *pkb,
+		     struct net_device *netdev,
+		     const void *ll_source ) {
+
+	struct ip6_header *ip6hdr = pkb->data;
+	union {
+		struct sockaddr_in6 sin6;
+		struct sockaddr_tcpip st;
+	} src, dest;
+
+	/* Sanity check */
+	if ( pkb_len ( pkb ) < sizeof ( *ip6hdr ) ) {
+		DBG ( "Packet too short (%d bytes)\n", pkb_len ( pkb ) );
+		goto drop;
+	}
+
+	/* TODO: Verify checksum */
+
+	/* Print IP6 header for debugging */
+	ipv6_dump ( ip6hdr );
+
+	/* Check header version */
+	if ( ip6hdr->ver_traffic_class_flow_label & 0xf0000000 != 0x60000000 ) {
+		DBG ( "Invalid protocol version\n" );
+		goto drop;
+	}
+
+	/* Check the payload length */
+	if ( ntohs ( ip6hdr->payload_len ) > pkb_len ( pkb ) ) {
+		DBG ( "Inconsistent packet length (%d bytes)\n",
+			ip6hdr->payload_len );
+		goto drop;
+	}
+
+	/* Ignore the traffic class and flow control values */
+
+	/* Construct socket address */
+	memset ( &src, 0, sizeof ( src ) );
+	src.sin6.sin_family = AF_INET6;
+	src.sin6.sin6_addr = ip6hdr->src;
+	memset ( &dest, 0, sizeof ( dest ) );
+	dest.sin6.sin_family = AF_INET6;
+	dest.sin6.sin6_addr = ip6hdr->dest;
+
+	/* Strip header */
+	pkb_unput ( pkb, pkb_len ( pkb ) - ntohs ( ip6hdr->payload_len ) -
+							sizeof ( *ip6hdr ) );
+	pkb_pull ( pkb, sizeof ( *ip6hdr ) );
+
+	/* Send it to the transport layer */
+	return ipv6_process_nxt_hdr ( pkb, ip6hdr->nxt_hdr, &src.st, &dest.st );
+
+  drop:
+	DBG ( "Packet dropped\n" );
+	free_pkb ( pkb );
+	return -1;
+}
+
+/**
+ * Print a IP6 address as xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx
+ */
+char * inet6_ntoa ( struct in6_addr in6 ) {
+	static char buf[40];
+	uint16_t *bytes = ( uint16_t* ) &in6;
+	sprintf ( buf, "%x:%x:%x:%x:%x:%x:%x:%x", bytes[0], bytes[1], bytes[2],
+			bytes[3], bytes[4], bytes[5], bytes[6], bytes[7] );
+	return buf;
 }
 
 static const char * ipv6_ntoa ( const void *net_addr ) {
-//	return inet6_ntoa ( * ( ( struct in6_addr * ) net_addr ) );
-	return "no support yet";
+	return inet6_ntoa ( * ( ( struct in6_addr * ) net_addr ) );
 }
 
 /** IPv6 protocol */
 struct net_protocol ipv6_protocol __net_protocol = {
-	.name = "IP6",
+	.name = "IPv6",
 	.net_proto = htons ( ETH_P_IPV6 ),
 	.net_addr_len = sizeof ( struct in6_addr ),
 	.rx = ipv6_rx,