commit 580f923b48ed5fbddf3e2f2bfda112b9c62665ab Author: Harald Welte Date: Wed Apr 6 22:54:30 2022 +0200 initial check-in of experimental attempt at TR - Ethernet bridge diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..374e70f --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +tr-bridge +*.o diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..12101b1 --- /dev/null +++ b/Makefile @@ -0,0 +1,8 @@ + +CFLAGS = -Wall -g + +tr-bridge: tr-bridge.c + $(CC) $(CFLAGS) -o $@ $^ + +clean: + @rm -f tr-bridge diff --git a/tr-bridge.c b/tr-bridge.c new file mode 100644 index 0000000..a40bc6a --- /dev/null +++ b/tr-bridge.c @@ -0,0 +1,269 @@ +/* simplistic Token Ring <-> Ethernet bridge in userspace based on AF_PACKET + * + * (C) 2022 by Harald Welte + * 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + + +#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(ðh.h_dest, trh->daddr, sizeof(ethh.h_dest)); + memcpy(ðh.h_source, trh->saddr, sizeof(ethh.h_source)); + ethh.h_proto = htons(ETH_P_802_2); + + return write_eth(bst->eth.socket, ðh, 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); +}