initial check-in of experimental attempt at TR - Ethernet bridge

.gitignore

@@ -0,0 +1,2 @@
+tr-bridge
+*.o

Makefile

@@ -0,0 +1,8 @@
+
+CFLAGS = -Wall -g
+
+tr-bridge: tr-bridge.c
+	$(CC) $(CFLAGS) -o $@ $^
+
+clean:
+	@rm -f tr-bridge

tr-bridge.c

@@ -0,0 +1,269 @@
+/* simplistic Token Ring <-> Ethernet bridge in userspace based on AF_PACKET
+ *
+ * (C) 2022 by Harald Welte <laforge@gnumonks.org>
+ * for Osmocom retronetworking project.
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ * You need to call this program with two arguments:
+ *   - first: the token ring device (e.g. 'tr0')
+ *   - second: the ethernet device (e.g. 'eth0')
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <sys/uio.h>
+#include <netinet/in.h>
+
+#include <linux/if.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <net/ethernet.h>
+
+
+
+#define TR_ALEN		6
+#define	AC		0x10
+#define LLC_FRAME	0x40
+
+struct tr_hdr {
+	uint8_t ac;
+	uint8_t fc;
+	uint8_t daddr[TR_ALEN];
+	uint8_t saddr[TR_ALEN];
+} __attribute((packed));
+
+
+struct bridge_state {
+	struct {
+		int socket;
+	} tr;
+	struct {
+		int socket;
+	} eth;
+};
+
+
+static int devname2ifindex(const char *ifname)
+{
+	struct ifreq ifr;
+	int sk, rc;
+
+	sk = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
+	if (sk < 0)
+		return sk;
+
+	memset(&ifr, 0, sizeof(ifr));
+	strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
+	ifr.ifr_name[sizeof(ifr.ifr_name)-1] = 0;
+
+	rc = ioctl(sk, SIOCGIFINDEX, &ifr);
+	close(sk);
+	if (rc < 0)
+		return rc;
+
+	return ifr.ifr_ifindex;
+}
+
+static int open_packet_socket(int ifindex)
+{
+	struct sockaddr_ll addr;
+	int fd, rc;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sll_family = AF_PACKET;
+	addr.sll_protocol = htons(ETH_P_ALL);
+	addr.sll_ifindex = ifindex;
+	/* we want only packets for _other_ hosts, not packets sent by us or received for us locally */
+	addr.sll_pkttype = PACKET_OTHERHOST;
+
+	fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+	if (fd < 0) {
+		fprintf(stderr, "Can not create AF_PACKET socket. Are you root or have CAP_NET_RAW?\n");
+		return fd;
+	}
+
+	/* there's a race condition between the above syscall and the bind() call below,
+	 * causing other packets to be received in between */
+
+	rc = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
+	if (rc < 0) {
+		fprintf(stderr, "Can not bind AF_PACKET socket to ifindex %d\n", ifindex);
+		close(fd);
+		return rc;
+	}
+
+	return fd;
+}
+
+static int open_packet_socket_for_netdev(const char *ifname)
+{
+	int rc;
+
+	rc = devname2ifindex(ifname);
+	if (rc < 0) {
+		fprintf(stderr, "Cannot find net-device '%s': %s\n", ifname, strerror(errno));
+		return rc;
+	}
+
+	return open_packet_socket(rc);
+}
+
+
+
+static int write_tr(int socket, const struct tr_hdr *trh, const uint8_t *payload, size_t payload_len)
+{
+	struct iovec iov[2] = {
+		{
+			.iov_base = (void *) trh,
+			.iov_len = sizeof(*trh),
+		}, {
+			.iov_base = (void *) payload,
+			.iov_len = payload_len,
+		}
+	};
+
+	return writev(socket, iov, 2);
+}
+
+static int eth2tr(struct bridge_state *bst)
+{
+	uint8_t buf[2000];
+	const struct ethhdr *ethh = (struct ethhdr *) buf;
+	struct tr_hdr trh;
+	int rc, ethlen;
+
+	rc = read(bst->eth.socket, buf, sizeof(buf));
+	if (rc <= 0) {
+		fprintf(stderr, "Error reading from ETH: %s\n", strerror(errno));
+		return rc;
+	}
+	ethlen = rc;
+
+	if (ethlen < sizeof(*ethh)) {
+		fprintf(stderr, "short frame from ETH\n");
+		return 0;
+	}
+
+	trh.ac = AC;
+	trh.fc = LLC_FRAME;
+	memcpy(&trh.daddr, ethh->h_dest, sizeof(trh.daddr));
+	memcpy(&trh.saddr, ethh->h_source, sizeof(trh.saddr));
+
+	return write_tr(bst->tr.socket, &trh, buf + sizeof(*ethh), ethlen - sizeof(*ethh));
+}
+
+
+static int write_eth(int socket, const struct ethhdr *ethh, const uint8_t *payload, size_t payload_len)
+{
+	struct iovec iov[2] = {
+		{
+			.iov_base = (void *) ethh,
+			.iov_len = sizeof(*ethh),
+		}, {
+			.iov_base = (void *) payload,
+			.iov_len = payload_len,
+		}
+	};
+
+	return writev(socket, iov, 2);
+}
+
+static int tr2eth(struct bridge_state *bst)
+{
+	uint8_t buf[2000];
+	const struct tr_hdr *trh = (const struct tr_hdr *) buf;
+	struct ethhdr ethh;
+	int rc, trlen;
+
+	rc = read(bst->tr.socket, buf, sizeof(buf));
+	if (rc <= 0) {
+		fprintf(stderr, "Error reading from TR: %s\n", strerror(errno));
+		return rc;
+	}
+	trlen = rc;
+
+	if (trlen < sizeof(*trh)) {
+		fprintf(stderr, "short frame from TR\n");
+		return 0;
+	}
+
+	memcpy(&ethh.h_dest, trh->daddr, sizeof(ethh.h_dest));
+	memcpy(&ethh.h_source, trh->saddr, sizeof(ethh.h_source));
+	ethh.h_proto = htons(ETH_P_802_2);
+
+	return write_eth(bst->eth.socket, &ethh, buf + sizeof(*trh), trlen - sizeof(*trh));
+}
+
+
+
+
+static int bridge_main(struct bridge_state *bst)
+{
+	int maxfd = 0;
+	int rc;
+	fd_set read_fds;
+
+	FD_ZERO(&read_fds);
+
+	if (bst->tr.socket > maxfd)
+		maxfd = bst->tr.socket;
+
+	if (bst->eth.socket > maxfd)
+		maxfd = bst->eth.socket;
+
+	while (1) {
+		FD_SET(bst->tr.socket, &read_fds);
+		FD_SET(bst->eth.socket, &read_fds);
+
+		rc = select(maxfd+1, &read_fds, NULL, NULL, NULL);
+		if (rc < 0 && errno != EAGAIN) 
+			break;
+
+		if (FD_ISSET(bst->tr.socket, &read_fds))
+			tr2eth(bst);
+
+		if (FD_ISSET(bst->eth.socket, &read_fds))
+			eth2tr(bst);
+	}
+
+	return rc;
+}
+
+int main(int argc, char **argv)
+{
+	struct bridge_state bst;
+	const char *tr_name, *eth_name;
+
+	if (argc < 3) {
+		fprintf(stderr, "You must specify both TR and ETH device\n");
+		exit(2);
+	}
+
+	tr_name = argv[1];
+	eth_name = argv[2];
+
+	bst.tr.socket = open_packet_socket_for_netdev(tr_name);
+	if (bst.tr.socket < 0) {
+		fprintf(stderr, "Error opening TR\n");
+		exit(1);
+	}
+
+	bst.eth.socket = open_packet_socket_for_netdev(eth_name);
+	if (bst.eth.socket < 0) {
+		fprintf(stderr, "Error opening ETH\n");
+		exit(1);
+	}
+
+	bridge_main(&bst);
+
+	exit(0);
+}