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
17 static ushort seq_no;
18
19 /* the ipv6 address to ping */
20 struct in6_addr net_ping_ip6;
21
22 int
ip6_make_ping(uchar * eth_dst_addr,struct in6_addr * neigh_addr,uchar * pkt)23 ip6_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
ping6_send(void)56 int 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
ping6_timeout(void)81 static void ping6_timeout(void)
82 {
83 eth_halt();
84 net_set_state(NETLOOP_FAIL); /* we did not get the reply */
85 }
86
ping6_start(void)87 void 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
ping6_receive(struct ethernet_hdr * et,struct ip6_hdr * ip6,int len)95 int 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