Merge branch '2022-12-05-add-IPv6-support'

To quote the author:
This patch set adds basic IPv6 support to U-boot.
It is based on Chris's Packham patches
(https://lists.denx.de/pipermail/u-boot/2017-January/279366.html)
Chris's patches were taken as base. There were efforts to launch it on
HiFive SiFive Unmatched board but the board didn't work well. The code was
refactored, fixed some bugs as CRC for little-endian, some parts were implemented in
our own way, something was taken from Linux. Finally we did manual tests and the
board worked well.

Testing was done on HiFive SiFive Unmatched board (RISC-V)

7 files changed, 968 insertions, 8 deletions

diff --git a/net/Kconfig b/net/Kconfig
index cb600fe5eb4..a1ec3f8542e 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -190,6 +190,16 @@ config PROT_TCP_SACK
 	  This option should be turn on if you want to achieve the fastest
 	  file transfer possible.
 
+config IPV6
+	bool "IPv6 support"
+	help
+	  Enable IPv6 support. It includes Neighbour Discovery protocol, ICMPv6
+	  and auxiliary stuff to make it work. Since it is enabled u-boot
+	  network subsystem can get and handle incoming packets and send packets
+	  through IPv6 network. It allows to use environment variables such as
+	  ip6addr, serverip6. If a u-boot command is capable to parse an IPv6
+	  address and find it, it will force using IPv6 in the network stack.
+
 endif   # if NET
 
 config SYS_RX_ETH_BUFFER
diff --git a/net/Makefile b/net/Makefile
index 4f757a224c2..13eef04029d 100644
--- a/net/Makefile
+++ b/net/Makefile
@@ -20,9 +20,12 @@ obj-$(CONFIG_DM_MDIO)  += mdio-uclass.o
 obj-$(CONFIG_DM_MDIO_MUX) += mdio-mux-uclass.o
 obj-$(CONFIG_NET)      += eth_common.o
 obj-$(CONFIG_CMD_LINK_LOCAL) += link_local.o
+obj-$(CONFIG_IPV6)     += ndisc.o
 obj-$(CONFIG_NET)      += net.o
+obj-$(CONFIG_IPV6)     += net6.o
 obj-$(CONFIG_CMD_NFS)  += nfs.o
 obj-$(CONFIG_CMD_PING) += ping.o
+obj-$(CONFIG_CMD_PING6) += ping6.o
 obj-$(CONFIG_CMD_PCAP) += pcap.o
 obj-$(CONFIG_CMD_RARP) += rarp.o
 obj-$(CONFIG_CMD_SNTP) += sntp.o
diff --git a/net/ndisc.c b/net/ndisc.c
new file mode 100644
index 00000000000..3c0eeeaea39
--- /dev/null
+++ b/net/ndisc.c
@@ -0,0 +1,289 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2013 Allied Telesis Labs NZ
+ * Chris Packham, <judge.packham@gmail.com>
+ *
+ * Copyright (C) 2022 YADRO
+ * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
+ */
+
+/* Neighbour Discovery for IPv6 */
+
+#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)
+
+/**
+ * ndisc_insert_option() - Insert an option into a neighbor discovery packet
+ *
+ * @ndisc:	pointer to ND packet
+ * @type:	option type to insert
+ * @data:	option data to insert
+ * @len:	data length
+ * Return: 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;
+}
+
+/**
+ * ndisc_extract_enetaddr() - Extract the Ethernet address from a ND 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.
+ *
+ * @ndisc:	pointer to ND packet
+ * @enetaddr:	extracted MAC addr
+ */
+static void ndisc_extract_enetaddr(struct nd_msg *ndisc, uchar enetaddr[6])
+{
+	memcpy(enetaddr, &ndisc->opt[2], 6);
+}
+
+/**
+ * ndisc_has_option() - Check if the ND packet has the specified option set
+ *
+ * @ip6:	pointer to IPv6 header
+ * @type:	option type to check
+ * Return: 1 if ND has that option, 0 therwise
+ */
+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;
+	unsigned short csum;
+	unsigned int pcsum;
+
+	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, PROT_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 */
+	pcsum = csum_partial((__u8 *)msg, len, 0);
+	csum = csum_ipv6_magic(&net_link_local_ip6, &dst_adr,
+			       len, PROT_ICMPV6, pcsum);
+	msg->icmph.icmp6_cksum = csum;
+	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;
+	unsigned short csum;
+
+	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,
+			   PROT_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));
+	msg->icmph.icmp6_dataun.u_nd_advt.solicited = 1;
+	msg->icmph.icmp6_dataun.u_nd_advt.override = 1;
+	/* 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 */
+	csum = csum_ipv6_magic(&net_link_local_ip6,
+			       neigh_addr, len, PROT_ICMPV6,
+			       csum_partial((__u8 *)msg, len, 0));
+	msg->icmph.icmp6_cksum = csum;
+	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;
+}
+
+int 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)
+				memcpy(net_nd_packet_mac, neigh_eth_addr, 7);
+
+			/* 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);
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/net/net.c b/net/net.c
index aca20e43b0b..1c39acc4936 100644
--- a/net/net.c
+++ b/net/net.c
@@ -91,6 +91,8 @@
 #include <image.h>
 #include <log.h>
 #include <net.h>
+#include <net6.h>
+#include <ndisc.h>
 #include <net/fastboot.h>
 #include <net/tftp.h>
 #include <net/ncsi.h>
@@ -343,8 +345,17 @@ void net_auto_load(void)
 
 static int net_init_loop(void)
 {
-	if (eth_get_dev())
+	if (eth_get_dev()) {
 		memcpy(net_ethaddr, eth_get_ethaddr(), 6);
+
+		if (IS_ENABLED(CONFIG_IPV6)) {
+			ip6_make_lladdr(&net_link_local_ip6, net_ethaddr);
+			if (!memcmp(&net_ip6, &net_null_addr_ip6,
+				    sizeof(struct in6_addr)))
+				memcpy(&net_ip6, &net_link_local_ip6,
+				       sizeof(struct in6_addr));
+		}
+	}
 	else
 		/*
 		 * Not ideal, but there's no way to get the actual error, and I
@@ -385,6 +396,7 @@ int net_init(void)
 				(i + 1) * PKTSIZE_ALIGN;
 		}
 		arp_init();
+		ndisc_init();
 		net_clear_handlers();
 
 		/* Only need to setup buffer pointers once. */
@@ -513,6 +525,11 @@ restart:
 			ping_start();
 			break;
 #endif
+#if defined(CONFIG_CMD_PING6)
+		case PING6:
+			ping6_start();
+			break;
+#endif
 #if defined(CONFIG_CMD_NFS) && !defined(CONFIG_SPL_BUILD)
 		case NFS:
 			nfs_start();
@@ -589,6 +606,11 @@ restart:
 		if (arp_timeout_check() > 0)
 			time_start = get_timer(0);
 
+		if (IS_ENABLED(CONFIG_IPV6)) {
+			if (use_ip6 && (ndisc_timeout_check() > 0))
+				time_start = get_timer(0);
+		}
+
 		/*
 		 *	Check the ethernet for a new packet.  The ethernet
 		 *	receive routine will process it.
@@ -1244,6 +1266,10 @@ void net_process_received_packet(uchar *in_packet, int len)
 		rarp_receive(ip, len);
 		break;
 #endif
+#if IS_ENABLED(CONFIG_IPV6)
+	case PROT_IP6:
+		net_ip6_handler(et, (struct ip6_hdr *)ip, len);
+#endif
 	case PROT_IP:
 		debug_cond(DEBUG_NET_PKT, "Got IP\n");
 		/* Before we start poking the header, make sure it is there */
@@ -1413,6 +1439,14 @@ static int net_check_prereq(enum proto_t protocol)
 		}
 		goto common;
 #endif
+#if defined(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_DNS)
 	case DNS:
 		if (net_dns_server.s_addr == 0) {
@@ -1434,7 +1468,14 @@ static int net_check_prereq(enum proto_t protocol)
 		/* Fall through */
 	case TFTPGET:
 	case TFTPPUT:
-		if (net_server_ip.s_addr == 0 && !is_serverip_in_cmd()) {
+		if (IS_ENABLED(CONFIG_IPV6) && use_ip6) {
+			if (!memcmp(&net_server_ip6, &net_null_addr_ip6,
+				    sizeof(struct in6_addr)) &&
+				    !strchr(net_boot_file_name, '[')) {
+				puts("*** ERROR: `serverip6' not set\n");
+				return 1;
+			}
+		} else if (net_server_ip.s_addr == 0 && !is_serverip_in_cmd()) {
 			puts("*** ERROR: `serverip' not set\n");
 			return 1;
 		}
@@ -1447,7 +1488,13 @@ common:
 	case NETCONS:
 	case FASTBOOT:
 	case TFTPSRV:
-		if (net_ip.s_addr == 0) {
+		if (IS_ENABLED(CONFIG_IPV6) && use_ip6) {
+			if (!memcmp(&net_link_local_ip6, &net_null_addr_ip6,
+				    sizeof(struct in6_addr))) {
+				puts("*** ERROR: `ip6addr` not set\n");
+				return 1;
+			}
+		} else if (net_ip.s_addr == 0) {
 			puts("*** ERROR: `ipaddr' not set\n");
 			return 1;
 		}
diff --git a/net/net6.c b/net/net6.c
new file mode 100644
index 00000000000..fdea0787885
--- /dev/null
+++ b/net/net6.c
@@ -0,0 +1,445 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2013 Allied Telesis Labs NZ
+ * Chris Packham, <judge.packham@gmail.com>
+ *
+ * Copyright (C) 2022 YADRO
+ * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
+ */
+
+/* Simple IPv6 network layer implementation */
+
+#include <common.h>
+#include <env_internal.h>
+#include <malloc.h>
+#include <net.h>
+#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 */
+u32 net_prefix_length;
+
+bool use_ip6;
+
+static int on_ip6addr(const char *name, const char *value, enum env_op op,
+		      int flags)
+{
+	char *mask;
+	size_t len;
+
+	if (flags & H_PROGRAMMATIC)
+		return 0;
+
+	if (op == env_op_delete) {
+		net_prefix_length = 0;
+		net_copy_ip6(&net_ip6, &net_null_addr_ip6);
+		return 0;
+	}
+
+	mask = strchr(value, '/');
+	len = strlen(value);
+
+	if (mask)
+		net_prefix_length = simple_strtoul(value + len, NULL, 10);
+
+	return string_to_ip6(value, len, &net_ip6);
+}
+
+U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr);
+
+static int on_gatewayip6(const char *name, const char *value, enum env_op op,
+			 int flags)
+{
+	if (flags & H_PROGRAMMATIC)
+		return 0;
+
+	return string_to_ip6(value, strlen(value), &net_gateway6);
+}
+
+U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6);
+
+static int on_serverip6(const char *name, const char *value, enum env_op op,
+			int flags)
+{
+	if (flags & H_PROGRAMMATIC)
+		return 0;
+
+	return string_to_ip6(value, strlen(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]);
+}
+
+int ip6_is_our_addr(struct in6_addr *addr)
+{
+	return !memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) ||
+	       !memcmp(addr, &net_ip6, sizeof(struct in6_addr));
+}
+
+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])
+{
+	unsigned char 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);
+}
+
+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];
+}
+
+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);
+
+	/* Opaque moment. If reverse it to zero it will not be checked on
+	 * receiver's side. It leads to bad negibour advertisement.
+	 */
+	if (sum == 0xffff)
+		return sum;
+
+	return ~sum;
+}
+
+static inline unsigned short from32to16(unsigned int x)
+{
+	/* add up 16-bit and 16-bit for 16+c bit */
+	x = (x & 0xffff) + (x >> 16);
+	/* add up carry.. */
+	x = (x & 0xffff) + (x >> 16);
+	return x;
+}
+
+static u32 csum_do_csum(const u8 *buff, int len)
+{
+	int odd;
+	unsigned int result = 0;
+
+	if (len <= 0)
+		goto out;
+	odd = 1 & (unsigned long)buff;
+	if (odd) {
+#ifdef __LITTLE_ENDIAN
+		result += (*buff << 8);
+#else
+		result = *buff;
+#endif
+		len--;
+		buff++;
+	}
+	if (len >= 2) {
+		if (2 & (unsigned long)buff) {
+			result += *(unsigned short *)buff;
+			len -= 2;
+			buff += 2;
+		}
+		if (len >= 4) {
+			const unsigned char *end = buff + ((u32)len & ~3);
+			unsigned int carry = 0;
+
+			do {
+				unsigned int w = *(unsigned int *)buff;
+
+				buff += 4;
+				result += carry;
+				result += w;
+				carry = (w > result);
+			} while (buff < end);
+			result += carry;
+			result = (result & 0xffff) + (result >> 16);
+		}
+		if (len & 2) {
+			result += *(unsigned short *)buff;
+			buff += 2;
+		}
+	}
+	if (len & 1)
+#ifdef __LITTLE_ENDIAN
+		result += *buff;
+#else
+		result += (*buff << 8);
+#endif
+	result = from32to16(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;
+}
+
+unsigned short int
+csum_ipv6_magic(struct in6_addr *saddr, struct in6_addr *daddr, u16 len,
+		unsigned short proto, unsigned int csum)
+{
+	int carry;
+	u32 ulen;
+	u32 uproto;
+	u32 sum = csum;
+
+	sum += saddr->s6_addr32[0];
+	carry = (sum < saddr->s6_addr32[0]);
+	sum += carry;
+
+	sum += saddr->s6_addr32[1];
+	carry = (sum < saddr->s6_addr32[1]);
+	sum += carry;
+
+	sum += saddr->s6_addr32[2];
+	carry = (sum < saddr->s6_addr32[2]);
+	sum += carry;
+
+	sum += saddr->s6_addr32[3];
+	carry = (sum < saddr->s6_addr32[3]);
+	sum += carry;
+
+	sum += daddr->s6_addr32[0];
+	carry = (sum < daddr->s6_addr32[0]);
+	sum += carry;
+
+	sum += daddr->s6_addr32[1];
+	carry = (sum < daddr->s6_addr32[1]);
+	sum += carry;
+
+	sum += daddr->s6_addr32[2];
+	carry = (sum < daddr->s6_addr32[2]);
+	sum += carry;
+
+	sum += daddr->s6_addr32[3];
+	carry = (sum < daddr->s6_addr32[3]);
+	sum += carry;
+
+	ulen = htonl((u32)len);
+	sum += ulen;
+	carry = (sum < ulen);
+	sum += carry;
+
+	uproto = htonl(proto);
+	sum += uproto;
+	carry = (sum < uproto);
+	sum += carry;
+
+	return csum_fold(sum);
+}
+
+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);
+}
+
+int net_send_udp_packet6(uchar *ether, struct in6_addr *dest, int dport,
+			 int sport, int len)
+{
+	uchar *pkt;
+	struct udp_hdr *udp;
+	u16 csum_p;
+
+	udp = (struct udp_hdr *)((uchar *)net_tx_packet + net_eth_hdr_size() +
+			IP6_HDR_SIZE);
+
+	udp->udp_dst = htons(dport);
+	udp->udp_src = htons(sport);
+	udp->udp_len = htons(len + UDP_HDR_SIZE);
+
+	/* checksum */
+	udp->udp_xsum = 0;
+	csum_p = csum_partial((u8 *)udp, len + UDP_HDR_SIZE, 0);
+	udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
+					IPPROTO_UDP, csum_p);
+
+	/* if MAC address was not discovered yet, save the packet and do
+	 * neighbour discovery
+	 */
+	if (!memcmp(ether, net_null_ethaddr, 6)) {
+		net_copy_ip6(&net_nd_sol_packet_ip6, dest);
+		net_nd_packet_mac = ether;
+
+		pkt = net_nd_tx_packet;
+		pkt += net_set_ether(pkt, net_nd_packet_mac, PROT_IP6);
+		pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
+				len + UDP_HDR_SIZE);
+		memcpy(pkt, (uchar *)udp, len + UDP_HDR_SIZE);
+
+		/* size of the waiting packet */
+		net_nd_tx_packet_size = (pkt - net_nd_tx_packet) +
+			UDP_HDR_SIZE + len;
+
+		/* and do the neighbor solicitation */
+		net_nd_try = 1;
+		net_nd_timer_start = get_timer(0);
+		ndisc_request();
+		return 1;	/* waiting */
+	}
+
+	pkt = (uchar *)net_tx_packet;
+	pkt += net_set_ether(pkt, ether, PROT_IP6);
+	pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
+			len + UDP_HDR_SIZE);
+	(void)eth_send(net_tx_packet, pkt - net_tx_packet + UDP_HDR_SIZE + len);
+
+	return 0;	/* transmitted */
+}
+
+int 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 csum_p;
+	u16 hlen;
+
+	if (len < IP6_HDR_SIZE)
+		return -EINVAL;
+
+	if (ip6->version != 6)
+		return -EINVAL;
+
+	switch (ip6->nexthdr) {
+	case PROT_ICMPV6:
+		icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+		csum = icmp->icmp6_cksum;
+		hlen = ntohs(ip6->payload_len);
+		icmp->icmp6_cksum = 0;
+		/* checksum */
+		csum_p = csum_partial((u8 *)icmp, hlen, 0);
+		icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+						    hlen, PROT_ICMPV6, csum_p);
+
+		if (icmp->icmp6_cksum != csum)
+			return -EINVAL;
+
+		switch (icmp->icmp6_type) {
+		case IPV6_ICMP_ECHO_REQUEST:
+		case IPV6_ICMP_ECHO_REPLY:
+			ping6_receive(et, ip6, len);
+			break;
+		case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
+		case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
+			ndisc_receive(et, ip6, len);
+			break;
+		default:
+			break;
+		}
+		break;
+	case IPPROTO_UDP:
+		udp = (struct udp_hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+		csum = udp->udp_xsum;
+		hlen = ntohs(ip6->payload_len);
+		udp->udp_xsum = 0;
+		/* checksum */
+		csum_p = csum_partial((u8 *)udp, hlen, 0);
+		udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+						hlen, IPPROTO_UDP, csum_p);
+
+		if (csum != udp->udp_xsum)
+			return -EINVAL;
+
+		/* IP header OK. Pass the packet to the current handler. */
+		net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
+					UDP_HDR_SIZE,
+				ntohs(udp->udp_dst),
+				zero_ip,
+				ntohs(udp->udp_src),
+				ntohs(udp->udp_len) - 8);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/net/ping6.c b/net/ping6.c
new file mode 100644
index 00000000000..4882a17f510
--- /dev/null
+++ b/net/ping6.c
@@ -0,0 +1,118 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2013 Allied Telesis Labs NZ
+ * Chris Packham, <judge.packham@gmail.com>
+ *
+ * Copyright (C) 2022 YADRO
+ * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
+ */
+
+/* Simple ping6 implementation */
+
+#include <common.h>
+#include <net.h>
+#include <net6.h>
+#include "ndisc.h"
+
+static ushort seq_no;
+
+/* the ipv6 address to ping */
+struct in6_addr net_ping_ip6;
+
+int
+ip6_make_ping(uchar *eth_dst_addr, struct in6_addr *neigh_addr, uchar *pkt)
+{
+	struct echo_msg *msg;
+	u16 len;
+	u16 csum_p;
+	uchar *pkt_old = pkt;
+
+	len = sizeof(struct echo_msg);
+
+	pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
+	pkt += ip6_add_hdr(pkt, &net_ip6, neigh_addr, PROT_ICMPV6,
+			   IPV6_NDISC_HOPLIMIT, len);
+
+	/* ICMPv6 - Echo */
+	msg = (struct echo_msg *)pkt;
+	msg->icmph.icmp6_type = IPV6_ICMP_ECHO_REQUEST;
+	msg->icmph.icmp6_code = 0;
+	msg->icmph.icmp6_cksum = 0;
+	msg->icmph.icmp6_identifier = 0;
+	msg->icmph.icmp6_sequence = htons(seq_no++);
+	msg->id = msg->icmph.icmp6_identifier;	/* these seem redundant */
+	msg->sequence = msg->icmph.icmp6_sequence;
+
+	/* checksum */
+	csum_p = csum_partial((u8 *)msg, len, 0);
+	msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_ip6, neigh_addr, len,
+						 PROT_ICMPV6, csum_p);
+
+	pkt += len;
+
+	return pkt - pkt_old;
+}
+
+int ping6_send(void)
+{
+	uchar *pkt;
+	static uchar mac[6];
+
+	/* always send neighbor solicit */
+
+	memcpy(mac, net_null_ethaddr, 6);
+
+	net_nd_sol_packet_ip6 = net_ping_ip6;
+	net_nd_packet_mac = mac;
+
+	pkt = net_nd_tx_packet;
+	pkt += ip6_make_ping(mac, &net_ping_ip6, pkt);
+
+	/* size of the waiting packet */
+	net_nd_tx_packet_size = (pkt - net_nd_tx_packet);
+
+	/* and do the ARP request */
+	net_nd_try = 1;
+	net_nd_timer_start = get_timer(0);
+	ndisc_request();
+	return 1;		/* waiting */
+}
+
+static void ping6_timeout(void)
+{
+	eth_halt();
+	net_set_state(NETLOOP_FAIL);	/* we did not get the reply */
+}
+
+void ping6_start(void)
+{
+	printf("Using %s device\n", eth_get_name());
+	net_set_timeout_handler(10000UL, ping6_timeout);
+
+	ping6_send();
+}
+
+int ping6_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
+{
+	struct icmp6hdr *icmp =
+	    (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+	struct in6_addr src_ip;
+
+	switch (icmp->icmp6_type) {
+	case IPV6_ICMP_ECHO_REPLY:
+		src_ip = ip6->saddr;
+		if (memcmp(&net_ping_ip6, &src_ip, sizeof(struct in6_addr)))
+			return -EINVAL;
+		net_set_state(NETLOOP_SUCCESS);
+		break;
+	case IPV6_ICMP_ECHO_REQUEST:
+		/* ignore for now.... */
+		debug("Got ICMPv6 ECHO REQUEST from %pI6c\n", &ip6->saddr);
+		return -EINVAL;
+	default:
+		debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/net/tftp.c b/net/tftp.c
index 39421f8daa7..c780c33f379 100644
--- a/net/tftp.c
+++ b/net/tftp.c
@@ -15,6 +15,7 @@
 #include <log.h>
 #include <mapmem.h>
 #include <net.h>
+#include <net6.h>
 #include <asm/global_data.h>
 #include <net/tftp.h>
 #include "bootp.h"
@@ -41,6 +42,7 @@ DECLARE_GLOBAL_DATA_PTR;
 static ulong timeout_ms = TIMEOUT;
 static int timeout_count_max = (CONFIG_NET_RETRY_COUNT * 2);
 static ulong time_start;   /* Record time we started tftp */
+static struct in6_addr tftp_remote_ip6;
 
 /*
  * These globals govern the timeout behavior when attempting a connection to a
@@ -116,6 +118,7 @@ static int	tftp_put_final_block_sent;
 
 /* default TFTP block size */
 #define TFTP_BLOCK_SIZE		512
+#define TFTP_MTU_BLOCKSIZE6 (CONFIG_TFTP_BLOCKSIZE - 20)
 /* sequence number is 16 bit */
 #define TFTP_SEQUENCE_SIZE	((ulong)(1<<16))
 
@@ -320,7 +323,11 @@ static void tftp_send(void)
 	 *	We will always be sending some sort of packet, so
 	 *	cobble together the packet headers now.
 	 */
-	pkt = net_tx_packet + net_eth_hdr_size() + IP_UDP_HDR_SIZE;
+	if (IS_ENABLED(CONFIG_IPV6) && use_ip6)
+		pkt = net_tx_packet + net_eth_hdr_size() +
+		      IP6_HDR_SIZE + UDP_HDR_SIZE;
+	else
+		pkt = net_tx_packet + net_eth_hdr_size() + IP_UDP_HDR_SIZE;
 
 	switch (tftp_state) {
 	case STATE_SEND_RRQ:
@@ -422,8 +429,14 @@ static void tftp_send(void)
 		break;
 	}
 
-	net_send_udp_packet(net_server_ethaddr, tftp_remote_ip,
-			    tftp_remote_port, tftp_our_port, len);
+	if (IS_ENABLED(CONFIG_IPV6) && use_ip6)
+		net_send_udp_packet6(net_server_ethaddr,
+				     &tftp_remote_ip6,
+				     tftp_remote_port,
+				     tftp_our_port, len);
+	else
+		net_send_udp_packet(net_server_ethaddr, tftp_remote_ip,
+				    tftp_remote_port, tftp_our_port, len);
 
 	if (err_pkt)
 		net_set_state(NETLOOP_FAIL);
@@ -797,6 +810,9 @@ void tftp_start(enum proto_t protocol)
 	debug("TFTP blocksize = %i, TFTP windowsize = %d timeout = %ld ms\n",
 	      tftp_block_size_option, tftp_window_size_option, timeout_ms);
 
+	if (IS_ENABLED(CONFIG_IPV6))
+		tftp_remote_ip6 = net_server_ip6;
+
 	tftp_remote_ip = net_server_ip;
 	if (!net_parse_bootfile(&tftp_remote_ip, tftp_filename, MAX_LEN)) {
 		sprintf(default_filename, "%02X%02X%02X%02X.img",
@@ -812,17 +828,49 @@ void tftp_start(enum proto_t protocol)
 		       tftp_filename);
 	}
 
+	if (IS_ENABLED(CONFIG_IPV6)) {
+		if (use_ip6) {
+			char *s, *e;
+			size_t len;
+
+			s = strchr(net_boot_file_name, '[');
+			e = strchr(net_boot_file_name, ']');
+			len = e - s;
+			if (s && e) {
+				string_to_ip6(s + 1, len, &tftp_remote_ip6);
+				strlcpy(tftp_filename, e + 2, MAX_LEN);
+			} else {
+				strlcpy(tftp_filename, net_boot_file_name, MAX_LEN);
+				tftp_filename[MAX_LEN - 1] = 0;
+			}
+		}
+	}
+
 	printf("Using %s device\n", eth_get_name());
-	printf("TFTP %s server %pI4; our IP address is %pI4",
+
+	if (IS_ENABLED(CONFIG_IPV6) && use_ip6) {
+		printf("TFTP from server %pI6c; our IP address is %pI6c",
+		       &tftp_remote_ip6, &net_ip6);
+
+		if (tftp_block_size_option > TFTP_MTU_BLOCKSIZE6)
+			tftp_block_size_option = TFTP_MTU_BLOCKSIZE6;
+	} else {
+		printf("TFTP %s server %pI4; our IP address is %pI4",
 #ifdef CONFIG_CMD_TFTPPUT
 	       protocol == TFTPPUT ? "to" : "from",
 #else
 	       "from",
 #endif
 	       &tftp_remote_ip, &net_ip);
+	}
 
 	/* Check if we need to send across this subnet */
-	if (net_gateway.s_addr && net_netmask.s_addr) {
+	if (IS_ENABLED(CONFIG_IPV6) && use_ip6) {
+		if (!ip6_addr_in_subnet(&net_ip6, &tftp_remote_ip6,
+					net_prefix_length))
+			printf("; sending through gateway %pI6c",
+			       &net_gateway6);
+	} else if (net_gateway.s_addr && net_netmask.s_addr) {
 		struct in_addr our_net;
 		struct in_addr remote_net;