source: rtems-libbsd/rtemsbsd/rtems/rtems-bsd-arp-processor.c @ dd35ec5

5-freebsd-12
Last change on this file since dd35ec5 was f8c3d6e, checked in by Sebastian Huber <sebastian.huber@…>, on Jan 20, 2014 at 2:01:35 PM

Add an ARP packet processor

  • Property mode set to 100644
File size: 6.1 KB
Line 
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
56struct 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
69static 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
85static const size_t arp_bpf_filter_len =
86    sizeof(arp_bpf_filter) / sizeof(arp_bpf_filter[0]);
87
88static void
89process_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
137rtems_bsd_arp_processor_context *
138rtems_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
202error:
203        close(fd);
204        free(ctx);
205
206        return NULL;
207}
208
209int
210rtems_bsd_arp_processor_get_file_descriptor(
211    const rtems_bsd_arp_processor_context *ctx)
212{
213        return ctx->fd;
214}
215
216int
217rtems_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}
Note: See TracBrowser for help on using the repository browser.