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