Root/tools/dirtpan/dirtpan.c

Source at commit cec05a0f5ea2fb4ff5637d418c7f5dae196abda8 created 6 years 9 months ago.
By Werner Almesberger, atusb/atusb.pro: we don't use power.lib anymore
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 <errno.h>
23#include <assert.h>
24#include <sys/types.h>
25#include <sys/time.h>
26#include <sys/wait.h>
27#include <sys/select.h>
28#include <sys/ioctl.h>
29#include <sys/socket.h>
30#include <linux/if.h>
31#include <linux/if_tun.h>
32
33#include <ieee802154.h>
34
35#include "daemon.h"
36
37
38/*
39 * Control byte structure:
40 *
41 * +--7--+--6--+--5--+--4--+--3--+--2--+--1--+--0--+
42 * | 0 | 0 | 0 | 0 | 0 | seq | pck_type |
43 * +-----+-----+-----+-----+-----+-----+-----+-----+
44 * =============
45 *
46 * Bits 6 and 7 must be zero to avoid conflicts with RFC 4944 (LoWPAN)
47 */
48
49#define SEQ 4
50#define PT_MASK 3
51
52enum packet_type {
53    pt_first = 0,
54    pt_next = 1,
55    pt_ack = 2,
56};
57
58
59#define TUN_DEV "/dev/net/tun"
60
61#define MAX_FRAG (127-11-2-1) /* MHDR, FCS, control byte */
62#define MAX_PACKET 2000
63#define MAX_TRIES 5
64#define T_REASS_MS 200
65#define T_ACK_MS 50
66
67
68static struct {
69    unsigned tx_pck; /* packets enqueued */
70    unsigned lost; /* packets never acked */
71    unsigned rx_pck; /* packets successfully received */
72    unsigned tx_frm; /* frames sent */
73    unsigned rx_frm; /* frames received */
74    unsigned tx_frst; /* pt_first sent */
75    unsigned rx_frst; /* pt_first received */
76    unsigned tx_nxt; /* pt_next sent */
77    unsigned rx_nxt; /* pt_next received */
78    unsigned tx_ack; /* pt_ack sent */
79    unsigned rx_ack; /* pt_ack received */
80    unsigned invalid; /* invalid packet type */
81    unsigned not_rx; /* pt_next but not receiving */
82    unsigned rx_seq; /* wrong rx sequence */
83    unsigned not_tx; /* pt_ack but not sending */
84    unsigned ack_seq; /* wrong ack sequence */
85    unsigned garbled; /* IPv4 length problem */
86    unsigned no_ack; /* no ack response (giving up) */
87    unsigned retry; /* retransmit */
88    unsigned reass; /* reassembly timeout */
89} stats;
90
91static int tun, net;
92static uint8_t rx_packet[MAX_PACKET], tx_packet[MAX_PACKET+1];
93static void *rx_pos, *tx_pos;
94static int rx_left, tx_left;
95static int txing = 0, rxing = 0;
96static int rx_seq, tx_seq = 0;
97static int retries;
98static int debug = 0;
99
100
101/* ----- Debugging --------------------------------------------------------- */
102
103
104static void debug_label(const char *label)
105{
106    fprintf(stderr, "%s(%c%c)",
107        label, txing ? 'T' : '-', rxing ? 'R' : '-');
108}
109
110
111static void dump(const void *buf, int size)
112{
113    const uint8_t *p = buf;
114
115    while (size--) {
116        fprintf(stderr, "%s%02x", p == buf ? "" : " ", *p);
117        p++;
118    }
119}
120
121
122static void debug_ip(const char *label, void *buf, int size)
123{
124    if (debug < 2)
125        return;
126    debug_label(label);
127    fprintf(stderr, ", %d: ", size);
128    dump(buf, size);
129    fprintf(stderr, "\n");
130}
131
132
133static void debug_dirt(const char *label, void *buf, int size)
134{
135    const uint8_t *p = buf;
136
137    if (!debug)
138        return;
139    if (debug == 1) {
140        if (size) {
141            fprintf(stderr, "%c%d",
142                (label[1] == '>' ? "FNA?" : "fna?")[*p & PT_MASK],
143                *p & SEQ ? 0 : 1);
144        }
145        return;
146    }
147    debug_label(label);
148    fprintf(stderr, ", %d+1: ", size-1);
149    if (size) {
150        fprintf(stderr, "%02x(%c%d) | ",
151            *p, "FNA?"[*p & PT_MASK], *p & SEQ ? 0 : 1);
152        dump(buf+1, size-1);
153    }
154    fprintf(stderr, "\n");
155
156}
157
158
159static void debug_timeout(const char *label)
160{
161    if (!debug)
162        return;
163    if (debug == 1) {
164        fprintf(stderr, "*%c", *label);
165        return;
166    }
167    debug_label(label);
168    fprintf(stderr, "\n");
169}
170
171
172/* ----- Statistics -------------------------------------------------------- */
173
174
175static void handle_usr1(int sig)
176{
177    fprintf(stderr, "\n"
178        "tx_pck\t%6u\n" "tx_lost\t%6u\n" "rx_pck\t%6u\n"
179        "tx_frm\t%6u\n" "rx_frm\t%6u\n" "tx_frst\t%6u\n"
180        "rx_frst\t%6u\n" "tx_nxt\t%6u\n" "rx_nxt\t%6u\n"
181        "tx_ack\t%6u\n" "rx_ack\t%6u\n" "invalid\t%6u\n"
182        "not_rx\t%6u\n" "rx_seq\t%6u\n" "not_tx\t%6u\n"
183        "ack_seq\t%6u\n" "garbled\t%6u\n" "no_ack\t%6u\n"
184        "retry\t%6u\n" "reass\t%6u\n",
185        stats.tx_pck, stats.lost, stats.rx_pck,
186        stats.tx_frm, stats.rx_frm, stats.tx_frst,
187        stats.rx_frst, stats.tx_nxt, stats.rx_nxt,
188        stats.tx_ack, stats.rx_ack, stats.invalid,
189        stats.not_rx, stats.rx_seq, stats.not_tx,
190        stats.ack_seq, stats.garbled, stats.no_ack,
191        stats.retry, stats.reass);
192}
193
194
195static void handle_usr2(int sig)
196{
197    memset(&stats, 0, sizeof(stats));
198}
199
200
201/* ----- Timers ------------------------------------------------------------ */
202
203
204static struct timeval t_reass, t_ack;
205
206
207static void start_timer(struct timeval *t, int ms)
208{
209    assert(!t->tv_sec && !t->tv_usec);
210    gettimeofday(t, NULL);
211    t->tv_usec += 1000*ms;
212    while (t->tv_usec >= 1000000) {
213        t->tv_sec++;
214        t->tv_usec -= 1000000;
215    }
216}
217
218
219static void stop_timer(struct timeval *t)
220{
221    assert(t->tv_sec || t->tv_usec);
222    t->tv_sec = 0;
223    t->tv_usec = 0;
224}
225
226
227static const struct timeval *next_timer(int n, ...)
228{
229    va_list ap;
230    const struct timeval *next = NULL;
231    const struct timeval *t;
232
233    va_start(ap, n);
234    while (n--) {
235        t = va_arg(ap, const struct timeval *);
236        if (!t->tv_sec && !t->tv_usec)
237            continue;
238        if (next) {
239            if (next->tv_sec < t->tv_sec)
240                continue;
241            if (next->tv_sec == t->tv_sec &&
242                next->tv_usec < t->tv_usec)
243                continue;
244        }
245        next = t;
246    }
247    va_end(ap);
248    return next;
249}
250
251
252static struct timeval *timer_delta(const struct timeval *t)
253{
254    static struct timeval d;
255
256    if (!t)
257        return NULL;
258
259    gettimeofday(&d, NULL);
260    d.tv_sec = t->tv_sec-d.tv_sec;
261    d.tv_usec = t->tv_usec-d.tv_usec;
262
263    while (d.tv_usec < 0) {
264        d.tv_sec--;
265        d.tv_usec += 1000000;
266    }
267    if (d.tv_sec < 0)
268        d.tv_sec = d.tv_usec = 0;
269
270    return &d;
271}
272
273
274/* ----- Packet/frame delivery --------------------------------------------- */
275
276
277static inline int send_size(void)
278{
279    return tx_left > MAX_FRAG ? MAX_FRAG : tx_left;
280}
281
282
283static void write_buf(int fd, void *buf, int size)
284{
285    ssize_t wrote;
286
287    wrote = write(fd, buf, size);
288    if (wrote < 0) {
289        perror("write");
290        return;
291    }
292    if (wrote != size)
293        fprintf(stderr, "short write: %d < %d\n", (int) wrote, size);
294}
295
296
297static void send_frame(void *buf, int size)
298{
299    debug_dirt("->", buf, size);
300    write_buf(net, buf, size);
301    stats.tx_frm++;
302}
303
304
305static void send_more(void)
306{
307    uint8_t *p = tx_pos-1;
308
309    if (tx_pos == tx_packet+1) {
310        *p = pt_first;
311        stats.tx_frst++;
312    } else {
313        *p = pt_next;
314        stats.tx_nxt++;
315    }
316    *p |= tx_seq ? SEQ : 0;
317    send_frame(p, send_size()+1);
318    start_timer(&t_ack, T_ACK_MS);
319}
320
321
322static void send_ack(int seq)
323{
324    uint8_t ack = pt_ack | (seq ? SEQ : 0);
325
326    send_frame(&ack, 1);
327    stats.tx_ack++;
328}
329
330
331/* ----- Main events ------------------------------------------------------- */
332
333
334static void rx_pck(void *buf, int size)
335{
336    const uint8_t *p = buf;
337    uint8_t ctrl, type, seq;
338
339    debug_dirt("-<", buf, size);
340
341    stats.rx_frm++;
342    if (size < 1) {
343        stats.invalid++;
344        return;
345    }
346
347    ctrl = *p;
348    type = ctrl & PT_MASK;
349    seq = !!(ctrl & SEQ);
350
351    switch (type) {
352    case pt_first:
353        stats.rx_frst++;
354        send_ack(seq);
355        if (rxing) {
356            stop_timer(&t_reass);
357            rxing = 0;
358        }
359        break;
360    case pt_next:
361        stats.rx_nxt++;
362        send_ack(seq);
363        if (!rxing) {
364            stats.not_rx++;
365            return;
366        }
367        if (seq == rx_seq) {
368            stats.rx_seq++;
369            return; /* retransmission */
370        }
371        break;
372    case pt_ack:
373        stats.rx_ack++;
374        if (!txing) {
375            stats.not_tx++;
376            return;
377        }
378        if (seq != tx_seq) {
379            stats.ack_seq++;
380            return;
381        }
382        stop_timer(&t_ack);
383        tx_pos += send_size();
384        tx_left -= send_size();
385        if (!tx_left) {
386            stats.lost--;
387            txing = 0;
388            return;
389        }
390        tx_seq = !tx_seq;
391        retries = 0;
392        send_more();
393        return;
394    default:
395        stats.invalid++;
396        return;
397    }
398
399    if (!rxing) {
400        if (size < 5) {
401            stats.garbled++;
402            return;
403        }
404        rx_left = p[3] << 8 | p[4];
405        if (rx_left > MAX_PACKET) {
406            stats.garbled++;
407            return;
408        }
409        start_timer(&t_reass, T_REASS_MS);
410        rxing = 1;
411        rx_pos = rx_packet;
412    }
413
414    if (rx_left < size-1) {
415        stats.garbled++;
416        stop_timer(&t_reass);
417        rxing = 0;
418        return;
419    }
420    memcpy(rx_pos, buf+1, size-1);
421    rx_pos += size-1;
422    rx_left -= size-1;
423    rx_seq = seq;
424
425    if (!rx_left) {
426        debug_ip("<-", rx_packet, rx_pos-(void *) rx_packet);
427        write_buf(tun, rx_packet, rx_pos-(void *) rx_packet);
428        stop_timer(&t_reass);
429        rxing = 0;
430        stats.rx_pck++;
431    }
432}
433
434
435static void tx_pck(void *buf, int size)
436{
437    const uint8_t *p = buf;
438
439    debug_ip(">-", buf, size);
440    assert(!txing);
441    txing = 1;
442    tx_pos = tx_packet+1;
443    tx_left = p[2] << 8 | p[3];
444    assert(tx_left <= MAX_PACKET);
445    assert(tx_left == size);
446    /*
447     * @@@ We could avoid the memcpy by reading directly into "tx_packet"
448     */
449    memcpy(tx_pos, buf, size);
450    tx_seq = !tx_seq;
451    retries = 0;
452    send_more();
453    stats.tx_pck++;
454    stats.lost++;
455}
456
457
458static void ack_timeout(void)
459{
460    debug_timeout("ACK-TO");
461    assert(txing);
462    stop_timer(&t_ack);
463    if (++retries == MAX_TRIES) {
464        txing = 0;
465        stats.no_ack++;
466    } else {
467        send_more();
468        stats.retry++;
469    }
470}
471
472
473static void reass_timeout(void)
474{
475    debug_timeout("REASS-TO");
476    assert(rxing);
477    stop_timer(&t_reass);
478    stats.reass++;
479    rxing = 0;
480}
481
482
483/* ----- Event dispatcher -------------------------------------------------- */
484
485
486static void event(void)
487{
488    uint8_t buf[MAX_PACKET];
489    const struct timeval *to;
490    fd_set rset;
491    int res;
492    ssize_t got;
493
494    FD_ZERO(&rset);
495    FD_SET(net, &rset);
496
497    /* only accept more work if we're idle */
498    if (!txing && !rxing)
499        FD_SET(tun, &rset);
500
501    to = next_timer(2, &t_reass, &t_ack);
502
503    res = select(net > tun ? net+1 : tun+1, &rset, NULL, NULL,
504        timer_delta(to));
505    if (res < 0) {
506        if (errno != EINTR)
507            perror("select");
508        return;
509    }
510    if (!res) {
511        assert(to);
512        if (to == &t_reass)
513            reass_timeout();
514        else
515            ack_timeout();
516    }
517    if (FD_ISSET(tun, &rset)) {
518        got = read(tun, buf, sizeof(buf));
519        if (got < 0) {
520            perror("read tun");
521            return;
522        }
523        tx_pck(buf, got);
524    }
525    if (FD_ISSET(net, &rset)) {
526        got = read(net, buf, sizeof(buf));
527        if (got < 0) {
528            perror("read net");
529            return;
530        }
531        rx_pck(buf, got);
532    }
533}
534
535
536/* ----- Setup ------------------------------------------------------------- */
537
538
539static int open_net(uint16_t pan, uint16_t me, uint16_t peer)
540{
541    struct sockaddr_ieee802154 addr;
542    int zero = 0;
543    int s;
544
545    s = socket(PF_IEEE802154, SOCK_DGRAM, 0);
546    if (s < 0) {
547        perror("socket 802.15.4");
548        exit(1);
549    }
550
551    addr.family = AF_IEEE802154;
552    addr.addr.addr_type = IEEE802154_ADDR_SHORT;
553    addr.addr.pan_id = pan;
554    addr.addr.short_addr = me;
555
556    if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
557        perror("bind 802.15.4");
558        exit(1);
559    }
560
561    addr.addr.short_addr = peer;
562    if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
563        perror("connect 802.15.4");
564        exit(1);
565    }
566
567    if (setsockopt(s, SOL_IEEE802154, WPAN_WANTACK, &zero, sizeof(zero))
568        < 0) {
569        perror("setsockopt SOL_IEEE802154 WPAN_WANTACK");
570        exit(1);
571    }
572
573    return s;
574}
575
576
577static int open_tun(const char *cmd)
578{
579    struct ifreq ifr;
580    int fd, res;
581
582    fd = open(TUN_DEV, O_RDWR);
583    if (fd < 0) {
584        perror(TUN_DEV);
585        exit(1);
586    }
587
588    memset(&ifr, 0, sizeof(ifr));
589    ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
590
591    if (ioctl(fd, TUNSETIFF, (void *) &ifr) < 0) {
592        perror("ioctl TUNSETIFF");
593        exit(1);
594    }
595
596    if (!cmd) {
597        fprintf(stderr, "%s\n", ifr.ifr_name);
598        return fd;
599    }
600
601    if (setenv("ITF", ifr.ifr_name, 1) < 0) {
602        perror("setenv");
603        exit(1);
604    }
605
606    res = system(cmd);
607    if (res < 0) {
608        perror("system");
609        exit(1);
610    }
611    if (WIFEXITED(res)) {
612        if (!WEXITSTATUS(res))
613            return fd;
614        exit(WEXITSTATUS(res));
615    }
616    if (WIFSIGNALED(res)) {
617        raise(WTERMSIG(res));
618        exit(1);
619    }
620
621    fprintf(stderr, "cryptic exit status %d\n", res);
622    exit(1);
623}
624
625
626/* ----- Command-line processing ------------------------------------------- */
627
628
629static void usage(const char *name)
630{
631    fprintf(stderr,
632"usage: %s [-b] [-d [-d]] pan_id src_addr dst_addr [command]\n\n"
633" pan_id PAN (network) identifier\n"
634" src_addr source short address\n"
635" dst_addr destination short address\n"
636" command configuration command to run after creating the TUN interface.\n"
637" The environment variable $ITF is set to the interface name.\n\n"
638" -b background the process after initialization\n"
639" -d ... increase verbosity of debug output\n"
640    , name);
641    exit(1);
642}
643
644
645static uint16_t addr(const char *name, const char *s)
646{
647    char *end;
648    unsigned long n;
649
650    n = strtoul(s, &end, 16);
651    if (*end)
652        usage(name);
653    if (n > 0xffff)
654        usage(name);
655    return n;
656}
657
658
659int main(int argc, char **argv)
660{
661    const char *cmd = NULL;
662    uint16_t pan, src, dst;
663    int foreground = 1;
664    int c;
665
666    while ((c = getopt(argc, argv, "bd")) != EOF)
667        switch (c) {
668        case 'b':
669            foreground = 0;
670            break;
671        case 'd':
672            debug++;
673            break;
674        default:
675            usage(*argv);
676        }
677
678    switch (argc-optind) {
679    case 4:
680        cmd = argv[optind+3];
681        /* fall through */
682    case 3:
683        pan = addr(*argv, argv[optind]);
684        src = addr(*argv, argv[optind+1]);
685        dst = addr(*argv, argv[optind+2]);
686        break;
687    default:
688        usage(*argv);
689    }
690
691    net = open_net(pan, src, dst);
692    tun = open_tun(cmd);
693
694
695    if (foreground) {
696        signal(SIGUSR1, handle_usr1);
697        signal(SIGUSR2, handle_usr2);
698    } else {
699        if (daemonize())
700            return 0;
701    }
702
703    while (1)
704        event();
705}
706

Archive Download this file

Branches:
buffer-frames
master
stefan/rzusb
sysmocom

Tags:
2016-atusb-production-boot.hex
v0.2-atusb-fw
v0.3-atusb-fw



interactive