introduce MAC address table support (selective bridging)

Makefile

@@ -1,7 +1,7 @@
 
 CFLAGS = -Wall -g
 
-tr-bridge: tr-bridge.c utils.c
+tr-bridge: tr-bridge.c utils.c mac_table.c
 	$(CC) $(CFLAGS) -o $@ $^
 
 clean:

mac_table.c

@@ -0,0 +1,78 @@
+
+#include <string.h>
+#include <errno.h>
+#include <stdio.h>
+#include <errno.h>
+
+#include "mac_table.h"
+#include "utils.h"
+
+
+bool mac_table_contains(struct mac_table *tbl, const uint8_t *addr)
+{
+	unsigned int i;
+	for (i = 0; i < tbl->num_entries; i++) {
+		if (!memcmp(tbl->addr[i], addr, 6))
+			return true;
+	}
+	return false;
+}
+
+int mac_table_add(struct mac_table *tbl, const uint8_t *addr)
+{
+	if (tbl->num_entries == MAC_TABLE_SIZE-1)
+		return -ENOSPC;
+
+	memcpy(tbl->addr[tbl->num_entries++], addr, 6);
+
+	return 0;
+}
+
+void mac_table_init(struct mac_table *tbl, const char *name)
+{
+	memset(tbl, 0, sizeof(*tbl));
+	tbl->name = name;
+}
+
+int mac_table_read(struct mac_table *tbl, const char *fname)
+{
+	FILE *f = fopen(fname, "r");
+	unsigned int num = 0;
+	char line[256];
+
+	if (!f) {
+		fprintf(stderr, "Cannot open mac table %s: %s\n", fname, strerror(errno));
+		return -errno;
+	}
+
+	printf("Reading MAC table %s from file %s...\n", tbl->name, fname);
+
+	while (fgets(line, sizeof(line), f)) {
+		uint8_t addr[6];
+		int rc;
+
+		if (line[strlen(line)-1] == '\n')
+			line[strlen(line)-1] = 0;
+
+		if (strlen(line) < 1)
+			continue;
+
+		/* skip comments */
+		if (line[0] == ';' || line[9] == '#')
+			continue;
+
+		rc = sscanf(line, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
+			    &addr[0], &addr[1], &addr[2], &addr[3], &addr[4], &addr[5]);
+		if (rc != 6) {
+			fprintf(stderr, "%s: Cannot parse line '%s'\n", tbl->name, line);
+			continue;
+		}
+		printf("%s: Read MAC address %s\n", tbl->name, mac2str(addr));
+
+		mac_table_add(tbl, addr);
+		num++;
+	}
+	fclose(f);
+
+	return num;
+}

mac_table.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <stdint.h>
+#include <stdbool.h>
+
+#define MAC_TABLE_SIZE	64
+
+struct mac_table {
+	const char *name;
+	uint8_t addr[MAC_TABLE_SIZE][6];
+	unsigned int num_entries;
+};
+
+static inline bool mac_table_empty(const struct mac_table *tbl)
+{
+	return tbl->num_entries == 0;
+}
+
+void mac_table_init(struct mac_table *tbl, const char *name);
+int mac_table_add(struct mac_table *tbl, const uint8_t *addr);
+bool mac_table_contains(struct mac_table *tbl, const uint8_t *addr);
+int mac_table_read(struct mac_table *tbl, const char *fname);

tr-bridge.c

@@ -8,6 +8,16 @@
  * You need to call this program with two arguments:
  *   - first: the token ring device (e.g. 'tr0')
  *   - second: the ethernet device (e.g. 'eth0')
+ *
+ * Optionally, you may psecify with two additional arguments:
+ *   - third: A file containing MAC addresses on TR side
+ *   - fourth: A file containing MAC addresses on ETH side
+ *
+ * The file syntax is one MAC address per line in 01:02:03:04:05:06 format.
+ *
+ * If MAC address files are specified, the bridge will only pass frames for those
+ * addresses and drop all other frames.  Please note that you need to specify the
+ * bit ordering _native_ to the respective side.  So Eth order for Eth; TR order for TR.
  */
 
 #include <stdint.h>
@@ -29,6 +39,7 @@
 #include <net/ethernet.h>
 
 #include "utils.h"
+#include "mac_table.h"
 
 
 #define TR_ALEN		6
@@ -46,9 +57,11 @@ struct tr_hdr {
 struct bridge_state {
 	struct {
 		int socket;
+		struct mac_table mac_tbl;
 	} tr;
 	struct {
 		int socket;
+		struct mac_table mac_tbl;
 	} eth;
 };
 
@@ -188,7 +201,12 @@ static int eth2tr(struct bridge_state *bst)
 	memcpy(&trh.saddr, ethh->h_source, sizeof(trh.saddr));
 	osmo_revbytebits_buf(trh.saddr, TR_ALEN);
 
-	return write_tr(bst->tr.socket, &trh, buf + sizeof(*ethh), ethlen - sizeof(*ethh));
+	if (mac_table_empty(&bst->tr.mac_tbl) || mac_table_contains(&bst->tr.mac_tbl, trh.daddr))
+		return write_tr(bst->tr.socket, &trh, buf + sizeof(*ethh), ethlen - sizeof(*ethh));
+	else {
+		printf("TR<-ETH: Ignoring frame to unknown MAC %s\n", mac2str(trh.daddr));
+		return 0;
+	}
 }
 
 
@@ -237,7 +255,12 @@ static int tr2eth(struct bridge_state *bst)
 	osmo_revbytebits_buf(ethh.h_source, TR_ALEN);
 	ethh.h_proto = htons(ETH_P_802_2);
 
-	return write_eth(bst->eth.socket, &ethh, buf + sizeof(*trh), trlen - sizeof(*trh));
+	if (mac_table_empty(&bst->eth.mac_tbl) || mac_table_contains(&bst->eth.mac_tbl, ethh.h_dest))
+		return write_eth(bst->eth.socket, &ethh, buf + sizeof(*trh), trlen - sizeof(*trh));
+	else {
+		printf("TR->ETH: Ignoring frame to unknown MAC %s\n", mac2str(ethh.h_dest));
+		return 0;
+	}
 }
 
 
@@ -280,6 +303,9 @@ int main(int argc, char **argv)
 	struct bridge_state bst;
 	const char *tr_name, *eth_name;
 
+	mac_table_init(&bst.tr.mac_tbl, "TR");
+	mac_table_init(&bst.eth.mac_tbl, "ETH");
+
 	if (argc < 3) {
 		fprintf(stderr, "You must specify both TR and ETH device\n");
 		exit(2);
@@ -288,6 +314,11 @@ int main(int argc, char **argv)
 	tr_name = argv[1];
 	eth_name = argv[2];
 
+	if (argc >= 5) {
+		mac_table_read(&bst.tr.mac_tbl, argv[3]);
+		mac_table_read(&bst.eth.mac_tbl, argv[4]);
+	}
+
 	bst.tr.socket = open_packet_socket_for_netdev(tr_name, htons(ETH_P_ALL));
 	if (bst.tr.socket < 0) {
 		fprintf(stderr, "Error opening TR\n");

utils.c

@@ -1,4 +1,7 @@
 #include <stdint.h>
+#include <stdio.h>
+
+#include "utils.h"
 
 /* Copied from libosmocore.git src/bits.c */
 
@@ -38,3 +41,13 @@ void osmo_revbytebits_buf(uint8_t *buf, int len)
 	for (i = 0; i < len; i++)
 		buf[i] = flip_table[buf[i]];
 }
+
+const char *mac2str(const uint8_t *addr)
+{
+	static char buf[32];
+
+	snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x",
+	         addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+	return buf;
+}

utils.h

@@ -1,3 +1,5 @@
 #pragma once
 
 void osmo_revbytebits_buf(uint8_t *buf, int len);
+
+const char *mac2str(const uint8_t *addr);