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