From 4201d3496daa53e050e6ea9c2b9ead46c4300b33 Mon Sep 17 00:00:00 2001 From: eduazocar Date: Fri, 16 Sep 2022 16:08:57 -0400 Subject: [PATCH] dist/tools/zep_dispatch: Added zep_dispatch, topogen and auto-node launch --- dist/tools/zep_dispatch/.gitignore | 3 + dist/tools/zep_dispatch/Makefile | 55 ++++ dist/tools/zep_dispatch/README.md | 184 +++++++++++++ dist/tools/zep_dispatch/example.topo | 10 + dist/tools/zep_dispatch/file.topo | 21 ++ dist/tools/zep_dispatch/launch.sh | 27 ++ dist/tools/zep_dispatch/main.c | 295 +++++++++++++++++++++ dist/tools/zep_dispatch/network.topo | 29 ++ dist/tools/zep_dispatch/start_network.sh | 118 +++++++++ dist/tools/zep_dispatch/topogen.c | 261 ++++++++++++++++++ dist/tools/zep_dispatch/topogen.sh | 47 ++++ dist/tools/zep_dispatch/topology.c | 322 +++++++++++++++++++++++ dist/tools/zep_dispatch/topology.h | 91 +++++++ dist/tools/zep_dispatch/zep_parser.c | 103 ++++++++ dist/tools/zep_dispatch/zep_parser.h | 53 ++++ 15 files changed, 1619 insertions(+) create mode 100644 dist/tools/zep_dispatch/.gitignore create mode 100644 dist/tools/zep_dispatch/Makefile create mode 100644 dist/tools/zep_dispatch/README.md create mode 100644 dist/tools/zep_dispatch/example.topo create mode 100644 dist/tools/zep_dispatch/file.topo create mode 100755 dist/tools/zep_dispatch/launch.sh create mode 100644 dist/tools/zep_dispatch/main.c create mode 100644 dist/tools/zep_dispatch/network.topo create mode 100755 dist/tools/zep_dispatch/start_network.sh create mode 100644 dist/tools/zep_dispatch/topogen.c create mode 100755 dist/tools/zep_dispatch/topogen.sh create mode 100644 dist/tools/zep_dispatch/topology.c create mode 100644 dist/tools/zep_dispatch/topology.h create mode 100644 dist/tools/zep_dispatch/zep_parser.c create mode 100644 dist/tools/zep_dispatch/zep_parser.h diff --git a/dist/tools/zep_dispatch/.gitignore b/dist/tools/zep_dispatch/.gitignore new file mode 100644 index 000000000..3c13b01d7 --- /dev/null +++ b/dist/tools/zep_dispatch/.gitignore @@ -0,0 +1,3 @@ +*.gv +*.pdf +*.svg diff --git a/dist/tools/zep_dispatch/Makefile b/dist/tools/zep_dispatch/Makefile new file mode 100644 index 000000000..e5fd85d28 --- /dev/null +++ b/dist/tools/zep_dispatch/Makefile @@ -0,0 +1,55 @@ +CFLAGS ?= -g -O3 -Wall -Wextra +CFLAGS += $(RIOT_INCLUDE) +CFLAGS += -DNDEBUG # avoid assert re-definition + +DISPATCH := bin/zep_dispatch +TOPOGEN := bin/topogen + +all: $(DISPATCH) $(TOPOGEN) + +bin: + mkdir bin + +RIOTBASE := $(CURDIR)/../../../RIOT + +ZEP_PORT_BASE ?= 17754 +TOPOLOGY ?= network.topo +GV_OUT ?= $(TOPOLOGY).gv + +RIOT_INCLUDE += -I$(RIOTBASE)/core/lib/include +RIOT_INCLUDE += -I$(RIOTBASE)/cpu/native/include +RIOT_INCLUDE += -I$(RIOTBASE)/drivers/include +RIOT_INCLUDE += -I$(RIOTBASE)/sys/include + +SRCS := main.c topology.c zep_parser.c +SRCS += $(RIOTBASE)/sys/net/link_layer/ieee802154/ieee802154.c +SRCS += $(RIOTBASE)/sys/fmt/fmt.c +SRCS += $(RIOTBASE)/sys/net/link_layer/l2util/l2util.c + +$(DISPATCH): $(SRCS) bin + $(CC) $(CFLAGS) $(CFLAGS_EXTRA) $(SRCS) -o $@ + +$(TOPOGEN): topogen.c bin + $(CC) $(CFLAGS) $< -o $@ -lm + +.PHONY: clean run graph help +clean: + rm -fr bin + +run: $(DISPATCH) $(TOPOLOGY) + $(DISPATCH) -t $(TOPOLOGY) -g $(GV_OUT) ::1 $(ZEP_PORT_BASE) + +$(TOPOLOGY): $(TOPOGEN) + ./topogen.sh $(TOPOLOGY) $(NODES) + +graph: + killall -USR1 zep_dispatch + dot -Tpdf $(GV_OUT) > $(GV_OUT).pdf + +help: + @echo "run start ZEP dispatcher with the given \$$TOPOLOGY file" + @echo "graph print topology to \$$GV_OUT.pdf" + @echo "clean remove ZEP dispatcher binary" + +launch: + ./launch.sh diff --git a/dist/tools/zep_dispatch/README.md b/dist/tools/zep_dispatch/README.md new file mode 100644 index 000000000..c85a21919 --- /dev/null +++ b/dist/tools/zep_dispatch/README.md @@ -0,0 +1,184 @@ +ZEP packet dispatcher +===================== + +The ZEP dispatcher is a simple daemon that forwards ZEP packets to all connected +nodes. + +``` +usage: zep_dispatch [-t topology] [-s seed] [-g graphviz_out]
+``` + +By default the dispatcher will forward every packet it receives to every other +connected node (flat topology). + +Advanced Topology Mode +---------------------- + +It is possible to simulate a more complex topology as well as packet loss by +specifying a topology file. + + +The file format is: + +``` + [weight_ab] [weight_ba] +``` + +This line defines a connection between and . +The weight of the edge -> is . + +An edge weight is a float value between 0 and 1 that represents the probability of +a successful packet transmission on that path. + +A weight of `0` would mean no connection, whereas a weight of `1` would be a perfect +connection. Intermediate values are possible, e.g. `0.6` would result in a packet +loss probability of 40%. + +The weights can be omitted. +If both weights are omitted, a perfect connection is assumed. +If is omitted, a symmetric connection is assumed and is used +for both directions. + +refer to the file `example.topo` for an example. + +There can only be as many nodes connected to the dispatcher as have been named in +the topology file. +Any additional nodes that try to connect will be ignored. + + +Packet capture +-------------- + +To view traffic in Wireshark you need to create a virtual 802.15.4 device to which +the network traffic is sent. This can then be selected as a capture source in Wireshark. + +To create the virtual device, load the `mac802154_hwsim` module + + sudo modprobe mac802154_hwsim + +This will create two wpan devices, `wpan0` and `wpan1`. + +If you then run the dispatcher with + + sudo zep_dispatch -w wpan0 ::1 17754 + +all traffic on the simulated network will be also sent to the virtual `wpan0` interface +where it can be captures with Wireshark. + + +Network visualization +--------------------- + +It is possible to generate a graph of the current simulated network by executing the + + make graph + +target. This sends a USR1 signal to the `zep_dispatch` process to generate a graph in +Graphviz format. + + +### Foren6 + +For real-time visualization of the (RPL) network, the [Foren6](https://cetic.github.io/foren6/) +tool can be used. + +Support for ZEP dispatcher is currently only available through [a fork](https://github.com/benpicco/foren6). + +#### Foren6 setup + +Start `foren6` with the `make run` target and in the 'Manage Sources' menu, add a ZEP sniffer: + +![](https://user-images.githubusercontent.com/1301112/134354399-7010ad73-044d-4ffa-ad99-61a6838af268.png) + +The 'target' field takes a hostname:port combination, but you can leave it blank. In that case the default `[::1]:17754` will be used. + +#### Foren6 usage + +Foren6 will connect to a running ZEP dispatcher. If the dispatcher started after Foren6 or +if the dispatcher was restarted, you have to re-connect by stopping the current capture ('Stop' button) +and starting it again ('Start' button). + +You should see a view of the DODAG with all nodes in the network as they send packets. + +![](https://user-images.githubusercontent.com/1301112/144511776-3a2d7072-8162-40dc-911f-dfe476d01112.png) + +#### RIOT setup + +A proper simulated network will need a border router and some mesh nodes. +The `gnrc_border_router` and `gnrc_networking` can serve as a starting point for those. + +By default the border router example will start the ZEP dispatcher with a flat topology, that is +each node is connected to every other node. +This makes for a very boring topology where no RPL is needed. + +To run the ZEP dispatcher with a custom topology, simply run it before the `gnrc_border_router` +example. You can use the `make run` target which will default to the `example.topo` topology. +This can be changed with the `TOPOLOGY` environment variable. + +Next, start the border router example with RPL enabled: + + USEMODULE=gnrc_rpl make -C examples/gnrc_border_router all term + +Verify that the border router got a prefix on it's downstream interface with `ifconfig`. + +``` +Iface 7 HWaddr: 36:DE Channel: 26 NID: 0x23 + Long HWaddr: BE:92:89:04:D7:B4:B6:DE + L2-PDU:102 MTU:1280 HL:64 RTR + RTR_ADV 6LO IPHC + Source address length: 8 + Link type: wireless + inet6 addr: fe80::bc92:8904:d7b4:b6de scope: link VAL + --> inet6 addr: 2001:db8::bc92:8904:d7b4:b6de scope: global VAL + inet6 group: ff02::2 + inet6 group: ff02::1 + inet6 group: ff02::1:ffb4:b6de + inet6 group: ff02::1a + +Iface 6 HWaddr: 7A:37:FC:7D:1A:AF + L2-PDU:1500 MTU:1500 HL:64 RTR + Source address length: 6 + Link type: wired + inet6 addr: fe80::7837:fcff:fe7d:1aaf scope: link VAL + inet6 addr: fe80::2 scope: link VAL + inet6 group: ff02::2 + inet6 group: ff02::1 + inet6 group: ff02::1:ff7d:1aaf + inet6 group: ff02::1:ff00:2 +``` + +Now start as many `gnrc_networking` nodes as you have mesh nodes defined in your topology file: + + USE_ZEP=1 make -C examples/gnrc_networking all term + USE_ZEP=1 make -C examples/gnrc_networking all term + … + +The node should be able to join the DODAG as you can verify with the `rpl` command: + +``` +instance table: [X] +parent table: [X] [ ] [ ] + +instance [0 | Iface: 7 | mop: 2 | ocp: 0 | mhri: 256 | mri 0] + dodag [2001:db8::bc92:8904:d7b4:b6de | R: 768 | OP: Router | PIO: on | TR(I=[8,20], k=10, c=119)] + parent [addr: fe80::6467:2e89:b035:d51f | rank: 512] +``` + +This should also be visible in Foren6. + +Topology generation +------------------- + +To generate a random topology use the `topogen.sh` script. +This will randomly distribute *N* nodes on on a *W* × *H* map. +Each node has a radio range *R* ± *V* where *V* is a random variance that can also be set to 0. + +The further away a node is from a sending node, the higher the packet loss probability. +Nodes outside the sending radius have a zero probability of receiving a packet. + +If you have `gnuplot` installed this will also generate a plot of the resulting node distribution: + +![example topology](https://gist.githubusercontent.com/benpicco/6fd6f7c79a30cbbc41c3a65e53ed3682/raw/33afb859b65d949238129096858d14e2319fb5fb/network.topo.svg) + +A light color means that a node only has a one-way connection to the network, gray means a node is +entirely isolated. diff --git a/dist/tools/zep_dispatch/example.topo b/dist/tools/zep_dispatch/example.topo new file mode 100644 index 000000000..f55bd7f13 --- /dev/null +++ b/dist/tools/zep_dispatch/example.topo @@ -0,0 +1,10 @@ +# uncomment to pin node A to a specific MAC address +# A := BA:7C:18:E4:C0:45:65:AF + +# A and B are connected with an ideal link +A B +# A and C are connected with a symmetric link with 10% packet loss +A C 0.9 +# B and C are on an asymmetric connection +# The path B -> C has 60% packet loss while C -> B has 30% packet loss +B C 0.4 0.7 diff --git a/dist/tools/zep_dispatch/file.topo b/dist/tools/zep_dispatch/file.topo new file mode 100644 index 000000000..83bf76921 --- /dev/null +++ b/dist/tools/zep_dispatch/file.topo @@ -0,0 +1,21 @@ +# MAC Addresses +A := C3:20:D8:BF:73:D0:F6:69 +B := A6:5A:49:45:82:62:C3:F2 +C := 23:19:A4:BD:C2:1E:55:77 +D := 2A:3A:AC:FA:9A:EF:48:5E +E := 10:A0:9E:04:71:15:6D:18 + +# seed = 1663393381 +# Connections +B C 0.99 +C D 0.87 +C E 0.79 +A E 0.47 +A B 0.99 + +# Node X Y range color +# A 29 50 27 0x3af3bf81 +# B 69 93 25 0x1d79dfc0 +# C 64 63 37 0x3af3bf81 +# D 94 47 22 0x1d79dfc0 +# E 52 54 26 0x3af3bf81 diff --git a/dist/tools/zep_dispatch/launch.sh b/dist/tools/zep_dispatch/launch.sh new file mode 100755 index 000000000..d2452bd3f --- /dev/null +++ b/dist/tools/zep_dispatch/launch.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Launcher to Foren6 nodes + +ARRAY=( + "C3:20:D8:BF:73:D0:F6:69" + "A6:5A:49:45:82:62:C3:F2" + "23:19:A4:BD:C2:1E:55:77" + "2A:3A:AC:FA:9A:EF:48:5E" + "10:A0:9E:04:71:15:6D:18" +) + +ARR_LEN=${#ARRAY[@]} + + +: "${ZEP_DIR:=pwd}" +: "${DIR_APPS:=$(cd "$(dirname "$0")"/../../../examples || exit; pwd)}" +BOARD=native +gnome-terminal --window -- make run TOPOLOGY=file.topo + +gnome-terminal --title=DODAG --tab -- make -C "${DIR_APPS}"/rpl_dodag BOARD="${BOARD}" ZEP_MAC="${ARRAY[0]}" term + +for ((i=1; i<"${ARR_LEN}"; i++)) +do + gnome-terminal --title=DAG_"${i}" --tab -- make -C "${DIR_APPS}"/rpl_dag BOARD="${BOARD}" ZEP_MAC="${ARRAY[$i]}" term + sleep 1; +done diff --git a/dist/tools/zep_dispatch/main.c b/dist/tools/zep_dispatch/main.c new file mode 100644 index 000000000..2acea1a70 --- /dev/null +++ b/dist/tools/zep_dispatch/main.c @@ -0,0 +1,295 @@ +/* + * Copyright (C) 2020 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#ifndef ZEP_DISPATCH_PDU +#define ZEP_DISPATCH_PDU 256 +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "kernel_defines.h" +#include "topology.h" +#include "zep_parser.h" + +#define ETH_P_IEEE802154 0x00F6 + +typedef struct { + list_node_t node; + struct sockaddr_in6 addr; +} zep_client_t; + +typedef void (*dispatch_cb_t)(void *ctx, void *buffer, size_t len, + int sock, struct sockaddr_in6 *src_addr); + +/* all nodes are directly connected */ +static void _send_flat(void *ctx, void *buffer, size_t len, + int sock, struct sockaddr_in6 *src_addr) +{ + list_node_t *head = ctx; + char addr_str[INET6_ADDRSTRLEN]; + + /* send packet to all other clients */ + bool known_node = false; + list_node_t *prev = head; + + for (list_node_t *n = head->next; n; n = n->next) { + struct sockaddr_in6 *addr = &container_of(n, zep_client_t, node)->addr; + + /* don't echo packet back to sender */ + if (memcmp(src_addr, addr, sizeof(*addr)) == 0) { + known_node = true; + /* remove client if sending fails */ + } + else if (sendto(sock, buffer, len, 0, (struct sockaddr *)addr, sizeof(*addr)) < 0) { + inet_ntop(src_addr->sin6_family, &addr->sin6_addr, addr_str, INET6_ADDRSTRLEN); + printf("removing [%s]:%d\n", addr_str, ntohs(addr->sin6_port)); + prev->next = n->next; + free(n); + continue; + } + + prev = n; + } + + /* if the client new, add it to the broadcast list */ + if (!known_node) { + inet_ntop(src_addr->sin6_family, &src_addr->sin6_addr, addr_str, INET6_ADDRSTRLEN); + printf("adding [%s]:%d\n", addr_str, ntohs(src_addr->sin6_port)); + zep_client_t *client = malloc(sizeof(zep_client_t)); + memcpy(&client->addr, src_addr, sizeof(*src_addr)); + list_add(head, &client->node); + } +} + +/* nodes are connected as described by topology */ +static void _send_topology(void *ctx, void *buffer, size_t len, + int sock, struct sockaddr_in6 *src_addr) +{ + uint8_t mac_src[8]; + uint8_t mac_src_len; + + if (zep_parse_mac(buffer, len, mac_src, &mac_src_len)) { + /* a sniffer node has no MAC address and will receive every packet */ + if (mac_src_len == 0) { + topology_set_sniffer(ctx, src_addr); + } else { + topology_add(ctx, mac_src, mac_src_len, src_addr); + } + } + topology_send(ctx, sock, src_addr, buffer, len); +} + +static void dispatch_loop(int sock, int tap, dispatch_cb_t dispatch, void *ctx) +{ + puts("entering loop…"); + while (1) { + uint8_t buffer[ZEP_DISPATCH_PDU]; + struct sockaddr_in6 src_addr; + socklen_t addr_len = sizeof(src_addr); + + /* receive incoming packet */ + ssize_t bytes_in = recvfrom(sock, buffer, sizeof(buffer), 0, + (struct sockaddr *)&src_addr, &addr_len); + + if (bytes_in <= 0) { + continue; + } + + /* send packet to virtual 802.15.4 interface */ + if (tap) { + size_t len = bytes_in; + const void *payload = zep_get_payload(buffer, &len); + if (payload) { + if (write(tap, payload, len) < 0) { + puts("Can't write to virtual 802.15.4 device"); + close(tap); + tap = 0; + } + } + } + + /* send packet to the topology */ + dispatch(ctx, buffer, bytes_in, sock, &src_addr); + } +} + +topology_t topology; +static const char *graphviz_file = "example.gv"; +static void _info_handler(int signal) +{ + if (signal != SIGUSR1) { + return; + } + + if (topology_print(graphviz_file, &topology)) { + fprintf(stderr, "can't open %s\n", graphviz_file); + } + else { + printf("graph written to %s\n", graphviz_file); + } +} + +/* open mac802154_hwsim device to send frames to it */ +static int _open_mac802154_hwsim(const char *iface) +{ + int res; + int fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_IEEE802154)); + + /* get interface index for interface name */ + struct ifreq ifr; + strncpy(ifr.ifr_name, iface, IFNAMSIZ); + + if ((res = ioctl(fd, SIOCGIFINDEX, &ifr)) < 0) { + goto error; + } + + /* bind socket to the device */ + struct sockaddr_ll sll = { + .sll_family = AF_PACKET, + .sll_ifindex = ifr.ifr_ifindex, + .sll_protocol = htons(ETH_P_IEEE802154), + }; + + if ((res = bind(fd, (struct sockaddr *)&sll, sizeof(sll))) < 0) { + goto error; + } + + return fd; + +error: + close(fd); + fprintf(stderr, "open mac802154_hwsim: %s\n", strerror(res)); + return res; +} + +static void _print_help(const char *progname) +{ + fprintf(stderr, "usage: %s [-t topology] [-s seed] " + "[-g graphviz_out] [-w interface]
\n", + progname); + + fprintf(stderr, "\npositional arguments:\n"); + fprintf(stderr, "\taddress\t\tlocal address to bind to\n"); + fprintf(stderr, "\tport\t\tlocal port to bind to\n"); + + fprintf(stderr, "\noptional arguments:\n"); + fprintf(stderr, "\t-t \tLoad toplogy from file\n"); + fprintf(stderr, "\t-s \tRandom seed used to simulate packet loss\n"); + fprintf(stderr, "\t-g \tFile to dump topology as Graphviz visualisation on SIGUSR1\n"); + fprintf(stderr, "\t-w \tSend frames to virtual 802.15.4 " + "interface (mac802154_hwsim)\n"); +} + +int main(int argc, char **argv) +{ + int c, tap_fd = 0; + unsigned int seed = time(NULL); + const char *topo_file = NULL; + const char *progname = argv[0]; + + const struct addrinfo hint = { + .ai_family = AF_UNSPEC, + .ai_socktype = SOCK_DGRAM, + .ai_protocol = IPPROTO_UDP, + .ai_flags = AI_NUMERICHOST, + }; + + while ((c = getopt(argc, argv, "t:s:g:w:")) != -1) { + switch (c) { + case 't': + topo_file = optarg; + break; + case 's': + seed = atoi(optarg); + break; + case 'g': + graphviz_file = optarg; + break; + case 'w': + tap_fd = _open_mac802154_hwsim(optarg); + if (tap_fd < 0) { + return tap_fd; + } + break; + default: + _print_help(progname); + exit(1); + } + } + + argc -= optind; + argv += optind; + + if (argc != 2) { + _print_help(progname); + exit(1); + } + + srand(seed); + + if (topo_file) { + if (topology_parse(topo_file, &topology)) { + fprintf(stderr, "can't open '%s'\n", topo_file); + return -1; + } + topology.flat = false; + } + else { + topology.flat = true; + } + + if (graphviz_file) { + signal(SIGUSR1, _info_handler); + } + + struct addrinfo *server_addr; + int res = getaddrinfo(argv[0], argv[1], + &hint, &server_addr); + + if (res != 0) { + perror("getaddrinfo()"); + exit(1); + } + + int sock = socket(server_addr->ai_family, server_addr->ai_socktype, + server_addr->ai_protocol); + + if (sock < 0) { + perror("socket() failed"); + exit(1); + } + + if (bind(sock, server_addr->ai_addr, server_addr->ai_addrlen) < 0) { + perror("bind() failed"); + exit(1); + } + + freeaddrinfo(server_addr); + + if (topology.flat) { + dispatch_loop(sock, tap_fd, _send_flat, &topology.nodes); + } + else { + dispatch_loop(sock, tap_fd, _send_topology, &topology); + } + + close(sock); + + return 0; +} diff --git a/dist/tools/zep_dispatch/network.topo b/dist/tools/zep_dispatch/network.topo new file mode 100644 index 000000000..29cd5ee70 --- /dev/null +++ b/dist/tools/zep_dispatch/network.topo @@ -0,0 +1,29 @@ +# seed = 1663037305 +# Connections +C H 0.00 0.20 +C I 0.00 0.40 +A D 0.15 0.00 +D H 0.00 0.33 +A E 0.44 0.32 +E H 0.45 0.58 +E I 0.36 0.36 +E J 0.14 0.00 +F G 0.40 0.00 +A H 0.41 0.46 +H I 0.41 0.23 +H J 0.76 0.39 +I J 0.05 0.00 +A J 0.23 0.00 + + +# Node X Y range color +# A 9 17 40 0x5bf3b45f +# B 35 77 26 0x0 +# C 65 21 19 0x2df9da2f +# D 17 50 21 0x2df9da2f +# E 29 7 33 0x5bf3b45f +# F 90 97 44 0x0 +# G 70 80 17 0x0 +# H 31 25 43 0x5bf3b45f +# I 50 8 33 0x5bf3b45f +# J 34 35 17 0x5bf3b45f diff --git a/dist/tools/zep_dispatch/start_network.sh b/dist/tools/zep_dispatch/start_network.sh new file mode 100755 index 000000000..8e7f4ee33 --- /dev/null +++ b/dist/tools/zep_dispatch/start_network.sh @@ -0,0 +1,118 @@ +#!/usr/bin/env bash + +ZEP_DISPATCH_DIR="$(cd "$(dirname "$0")" && pwd -P)" +UHCPD="$(cd "${ZEP_DISPATCH_DIR}/../uhcpd/bin" && pwd -P)/uhcpd" +DHCPD="$(cd "${ZEP_DISPATCH_DIR}/../dhcpv6-pd_ia/" && pwd -P)/dhcpv6-pd_ia.py" +RADVD="$(cd "${ZEP_DISPATCH_DIR}/../radvd/" && pwd -P)/radvd.sh" +ZEP_DISPATCH="${ZEP_DISPATCH_DIR}/bin/zep_dispatch" + +TAP_GLB="fdea:dbee:f::1/64" + +NOSUDO="sudo -u ${SUDO_USER}" + +create_tap() { + ip tuntap add "${TAP}" mode tap user "${SUDO_USER}" + sysctl -w net.ipv6.conf."${TAP}".forwarding=1 + sysctl -w net.ipv6.conf."${TAP}".accept_ra=0 + ip link set "${TAP}" up + ip a a fe80::1/64 dev "${TAP}" + ip a a ${TAP_GLB} dev "${TAP}" +} + +remove_tap() { + ip tuntap del "${TAP}" mode tap +} + +cleanup() { + echo "Cleaning up..." + remove_tap + if [ -n "${UHCPD_PID}" ]; then + kill "${UHCPD_PID}" + fi + if [ -n "${ZEP_DISPATCH_PID}" ]; then + kill "${ZEP_DISPATCH_PID}" + fi + if [ -n "${DHCPD_PIDFILE}" ]; then + kill "$(cat "${DHCPD_PIDFILE}")" + rm "${DHCPD_PIDFILE}" + fi + trap "" INT QUIT TERM EXIT +} + +start_uhcpd() { + ip route add "${PREFIX}" via fe80::2 dev "${TAP}" + ${UHCPD} "${TAP}" "${PREFIX}" > /dev/null & + UHCPD_PID=$! +} + +start_dhcpd() { + ip route add "${PREFIX}" via fe80::2 dev "${TAP}" + DHCPD_PIDFILE=$(mktemp) + ${DHCPD} -d -p "${DHCPD_PIDFILE}" "${TAP}" "${PREFIX}" 2> /dev/null +} + +start_radvd() { + ADDR=$(echo "${PREFIX}" | sed -e 's/::\//::1\//') + ip a a "${ADDR}" dev "${TAP}" + sysctl net.ipv6.conf."${TAP}".accept_ra=2 + sysctl net.ipv6.conf."${TAP}".accept_ra_rt_info_max_plen=64 + ${RADVD} -c "${TAP}" "${PREFIX}" +} + +start_zep_dispatch() { + ${ZEP_DISPATCH} :: "${ZEP_PORT_BASE}" > /dev/null & + ZEP_DISPATCH_PID=$! +} + +if [ "$1" = "-d" ] || [ "$1" = "--use-dhcpv6" ]; then + USE_DHCPV6=1 + shift 1 +else + USE_DHCPV6=0 +fi + +if [ "$1" = "-r" ] || [ "$1" = "--use-radvd" ]; then + USE_RADVD=1 + shift 1 +else + USE_RADVD=0 +fi + +if [ "$1" = "-z" ] || [ "$1" = "--use-zep-dispatch" ]; then + USE_ZEP_DISPATCH=1 + ZEP_PORT_BASE=$2 + shift 2 +else + USE_ZEP_DISPATCH=0 +fi + +ELFFILE=$1 +PREFIX=$2 +shift 2 + +# tap will be the last argument +for TAP in "$@"; do :; done + +[[ -z "${ELFFILE}" || -z "${PREFIX}" || -z "${TAP}" ]] && { + echo "usage: $0 [-d|--use-dhcp] [-r|--use-radvd] [-z|--use-zep ] " \ + " [elf args]" + exit 1 +} + +trap "cleanup" INT QUIT TERM EXIT + +create_tap + +if [ ${USE_ZEP_DISPATCH} -eq 1 ]; then + start_zep_dispatch +fi + +if [ ${USE_DHCPV6} -eq 1 ]; then + start_dhcpd +elif [ ${USE_RADVD} -eq 1 ]; then + start_radvd +else + start_uhcpd +fi + +${NOSUDO} "${ELFFILE}" "${TAP}" "$@" diff --git a/dist/tools/zep_dispatch/topogen.c b/dist/tools/zep_dispatch/topogen.c new file mode 100644 index 000000000..73a4ebdf6 --- /dev/null +++ b/dist/tools/zep_dispatch/topogen.c @@ -0,0 +1,261 @@ +/* + * Copyright (C) 2021 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#include +#include +#include +#include +#include +#include +#include + +#define MAC_BYTE_SIZE 8 +typedef struct { + union { + uint8_t _u8[8]; + uint16_t _u16[4]; + uint32_t _u32[2]; + } mac; +} mac_t; + +struct node { + char name[8]; + mac_t mac; + int x; + int y; + unsigned r; + bool rx_from_root; + bool tx_to_root; +}; + +struct world { + unsigned w; + unsigned h; + unsigned num_nodes; + struct node *nodes; +}; + +static unsigned random_range(unsigned lower, unsigned upper) { + unsigned range = upper - lower + 1; + return lower + rand() % range; +} + +static struct node *node_generate(struct node *n, const struct world *w, unsigned range) { + n->x = random_range(0, w->w); + n->y = random_range(0, w->h); + n->r = range; + return n; +} + +static double node_distance(const struct node *a, const struct node *b) { + return sqrt(pow(a->x - b->x, 2) + pow(a->y - b->y, 2)); +} + +static double node_distance_weight(const struct node *a, const struct node *b) { + double w = 1 - node_distance(a, b) / a->r; + + if (w < 0) { + return 0; + } + return w; +} + +static void node_name(struct node *n, unsigned idx) { + char *s = n->name; + const char *end = s + sizeof(n->name) - 1; + + do { + uint8_t rem = idx % 26; + *s++ = 'A' + rem; + idx -= rem; + } while (idx && s != end); + *s = 0; +} + +static void world_gen(struct world *w, unsigned num_nodes, unsigned width, unsigned height, + unsigned range, unsigned var) { + w->w = width; + w->h = height; + w->num_nodes = num_nodes; + w->nodes = calloc(num_nodes, sizeof(*w->nodes)); + + for (unsigned i = 0; i < num_nodes; ++i) { + node_generate(&w->nodes[i], w, random_range(range - var, range + var)); + node_name(&w->nodes[i], i); + } +} + +static unsigned _color(const struct node *n, unsigned base) { + if (n->rx_from_root && n->tx_to_root) { + return base; + } else if (n->rx_from_root) { + return base / 2; + } else if (n->tx_to_root) { + return base / 3; + } + + return 0; +} + +static void _print_nodes(struct node *nodes, unsigned num, unsigned color) { + for (unsigned i = 0; i < num; ++i) { + printf("# %s\t%d\t%d\t%u\t0x%x\n", nodes[i].name, nodes[i].x, nodes[i].y, nodes[i].r, + _color(&nodes[i], color)); + } +} + +/* To visualize the network we color the nodes based on whether they have + * a (bi-directional) connection to the root node via some other node(s). + */ +static void _calc_connections(struct node *nodes, unsigned num) { + bool changes = true; + nodes->rx_from_root = true; + nodes->tx_to_root = true; + + /* super basic algorithm - just loop unti there are no more changes + * in the node's connection states. */ + while (changes) { + changes = false; + + for (unsigned i = 0; i < num; ++i) { + const struct node *n = &nodes[i]; + + for (unsigned j = 0; j < num; ++j) { + struct node *m = &nodes[j]; + + /* node is already fully connected */ + if (m->rx_from_root && m->tx_to_root) { + continue; + } + + /* m can receive from n and n can receive from root */ + if (node_distance_weight(n, m) > 0) { + if (!m->rx_from_root && n->rx_from_root) { + m->rx_from_root = true; + changes = true; + } + } + + /* m can send to n and n can send to root */ + if (node_distance_weight(m, n) > 0) { + if (!m->tx_to_root && n->tx_to_root) { + m->tx_to_root = true; + changes = true; + } + } + } + } + } +} + +static void _print_distance(struct node *nodes, unsigned num, bool recursive) { + struct node *start = nodes; + + for (unsigned i = 1; i < num; ++i) { + struct node *n = &nodes[i]; + + double to_node = node_distance_weight(start, n); + double from_node = node_distance_weight(n, start); + + if (to_node > 0 || from_node > 0) { + printf("%s\t%s\t%.2f\t%.2f\n", start->name, n->name, to_node, from_node); + } + + if (recursive) { + _print_distance(n, num - i, false); + } + } +} + +static void get_random_mac(mac_t *mac) { + for (uint8_t i = 0; i < MAC_BYTE_SIZE; i++) { + mac->mac._u8[i] = (uint8_t)(rand() % 255); + } +} + +static void printf_random_macs(struct node *nodes, unsigned int numof_nodes) { + for (unsigned i = 0; i < numof_nodes; ++i) { + struct node *n = &nodes[i]; + get_random_mac(&n->mac); + printf("%s := ", n->name); + for (uint8_t j = 0; j < MAC_BYTE_SIZE; j++) { + printf("%02X", n->mac.mac._u8[j]); + if (j < MAC_BYTE_SIZE - 1) { + printf(":"); + } + } + printf("\n"); + } +} + +static void _print_help(const char *name) { + puts("Generate a number of nodes that are randomly placed in a rectangular area"); + printf("usage: %s [-s ]" + " [-w ]" + " [-h ]" + " [-r ]" + " [-v ]" + " [-n ]" + "\n", + name); +} + +int main(int argc, char **argv) { + const char *progname = argv[0]; + + unsigned width = 100; + unsigned height = 100; + unsigned seed = time(NULL); + unsigned range = 25; + unsigned var = 0; + unsigned num = 10; + char c; + + while ((c = getopt(argc, argv, "s:w:h:r:v:n:")) != -1) { + switch (c) { + case 's': + seed = atoi(optarg); + break; + case 'w': + width = atoi(optarg); + break; + case 'h': + height = atoi(optarg); + break; + case 'r': + range = atoi(optarg); + break; + case 'v': + var = atoi(optarg); + break; + case 'n': + num = atoi(optarg); + break; + default: + _print_help(progname); + exit(1); + } + } + + srand(seed); + + struct world w; + world_gen(&w, num, width, height, range, var); + puts("# MAC Addresses"); + printf_random_macs(w.nodes, w.num_nodes); + puts(""); + printf("# seed = %u\n", seed); + puts("# Connections"); + _print_distance(w.nodes, w.num_nodes, true); + puts(""); + puts(""); + puts("# Node\tX\tY\trange\tcolor"); + _calc_connections(w.nodes, w.num_nodes); + _print_nodes(w.nodes, w.num_nodes, rand()); + + return 0; +} diff --git a/dist/tools/zep_dispatch/topogen.sh b/dist/tools/zep_dispatch/topogen.sh new file mode 100755 index 000000000..c7adfefcb --- /dev/null +++ b/dist/tools/zep_dispatch/topogen.sh @@ -0,0 +1,47 @@ +#!/bin/bash + +if [ -z "$1" ]; then + FILE=out.top +else + FILE="$1" +fi + +if tput colors &> /dev/null && [ "$(tput colors)" -ge 8 ]; then + COK="\e[1;32m" + CWARN="\e[1;33m" + CRESET="\e[0m" +else + COK= + CWARN= + CRESET= +fi + +WIDTH=100 # X dimension of the simulated world +HEIGHT=100 # Y dimension of the simulated world +RANGE=30 # base node radio range +VARIANCE=15 # random offset to radio range + +if [ -z "$2" ]; then + NUM=10 # number of nodes +else + NUM=$2 +fi +if [ ! -f "$FILE" ]; then + echo "writing to $FILE" + ./bin/topogen -w $WIDTH -h $HEIGHT -r $RANGE -v $VARIANCE -n "$NUM" > "$FILE" +fi + +if ! command -v gnuplot > /dev/null; then + printf "${CWARN}%s${CRESET}\n" "gnuplot not installed" + exit 0 +fi + +printf "${COK}%s${CRESET}\n" "rendering to ${FILE}.svg" +gnuplot -e "set xrange [0:$WIDTH]" \ + -e "set yrange [0:$HEIGHT]" \ + -e "set terminal svg size $(( WIDTH * 8 )), $(( HEIGHT * 8 ))" \ + -e "set output '$FILE.svg'" \ + -e "set datafile commentschars '!'" \ + -e "set style fill transparent solid 0.25" \ + -e "plot '$FILE' index 1 using 3:4:(\$5*2):(\$5*2):6 with ellipses lc rgb var notitle, \ + '$FILE' index 1 using 3:4:2 with labels point pt 2 offset char 0.5,0.5 notitle" diff --git a/dist/tools/zep_dispatch/topology.c b/dist/tools/zep_dispatch/topology.c new file mode 100644 index 000000000..c12824478 --- /dev/null +++ b/dist/tools/zep_dispatch/topology.c @@ -0,0 +1,322 @@ +/* + * Copyright (C) 2021 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kernel_defines.h" +#include "topology.h" +#include "zep_parser.h" + +#define NODE_NAME_MAX_LEN 32 +#define HW_ADDR_MAX_LEN 8 + +struct node { + list_node_t next; + char name[NODE_NAME_MAX_LEN]; + uint8_t mac[HW_ADDR_MAX_LEN]; + struct sockaddr_in6 addr; + uint8_t mac_len; +}; + +struct edge { + list_node_t next; + struct node *a; + struct node *b; + float weight_a_b; + float weight_b_a; +}; + +size_t l2util_addr_from_str(const char *str, uint8_t *out); + +static char *_fmt_addr(char *out, size_t out_len, const uint8_t *addr, uint8_t addr_len) +{ + char *start = out; + + if (out_len < 3 * addr_len) { + return NULL; + } + + while (addr_len--) { + out += sprintf(out, "%02X", *addr++); + *(out++) = addr_len ? ':' : '\0'; + } + + return start; +} + +static struct node *_find_node_by_name(const list_node_t *nodes, const char *name) +{ + for (list_node_t *node = nodes->next; node; node = node->next) { + struct node *super = container_of(node, struct node, next); + if (strncmp(super->name, name, sizeof(super->name)) == 0) { + return super; + } + } + + return NULL; +} + +static struct node *_find_or_create_node(list_node_t *nodes, const char *name) +{ + struct node *node = _find_node_by_name(nodes, name); + + if (node == NULL) { + node = calloc(1, sizeof(*node)); + strncpy(node->name, name, sizeof(node->name) - 1); + list_add(nodes, &node->next); + } + + return node; +} + +static bool _parse_line(char *line, list_node_t *nodes, list_node_t *edges) +{ + struct edge *e; + + if (*line == '#') { + return true; + } + + char *a = strtok(line, "\t "); + char *b = strtok(NULL, "\n\t "); + char *e_ab = strtok(NULL, "\n\t "); + char *e_ba = strtok(NULL, "\n\t "); + + if (a == NULL || b == NULL) { + return false; + } + + /* add node with a defined MAC address */ + if (strcmp(b, ":=") == 0) { + struct node *n = _find_or_create_node(nodes, a); + if (n == NULL) { + return false; + } + + n->mac_len = l2util_addr_from_str(e_ab, n->mac); + return true; + } + + if (e_ab == NULL) { + e_ab = "1"; + } + + if (e_ba == NULL) { + e_ba = e_ab; + } + + e = malloc(sizeof(*e)); + + e->a = _find_or_create_node(nodes, a); + e->b = _find_or_create_node(nodes, b); + e->weight_a_b = atof(e_ab); + e->weight_b_a = atof(e_ba); + + list_add(edges, &e->next); + + return true; +} + +int topology_print(const char *file, const topology_t *t) +{ + FILE *out; + char addr_str[3 * HW_ADDR_MAX_LEN]; + + if (t->flat) { + // TODO + return 0; + } + + if (strcmp(file, "-") == 0) { + out = stdout; + } + else { + out = fopen(file, "w"); + } + + if (out == NULL) { + return -1; + } + + fprintf(out, "digraph G {\n"); + + for (list_node_t *node = t->nodes.next; node; node = node->next) { + struct node *super = container_of(node, struct node, next); + fprintf(out, "\t%s [ label = \"%s\\n[%s]\" ]\n", + super->name, super->name, + super->mac_len ? _fmt_addr(addr_str, sizeof(addr_str), super->mac, super->mac_len) + : "disconnected"); + } + + fprintf(out, "\n"); + + for (list_node_t *edge = t->edges.next; edge; edge = edge->next) { + struct edge *super = container_of(edge, struct edge, next); + if (super->weight_a_b) { + fprintf(out, "\t%s -> %s [ label = \"%.2f\" ]\n", + super->a->name, super->b->name, super->weight_a_b); + } + if (super->weight_b_a) { + fprintf(out, "\t%s -> %s [ label = \"%.2f\" ]\n", + super->b->name, super->a->name, super->weight_b_a); + } + } + + fprintf(out, "}\n"); + + if (out != stdout) { + fclose(out); + } + + return 0; +} + +int topology_parse(const char *file, topology_t *out) +{ + FILE *in; + + memset(out, 0, sizeof(*out)); + + if (strcmp(file, "-") == 0) { + in = stdin; + } + else { + in = fopen(file, "r"); + } + + if (in == NULL) { + return -1; + } + + char *line = NULL; + size_t line_len = 0; + + while (getline(&line, &line_len, in) > 0) { + _parse_line(line, &out->nodes, &out->edges); + } + + if (line) { + free(line); + } + + return 0; +} + +void topology_send(const topology_t *t, int sock, + const struct sockaddr_in6 *src_addr, + void *buffer, size_t len) +{ + if (t->has_sniffer) { + sendto(sock, buffer, len, 0, + (struct sockaddr *)&t->sniffer_addr, sizeof(t->sniffer_addr)); + } + + for (list_node_t *edge = t->edges.next; edge; edge = edge->next) { + struct edge *super = container_of(edge, struct edge, next); + + if (!super->a->mac_len || !super->b->mac_len) { + continue; + } + + if (memcmp(&super->a->addr, src_addr, sizeof(*src_addr)) == 0) { + /* packet loss */ + if (random() > super->weight_a_b * RAND_MAX) { + return; + } + zep_set_lqi(buffer, super->weight_a_b * 0xFF); + sendto(sock, buffer, len, 0, + (struct sockaddr *)&super->b->addr, + sizeof(super->b->addr)); + } + else if (memcmp(&super->b->addr, src_addr, sizeof(*src_addr)) == 0) { + /* packet loss */ + if (random() > super->weight_b_a * RAND_MAX) { + return; + } + zep_set_lqi(buffer, super->weight_b_a * 0xFF); + sendto(sock, buffer, len, 0, + (struct sockaddr *)&super->a->addr, + sizeof(super->a->addr)); + } + } +} + +bool topology_add(topology_t *t, const uint8_t *mac, uint8_t mac_len, + struct sockaddr_in6 *addr) +{ + struct node *empty = NULL; + char addr_str[3 * HW_ADDR_MAX_LEN]; + + if (mac_len > HW_ADDR_MAX_LEN) { + fprintf(stderr, "discarding frame with %u byte address\n", mac_len); + return false; + } + + for (list_node_t *node = t->nodes.next; node; node = node->next) { + struct node *super = container_of(node, struct node, next); + + /* store free node */ + if (!super->mac_len) { + empty = super; + continue; + } + + if (mac_len != super->mac_len) { + continue; + } + + /* node is already in the list - either it is connected or MAC was pinned */ + if (memcmp(super->mac, mac, mac_len) == 0) { + if (super->addr.sin6_port == addr->sin6_port) { + /* abort if node is already connected */ + return true; + } else { + /* use pre-allocated node */ + empty = super; + break; + } + } + } + + /* topology full - can't add node */ + if (empty == NULL) { + fprintf(stderr, "can't add %s - topology full\n", + _fmt_addr(addr_str, sizeof(addr_str), mac, mac_len)); + return false; + } + + printf("adding node %s\n", _fmt_addr(addr_str, sizeof(addr_str), mac, mac_len)); + + /* add new node to empty spot */ + memcpy(empty->mac, mac, sizeof(empty->mac)); + memcpy(&empty->addr, addr, sizeof(empty->addr)); + empty->mac_len = mac_len; + + return true; +} + +void topology_set_sniffer(topology_t *t, struct sockaddr_in6 *addr) +{ + char addr_str[INET6_ADDRSTRLEN]; + getnameinfo((struct sockaddr*)addr, sizeof(*addr), + addr_str, sizeof(addr_str), 0, 0, NI_NUMERICHOST); + if (t->has_sniffer) { + printf("replace sniffer with %s\n", addr_str); + } else { + printf("adding sniffer %s\n", addr_str); + } + + memcpy(&t->sniffer_addr, addr, sizeof(t->sniffer_addr)); + t->has_sniffer = true; +} diff --git a/dist/tools/zep_dispatch/topology.h b/dist/tools/zep_dispatch/topology.h new file mode 100644 index 000000000..9b3d61d98 --- /dev/null +++ b/dist/tools/zep_dispatch/topology.h @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2021 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#ifndef TOPOLOGY_H +#define TOPOLOGY_H + +#include "list.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Struct describing a graph of nodes and their connections + */ +typedef struct { + list_node_t nodes; /**< list of nodes */ + list_node_t edges; /**< list of connections between nodes. Unused if topology is flat */ + struct sockaddr_in6 sniffer_addr; /**< address of sniffer node. Unused if topology is flat */ + bool has_sniffer; /**< true if a sniffer node is connected. Unused if topology is flat */ + bool flat; /**< flat topology, all nodes are connected to each other */ +} topology_t; + +/** + * @brief Parse a file with topology information + * + * @param[in] file Filename to open & parse + * May be "-" to use stdin + * @param[out] out Topology from file + * + * @return 0 on success, error otherwise + */ +int topology_parse(const char *file, topology_t *out); + +/** + * @brief Print topology as Graphviz diagram + * + * @param[in] file_out Filename to write to + * May be "-" to use stdout + * @param[in] t The topology to render + * + * @return 0 on success, error otherwise + */ +int topology_print(const char *file_out, const topology_t *t); + +/** + * @brief Populate a spot in the topology with a connected node + * + * @param[in, out] t topology to use + * @param[in] mac ZEP l2 address of the new node + * @param[in] mac_len ZEP l2 address length + * @param[in] addr real address of the virtual node + * + * @return true if the node could be added to the topology + */ +bool topology_add(topology_t *t, const uint8_t *mac, uint8_t mac_len, + struct sockaddr_in6 *addr); + +/** + * @brief Add a sniffer to the topology + * A sniffer node will receive every packet but won't be able to + * send packets on it's own. + * + * @param[in, out] t topology to configure + * @param[in] addr real address of the sniffer + */ +void topology_set_sniffer(topology_t *t, struct sockaddr_in6 *addr); + +/** + * @brief Send a buffer to all nodes connected to a source node + * + * @param[in] t topology to use + * @param[in] sock socket to use for sending + * @param[in] src_addr source node address + * @param[in] buffer ZEP frame to send + * @param[in] len ZEP frame length + */ +void topology_send(const topology_t *t, int sock, + const struct sockaddr_in6 *src_addr, + void *buffer, size_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* TOPOLOGY_H */ diff --git a/dist/tools/zep_dispatch/zep_parser.c b/dist/tools/zep_dispatch/zep_parser.c new file mode 100644 index 000000000..851a1e76b --- /dev/null +++ b/dist/tools/zep_dispatch/zep_parser.c @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2021 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#include + +#include "net/ieee802154.h" +#include "net/zep.h" +#include "zep_parser.h" + +#define SOCKET_ZEP_V2_TYPE_HELLO (255) + +const void *zep_get_payload(const void *buffer, size_t *len) { + const void *payload; + const zep_v2_data_hdr_t *zep = buffer; + + if (*len == 0) { + return NULL; + } + + if ((zep->hdr.preamble[0] != 'E') || (zep->hdr.preamble[1] != 'X')) { + return NULL; + } + + if (zep->hdr.version != 2) { + return NULL; + } + + switch (zep->type) { + case ZEP_V2_TYPE_DATA: + payload = (zep_v2_data_hdr_t *)zep + 1; + break; + case ZEP_V2_TYPE_ACK: + payload = (zep_v2_ack_hdr_t *)zep + 1; + break; + default: + return NULL; + } + + *len = zep->length - IEEE802154_FCS_LEN; + + return payload; +} + +bool zep_parse_mac(const void *buffer, size_t len, void *out, uint8_t *out_len) { + const void *payload; + const zep_v2_data_hdr_t *zep = buffer; + + if (len == 0) { + return false; + } + + if ((zep->hdr.preamble[0] != 'E') || (zep->hdr.preamble[1] != 'X')) { + return false; + } + + if (zep->hdr.version != 2) { + return false; + } + + switch (zep->type) { + case ZEP_V2_TYPE_DATA: + payload = (zep_v2_data_hdr_t *)zep + 1; + break; + case ZEP_V2_TYPE_ACK: + payload = (zep_v2_ack_hdr_t *)zep + 1; + break; + case SOCKET_ZEP_V2_TYPE_HELLO: + /* HELLO packet only contains HW addr as payload */ + payload = (zep_v2_data_hdr_t *)zep + 1; + *out_len = zep->length; + memcpy(out, payload, zep->length); + *out_len = zep->length; + return true; + default: + return false; + } + + le_uint16_t dst_pan; + int res = ieee802154_get_src(payload, out, &dst_pan); + + if (res <= 0) { + return false; + } + + *out_len = res; + + /* check that we are not out of bounds */ + return (uintptr_t)payload + *out_len < (uintptr_t)buffer + len; +} + +void zep_set_lqi(void *buffer, uint8_t lqi) { + zep_v2_data_hdr_t *zep = buffer; + + if (zep->type != ZEP_V2_TYPE_DATA) { + return; + } + + zep->lqi_val = lqi; +} diff --git a/dist/tools/zep_dispatch/zep_parser.h b/dist/tools/zep_dispatch/zep_parser.h new file mode 100644 index 000000000..fa2f7c8dc --- /dev/null +++ b/dist/tools/zep_dispatch/zep_parser.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2021 Benjamin Valentin + * + * This file is subject to the terms and conditions of the GNU General Public + * License v2. See the file LICENSE for more details. + */ + +#ifndef ZEP_PARSER_H +#define ZEP_PARSER_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Get the payload of a ZEP frame + * + * @param[in] buffer ZEP frame + * @param[in, out] len size of buffer, will contain size of payload + * + * @return pointer to payload on success + * NULL if no payload was found + */ +const void *zep_get_payload(const void *buffer, size_t *len); + +/** + * @brief Parse l2 source address of a ZEP frame + * + * @param[in] buffer ZEP frame + * @param[in] len size of buffer + * @param[out] out destination for l2 address + * @param[out] out_len l2 address length + * + * @return true if l2 address was found + */ +bool zep_parse_mac(const void *buffer, size_t len, void *out, uint8_t *out_len); + +/** + * @brief Set link quality information in ZEP frame + * + * @param[out] buffer ZEP frame to modify + * @param[in] lqi link quality to write to ZEP header + */ +void zep_set_lqi(void *buffer, uint8_t lqi); + +#ifdef __cplusplus +} +#endif + +#endif /* ZEP_PARSER_H */