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 <log.h>
12 #include <malloc.h>
13 #include <net.h>
14 #include <asm/eth.h>
15 #include <asm/global_data.h>
16 #include <asm/test.h>
17
18 DECLARE_GLOBAL_DATA_PTR;
19
20 static bool skip_timeout;
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 */
sandbox_eth_disable_response(int index,bool disable)28 void sandbox_eth_disable_response(int index, bool disable)
29 {
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;
40 }
41
42 /*
43 * sandbox_eth_skip_timeout()
44 *
45 * When the first packet read is attempted, fast-forward time
46 */
sandbox_eth_skip_timeout(void)47 void sandbox_eth_skip_timeout(void)
48 {
49 skip_timeout = true;
50 }
51
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 */
sandbox_eth_arp_req_to_reply(struct udevice * dev,void * packet,unsigned int len)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
76 /* Don't allow the buffer to overrun */
77 if (priv->recv_packets >= PKTBUFSRX)
78 return 0;
79
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 */
84 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
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
100 priv->recv_packet_length[priv->recv_packets] =
101 ETHER_HDR_SIZE + ARP_HDR_SIZE;
102 ++priv->recv_packets;
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 */
sandbox_eth_ping_req_to_reply(struct udevice * dev,void * packet,unsigned int len)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
138 /* Don't allow the buffer to overrun */
139 if (priv->recv_packets >= PKTBUFSRX)
140 return 0;
141
142 /* reply to the ping */
143 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
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
159 priv->recv_packet_length[priv->recv_packets] = len;
160 ++priv->recv_packets;
161
162 return 0;
163 }
164
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 */
sandbox_eth_recv_arp_req(struct udevice * dev)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
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 */
sandbox_eth_recv_ping_req(struct udevice * dev)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
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 */
sb_default_handler(struct udevice * dev,void * packet,unsigned int len)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 */
sandbox_eth_set_tx_handler(int index,sandbox_eth_tx_hand_f * handler)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
303 /*
304 * Set priv ptr
305 *
306 * priv - priv void ptr to store in the device
307 */
sandbox_eth_set_priv(int index,void * priv)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
sb_eth_start(struct udevice * dev)323 static int sb_eth_start(struct udevice *dev)
324 {
325 struct eth_sandbox_priv *priv = dev_get_priv(dev);
326
327 debug("eth_sandbox: Start\n");
328
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 }
334
335 return 0;
336 }
337
sb_eth_send(struct udevice * dev,void * packet,int length)338 static int sb_eth_send(struct udevice *dev, void *packet, int length)
339 {
340 struct eth_sandbox_priv *priv = dev_get_priv(dev);
341
342 debug("eth_sandbox: Send packet %d\n", length);
343
344 if (priv->disabled)
345 return 0;
346
347 return priv->tx_handler(dev, packet, length);
348 }
349
sb_eth_recv(struct udevice * dev,int flags,uchar ** packetp)350 static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
351 {
352 struct eth_sandbox_priv *priv = dev_get_priv(dev);
353
354 if (skip_timeout) {
355 timer_test_add_offset(11000UL);
356 skip_timeout = false;
357 }
358
359 if (priv->recv_packets) {
360 int lcl_recv_packet_length = priv->recv_packet_length[0];
361
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];
365 return lcl_recv_packet_length;
366 }
367 return 0;
368 }
369
sb_eth_free_pkt(struct udevice * dev,uchar * packet,int length)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
sb_eth_stop(struct udevice * dev)390 static void sb_eth_stop(struct udevice *dev)
391 {
392 debug("eth_sandbox: Stop\n");
393 }
394
sb_eth_write_hwaddr(struct udevice * dev)395 static int sb_eth_write_hwaddr(struct udevice *dev)
396 {
397 struct eth_pdata *pdata = dev_get_plat(dev);
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,
408 .free_pkt = sb_eth_free_pkt,
409 .stop = sb_eth_stop,
410 .write_hwaddr = sb_eth_write_hwaddr,
411 };
412
sb_eth_remove(struct udevice * dev)413 static int sb_eth_remove(struct udevice *dev)
414 {
415 return 0;
416 }
417
sb_eth_of_to_plat(struct udevice * dev)418 static int sb_eth_of_to_plat(struct udevice *dev)
419 {
420 struct eth_pdata *pdata = dev_get_plat(dev);
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);
432 priv->disabled = false;
433 priv->tx_handler = sb_default_handler;
434
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,
447 .of_to_plat = sb_eth_of_to_plat,
448 .remove = sb_eth_remove,
449 .ops = &sb_eth_ops,
450 .priv_auto = sizeof(struct eth_sandbox_priv),
451 .plat_auto = sizeof(struct eth_pdata),
452 };
453