Split UdpVpn in -Client and -Server flavour

.gitignore

@@ -88,3 +88,5 @@ docs/_build/
 # PyBuilder
 target/
 
+# ---> Local ignores
+congestvpn

Makefile

@@ -0,0 +1,23 @@
+CXX=g++
+CXXFLAGS=-O2 -g -Wall -Wextra -std=c++17
+CXXLIBS=
+
+OBJS= \
+	  UdpVpn.o UdpVpnClient.o UdpVpnServer.o \
+	  VpnPeer.o \
+	  TunDevice.o \
+	  ip_header.o util.o main.o
+TARGET=congestvpn
+
+all: $(TARGET)
+
+$(TARGET): $(OBJS)
+	$(CXX) $(CXXFLAGS) $(CXXLIBS) -o $@ $^
+
+%.o: %.cpp
+	$(CXX) $(CXXFLAGS) -c -o $@ $<
+
+clean:
+	rm -f $(OBJS) $(TARGET)
+
+.PHONY: all clean

TunDevice.cpp

@@ -0,0 +1,108 @@
+#include <linux/if.h>
+#include <linux/if_tun.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <cstdlib>
+#include <cstring>
+
+#include "TunDevice.hpp"
+
+static const size_t TUN_MTU = 1500;  // TODO determine this cleanly
+
+TunDevice::TunDevice(const std::string& dev)
+{
+    struct ifreq ifr;
+    int fd;
+
+    if( (fd = open("/dev/net/tun", O_RDWR)) < 0 ) {
+        throw TunDevice::InitializationError(
+                "Cannot open /dev/net/tun", errno, true);
+    }
+
+    memset(&ifr, 0, sizeof(ifr));
+
+    /* Flags: IFF_TUN   - TUN device (no Ethernet headers)
+     *        IFF_TAP   - TAP device
+     *
+     *        IFF_NO_PI - Do not provide packet information
+     */
+    ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
+    if(!dev.empty()) {
+        if(dev.size() >= IFNAMSIZ - 2)
+            throw TunDevice::InitializationError("Device name is too long.");
+        strncpy(ifr.ifr_name, dev.c_str(), IFNAMSIZ-1);
+    }
+
+    if(ioctl(fd, TUNSETIFF, (void *) &ifr) < 0){
+        close(fd);
+        throw TunDevice::InitializationError(
+                "Tunnel interface failed [TUNSETIFF]", errno, true);
+    }
+    _dev_name = ifr.ifr_name;
+    _fd = fd;
+
+    // Bring interface up
+    kdebugf("Bringing interface up...\n");
+    int sockfd = socket(AF_INET, SOCK_DGRAM, 0); // Any socket will do
+    if(ioctl(sockfd, SIOCGIFFLAGS, (void*) &ifr) < 0) {
+        close(fd);
+        close(sockfd);
+        throw TunDevice::InitializationError(
+                "Could not get tunnel interface flags", errno, true);
+    }
+    ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
+
+    if(ioctl(sockfd, SIOCSIFFLAGS, (void*) &ifr) < 0) {
+        close(fd);
+        close(sockfd);
+        throw TunDevice::InitializationError(
+                "Could not bring tunnel interface up", errno, true);
+    }
+    close(sockfd);
+
+    // The device is now fully set up
+    _poll_fd.fd = _fd;
+    _poll_fd.events = POLLIN;
+}
+
+TunDevice::~TunDevice() {
+    close(_fd);
+}
+
+size_t TunDevice::poll_packet(char* read_buffer, size_t buf_size, int timeout) {
+    int poll_rc = poll(&_poll_fd, 1, timeout);
+    if(poll_rc < 0) {
+        if(errno == EINTR) // Interrupt.
+            return 0;
+        throw TunDevice::NetError(
+                "Error polling from interface", errno, true);
+    }
+    else if(poll_rc == 0 || (_poll_fd.revents & POLLIN) == 0) {
+        // Nothing to read
+        return 0;
+    }
+
+    return this->read(read_buffer, buf_size);
+}
+
+size_t TunDevice::read(char* read_buffer, size_t buf_size) {
+    int nread = ::read(_fd, read_buffer, buf_size);
+    if(nread < 0) {
+        throw TunDevice::NetError(
+                "Error reading from interface", errno, true);
+    }
+    _last_read_size = nread;
+    return _last_read_size;
+}
+
+size_t TunDevice::write(const char* data, size_t len) {
+    int nwritten = ::write(_fd, data, len);
+    if(nwritten < 0) {
+        throw TunDevice::NetError(
+                "Error writing to interface: ", errno, true);
+    }
+    return nwritten;
+}

TunDevice.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <string>
+#include <poll.h>
+#include "util.hpp"
+
+class TunDevice {
+    public:
+        class InitializationError : public MsgException {
+            public:
+                InitializationError(
+                        const std::string& msg,
+                        int code=0,
+                        bool is_perror=false)
+                    : MsgException(msg, code, is_perror) {}
+        };
+        class NetError : public MsgException {
+            public:
+                NetError(
+                        const std::string& msg,
+                        int code=0,
+                        bool is_perror=false)
+                    : MsgException(msg, code, is_perror) {}
+        };
+
+        TunDevice(const std::string& dev);
+        ~TunDevice();
+
+        const std::string& get_dev_name() const { return _dev_name; }
+        int get_fd() const { return _fd; }
+
+        /* Reads a packet from the device.
+         * Timeouts after `timeout` ms, or never if `timeout < 0`.
+         * Upon timeout, returns 0.
+         */
+        size_t poll_packet(char* read_buffer, size_t buf_size, int timeout=-1);
+
+        /* Reads a packet, blocking if none is available. */
+        size_t read(char* read_buffer, size_t buf_size);
+
+        size_t write(const char* data, size_t len);
+
+    private:
+        int _fd;
+        std::string _dev_name;
+        struct pollfd _poll_fd;
+        size_t _last_read_size;
+};

UdpVpn.cpp

@@ -0,0 +1,106 @@
+#include "UdpVpn.hpp"
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <string.h>
+#include <poll.h>
+#include <errno.h>
+
+#include "ip_header.hpp"
+
+UdpVpn::UdpVpn()
+    : _stopped(false), _tun_dev("cvpn%d")
+{
+    _socket = socket(AF_INET6, SOCK_DGRAM, 0);
+    if(_socket < 0)
+        throw UdpVpn::InitializationError("Cannot create socket", errno, true);
+}
+
+UdpVpn::~UdpVpn() {
+    close(_socket);
+}
+
+void UdpVpn::run() {
+    int rc;
+    int start_at_fd = 0;  // read from polled fds in round-robin fashion
+    int cur_fd;
+    int nfds = 2;
+    struct pollfd poll_fds[2];
+
+    // poll_fds[0]: tun device
+    poll_fds[0].fd = _tun_dev.get_fd();
+    poll_fds[0].events = POLLIN;
+
+    // poll_fds[1]: UDP socket device
+    poll_fds[1].fd = _socket;
+    poll_fds[1].events = POLLIN;
+
+    while(!_stopped) {
+        rc = poll(poll_fds, nfds, -1);
+
+        if(rc < 0) {
+            if(errno == EINTR)  // Interrupt.
+                continue;
+            throw UdpVpn::NetError(
+                    "Error polling from interface", errno, true);
+        }
+        else if(rc == 0)  // Nothing to read
+            continue;
+
+        cur_fd = start_at_fd;
+        do {
+            if(poll_fds[cur_fd].revents & POLLIN) {
+                if(cur_fd == 0)
+                    receive_from_tun();
+                else if(cur_fd == 1)
+                    receive_from_udp();
+                break;
+            }
+
+            cur_fd = (cur_fd + 1) % nfds;
+        } while(cur_fd != start_at_fd);
+
+        start_at_fd = (start_at_fd + 1) % nfds;
+    }
+}
+
+size_t UdpVpn::read_from_tun(char* buffer, size_t len) {
+    // We know that there is data available -- use `read()`
+    size_t nread = _tun_dev.read(buffer, len);
+
+    if(nread == 0)
+        return 0;
+
+    return nread;
+}
+
+size_t UdpVpn::read_from_udp(char* buffer, size_t len,
+        sockaddr_in6& peer_addr)
+{
+    ssize_t nread;
+    socklen_t peer_addr_len = sizeof(peer_addr);
+    nread = recvfrom(_socket, buffer, len, MSG_WAITALL,
+            (struct sockaddr*) &peer_addr, &peer_addr_len);
+
+    if(nread < 0)
+        throw UdpVpn::NetError("Cannot receive datagram", errno, true);
+    if(nread == 0)
+        return 0;
+
+    if(peer_addr.sin6_family != AF_INET6) {
+        debugf("WARNING: Received non-ipv6 family datagram %d. Ignoring.\n",
+                peer_addr.sin6_family);
+        return 0;
+    }
+    if(peer_addr_len != sizeof(peer_addr)) {
+        debugf("WARNING: received unexpected source address length %u."
+                "Ignoring.\n",
+                peer_addr_len);
+        return 0;
+    }
+
+    return nread;
+}

UdpVpn.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include "util.hpp"
+#include "TunDevice.hpp"
+#include "VpnPeer.hpp"
+
+/** Handles UDP communication */
+
+class UdpVpn {
+    public:
+        class InitializationError : public MsgException {
+            public:
+                InitializationError(
+                        const std::string& msg,
+                        int code=0,
+                        bool is_perror=false)
+                    : MsgException(msg, code, is_perror) {}
+        };
+        class NetError : public MsgException {
+            public:
+                NetError(
+                        const std::string& msg,
+                        int code=0,
+                        bool is_perror=false)
+                    : MsgException(msg, code, is_perror) {}
+        };
+
+        UdpVpn();
+        virtual ~UdpVpn();
+
+        int get_socket_fd() const { return _socket; }
+        const TunDevice& get_tun_dev() const { return _tun_dev; }
+
+        // Run the server.
+        void run();
+
+        // Stop the server. Can be called from an interrupt.
+        void stop() { _stopped = true; }
+
+    protected:
+        virtual void receive_from_tun() = 0;
+        virtual void receive_from_udp() = 0;
+
+        size_t read_from_tun(char* buffer, size_t len);
+        size_t read_from_udp(char* buffer, size_t len, sockaddr_in6& peer_addr);
+
+        int _socket;
+        bool _stopped;
+
+        TunDevice _tun_dev;
+};

UdpVpnClient.cpp

@@ -0,0 +1,67 @@
+#include <cstring>
+
+#include "UdpVpnClient.hpp"
+#include "ip_header.hpp"
+
+UdpVpnClient::UdpVpnClient(const struct sockaddr_in6& server) : UdpVpn() {
+    memset(&_server_addr, 0, sizeof(_server_addr));
+    set_server(server);
+}
+
+void UdpVpnClient::set_server(const struct sockaddr_in6& server_addr) {
+    if(server_addr.sin6_family != AF_INET6)
+        throw UdpVpn::InitializationError("Server address must be IPv6");
+
+    memcpy(&_server_addr, &server_addr, sizeof(_server_addr));
+}
+
+void UdpVpnClient::receive_from_tun() {
+    char buffer[1500];
+    size_t nread = read_from_tun(buffer, 1500);
+    if(nread == 0)
+        return;
+
+    // Parse inner packet header -- assume IPv6 for now [FIXME]
+    IPv6Header inner_header;
+    if(!parse_ipv6_header(buffer, nread, inner_header)) {
+        // Not a valid header -- ignore the packet
+        debugf("Ignoring outgoing packet with invalid header\n");
+        return;
+    }
+
+    kdebugf("Transmitting %s -> %s, size %d\n",
+            format_address(inner_header.source.s6_addr),
+            format_address(inner_header.dest.s6_addr),
+            nread);
+
+    write_to_server(buffer, nread);
+}
+
+void UdpVpnClient::receive_from_udp() {
+    char buffer[1500];
+    sockaddr_in6 peer_addr;
+    size_t nread = read_from_udp(buffer, 1500, peer_addr);
+
+    // Parse inner packet header -- assume IPv6 for now [FIXME]
+    IPv6Header inner_header;
+    if(!parse_ipv6_header(buffer, nread, inner_header)) {
+        // Not a valid header -- ignore the packet
+        debugf("Ignoring packet with invalid header\n");
+        return;
+    }
+
+    // Reinject into tun
+    kdebugf("Receiving packet of size %d from peer\n", nread);
+    _tun_dev.write(buffer, nread);
+}
+
+size_t UdpVpnClient::write_to_server(const char* data, size_t len) {
+    ssize_t nsent;
+
+    nsent = sendto(_socket, data, len, MSG_CONFIRM,
+            (const struct sockaddr*) &_server_addr, sizeof(_server_addr));
+    if(nsent < 0)
+        throw NetError("Could not send UDP packet", errno, true);
+
+    return (size_t) nsent;
+}

UdpVpnClient.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "UdpVpn.hpp"
+
+class UdpVpnClient: public UdpVpn {
+    public:
+        UdpVpnClient(const struct sockaddr_in6& server);
+
+    protected:
+        void set_server(const struct sockaddr_in6& server_addr);
+
+        virtual void receive_from_tun();
+        virtual void receive_from_udp();
+
+        size_t write_to_server(const char* data, size_t len);
+
+        struct sockaddr_in6 _server_addr;
+};

UdpVpnServer.cpp

@@ -0,0 +1,104 @@
+#include <cstring>
+
+#include "UdpVpnServer.hpp"
+#include "ip_header.hpp"
+
+UdpVpnServer::UdpVpnServer(in_port_t port) : UdpVpn() {
+    memset(&_bind_addr, 0, sizeof(_bind_addr));
+    bind(in6addr_any, port);
+}
+
+UdpVpnServer::UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port)
+    : UdpVpn()
+{
+    memset(&_bind_addr, 0, sizeof(_bind_addr));
+    bind(bind_addr6, port);
+}
+
+void UdpVpnServer::bind(const struct in6_addr& bind_addr6, in_port_t port) {
+    int rc;
+
+    _bind_addr.sin6_family = AF_INET6;
+    _bind_addr.sin6_port = htons(port);
+    _bind_addr.sin6_addr = bind_addr6;
+
+    rc = ::bind(
+            _socket, (const struct sockaddr*)&_bind_addr, sizeof(_bind_addr));
+    if(rc < 0) {
+        throw UdpVpn::InitializationError("Cannot bind socket", errno, true);
+    }
+
+    debugf("> Listening on port %d\n", port);
+}
+
+
+std::shared_ptr<VpnPeer> UdpVpnServer::get_peer_for_ip(
+        const in6_addr& peer_addr)
+{
+    auto peer_iter = _peers.find(peer_addr);
+    if(peer_iter == _peers.end()) // Unknown peer
+        return nullptr;
+    return peer_iter->second;
+}
+
+void UdpVpnServer::receive_from_tun() {
+    char buffer[1500];
+    size_t nread = read_from_tun(buffer, 1500);
+    if(nread == 0)
+        return;
+
+    // Parse inner packet header -- assume IPv6 for now [FIXME]
+    IPv6Header inner_header;
+    if(!parse_ipv6_header(buffer, nread, inner_header)) {
+        // Not a valid header -- ignore the packet
+        debugf("Ignoring outgoing packet with invalid header\n");
+        return;
+    }
+
+    // Recover VpnPeer -- or drop if new
+    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.dest);
+    if(!peer) {
+        debugf("Dropping packet for destination %s -- unknown peer.\n",
+                format_address(inner_header.dest.s6_addr));
+        return;
+    }
+
+    kdebugf("Transmitting %s -> %s, size %d\n",
+            format_address(inner_header.source.s6_addr),
+            format_address(inner_header.dest.s6_addr),
+            nread);
+    peer->write(buffer, nread);
+}
+
+void UdpVpnServer::receive_from_udp() {
+    char buffer[1500];
+    sockaddr_in6 peer_addr;
+    size_t nread = read_from_udp(buffer, 1500, peer_addr);
+
+    // Parse inner packet header -- assume IPv6 for now [FIXME]
+    IPv6Header inner_header;
+    if(!parse_ipv6_header(buffer, nread, inner_header)) {
+        // Not a valid header -- ignore the packet
+        debugf("Ignoring packet with invalid header\n");
+        return;
+    }
+
+    // Recover VpnPeer -- or create if new
+    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.source);
+    if(!peer) {
+        peer = std::make_shared<VpnPeer>(this, peer_addr, inner_header.source);
+        auto insert_result = _peers.insert({inner_header.source, peer});
+
+        debugf("Got new peer %s:%d -- %s [really inserted=%d]\n",
+                format_address(peer_addr.sin6_addr.s6_addr),
+                htons(peer_addr.sin6_port),
+                format_address(inner_header.source.s6_addr),
+                insert_result.second);
+    }
+    // VpnPeer* peer = (peer_iter->second);
+    // TODO -- pass the packet to `peer` for a cleaner flow
+
+    // Reinject into tun
+    kdebugf("Receiving packet of size %d from peer\n", nread);
+    _tun_dev.write(buffer, nread);
+}

UdpVpnServer.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <unordered_map>
+#include <memory>
+
+#include "UdpVpn.hpp"
+
+class UdpVpnServer: public UdpVpn {
+    public:
+        UdpVpnServer(in_port_t port);
+        UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port);
+    protected:
+        void bind(const struct in6_addr& bind_addr6, in_port_t port);
+
+        /** Get the peer associated to this (internal) IP. */
+        std::shared_ptr<VpnPeer> get_peer_for_ip(const in6_addr& peer_addr);
+
+        virtual void receive_from_tun();
+        virtual void receive_from_udp();
+
+        struct sockaddr_in6 _bind_addr;
+        std::unordered_map<in6_addr, std::shared_ptr<VpnPeer>> _peers;
+};

VpnPeer.cpp

@@ -0,0 +1,23 @@
+#include "VpnPeer.hpp"
+#include "UdpVpn.hpp"
+
+#include <cstdint>
+#include <cstring>
+#include <functional>
+
+VpnPeer::VpnPeer(UdpVpn* vpn, const sockaddr_in6& ext_addr,
+        const in6_addr int_addr)
+    : _vpn(vpn), _ext_addr(ext_addr), _int_addr(int_addr)
+{}
+
+
+size_t VpnPeer::write(const char* data, size_t len) {
+    ssize_t nsent;
+
+    nsent = sendto(_vpn->get_socket_fd(), data, len, MSG_CONFIRM,
+            (const struct sockaddr*) &_ext_addr, sizeof(_ext_addr));
+    if(nsent < 0)
+        throw NetError("Could not send UDP packet", errno, true);
+
+    return (size_t) nsent;
+}

VpnPeer.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+/** A peer of a bound (server) instance of UdpVpn */
+
+#include <netinet/in.h>
+#include "util.hpp"
+
+class UdpVpn;
+
+class VpnPeer {
+    public:
+        class NetError : public MsgException {
+            public:
+                NetError(
+                        const std::string& msg,
+                        int code=0,
+                        bool is_perror=false)
+                    : MsgException(msg, code, is_perror) {}
+        };
+
+        VpnPeer(UdpVpn* vpn, const sockaddr_in6& ext_addr,
+                const in6_addr int_addr);
+
+        const sockaddr_in6& get_ext_addr() const { return _ext_addr; }
+        const in6_addr& get_int_addr() const { return _int_addr; }
+
+        size_t write(const char* data, size_t len);
+
+    private:
+        UdpVpn* _vpn;
+        sockaddr_in6 _ext_addr;
+        in6_addr _int_addr;
+};

ip_header.cpp

@@ -0,0 +1,19 @@
+#include <cstring>
+
+#include "ip_header.hpp"
+#include "util.hpp"
+
+bool parse_ipv6_header(const char* data, size_t len, IPv6Header& header) {
+    if(len < 40) { // header too short
+        kdebugf("Bad v6 header -- too short.\n");
+        return false;
+    }
+    if((data[0] & 0xf0) != 0x60) { // bad version number
+        kdebugf("Bad v6 header -- bad version number (byte = %x).\n", data[0]);
+        return false;
+    }
+
+    memcpy(&(header.source), data + 8, 16);
+    memcpy(&(header.dest), data + 24, 16);
+    return true;
+}

ip_header.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+#include <netinet/in.h>
+
+struct IPv6Header {
+    in6_addr source;
+    in6_addr dest;
+};
+
+/** Parse an IPv6 header, filling `header`. Returns `true` on success. */
+bool parse_ipv6_header(const char* data, size_t len, IPv6Header& header);

main.cpp

@@ -0,0 +1,143 @@
+#include <cstdio>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <signal.h>
+#include <string.h>
+
+#include "UdpVpn.hpp"
+#include "UdpVpnServer.hpp"
+#include "UdpVpnClient.hpp"
+#include "util.hpp"
+
+struct ProgOptions {
+    bool listen;
+    in6_addr bind_addr;
+    in_port_t bind_port;
+
+    bool has_peer;
+    in6_addr server_addr;
+    in_port_t server_port;
+};
+
+static UdpVpn* vpn_instance = nullptr;
+
+void stop_sig_handler(int signal) {
+    printf("Received signal %d. Stopping.\n", signal);
+    if(vpn_instance != nullptr)
+        vpn_instance->stop();
+}
+
+bool parse_options(int argc, char** argv, ProgOptions& opts) {
+    int option;
+    memset(&opts, 0, sizeof(opts));
+
+    while((option = getopt(argc, argv, ":d:l:b:s:p:")) >= 0) {
+        switch(option) {
+            case 'd': // debug
+                debug = atoi(optarg);
+                break;
+            case 'l': // listen, aka "call bind()"
+                opts.listen = true;
+                opts.bind_port = atoi(optarg);
+                break;
+            case 'b': // bind to address
+                inet_pton(AF_INET6, optarg, &opts.bind_addr);
+                break;
+            case 's': // server -- initial peer
+                opts.has_peer = true;
+                inet_pton(AF_INET6, optarg, &opts.server_addr);
+                break;
+            case 'p': // port -- initial peer address
+                opts.server_port = atoi(optarg);
+                break;
+            case ':':
+                fprintf(stderr, "Option %c requires a value.\n", optopt);
+                return false;
+            default:
+            case '?':
+                fprintf(stderr, "Unknown option %c.\n", optopt);
+                return false;
+        }
+    }
+
+    return true;
+}
+
+int main(int argc, char** argv) {
+    debug = 0;
+    ProgOptions program_options;
+    parse_options(argc, argv, program_options);
+
+    printf("==== Options ====\n");
+    printf("Debug level: %d\n", debug);
+    printf("Bind socket: %d\n", program_options.listen);
+    if(program_options.listen) {
+        char bind_addr_str[INET6_ADDRSTRLEN];
+        inet_ntop(
+                AF_INET6, &(program_options.bind_addr),
+                bind_addr_str, INET6_ADDRSTRLEN);
+        printf("\taddr: %s\n", bind_addr_str);
+        printf("\tport: %d\n", program_options.bind_port);
+    }
+    printf("Start with peer: %d\n", program_options.has_peer);
+    if(program_options.has_peer) {
+        char peer_addr_str[INET6_ADDRSTRLEN];
+        inet_ntop(
+                AF_INET6, &(program_options.server_addr),
+                peer_addr_str, INET6_ADDRSTRLEN);
+        printf("\taddr: %s\n", peer_addr_str);
+        printf("\tport: %d\n", program_options.server_port);
+    }
+    printf("=== END OPTIONS ==\n\n");
+
+    signal(SIGINT, stop_sig_handler);
+
+    try {
+        if(program_options.listen && program_options.has_peer) {
+            fprintf(stderr,
+                    "ERROR: Cannot be a server and a client at the same time "
+                    "-- provide either -l or -s.\n");
+            return 1;
+        }
+        if(program_options.listen) {
+            vpn_instance = new UdpVpnServer(
+                program_options.bind_addr, program_options.bind_port);
+        } else if(program_options.has_peer) {
+            if(program_options.server_port == 0) {
+                fprintf(stderr,
+                        "ERROR: A client instance must be given a server port "
+                        "-- please provide -p.\n");
+                return 1;
+            }
+
+            struct sockaddr_in6 server_addr;
+            server_addr.sin6_family = AF_INET6; memcpy(&server_addr.sin6_addr, &program_options.server_addr,
+                    sizeof(server_addr.sin6_addr));
+            server_addr.sin6_port = htons(program_options.server_port);
+
+            vpn_instance = new UdpVpnClient(server_addr);
+        } else {
+            fprintf(stderr,
+                    "ERROR: Must be either a server or a client "
+                    "-- provide either -l or -s.\n");
+            return 1;
+        }
+
+        printf("Starting to listen...\n");
+        vpn_instance->run();
+
+        delete vpn_instance;
+
+        printf("Shutting down.\n");
+    } catch(const TunDevice::InitializationError& exn) {
+        fprintf(stderr, "TUN INIT ERROR: %s\n", exn.what());
+    } catch(const TunDevice::NetError& exn) {
+        fprintf(stderr, "TUN NET ERROR: %s\n", exn.what());
+    } catch(const UdpVpn::InitializationError& exn) {
+        fprintf(stderr, "VPN INIT ERROR: %s\n", exn.what());
+    } catch(const UdpVpn::NetError& exn) {
+        fprintf(stderr, "VPN NET ERROR: %s\n", exn.what());
+    }
+
+    return 0;
+}

util.cpp

@@ -0,0 +1,71 @@
+#include <cstdio>
+#include <string.h>
+#include <stdarg.h>
+#include <arpa/inet.h>
+
+#include "util.hpp"
+
+int debug;
+
+void do_debugf(int level, const char *format, ...)
+{
+    va_list args;
+    va_start(args, format);
+    if(debug >= level) {
+        vfprintf(stderr, format, args);
+        fflush(stderr);
+    }
+    va_end(args);
+}
+
+const char *
+format_address(const unsigned char *address)
+{
+    static char buf[4][INET6_ADDRSTRLEN];
+    static int i = 0;
+    i = (i + 1) % 4;
+    /*
+    if(v4mapped(address))
+        inet_ntop(AF_INET, address + 12, buf[i], INET6_ADDRSTRLEN);
+    else
+    */
+    inet_ntop(AF_INET6, address, buf[i], INET6_ADDRSTRLEN);
+    return buf[i];
+}
+
+namespace std {
+size_t hash<in6_addr>::operator() (const in6_addr& addr) const {
+    size_t out_hash = 0;
+    for(int i=0; i < 4; ++i) {
+        uint32_t value;
+        memcpy((unsigned char*)(&value),
+               addr.s6_addr + 4*i,
+               4);
+        out_hash ^= (std::hash<uint32_t>{}(value) << 1);
+    }
+    return out_hash;
+}
+
+bool equal_to<in6_addr>::operator()(
+        const in6_addr& lhs, const in6_addr& rhs) const
+{
+    return memcmp(lhs.s6_addr, rhs.s6_addr, sizeof(lhs.s6_addr)) == 0;
+}
+}
+
+MsgException::MsgException(const std::string& msg, int code, bool is_perror)
+    : _msg(msg), _code(code)
+{
+    _what = _msg;
+
+    if(_code != 0) {
+        if(is_perror) {
+            _what += ": ";
+            _what += strerror(errno);
+        }
+
+        char remainder[20];
+        sprintf(remainder, " (code %d)", _code);
+        _what += remainder;
+    }
+}

util.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <exception>
+#include <string>
+#include <netinet/in.h>
+
+/* Debugging -- taken from babeld */
+
+extern int debug;
+
+#if defined(__GNUC__) && (__GNUC__ >= 3)
+#define ATTRIBUTE(x) __attribute__ (x)
+#define LIKELY(_x) __builtin_expect(!!(_x), 1)
+#define UNLIKELY(_x) __builtin_expect(!!(_x), 0)
+#else
+#define ATTRIBUTE(x) /**/
+#define LIKELY(_x) !!(_x)
+#define UNLIKELY(_x) !!(_x)
+#endif
+
+#if defined __STDC_VERSION__ && __STDC_VERSION__ >= 199901L
+#define debugf(...) \
+    do { \
+        if(UNLIKELY(debug >= 2)) do_debugf(2, __VA_ARGS__);     \
+    } while(0)
+#define kdebugf(...) \
+    do { \
+        if(UNLIKELY(debug >= 3)) do_debugf(3, __VA_ARGS__);     \
+    } while(0)
+#elif defined __GNUC__
+#define debugf(_args...) \
+    do { \
+        if(UNLIKELY(debug >= 2)) do_debugf(2, _args);   \
+    } while(0)
+#define kdebugf(_args...) \
+    do { \
+        if(UNLIKELY(debug >= 3)) do_debugf(3, _args);   \
+    } while(0)
+#else
+static inline void debugf(const char *format, ...) { return; }
+static inline void kdebugf(const char *format, ...) { return; }
+#endif
+
+void do_debugf(int level, const char *format, ...);
+
+
+/** format_address -- taken from babeld */
+const char* format_address(const unsigned char* address);
+
+
+/** in6_addr hash & equality */
+namespace std {
+    template<>
+    class hash<in6_addr> {
+        public:
+            size_t operator()(const in6_addr& addr) const;
+    };
+
+    template<>
+    class equal_to<in6_addr> {
+        public:
+            bool operator()(const in6_addr& lhs, const in6_addr& rhs) const;
+    };
+}
+
+
+
+/** MsgException -- an exception bearing a passed explanation message
+ *
+ * If `is_perror` is true, then the `strerror` corresponding message is appened
+ * to the message in `what()`.
+ */
+class MsgException : public std::exception {
+    public:
+        MsgException(const std::string& msg, int code=0, bool is_perror=false);
+        int errcode() const noexcept { return _code; }
+        const char* what() const noexcept { return _what.c_str(); };
+
+    private:
+        std::string _msg;
+        int _code;
+        std::string _what;
+};