/* 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')
 *
 * 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>
#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>

#include "utils.h"
#include "mac_table.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;
		struct mac_table mac_tbl;
	} tr;
	struct {
		int socket;
		struct mac_table mac_tbl;
	} 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 enable_promisc(int sk, int ifindex)
{
	struct packet_mreq mreq = {0};
	int rc;

	mreq.mr_ifindex = ifindex;
	mreq.mr_type = PACKET_MR_PROMISC;

	rc = setsockopt(sk, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
	if (rc < 0) {
		fprintf(stderr, "Unable to set promiscuous mode: %s\n", strerror(errno));
		return rc;
	}

	return 0;
}

static int open_packet_socket(int ifindex, int proto)
{
	struct sockaddr_ll addr;
	int fd, rc;

	memset(&addr, 0, sizeof(addr));
	addr.sll_family = AF_PACKET;
	addr.sll_protocol = htons(proto);
	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(proto));
	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;
	}

	rc = enable_promisc(fd, ifindex);
	if (rc < 0) {
		close(fd);
		return rc;
	}

	return fd;
}

static int open_packet_socket_for_netdev(const char *ifname, int proto)
{
	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, proto);
}



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)
{
	char tr_src[MAC_STR_SIZE], tr_dst[MAC_STR_SIZE], eth_src[MAC_STR_SIZE], eth_dst[MAC_STR_SIZE];
	uint8_t buf[2000];
	const struct ethhdr *ethh = (struct ethhdr *) buf;
	struct tr_hdr trh;
	int rc, ethlen, eth_payload_len;

	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;
	}

	mac2str_buf(eth_src, ethh->h_source);
	mac2str_buf(eth_dst, ethh->h_dest);

	/* 802.3 frame: 'proto' is actually the length */
	eth_payload_len = htons(ethh->h_proto);
	if (sizeof(*ethh) + eth_payload_len > ethlen) {
		fprintf(stderr, "TR<-ETH: Not bridging unrealistically long (%zu > %u) frame %s->%s\n",
			sizeof(*ethh) + eth_payload_len, ethlen, eth_src, eth_dst);
		return 0;
	}

	trh.ac = AC;
	trh.fc = LLC_FRAME;
	memcpy(&trh.daddr, ethh->h_dest, sizeof(trh.daddr));
	osmo_revbytebits_buf(trh.daddr, TR_ALEN);
	memcpy(&trh.saddr, ethh->h_source, sizeof(trh.saddr));
	osmo_revbytebits_buf(trh.saddr, TR_ALEN);

	mac2str_buf(tr_src, trh.saddr);
	mac2str_buf(tr_dst, trh.daddr);

	if (mac_table_empty(&bst->tr.mac_tbl) || mac_table_contains(&bst->tr.mac_tbl, trh.daddr)) {
		printf("TR<-ETH: %s/%s <- %s/%s\n", tr_src, eth_src, tr_dst, eth_dst);
		return write_tr(bst->tr.socket, &trh, buf + sizeof(*ethh), eth_payload_len);
	} else {
		printf("TR<-ETH: Ignoring frame to unknown MAC %s\n", mac2str(trh.daddr));
		return 0;
	}
}


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)
{
	char tr_src[MAC_STR_SIZE], tr_dst[MAC_STR_SIZE], eth_src[MAC_STR_SIZE], eth_dst[MAC_STR_SIZE];
	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;
	}
	mac2str_buf(tr_src, trh->saddr);
	mac2str_buf(tr_dst, trh->daddr);

	if (trh->fc != LLC_FRAME) {
		fprintf(stderr, "TR->ETH: Not bridging unexpected non-LLC frame %s->%s FC 0x%02x\n",
			tr_src, tr_dst, trh->fc);
		return 0;
	}

	memcpy(&ethh.h_dest, trh->daddr, sizeof(ethh.h_dest));
	osmo_revbytebits_buf(ethh.h_dest, TR_ALEN);
	memcpy(&ethh.h_source, trh->saddr, sizeof(ethh.h_source));
	osmo_revbytebits_buf(ethh.h_source, TR_ALEN);
	/* We're doing 802.3 and not Ethernet II */
	ethh.h_proto = htons(trlen - sizeof(*trh));

	mac2str_buf(eth_src, ethh.h_source);
	mac2str_buf(eth_dst, ethh.h_dest);

	if (mac_table_empty(&bst->eth.mac_tbl) || mac_table_contains(&bst->eth.mac_tbl, ethh.h_dest)) {
		printf("TR->ETH: %s/%s -> %s/%s\n", tr_src, eth_src, tr_dst, eth_dst);
		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;
	}
}




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;

	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);
	}

	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, ETH_P_ALL);
	if (bst.tr.socket < 0) {
		fprintf(stderr, "Error opening TR\n");
		exit(1);
	}

	bst.eth.socket = open_packet_socket_for_netdev(eth_name, ETH_P_802_2);
	if (bst.eth.socket < 0) {
		fprintf(stderr, "Error opening ETH\n");
		exit(1);
	}

	bridge_main(&bst);

	exit(0);
}