Commit | Line | Data |
---|---|---|
83d290c5 | 1 | // SPDX-License-Identifier: GPL-2.0 |
3ea143ab JH |
2 | /* |
3 | * Copyright (c) 2015 National Instruments | |
4 | * | |
5 | * (C) Copyright 2015 | |
6 | * Joe Hershberger <joe.hershberger@ni.com> | |
3ea143ab JH |
7 | */ |
8 | ||
9 | #include <common.h> | |
10 | #include <dm.h> | |
f7ae49fc | 11 | #include <log.h> |
3ea143ab JH |
12 | #include <malloc.h> |
13 | #include <net.h> | |
c7eb733d | 14 | #include <asm/eth.h> |
401d1c4f | 15 | #include <asm/global_data.h> |
6f2707c6 | 16 | #include <asm/test.h> |
3ea143ab JH |
17 | |
18 | DECLARE_GLOBAL_DATA_PTR; | |
19 | ||
6f2707c6 | 20 | static bool skip_timeout; |
2eede1f3 JH |
21 | |
22 | /* | |
23 | * sandbox_eth_disable_response() | |
24 | * | |
25 | * index - The alias index (also DM seq number) | |
26 | * disable - If non-zero, ignore sent packets and don't send mock response | |
27 | */ | |
28 | void sandbox_eth_disable_response(int index, bool disable) | |
29 | { | |
e4ab9a65 JH |
30 | struct udevice *dev; |
31 | struct eth_sandbox_priv *priv; | |
32 | int ret; | |
33 | ||
34 | ret = uclass_get_device(UCLASS_ETH, index, &dev); | |
35 | if (ret) | |
36 | return; | |
37 | ||
38 | priv = dev_get_priv(dev); | |
39 | priv->disabled = disable; | |
2eede1f3 JH |
40 | } |
41 | ||
6f2707c6 JH |
42 | /* |
43 | * sandbox_eth_skip_timeout() | |
44 | * | |
45 | * When the first packet read is attempted, fast-forward time | |
46 | */ | |
47 | void sandbox_eth_skip_timeout(void) | |
48 | { | |
49 | skip_timeout = true; | |
50 | } | |
51 | ||
e95bb161 JH |
52 | /* |
53 | * sandbox_eth_arp_req_to_reply() | |
54 | * | |
55 | * Check for an arp request to be sent. If so, inject a reply | |
56 | * | |
57 | * returns 0 if injected, -EAGAIN if not | |
58 | */ | |
59 | int sandbox_eth_arp_req_to_reply(struct udevice *dev, void *packet, | |
60 | unsigned int len) | |
61 | { | |
62 | struct eth_sandbox_priv *priv = dev_get_priv(dev); | |
63 | struct ethernet_hdr *eth = packet; | |
64 | struct arp_hdr *arp; | |
65 | struct ethernet_hdr *eth_recv; | |
66 | struct arp_hdr *arp_recv; | |
67 | ||
68 | if (ntohs(eth->et_protlen) != PROT_ARP) | |
69 | return -EAGAIN; | |
70 | ||
71 | arp = packet + ETHER_HDR_SIZE; | |
72 | ||
73 | if (ntohs(arp->ar_op) != ARPOP_REQUEST) | |
74 | return -EAGAIN; | |
75 | ||
c67a4207 JH |
76 | /* Don't allow the buffer to overrun */ |
77 | if (priv->recv_packets >= PKTBUFSRX) | |
78 | return 0; | |
79 | ||
e95bb161 JH |
80 | /* store this as the assumed IP of the fake host */ |
81 | priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa); | |
82 | ||
83 | /* Formulate a fake response */ | |
c67a4207 | 84 | eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets]; |
e95bb161 JH |
85 | memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN); |
86 | memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); | |
87 | eth_recv->et_protlen = htons(PROT_ARP); | |
88 | ||
89 | arp_recv = (void *)eth_recv + ETHER_HDR_SIZE; | |
90 | arp_recv->ar_hrd = htons(ARP_ETHER); | |
91 | arp_recv->ar_pro = htons(PROT_IP); | |
92 | arp_recv->ar_hln = ARP_HLEN; | |
93 | arp_recv->ar_pln = ARP_PLEN; | |
94 | arp_recv->ar_op = htons(ARPOP_REPLY); | |
95 | memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN); | |
96 | net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr); | |
97 | memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN); | |
98 | net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa); | |
99 | ||
c67a4207 JH |
100 | priv->recv_packet_length[priv->recv_packets] = |
101 | ETHER_HDR_SIZE + ARP_HDR_SIZE; | |
102 | ++priv->recv_packets; | |
e95bb161 JH |
103 | |
104 | return 0; | |
105 | } | |
106 | ||
107 | /* | |
108 | * sandbox_eth_ping_req_to_reply() | |
109 | * | |
110 | * Check for a ping request to be sent. If so, inject a reply | |
111 | * | |
112 | * returns 0 if injected, -EAGAIN if not | |
113 | */ | |
114 | int sandbox_eth_ping_req_to_reply(struct udevice *dev, void *packet, | |
115 | unsigned int len) | |
116 | { | |
117 | struct eth_sandbox_priv *priv = dev_get_priv(dev); | |
118 | struct ethernet_hdr *eth = packet; | |
119 | struct ip_udp_hdr *ip; | |
120 | struct icmp_hdr *icmp; | |
121 | struct ethernet_hdr *eth_recv; | |
122 | struct ip_udp_hdr *ipr; | |
123 | struct icmp_hdr *icmpr; | |
124 | ||
125 | if (ntohs(eth->et_protlen) != PROT_IP) | |
126 | return -EAGAIN; | |
127 | ||
128 | ip = packet + ETHER_HDR_SIZE; | |
129 | ||
130 | if (ip->ip_p != IPPROTO_ICMP) | |
131 | return -EAGAIN; | |
132 | ||
133 | icmp = (struct icmp_hdr *)&ip->udp_src; | |
134 | ||
135 | if (icmp->type != ICMP_ECHO_REQUEST) | |
136 | return -EAGAIN; | |
137 | ||
c67a4207 JH |
138 | /* Don't allow the buffer to overrun */ |
139 | if (priv->recv_packets >= PKTBUFSRX) | |
140 | return 0; | |
141 | ||
e95bb161 | 142 | /* reply to the ping */ |
c67a4207 | 143 | eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets]; |
e95bb161 JH |
144 | memcpy(eth_recv, packet, len); |
145 | ipr = (void *)eth_recv + ETHER_HDR_SIZE; | |
146 | icmpr = (struct icmp_hdr *)&ipr->udp_src; | |
147 | memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN); | |
148 | memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); | |
149 | ipr->ip_sum = 0; | |
150 | ipr->ip_off = 0; | |
151 | net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src); | |
152 | net_write_ip((void *)&ipr->ip_src, priv->fake_host_ipaddr); | |
153 | ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE); | |
154 | ||
155 | icmpr->type = ICMP_ECHO_REPLY; | |
156 | icmpr->checksum = 0; | |
157 | icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE); | |
158 | ||
c67a4207 JH |
159 | priv->recv_packet_length[priv->recv_packets] = len; |
160 | ++priv->recv_packets; | |
e95bb161 JH |
161 | |
162 | return 0; | |
163 | } | |
164 | ||
45988dae JH |
165 | /* |
166 | * sandbox_eth_recv_arp_req() | |
167 | * | |
168 | * Inject an ARP request for this target | |
169 | * | |
170 | * returns 0 if injected, -EOVERFLOW if not | |
171 | */ | |
172 | int sandbox_eth_recv_arp_req(struct udevice *dev) | |
173 | { | |
174 | struct eth_sandbox_priv *priv = dev_get_priv(dev); | |
175 | struct ethernet_hdr *eth_recv; | |
176 | struct arp_hdr *arp_recv; | |
177 | ||
178 | /* Don't allow the buffer to overrun */ | |
179 | if (priv->recv_packets >= PKTBUFSRX) | |
180 | return -EOVERFLOW; | |
181 | ||
182 | /* Formulate a fake request */ | |
183 | eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets]; | |
184 | memcpy(eth_recv->et_dest, net_bcast_ethaddr, ARP_HLEN); | |
185 | memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); | |
186 | eth_recv->et_protlen = htons(PROT_ARP); | |
187 | ||
188 | arp_recv = (void *)eth_recv + ETHER_HDR_SIZE; | |
189 | arp_recv->ar_hrd = htons(ARP_ETHER); | |
190 | arp_recv->ar_pro = htons(PROT_IP); | |
191 | arp_recv->ar_hln = ARP_HLEN; | |
192 | arp_recv->ar_pln = ARP_PLEN; | |
193 | arp_recv->ar_op = htons(ARPOP_REQUEST); | |
194 | memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr, ARP_HLEN); | |
195 | net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr); | |
196 | memcpy(&arp_recv->ar_tha, net_null_ethaddr, ARP_HLEN); | |
197 | net_write_ip(&arp_recv->ar_tpa, net_ip); | |
198 | ||
199 | priv->recv_packet_length[priv->recv_packets] = | |
200 | ETHER_HDR_SIZE + ARP_HDR_SIZE; | |
201 | ++priv->recv_packets; | |
202 | ||
203 | return 0; | |
204 | } | |
205 | ||
72ff0042 JH |
206 | /* |
207 | * sandbox_eth_recv_ping_req() | |
208 | * | |
209 | * Inject a ping request for this target | |
210 | * | |
211 | * returns 0 if injected, -EOVERFLOW if not | |
212 | */ | |
213 | int sandbox_eth_recv_ping_req(struct udevice *dev) | |
214 | { | |
215 | struct eth_sandbox_priv *priv = dev_get_priv(dev); | |
216 | struct ethernet_hdr *eth_recv; | |
217 | struct ip_udp_hdr *ipr; | |
218 | struct icmp_hdr *icmpr; | |
219 | ||
220 | /* Don't allow the buffer to overrun */ | |
221 | if (priv->recv_packets >= PKTBUFSRX) | |
222 | return -EOVERFLOW; | |
223 | ||
224 | /* Formulate a fake ping */ | |
225 | eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets]; | |
226 | ||
227 | memcpy(eth_recv->et_dest, net_ethaddr, ARP_HLEN); | |
228 | memcpy(eth_recv->et_src, priv->fake_host_hwaddr, ARP_HLEN); | |
229 | eth_recv->et_protlen = htons(PROT_IP); | |
230 | ||
231 | ipr = (void *)eth_recv + ETHER_HDR_SIZE; | |
232 | ipr->ip_hl_v = 0x45; | |
233 | ipr->ip_len = htons(IP_ICMP_HDR_SIZE); | |
234 | ipr->ip_off = htons(IP_FLAGS_DFRAG); | |
235 | ipr->ip_p = IPPROTO_ICMP; | |
236 | ipr->ip_sum = 0; | |
237 | net_write_ip(&ipr->ip_src, priv->fake_host_ipaddr); | |
238 | net_write_ip(&ipr->ip_dst, net_ip); | |
239 | ipr->ip_sum = compute_ip_checksum(ipr, IP_HDR_SIZE); | |
240 | ||
241 | icmpr = (struct icmp_hdr *)&ipr->udp_src; | |
242 | ||
243 | icmpr->type = ICMP_ECHO_REQUEST; | |
244 | icmpr->code = 0; | |
245 | icmpr->checksum = 0; | |
246 | icmpr->un.echo.id = 0; | |
247 | icmpr->un.echo.sequence = htons(1); | |
248 | icmpr->checksum = compute_ip_checksum(icmpr, ICMP_HDR_SIZE); | |
249 | ||
250 | priv->recv_packet_length[priv->recv_packets] = | |
251 | ETHER_HDR_SIZE + IP_ICMP_HDR_SIZE; | |
252 | ++priv->recv_packets; | |
253 | ||
254 | return 0; | |
255 | } | |
256 | ||
c7eb733d JH |
257 | /* |
258 | * sb_default_handler() | |
259 | * | |
260 | * perform typical responses to simple ping | |
261 | * | |
262 | * dev - device pointer | |
263 | * pkt - "sent" packet buffer | |
264 | * len - length of packet | |
265 | */ | |
266 | static int sb_default_handler(struct udevice *dev, void *packet, | |
267 | unsigned int len) | |
268 | { | |
269 | if (!sandbox_eth_arp_req_to_reply(dev, packet, len)) | |
270 | return 0; | |
271 | if (!sandbox_eth_ping_req_to_reply(dev, packet, len)) | |
272 | return 0; | |
273 | ||
274 | return 0; | |
275 | } | |
276 | ||
277 | /* | |
278 | * sandbox_eth_set_tx_handler() | |
279 | * | |
280 | * Set a custom response to a packet being sent through the sandbox eth test | |
281 | * driver | |
282 | * | |
283 | * index - interface to set the handler for | |
284 | * handler - The func ptr to call on send. If NULL, set to default handler | |
285 | */ | |
286 | void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler) | |
287 | { | |
288 | struct udevice *dev; | |
289 | struct eth_sandbox_priv *priv; | |
290 | int ret; | |
291 | ||
292 | ret = uclass_get_device(UCLASS_ETH, index, &dev); | |
293 | if (ret) | |
294 | return; | |
295 | ||
296 | priv = dev_get_priv(dev); | |
297 | if (handler) | |
298 | priv->tx_handler = handler; | |
299 | else | |
300 | priv->tx_handler = sb_default_handler; | |
301 | } | |
302 | ||
9cbe5972 JH |
303 | /* |
304 | * Set priv ptr | |
305 | * | |
306 | * priv - priv void ptr to store in the device | |
307 | */ | |
308 | void sandbox_eth_set_priv(int index, void *priv) | |
309 | { | |
310 | struct udevice *dev; | |
311 | struct eth_sandbox_priv *dev_priv; | |
312 | int ret; | |
313 | ||
314 | ret = uclass_get_device(UCLASS_ETH, index, &dev); | |
315 | if (ret) | |
316 | return; | |
317 | ||
318 | dev_priv = dev_get_priv(dev); | |
319 | ||
320 | dev_priv->priv = priv; | |
321 | } | |
322 | ||
3ea143ab JH |
323 | static int sb_eth_start(struct udevice *dev) |
324 | { | |
d87a457b JH |
325 | struct eth_sandbox_priv *priv = dev_get_priv(dev); |
326 | ||
3ea143ab JH |
327 | debug("eth_sandbox: Start\n"); |
328 | ||
c67a4207 JH |
329 | priv->recv_packets = 0; |
330 | for (int i = 0; i < PKTBUFSRX; i++) { | |
331 | priv->recv_packet_buffer[i] = net_rx_packets[i]; | |
332 | priv->recv_packet_length[i] = 0; | |
333 | } | |
b32dd183 | 334 | |
3ea143ab JH |
335 | return 0; |
336 | } | |
337 | ||
338 | static int sb_eth_send(struct udevice *dev, void *packet, int length) | |
339 | { | |
d87a457b | 340 | struct eth_sandbox_priv *priv = dev_get_priv(dev); |
d87a457b | 341 | |
3ea143ab JH |
342 | debug("eth_sandbox: Send packet %d\n", length); |
343 | ||
e4ab9a65 | 344 | if (priv->disabled) |
2eede1f3 JH |
345 | return 0; |
346 | ||
c7eb733d | 347 | return priv->tx_handler(dev, packet, length); |
3ea143ab JH |
348 | } |
349 | ||
a1ca92ea | 350 | static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp) |
3ea143ab | 351 | { |
d87a457b JH |
352 | struct eth_sandbox_priv *priv = dev_get_priv(dev); |
353 | ||
6f2707c6 | 354 | if (skip_timeout) { |
d0a9b82b | 355 | timer_test_add_offset(11000UL); |
6f2707c6 JH |
356 | skip_timeout = false; |
357 | } | |
358 | ||
c67a4207 JH |
359 | if (priv->recv_packets) { |
360 | int lcl_recv_packet_length = priv->recv_packet_length[0]; | |
d87a457b | 361 | |
c67a4207 JH |
362 | debug("eth_sandbox: received packet[%d], %d waiting\n", |
363 | lcl_recv_packet_length, priv->recv_packets - 1); | |
364 | *packetp = priv->recv_packet_buffer[0]; | |
d87a457b JH |
365 | return lcl_recv_packet_length; |
366 | } | |
3ea143ab JH |
367 | return 0; |
368 | } | |
369 | ||
c67a4207 JH |
370 | static int sb_eth_free_pkt(struct udevice *dev, uchar *packet, int length) |
371 | { | |
372 | struct eth_sandbox_priv *priv = dev_get_priv(dev); | |
373 | int i; | |
374 | ||
375 | if (!priv->recv_packets) | |
376 | return 0; | |
377 | ||
378 | --priv->recv_packets; | |
379 | for (i = 0; i < priv->recv_packets; i++) { | |
380 | priv->recv_packet_length[i] = priv->recv_packet_length[i + 1]; | |
381 | memcpy(priv->recv_packet_buffer[i], | |
382 | priv->recv_packet_buffer[i + 1], | |
383 | priv->recv_packet_length[i + 1]); | |
384 | } | |
385 | priv->recv_packet_length[priv->recv_packets] = 0; | |
386 | ||
387 | return 0; | |
388 | } | |
389 | ||
3ea143ab JH |
390 | static void sb_eth_stop(struct udevice *dev) |
391 | { | |
392 | debug("eth_sandbox: Stop\n"); | |
393 | } | |
394 | ||
395 | static int sb_eth_write_hwaddr(struct udevice *dev) | |
396 | { | |
c69cda25 | 397 | struct eth_pdata *pdata = dev_get_plat(dev); |
3ea143ab JH |
398 | |
399 | debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name, | |
400 | pdata->enetaddr); | |
401 | return 0; | |
402 | } | |
403 | ||
404 | static const struct eth_ops sb_eth_ops = { | |
405 | .start = sb_eth_start, | |
406 | .send = sb_eth_send, | |
407 | .recv = sb_eth_recv, | |
c67a4207 | 408 | .free_pkt = sb_eth_free_pkt, |
3ea143ab JH |
409 | .stop = sb_eth_stop, |
410 | .write_hwaddr = sb_eth_write_hwaddr, | |
411 | }; | |
412 | ||
413 | static int sb_eth_remove(struct udevice *dev) | |
414 | { | |
415 | return 0; | |
416 | } | |
417 | ||
d1998a9f | 418 | static int sb_eth_of_to_plat(struct udevice *dev) |
3ea143ab | 419 | { |
c69cda25 | 420 | struct eth_pdata *pdata = dev_get_plat(dev); |
b32dd183 JH |
421 | struct eth_sandbox_priv *priv = dev_get_priv(dev); |
422 | const u8 *mac; | |
423 | ||
424 | pdata->iobase = dev_read_addr(dev); | |
425 | ||
426 | mac = dev_read_u8_array_ptr(dev, "fake-host-hwaddr", ARP_HLEN); | |
427 | if (!mac) { | |
428 | printf("'fake-host-hwaddr' is missing from the DT\n"); | |
429 | return -EINVAL; | |
430 | } | |
431 | memcpy(priv->fake_host_hwaddr, mac, ARP_HLEN); | |
e4ab9a65 | 432 | priv->disabled = false; |
c7eb733d | 433 | priv->tx_handler = sb_default_handler; |
3ea143ab | 434 | |
3ea143ab JH |
435 | return 0; |
436 | } | |
437 | ||
438 | static const struct udevice_id sb_eth_ids[] = { | |
439 | { .compatible = "sandbox,eth" }, | |
440 | { } | |
441 | }; | |
442 | ||
443 | U_BOOT_DRIVER(eth_sandbox) = { | |
444 | .name = "eth_sandbox", | |
445 | .id = UCLASS_ETH, | |
446 | .of_match = sb_eth_ids, | |
d1998a9f | 447 | .of_to_plat = sb_eth_of_to_plat, |
3ea143ab JH |
448 | .remove = sb_eth_remove, |
449 | .ops = &sb_eth_ops, | |
41575d8e | 450 | .priv_auto = sizeof(struct eth_sandbox_priv), |
caa4daa2 | 451 | .plat_auto = sizeof(struct eth_pdata), |
3ea143ab | 452 | }; |