[U-Boot] [RFC PATCH v2 07/11] net: IPv6 support

[Chris Packham <judge.packham@gmail.com>]

Adds basic support for IPv6. Neighbor discovery and ping6 are the only
things supported at the moment.

Helped-by: Hanna Hawa <hannah@marvell.com> [endian & alignment fixes]
Signed-off-by: Chris Packham <judge.packham@gmail.com>

---
Now we have something functional. With this and the next patch you can
do something like 'setenv ipaddr6 3ffe::1/64' and 'ping6 3ffe::2' should
work.

I seem to have a problem that when you send a ping6 for a non-existent
address that ends up stuck and the next non-ipv6 net operation tries to
resolve it. I suspect this is because the pending neighbor discovery
information isn't cleaned up properly, I need to look into that.

Changes in v2:
- split ping6 support into separate patch
- split environment variables into separate patch
- change ip6_ndisc_* to ndisc_*, fix CamelCase

 include/net.h  |   1 +
 include/net6.h | 194 +++++++++++++++++++++++++++++++++++++
 net/Makefile   |   2 +
 net/ndisc.c    | 266 ++++++++++++++++++++++++++++++++++++++++++++++++++
 net/ndisc.h    |  25 +++++
 net/net.c      |  38 +++++++-
 net/net6.c     | 299 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 823 insertions(+), 2 deletions(-)
 create mode 100644 net/ndisc.c
 create mode 100644 net/ndisc.h

diff --git a/include/net.h b/include/net.h
index e8bd2b7..8b7c878 100644
--- a/include/net.h
+++ b/include/net.h
@@ -316,6 +316,7 @@ struct vlan_ethernet_hdr {
 #define VLAN_ETHER_HDR_SIZE	(sizeof(struct vlan_ethernet_hdr))
 
 #define PROT_IP		0x0800		/* IP protocol			*/
+#define PROT_IP6        0x86DD          /* IPv6 protocol		*/
 #define PROT_ARP	0x0806		/* IP ARP protocol		*/
 #define PROT_RARP	0x8035		/* IP ARP protocol		*/
 #define PROT_VLAN	0x8100		/* IEEE 802.1q protocol		*/
diff --git a/include/net6.h b/include/net6.h
index a41eb87..ff97c39 100644
--- a/include/net6.h
+++ b/include/net6.h
@@ -22,6 +22,16 @@ struct in6_addr {
 #define s6_addr32	in6_u.u6_addr32
 };
 
+#define IN6ADDRSZ	sizeof(struct in6_addr)
+#define INETHADDRSZ	sizeof(net_ethaddr)
+
+#define IPV6_ADDRSCOPE_INTF	0x01
+#define IPV6_ADDRSCOPE_LINK	0x02
+#define IPV6_ADDRSCOPE_AMDIN	0x04
+#define IPV6_ADDRSCOPE_SITE	0x05
+#define IPV6_ADDRSCOPE_ORG	0x08
+#define IPV6_ADDRSCOPE_GLOBAL	0x0E
+
 /**
  * struct ipv6hdr - Internet Protocol V6 (IPv6) header.
  *
@@ -45,6 +55,145 @@ struct ip6_hdr {
 	struct in6_addr	daddr;
 };
 
+#define IP6_HDR_SIZE (sizeof(struct ip6_hdr))
+
+/* Handy for static initialisations of struct in6_addr, atlhough the
+ * c99 '= { 0 }' idiom might work depending on you compiler. */
+#define ZERO_IPV6_ADDR { { { 0x00, 0x00, 0x00, 0x00, \
+			  0x00, 0x00, 0x00, 0x00, \
+			  0x00, 0x00, 0x00, 0x00, \
+			  0x00, 0x00, 0x00, 0x00 } } }
+
+#define IPV6_LINK_LOCAL_PREFIX	0xfe80
+
+enum {
+	__ND_OPT_PREFIX_INFO_END	= 0,
+	ND_OPT_SOURCE_LL_ADDR		= 1,
+	ND_OPT_TARGET_LL_ADDR		= 2,
+	ND_OPT_PREFIX_INFO		= 3,
+	ND_OPT_REDIRECT_HDR		= 4,
+	ND_OPT_MTU			= 5,
+	__ND_OPT_MAX
+};
+
+/* ICMPv6 */
+#define IPPROTO_ICMPV6			58
+/* hop limit for neighbour discovery packets */
+#define IPV6_NDISC_HOPLIMIT             255
+#define NDISC_TIMEOUT			5000UL
+#define NDISC_TIMEOUT_COUNT             3
+
+struct icmp6hdr {
+	__u8	icmp6_type;
+#define IPV6_ICMP_ECHO_REQUEST			128
+#define IPV6_ICMP_ECHO_REPLY			129
+#define IPV6_NDISC_ROUTER_SOLICITATION		133
+#define IPV6_NDISC_ROUTER_ADVERTISEMENT		134
+#define IPV6_NDISC_NEIGHBOUR_SOLICITATION	135
+#define IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT	136
+#define IPV6_NDISC_REDIRECT			137
+	__u8	icmp6_code;
+	__be16	icmp6_cksum;
+
+	union {
+		__be32	un_data32[1];
+		__be16	un_data16[2];
+		__u8	un_data8[4];
+
+		struct icmpv6_echo {
+			__be16		identifier;
+			__be16		sequence;
+		} u_echo;
+
+		struct icmpv6_nd_advt {
+#if defined(__LITTLE_ENDIAN_BITFIELD)
+			__be32		reserved:5,
+					override:1,
+					solicited:1,
+					router:1,
+					reserved2:24;
+#elif defined(__BIG_ENDIAN_BITFIELD)
+			__be32		router:1,
+					solicited:1,
+					override:1,
+					reserved:29;
+#else
+#error	"Please fix <asm/byteorder.h>"
+#endif
+		} u_nd_advt;
+
+		struct icmpv6_nd_ra {
+			__u8		hop_limit;
+#if defined(__LITTLE_ENDIAN_BITFIELD)
+			__u8		reserved:6,
+					other:1,
+					managed:1;
+
+#elif defined(__BIG_ENDIAN_BITFIELD)
+			__u8		managed:1,
+					other:1,
+					reserved:6;
+#else
+#error	"Please fix <asm/byteorder.h>"
+#endif
+			__be16		rt_lifetime;
+		} u_nd_ra;
+	} icmp6_dataun;
+#define icmp6_identifier	icmp6_dataun.u_echo.identifier
+#define icmp6_sequence		icmp6_dataun.u_echo.sequence
+#define icmp6_pointer		icmp6_dataun.un_data32[0]
+#define icmp6_mtu		icmp6_dataun.un_data32[0]
+#define icmp6_unused		icmp6_dataun.un_data32[0]
+#define icmp6_maxdelay		icmp6_dataun.un_data16[0]
+#define icmp6_router		icmp6_dataun.u_nd_advt.router
+#define icmp6_solicited		icmp6_dataun.u_nd_advt.solicited
+#define icmp6_override		icmp6_dataun.u_nd_advt.override
+#define icmp6_ndiscreserved	icmp6_dataun.u_nd_advt.reserved
+#define icmp6_hop_limit		icmp6_dataun.u_nd_ra.hop_limit
+#define icmp6_addrconf_managed	icmp6_dataun.u_nd_ra.managed
+#define icmp6_addrconf_other	icmp6_dataun.u_nd_ra.other
+#define icmp6_rt_lifetime	icmp6_dataun.u_nd_ra.rt_lifetime
+};
+
+struct nd_msg {
+	struct icmp6hdr	icmph;
+	struct in6_addr	target;
+	__u8		opt[0];
+};
+
+struct rs_msg {
+	struct icmp6hdr	icmph;
+	__u8		opt[0];
+};
+
+struct ra_msg {
+	struct icmp6hdr	icmph;
+	__u32		reachable_time;
+	__u32		retrans_timer;
+};
+
+struct echo_msg {
+	struct icmp6hdr	icmph;
+	__u16		id;
+	__u16		sequence;
+};
+
+struct nd_opt_hdr {
+	__u8		nd_opt_type;
+	__u8		nd_opt_len;
+} __attribute__((__packed__));
+
+extern struct in6_addr const net_null_addr_ip6;	/* NULL IPv6 address */
+extern struct in6_addr net_gateway6;	/* Our gateways IPv6 address */
+extern struct in6_addr net_ip6;	/* Our IPv6 addr (0 = unknown) */
+extern struct in6_addr net_link_local_ip6;	/* Our link local IPv6 addr */
+extern u_int32_t net_prefix_length;	/* Our prefixlength (0 = unknown) */
+extern struct in6_addr net_server_ip6;	/* Server IPv6 addr (0 = unknown) */
+
+#ifdef CONFIG_CMD_PING
+extern struct in6_addr net_ping_ip6;	/* the ipv6 address to ping */
+#endif
+
 /* ::ffff:0:0/96 is reserved for v4 mapped addresses */
 static inline int ipv6_addr_v4mapped(const struct in6_addr *a)
 {
@@ -61,4 +210,49 @@ static inline int ipv6_addr_is_isatap(const struct in6_addr *a)
 /* Convert a string to an ipv6 address */
 int string_to_ip6(const char *s, struct in6_addr *addr);
 
+/* check that an IPv6 address is unspecified (zero) */
+int ip6_is_unspecified_addr(struct in6_addr *addr);
+
+/* check that an IPv6 address is ours */
+int ip6_is_our_addr(struct in6_addr *addr);
+
+void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6]);
+
+void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr);
+
+void ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
+			      struct in6_addr *mcast_addr);
+
+/* check if neighbour is in the same subnet as us */
+int ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
+		       __u32 prefix_length);
+
+unsigned int csum_partial(const unsigned char *buff, int len, unsigned int sum);
+
+unsigned short int csum_ipv6_magic(struct in6_addr *saddr,
+				   struct in6_addr *daddr, __u16 len,
+				   unsigned short proto, unsigned int csum);
+
+int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
+		int nextheader, int hoplimit, int payload_len);
+
+/* sends an IPv6 echo request to a host */
+int ping6_send(void);
+
+/* starts a Ping6 process */
+void ping6_start(void);
+
+/* handles reception of icmpv6 echo request/reply */
+void ping6_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6,
+			  int len);
+
+/* handler for incoming IPv6 echo packet */
+void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
+			    int len);
+
+/* copy IPv6 */
+static inline void net_copy_ip6(void *to, const void *from)
+{
+	memcpy((void *)to, from, sizeof(struct in6_addr));
+}
 #endif /* __NET6_H__ */
diff --git a/net/Makefile b/net/Makefile
index 6da8019..d3534c0 100644
--- a/net/Makefile
+++ b/net/Makefile
@@ -21,3 +21,5 @@ obj-$(CONFIG_CMD_RARP) += rarp.o
 obj-$(CONFIG_CMD_SNTP) += sntp.o
 obj-$(CONFIG_CMD_NET)  += tftp.o
 obj-$(CONFIG_NET6)     += net6.o
+obj-$(CONFIG_NET6)     += ndisc.o
+obj-$(CONFIG_CMD_PING6) += ping6.o
diff --git a/net/ndisc.c b/net/ndisc.c
new file mode 100644
index 0000000..290eaee
--- /dev/null
+++ b/net/ndisc.c
@@ -0,0 +1,266 @@
+/*
+ * net/ndisc.c
+ *
+ * (C) Copyright 2013 Allied Telesis Labs NZ
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <net.h>
+#include <net6.h>
+#include "ndisc.h"
+
+/* IPv6 destination address of packet waiting for ND */
+struct in6_addr net_nd_sol_packet_ip6 = ZERO_IPV6_ADDR;
+/* IPv6 address we are expecting ND advert from */
+static struct in6_addr net_nd_rep_packet_ip6 = ZERO_IPV6_ADDR;
+/* MAC destination address of packet waiting for ND */
+uchar *net_nd_packet_mac;
+/* pointer to packet waiting to be transmitted after ND is resolved */
+uchar *net_nd_tx_packet;
+static uchar net_nd_packet_buf[PKTSIZE_ALIGN + PKTALIGN];
+/* size of packet waiting to be transmitted */
+int net_nd_tx_packet_size;
+/* the timer for ND resolution */
+ulong net_nd_timer_start;
+/* the number of requests we have sent so far */
+int net_nd_try;
+
+#define IP6_NDISC_OPT_SPACE(len) (((len)+2+7)&~7)
+
+/**
+ * Insert an option into a neighbor discovery packet.
+ * Returns the number of bytes inserted (which may be >= len)
+ */
+static int
+ndisc_insert_option(struct nd_msg *ndisc, int type, u8 *data, int len)
+{
+	int space = IP6_NDISC_OPT_SPACE(len);
+
+	ndisc->opt[0] = type;
+	ndisc->opt[1] = space >> 3;
+	memcpy(&ndisc->opt[2], data, len);
+	len += 2;
+
+	/* fill the remainder with 0 */
+	if ((space - len) > 0)
+		memset(&ndisc->opt[len], 0, space - len);
+
+	return space;
+}
+
+/**
+ * Extract the Ethernet address from a neighbor discovery packet.
+ * Note that the link layer address could be anything but the only networking
+ * media that u-boot supports is Ethernet so we assume we're extracting a 6
+ * byte Ethernet MAC address.
+ */
+static void ndisc_extract_enetaddr(struct nd_msg *ndisc, uchar enetaddr[6])
+{
+	memcpy(enetaddr, &ndisc->opt[2], 6);
+}
+
+/**
+ * Check to see if the neighbor discovery packet has
+ * the specified option set.
+ */
+static int ndisc_has_option(struct ip6_hdr *ip6, __u8 type)
+{
+	struct nd_msg *ndisc = (struct nd_msg *)(((uchar *)ip6) + IP6_HDR_SIZE);
+
+	if (ip6->payload_len <= sizeof(struct icmp6hdr))
+		return 0;
+
+	return ndisc->opt[0] == type;
+}
+
+static void ip6_send_ns(struct in6_addr *neigh_addr)
+{
+	struct in6_addr dst_adr;
+	unsigned char enetaddr[6];
+	struct nd_msg *msg;
+	__u16 len;
+	uchar *pkt;
+
+	debug("sending neighbor solicitation for %pI6c our address %pI6c\n",
+	      neigh_addr, &net_link_local_ip6);
+
+	/* calculate src, dest IPv6 addr and dest Eth addr */
+	ip6_make_snma(&dst_adr, neigh_addr);
+	ip6_make_mult_ethdstaddr(enetaddr, &dst_adr);
+	len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
+	    IP6_NDISC_OPT_SPACE(INETHADDRSZ);
+
+	pkt = (uchar *)net_tx_packet;
+	pkt += net_set_ether(pkt, enetaddr, PROT_IP6);
+	pkt += ip6_add_hdr(pkt, &net_link_local_ip6, &dst_adr, IPPROTO_ICMPV6,
+			   IPV6_NDISC_HOPLIMIT, len);
+
+	/* ICMPv6 - NS */
+	msg = (struct nd_msg *)pkt;
+	msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_SOLICITATION;
+	msg->icmph.icmp6_code = 0;
+	memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
+	memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
+
+	/* Set the target address and llsaddr option */
+	net_copy_ip6(&msg->target, neigh_addr);
+	ndisc_insert_option(msg, ND_OPT_SOURCE_LL_ADDR, net_ethaddr,
+			    INETHADDRSZ);
+
+	/* checksum */
+	msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_link_local_ip6, &dst_adr,
+						 len, IPPROTO_ICMPV6,
+						 csum_partial((__u8 *)msg, len, 0));
+
+	pkt += len;
+
+	/* send it! */
+	net_send_packet(net_tx_packet, (pkt - net_tx_packet));
+}
+
+static void
+ip6_send_na(uchar *eth_dst_addr, struct in6_addr *neigh_addr,
+	    struct in6_addr *target)
+{
+	struct nd_msg *msg;
+	__u16 len;
+	uchar *pkt;
+
+	debug("sending neighbor advertisement for %pI6c to %pI6c (%pM)\n",
+	      target, neigh_addr, eth_dst_addr);
+
+	len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
+	    IP6_NDISC_OPT_SPACE(INETHADDRSZ);
+
+	pkt = (uchar *)net_tx_packet;
+	pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
+	pkt += ip6_add_hdr(pkt, &net_link_local_ip6, neigh_addr,
+			   IPPROTO_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
+
+	/* ICMPv6 - NA */
+	msg = (struct nd_msg *)pkt;
+	msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT;
+	msg->icmph.icmp6_code = 0;
+	memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
+	memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
+
+	/* Set the target address and lltargetaddr option */
+	net_copy_ip6(&msg->target, target);
+	ndisc_insert_option(msg, ND_OPT_TARGET_LL_ADDR, net_ethaddr,
+			    INETHADDRSZ);
+
+	/* checksum */
+	msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_link_local_ip6,
+						 neigh_addr, len, IPPROTO_ICMPV6,
+						 csum_partial((__u8 *)msg, len, 0));
+
+	pkt += len;
+
+	/* send it! */
+	net_send_packet(net_tx_packet, (pkt - net_tx_packet));
+}
+
+void ndisc_request(void)
+{
+	if (!ip6_addr_in_subnet(&net_ip6, &net_nd_sol_packet_ip6,
+				net_prefix_length)) {
+		if (ip6_is_unspecified_addr(&net_gateway6)) {
+			puts("## Warning: gatewayip6 is needed but not set\n");
+			net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
+		} else {
+			net_nd_rep_packet_ip6 = net_gateway6;
+		}
+	} else {
+		net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
+	}
+
+	ip6_send_ns(&net_nd_rep_packet_ip6);
+}
+
+int ndisc_timeout_check(void)
+{
+	ulong t;
+
+	if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
+		return 0;
+
+	t = get_timer(0);
+
+	/* check for NDISC timeout */
+	if ((t - net_nd_timer_start) > NDISC_TIMEOUT) {
+		net_nd_try++;
+		if (net_nd_try >= NDISC_TIMEOUT_COUNT) {
+			puts("\nNeighbour discovery retry count exceeded; "
+			     "starting again\n");
+			net_nd_try = 0;
+			net_set_state(NETLOOP_FAIL);
+		} else {
+			net_nd_timer_start = t;
+			ndisc_request();
+		}
+	}
+	return 1;
+}
+
+void ndisc_init(void)
+{
+	net_nd_packet_mac = NULL;
+	net_nd_tx_packet = NULL;
+	net_nd_sol_packet_ip6 = net_null_addr_ip6;
+	net_nd_rep_packet_ip6 = net_null_addr_ip6;
+	net_nd_tx_packet_size = 0;
+	net_nd_tx_packet = &net_nd_packet_buf[0] + (PKTALIGN - 1);
+	net_nd_tx_packet -= (ulong)net_nd_tx_packet % PKTALIGN;
+}
+
+void ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
+{
+	struct icmp6hdr *icmp =
+	    (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+	struct nd_msg *ndisc = (struct nd_msg *)icmp;
+	uchar neigh_eth_addr[6];
+
+	switch (icmp->icmp6_type) {
+	case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
+		debug("received neighbor solicitation for %pI6c from %pI6c\n",
+		      &ndisc->target, &ip6->saddr);
+		if (ip6_is_our_addr(&ndisc->target) &&
+		    ndisc_has_option(ip6, ND_OPT_SOURCE_LL_ADDR)) {
+			ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
+			ip6_send_na(neigh_eth_addr, &ip6->saddr,
+				    &ndisc->target);
+		}
+		break;
+
+	case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
+		/* are we waiting for a reply ? */
+		if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
+			break;
+
+		if ((memcmp(&ndisc->target, &net_nd_rep_packet_ip6,
+			    sizeof(struct in6_addr)) == 0) &&
+		    ndisc_has_option(ip6, ND_OPT_TARGET_LL_ADDR)) {
+			ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
+
+			/* save address for later use */
+			if (net_nd_packet_mac != NULL)
+				memcpy(net_nd_packet_mac, neigh_eth_addr, 6);
+
+			/* modify header, and transmit it */
+			memcpy(((struct ethernet_hdr *)net_nd_tx_packet)->et_dest,
+			       neigh_eth_addr, 6);
+			net_send_packet(net_nd_tx_packet,
+					net_nd_tx_packet_size);
+
+			/* no ND request pending now */
+			net_nd_sol_packet_ip6 = net_null_addr_ip6;
+			net_nd_tx_packet_size = 0;
+			net_nd_packet_mac = NULL;
+		}
+		break;
+	default:
+		debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
+	}
+}
diff --git a/net/ndisc.h b/net/ndisc.h
new file mode 100644
index 0000000..75138d0
--- /dev/null
+++ b/net/ndisc.h
@@ -0,0 +1,25 @@
+/*
+ * net/ndisc.h
+ *
+ * (C) Copyright 2013 Allied Telesis Labs NZ
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/* IPv6 destination address of packet waiting for ND */
+extern struct in6_addr net_nd_sol_packet_ip6;
+/* MAC destination address of packet waiting for ND */
+extern uchar *net_nd_packet_mac;
+/* pointer to packet waiting to be transmitted after ND is resolved */
+extern uchar *net_nd_tx_packet;
+/* size of packet waiting to be transmitted */
+extern int net_nd_tx_packet_size;
+/* the timer for ND resolution */
+extern ulong net_nd_timer_start;
+/* the number of requests we have sent so far */
+extern int net_nd_try;
+
+void ndisc_init(void);
+void ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len);
+void ndisc_request(void);
+int ndisc_timeout_check(void);
diff --git a/net/net.c b/net/net.c
index 2926bce..ca24673 100644
--- a/net/net.c
+++ b/net/net.c
@@ -86,6 +86,7 @@
 #include <environment.h>
 #include <errno.h>
 #include <net.h>
+#include <net6.h>
 #include <net/tftp.h>
 #if defined(CONFIG_STATUS_LED)
 #include <miiphy.h>
@@ -94,6 +95,7 @@
 #include <watchdog.h>
 #include <linux/compiler.h>
 #include "arp.h"
+#include "ndisc.h"
 #include "bootp.h"
 #include "cdp.h"
 #if defined(CONFIG_CMD_DNS)
@@ -341,8 +343,12 @@ void net_auto_load(void)
 
 static void net_init_loop(void)
 {
-	if (eth_get_dev())
+	if (eth_get_dev()) {
 		memcpy(net_ethaddr, eth_get_ethaddr(), 6);
+#ifdef CONFIG_NET6
+		ip6_make_lladdr(&net_link_local_ip6, net_ethaddr);
+#endif
+	}
 
 	return;
 }
@@ -376,6 +382,9 @@ void net_init(void)
 				(i + 1) * PKTSIZE_ALIGN;
 		}
 		arp_init();
+#ifdef CONFIG_NET6
+		ndisc_init();
+#endif
 		net_clear_handlers();
 
 		/* Only need to setup buffer pointers once. */
@@ -478,6 +487,11 @@ restart:
 			ping_start();
 			break;
 #endif
+#ifdef CONFIG_CMD_PING6
+		case PING6:
+			ping6_start();
+			break;
+#endif
 #if defined(CONFIG_CMD_NFS)
 		case NFS:
 			nfs_start();
@@ -555,6 +569,9 @@ restart:
 		if (ctrlc()) {
 			/* cancel any ARP that may not have completed */
 			net_arp_wait_packet_ip.s_addr = 0;
+#ifdef CONFIG_NET6
+			net_nd_sol_packet_ip6 = net_null_addr_ip6;
+#endif
 
 			net_cleanup_loop();
 			eth_halt();
@@ -570,7 +587,11 @@ restart:
 		}
 
 		if (arp_timeout_check() > 0) {
-		    time_start = get_timer(0);
+			time_start = get_timer(0);
+#ifdef CONFIG_NET6
+		} else if (ndisc_timeout_check() > 0) {
+			time_start = get_timer(0);
+#endif
 		}
 
 		/*
@@ -1143,6 +1164,11 @@ void net_process_received_packet(uchar *in_packet, int len)
 		rarp_receive(ip, len);
 		break;
 #endif
+#ifdef CONFIG_NET6
+	case PROT_IP6:
+		net_ip6_handler(et, (struct ip6_hdr *)ip, len);
+		break;
+#endif
 	case PROT_IP:
 		debug_cond(DEBUG_NET_PKT, "Got IP\n");
 		/* Before we start poking the header, make sure it is there */
@@ -1297,6 +1323,14 @@ static int net_check_prereq(enum proto_t protocol)
 		}
 		goto common;
 #endif
+#ifdef CONFIG_CMD_PING6
+	case PING6:
+		if (ip6_is_unspecified_addr(&net_ping_ip6)) {
+			puts("*** ERROR: ping address not given\n");
+			return 1;
+		}
+		goto common;
+#endif
 #if defined(CONFIG_CMD_SNTP)
 	case SNTP:
 		if (net_ntp_server.s_addr == 0) {
diff --git a/net/net6.c b/net/net6.c
index f778cef..955a089 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -8,6 +8,34 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+/*
+ * General Desription:
+ *
+ * The user interface supports commands for TFTP6.
+ * Also, we support Neighbour discovery internally. Depending on available
+ * data, these interact as follows:
+ *
+ * Neighbour Discovery:
+ *
+ *      Prerequisites:  - own ethernet address
+ *                      - own IPv6 address
+ *                      - TFTP server IPv6 address
+ *      We want:        - TFTP server ethernet address
+ *      Next step:      TFTP
+ *
+ * TFTP over IPv6:
+ *
+ *      Prerequisites:  - own ethernet address
+ *                      - own IPv6 address
+ *                      - TFTP server IPv6 address
+ *                      - TFTP server ethernet address
+ *                      - name of bootfile (if unknown, we use a default name
+ *                        derived from our own IPv6 address)
+ *      We want:        - load the boot file
+ *      Next step:      none
+ *
+ */
+#define DEBUG
 #include <common.h>
 #include <environment.h>
 #include <malloc.h>
@@ -15,10 +43,14 @@
 #include <net6.h>
 #include "ndisc.h"
 
+/* NULL IPv6 address */
+struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR;
 /* Our gateway's IPv6 address */
 struct in6_addr net_gateway6 = ZERO_IPV6_ADDR;
 /* Our IPv6 addr (0 = unknown) */
 struct in6_addr net_ip6 = ZERO_IPV6_ADDR;
+/* Our link local IPv6 addr (0 = unknown) */
+struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR;
 /* set server IPv6 addr (0 = unknown) */
 struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR;
 /* The prefix length of our network */
@@ -87,3 +119,270 @@ static int on_serverip6(const char *name, const char *value, enum env_op op,
 	return string_to_ip6(value, &net_server_ip6);
 }
 U_BOOT_ENV_CALLBACK(serverip6, on_serverip6);
+
+int ip6_is_unspecified_addr(struct in6_addr *addr)
+{
+	return (addr->s6_addr32[0] | addr->s6_addr32[1] |
+		addr->s6_addr32[2] | addr->s6_addr32[3]) == 0;
+}
+
+/**
+ * We have 2 addresses that we should respond to. A link
+ * local address and a global address. This returns true
+ * if the specified address matches either of these.
+ */
+int ip6_is_our_addr(struct in6_addr *addr)
+{
+	return memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) == 0 ||
+	       memcmp(addr, &net_ip6, sizeof(struct in6_addr)) == 0;
+}
+
+void ip6_make_eui(unsigned char eui[8], unsigned char const enetaddr[6])
+{
+	memcpy(eui, enetaddr, 3);
+	memcpy(&eui[5], &enetaddr[3], 3);
+	eui[3] = 0xFF;
+	eui[4] = 0xFE;
+	eui[0] ^= 2;		/* "u" bit set to indicate global scope */
+}
+
+void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6])
+{
+	uchar eui[8];
+
+	memset(lladr, 0, sizeof(struct in6_addr));
+	lladr->s6_addr16[0] = htons(IPV6_LINK_LOCAL_PREFIX);
+	ip6_make_eui(eui, enetaddr);
+	memcpy(&lladr->s6_addr[8], eui, 8);
+}
+
+/*
+ * Given an IPv6 address generate an equivalent Solicited Node Multicast
+ * Address (SNMA) as described in RFC2461.
+ */
+void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
+{
+	memset(mcast_addr, 0, sizeof(struct in6_addr));
+	mcast_addr->s6_addr[0] = 0xff;
+	mcast_addr->s6_addr[1] = IPV6_ADDRSCOPE_LINK;
+	mcast_addr->s6_addr[11] = 0x01;
+	mcast_addr->s6_addr[12] = 0xff;
+	mcast_addr->s6_addr[13] = ip6_addr->s6_addr[13];
+	mcast_addr->s6_addr[14] = ip6_addr->s6_addr[14];
+	mcast_addr->s6_addr[15] = ip6_addr->s6_addr[15];
+}
+
+/*
+ * Given an IPv6 address generate the multicast MAC address that corresponds to
+ * it.
+ */
+void
+ip6_make_mult_ethdstaddr(unsigned char enetaddr[6], struct in6_addr *mcast_addr)
+{
+	enetaddr[0] = 0x33;
+	enetaddr[1] = 0x33;
+	memcpy(&enetaddr[2], &mcast_addr->s6_addr[12], 4);
+}
+
+int
+ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
+		   __u32 plen)
+{
+	__be32 *addr_dwords;
+	__be32 *neigh_dwords;
+
+	addr_dwords = our_addr->s6_addr32;
+	neigh_dwords = neigh_addr->s6_addr32;
+
+	while (plen > 32) {
+		if (*addr_dwords++ != *neigh_dwords++)
+			return 0;
+
+		plen -= 32;
+	}
+
+	/* Check any remaining bits. */
+	if (plen > 0) {
+		if ((*addr_dwords >> (32 - plen)) !=
+		    (*neigh_dwords >> (32 - plen))) {
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+static inline unsigned int csum_fold(unsigned int sum)
+{
+	sum = (sum & 0xffff) + (sum >> 16);
+	sum = (sum & 0xffff) + (sum >> 16);
+
+	return ~sum;
+}
+
+static __u32 csum_do_csum(const __u8 *buff, int len)
+{
+	int odd, count;
+	unsigned long result = 0;
+
+	if (len <= 0)
+		goto out;
+	odd = 1 & (unsigned long)buff;
+	if (odd) {
+		result = *buff;
+		len--;
+		buff++;
+	}
+	count = len >> 1;	/* nr of 16-bit words.. */
+	if (count) {
+		if (2 & (unsigned long)buff) {
+			result += *(unsigned short *)buff;
+			count--;
+			len -= 2;
+			buff += 2;
+		}
+		count >>= 1;	/* nr of 32-bit words.. */
+		if (count) {
+			unsigned long carry = 0;
+			do {
+				unsigned long w = *(unsigned long *)buff;
+				count--;
+				buff += 4;
+				result += carry;
+				result += w;
+				carry = (w > result);
+			} while (count);
+			result += carry;
+			result = (result & 0xffff) + (result >> 16);
+		}
+		if (len & 2) {
+			result += *(unsigned short *)buff;
+			buff += 2;
+		}
+	}
+	if (len & 1)
+		result += (*buff << 8);
+	result = ~csum_fold(result);
+	if (odd)
+		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
+out:
+	return result;
+}
+
+unsigned int csum_partial(const unsigned char *buff, int len, unsigned int sum)
+{
+	unsigned int result = csum_do_csum(buff, len);
+
+	/* add in old sum, and carry.. */
+	result += sum;
+	/* 16+c bits -> 16 bits */
+	result = (result & 0xffff) + (result >> 16);
+	return result;
+}
+
+/*
+ * Compute checksum of IPv6 "psuedo-header" per RFC2460 section 8.1
+ */
+unsigned short int
+csum_ipv6_magic(struct in6_addr *saddr, struct in6_addr *daddr,
+		__u16 len, unsigned short proto, unsigned int csum)
+{
+	int i;
+	int carry;
+	__u32 ulen;
+	__u32 uproto;
+	unsigned int finalsum;
+
+	for (i = 0; i < 4; i++) {
+		csum += saddr->s6_addr32[i];
+		carry = (csum < saddr->s6_addr32[i]);
+		csum += carry;
+
+		csum += daddr->s6_addr32[i];
+		carry = (csum < daddr->s6_addr32[i]);
+		csum += carry;
+	}
+
+	ulen = htonl((__u32)len);
+	csum += ulen;
+	carry = (csum < ulen);
+	csum += carry;
+
+	uproto = htonl(proto);
+	csum += uproto;
+	carry = (csum < uproto);
+	csum += carry;
+
+	finalsum = csum_fold(csum);
+	if ((finalsum & 0xffff) == 0x0000)
+		return 0xffff;
+	else if ((finalsum & 0xffff) == 0xffff)
+		return 0x0000;
+	else
+		return finalsum;
+}
+
+int
+ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
+	    int nextheader, int hoplimit, int payload_len)
+{
+	struct ip6_hdr *ip6 = (struct ip6_hdr *)xip;
+
+	ip6->version = 6;
+	ip6->priority = 0;
+	ip6->flow_lbl[0] = 0;
+	ip6->flow_lbl[1] = 0;
+	ip6->flow_lbl[2] = 0;
+	ip6->payload_len = htons(payload_len);
+	ip6->nexthdr = nextheader;
+	ip6->hop_limit = hoplimit;
+	net_copy_ip6(&ip6->saddr, src);
+	net_copy_ip6(&ip6->daddr, dest);
+
+	return sizeof(struct ip6_hdr);
+}
+
+void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
+{
+	struct in_addr zero_ip = {.s_addr = 0 };
+	struct icmp6hdr *icmp;
+	struct udp_hdr *udp;
+	__u16 csum;
+	__u16 hlen;
+
+	if (len < IP6_HDR_SIZE)
+		return;
+
+	if (ip6->version != 6)
+		return;
+
+	switch (ip6->nexthdr) {
+	case IPPROTO_ICMPV6:
+		icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+		csum = icmp->icmp6_cksum;
+		hlen = ntohs(ip6->payload_len);
+		icmp->icmp6_cksum = 0;
+		/* checksum */
+		icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+						    hlen, IPPROTO_ICMPV6,
+						    csum_partial((__u8 *)icmp, hlen, 0));
+		if (icmp->icmp6_cksum != csum)
+			return;
+
+		switch (icmp->icmp6_type) {
+		case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
+		case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
+			ndisc_receive(et, ip6, len);
+			break;
+
+		default:
+			return;
+			break;
+		}
+		break;
+
+	default:
+		return;
+		break;
+	}
+}
-- 
2.5.3