sandbox.c 11 KB

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