270 lines
5.4 KiB
C
270 lines
5.4 KiB
C
/* 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(ð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);
|
|
}
|