allow Wireguard to replace ppp

tunnel_wireguard_over_serial.c

/* isc license probably */
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>

#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <termios.h>
#include <arpa/inet.h>
#include <sys/socket.h>

#define NOPE(...) do { fprintf(stderr, "error: " __VA_ARGS__); exit(EXIT_FAILURE); } while(0)

#define END_BYTE 0x7E
#define ESC_BYTE 0x7D
#define ESC_MASK 0x20

static void writeall(int fd, const unsigned char * buf, const size_t count) {
    for (const unsigned char * cursor = buf, * end = buf + count; cursor < end; ) {
        const ssize_t ret = write(fd, cursor, end - cursor);
        if (-1 == ret) NOPE("%s: cannot write(): %s\n", __func__, strerror(errno));
        cursor += ret;
    }
}

int main(const int argc, char ** const argv) {
    const char * const slash_in_argvzero = strrchr(argv[0], '/');
    const char * const progname = slash_in_argvzero ? slash_in_argvzero + 1 : argv[0];
    
    if (argc < 2) {
        fprintf(stderr, "%s: Forward packets between udp and serial, with ppp-like framing, for the purpose of tunneling Wireguard over a raw serial line\n\n", progname);
        fprintf(stderr, "Usage: %s [serial device] [baud rate] [udp port to listen on] [udp port to forward to]\n\n", argv[0]);
        fprintf(stderr, "Example: given a local Wireguard network with a ListenPort of 51820, configured to talk to a peer with an assumed Endpoint of 127.0.0.1:51821, you would run:\n");
        fprintf(stderr, " %s /dev/ttyS0 115200 51821 51820\n", argv[0]);
        exit(EXIT_FAILURE);
    }
    
    /* parse cmdline arguments */
    const char * const path_serial_device = argc > 1 ? argv[1] : "/dev/ttyS0";
    const unsigned int baud = argc > 2 ? (unsigned int)strtoul(argv[2], NULL, 10) : 115200;
    const unsigned short udp_port = argc > 3 ? strtoul(argv[3], NULL, 10) : 51821;
    const unsigned short udp_local_port = argc > 4 ? strtoul(argv[4], NULL, 10) : 51820;
    
    const int fd_serial = open(path_serial_device, O_RDWR | O_NOCTTY);
    if (-1 == fd_serial) NOPE("%s: cannot open %s: %s\n", progname, path_serial_device, strerror(errno));

    struct termios ts;
    if (-1 == tcgetattr(fd_serial, &ts)) NOPE("%s: cannot tcgetattr: %s\n", progname, strerror(errno));
    cfmakeraw(&ts);
    
    /* turn off input and output processing as much as possible */
    ts.c_iflag = 0;
    ts.c_oflag = 0;
    
    /* turn on ICANON */
    ts.c_lflag = ICANON;
    
    /* but also disable all control characters, except for VEOL */
    for (size_t icc = 0; icc < sizeof(ts.c_cc) / sizeof(ts.c_cc[0]); icc++)
        ts.c_cc[icc] = VEOL == icc ? END_BYTE : _POSIX_VDISABLE;
    
    /* if your desired baud rate is not represented here, fix it */
    if (-1 == cfsetspeed(&ts, 2400 == baud ? B2400 : 4800 == baud ? B4800 : 9600 == baud ? B9600 : 19200 == baud ? B19200 : B115200))
        NOPE("%s: cannot cfsetspeed: %s\n", progname, strerror(errno));
    if (-1 == tcsetattr(fd_serial, TCSANOW, &ts)) NOPE("%s: cannot tcsetattr: %s\n", progname, strerror(errno));
    
    /* attempt to clear stale data */
    if (-1 == tcflush(fd_serial, TCIOFLUSH)) NOPE("%s: cannot tcflush: %s\n", progname, strerror(errno));
    
    /* open a socket for receiving udp packets */
    const int fd_udp = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (-1 == fd_udp) NOPE("%s: cannot socket(): %s\n", progname, strerror(errno));
    
    if (bind(fd_udp, (struct sockaddr *)&(struct sockaddr_in) {
        .sin_family = AF_INET,
        .sin_port = htons(udp_port),
        .sin_addr.s_addr = htonl(INADDR_ANY)
    }, sizeof(struct sockaddr_in)))
        NOPE("%s: cannot bind(%d): %s\n", progname, udp_port, strerror(errno));
    
    /* this will be overwritten each time we get a new packet from the actual peer */
    struct sockaddr_in peer = {
        .sin_family = AF_INET,
        .sin_port = htons(udp_local_port),
        .sin_addr.s_addr = htonl(INADDR_LOOPBACK)
    };
        
    unsigned char serial_buffer[3072];
    unsigned char * cursor = serial_buffer;
    
    while (1) {
        struct pollfd pollfds[] = {
            {  .fd = fd_serial, .events = POLLIN },
            {  .fd = fd_udp, .events = POLLIN },
        };
        if (-1 == poll(pollfds, sizeof(pollfds) / sizeof(struct pollfd), -1)) NOPE("%s: cannot poll: %s\n", progname, strerror(errno));
        
        /* at least one new byte can be read from the serial line */
        if (POLLIN == pollfds[0].revents) {
            /* if this read would go off the end of the buffer... */
            if (cursor - serial_buffer == sizeof(serial_buffer)) {
                /* print a warning and reset the buffer */
                fprintf(stderr, "%s: never got end of packet, resetting serial buffer\n", progname);
                cursor = serial_buffer;
            }
            
            /* got new byte(s) from serial */
            const ssize_t count = read(fd_serial, cursor, sizeof(serial_buffer) - (cursor - serial_buffer));
            if (-1 == count) NOPE("%s: cannot read(): %s\n", progname, strerror(errno));
            if (!count) break;
                        
            /* find end byte in the characters which were just read */
            unsigned char * const end = memchr(cursor, END_BYTE, count);

            if (!end) {
                cursor += count;
                continue;
            }

            const size_t bytes_buffered_before_end = end - serial_buffer;
            const size_t bytes_remaining_after_end = count - (end - cursor) - 1;

            unsigned char plain[3072];
            size_t plain_size = 0;
            
            for (size_t iencoded = 0; iencoded < bytes_buffered_before_end; ) {
                const unsigned char encoded = serial_buffer[iencoded++];
                plain[plain_size++] = ESC_BYTE == encoded ? serial_buffer[iencoded++] ^ ESC_MASK : encoded;
            }
            
            fprintf(stderr, "%s: forwarding %zu->%zu bytes to udp %d\n", progname, bytes_buffered_before_end + 1, plain_size, ntohs(peer.sin_port));
            
            if (!peer.sin_port)
                fprintf(stderr, "%s: got %zd bytes from serial, but no local udp destination known yet\n", progname, count);
            else
                if (-1 == sendto(fd_udp, plain, plain_size, 0, (void *)&peer, sizeof(peer)))
                    fprintf(stderr, "%s warning: failed to send to %u: %s\n", progname, ntohs(peer.sin_port), strerror(errno));
            
            /* not worth optimizing because it should never happen, but if it does due to weird termios behaviour, handle it correctly */
            if (bytes_remaining_after_end)
                memmove(serial_buffer, end + 1, bytes_remaining_after_end);
            cursor = serial_buffer + bytes_remaining_after_end;
        }
        
        /* at least one complete new udp packet can be recv'd */
        if (POLLIN == pollfds[1].revents) {
            unsigned char plain[1535];
            const ssize_t count = recvfrom(fd_udp, plain, sizeof(plain), 0, (void *)&peer, &(socklen_t) { sizeof(peer) });
            if (-1 == count) NOPE("%s: cannot recvfrom(): %s\n", progname, strerror(errno));
            
            unsigned char encoded[3071];
            size_t encoded_size = 0;
            
            /* escape bytes as necessary */
            for (size_t iplain = 0; iplain < (size_t)count; ) {
                const unsigned char plain_byte = plain[iplain++];
                if (END_BYTE == plain_byte || ESC_BYTE == plain_byte) {
                    encoded[encoded_size++] = ESC_BYTE;
                    encoded[encoded_size++] = plain_byte ^ ESC_MASK;
                }
                else
                    encoded[encoded_size++] = plain_byte;
            }
            
            /* append end byte */
            encoded[encoded_size++] = END_BYTE;
            
            fprintf(stderr, "%s: forwarding %zd->%zu bytes to serial\n", progname, count, encoded_size);
            writeall(fd_serial, encoded, encoded_size);
        }
    }
}