diff --git a/dist/tools/zep_dispatch/.gitignore b/dist/tools/zep_dispatch/.gitignore new file mode 100644 index 0000000000..0a293e49d7 --- /dev/null +++ b/dist/tools/zep_dispatch/.gitignore @@ -0,0 +1,2 @@ +*.gv +*.pdf diff --git a/dist/tools/zep_dispatch/Makefile b/dist/tools/zep_dispatch/Makefile index c6f881c198..9e4ffed41f 100644 --- a/dist/tools/zep_dispatch/Makefile +++ b/dist/tools/zep_dispatch/Makefile @@ -1,4 +1,6 @@ -CFLAGS?=-g -O3 -Wall -Wextra +CFLAGS ?= -g -O3 -Wall -Wextra +CFLAGS += $(RIOT_INCLUDE) +CFLAGS += -DNDEBUG # avoid assert re-definition BINARY := bin/zep_dispatch all: bin $(BINARY) @@ -6,11 +8,35 @@ all: bin $(BINARY) bin: mkdir bin -RIOTBASE:=../../.. -RIOT_INCLUDE=$(RIOTBASE)/core/include -SRCS:=$(wildcard *.c) -$(BINARY): $(SRCS) - $(CC) $(CFLAGS) $(CFLAGS_EXTRA) -I$(RIOT_INCLUDE) $(SRCS) -o $@ +RIOTBASE := ../../.. +ZEP_PORT_BASE ?= 17754 +TOPOLOGY ?= example.topo +GV_OUT ?= example.gv + +RIOT_INCLUDE += -I$(RIOTBASE)/core/include +RIOT_INCLUDE += -I$(RIOTBASE)/cpu/native/include +RIOT_INCLUDE += -I$(RIOTBASE)/drivers/include +RIOT_INCLUDE += -I$(RIOTBASE)/sys/include + +SRCS := $(wildcard *.c) +SRCS += $(RIOTBASE)/sys/net/link_layer/ieee802154/ieee802154.c + +$(BINARY): $(SRCS) + $(CC) $(CFLAGS) $(CFLAGS_EXTRA) $(SRCS) -o $@ + +.PHONY: clean run graph help clean: rm -f $(BINARY) + +run: $(BINARY) + $(BINARY) -t $(TOPOLOGY) -g $(GV_OUT) ::1 $(ZEP_PORT_BASE) + +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" diff --git a/dist/tools/zep_dispatch/README.md b/dist/tools/zep_dispatch/README.md new file mode 100644 index 0000000000..f71d6b1158 --- /dev/null +++ b/dist/tools/zep_dispatch/README.md @@ -0,0 +1,46 @@ +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. diff --git a/dist/tools/zep_dispatch/example.topo b/dist/tools/zep_dispatch/example.topo new file mode 100644 index 0000000000..7e46fc4980 --- /dev/null +++ b/dist/tools/zep_dispatch/example.topo @@ -0,0 +1,7 @@ +# 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/main.c b/dist/tools/zep_dispatch/main.c index 00432556a0..0d689c2631 100644 --- a/dist/tools/zep_dispatch/main.c +++ b/dist/tools/zep_dispatch/main.c @@ -15,23 +15,77 @@ #include #include #include +#include +#include #include -#include "list.h" #include "kernel_defines.h" +#include "topology.h" +#include "zep_parser.h" typedef struct { list_node_t node; struct sockaddr_in6 addr; } zep_client_t; -static void dispatch_loop(int sock) -{ - list_node_t head = { .next = NULL }; +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(AF_INET6, &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(AF_INET6, &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) && + topology_add(ctx, mac_src, mac_src_len, src_addr)) { + topology_send(ctx, sock, mac_src, mac_src_len, buffer, len); + } +} + +static void dispatch_loop(int sock, dispatch_cb_t dispatch, void *ctx) +{ puts("entering loop…"); while (1) { - char addr_str[INET6_ADDRSTRLEN]; uint8_t buffer[ZEP_DISPATCH_PDU]; struct sockaddr_in6 src_addr; socklen_t addr_len = sizeof(src_addr); @@ -44,46 +98,88 @@ static void dispatch_loop(int sock) continue; } - /* 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, addr_len) == 0) { - known_node = true; - /* remove client if sending fails */ - } else if (sendto(sock, buffer, bytes_in, 0, (struct sockaddr*)addr, addr_len) < 0) { - inet_ntop(AF_INET6, &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(AF_INET6, &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, addr_len); - list_add(&head, &client->node); - } + dispatch(ctx, buffer, bytes_in, sock, &src_addr); } } +static topology_t topology; +static const char *graphviz_file; +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); + } +} + +static void _print_help(const char *progname) +{ + fprintf(stderr, "usage: %s [-t topology] [-s seed] [-g graphviz_out]
\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"); +} + int main(int argc, char **argv) { - if (argc < 3) { - fprintf(stderr, "usage: %s
\n", - argv[0]); + int c; + unsigned int seed = time(NULL); + const char *topo_file = NULL; + const char *progname = argv[0]; + + while ((c = getopt(argc, argv, "t:s:g:")) != -1) { + switch (c) { + case 't': + topo_file = optarg; + break; + case 's': + seed = atoi(optarg); + break; + case 'g': + graphviz_file = optarg; + 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 hint = { .ai_family = AF_INET6, .ai_socktype = SOCK_DGRAM, @@ -92,7 +188,7 @@ int main(int argc, char **argv) }; struct addrinfo *server_addr; - int res = getaddrinfo(argv[1], argv[2], + int res = getaddrinfo(argv[0], argv[1], &hint, &server_addr); if (res != 0) { perror("getaddrinfo()"); @@ -113,7 +209,11 @@ int main(int argc, char **argv) freeaddrinfo(server_addr); - dispatch_loop(sock); + if (topology.flat) { + dispatch_loop(sock, _send_flat, &topology.nodes); + } else { + dispatch_loop(sock, _send_topology, &topology); + } close(sock); diff --git a/dist/tools/zep_dispatch/topology.c b/dist/tools/zep_dispatch/topology.c new file mode 100644 index 0000000000..8f88ae8707 --- /dev/null +++ b/dist/tools/zep_dispatch/topology.c @@ -0,0 +1,272 @@ +/* + * 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; +}; + +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 = malloc(sizeof(*node)); + strncpy(node->name, name, sizeof(node->name) - 1); + node->mac_len = 0; + 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; + } + + 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); + fprintf(out, "\t%s -> %s [ label = \"%.2f\" ]\n", + super->a->name, super->b->name, super->weight_a_b); + 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 uint8_t *mac_src, size_t mac_src_len, + void *buffer, size_t len) +{ + 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 ((mac_src_len == super->a->mac_len) && + (memcmp(super->a->mac, mac_src, mac_src_len) == 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 ((mac_src_len == super->a->mac_len) && + (memcmp(super->b->mac, mac_src, mac_src_len) == 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; + } + + /* abort if node is already in list */ + if (memcmp(super->mac, mac, mac_len) == 0) { + return true; + } + } + + /* 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; +} diff --git a/dist/tools/zep_dispatch/topology.h b/dist/tools/zep_dispatch/topology.h new file mode 100644 index 0000000000..2232cc40aa --- /dev/null +++ b/dist/tools/zep_dispatch/topology.h @@ -0,0 +1,79 @@ +/* + * 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" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Struct describing a graph of nodes and their connections + */ +typedef struct { + bool flat; /**< flat topology, all nodes are connected to each other */ + list_node_t nodes; /**< list of nodes */ + list_node_t edges; /**< list of connections between nodes. Unused if topology is flat */ +} 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 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] mac_src ZEP source l2 address + * @param[in] mac_src_len ZEP source l2 address length + * @param[in] buffer ZEP frame to send + * @param[in] len ZEP frame length + */ +void topology_send(const topology_t *t, int sock, + const uint8_t *mac_src, size_t mac_src_len, + 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 0000000000..f769c33e11 --- /dev/null +++ b/dist/tools/zep_dispatch/zep_parser.c @@ -0,0 +1,78 @@ +/* + * 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) + +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; + + if (*out_len < zep->length) { + return false; + } + + 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 0000000000..1a9c0c0ba3 --- /dev/null +++ b/dist/tools/zep_dispatch/zep_parser.h @@ -0,0 +1,42 @@ +/* + * 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 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 */