mirror of
https://github.com/RIOT-OS/RIOT.git
synced 2025-01-18 12:52:44 +01:00
5ef70dbee9
There can only be a single sniffer, but instead of ignoring any subsequent sniffers, replace the existing one. This avoids the silly behaviour that you can't attach the sniffer anymore should you ever quit it.
300 lines
7.2 KiB
C
300 lines
7.2 KiB
C
/*
|
|
* 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 <arpa/inet.h>
|
|
#include <netdb.h>
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <limits.h>
|
|
|
|
#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 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;
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
|
|
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;
|
|
}
|