blob: decce2fa59c4c5c670b6db9c2bbecd761bbbb5ca [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>
11#include <malloc.h>
12#include <net.h>
Joe Hershbergerc7eb7332018-09-26 16:48:55 -050013#include <asm/eth.h>
Joe Hershberger6f2707c2015-04-21 13:57:19 -050014#include <asm/test.h>
Joe Hershberger3ea143a2015-03-22 17:09:13 -050015
16DECLARE_GLOBAL_DATA_PTR;
17
Joe Hershberger6f2707c2015-04-21 13:57:19 -050018static bool skip_timeout;
Joe Hershberger2eede1f2015-03-22 17:09:19 -050019
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 */
26void sandbox_eth_disable_response(int index, bool disable)
27{
Joe Hershbergere4ab9a62018-09-26 16:48:53 -050028 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;
Joe Hershberger2eede1f2015-03-22 17:09:19 -050038}
39
Joe Hershberger6f2707c2015-04-21 13:57:19 -050040/*
41 * sandbox_eth_skip_timeout()
42 *
43 * When the first packet read is attempted, fast-forward time
44 */
45void sandbox_eth_skip_timeout(void)
46{
47 skip_timeout = true;
48}
49
Joe Hershbergere95bb162018-09-26 16:48:54 -050050/*
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 */
57int 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -050074 /* Don't allow the buffer to overrun */
75 if (priv->recv_packets >= PKTBUFSRX)
76 return 0;
77
Joe Hershbergere95bb162018-09-26 16:48:54 -050078 /* 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 */
Joe Hershbergerc67a4202018-09-26 16:48:57 -050082 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
Joe Hershbergere95bb162018-09-26 16:48:54 -050083 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -050098 priv->recv_packet_length[priv->recv_packets] =
99 ETHER_HDR_SIZE + ARP_HDR_SIZE;
100 ++priv->recv_packets;
Joe Hershbergere95bb162018-09-26 16:48:54 -0500101
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 */
112int 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500136 /* Don't allow the buffer to overrun */
137 if (priv->recv_packets >= PKTBUFSRX)
138 return 0;
139
Joe Hershbergere95bb162018-09-26 16:48:54 -0500140 /* reply to the ping */
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500141 eth_recv = (void *)priv->recv_packet_buffer[priv->recv_packets];
Joe Hershbergere95bb162018-09-26 16:48:54 -0500142 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
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500157 priv->recv_packet_length[priv->recv_packets] = len;
158 ++priv->recv_packets;
Joe Hershbergere95bb162018-09-26 16:48:54 -0500159
160 return 0;
161}
162
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500163/*
Joe Hershberger45988da2018-09-26 16:49:00 -0500164 * sandbox_eth_recv_arp_req()
165 *
166 * Inject an ARP request for this target
167 *
168 * returns 0 if injected, -EOVERFLOW if not
169 */
170int 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/*
Joe Hershberger72ff0042018-09-26 16:49:01 -0500205 * sandbox_eth_recv_ping_req()
206 *
207 * Inject a ping request for this target
208 *
209 * returns 0 if injected, -EOVERFLOW if not
210 */
211int 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/*
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500256 * 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 */
264static 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 */
284void 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
Joe Hershberger9cbe5972018-09-26 16:48:59 -0500301/*
302 * Set priv ptr
303 *
304 * priv - priv void ptr to store in the device
305 */
306void 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
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500321static int sb_eth_start(struct udevice *dev)
322{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500323 struct eth_sandbox_priv *priv = dev_get_priv(dev);
324
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500325 debug("eth_sandbox: Start\n");
326
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500327 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 }
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500332
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500333 return 0;
334}
335
336static int sb_eth_send(struct udevice *dev, void *packet, int length)
337{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500338 struct eth_sandbox_priv *priv = dev_get_priv(dev);
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500339
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500340 debug("eth_sandbox: Send packet %d\n", length);
341
Joe Hershbergere4ab9a62018-09-26 16:48:53 -0500342 if (priv->disabled)
Joe Hershberger2eede1f2015-03-22 17:09:19 -0500343 return 0;
344
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500345 return priv->tx_handler(dev, packet, length);
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500346}
347
Simon Glassa1ca92e2015-07-06 16:47:49 -0600348static int sb_eth_recv(struct udevice *dev, int flags, uchar **packetp)
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500349{
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500350 struct eth_sandbox_priv *priv = dev_get_priv(dev);
351
Joe Hershberger6f2707c2015-04-21 13:57:19 -0500352 if (skip_timeout) {
Joe Hershbergerc5a75332015-12-21 16:31:35 -0600353 sandbox_timer_add_offset(11000UL);
Joe Hershberger6f2707c2015-04-21 13:57:19 -0500354 skip_timeout = false;
355 }
356
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500357 if (priv->recv_packets) {
358 int lcl_recv_packet_length = priv->recv_packet_length[0];
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500359
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500360 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];
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500363 return lcl_recv_packet_length;
364 }
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500365 return 0;
366}
367
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500368static 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
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500388static void sb_eth_stop(struct udevice *dev)
389{
390 debug("eth_sandbox: Stop\n");
391}
392
393static 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
402static const struct eth_ops sb_eth_ops = {
403 .start = sb_eth_start,
404 .send = sb_eth_send,
405 .recv = sb_eth_recv,
Joe Hershbergerc67a4202018-09-26 16:48:57 -0500406 .free_pkt = sb_eth_free_pkt,
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500407 .stop = sb_eth_stop,
408 .write_hwaddr = sb_eth_write_hwaddr,
409};
410
411static int sb_eth_remove(struct udevice *dev)
412{
413 return 0;
414}
415
416static int sb_eth_ofdata_to_platdata(struct udevice *dev)
417{
418 struct eth_pdata *pdata = dev_get_platdata(dev);
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500419 struct eth_sandbox_priv *priv = dev_get_priv(dev);
420 const u8 *mac;
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500421
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500422 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);
Joe Hershbergere4ab9a62018-09-26 16:48:53 -0500430 priv->disabled = false;
Joe Hershbergerc7eb7332018-09-26 16:48:55 -0500431 priv->tx_handler = sb_default_handler;
Joe Hershbergerb32dd182018-07-02 14:47:49 -0500432
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500433 return 0;
434}
435
436static const struct udevice_id sb_eth_ids[] = {
437 { .compatible = "sandbox,eth" },
438 { }
439};
440
441U_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,
Joe Hershbergerd87a4572015-03-22 17:09:14 -0500448 .priv_auto_alloc_size = sizeof(struct eth_sandbox_priv),
Joe Hershberger3ea143a2015-03-22 17:09:13 -0500449 .platdata_auto_alloc_size = sizeof(struct eth_pdata),
450};