net: ping6: Add ping6 command

Implement ping6 command to ping hosts using IPv6. It works the same way as
an ordinary ping command. There is no ICMP request so it is not possible
to ping our host. This patch adds options in Kconfig and Makefile to
build ping6 command.

Series-changes: 3
- Added structures and functions descriptions
- Added to ping6_receive() return value instead of void

Series-changes: 4
- Fixed structures and functions description style

Signed-off-by: Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
Reviewed-by: Ramon Fried <rfried.dev@gmail.com>
Reviewed-by: Simon Glass <sjg@chromium.org>

4 files changed, 136 insertions, 0 deletions

diff --git a/net/Makefile b/net/Makefile
index df0ea500fe..13eef04029 100644
--- a/net/Makefile
+++ b/net/Makefile
@@ -25,6 +25,7 @@ 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/net.c b/net/net.c
index 28c8dfdef2..1c39acc493 100644
--- a/net/net.c
+++ b/net/net.c
@@ -525,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();
@@ -1434,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) {
diff --git a/net/net6.c b/net/net6.c
index e604227fe5..fdea078788 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -404,6 +404,10 @@ int net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 			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);
diff --git a/net/ping6.c b/net/ping6.c
new file mode 100644
index 0000000000..4882a17f51
--- /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;
+}