h804tun/tundev.cpp

#include <arpa/inet.h>
#include <fcntl.h>
#include <linux/if.h>
#include <linux/if_tun.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cstring>
#include <sstream>
#include <vector>

#include "common.h"
#include "tundev.h"

using h804::check;

namespace h804::tun {

class _autoclose {
    int fd;
    bool do_close;

   public:
    _autoclose(int fd) : fd(fd), do_close(true) {}
    void defuse() { do_close = false; }
    ~_autoclose() {
        if (do_close) {
            close(fd);
        }
    }
};

TunnelDevice::TunnelDevice(const std::string& local_ip, const std::string& local_netmask) {
    // Create tunnel
    ifreq ifr = {};

    check((tun_fd = open("/dev/net/tun", O_RDWR)), "Failed to open /dev/net/tun");

    _autoclose guard(tun_fd);

    ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
    strncpy(ifr.ifr_name, "tun%d", IFNAMSIZ);

    check(ioctl(tun_fd, TUNSETIFF, &ifr), "Failed to request tunnel");

    name = std::string(ifr.ifr_name);

    info << "Tunnel name: " + name;

    // Configure tunnel
    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    check(sockfd, "Failed to create socket");

    _autoclose sockguard(sockfd);

    ifr = {};
    strncpy(ifr.ifr_name, name.c_str(), IFNAMSIZ);
    // Set MTU
    ifr.ifr_mtu = TUN_MTU;
    check(ioctl(sockfd, SIOCSIFMTU, &ifr), "Failed to set MTU");
    // Set IP address
    ifr.ifr_addr.sa_family = AF_INET;
    inet_pton(AF_INET, local_ip.c_str(), ifr.ifr_addr.sa_data + 2);
    check(ioctl(sockfd, SIOCSIFADDR, &ifr), "Failed to set IP address");

    // Set netmask
    inet_pton(AF_INET, local_netmask.c_str(), ifr.ifr_addr.sa_data + 2);
    check(ioctl(sockfd, SIOCSIFNETMASK, &ifr), "Failed to set netmask");

    // Bring up
    check(ioctl(sockfd, SIOCGIFFLAGS, &ifr), "Failed to get iface flags");

    strncpy(ifr.ifr_name, name.c_str(), IFNAMSIZ);
    ifr.ifr_flags |= IFF_UP | IFF_NOARP | IFF_MULTICAST;
    check(ioctl(sockfd, SIOCSIFFLAGS, &ifr), "Failed to bring interface up");

    info << "Tunnel device up";
    guard.defuse();
}

void TunnelDevice::write(const std::array<uint8_t, TUN_MTU>& buf, size_t len) const {
    ssize_t bytes_written = ::write(tun_fd, buf.data(), len);
    if (bytes_written < 0 && errno == EINVAL) {
        warn << "Tunnel rejected packet";
    } else {
        check(bytes_written, "Write failure");
        if (bytes_written != (ssize_t)len) {
            throw std::runtime_error("Did not write all bytes");
        }
    }
}

size_t TunnelDevice::read(std::array<uint8_t, TUN_MTU>& buf) const {
    ssize_t read_bytes = ::read(tun_fd, buf.data(), TUN_MTU);
    check(read_bytes, "Read failure");
    return read_bytes;
}

const std::string& TunnelDevice::get_name() const { return name; }

TunnelDevice::~TunnelDevice() {
    debug << "Closing tunnel device";
    close(tun_fd);
}

// macaddr_t

macaddr_t::macaddr_t() {
    for (size_t i = 0; i < 6; i++) {
        this->operator[](i) = 0;
    }
}

macaddr_t::macaddr_t(const std::string& str) {
    int data[6];
    constexpr auto fmt = "%x:%x:%x:%x:%x:%x";
    int num = sscanf(str.c_str(), fmt, &data[0], &data[1], &data[2], &data[3], &data[4], &data[5]);
    if (num < 6) {
        throw std::invalid_argument("Not a mac address");
    }
    for (size_t i = 0; i < 6; i++) {
        this->operator[](i) = data[i];
    }
}

macaddr_t::macaddr_t(const uint8_t octets[6]) {
    for (size_t i = 0; i < 6; i++) {
        this->operator[](i) = octets[i];
    }
}

macaddr_t::macaddr_t(const sockaddr_ll& linux_addr) : macaddr_t(&linux_addr.sll_addr[0]) {}

sockaddr_ll macaddr_t::to_linux(int if_idx) const {
    sockaddr_ll ret = {};
    ret.sll_ifindex = if_idx;
    ret.sll_halen = ETH_ALEN;
    for (size_t i = 0; i < 6; i++) {
        ret.sll_addr[i] = this->operator[](i);
    }
    return ret;
}

std::string macaddr_t::to_string() const {
    std::ostringstream str;
    str << std::hex << ((int)this->operator[](0));
    for (size_t i = 1; i < 6; i++) {
        str << ":" << std::hex << ((int)this->operator[](i));
    }
    return str.str();
}

macaddr_t::operator std::string() const { return this->to_string(); }

bool macaddr_t::operator==(const macaddr_t& other) const {
    for (size_t i = 0; i < 6; i++) {
        if (this->operator[](i) != other[i]) {
            return false;
        }
    }
    return true;
}

// EthSocket

EthSocket::EthSocket(const std::string& if_name) : source_mac() {
    info << "Starting packet interface";
    eth_fd = socket(AF_PACKET, SOCK_RAW, ETH_P_802_EX1);
    check(eth_fd, "Failed to open socket");

    _autoclose guard(eth_fd);

    ifreq req = {};
    strncpy(req.ifr_name, if_name.c_str(), IFNAMSIZ);
    check(ioctl(eth_fd, SIOCGIFINDEX, &req), "Failed to get interface index");

    if_index = req.ifr_ifindex;
    req = {};
    strncpy(req.ifr_name, if_name.c_str(), IFNAMSIZ);
    check(ioctl(eth_fd, SIOCGIFHWADDR, &req), "Failed to get machine mac address");

    for (size_t i = 0; i < 6; i++) {
        source_mac[i] = req.ifr_hwaddr.sa_data[i];
    }
    info << "Packet interface up";
    info << "MAC: " + source_mac.to_string();

    guard.defuse();
}

typedef std::array<uint8_t, TUN_MTU> buf_t;

void EthSocket::write(const buf_t& buf, size_t len, const macaddr_t& to) const {
    if (len > TUN_MTU) {
        warn << "Packet over MTU";
    }
    std::vector<uint8_t> packet(len + sizeof(ethhdr));
    ethhdr hdr = {};
    for (size_t i = 0; i < 6; i++) {
        hdr.h_dest[i] = to[i];
        hdr.h_source[i] = source_mac[i];
    }
    hdr.h_proto = ETH_TYPE;
    memcpy(packet.data(), &hdr, sizeof(hdr));
    memcpy(&(packet.data())[sizeof(hdr)], buf.data(), len);
    sockaddr_ll dest_addr = to.to_linux(if_index);
    constexpr auto el = sizeof(ethhdr);
    constexpr auto sl = sizeof(sockaddr_ll);
    auto ret = sendto(eth_fd, packet.data(), len + el, 0, (sockaddr*)&dest_addr, sl);
    check(ret, "sendto() failed");
}

size_t EthSocket::read(std::array<uint8_t, TUN_MTU>& buf, macaddr_t& from_mac) const {
    static std::array<uint8_t, TUN_MTU + sizeof(ethhdr)> raw_buf;
    auto ret = recvfrom(eth_fd, raw_buf.data(), TUN_MTU + sizeof(ethhdr), 0, NULL, 0);
    check(ret, "recvfrom() failed");

    if (ret < (ssize_t)sizeof(ethhdr)) {
        error << "Invalid ethernet header recieved";
    }

    const ethhdr* hdr = reinterpret_cast<const ethhdr*>(raw_buf.data());

    for (size_t i = 0; i < 6; i++) {
        from_mac[i] = hdr->h_source[i];
    }

    std::copy(raw_buf.begin() + sizeof(ethhdr), raw_buf.end(), buf.begin());

    return ret - sizeof(ethhdr);
}

const macaddr_t& EthSocket::get_source_mac() const {
    return source_mac;
}

EthSocket::~EthSocket() {
    debug << "Closing packet interface";
    close(eth_fd);
}
}  // namespace h804::tun