Root/tools/dirtpan/dirtpan.c

Source at commit a800eb8794253e0a3d655b1ae8bd5654d72d6e63 created 8 years 11 days ago.
By Werner Almesberger, tools/dirtpan/dirtpan.c: added missing #include "daemon.h", oops
1/*
2 * dirtpan/dirtpan.c - Quick and dirty IPv4 over 802.15.4 tunnel
3 *
4 * Written 2011 by Werner Almesberger
5 * Copyright 2011 Werner Almesberger
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 */
12
13
14#include <stdarg.h>
15#include <stdint.h>
16#include <stdlib.h>
17#include <stdio.h>
18#include <unistd.h>
19#include <string.h>
20#include <fcntl.h>
21#include <signal.h>
22#include <assert.h>
23#include <sys/types.h>
24#include <sys/time.h>
25#include <sys/wait.h>
26#include <sys/select.h>
27#include <sys/ioctl.h>
28#include <sys/socket.h>
29#include <linux/if.h>
30#include <linux/if_tun.h>
31
32#include <ieee802154.h>
33
34#include "daemon.h"
35
36
37/*
38 * Control byte structure:
39 *
40 * +--7--+--6--+--5--+--4--+--3--+--2--+--1--+
41 * | 0 | 0 | 0 | 0 | seq | pck_type |
42 * +-----+-----+-----+-----+-----+-----+-----+
43 */
44
45#define SEQ 4
46#define PT_MASK 3
47
48enum packet_type {
49    pt_first = 0,
50    pt_next = 1,
51    pt_ack = 2,
52};
53
54
55#define TUN_DEV "/dev/net/tun"
56
57#define MAX_FRAG (127-11-2-1) /* MHDR, FCS, control byte */
58#define MAX_PACKET 2000
59#define MAX_TRIES 5
60#define T_REASS_MS 200
61#define T_ACK_MS 50
62
63
64static int tun, net;
65static uint8_t rx_packet[MAX_PACKET], tx_packet[MAX_PACKET+1];
66static void *rx_pos, *tx_pos;
67static int rx_left, tx_left;
68static int txing = 0, rxing = 0;
69static int rx_seq, tx_seq = 0;
70static int retries;
71static int debug = 0;
72
73
74/* ----- Debugging --------------------------------------------------------- */
75
76
77static void debug_label(const char *label)
78{
79    fprintf(stderr, "%s(%c%c)",
80        label, txing ? 'T' : '-', rxing ? 'R' : '-');
81}
82
83
84static void dump(const void *buf, int size)
85{
86    const uint8_t *p = buf;
87
88    while (size--) {
89        fprintf(stderr, "%s%02x", p == buf ? "" : " ", *p);
90        p++;
91    }
92}
93
94
95static void debug_ip(const char *label, void *buf, int size)
96{
97    if (debug < 2)
98        return;
99    debug_label(label);
100    fprintf(stderr, ", %d: ", size);
101    dump(buf, size);
102    fprintf(stderr, "\n");
103}
104
105
106static void debug_dirt(const char *label, void *buf, int size)
107{
108    const uint8_t *p = buf;
109
110    if (!debug)
111        return;
112    if (debug == 1) {
113        if (size) {
114            fprintf(stderr, "%c%d",
115                (label[1] == '>' ? "FNA?" : "fna?")[*p & PT_MASK],
116                *p & SEQ ? 0 : 1);
117        }
118        return;
119    }
120    debug_label(label);
121    fprintf(stderr, ", %d+1: ", size-1);
122    if (size) {
123        fprintf(stderr, "%02x(%c%d) | ",
124            *p, "FNA?"[*p & PT_MASK], *p & SEQ ? 0 : 1);
125        dump(buf+1, size-1);
126    }
127    fprintf(stderr, "\n");
128
129}
130
131
132static void debug_timeout(const char *label)
133{
134    if (!debug)
135        return;
136    if (debug == 1) {
137        fprintf(stderr, "*%c", *label);
138        return;
139    }
140    debug_label(label);
141    fprintf(stderr, "\n");
142}
143
144
145/* ----- Timers ------------------------------------------------------------ */
146
147
148static struct timeval t_reass, t_ack;
149
150
151static void start_timer(struct timeval *t, int ms)
152{
153    assert(!t->tv_sec && !t->tv_usec);
154    gettimeofday(t, NULL);
155    t->tv_usec += 1000*ms;
156    while (t->tv_usec >= 1000000) {
157        t->tv_sec++;
158        t->tv_usec -= 1000000;
159    }
160}
161
162
163static void stop_timer(struct timeval *t)
164{
165    assert(t->tv_sec || t->tv_usec);
166    t->tv_sec = 0;
167    t->tv_usec = 0;
168}
169
170
171static const struct timeval *next_timer(int n, ...)
172{
173    va_list ap;
174    const struct timeval *next = NULL;
175    const struct timeval *t;
176
177    va_start(ap, n);
178    while (n--) {
179        t = va_arg(ap, const struct timeval *);
180        if (!t->tv_sec && !t->tv_usec)
181            continue;
182        if (next) {
183            if (next->tv_sec < t->tv_sec)
184                continue;
185            if (next->tv_sec == t->tv_sec &&
186                next->tv_usec < t->tv_usec)
187                continue;
188        }
189        next = t;
190    }
191    va_end(ap);
192    return next;
193}
194
195
196static struct timeval *timer_delta(const struct timeval *t)
197{
198    static struct timeval d;
199
200    if (!t)
201        return NULL;
202
203    gettimeofday(&d, NULL);
204    d.tv_sec = t->tv_sec-d.tv_sec;
205    d.tv_usec = t->tv_usec-d.tv_usec;
206
207    while (d.tv_usec < 0) {
208        d.tv_sec--;
209        d.tv_usec += 1000000;
210    }
211    if (d.tv_sec < 0)
212        d.tv_sec = d.tv_usec = 0;
213
214    return &d;
215}
216
217
218/* ----- Packet/frame delivery --------------------------------------------- */
219
220
221static inline int send_size(void)
222{
223    return tx_left > MAX_FRAG ? MAX_FRAG : tx_left;
224}
225
226
227static void write_buf(int fd, void *buf, int size)
228{
229    ssize_t wrote;
230
231    wrote = write(fd, buf, size);
232    if (wrote < 0) {
233        perror("write");
234        return;
235    }
236    if (wrote != size)
237        fprintf(stderr, "short write: %d < %d\n", (int) wrote, size);
238}
239
240
241static void send_frame(void *buf, int size)
242{
243    debug_dirt("->", buf, size);
244    write_buf(net, buf, size);
245}
246
247
248static void send_more(void)
249{
250    uint8_t *p = tx_pos-1;
251
252    *p = (tx_pos == tx_packet+1 ? pt_first : pt_next) | (tx_seq ? SEQ : 0);
253    send_frame(p, send_size()+1);
254    start_timer(&t_ack, T_ACK_MS);
255}
256
257
258static void send_ack(int seq)
259{
260    uint8_t ack = pt_ack | (seq ? SEQ : 0);
261
262    send_frame(&ack, 1);
263}
264
265
266/* ----- Main events ------------------------------------------------------- */
267
268
269static void rx_pck(void *buf, int size)
270{
271    const uint8_t *p = buf;
272    uint8_t ctrl, type, seq;
273
274    debug_dirt("-<", buf, size);
275
276    if (size < 1)
277        return;
278
279    ctrl = *p;
280    type = ctrl & PT_MASK;
281    seq = !!(ctrl & SEQ);
282
283    switch (type) {
284    case pt_first:
285        send_ack(seq);
286        if (rxing) {
287            stop_timer(&t_reass);
288            rxing = 0;
289        }
290        break;
291    case pt_next:
292        send_ack(seq);
293        if (!rxing)
294            return;
295        if (seq == rx_seq)
296            return; /* retransmission */
297        break;
298    case pt_ack:
299        if (!txing)
300            return;
301        if (seq != tx_seq)
302            return;
303        stop_timer(&t_ack);
304        tx_pos += send_size();
305        tx_left -= send_size();
306        if (!tx_left) {
307            txing = 0;
308            return;
309        }
310        tx_seq = !tx_seq;
311        retries = 0;
312        send_more();
313        return;
314    default:
315        abort();
316    }
317
318    if (!rxing) {
319        if (size < 5)
320            return;
321        rx_left = p[3] << 8 | p[4];
322        if (rx_left > MAX_PACKET)
323            return;
324        start_timer(&t_reass, T_REASS_MS);
325        rxing = 1;
326        rx_pos = rx_packet;
327    }
328
329    if (rx_left < size-1) {
330        stop_timer(&t_reass);
331        rxing = 0;
332        return;
333    }
334    memcpy(rx_pos, buf+1, size-1);
335    rx_pos += size-1;
336    rx_left -= size-1;
337    rx_seq = seq;
338
339    if (!rx_left) {
340        debug_ip("<-", rx_packet, rx_pos-(void *) rx_packet);
341        write_buf(tun, rx_packet, rx_pos-(void *) rx_packet);
342        stop_timer(&t_reass);
343        rxing = 0;
344    }
345}
346
347
348static void tx_pck(void *buf, int size)
349{
350    const uint8_t *p = buf;
351
352    debug_ip(">-", buf, size);
353    assert(!txing);
354    txing = 1;
355    tx_pos = tx_packet+1;
356    tx_left = p[2] << 8 | p[3];
357    assert(tx_left <= MAX_PACKET);
358    assert(tx_left == size);
359    /*
360     * @@@ We could avoid the memcpy by reading directly into "tx_packet"
361     */
362    memcpy(tx_pos, buf, size);
363    tx_seq = !tx_seq;
364    retries = 0;
365    send_more();
366}
367
368
369static void ack_timeout(void)
370{
371    debug_timeout("ACK-TO");
372    assert(txing);
373    stop_timer(&t_ack);
374    if (++retries == MAX_TRIES)
375        txing = 0;
376    else
377        send_more();
378}
379
380
381static void reass_timeout(void)
382{
383    debug_timeout("REASS-TO");
384    assert(rxing);
385    stop_timer(&t_reass);
386    rxing = 0;
387}
388
389
390/* ----- Event dispatcher -------------------------------------------------- */
391
392
393static void event(void)
394{
395    uint8_t buf[MAX_PACKET];
396    const struct timeval *to;
397    fd_set rset;
398    int res;
399    ssize_t got;
400
401    FD_ZERO(&rset);
402    FD_SET(net, &rset);
403
404    /* only accept more work if we're idle */
405    if (!txing && !rxing)
406        FD_SET(tun, &rset);
407
408    to = next_timer(2, &t_reass, &t_ack);
409
410    res = select(net > tun ? net+1 : tun+1, &rset, NULL, NULL,
411        timer_delta(to));
412    if (res < 0) {
413        perror("select");
414        return;
415    }
416    if (!res) {
417        assert(to);
418        if (to == &t_reass)
419            reass_timeout();
420        else
421            ack_timeout();
422    }
423    if (FD_ISSET(tun, &rset)) {
424        got = read(tun, buf, sizeof(buf));
425        if (got < 0) {
426            perror("read tun");
427            return;
428        }
429        tx_pck(buf, got);
430    }
431    if (FD_ISSET(net, &rset)) {
432        got = read(net, buf, sizeof(buf));
433        if (got < 0) {
434            perror("read net");
435            return;
436        }
437        rx_pck(buf, got);
438    }
439}
440
441
442/* ----- Setup ------------------------------------------------------------- */
443
444
445static int open_net(uint16_t pan, uint16_t me, uint16_t peer)
446{
447    struct sockaddr_ieee802154 addr;
448    int zero = 0;
449    int s;
450
451    s = socket(PF_IEEE802154, SOCK_DGRAM, 0);
452    if (s < 0) {
453        perror("socket 802.15.4");
454        exit(1);
455    }
456
457    addr.family = AF_IEEE802154;
458    addr.addr.addr_type = IEEE802154_ADDR_SHORT;
459    addr.addr.pan_id = pan;
460    addr.addr.short_addr = me;
461
462    if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
463        perror("bind 802.15.4");
464        exit(1);
465    }
466
467    addr.addr.short_addr = peer;
468    if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
469        perror("connect 802.15.4");
470        exit(1);
471    }
472
473    if (setsockopt(s, SOL_IEEE802154, WPAN_WANTACK, &zero, sizeof(zero))
474        < 0) {
475        perror("setsockopt SOL_IEEE802154 WPAN_WANTACK");
476        exit(1);
477    }
478
479    return s;
480}
481
482
483static int open_tun(const char *cmd)
484{
485    struct ifreq ifr;
486    int fd, res;
487
488    fd = open(TUN_DEV, O_RDWR);
489    if (fd < 0) {
490        perror(TUN_DEV);
491        exit(1);
492    }
493
494    memset(&ifr, 0, sizeof(ifr));
495    ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
496
497    if (ioctl(fd, TUNSETIFF, (void *) &ifr) < 0) {
498        perror("ioctl TUNSETIFF");
499        exit(1);
500    }
501
502    if (!cmd) {
503        fprintf(stderr, "%s\n", ifr.ifr_name);
504        return fd;
505    }
506
507    if (setenv("ITF", ifr.ifr_name, 1) < 0) {
508        perror("setenv");
509        exit(1);
510    }
511
512    res = system(cmd);
513    if (res < 0) {
514        perror("system");
515        exit(1);
516    }
517    if (WIFEXITED(res)) {
518        if (!WEXITSTATUS(res))
519            return fd;
520        exit(WEXITSTATUS(res));
521    }
522    if (WIFSIGNALED(res)) {
523        raise(WTERMSIG(res));
524        exit(1);
525    }
526
527    fprintf(stderr, "cryptic exit status %d\n", res);
528    exit(1);
529}
530
531
532/* ----- Command-line processing ------------------------------------------- */
533
534
535static void usage(const char *name)
536{
537    fprintf(stderr,
538"usage: %s [-b] [-d [-d]] pan_id src_addr dst_addr [command]\n\n"
539" pan_id PAN (network) identifier\n"
540" src_addr source short address\n"
541" dst_addr destination short address\n"
542" command configuration command to run after creating the TUN interface.\n"
543" The environment variable $ITF is set to the interface name.\n\n"
544" -b background the process after initialization\n"
545" -d ... increase verbosity of debug output\n"
546    , name);
547    exit(1);
548}
549
550
551static uint16_t addr(const char *name, const char *s)
552{
553    char *end;
554    unsigned long n;
555
556    n = strtoul(s, &end, 16);
557    if (*end)
558        usage(name);
559    if (n > 0xffff)
560        usage(name);
561    return n;
562}
563
564
565int main(int argc, char **argv)
566{
567    const char *cmd = NULL;
568    uint16_t pan, src, dst;
569    int foreground = 1;
570    int c;
571
572    while ((c = getopt(argc, argv, "bd")) != EOF)
573        switch (c) {
574        case 'b':
575            foreground = 0;
576            break;
577        case 'd':
578            debug++;
579            break;
580        default:
581            usage(*argv);
582        }
583
584    switch (argc-optind) {
585    case 4:
586        cmd = argv[optind+3];
587        /* fall through */
588    case 3:
589        pan = addr(*argv, argv[optind]);
590        src = addr(*argv, argv[optind+1]);
591        dst = addr(*argv, argv[optind+2]);
592        break;
593    default:
594        usage(*argv);
595    }
596
597    net = open_net(pan, src, dst);
598    tun = open_tun(cmd);
599
600    if (foreground || !daemonize())
601        while (1)
602            event();
603
604    return 0;
605}
606

Archive Download this file



interactive