1 | /* |
---|
2 | * Copyright (c) 2014 embedded brains GmbH. All rights reserved. |
---|
3 | * |
---|
4 | * embedded brains GmbH |
---|
5 | * Dornierstr. 4 |
---|
6 | * 82178 Puchheim |
---|
7 | * Germany |
---|
8 | * <rtems@embedded-brains.de> |
---|
9 | * |
---|
10 | * Copyright (c) 2006-2013 Roy Marples <roy@marples.name> |
---|
11 | * |
---|
12 | * Some parts are derived from the dhcpcd sources. |
---|
13 | * |
---|
14 | * http://roy.marples.name/projects/dhcpcd |
---|
15 | * |
---|
16 | * Redistribution and use in source and binary forms, with or without |
---|
17 | * modification, are permitted provided that the following conditions |
---|
18 | * are met: |
---|
19 | * 1. Redistributions of source code must retain the above copyright |
---|
20 | * notice, this list of conditions and the following disclaimer. |
---|
21 | * 2. Redistributions in binary form must reproduce the above copyright |
---|
22 | * notice, this list of conditions and the following disclaimer in the |
---|
23 | * documentation and/or other materials provided with the distribution. |
---|
24 | * |
---|
25 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND |
---|
26 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
---|
27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
---|
28 | * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE |
---|
29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
---|
30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
---|
31 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
---|
32 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
---|
33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
---|
34 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
---|
35 | * SUCH DAMAGE. |
---|
36 | */ |
---|
37 | |
---|
38 | #include <rtems/bsd/util.h> |
---|
39 | |
---|
40 | #include <sys/cdefs.h> |
---|
41 | #include <sys/ioctl.h> |
---|
42 | #include <sys/time.h> |
---|
43 | #include <sys/types.h> |
---|
44 | #include <sys/socket.h> |
---|
45 | |
---|
46 | #include <net/bpf.h> |
---|
47 | #include <net/if_dl.h> |
---|
48 | #include <arpa/inet.h> |
---|
49 | |
---|
50 | #include <errno.h> |
---|
51 | #include <fcntl.h> |
---|
52 | #include <ifaddrs.h> |
---|
53 | #include <stdlib.h> |
---|
54 | #include <string.h> |
---|
55 | |
---|
56 | struct rtems_bsd_arp_processor_context { |
---|
57 | int fd; |
---|
58 | uint8_t eaddr[ETHER_ADDR_LEN]; |
---|
59 | size_t buf_size; |
---|
60 | uint8_t buf[]; |
---|
61 | }; |
---|
62 | |
---|
63 | #ifndef BPF_ETHCOOK |
---|
64 | # define BPF_ETHCOOK 0 |
---|
65 | #endif |
---|
66 | #ifndef BPF_WHOLEPACKET |
---|
67 | # define BPF_WHOLEPACKET ~0U |
---|
68 | #endif |
---|
69 | static const struct bpf_insn arp_bpf_filter[] = { |
---|
70 | /* Make sure this is an ARP packet... */ |
---|
71 | BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12), |
---|
72 | BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_ARP, 0, 3), |
---|
73 | /* Make sure this is an ARP REQUEST... */ |
---|
74 | BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20 + BPF_ETHCOOK), |
---|
75 | BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ARPOP_REQUEST, 2, 0), |
---|
76 | /* or ARP REPLY... */ |
---|
77 | BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20 + BPF_ETHCOOK), |
---|
78 | BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ARPOP_REPLY, 0, 1), |
---|
79 | /* If we passed all the tests, ask for the whole packet. */ |
---|
80 | BPF_STMT(BPF_RET + BPF_K, BPF_WHOLEPACKET), |
---|
81 | /* Otherwise, drop it. */ |
---|
82 | BPF_STMT(BPF_RET + BPF_K, 0) |
---|
83 | }; |
---|
84 | |
---|
85 | static const size_t arp_bpf_filter_len = |
---|
86 | sizeof(arp_bpf_filter) / sizeof(arp_bpf_filter[0]); |
---|
87 | |
---|
88 | static void |
---|
89 | process_arp_packet(const uint8_t *pkt, size_t pkt_size, int fd, |
---|
90 | const uint8_t eaddr[ETHER_ADDR_LEN], rtems_bsd_arp_processor processor, |
---|
91 | void *arg) |
---|
92 | { |
---|
93 | struct arphdr ar; |
---|
94 | uint32_t spa; |
---|
95 | uint32_t tpa; |
---|
96 | const uint8_t *sha; |
---|
97 | const uint8_t *tha; |
---|
98 | |
---|
99 | /* We must have a full ARP header */ |
---|
100 | if (pkt_size < sizeof(ar)) |
---|
101 | return; |
---|
102 | |
---|
103 | memcpy(&ar, pkt, sizeof(ar)); |
---|
104 | |
---|
105 | /* Protocol must be IP. */ |
---|
106 | if (ar.ar_pro != htons(ETHERTYPE_IP)) |
---|
107 | return; |
---|
108 | |
---|
109 | if (ar.ar_pln != sizeof(spa)) |
---|
110 | return; |
---|
111 | |
---|
112 | /* Only these types are recognised */ |
---|
113 | if (ar.ar_op != htons(ARPOP_REPLY) && |
---|
114 | ar.ar_op != htons(ARPOP_REQUEST)) |
---|
115 | return; |
---|
116 | |
---|
117 | /* Get pointers to the hardware addresses */ |
---|
118 | sha = pkt + sizeof(ar); |
---|
119 | tha = sha + ar.ar_hln + ar.ar_pln; |
---|
120 | |
---|
121 | /* Ensure we got all the data */ |
---|
122 | if ((tha + ar.ar_hln + ar.ar_pln) - pkt > pkt_size) |
---|
123 | return; |
---|
124 | |
---|
125 | /* Ignore messages from ourself */ |
---|
126 | if (ar.ar_hln == ETHER_ADDR_LEN && |
---|
127 | memcmp(sha, &eaddr[0], ETHER_ADDR_LEN) == 0) |
---|
128 | return; |
---|
129 | |
---|
130 | /* Copy out the IP addresses */ |
---|
131 | memcpy(&spa, sha + ar.ar_hln, ar.ar_pln); |
---|
132 | memcpy(&tpa, tha + ar.ar_hln, ar.ar_pln); |
---|
133 | |
---|
134 | (*processor)(arg, fd, eaddr, &ar, spa, tpa, sha, tha); |
---|
135 | } |
---|
136 | |
---|
137 | rtems_bsd_arp_processor_context * |
---|
138 | rtems_bsd_arp_processor_create(const char *ifname) |
---|
139 | { |
---|
140 | rtems_bsd_arp_processor_context *ctx = NULL; |
---|
141 | int fd = -1; |
---|
142 | struct ifreq ifr; |
---|
143 | int buf_len; |
---|
144 | size_t buf_size; |
---|
145 | struct bpf_version pv; |
---|
146 | struct bpf_program pf; |
---|
147 | int flags; |
---|
148 | int rv; |
---|
149 | |
---|
150 | fd = open("/dev/bpf", O_RDWR); |
---|
151 | if (fd < 0) { |
---|
152 | goto error; |
---|
153 | } |
---|
154 | |
---|
155 | rv = ioctl(fd, BIOCVERSION, &pv); |
---|
156 | if (rv != 0 || pv.bv_major != BPF_MAJOR_VERSION || |
---|
157 | pv.bv_minor < BPF_MINOR_VERSION) { |
---|
158 | goto error; |
---|
159 | } |
---|
160 | |
---|
161 | memset(&ifr, 0, sizeof(ifr)); |
---|
162 | strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); |
---|
163 | rv = ioctl(fd, BIOCSETIF, &ifr); |
---|
164 | if (rv != 0) { |
---|
165 | goto error; |
---|
166 | } |
---|
167 | |
---|
168 | rv = ioctl(fd, BIOCGBLEN, &buf_len); |
---|
169 | if (rv != 0 || buf_len <= 0) { |
---|
170 | goto error; |
---|
171 | } |
---|
172 | buf_size = (size_t) buf_len; |
---|
173 | |
---|
174 | flags = 1; |
---|
175 | rv = ioctl(fd, BIOCIMMEDIATE, &flags); |
---|
176 | if (rv != 0) { |
---|
177 | goto error; |
---|
178 | } |
---|
179 | |
---|
180 | pf.bf_insns = __DECONST(struct bpf_insn *, arp_bpf_filter); |
---|
181 | pf.bf_len = arp_bpf_filter_len; |
---|
182 | rv = ioctl(fd, BIOCSETF, &pf); |
---|
183 | if (rv != 0) { |
---|
184 | goto error; |
---|
185 | } |
---|
186 | |
---|
187 | ctx = malloc(sizeof(*ctx) + buf_size); |
---|
188 | if (ctx == NULL) { |
---|
189 | goto error; |
---|
190 | } |
---|
191 | |
---|
192 | rv = rtems_bsd_get_ethernet_addr(ifname, &ctx->eaddr[0]); |
---|
193 | if (rv != 0) { |
---|
194 | goto error; |
---|
195 | } |
---|
196 | |
---|
197 | ctx->fd = fd; |
---|
198 | ctx->buf_size = buf_size; |
---|
199 | |
---|
200 | return ctx; |
---|
201 | |
---|
202 | error: |
---|
203 | close(fd); |
---|
204 | free(ctx); |
---|
205 | |
---|
206 | return NULL; |
---|
207 | } |
---|
208 | |
---|
209 | int |
---|
210 | rtems_bsd_arp_processor_get_file_descriptor( |
---|
211 | const rtems_bsd_arp_processor_context *ctx) |
---|
212 | { |
---|
213 | return ctx->fd; |
---|
214 | } |
---|
215 | |
---|
216 | int |
---|
217 | rtems_bsd_arp_processor_process( |
---|
218 | rtems_bsd_arp_processor_context *ctx, |
---|
219 | rtems_bsd_arp_processor processor, void *arg) |
---|
220 | { |
---|
221 | ssize_t n; |
---|
222 | size_t buf_avail; |
---|
223 | size_t buf_pos; |
---|
224 | |
---|
225 | n = read(ctx->fd, &ctx->buf[0], ctx->buf_size); |
---|
226 | if (n < 0) { |
---|
227 | return -1; |
---|
228 | } |
---|
229 | |
---|
230 | buf_avail = (size_t) n; |
---|
231 | buf_pos = 0; |
---|
232 | |
---|
233 | while (buf_pos < buf_avail) { |
---|
234 | const uint8_t *pkt = &ctx->buf[0] + buf_pos; |
---|
235 | struct bpf_hdr bp; |
---|
236 | |
---|
237 | memcpy(&bp, pkt, sizeof(bp)); |
---|
238 | |
---|
239 | if (bp.bh_caplen == bp.bh_datalen && |
---|
240 | buf_pos + bp.bh_caplen + bp.bh_hdrlen <= buf_avail) { |
---|
241 | const uint8_t *payload = pkt + bp.bh_hdrlen + |
---|
242 | ETHER_HDR_LEN; |
---|
243 | size_t payload_size = bp.bh_caplen - ETHER_HDR_LEN; |
---|
244 | |
---|
245 | process_arp_packet(payload, payload_size, ctx->fd, ctx->eaddr, |
---|
246 | processor, NULL); |
---|
247 | } |
---|
248 | |
---|
249 | buf_pos += BPF_WORDALIGN(bp.bh_hdrlen + bp.bh_caplen); |
---|
250 | } |
---|
251 | |
---|
252 | return 0; |
---|
253 | } |
---|