1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Copyright (C) 2013 Allied Telesis Labs NZ
4 * Chris Packham, <judge.packham@gmail.com>
5 *
6 * Copyright (C) 2022 YADRO
7 * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
8 */
9
10/* Simple ping6 implementation */
11
12#include <common.h>
13#include <net.h>
14#include <net6.h>
15#include "ndisc.h"
16
17static ushort seq_no;
18
19/* the ipv6 address to ping */
20struct in6_addr net_ping_ip6;
21
22int
23ip6_make_ping(uchar *eth_dst_addr, struct in6_addr *neigh_addr, uchar *pkt)
24{
25	struct echo_msg *msg;
26	u16 len;
27	u16 csum_p;
28	uchar *pkt_old = pkt;
29
30	len = sizeof(struct echo_msg);
31
32	pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
33	pkt += ip6_add_hdr(pkt, &net_ip6, neigh_addr, PROT_ICMPV6,
34			   IPV6_NDISC_HOPLIMIT, len);
35
36	/* ICMPv6 - Echo */
37	msg = (struct echo_msg *)pkt;
38	msg->icmph.icmp6_type = IPV6_ICMP_ECHO_REQUEST;
39	msg->icmph.icmp6_code = 0;
40	msg->icmph.icmp6_cksum = 0;
41	msg->icmph.icmp6_identifier = 0;
42	msg->icmph.icmp6_sequence = htons(seq_no++);
43	msg->id = msg->icmph.icmp6_identifier;	/* these seem redundant */
44	msg->sequence = msg->icmph.icmp6_sequence;
45
46	/* checksum */
47	csum_p = csum_partial((u8 *)msg, len, 0);
48	msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_ip6, neigh_addr, len,
49						 PROT_ICMPV6, csum_p);
50
51	pkt += len;
52
53	return pkt - pkt_old;
54}
55
56int ping6_send(void)
57{
58	uchar *pkt;
59	static uchar mac[6];
60
61	/* always send neighbor solicit */
62
63	memcpy(mac, net_null_ethaddr, 6);
64
65	net_nd_sol_packet_ip6 = net_ping_ip6;
66	net_nd_packet_mac = mac;
67
68	pkt = net_nd_tx_packet;
69	pkt += ip6_make_ping(mac, &net_ping_ip6, pkt);
70
71	/* size of the waiting packet */
72	net_nd_tx_packet_size = (pkt - net_nd_tx_packet);
73
74	/* and do the ARP request */
75	net_nd_try = 1;
76	net_nd_timer_start = get_timer(0);
77	ndisc_request();
78	return 1;		/* waiting */
79}
80
81static void ping6_timeout(void)
82{
83	eth_halt();
84	net_set_state(NETLOOP_FAIL);	/* we did not get the reply */
85}
86
87void ping6_start(void)
88{
89	printf("Using %s device\n", eth_get_name());
90	net_set_timeout_handler(10000UL, ping6_timeout);
91
92	ping6_send();
93}
94
95int ping6_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
96{
97	struct icmp6hdr *icmp =
98	    (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
99	struct in6_addr src_ip;
100
101	switch (icmp->icmp6_type) {
102	case IPV6_ICMP_ECHO_REPLY:
103		src_ip = ip6->saddr;
104		if (memcmp(&net_ping_ip6, &src_ip, sizeof(struct in6_addr)))
105			return -EINVAL;
106		net_set_state(NETLOOP_SUCCESS);
107		break;
108	case IPV6_ICMP_ECHO_REQUEST:
109		/* ignore for now.... */
110		debug("Got ICMPv6 ECHO REQUEST from %pI6c\n", &ip6->saddr);
111		return -EINVAL;
112	default:
113		debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
114		return -EINVAL;
115	}
116
117	return 0;
118}
119