eth-raw-os.c 7.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (c) 2015-2018 National Instruments
  4. * Copyright (c) 2015-2018 Joe Hershberger <joe.hershberger@ni.com>
  5. */
  6. #include <asm/eth-raw-os.h>
  7. #include <errno.h>
  8. #include <fcntl.h>
  9. #include <net/if.h>
  10. #include <netinet/in.h>
  11. #include <netinet/ip.h>
  12. #include <netinet/udp.h>
  13. #include <stdbool.h>
  14. #include <stdio.h>
  15. #include <stdlib.h>
  16. #include <string.h>
  17. #include <sys/types.h>
  18. #include <sys/ioctl.h>
  19. #include <sys/socket.h>
  20. #include <unistd.h>
  21. #include <arpa/inet.h>
  22. #include <linux/if_ether.h>
  23. #include <linux/if_packet.h>
  24. #include <os.h>
  25. struct sandbox_eth_raw_if_nameindex *sandbox_eth_raw_if_nameindex(void)
  26. {
  27. return (struct sandbox_eth_raw_if_nameindex *)if_nameindex();
  28. }
  29. void sandbox_eth_raw_if_freenameindex(struct sandbox_eth_raw_if_nameindex *ptr)
  30. {
  31. if_freenameindex((struct if_nameindex *)ptr);
  32. }
  33. int sandbox_eth_raw_os_is_local(const char *ifname)
  34. {
  35. int fd = socket(AF_INET, SOCK_DGRAM, 0);
  36. struct ifreq ifr;
  37. int ret = 0;
  38. if (fd < 0)
  39. return -errno;
  40. memset(&ifr, 0, sizeof(ifr));
  41. strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
  42. ret = ioctl(fd, SIOCGIFFLAGS, &ifr);
  43. if (ret < 0) {
  44. ret = -errno;
  45. goto out;
  46. }
  47. ret = !!(ifr.ifr_flags & IFF_LOOPBACK);
  48. out:
  49. close(fd);
  50. return ret;
  51. }
  52. int sandbox_eth_raw_os_idx_to_name(struct eth_sandbox_raw_priv *priv)
  53. {
  54. if (!if_indextoname(priv->host_ifindex, priv->host_ifname))
  55. return -errno;
  56. return 0;
  57. }
  58. static int _raw_packet_start(struct eth_sandbox_raw_priv *priv,
  59. unsigned char *ethmac)
  60. {
  61. struct sockaddr_ll *device;
  62. struct packet_mreq mr;
  63. int ret;
  64. int flags;
  65. /* Prepare device struct */
  66. priv->local_bind_sd = -1;
  67. priv->device = malloc(sizeof(struct sockaddr_ll));
  68. if (priv->device == NULL)
  69. return -ENOMEM;
  70. device = priv->device;
  71. memset(device, 0, sizeof(struct sockaddr_ll));
  72. device->sll_ifindex = if_nametoindex(priv->host_ifname);
  73. priv->host_ifindex = device->sll_ifindex;
  74. device->sll_family = AF_PACKET;
  75. memcpy(device->sll_addr, ethmac, 6);
  76. device->sll_halen = htons(6);
  77. /* Open socket */
  78. priv->sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  79. if (priv->sd < 0) {
  80. printf("Failed to open socket: %d %s\n", errno,
  81. strerror(errno));
  82. return -errno;
  83. }
  84. /* Bind to the specified interface */
  85. ret = setsockopt(priv->sd, SOL_SOCKET, SO_BINDTODEVICE,
  86. priv->host_ifname, strlen(priv->host_ifname) + 1);
  87. if (ret < 0) {
  88. printf("Failed to bind to '%s': %d %s\n", priv->host_ifname,
  89. errno, strerror(errno));
  90. return -errno;
  91. }
  92. /* Make the socket non-blocking */
  93. flags = fcntl(priv->sd, F_GETFL, 0);
  94. fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
  95. /* Enable promiscuous mode to receive responses meant for us */
  96. mr.mr_ifindex = device->sll_ifindex;
  97. mr.mr_type = PACKET_MR_PROMISC;
  98. ret = setsockopt(priv->sd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
  99. &mr, sizeof(mr));
  100. if (ret < 0) {
  101. struct ifreq ifr;
  102. printf("Failed to set promiscuous mode: %d %s\n"
  103. "Falling back to the old \"flags\" way...\n",
  104. errno, strerror(errno));
  105. if (strlen(priv->host_ifname) >= IFNAMSIZ) {
  106. printf("Interface name %s is too long.\n",
  107. priv->host_ifname);
  108. return -EINVAL;
  109. }
  110. strncpy(ifr.ifr_name, priv->host_ifname, IFNAMSIZ);
  111. if (ioctl(priv->sd, SIOCGIFFLAGS, &ifr) < 0) {
  112. printf("Failed to read flags: %d %s\n", errno,
  113. strerror(errno));
  114. return -errno;
  115. }
  116. ifr.ifr_flags |= IFF_PROMISC;
  117. if (ioctl(priv->sd, SIOCSIFFLAGS, &ifr) < 0) {
  118. printf("Failed to write flags: %d %s\n", errno,
  119. strerror(errno));
  120. return -errno;
  121. }
  122. }
  123. return 0;
  124. }
  125. static int _local_inet_start(struct eth_sandbox_raw_priv *priv)
  126. {
  127. struct sockaddr_in *device;
  128. int ret;
  129. int flags;
  130. int one = 1;
  131. /* Prepare device struct */
  132. priv->local_bind_sd = -1;
  133. priv->local_bind_udp_port = 0;
  134. priv->device = malloc(sizeof(struct sockaddr_in));
  135. if (priv->device == NULL)
  136. return -ENOMEM;
  137. device = priv->device;
  138. memset(device, 0, sizeof(struct sockaddr_in));
  139. device->sin_family = AF_INET;
  140. device->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
  141. /**
  142. * Open socket
  143. * Since we specify UDP here, any incoming ICMP packets will
  144. * not be received, so things like ping will not work on this
  145. * localhost interface.
  146. */
  147. priv->sd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP);
  148. if (priv->sd < 0) {
  149. printf("Failed to open socket: %d %s\n", errno,
  150. strerror(errno));
  151. return -errno;
  152. }
  153. /* Make the socket non-blocking */
  154. flags = fcntl(priv->sd, F_GETFL, 0);
  155. fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
  156. /* Include the UDP/IP headers on send and receive */
  157. ret = setsockopt(priv->sd, IPPROTO_IP, IP_HDRINCL, &one,
  158. sizeof(one));
  159. if (ret < 0) {
  160. printf("Failed to set header include option: %d %s\n", errno,
  161. strerror(errno));
  162. return -errno;
  163. }
  164. return 0;
  165. }
  166. int sandbox_eth_raw_os_start(struct eth_sandbox_raw_priv *priv,
  167. unsigned char *ethmac)
  168. {
  169. if (priv->local)
  170. return _local_inet_start(priv);
  171. else
  172. return _raw_packet_start(priv, ethmac);
  173. }
  174. int sandbox_eth_raw_os_send(void *packet, int length,
  175. struct eth_sandbox_raw_priv *priv)
  176. {
  177. int retval;
  178. struct udphdr *udph = packet + sizeof(struct iphdr);
  179. if (priv->sd < 0 || !priv->device)
  180. return -EINVAL;
  181. /*
  182. * This block of code came about when testing tftp on the localhost
  183. * interface. When using the RAW AF_INET API, the network stack is still
  184. * in play responding to incoming traffic based on open "ports". Since
  185. * it is raw (at the IP layer, no Ethernet) the network stack tells the
  186. * TFTP server that the port it responded to is closed. This causes the
  187. * TFTP transfer to be aborted. This block of code inspects the outgoing
  188. * packet as formulated by the u-boot network stack to determine the
  189. * source port (that the TFTP server will send packets back to) and
  190. * opens a typical UDP socket on that port, thus preventing the network
  191. * stack from sending that ICMP message claiming that the port has no
  192. * bound socket.
  193. */
  194. if (priv->local && (priv->local_bind_sd == -1 ||
  195. priv->local_bind_udp_port != udph->source)) {
  196. struct iphdr *iph = packet;
  197. struct sockaddr_in addr;
  198. if (priv->local_bind_sd != -1)
  199. close(priv->local_bind_sd);
  200. /* A normal UDP socket is required to bind */
  201. priv->local_bind_sd = socket(AF_INET, SOCK_DGRAM, 0);
  202. if (priv->local_bind_sd < 0) {
  203. printf("Failed to open bind sd: %d %s\n", errno,
  204. strerror(errno));
  205. return -errno;
  206. }
  207. priv->local_bind_udp_port = udph->source;
  208. /**
  209. * Bind the UDP port that we intend to use as our source port
  210. * so that the kernel will not send an ICMP port unreachable
  211. * message to the server
  212. */
  213. addr.sin_family = AF_INET;
  214. addr.sin_port = udph->source;
  215. addr.sin_addr.s_addr = iph->saddr;
  216. retval = bind(priv->local_bind_sd, (struct sockaddr *)&addr,
  217. sizeof(addr));
  218. if (retval < 0)
  219. printf("Failed to bind: %d %s\n", errno,
  220. strerror(errno));
  221. }
  222. retval = sendto(priv->sd, packet, length, 0,
  223. (struct sockaddr *)priv->device,
  224. sizeof(struct sockaddr_ll));
  225. if (retval < 0) {
  226. printf("Failed to send packet: %d %s\n", errno,
  227. strerror(errno));
  228. return -errno;
  229. }
  230. return retval;
  231. }
  232. int sandbox_eth_raw_os_recv(void *packet, int *length,
  233. const struct eth_sandbox_raw_priv *priv)
  234. {
  235. int retval;
  236. int saddr_size;
  237. if (priv->sd < 0 || !priv->device)
  238. return -EINVAL;
  239. saddr_size = sizeof(struct sockaddr);
  240. retval = recvfrom(priv->sd, packet, 1536, 0,
  241. (struct sockaddr *)priv->device,
  242. (socklen_t *)&saddr_size);
  243. *length = 0;
  244. if (retval >= 0) {
  245. *length = retval;
  246. return 0;
  247. }
  248. /* The socket is non-blocking, so expect EAGAIN when there is no data */
  249. if (errno == EAGAIN)
  250. return 0;
  251. return -errno;
  252. }
  253. void sandbox_eth_raw_os_stop(struct eth_sandbox_raw_priv *priv)
  254. {
  255. free(priv->device);
  256. priv->device = NULL;
  257. close(priv->sd);
  258. priv->sd = -1;
  259. if (priv->local) {
  260. if (priv->local_bind_sd != -1)
  261. close(priv->local_bind_sd);
  262. priv->local_bind_sd = -1;
  263. priv->local_bind_udp_port = 0;
  264. }
  265. }