1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2015 National Instruments
4 *
5 * (C) Copyright 2015
6 * Joe Hershberger <joe.hershberger@ni.com>
7 */
8
9 #include <dm.h>
10 #include <log.h>
11 #include <malloc.h>
12 #include <asm/eth.h>
13 #include <asm/global_data.h>
14 #include <asm/test.h>
15 #include <asm/types.h>
16
17 /*
18 * Structure definitions for network protocols. Since this file is used for
19 * both NET and NET_LWIP, and given that the two network stacks do have
20 * conflicting types (for instance struct icmp_hdr), it is on purpose that the
21 * structures are defined locally with minimal dependencies -- <asm/types.h> is
22 * included for the bit types and that's it.
23 */
24
25 #define ETHADDR_LEN 6
26 #define IP4_LEN 4
27
28 struct ethhdr {
29 u8 dst[ETHADDR_LEN];
30 u8 src[ETHADDR_LEN];
31 u16 protlen;
32 } __attribute__((packed));
33
34 #define ETHHDR_SIZE (sizeof(struct ethhdr))
35
36 struct arphdr {
37 u16 htype;
38 u16 ptype;
39 u8 hlen;
40 u8 plen;
41 u16 op;
42 } __attribute__((packed));
43
44 #define ARPHDR_SIZE (sizeof(struct arphdr))
45
46 #define ARP_REQUEST 1
47 #define ARP_REPLY 2
48
49 struct arpdata {
50 u8 sha[ETHADDR_LEN];
51 u32 spa;
52 u8 tha[ETHADDR_LEN];
53 u32 tpa;
54 } __attribute__((packed));
55
56 #define ARPDATA_SIZE (sizeof(struct arpdata))
57
58 struct iphdr {
59 u8 hl_v;
60 u8 tos;
61 u16 len;
62 u16 id;
63 u16 off;
64 u8 ttl;
65 u8 prot;
66 u16 sum;
67 u32 src;
68 u32 dst;
69 } __attribute__((packed));
70
71 #define IPHDR_SIZE (sizeof(struct iphdr))
72
73 struct icmphdr {
74 u8 type;
75 u8 code;
76 u16 checksum;
77 u16 id;
78 u16 sequence;
79 } __attribute__((packed));
80
81 #define ICMPHDR_SIZE (sizeof(struct icmphdr))
82
83 #define ICMP_ECHO_REQUEST 8
84 #define ICMP_ECHO_REPLY 0
85 #define IPPROTO_ICMP 1
86
87 DECLARE_GLOBAL_DATA_PTR;
88
89 static const u8 null_ethaddr[6];
90 static bool skip_timeout;
91
92 /*
93 * sandbox_eth_disable_response()
94 *
95 * index - The alias index (also DM seq number)
96 * disable - If non-zero, ignore sent packets and don't send mock response
97 */
sandbox_eth_disable_response(int index,bool disable)98 void sandbox_eth_disable_response(int index, bool disable)
99 {
100 struct udevice *dev;
101 struct eth_sandbox_priv *priv;
102 int ret;
103
104 ret = uclass_get_device(UCLASS_ETH, index, &dev);
105 if (ret)
106 return;
107
108 priv = dev_get_priv(dev);
109 priv->disabled = disable;
110 }
111
112 /*
113 * sandbox_eth_skip_timeout()
114 *
115 * When the first packet read is attempted, fast-forward time
116 */
sandbox_eth_skip_timeout(void)117 void sandbox_eth_skip_timeout(void)
118 {
119 skip_timeout = true;
120 }
121
122 /*
123 * sandbox_eth_arp_req_to_reply()
124 *
125 * Check for an arp request to be sent. If so, inject a reply
126 *
127 * returns 0 if injected, -EAGAIN if not
128 */
sandbox_eth_arp_req_to_reply(struct udevice * dev,void * packet,unsigned int len)129 int sandbox_eth_arp_req_to_reply(struct udevice *dev, void *packet,
130 unsigned int len)
131 {
132 struct eth_sandbox_priv *priv = dev_get_priv(dev);
133 struct ethhdr *eth = packet;
134 struct arphdr *arp;
135 struct arpdata *arpd;
136 struct ethhdr *eth_recv;
137 struct arphdr *arp_recv;
138 struct arpdata *arp_recvd;
139
140 if (ntohs(eth->protlen) != PROT_ARP)
141 return -EAGAIN;
142
143 arp = packet + ETHHDR_SIZE;
144
145 if (ntohs(arp->op) != ARP_REQUEST)
146 return -EAGAIN;
147
148 /* Don't allow the buffer to overrun */
149 if (priv->recv_packets >= PKTBUFSRX)
150 return 0;
151
152 /* store this as the assumed IP of the fake host */
153 arpd = (struct arpdata *)(arp + 1);
154 priv->fake_host_ipaddr.s_addr = arpd->tpa;
155
156 /* Formulate a fake response */
157 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
158 memcpy(eth_recv->dst, eth->src, ETHADDR_LEN);
159 memcpy(eth_recv->src, priv->fake_host_hwaddr, ETHADDR_LEN);
160 eth_recv->protlen = htons(PROT_ARP);
161
162 arp_recv = (void *)eth_recv + ETHHDR_SIZE;
163 arp_recv->htype = htons(ARP_ETHER);
164 arp_recv->ptype = htons(PROT_IP);
165 arp_recv->hlen = ETHADDR_LEN;
166 arp_recv->plen = IP4_LEN;
167 arp_recv->op = htons(ARP_REPLY);
168 arp_recvd = (struct arpdata *)(arp_recv + 1);
169 memcpy(&arp_recvd->sha, priv->fake_host_hwaddr, ETHADDR_LEN);
170 arp_recvd->spa = priv->fake_host_ipaddr.s_addr;
171 memcpy(&arp_recvd->tha, &arpd->sha, ETHADDR_LEN);
172 arp_recvd->tpa = arpd->spa;
173
174 priv->recv_packet_length[priv->recv_packets] = ETHHDR_SIZE +
175 ARPHDR_SIZE + ARPDATA_SIZE;
176 ++priv->recv_packets;
177
178 return 0;
179 }
180
181 /*
182 * sandbox_eth_ping_req_to_reply()
183 *
184 * Check for a ping request to be sent. If so, inject a reply
185 *
186 * returns 0 if injected, -EAGAIN if not
187 */
sandbox_eth_ping_req_to_reply(struct udevice * dev,void * packet,unsigned int len)188 int sandbox_eth_ping_req_to_reply(struct udevice *dev, void *packet,
189 unsigned int len)
190 {
191 struct eth_sandbox_priv *priv = dev_get_priv(dev);
192 struct ethhdr *eth = packet;
193 struct iphdr *ip;
194 struct icmphdr *icmp;
195 struct ethhdr *eth_recv;
196 struct iphdr *ipr;
197 struct icmphdr *icmpr;
198
199 if (ntohs(eth->protlen) != PROT_IP)
200 return -EAGAIN;
201
202 ip = packet + ETHHDR_SIZE;
203
204 if (ip->prot != IPPROTO_ICMP)
205 return -EAGAIN;
206
207 icmp = (struct icmphdr *)(ip + 1);
208
209 if (icmp->type != ICMP_ECHO_REQUEST)
210 return -EAGAIN;
211
212 /* Don't allow the buffer to overrun */
213 if (priv->recv_packets >= PKTBUFSRX)
214 return 0;
215
216 /* reply to the ping */
217 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
218 memcpy(eth_recv, packet, len);
219 ipr = (void *)eth_recv + ETHHDR_SIZE;
220 icmpr = (struct icmphdr *)(ipr + 1);
221 memcpy(eth_recv->dst, eth->src, ETHADDR_LEN);
222 memcpy(eth_recv->src, priv->fake_host_hwaddr, ETHADDR_LEN);
223 ipr->sum = 0;
224 ipr->off = 0;
225 ipr->dst = ip->src;
226 ipr->src = priv->fake_host_ipaddr.s_addr;
227 ipr->sum = compute_ip_checksum(ipr, IPHDR_SIZE);
228
229 icmpr->type = ICMP_ECHO_REPLY;
230 icmpr->checksum = 0;
231 icmpr->checksum = compute_ip_checksum(icmpr, ICMPHDR_SIZE);
232
233 priv->recv_packet_length[priv->recv_packets] = len;
234 ++priv->recv_packets;
235
236 return 0;
237 }
238
239 /*
240 * sandbox_eth_recv_arp_req()
241 *
242 * Inject an ARP request for this target
243 *
244 * returns 0 if injected, -EOVERFLOW if not
245 */
sandbox_eth_recv_arp_req(struct udevice * dev)246 int sandbox_eth_recv_arp_req(struct udevice *dev)
247 {
248 struct eth_sandbox_priv *priv = dev_get_priv(dev);
249 struct ethhdr *eth_recv;
250 struct arphdr *arp_recv;
251 struct arpdata *arp_recvd;
252
253 /* Don't allow the buffer to overrun */
254 if (priv->recv_packets >= PKTBUFSRX)
255 return -EOVERFLOW;
256
257 /* Formulate a fake request */
258 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
259 memcpy(eth_recv->dst, net_bcast_ethaddr, ETHADDR_LEN);
260 memcpy(eth_recv->src, priv->fake_host_hwaddr, ETHADDR_LEN);
261 eth_recv->protlen = htons(PROT_ARP);
262
263 arp_recv = (void *)eth_recv + ETHHDR_SIZE;
264 arp_recv->htype = htons(ARP_ETHER);
265 arp_recv->ptype = htons(PROT_IP);
266 arp_recv->hlen = ETHADDR_LEN;
267 arp_recv->plen = IP4_LEN;
268 arp_recv->op = htons(ARP_REQUEST);
269 arp_recvd = (struct arpdata *)(arp_recv + 1);
270 memcpy(&arp_recvd->sha, priv->fake_host_hwaddr, ETHADDR_LEN);
271 arp_recvd->spa = priv->fake_host_ipaddr.s_addr;
272 memcpy(&arp_recvd->tha, null_ethaddr, ETHADDR_LEN);
273 arp_recvd->tpa = net_ip.s_addr;
274
275 priv->recv_packet_length[priv->recv_packets] =
276 ETHHDR_SIZE + ARPHDR_SIZE + ARPDATA_SIZE;
277 ++priv->recv_packets;
278
279 return 0;
280 }
281
282 /*
283 * sandbox_eth_recv_ping_req()
284 *
285 * Inject a ping request for this target
286 *
287 * returns 0 if injected, -EOVERFLOW if not
288 */
sandbox_eth_recv_ping_req(struct udevice * dev)289 int sandbox_eth_recv_ping_req(struct udevice *dev)
290 {
291 struct eth_sandbox_priv *priv = dev_get_priv(dev);
292 struct eth_pdata *pdata = dev_get_plat(dev);
293 struct ethhdr *eth_recv;
294 struct iphdr *ipr;
295 struct icmphdr *icmpr;
296
297 /* Don't allow the buffer to overrun */
298 if (priv->recv_packets >= PKTBUFSRX)
299 return -EOVERFLOW;
300
301 /* Formulate a fake ping */
302 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
303
304 memcpy(eth_recv->dst, pdata->enetaddr, ETHADDR_LEN);
305 memcpy(eth_recv->src, priv->fake_host_hwaddr, ETHADDR_LEN);
306 eth_recv->protlen = htons(PROT_IP);
307
308 ipr = (void *)eth_recv + ETHHDR_SIZE;
309 ipr->hl_v = 0x45;
310 ipr->len = htons(IPHDR_SIZE + ICMPHDR_SIZE);
311 ipr->off = htons(IP_FLAGS_DFRAG);
312 ipr->prot = IPPROTO_ICMP;
313 ipr->sum = 0;
314 ipr->src = priv->fake_host_ipaddr.s_addr;
315 ipr->dst = net_ip.s_addr;
316 ipr->sum = compute_ip_checksum(ipr, IPHDR_SIZE);
317
318 icmpr = (struct icmphdr *)(ipr + 1);
319
320 icmpr->type = ICMP_ECHO_REQUEST;
321 icmpr->code = 0;
322 icmpr->checksum = 0;
323 icmpr->id = 0;
324 icmpr->sequence = htons(1);
325 icmpr->checksum = compute_ip_checksum(icmpr, ICMPHDR_SIZE);
326
327 priv->recv_packet_length[priv->recv_packets] =
328 ETHHDR_SIZE + IPHDR_SIZE + ICMPHDR_SIZE;
329 ++priv->recv_packets;
330
331 return 0;
332 }
333
334 /*
335 * sb_default_handler()
336 *
337 * perform typical responses to simple ping
338 *
339 * dev - device pointer
340 * pkt - "sent" packet buffer
341 * len - length of packet
342 */
sb_default_handler(struct udevice * dev,void * packet,unsigned int len)343 static int sb_default_handler(struct udevice *dev, void *packet,
344 unsigned int len)
345 {
346 if (!sandbox_eth_arp_req_to_reply(dev, packet, len))
347 return 0;
348 if (!sandbox_eth_ping_req_to_reply(dev, packet, len))
349 return 0;
350
351 return 0;
352 }
353
354 /*
355 * sandbox_eth_set_tx_handler()
356 *
357 * Set a custom response to a packet being sent through the sandbox eth test
358 * driver
359 *
360 * index - interface to set the handler for
361 * handler - The func ptr to call on send. If NULL, set to default handler
362 */
sandbox_eth_set_tx_handler(int index,sandbox_eth_tx_hand_f * handler)363 void sandbox_eth_set_tx_handler(int index, sandbox_eth_tx_hand_f *handler)
364 {
365 struct udevice *dev;
366 struct eth_sandbox_priv *priv;
367 int ret;
368
369 ret = uclass_get_device(UCLASS_ETH, index, &dev);
370 if (ret)
371 return;
372
373 priv = dev_get_priv(dev);
374 if (handler)
375 priv->tx_handler = handler;
376 else
377 priv->tx_handler = sb_default_handler;
378 }
379
380 /*
381 * Set priv ptr
382 *
383 * priv - priv void ptr to store in the device
384 */
sandbox_eth_set_priv(int index,void * priv)385 void sandbox_eth_set_priv(int index, void *priv)
386 {
387 struct udevice *dev;
388 struct eth_sandbox_priv *dev_priv;
389 int ret;
390
391 ret = uclass_get_device(UCLASS_ETH, index, &dev);
392 if (ret)
393 return;
394
395 dev_priv = dev_get_priv(dev);
396
397 dev_priv->priv = priv;
398 }
399
sb_eth_start(struct udevice * dev)400 static int sb_eth_start(struct udevice *dev)
401 {
402 struct eth_sandbox_priv *priv = dev_get_priv(dev);
403
404 debug("eth_sandbox: Start\n");
405
406 priv->recv_packets = 0;
407 for (int i = 0; i < PKTBUFSRX; i++) {
408 priv->recv_packet_buffer[i] = net_rx_packets[i];
409 priv->recv_packet_length[i] = 0;
410 }
411
412 return 0;
413 }
414
sb_eth_send(struct udevice * dev,void * packet,int length)415 static int sb_eth_send(struct udevice *dev, void *packet, int length)
416 {
417 struct eth_sandbox_priv *priv = dev_get_priv(dev);
418
419 debug("eth_sandbox: Send packet %d\n", length);
420
421 if (priv->disabled)
422 return 0;
423
424 return priv->tx_handler(dev, packet, length);
425 }
426
sb_eth_recv(struct udevice * dev,int flags,uchar ** packetp)427 static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
428 {
429 struct eth_sandbox_priv *priv = dev_get_priv(dev);
430
431 if (skip_timeout) {
432 timer_test_add_offset(11000UL);
433 skip_timeout = false;
434 }
435
436 if (priv->recv_packets) {
437 int lcl_recv_packet_length = priv->recv_packet_length[0];
438
439 debug("eth_sandbox: received packet[%d], %d waiting\n",
440 lcl_recv_packet_length, priv->recv_packets - 1);
441 *packetp = priv->recv_packet_buffer[0];
442 return lcl_recv_packet_length;
443 }
444 return 0;
445 }
446
sb_eth_free_pkt(struct udevice * dev,uchar * packet,int length)447 static int sb_eth_free_pkt(struct udevice *dev, uchar *packet, int length)
448 {
449 struct eth_sandbox_priv *priv = dev_get_priv(dev);
450 int i;
451
452 if (!priv->recv_packets)
453 return 0;
454
455 --priv->recv_packets;
456 for (i = 0; i < priv->recv_packets; i++) {
457 priv->recv_packet_length[i] = priv->recv_packet_length[i + 1];
458 memcpy(priv->recv_packet_buffer[i],
459 priv->recv_packet_buffer[i + 1],
460 priv->recv_packet_length[i + 1]);
461 }
462 priv->recv_packet_length[priv->recv_packets] = 0;
463
464 return 0;
465 }
466
sb_eth_stop(struct udevice * dev)467 static void sb_eth_stop(struct udevice *dev)
468 {
469 debug("eth_sandbox: Stop\n");
470 }
471
sb_eth_write_hwaddr(struct udevice * dev)472 static int sb_eth_write_hwaddr(struct udevice *dev)
473 {
474 struct eth_pdata *pdata = dev_get_plat(dev);
475 struct eth_sandbox_priv *priv = dev_get_priv(dev);
476
477 debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
478 pdata->enetaddr);
479 memcpy(priv->fake_host_hwaddr, pdata->enetaddr, ETHADDR_LEN);
480 return 0;
481 }
482
483 static const struct eth_ops sb_eth_ops = {
484 .start = sb_eth_start,
485 .send = sb_eth_send,
486 .recv = sb_eth_recv,
487 .free_pkt = sb_eth_free_pkt,
488 .stop = sb_eth_stop,
489 .write_hwaddr = sb_eth_write_hwaddr,
490 };
491
sb_eth_remove(struct udevice * dev)492 static int sb_eth_remove(struct udevice *dev)
493 {
494 return 0;
495 }
496
sb_eth_of_to_plat(struct udevice * dev)497 static int sb_eth_of_to_plat(struct udevice *dev)
498 {
499 struct eth_pdata *pdata = dev_get_plat(dev);
500 struct eth_sandbox_priv *priv = dev_get_priv(dev);
501
502 pdata->iobase = dev_read_addr(dev);
503 priv->disabled = false;
504 priv->tx_handler = sb_default_handler;
505
506 return 0;
507 }
508
509 static const struct udevice_id sb_eth_ids[] = {
510 { .compatible = "sandbox,eth" },
511 { }
512 };
513
514 U_BOOT_DRIVER(eth_sandbox) = {
515 .name = "eth_sandbox",
516 .id = UCLASS_ETH,
517 .of_match = sb_eth_ids,
518 .of_to_plat = sb_eth_of_to_plat,
519 .remove = sb_eth_remove,
520 .ops = &sb_eth_ops,
521 .priv_auto = sizeof(struct eth_sandbox_priv),
522 .plat_auto = sizeof(struct eth_pdata),
523 };
524