blob: 37459dfa0a409bb6a66ab06fef76d687f5c27681 [file] [log] [blame]
Tom Rini83d290c2018-05-06 17:58:06 -04001// SPDX-License-Identifier: GPL-2.0
Joe Hershberger3ea143a2015-03-22 17:09:13 -05002/*
3 * Copyright (c) 2015 National Instruments
4 *
5 * (C) Copyright 2015
6 * Joe Hershberger <joe.hershberger@ni.com>
Joe Hershberger3ea143a2015-03-22 17:09:13 -05007 */
8
9#include <common.h>
10#include <dm.h>
Simon Glassf7ae49f2020-05-10 11:40:05 -060011#include <log.h>
Joe Hershberger3ea143a2015-03-22 17:09:13 -050012#include <malloc.h>
13#include <net.h>
Joe Hershbergerc7eb7332018-09-26 16:48:55 -050014#include <asm/eth.h>
Simon Glass401d1c42020-10-30 21:38:53 -060015#include <asm/global_data.h>
Joe Hershberger6f2707c2015-04-21 13:57:19 -050016#include <asm/test.h>
Joe Hershberger3ea143a2015-03-22 17:09:13 -050017
18DECLARE_GLOBAL_DATA_PTR;
19
Joe Hershberger6f2707c2015-04-21 13:57:19 -050020static bool skip_timeout;
Joe Hershberger2eede1f2015-03-22 17:09:19 -050021
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 */
28void sandbox_eth_disable_response(int index, bool disable)
29{
Joe Hershbergere4ab9a62018-09-26 16:48:53 -050030 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;
Joe Hershberger2eede1f2015-03-22 17:09:19 -050040}
41
Joe Hershberger6f2707c2015-04-21 13:57:19 -050042/*
43 * sandbox_eth_skip_timeout()
44 *
45 * When the first packet read is attempted, fast-forward time
46 */
47void sandbox_eth_skip_timeout(void)
48{
49 skip_timeout = true;
50}
51
Joe Hershbergere95bb162018-09-26 16:48:54 -050052/*
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 */
59int 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -050076 /* Don't allow the buffer to overrun */
77 if (priv->recv_packets >= PKTBUFSRX)
78 return 0;
79
Joe Hershbergere95bb162018-09-26 16:48:54 -050080 /* 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 */
Joe Hershbergerc67a4202018-09-26 16:48:57 -050084 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
Joe Hershbergere95bb162018-09-26 16:48:54 -050085 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500100 priv->recv_packet_length[priv->recv_packets] =
101 ETHER_HDR_SIZE + ARP_HDR_SIZE;
102 ++priv->recv_packets;
Joe Hershbergere95bb162018-09-26 16:48:54 -0500103
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 */
114int 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500138 /* Don't allow the buffer to overrun */
139 if (priv->recv_packets >= PKTBUFSRX)
140 return 0;
141
Joe Hershbergere95bb162018-09-26 16:48:54 -0500142 /* reply to the ping */
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500143 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
Joe Hershbergere95bb162018-09-26 16:48:54 -0500144 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500159 priv->recv_packet_length[priv->recv_packets] = len;
160 ++priv->recv_packets;
Joe Hershbergere95bb162018-09-26 16:48:54 -0500161
162 return 0;
163}
164
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500165/*
Joe Hershberger45988da2018-09-26 16:49:00 -0500166 * sandbox_eth_recv_arp_req()
167 *
168 * Inject an ARP request for this target
169 *
170 * returns 0 if injected, -EOVERFLOW if not
171 */
172int 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/*
Joe Hershberger72ff0042018-09-26 16:49:01 -0500207 * sandbox_eth_recv_ping_req()
208 *
209 * Inject a ping request for this target
210 *
211 * returns 0 if injected, -EOVERFLOW if not
212 */
213int 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/*
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500258 * 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 */
266static 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 */
286void 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
Joe Hershberger9cbe5972018-09-26 16:48:59 -0500303/*
304 * Set priv ptr
305 *
306 * priv - priv void ptr to store in the device
307 */
308void 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
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500323static int sb_eth_start(struct udevice *dev)
324{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500325 struct eth_sandbox_priv *priv = dev_get_priv(dev);
326
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500327 debug("eth_sandbox: Start\n");
328
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500329 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 }
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500334
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500335 return 0;
336}
337
338static int sb_eth_send(struct udevice *dev, void *packet, int length)
339{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500340 struct eth_sandbox_priv *priv = dev_get_priv(dev);
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500341
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500342 debug("eth_sandbox: Send packet %d\n", length);
343
Joe Hershbergere4ab9a62018-09-26 16:48:53 -0500344 if (priv->disabled)
Joe Hershberger2eede1f2015-03-22 17:09:19 -0500345 return 0;
346
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500347 return priv->tx_handler(dev, packet, length);
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500348}
349
Simon Glassa1ca92e2015-07-06 16:47:49 -0600350static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500351{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500352 struct eth_sandbox_priv *priv = dev_get_priv(dev);
353
Joe Hershberger6f2707c2015-04-21 13:57:19 -0500354 if (skip_timeout) {
Neil Armstrongd0a9b822019-04-11 17:01:23 +0200355 timer_test_add_offset(11000UL);
Joe Hershberger6f2707c2015-04-21 13:57:19 -0500356 skip_timeout = false;
357 }
358
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500359 if (priv->recv_packets) {
360 int lcl_recv_packet_length = priv->recv_packet_length[0];
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500361
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500362 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];
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500365 return lcl_recv_packet_length;
366 }
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500367 return 0;
368}
369
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500370static 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
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500390static void sb_eth_stop(struct udevice *dev)
391{
392 debug("eth_sandbox: Stop\n");
393}
394
395static int sb_eth_write_hwaddr(struct udevice *dev)
396{
Simon Glassc69cda22020-12-03 16:55:20 -0700397 struct eth_pdata *pdata = dev_get_plat(dev);
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500398
399 debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
400 pdata->enetaddr);
401 return 0;
402}
403
404static const struct eth_ops sb_eth_ops = {
405 .start = sb_eth_start,
406 .send = sb_eth_send,
407 .recv = sb_eth_recv,
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500408 .free_pkt = sb_eth_free_pkt,
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500409 .stop = sb_eth_stop,
410 .write_hwaddr = sb_eth_write_hwaddr,
411};
412
413static int sb_eth_remove(struct udevice *dev)
414{
415 return 0;
416}
417
Simon Glassd1998a92020-12-03 16:55:21 -0700418static int sb_eth_of_to_plat(struct udevice *dev)
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500419{
Simon Glassc69cda22020-12-03 16:55:20 -0700420 struct eth_pdata *pdata = dev_get_plat(dev);
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500421 struct eth_sandbox_priv *priv = dev_get_priv(dev);
422 const u8 *mac;
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500423
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500424 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);
Joe Hershbergere4ab9a62018-09-26 16:48:53 -0500432 priv->disabled = false;
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500433 priv->tx_handler = sb_default_handler;
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500434
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500435 return 0;
436}
437
438static const struct udevice_id sb_eth_ids[] = {
439 { .compatible = "sandbox,eth" },
440 { }
441};
442
443U_BOOT_DRIVER(eth_sandbox) = {
444 .name = "eth_sandbox",
445 .id = UCLASS_ETH,
446 .of_match = sb_eth_ids,
Simon Glassd1998a92020-12-03 16:55:21 -0700447 .of_to_plat = sb_eth_of_to_plat,
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500448 .remove = sb_eth_remove,
449 .ops = &sb_eth_ops,
Simon Glass41575d82020-12-03 16:55:17 -0700450 .priv_auto = sizeof(struct eth_sandbox_priv),
Simon Glasscaa4daa2020-12-03 16:55:18 -0700451 .plat_auto = sizeof(struct eth_pdata),
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500452};