[U-Boot] [RFC PATCH v2 08/11] net: Add ping6 command and implementation

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

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

---

Changes in v2:
- split ping6 support into it's own patch

 common/Kconfig   |   6 +++
 common/cmd_net.c |  28 ++++++++++++++
 include/net.h    |   4 +-
 net/net6.c       |   7 ++++
 net/ping6.c      | 111 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 154 insertions(+), 2 deletions(-)
 create mode 100644 net/ping6.c

diff --git a/common/Kconfig b/common/Kconfig
index 0388a6c..b1effc6 100644
--- a/common/Kconfig
+++ b/common/Kconfig
@@ -431,6 +431,12 @@ config CMD_PING
 	help
 	  Send ICMP ECHO_REQUEST to network host
 
+config CMD_PING6
+	bool "ping6"
+	depends on CMD_NET6
+	help
+	  Send ICMPv6 ECHO_REQUEST to network host
+
 config CMD_CDP
 	bool "cdp"
 	help
diff --git a/common/cmd_net.c b/common/cmd_net.c
index b2f3c7b..271f91d 100644
--- a/common/cmd_net.c
+++ b/common/cmd_net.c
@@ -11,6 +11,7 @@
 #include <common.h>
 #include <command.h>
 #include <net.h>
+#include <net6.h>
 
 static int netboot_common(enum proto_t, cmd_tbl_t *, int, char * const []);
 
@@ -284,6 +285,33 @@ U_BOOT_CMD(
 );
 #endif
 
+#ifdef CONFIG_CMD_PING6
+int do_ping6(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	if (argc < 2)
+		return -1;
+
+	if (string_to_ip6(argv[1], &net_ping_ip6) != 0)
+		return CMD_RET_USAGE;
+
+	if (net_loop(PING6) < 0) {
+		printf("ping6 failed; host %pI6c is not alive\n",
+		       &net_ping_ip6);
+		return 1;
+	}
+
+	printf("host %pI6c is alive\n", &net_ping_ip6);
+
+	return 0;
+}
+
+U_BOOT_CMD(
+	ping6,  2,      1,      do_ping6,
+	"send ICMPv6 ECHO_REQUEST to network host",
+	"pingAddress"
+);
+#endif /* CONFIG_CMD_PING6 */
+
 #if defined(CONFIG_CMD_CDP)
 
 static void cdp_update_env(void)
diff --git a/include/net.h b/include/net.h
index 8b7c878..6a9832c 100644
--- a/include/net.h
+++ b/include/net.h
@@ -524,8 +524,8 @@ extern ushort		net_native_vlan;	/* Our Native VLAN */
 extern int		net_restart_wrap;	/* Tried all network devices */
 
 enum proto_t {
-	BOOTP, RARP, ARP, TFTPGET, DHCP, PING, DNS, NFS, CDP, NETCONS, SNTP,
-	TFTPSRV, TFTPPUT, LINKLOCAL
+	BOOTP, RARP, ARP, TFTPGET, DHCP, PING, PING6, DNS, NFS, CDP, NETCONS,
+	SNTP, TFTPSRV, TFTPPUT, LINKLOCAL
 };
 
 extern char	net_boot_file_name[1024];/* Boot File name */
diff --git a/net/net6.c b/net/net6.c
index 955a089..8f0c721 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -370,6 +370,13 @@ void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 			return;
 
 		switch (icmp->icmp6_type) {
+#ifdef CONFIG_CMD_PING6
+		case IPV6_ICMP_ECHO_REQUEST:
+		case IPV6_ICMP_ECHO_REPLY:
+			ping6_receive(et, ip6, len);
+			break;
+#endif /* CONFIG_CMD_PING6 */
+
 		case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
 		case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
 			ndisc_receive(et, ip6, len);
diff --git a/net/ping6.c b/net/ping6.c
new file mode 100644
index 0000000..34796c6
--- /dev/null
+++ b/net/ping6.c
@@ -0,0 +1,111 @@
+/*
+ * net/ping6.c
+ *
+ * (C) Copyright 2013 Allied Telesis Labs NZ
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#define DEBUG
+#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;
+	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, IPPROTO_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 */
+	msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_ip6, neigh_addr, len,
+						 IPPROTO_ICMPV6,
+						 csum_partial((__u8 *)msg, len, 0));
+
+	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();
+}
+
+void 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)) != 0)
+			return;
+		net_set_state(NETLOOP_SUCCESS);
+		break;
+	case IPV6_ICMP_ECHO_REQUEST:
+		debug("Got ICMPv6 ECHO REQUEST from %pI6c\n", &ip6->saddr);
+		/* ignore for now.... */
+		break;
+	default:
+		debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
+	}
+}
-- 
2.5.3