NFQueue.cpp

Go to the documentation of this file.

#include "bsc/netfilter/NFQueue.hpp"
#include "bsc/logging/Logger.hpp"
 
#if defined(BSC_HAS_NFQ)
#include <arpa/inet.h>
#include <cerrno>
#include <cstring>
#include <linux/netfilter.h>
#include <linux/netfilter/nfnetlink_queue.h>
#include <linux/types.h>
#include <libnetfilter_queue/libnetfilter_queue.h>
#include <memory>
#include <netinet/ip.h>
#include <netinet/ip6.h>
#include <netinet/tcp.h>
#include <netinet/udp.h>
#include <sys/socket.h>
#include <thread>
#include <unistd.h>
 
namespace bsc::netfilter {
namespace {
constexpr int kAF_INET = 2;
constexpr int kAF_INET6 = 10;
 
struct ParsedPacket {
    bsc::model::Connection conn;
    bool ok{false};
};
 
std::string to_ipv4(std::uint32_t ip) {
    in_addr a{};
    a.s_addr = ip;
    char buf[INET_ADDRSTRLEN]{};
    inet_ntop(AF_INET, &a, buf, sizeof(buf));
    return buf;
}
 
std::string to_ipv6(const in6_addr& addr) {
    char buf[INET6_ADDRSTRLEN]{};
    inet_ntop(AF_INET6, &addr, buf, sizeof(buf));
    return buf;
}
 
ParsedPacket parse_payload(unsigned char* data, int len) {
    ParsedPacket out;
    if (!data || len < 1) return out;
 
    const auto version = static_cast<std::uint8_t>(data[0] >> 4);
    if (version == 4) {
        if (len < static_cast<int>(sizeof(iphdr))) return out;
        const auto* ip = reinterpret_cast<const iphdr*>(data);
        const auto ihl = static_cast<int>(ip->ihl) * 4;
        if (len < ihl) return out;
        out.conn.src_ip = to_ipv4(ip->saddr);
        out.conn.dst_ip = to_ipv4(ip->daddr);
        if (ip->protocol == IPPROTO_TCP) {
            if (len < ihl + static_cast<int>(sizeof(tcphdr))) return out;
            const auto* tcp = reinterpret_cast<const tcphdr*>(data + ihl);
            out.conn.protocol = "tcp";
            out.conn.src_port = ntohs(tcp->source);
            out.conn.dst_port = ntohs(tcp->dest);
        } else if (ip->protocol == IPPROTO_UDP) {
            if (len < ihl + static_cast<int>(sizeof(udphdr))) return out;
            const auto* udp = reinterpret_cast<const udphdr*>(data + ihl);
            out.conn.protocol = "udp";
            out.conn.src_port = ntohs(udp->source);
            out.conn.dst_port = ntohs(udp->dest);
        } else {
            return out;
        }
        out.ok = true;
        return out;
    }
 
    if (version == 6) {
        if (len < static_cast<int>(sizeof(ip6_hdr))) return out;
        const auto* ip6 = reinterpret_cast<const ip6_hdr*>(data);
        out.conn.src_ip = to_ipv6(ip6->ip6_src);
        out.conn.dst_ip = to_ipv6(ip6->ip6_dst);
        const auto nexthdr = ip6->ip6_nxt;
        const auto offset = static_cast<int>(sizeof(ip6_hdr));
        if (nexthdr == IPPROTO_TCP) {
            if (len < offset + static_cast<int>(sizeof(tcphdr))) return out;
            const auto* tcp = reinterpret_cast<const tcphdr*>(data + offset);
            out.conn.protocol = "tcp6";
            out.conn.src_port = ntohs(tcp->source);
            out.conn.dst_port = ntohs(tcp->dest);
        } else if (nexthdr == IPPROTO_UDP) {
            if (len < offset + static_cast<int>(sizeof(udphdr))) return out;
            const auto* udp = reinterpret_cast<const udphdr*>(data + offset);
            out.conn.protocol = "udp6";
            out.conn.src_port = ntohs(udp->source);
            out.conn.dst_port = ntohs(udp->dest);
        } else {
            return out;
        }
        out.ok = true;
    }
    return out;
}
 
class QueueOwner;
static int nfq_callback(nfq_q_handle* qh, nfgenmsg*, nfq_data* nfa, void* data);
 
class QueueOwner {
  public:
    std::uint16_t queue_id{};
    NFQueue::Handler handler;
    nfq_handle* handle{nullptr};
    nfq_q_handle* queue{nullptr};
    int fd{-1};
    std::thread thread;
    std::atomic_bool running{false};
 
    ~QueueOwner() { stop(); }
 
    bool start() {
        handle = nfq_open();
        if (!handle) return false;
        if (nfq_unbind_pf(handle, kAF_INET) < 0 && errno != ENOENT) {
            bsc::logging::Logger::instance().warning("nfq_unbind_pf(AF_INET) failed");
        }
        if (nfq_unbind_pf(handle, kAF_INET6) < 0 && errno != ENOENT) {
            bsc::logging::Logger::instance().warning("nfq_unbind_pf(AF_INET6) failed");
        }
        if (nfq_bind_pf(handle, kAF_INET) < 0 || nfq_bind_pf(handle, kAF_INET6) < 0) {
            stop();
            return false;
        }
        queue = nfq_create_queue(handle, queue_id, &nfq_callback, this);
        if (!queue) {
            stop();
            return false;
        }
        if (nfq_set_mode(queue, NFQNL_COPY_PACKET, 0xffff) < 0) {
            stop();
            return false;
        }
        nfq_set_queue_maxlen(queue, 4096);
#ifdef NFQA_CFG_F_UID_GID
        nfq_set_queue_flags(queue, NFQA_CFG_F_UID_GID, NFQA_CFG_F_UID_GID);
#endif
        fd = nfq_fd(handle);
        running = true;
        thread = std::thread([this]() {
            char buf[8192] __attribute__((aligned));
            while (running) {
                const int rv = recv(fd, buf, sizeof(buf), 0);
                if (rv >= 0) {
                    nfq_handle_packet(handle, buf, rv);
                    continue;
                }
                if (errno == EINTR) continue;
                if (running) {
                    bsc::logging::Logger::instance().warning(std::string("nfqueue recv failed: ") + std::strerror(errno));
                }
                break;
            }
        });
        return true;
    }
 
    void stop() {
        running = false;
        if (fd >= 0) {
            ::shutdown(fd, SHUT_RDWR);
            ::close(fd);
            fd = -1;
        }
        if (thread.joinable()) thread.join();
        if (queue) {
            nfq_destroy_queue(queue);
            queue = nullptr;
        }
        if (handle) {
            nfq_close(handle);
            handle = nullptr;
        }
    }
};
 
static int nfq_callback(nfq_q_handle* qh, nfgenmsg*, nfq_data* nfa, void* data) {
    auto* owner = reinterpret_cast<QueueOwner*>(data);
    if (!owner) return 0;
 
    std::uint32_t id = 0;
    if (auto* ph = nfq_get_msg_packet_hdr(nfa)) id = ntohl(ph->packet_id);
 
    unsigned char* payload = nullptr;
    const int payload_len = nfq_get_payload(nfa, &payload);
    auto parsed = parse_payload(payload, payload_len);
    PacketInfo info;
    info.id = id;
    info.queue_id = owner->queue_id;
    info.mark = nfq_get_nfmark(nfa);
    info.in_dev = nfq_get_indev(nfa);
    info.out_dev = nfq_get_outdev(nfa);
#ifdef NFQA_CFG_F_UID_GID
    std::uint32_t uid = 0xffffffff;
    if (nfq_get_uid(nfa, &uid) >= 0) info.uid = uid;
#endif
    if (parsed.ok) info.connection = std::move(parsed.conn);
    // Attenzione: user_id=0 e' un UID valido (root). Usiamo il flag letto da
    // nfq_get_uid (info.uid e' 0xffffffff quando non disponibile).
    if (info.uid != 0xffffffff && info.connection.user_id == 0xffffffff) info.connection.user_id = info.uid;
 
    Decision decision;
    if (owner->handler) decision = owner->handler(info);
    return nfq_set_verdict2(qh, id, static_cast<std::uint32_t>(decision.verdict), decision.set_mark ? decision.mark : 0, 0, nullptr);
}
} // namespace
 
class NFQueue::Impl {
  public:
    std::unique_ptr<QueueOwner> owner;
};
 
NFQueue::NFQueue() : impl_(std::make_unique<Impl>()) {}
NFQueue::~NFQueue() { close(); }
 
bool NFQueue::open(std::uint16_t queue_id, Handler handler) {
    close();
    impl_->owner = std::make_unique<QueueOwner>();
    impl_->owner->queue_id = queue_id;
    impl_->owner->handler = std::move(handler);
    running_ = impl_->owner->start();
    return running_;
}
 
void NFQueue::close() {
    running_ = false;
    if (impl_ && impl_->owner) {
        impl_->owner->stop();
        impl_->owner.reset();
    }
}
 
bool available() { return true; }
 
} // namespace bsc::netfilter
 
#else
 
namespace bsc::netfilter {
class NFQueue::Impl {};
NFQueue::NFQueue() : impl_(std::make_unique<Impl>()) {}
NFQueue::~NFQueue() = default;
bool NFQueue::open(std::uint16_t, Handler) { return false; }
void NFQueue::close() {}
bool available() { return false; }
} // namespace bsc::netfilter
 
#endif