[U-Boot] [RFC PATCH v2 09/11] net: TFTP over IPv6

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

Add support for UDP/TFTP over IPv6. To support specifying an server IPv6
address in the command square brackets must be used to separate the
address from the filename. e.g
  tftpboot6 [2001:db8::1]:zImage

Signed-off-by: Chris Packham <judge.packham@gmail.com>
---

Changes in v2: None

 common/Kconfig   |  9 ++++++++
 common/cmd_net.c | 13 ++++++++++++
 include/net.h    |  2 +-
 include/net6.h   |  4 ++++
 net/net.c        |  3 +++
 net/net6.c       | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 net/tftp.c       | 58 ++++++++++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 152 insertions(+), 1 deletion(-)

diff --git a/common/Kconfig b/common/Kconfig
index b1effc6..5914328 100644
--- a/common/Kconfig
+++ b/common/Kconfig
@@ -400,6 +400,15 @@ config CMD_NET
 	  bootp - boot image via network using BOOTP/TFTP protocol
 	  tftpboot - boot image via network using TFTP protocol
 
+config CMD_NET6
+	bool "ipv6 commands"
+	select NET
+	select NET6
+	default n
+	help
+	  IPv6 network commands
+	  tftpboot6 - boot image via network using TFTP protocol
+
 config CMD_TFTPPUT
 	bool "tftp put"
 	help
diff --git a/common/cmd_net.c b/common/cmd_net.c
index 271f91d..47b56ee 100644
--- a/common/cmd_net.c
+++ b/common/cmd_net.c
@@ -42,6 +42,19 @@ U_BOOT_CMD(
 	"[loadAddress] [[hostIPaddr:]bootfilename]"
 );
 
+#ifdef CONFIG_CMD_NET6
+int do_tftpb6(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	return netboot_common(TFTP6, cmdtp, argc, argv);
+}
+
+U_BOOT_CMD(
+	tftpboot6,	3,	1,	do_tftpb6,
+	"boot image via network using TFTP protocol",
+	"[loadAddress] [[hostIP6Addr]:][bootfilename]"
+);
+#endif
+
 #ifdef CONFIG_CMD_TFTPPUT
 int do_tftpput(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
diff --git a/include/net.h b/include/net.h
index 6a9832c..69e6a17 100644
--- a/include/net.h
+++ b/include/net.h
@@ -525,7 +525,7 @@ extern int		net_restart_wrap;	/* Tried all network devices */
 
 enum proto_t {
 	BOOTP, RARP, ARP, TFTPGET, DHCP, PING, PING6, DNS, NFS, CDP, NETCONS,
-	SNTP, TFTPSRV, TFTPPUT, LINKLOCAL
+	SNTP, TFTPSRV, TFTPPUT, TFTP6, LINKLOCAL
 };
 
 extern char	net_boot_file_name[1024];/* Boot File name */
diff --git a/include/net6.h b/include/net6.h
index ff97c39..7ae1777 100644
--- a/include/net6.h
+++ b/include/net6.h
@@ -246,6 +246,10 @@ void ping6_start(void);
 void ping6_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6,
 			  int len);
 
+/* Transmit UDP packet using IPv6, performing neighbour discovery if needed */
+int net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
+				int dport, int sport, int len);
+
 /* handler for incoming IPv6 echo packet */
 void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
 			    int len);
diff --git a/net/net.c b/net/net.c
index ca24673..2b0d17e 100644
--- a/net/net.c
+++ b/net/net.c
@@ -453,6 +453,9 @@ restart:
 #ifdef CONFIG_CMD_TFTPPUT
 		case TFTPPUT:
 #endif
+#ifdef CONFIG_CMD_NET6
+		case TFTP6:
+#endif
 			/* always use ARP to get server ethernet address */
 			tftp_start(protocol);
 			break;
diff --git a/net/net6.c b/net/net6.c
index 8f0c721..5b8a003 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -342,6 +342,50 @@ ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *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;
+
+	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;
+	udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
+		IPPROTO_UDP, csum_partial((__u8 *)udp, len + UDP_HDR_SIZE, 0));
+
+	/* if MAC address was not discovered yet, save the packet and do neighbour discovery */
+	if (memcmp(ether, net_null_ethaddr, 6) == 0) {
+		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 */
+}
+
 void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 {
 	struct in_addr zero_ip = {.s_addr = 0 };
@@ -388,6 +432,26 @@ void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 		}
 		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 */
+		udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+				hlen, IPPROTO_UDP, csum_partial((__u8 *)udp, hlen, 0));
+		if (csum != udp->udp_xsum)
+			return;
+
+		/* 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;
 		break;
diff --git a/net/tftp.c b/net/tftp.c
index f2889fe..aa9e6e4 100644
--- a/net/tftp.c
+++ b/net/tftp.c
@@ -10,6 +10,7 @@
 #include <command.h>
 #include <mapmem.h>
 #include <net.h>
+#include <net6.h>
 #include <net/tftp.h>
 #include "bootp.h"
 #ifdef CONFIG_SYS_DIRECT_FLASH_TFTP
@@ -66,6 +67,9 @@ enum {
 };
 
 static struct in_addr tftp_remote_ip;
+#ifdef CONFIG_CMD_NET6
+static struct in6_addr tftp_remote_ip6;
+#endif
 /* The UDP port at their end */
 static int	tftp_remote_port;
 /* The UDP port at our end */
@@ -94,6 +98,10 @@ static int	tftp_put_final_block_sent;
 #else
 #define tftp_put_active	0
 #endif
+#ifdef CONFIG_CMD_NET6
+/* 1 if using IPv6, else 0 */
+static int      tftp6_active;
+#endif
 
 #define STATE_SEND_RRQ	1
 #define STATE_DATA	2
@@ -129,6 +137,8 @@ static char tftp_filename[MAX_LEN];
 #else
 #define TFTP_MTU_BLOCKSIZE 1468
 #endif
+/* IPv6 adds 20 bytes extra overhead */
+#define TFTP_MTU_BLOCKSIZE6 (TFTP_MTU_BLOCKSIZE - 20)
 
 static unsigned short tftp_block_size = TFTP_BLOCK_SIZE;
 static unsigned short tftp_block_size_option = TFTP_MTU_BLOCKSIZE;
@@ -341,6 +351,12 @@ static void tftp_send(void)
 	 *	We will always be sending some sort of packet, so
 	 *	cobble together the packet headers now.
 	 */
+#ifdef CONFIG_CMD_NET6
+	if (tftp6_active)
+		pkt = net_tx_packet + net_eth_hdr_size() +
+		      IP6_HDR_SIZE + UDP_HDR_SIZE;
+	else
+#endif
 	pkt = net_tx_packet + net_eth_hdr_size() + IP_UDP_HDR_SIZE;
 
 	switch (tftp_state) {
@@ -440,6 +456,12 @@ static void tftp_send(void)
 		break;
 	}
 
+#ifdef CONFIG_CMD_NET6
+	if (tftp6_active)
+		net_send_udp_packet6(net_server_ethaddr, &tftp_remote_ip6,
+				     tftp_remote_port, tftp_our_port, len);
+	else
+#endif
 	net_send_udp_packet(net_server_ethaddr, tftp_remote_ip,
 			    tftp_remote_port, tftp_our_port, len);
 }
@@ -733,6 +755,10 @@ void tftp_start(enum proto_t protocol)
 	debug("TFTP blocksize = %i, timeout = %ld ms\n",
 	      tftp_block_size_option, timeout_ms);
 
+#ifdef CONFIG_CMD_NET6
+	tftp6_active = (protocol == TFTP6);
+	tftp_remote_ip6 = net_server_ip6;
+#endif
 	tftp_remote_ip = net_server_ip;
 	if (net_boot_file_name[0] == '\0') {
 		sprintf(default_filename, "%02X%02X%02X%02X.img",
@@ -746,6 +772,20 @@ void tftp_start(enum proto_t protocol)
 
 		printf("*** Warning: no boot file name; using '%s'\n",
 		       tftp_filename);
+#ifdef CONFIG_CMD_NET6
+	} else if (tftp6_active) {
+		char *s, *e;
+		s = strchr(net_boot_file_name, '[');
+		e = strchr(net_boot_file_name, ']');
+		if (s && e) {
+			*e++ = 0;
+			string_to_ip6(s + 1, &tftp_remote_ip6);
+			strncpy(tftp_filename, e + 1, MAX_LEN);
+		} else {
+			strncpy(tftp_filename, net_boot_file_name, MAX_LEN);
+			tftp_filename[MAX_LEN - 1] = 0;
+		}
+#endif
 	} else {
 		char *p = strchr(net_boot_file_name, ':');
 
@@ -760,6 +800,15 @@ void tftp_start(enum proto_t protocol)
 	}
 
 	printf("Using %s device\n", eth_get_name());
+#ifdef CONFIG_CMD_NET6
+	if (tftp6_active) {
+		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
+#endif
 	printf("TFTP %s server %pI4; our IP address is %pI4",
 #ifdef CONFIG_CMD_TFTPPUT
 	       protocol == TFTPPUT ? "to" : "from",
@@ -769,6 +818,15 @@ void tftp_start(enum proto_t protocol)
 	       &tftp_remote_ip, &net_ip);
 
 	/* Check if we need to send across this subnet */
+#ifdef CONFIG_CMD_NET6
+	if (tftp6_active) {
+		if (!ip6_addr_in_subnet(&net_ip6, &tftp_remote_ip6,
+					net_prefix_length)) {
+			printf("; sending through gateway %pI6c",
+			       &net_gateway6);
+		}
+	} else
+#endif
 	if (net_gateway.s_addr && net_netmask.s_addr) {
 		struct in_addr our_net;
 		struct in_addr remote_net;
-- 
2.5.3