sandbox.c 11 KB

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