1 // SPDX-License-Identifier: GPL-2.0+
2 /* Copyright (C) 2024 Linaro Ltd. */
3
4 #include <command.h>
5 #include <console.h>
6 #include <dm/device.h>
7 #include <linux/delay.h>
8 #include <linux/errno.h>
9 #include <lwip/icmp.h>
10 #include <lwip/inet_chksum.h>
11 #include <lwip/raw.h>
12 #include <lwip/timeouts.h>
13 #include <net.h>
14 #include <time.h>
15
16 U_BOOT_CMD(ping, 2, 1, do_ping, "send ICMP ECHO_REQUEST to network host",
17 "pingAddressOrHostName");
18
19 #define PING_DELAY_MS 1000
20 #define PING_COUNT 5
21 /* Ping identifier - must fit on a u16_t */
22 #define PING_ID 0xAFAF
23
24 struct ping_ctx {
25 ip_addr_t target;
26 struct raw_pcb *pcb;
27 struct icmp_echo_hdr *iecho;
28 uint16_t seq_num;
29 bool alive;
30 };
31
ping_recv(void * arg,struct raw_pcb * pcb,struct pbuf * p,const ip_addr_t * addr)32 static u8_t ping_recv(void *arg, struct raw_pcb *pcb, struct pbuf *p,
33 const ip_addr_t *addr)
34 {
35 struct ping_ctx *ctx = arg;
36 struct icmp_echo_hdr *iecho = ctx->iecho;
37
38 if (addr->addr != ctx->target.addr)
39 return 0;
40
41 if ((p->tot_len >= (IP_HLEN + sizeof(struct icmp_echo_hdr))) &&
42 pbuf_remove_header(p, IP_HLEN) == 0) {
43 iecho = (struct icmp_echo_hdr *)p->payload;
44
45 if (iecho->id == PING_ID &&
46 iecho->seqno == lwip_htons(ctx->seq_num)) {
47 ctx->alive = true;
48 printf("host %s is alive\n", ipaddr_ntoa(addr));
49 pbuf_free(p);
50 return 1; /* eat the packet */
51 }
52 /* not eaten, restore original packet */
53 pbuf_add_header(p, IP_HLEN);
54 }
55
56 return 0; /* don't eat the packet */
57 }
58
ping_raw_init(struct ping_ctx * ctx)59 static int ping_raw_init(struct ping_ctx *ctx)
60 {
61 ctx->pcb = raw_new(IP_PROTO_ICMP);
62 if (!ctx->pcb)
63 return -ENOMEM;
64
65 raw_recv(ctx->pcb, ping_recv, ctx);
66 raw_bind(ctx->pcb, IP_ADDR_ANY);
67
68 return 0;
69 }
70
ping_raw_stop(struct ping_ctx * ctx)71 static void ping_raw_stop(struct ping_ctx *ctx)
72 {
73 if (ctx->pcb)
74 raw_remove(ctx->pcb);
75 }
76
ping_prepare_echo(struct ping_ctx * ctx)77 static void ping_prepare_echo(struct ping_ctx *ctx)
78 {
79 struct icmp_echo_hdr *iecho = ctx->iecho;
80
81 ICMPH_TYPE_SET(iecho, ICMP_ECHO);
82 ICMPH_CODE_SET(iecho, 0);
83 iecho->chksum = 0;
84 iecho->id = PING_ID;
85 iecho->seqno = lwip_htons(ctx->seq_num);
86
87 iecho->chksum = inet_chksum(iecho, sizeof(*iecho));
88 }
89
ping_send_icmp(struct ping_ctx * ctx)90 static void ping_send_icmp(struct ping_ctx *ctx)
91 {
92 struct pbuf *p;
93 size_t ping_size = sizeof(struct icmp_echo_hdr);
94
95 p = pbuf_alloc(PBUF_IP, (u16_t)ping_size, PBUF_RAM);
96 if (!p)
97 return;
98
99 if (p->len == p->tot_len && !p->next) {
100 ctx->iecho = (struct icmp_echo_hdr *)p->payload;
101 ping_prepare_echo(ctx);
102 raw_sendto(ctx->pcb, p, &ctx->target);
103 }
104
105 pbuf_free(p);
106 }
107
ping_send(void * arg)108 static void ping_send(void *arg)
109 {
110 struct ping_ctx *ctx = arg;
111
112 ctx->seq_num++;
113 if (ctx->seq_num <= PING_COUNT) {
114 ping_send_icmp(ctx);
115 sys_timeout(PING_DELAY_MS, ping_send, ctx);
116 }
117 }
118
ping_loop(struct udevice * udev,const ip_addr_t * addr)119 static int ping_loop(struct udevice *udev, const ip_addr_t *addr)
120 {
121 struct ping_ctx ctx = {};
122 struct netif *netif;
123 int ret;
124
125 netif = net_lwip_new_netif(udev);
126 if (!netif)
127 return -ENODEV;
128
129 printf("Using %s device\n", udev->name);
130
131 ret = ping_raw_init(&ctx);
132 if (ret < 0) {
133 net_lwip_remove_netif(netif);
134 return ret;
135 }
136
137 ctx.target = *addr;
138
139 ping_send(&ctx);
140
141 do {
142 net_lwip_rx(udev, netif);
143 if (ctx.alive)
144 break;
145 if (ctrlc()) {
146 printf("\nAbort\n");
147 break;
148 }
149 } while (ctx.seq_num <= PING_COUNT);
150
151 sys_untimeout(ping_send, &ctx);
152 ping_raw_stop(&ctx);
153
154 net_lwip_remove_netif(netif);
155
156 if (ctx.alive)
157 return 0;
158
159 printf("ping failed; host %s is not alive\n", ipaddr_ntoa(addr));
160 return -1;
161 }
162
do_ping(struct cmd_tbl * cmdtp,int flag,int argc,char * const argv[])163 int do_ping(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
164 {
165 ip_addr_t addr;
166
167 if (argc < 2)
168 return CMD_RET_USAGE;
169
170 if (net_lwip_dns_resolve(argv[1], &addr))
171 return CMD_RET_USAGE;
172
173 net_try_count = 1;
174 restart:
175 if (net_lwip_eth_start() < 0 || ping_loop(eth_get_dev(), &addr) < 0) {
176 if (net_start_again() == 0)
177 goto restart;
178 else
179 return CMD_RET_FAILURE;
180 }
181
182 return CMD_RET_SUCCESS;
183 }
184