1 files changed, 147 insertions, 101 deletions

diff --git a/src/net/icmpv6.c b/src/net/icmpv6.c
index 7242380..54426be 100644
--- a/src/net/icmpv6.c
+++ b/src/net/icmpv6.c
@@ -1,126 +1,172 @@
-#include <stdint.h>
+/*
+ * Copyright (C) 2013 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER );
+
 #include <string.h>
-#include <byteswap.h>
 #include <errno.h>
+#include <byteswap.h>
 #include <ipxe/in.h>
-#include <ipxe/ip6.h>
-#include <ipxe/if_ether.h>
 #include <ipxe/iobuf.h>
-#include <ipxe/ndp.h>
-#include <ipxe/icmp6.h>
 #include <ipxe/tcpip.h>
-#include <ipxe/netdevice.h>
+#include <ipxe/icmpv6.h>
+
+/** @file
+ *
+ * ICMPv6 protocol
+ *
+ */
 
 /**
- * Send neighbour solicitation packet
+ * Process received ICMPv6 echo request packet
  *
- * @v netdev	Network device
- * @v src	Source address
- * @v dest	Destination address
+ * @v iobuf		I/O buffer
+ * @v netdev		Network device
+ * @v sin6_src		Source socket address
+ * @v sin6_dest		Destination socket address
+ * @ret rc		Return status code
+ */
+static int icmpv6_rx_echo ( struct io_buffer *iobuf,
+			    struct net_device *netdev,
+			    struct sockaddr_in6 *sin6_src,
+			    struct sockaddr_in6 *sin6_dest __unused ) {
+	struct sockaddr_tcpip *st_src =
+		( ( struct sockaddr_tcpip * ) sin6_src );
+	struct icmpv6_echo *echo = iobuf->data;
+	size_t len = iob_len ( iobuf );
+	int rc;
+
+	/* Sanity check */
+	if ( iob_len ( iobuf ) < sizeof ( *echo ) ) {
+		DBGC ( netdev, "ICMPv6 echo request too short at %zd bytes "
+		       "(min %zd bytes)\n", iob_len ( iobuf ),
+		       sizeof ( *echo ) );
+		rc = -EINVAL;
+		goto done;
+	}
+	DBGC ( netdev, "ICMPv6 echo request from %s (id %#04x seq %#04x)\n",
+	       inet6_ntoa ( &sin6_dest->sin6_addr ), ntohs ( echo->ident ),
+	       ntohs ( echo->sequence ) );
+
+	/* Convert echo request to echo reply and recalculate checksum */
+	echo->icmp.type = ICMPV6_ECHO_REPLY;
+	echo->icmp.chksum = 0;
+	echo->icmp.chksum = tcpip_chksum ( echo, len );
+
+	/* Transmit echo reply */
+	if ( ( rc = tcpip_tx ( iob_disown ( iobuf ), &icmpv6_protocol, NULL,
+			       st_src, netdev, &echo->icmp.chksum ) ) != 0 ) {
+		DBGC ( netdev, "ICMPv6 could not transmit reply: %s\n",
+		       strerror ( rc ) );
+		goto done;
+	}
+
+ done:
+	free_iob ( iobuf );
+	return rc;
+}
+
+/** ICMPv6 echo request handlers */
+struct icmpv6_handler icmpv6_echo_handler __icmpv6_handler = {
+	.type = ICMPV6_ECHO_REQUEST,
+	.rx = icmpv6_rx_echo,
+};
+
+/**
+ * Identify ICMPv6 handler
  *
- * This function prepares a neighbour solicitation packet and sends it to the
- * network layer.
+ * @v type		ICMPv6 type
+ * @ret handler		ICMPv6 handler, or NULL if not found
  */
-int icmp6_send_solicit ( struct net_device *netdev, struct in6_addr *src __unused,
-			 struct in6_addr *dest ) {
-	union {
-		struct sockaddr_in6 sin6;
-		struct sockaddr_tcpip st;
-	} st_dest;
-	struct ll_protocol *ll_protocol = netdev->ll_protocol;
-	struct neighbour_solicit *nsolicit;
-	struct io_buffer *iobuf = alloc_iob ( sizeof ( *nsolicit ) + MIN_IOB_LEN );
-	iob_reserve ( iobuf, MAX_HDR_LEN );
-	nsolicit = iob_put ( iobuf, sizeof ( *nsolicit ) );
-
-	/* Fill up the headers */
-	memset ( nsolicit, 0, sizeof ( *nsolicit ) );
-	nsolicit->type = ICMP6_NSOLICIT;
-	nsolicit->code = 0;
-	nsolicit->target = *dest;
-	nsolicit->opt_type = 1;
-	nsolicit->opt_len = ( 2 + ll_protocol->ll_addr_len ) / 8;
-	memcpy ( nsolicit->opt_ll_addr, netdev->ll_addr,
-				netdev->ll_protocol->ll_addr_len );
-	/* Partial checksum */
-	nsolicit->csum = 0;
-	nsolicit->csum = tcpip_chksum ( nsolicit, sizeof ( *nsolicit ) );
-
-	/* Solicited multicast address */
-	st_dest.sin6.sin6_family = AF_INET6;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr8[0] = 0xff;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr8[2] = 0x02;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr16[1] = 0x0000;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr32[1] = 0x00000000;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr16[4] = 0x0000;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr16[5] = 0x0001;
-	st_dest.sin6.sin6_addr.in6_u.u6_addr32[3] = dest->in6_u.u6_addr32[3];
-	st_dest.sin6.sin6_addr.in6_u.u6_addr8[13] = 0xff;
-	
-	/* Send packet over IP6 */
-	return tcpip_tx ( iobuf, &icmp6_protocol, NULL, &st_dest.st,
-			  NULL, &nsolicit->csum );
+static struct icmpv6_handler * icmpv6_handler ( unsigned int type ) {
+	struct icmpv6_handler *handler;
+
+	for_each_table_entry ( handler, ICMPV6_HANDLERS ) {
+		if ( handler->type == type )
+			return handler;
+	}
+	return NULL;
 }
 
 /**
- * Process ICMP6 headers
+ * Process a received packet
  *
- * @v iobuf	I/O buffer
- * @v st_src	Source address
- * @v st_dest	Destination address
+ * @v iobuf		I/O buffer
+ * @v netdev		Network device
+ * @v st_src		Partially-filled source address
+ * @v st_dest		Partially-filled destination address
+ * @v pshdr_csum	Pseudo-header checksum
+ * @ret rc		Return status code
  */
-static int icmp6_rx ( struct io_buffer *iobuf, struct net_device *netdev __unused, struct sockaddr_tcpip *st_src,
-		      struct sockaddr_tcpip *st_dest, __unused uint16_t pshdr_csum ) {
-	struct icmp6_header *icmp6hdr = iobuf->data;
+static int icmpv6_rx ( struct io_buffer *iobuf, struct net_device *netdev,
+		       struct sockaddr_tcpip *st_src,
+		       struct sockaddr_tcpip *st_dest, uint16_t pshdr_csum ) {
+	struct sockaddr_in6 *sin6_src = ( ( struct sockaddr_in6 * ) st_src );
+	struct sockaddr_in6 *sin6_dest = ( ( struct sockaddr_in6 * ) st_dest );
+	struct icmpv6_header *icmp = iobuf->data;
+	size_t len = iob_len ( iobuf );
+	struct icmpv6_handler *handler;
+	unsigned int csum;
+	int rc;
 
 	/* Sanity check */
-	if ( iob_len ( iobuf ) < sizeof ( *icmp6hdr ) ) {
-		DBG ( "Packet too short (%zd bytes)\n", iob_len ( iobuf ) );
-		free_iob ( iobuf );
-		return -EINVAL;
+	if ( len < sizeof ( *icmp ) ) {
+		DBGC ( netdev, "ICMPv6 packet too short at %zd bytes (min %zd "
+		       "bytes)\n", len, sizeof ( *icmp ) );
+		rc = -EINVAL;
+		goto done;
 	}
 
-	/* TODO: Verify checksum */
+	/* Verify checksum */
+	csum = tcpip_continue_chksum ( pshdr_csum, icmp, len );
+	if ( csum != 0 ) {
+		DBGC ( netdev, "ICMPv6 checksum incorrect (is %04x, should be "
+		       "0000)\n", csum );
+		DBGC_HDA ( netdev, 0, icmp, len );
+		rc = -EINVAL;
+		goto done;
+	}
 
-	/* Process the ICMP header */
-	switch ( icmp6hdr->type ) {
-	case ICMP6_NADVERT:
-		return ndp_process_advert ( iobuf, st_src, st_dest );
+	/* Identify handler */
+	handler = icmpv6_handler ( icmp->type );
+	if ( ! handler ) {
+		DBGC ( netdev, "ICMPv6 unrecognised type %d\n", icmp->type );
+		rc = -ENOTSUP;
+		goto done;
+	}
+
+	/* Pass to handler */
+	if ( ( rc = handler->rx ( iob_disown ( iobuf ), netdev, sin6_src,
+				  sin6_dest ) ) != 0 ) {
+		DBGC ( netdev, "ICMPv6 could not handle type %d: %s\n",
+		       icmp->type, strerror ( rc ) );
+		goto done;
 	}
-	return -ENOSYS;
-}
 
-#if 0
-void icmp6_test_nadvert (struct net_device *netdev, struct sockaddr_in6 *server_p, char *ll_addr) {
-
-		struct sockaddr_in6 server;
-		memcpy ( &server, server_p, sizeof ( server ) );
-                struct io_buffer *rxiobuf = alloc_iob ( 500 );
-                iob_reserve ( rxiobuf, MAX_HDR_LEN );
-                struct neighbour_advert *nadvert = iob_put ( rxiobuf, sizeof ( *nadvert ) );
-                nadvert->type = 136;
-                nadvert->code = 0;
-                nadvert->flags = ICMP6_FLAGS_SOLICITED;
-		nadvert->csum = 0xffff;
-		nadvert->target = server.sin6_addr;
-                nadvert->opt_type = 2;
-                nadvert->opt_len = 1;
-                memcpy ( nadvert->opt_ll_addr, ll_addr, 6 );
-                struct ip6_header *ip6hdr = iob_push ( rxiobuf, sizeof ( *ip6hdr ) );
-                ip6hdr->ver_traffic_class_flow_label = htonl ( 0x60000000 );
-		ip6hdr->hop_limit = 255;
-		ip6hdr->nxt_hdr = 58;
-		ip6hdr->payload_len = htons ( sizeof ( *nadvert ) );
-                ip6hdr->src = server.sin6_addr;
-                ip6hdr->dest = server.sin6_addr;
-		hex_dump ( rxiobuf->data, iob_len ( rxiobuf ) );
-                net_rx ( rxiobuf, netdev, htons ( ETH_P_IPV6 ), ll_addr );
+ done:
+	free_iob ( iobuf );
+	return rc;
 }
-#endif
 
-/** ICMP6 protocol */
-struct tcpip_protocol icmp6_protocol __tcpip_protocol = {
-	.name = "ICMP6",
-	.rx = icmp6_rx,
-	.tcpip_proto = IP_ICMP6, // 58
+/** ICMPv6 TCP/IP protocol */
+struct tcpip_protocol icmpv6_protocol __tcpip_protocol = {
+	.name = "ICMPv6",
+	.rx = icmpv6_rx,
+	.tcpip_proto = IP_ICMP6,
 };