#include <errno.h>
#include <fcntl.h>
#include <inttypes.h>
#include <netdb.h>
#include <poll.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <syslog.h>
#include <time.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/icmp6.h>
#include <sys/resource.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>

#include <dnet.h>
#include <pcap.h>

#define DEFAULT_CFFILE "/etc/matd.cf"
#define DEFAULT_NTHREADS 0
#define DEFAULT_STATFILE "/dev/null"
#define SYSLOG_FACILITY LOG_USER
#define UMASK 022
#define PCAP_SNAPLEN 65535
#define PCAP_TIMEOUT 10
#define HASH_FACTOR 31 /* 33 */

#ifndef FALSE
#define FALSE 0
#endif
#ifndef TRUE
#define TRUE !FALSE
#endif

#if !defined SIGPOLL && defined SIGIO
#define SIGPOLL SIGIO
#endif
#if !defined CLOCK_HIGHRES && defined CLOCK_MONOTONIC
#define CLOCK_HIGHRES CLOCK_MONOTONIC
#endif

#if defined __FreeBSD__ && __FreeBSD_version < 602000
#define PCAP_D_IN D_IN
#elif defined __sun && defined __SVR4
#define pcap_setdirection(p, d) 0
#endif

#define _pthread_abort_on_error(func, ...) do { \
    if (pthread_ ## func(__VA_ARGS__)) \
        abort(); \
} while(FALSE)

#define pthread_cond_signal(cond)      _pthread_abort_on_error(cond_signal, cond)
#define pthread_cond_broadcast(cond)   _pthread_abort_on_error(cond_broadcast, cond)
#define pthread_cond_wait(cond, mutex) _pthread_abort_on_error(cond_wait, cond, mutex)

#define pthread_mutex_lock(mutex)   _pthread_abort_on_error(mutex_lock, mutex)
#define pthread_mutex_unlock(mutex) _pthread_abort_on_error(mutex_unlock, mutex)

#define arraylen(a) (sizeof a / sizeof *a)

#define syslogx(priority, ...) do { \
    sigset_t oset; \
    sigprocmask(SIG_SETMASK, &filledsigset, &oset); \
    syslog(priority, __VA_ARGS__); \
    sigprocmask(SIG_SETMASK, &oset, NULL); \
} while(FALSE)

typedef struct {
    unsigned weight;
    size_t npackets;
    eth_addr_t ifaddr;
} target_t;

typedef struct {
    const char *argv0, *cffile, *statfile;
    int proto, isforeground:1, ispktpassing:1;
    volatile int isterminate:1;
    unsigned sum_weights;
    size_t nthreads, ntargets, nthr_idle;
    target_t *targets;
    struct addrinfo *addrinfo;
    pthread_cond_t *pkt_cond, *thr_cond;
    pthread_mutex_t *mutex;
    pcap_t *pcap;
    eth_t *eth;
    eth_addr_t ifaddr;
    struct timespec starttime, configtime;
    struct eth_hdr *eth_hdr;
    struct pcap_pkthdr pcap_pkthdr;
#ifdef IS_FW_BUG_FIXED
    struct fw_rule fw_rule;
#endif
} config_t;

extern char *optarg;
extern int optind, opterr, optopt;

static volatile int isreconfigure = FALSE, isterminate = FALSE, isdumpstat = FALSE;
static sigset_t filledsigset;

static const int
    ignoresignals[] = {
        SIGPIPE, SIGALRM, SIGUSR2, SIGPOLL,
        SIGVTALRM, SIGPROF
    },
    coredumpsignals[] = {
        SIGQUIT, SIGILL, SIGTRAP, SIGABRT,
        SIGEMT, SIGFPE, SIGBUS, SIGSEGV,
        SIGSYS, SIGXCPU, SIGXFSZ
    };

void printusage(const config_t *config)
{
    fprintf(stderr, "usage: %s [-Fh] [-f cffile] [-n nthreads] [-s statfile]\n"
                    "\t-f cffile: configuration file [%s]\n"
                    "\t-F: run in foreground\n"
                    "\t-h: this help\n"
                    "\t-n nthreads: number of worker threads [%u]\n"
                    "\t-s statfile: dump statistics to statfile on SIGUSR1 [%s]\n",
            config->argv0, DEFAULT_CFFILE, DEFAULT_NTHREADS, DEFAULT_STATFILE);
}

void sighandler(int sig)
{
    switch (sig) {
    case SIGHUP:
        syslogx(LOG_INFO, "Received %s. Reconfiguring...", strsignal(sig));
        isreconfigure = TRUE;
        break;
    case SIGINT:
    case SIGTERM:
        syslogx(LOG_INFO, "Received %s. Terminating...", strsignal(sig));
        isterminate = TRUE;
        break;
    case SIGUSR1:
        syslogx(LOG_INFO, "Received %s. Dumping statistics...", strsignal(sig));
        isdumpstat = TRUE;
        break;
    default:
        syslogx(LOG_CRIT, "%s occurred. Core dumping to /tmp...", strsignal(sig));
        chdir("/tmp");
        kill(getpid(), sig);
    }
}

int initialize(config_t *config, int argc, char *const *argv)
{
    static pthread_cond_t pkt_cond = PTHREAD_COND_INITIALIZER, thr_cond = PTHREAD_COND_INITIALIZER;
    static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
    char *p;
    int i;
    struct sigaction sa;
    struct stat st;

    if (clock_gettime(CLOCK_HIGHRES, &config->starttime)) {
        perror("error: clock_gettime");
        return errno;
    }

    config->argv0 = (p = strrchr(*argv, '/')) ? ++p : *argv;
    config->pkt_cond = &pkt_cond;
    config->thr_cond = &thr_cond;
    config->mutex = &mutex;
    config->nthr_idle = 0;
    config->ispktpassing = FALSE;

    config->isforeground = FALSE;
    config->cffile = DEFAULT_CFFILE;
    config->nthreads = DEFAULT_NTHREADS;
    config->statfile = DEFAULT_STATFILE;

    while ((i = getopt(argc, argv, "f:Fn:s:h")) != -1)
        switch(i) {
        case 'f':
            if (*optarg != '/') {
                fprintf(stderr, "-%c: %s: Absolute path required.\n", i, optarg);
                return EINVAL;
            }
            else if (stat(optarg, &st) || (S_ISDIR(st.st_mode) && (errno = EISDIR))) {
                fprintf(stderr, "-%c: %s: %s\n", i, optarg, strerror(errno));
                return errno;
            }
            config->cffile = optarg;
            break;
        case 'F':
            config->isforeground = TRUE;
            break;
        case 'n':
            config->nthreads = strtoul(optarg, NULL, 10);
            break;
        case 's':
            if (*optarg != '/') {
                fprintf(stderr, "-%c: %s: Absolute path required.\n", i, optarg);
                return EINVAL;
            }
            else if (!stat(optarg, &st) && S_ISDIR(st.st_mode) && (errno = EISDIR)) {
                fprintf(stderr, "-%c: %s: %s\n", i, optarg, strerror(errno));
                return errno;
            }
            config->statfile = optarg;
            break;
        case 'h':
        case '?':
            printusage(config);
            return -1;
        }

    sigfillset(&filledsigset);
    openlog(config->argv0, LOG_PID, SYSLOG_FACILITY);
    umask(UMASK);

    sa.sa_handler = SIG_IGN;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = 0;
    for (i = 0; i < arraylen(ignoresignals); i++)
        sigaction(ignoresignals[i], &sa, NULL);

    sa.sa_handler = sighandler;
    sigaction(SIGHUP, &sa, NULL);
    sigaction(SIGINT, &sa, NULL);
    sigaction(SIGTERM, &sa, NULL);
    sigaction(SIGUSR1, &sa, NULL);

    for (i = 0; i < arraylen(coredumpsignals); i++)
        sigaddset(&sa.sa_mask, coredumpsignals[i]);
    sa.sa_flags = SA_RESETHAND|SA_RESTART;
    for (i = 0; i < arraylen(coredumpsignals); i++)
        sigaction(coredumpsignals[i], &sa, NULL);

    return 0;
}

int daemonize(void)
{
    switch (fork()) {
    case -1:
        perror("error: fork");
        return errno;
    case 0:
        break;
    default:
        _exit(0);
    }

    freopen("/dev/null", "r+", stdin);
    freopen("/dev/null", "r+", stdout);
    freopen("/dev/null", "r+", stderr);

    setsid();
    chdir("/");

    return 0;
}

static int getethaddr(const config_t *config, const char *host, eth_addr_t *eth_addr)
{
    int i, sock;
    struct addrinfo *addrinfo, ai_hints;
    struct pollfd pollfd;

    if (!eth_pton(host, eth_addr))
        return 0;

    memset(&ai_hints, 0, sizeof ai_hints);
    ai_hints.ai_family = PF_UNSPEC;
    ai_hints.ai_socktype = SOCK_DGRAM;
    if ((i = getaddrinfo(host, "discard", &ai_hints, &addrinfo))) {
        syslogx(LOG_ERR, "getaddrinfo: %s", i == EAI_SYSTEM ? strerror(errno) : gai_strerror(i));
        return i == EAI_SYSTEM ? errno : i;
    }
    if (addrinfo->ai_family != PF_INET && addrinfo->ai_family != PF_INET6) {
        syslogx(LOG_ERR, "getethaddr: %s", strerror(EINVAL));
        freeaddrinfo(addrinfo);
        return EINVAL;
    }
    if ((sock = addrinfo->ai_family == PF_INET6 ? socket(addrinfo->ai_family, SOCK_RAW, IPPROTO_ICMPV6)
                    : socket(addrinfo->ai_family, addrinfo->ai_socktype, addrinfo->ai_protocol)) == -1) {
        syslogx(LOG_ERR, "socket: %m");
        freeaddrinfo(addrinfo);
        return errno;
    }
    pollfd.fd = sock;
    pollfd.events = POLLRDNORM;
    if (addrinfo->ai_family == PF_INET6) {
        char _req[sizeof(struct nd_neighbor_solicit) + sizeof(struct nd_opt_hdr) + sizeof(eth_addr_t)],
             _res[sizeof(struct nd_neighbor_advert) + sizeof(struct nd_opt_hdr) + sizeof(eth_addr_t)];
        struct icmp6_filter icmp6_filter;
        struct nd_neighbor_solicit *req = (struct nd_neighbor_solicit *)_req;
        struct nd_neighbor_advert *res = (struct nd_neighbor_advert *)_res;
        struct nd_opt_hdr *req_opt = (struct nd_opt_hdr *)(req + 1),
                          *res_opt = (struct nd_opt_hdr *)(res + 1);
        eth_addr_t *req_addr = (eth_addr_t *)(req_opt + 1),
                   *res_addr = (eth_addr_t *)(res_opt + 1);

        ICMP6_FILTER_SETBLOCKALL(&icmp6_filter);
        ICMP6_FILTER_SETPASS(ND_NEIGHBOR_ADVERT, &icmp6_filter);
        if (setsockopt(sock, IPPROTO_ICMPV6, ICMP6_FILTER, &icmp6_filter, sizeof icmp6_filter)) {
            syslogx(LOG_ERR, "setsockopt: %m");
            close(sock);
            freeaddrinfo(addrinfo);
            return errno;
        }
        req->nd_ns_type = ND_NEIGHBOR_SOLICIT;
        req->nd_ns_code = 0;
        req->nd_ns_cksum = 0;
        req->nd_ns_reserved = 0;
        req->nd_ns_target = ((struct sockaddr_in6 *)addrinfo->ai_addr)->sin6_addr;
        req_opt->nd_opt_type = ND_OPT_SOURCE_LINKADDR;
        req_opt->nd_opt_len = sizeof *req_addr;
        *req_addr = config->ifaddr;
        for (i = 0; i < 5; i++) {
            sendto(sock, req, sizeof *req + sizeof *req_opt + sizeof *req_addr, 0, addrinfo->ai_addr, addrinfo->ai_addrlen);
            while (poll(&pollfd, 1, 100) > 0)
                if (recv(sock, res, sizeof *res + sizeof *res_opt + sizeof *res_addr, 0)
                        == sizeof *res + sizeof *res_opt + sizeof *res_addr
                    && res->nd_na_flags_reserved & ND_NA_FLAG_SOLICITED
                    && !memcmp(&res->nd_na_target, &req->nd_ns_target, sizeof res->nd_na_target)
                    && res_opt->nd_opt_type == ND_OPT_TARGET_LINKADDR
                    && res_opt->nd_opt_len == sizeof *res_addr) {
                    *eth_addr = *res_addr;
                    pollfd.fd = -1;
                    i = 5;
                    break;
                }
        }
    }
    else {
        arp_t *arp;
        struct arp_entry arp_entry;

        if (!(arp = arp_open())) {
            syslogx(LOG_ERR, "arp_open: %m");
            close(sock);
            freeaddrinfo(addrinfo);
            return errno;
        }
        arp_entry.arp_pa.addr_type = ADDR_TYPE_IP;
        arp_entry.arp_pa.addr_ip = *(ip_addr_t *)&((struct sockaddr_in *)addrinfo->ai_addr)->sin_addr;
        if (arp_get(arp, &arp_entry)) {
            sendto(sock, NULL, 0, 0, addrinfo->ai_addr, addrinfo->ai_addrlen);
            if (arp_get(arp, &arp_entry)) {
                syslogx(LOG_ERR, "arp_get: %m");
                arp_close(arp);
                close(sock);
                freeaddrinfo(addrinfo);
                return errno;
            }
        }
        arp_close(arp);
        *eth_addr = arp_entry.arp_ha.addr_eth;
        pollfd.fd = -1;
    }
    close(sock);
    freeaddrinfo(addrinfo);

    return pollfd.fd != -1 ? EINVAL : 0;
}

int readconfig(config_t *config)
{
    static const char bpf_codefmt[] = "dst host %s and %s dst port %s";
    char *p, *endp, *eol, *buf, *host, *port, *ifname, *targets,
         *bpf_code, pcap_errmsg[PCAP_ERRBUF_SIZE];
    int i;
#ifdef IS_FW_BUG_FIXED
    fw_t *fw;
#endif
    intf_t *intf;
    pcap_if_t *pcap_if, *pcap_ifs;
    struct addrinfo ai_hints;
    struct stat st;
    struct bpf_program bpf_program;
    struct intf_entry intf_entry;

    buf = NULL;
    if ((i = open(config->cffile, O_RDONLY)) == -1
        || fstat(i, &st)
        || !(buf = malloc(st.st_size + 1))
        || read(i, buf, st.st_size) != st.st_size) {
        syslogx(LOG_ERR, "readconfig: %m");
        free(buf);
        if (i != -1)
            close(i);
        return errno;
    }
    close(i);
    *(endp = buf + st.st_size) = 0;

    config->proto = IPPROTO_IP;
    host = port = ifname = targets = NULL;
    for (p = buf; p < endp; p = ++eol) {
        (eol = strchr(p, '\n')) || (eol = endp);
        *eol = 0;
        if (!*p || *p == '#')
            continue;
        else if (!strncmp(p, "proto=", arraylen("proto=") - 1))
            config->proto =  !strcmp(p + arraylen("proto=") - 1, "tcp") ? IPPROTO_TCP
                           : !strcmp(p + arraylen("proto=") - 1, "udp") ? IPPROTO_UDP : IPPROTO_IP;
        else if (!strncmp(p, "listen=", arraylen("listen=") - 1)) {
            host = p + arraylen("listen=") - 1;
            (port = strrchr(host, ':')) && (*port++ = 0);
        }
        else if (!strncmp(p, "output_if=", arraylen("output_if=") - 1))
            ifname = p + arraylen("output_if=") - 1;
        else if (!strncmp(p, "targets=", arraylen("targets=") - 1))
            targets = p + arraylen("targets=") - 1;
        else {
            syslogx(LOG_ERR, "readconfig: %s", strerror(EINVAL));
            free(buf);
            return EINVAL;
        }
    }
    if (config->proto == IPPROTO_IP || !host || !port || !ifname || !targets) {
        syslogx(LOG_ERR, "readconfig: %s", strerror(EINVAL));
        free(buf);
        return EINVAL;
    }

    memset(&ai_hints, 0, sizeof ai_hints);
    ai_hints.ai_family = PF_UNSPEC;
    ai_hints.ai_socktype = config->proto == IPPROTO_TCP ? SOCK_STREAM : SOCK_DGRAM;
    if ((i = getaddrinfo(host, port, &ai_hints, &config->addrinfo))) {
        syslogx(LOG_ERR, "getaddrinfo: %s", i == EAI_SYSTEM ? strerror(errno) : gai_strerror(i));
        free(buf);
        return i == EAI_SYSTEM ? errno : i;
    }
    if (config->addrinfo->ai_family != PF_INET && config->addrinfo->ai_family != PF_INET6) {
        syslogx(LOG_ERR, "readconfig: %s", strerror(EINVAL));
        freeaddrinfo(config->addrinfo);
        free(buf);
        return EINVAL;
    }

    if (pcap_findalldevs(&pcap_ifs, pcap_errmsg)) {
        syslogx(LOG_ERR, "pcap_findalldevs: %s", pcap_errmsg);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }
    for (pcap_if = pcap_ifs; pcap_if; pcap_if = pcap_if->next) {
        pcap_addr_t *pcap_addr;

        for (pcap_addr = pcap_if->addresses; pcap_addr; pcap_addr = pcap_addr->next)
            if (pcap_addr->addr->sa_family == config->addrinfo->ai_addr->sa_family
                && ((pcap_addr->addr->sa_family == AF_INET6
                     && !memcmp(&((struct sockaddr_in6 *)pcap_addr->addr)->sin6_addr,
                                &((struct sockaddr_in6 *)config->addrinfo->ai_addr)->sin6_addr,
                                sizeof ((struct sockaddr_in6 *)pcap_addr->addr)->sin6_addr))
                    || (pcap_addr->addr->sa_family == AF_INET
                        && !memcmp(&((struct sockaddr_in *)pcap_addr->addr)->sin_addr,
                                   &((struct sockaddr_in *)config->addrinfo->ai_addr)->sin_addr,
                                   sizeof ((struct sockaddr_in *)pcap_addr->addr)->sin_addr))))
                break;
        if (pcap_addr)
            break;
    }
    if (!pcap_if) {
        syslogx(LOG_ERR, "pcap_findalldevs: %s", strerror(ENODEV));
        pcap_freealldevs(pcap_ifs);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return ENODEV;
    }
    if (!(config->pcap = pcap_open_live(pcap_if->name, PCAP_SNAPLEN, FALSE, PCAP_TIMEOUT, pcap_errmsg))) {
        syslogx(LOG_ERR, "pcap_open_live: %s", pcap_errmsg);
        pcap_freealldevs(pcap_ifs);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }
#ifdef IS_FW_BUG_FIXED
    strlcpy(config->fw_rule.fw_device, pcap_if->name, sizeof config->fw_rule.fw_device);
#endif
    pcap_freealldevs(pcap_ifs);
#ifdef IS_FW_BUG_FIXED
    config->fw_rule.fw_op = FW_OP_BLOCK;
    config->fw_rule.fw_dir = FW_DIR_IN;
    config->fw_rule.fw_proto = config->proto;
    *config->fw_rule.fw_sport = config->fw_rule.fw_sport[1] = 0;
    if (config->addrinfo->ai_addr->sa_family == AF_INET6) {
        config->fw_rule.fw_src.addr_type = config->fw_rule.fw_dst.addr_type = ADDR_TYPE_IP6;
        config->fw_rule.fw_src.addr_bits = config->fw_rule.fw_dst.addr_bits = IP6_ADDR_BITS;
        config->fw_rule.fw_src.addr_ip6 = *(ip6_addr_t *)IP6_ADDR_UNSPEC;
        config->fw_rule.fw_dst.addr_ip6 = *(ip6_addr_t *)&((struct sockaddr_in6 *)config->addrinfo->ai_addr)->sin6_addr;
        *config->fw_rule.fw_dport = config->fw_rule.fw_dport[1] = ((struct sockaddr_in6 *)config->addrinfo->ai_addr)->sin6_port;
    }
    else {
        config->fw_rule.fw_src.addr_type = config->fw_rule.fw_dst.addr_type = ADDR_TYPE_IP;
        config->fw_rule.fw_src.addr_bits = config->fw_rule.fw_dst.addr_bits = IP_ADDR_BITS;
        config->fw_rule.fw_src.addr_ip = IP_ADDR_ANY;
        config->fw_rule.fw_dst.addr_ip = *(ip_addr_t *)&((struct sockaddr_in *)config->addrinfo->ai_addr)->sin_addr;
        *config->fw_rule.fw_dport = config->fw_rule.fw_dport[1] = ((struct sockaddr_in *)config->addrinfo->ai_addr)->sin_port;
    }
#endif
    if (!(bpf_code = malloc(sizeof bpf_codefmt - 6 + 3 + strlen(host) + strlen(port)))) {
        syslogx(LOG_ERR, "malloc: %m");
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }
    sprintf(bpf_code, bpf_codefmt, host, config->proto == IPPROTO_TCP ? "tcp" : "udp", port);
    if (pcap_compile(config->pcap, &bpf_program, bpf_code, TRUE, 0)) {
        syslogx(LOG_ERR, "pcap_compile: %s", pcap_geterr(config->pcap));
        free(bpf_code);
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return EINVAL;
    }
    free(bpf_code);
    if (pcap_setfilter(config->pcap, &bpf_program)) {
        syslogx(LOG_ERR, "pcap_setfilter: %s", pcap_geterr(config->pcap));
        pcap_freecode(&bpf_program);
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }
    pcap_freecode(&bpf_program);
    if (pcap_setdirection(config->pcap, PCAP_D_IN))
        syslogx(LOG_WARNING, "pcap_setdirection: %s", pcap_geterr(config->pcap));

    if (!(intf = intf_open())) {
        syslogx(LOG_ERR, "intf_open: %m");
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }
    intf_entry.intf_len = sizeof intf_entry;
    strlcpy(intf_entry.intf_name, ifname, sizeof intf_entry.intf_name);
    if (intf_get(intf, &intf_entry)) {
        syslogx(LOG_ERR, "intf_get: %s", strerror(ENODEV));
        intf_close(intf);
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return ENODEV;
    }
    intf_close(intf);
    config->ifaddr = intf_entry.intf_link_addr.addr_eth;
    if (!(config->eth = eth_open(ifname))) {
        syslogx(LOG_ERR, "eth_open: %m");
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return errno;
    }

    for (p = targets, config->ntargets = 0, config->targets = NULL, config->sum_weights = 0;
         p && *p; (p = eol) && p++, config->ntargets++) {
        char *weight;

        if (!(config->ntargets % 8)) {
            target_t *_targets;

            if (!(_targets = realloc(config->targets, 8 * (1 + config->ntargets / 8) * sizeof *config->targets))) {
                syslogx(LOG_ERR, "realloc: %m");
                free(config->targets);
                eth_close(config->eth);
                pcap_close(config->pcap);
                freeaddrinfo(config->addrinfo);
                free(buf);
                return errno;
            }
            config->targets = _targets;
        }
        (eol = strchr(p, ',')) && (*eol = 0);
        (weight = strchr(p, '=')) && (*weight++ = 0);
        if (getethaddr(config, p, &config->targets[config->ntargets].ifaddr)) {
            syslogx(LOG_ERR, "getethaddr: %s", strerror(EINVAL));
            free(config->targets);
            eth_close(config->eth);
            pcap_close(config->pcap);
            freeaddrinfo(config->addrinfo);
            free(buf);
            return EINVAL;
        }
        config->sum_weights += (config->targets[config->ntargets].weight = weight ? strtoul(weight, NULL, 10) : 1);
        config->targets[config->ntargets].npackets = 0;
    }
    if (!config->ntargets) {
        syslogx(LOG_ERR, "readconfig: %s", strerror(EINVAL));
        free(config->targets);
        eth_close(config->eth);
        pcap_close(config->pcap);
        freeaddrinfo(config->addrinfo);
        free(buf);
        return EINVAL;
    }
    free(buf);

#ifdef IS_FW_BUG_FIXED
    if (!(fw = fw_open()))
        syslogx(LOG_WARNING, "fw_open: %m");
    else {
        if (fw_add(fw, &config->fw_rule))
            syslogx(LOG_WARNING, "fw_add: %m");
        fw_close(fw);
    }
#endif

    if (clock_gettime(CLOCK_HIGHRES, &config->configtime))
        syslogx(LOG_WARNING, "clock_gettime: %m");

    return 0;
}

void freeconfig(config_t *config)
{
#ifdef IS_FW_BUG_FIXED
    fw_t *fw;

    if (!(fw = fw_open()))
        syslogx(LOG_WARNING, "fw_open: %m");
    else {
        if (fw_delete(fw, &config->fw_rule))
            syslogx(LOG_WARNING, "fw_delete: %m");
        fw_close(fw);
    }
#endif

    free(config->targets);
    eth_close(config->eth);
    pcap_close(config->pcap);
    freeaddrinfo(config->addrinfo);
}

#define _time_hour(t) (unsigned long)(t / (60 * 60))
#define _time_min(t)  (unsigned)(t / 60 % 60)
#define _time_sec(t)  (unsigned)(t % 60)

void dumpstat(const config_t *config)
{
    char date[arraylen("Thu, 01 Jan 1970 09:00:00")], zone[5];
    int i;
    FILE *fp;
    struct timespec ts_now, ts_start, ts_config;
    struct rusage ru;
    struct tm tm;

    isdumpstat = FALSE;
    if (clock_gettime(CLOCK_REALTIME, &ts_now) || clock_gettime(CLOCK_HIGHRES, &ts_start)
        || getrusage(RUSAGE_SELF, &ru) || !(fp = fopen(config->statfile, "a"))) {
        syslogx(LOG_ERR, "dumpstat: %m");
        return;
    }
    strftime(date, sizeof date, "%a, %d %b %Y %T", localtime_r(&ts_now.tv_sec, &tm));
    strftime(zone, sizeof zone, "%Z", &tm);
    ts_config = ts_start;
    ts_start.tv_sec -= config->starttime.tv_sec + ((ts_start.tv_nsec -= config->starttime.tv_nsec) < 0 ? (ts_start.tv_nsec += 1000000000, 1) : 0);
    if (!ts_start.tv_sec && ts_start.tv_nsec < 499999)
        ts_start.tv_nsec = 499999; /* to avoid SIGFPE */
    ts_config.tv_sec -= config->configtime.tv_sec + ((ts_config.tv_nsec -= config->configtime.tv_nsec) < 0 ? (ts_config.tv_nsec += 1000000000, 1) : 0);
    if (!ts_config.tv_sec && ts_config.tv_nsec < 499999)
        ts_config.tv_nsec = 499999; /* to avoid SIGFPE */

    fprintf(fp, "[%s statistics] %s.%.3u (%s)\n"
                "user CPU time = %lu:%.2u:%.2u.%.3u, system CPU time = %lu:%.2u:%.2u.%.3u\n"
                "elapsed time = %lu:%.2u:%.2u.%.3u, CPU load = %.2f%%\n\n",
            config->argv0, date, (unsigned)((ts_now.tv_nsec + 500000) / 1000000), zone,
            _time_hour(ru.ru_utime.tv_sec), _time_min(ru.ru_utime.tv_sec),
            _time_sec(ru.ru_utime.tv_sec), (unsigned)((ru.ru_utime.tv_usec + 500) / 1000),
            _time_hour(ru.ru_stime.tv_sec), _time_min(ru.ru_stime.tv_sec),
            _time_sec(ru.ru_stime.tv_sec), (unsigned)((ru.ru_stime.tv_usec + 500) / 1000),
            _time_hour(ts_start.tv_sec), _time_min(ts_start.tv_sec),
            _time_sec(ts_start.tv_sec), (unsigned)((ts_start.tv_nsec + 500000) / 1000000),
            100.0 * (ru.ru_utime.tv_sec + ru.ru_stime.tv_sec + (ru.ru_utime.tv_usec + ru.ru_stime.tv_usec) / 1000000.0)
                  / (ts_start.tv_sec + ts_start.tv_nsec / 1000000000.0));

    if (config->nthreads)
        fprintf(fp, "total worker threads = %lu, idle worker threads = %lu\n\n",
                (unsigned long)config->nthreads, (volatile unsigned long)config->nthr_idle);

    fprintf(fp, "minor page faults = %li, major page faults = %li, swaps = %li\n"
                "block inputs = %li, block outputs = %li\n"
                "messages sent = %li, messages received = %li\n"
                "signals = %li, vol ctx switches = %li, invol ctx switches = %li\n\n"
                "forwarded packets:\n",
            ru.ru_minflt, ru.ru_majflt, ru.ru_nswap,
            ru.ru_inblock, ru.ru_oublock,
            ru.ru_msgsnd, ru.ru_msgrcv,
            ru.ru_nsignals, ru.ru_nvcsw, ru.ru_nivcsw);

    for (i = 0; i < config->ntargets; i++) {
        char macaddr[arraylen("00:00:00:00:00:00")];

        fprintf(fp, "\t%s = %lu\n", eth_ntop(&config->targets[i].ifaddr, macaddr, sizeof macaddr),
                (unsigned long)config->targets[i].npackets);
    }
    fprintf(fp, "elapsed time (configuration age) = %lu:%.2u:%.2u.%.3u\n\n",
            _time_hour(ts_config.tv_sec), _time_min(ts_config.tv_sec),
            _time_sec(ts_config.tv_sec), (unsigned)((ts_config.tv_nsec + 500000) / 1000000));

    fclose(fp);
}

void pcap_callback(u_char *config, const struct pcap_pkthdr *pcap_pkthdr, const u_char *eth_hdr)
{
#define config ((config_t *)config)
#define eth_hdr ((struct eth_hdr *)eth_hdr)
    int i;
    unsigned hash;

    if (ntohs(eth_hdr->eth_type) == ETH_TYPE_IPV6) {
        struct ip6_hdr *ip6_hdr = (struct ip6_hdr *)(eth_hdr + 1);
        struct ip6_ext_hdr *ip6_ext_hdr = (struct ip6_ext_hdr *)(ip6_hdr + 1);
        struct tcp_hdr *tcp_hdr;
        uint8_t nxt = ip6_hdr->ip6_nxt;
        union {
            ip6_addr_t *ip6_addr;
            uint16_t *chunks;
        } srcaddr = {&ip6_hdr->ip6_src};

        while (nxt != config->proto && nxt != IPPROTO_NONE) {
            static const uint8_t exthdrs[] = {IPPROTO_HOPOPTS, IPPROTO_DSTOPTS, IPPROTO_ROUTING, IPPROTO_FRAGMENT, IPPROTO_AH/*, IPPROTO_ESP*/};
            uint8_t _nxt = ip6_ext_hdr->ext_nxt;

            for (i = 0; i < arraylen(exthdrs) && nxt != exthdrs[i]; i++) ;
            if (i == arraylen(exthdrs))
                break;
            ip6_ext_hdr = (struct ip6_ext_hdr *)(((uint8_t *)ip6_ext_hdr) + (nxt == IPPROTO_FRAGMENT ? 2 + sizeof(struct ip6_ext_data_fragment)
                                                                             : nxt == IPPROTO_AH ? (ip6_ext_hdr->ext_len + 2) << 2 : (ip6_ext_hdr->ext_len + 1) << 3));
            nxt = _nxt;
        }
        if (nxt != config->proto)
            return;
        tcp_hdr = (struct tcp_hdr *)ip6_ext_hdr;

        /* &tcp_hdr->th_sport == &udp_hdr->uh_sport */
        for (i = 0, hash = ntohs(tcp_hdr->th_sport); i < sizeof *srcaddr.ip6_addr / sizeof *srcaddr.chunks; i++)
            hash = HASH_FACTOR * hash + ntohs(srcaddr.chunks[i]);
    }
    else {
        struct ip_hdr *ip_hdr = (struct ip_hdr *)(eth_hdr + 1);
        struct tcp_hdr *tcp_hdr = (struct tcp_hdr *)(((uint8_t *)ip_hdr) + (ip_hdr->ip_hl << 2));
        union {
            ip_addr_t *ip_addr;
            uint16_t *chunks;
        } srcaddr = {&ip_hdr->ip_src};

        /* &tcp_hdr->th_sport == &udp_hdr->uh_sport */
        for (i = 0, hash = ntohs(tcp_hdr->th_sport); i < sizeof *srcaddr.ip_addr / sizeof *srcaddr.chunks; i++)
            hash = HASH_FACTOR * hash + ntohs(srcaddr.chunks[i]);
    }
    hash %= config->sum_weights;
    for (i = 0; (int)(hash -= config->targets[i].weight) >= 0; i++) ;
    config->targets[i].npackets++;
    eth_hdr->eth_dst = config->targets[i].ifaddr;
    eth_hdr->eth_src = config->ifaddr;
    eth_send(config->eth, eth_hdr, pcap_pkthdr->caplen);
#undef eth_hdr
#undef config
}

void *workerloop(void *config)
{
#define config ((config_t *)config)
    while (!config->isterminate) {
        struct eth_hdr *eth_hdr;
        struct pcap_pkthdr pcap_pkthdr;

        pthread_mutex_lock(config->mutex);
        if (!config->nthr_idle++ || config->nthr_idle == config->nthreads)
            pthread_cond_signal(config->thr_cond);
        while (!config->isterminate && !config->ispktpassing)
            pthread_cond_wait(config->pkt_cond, config->mutex);
        config->nthr_idle--;
        if (config->isterminate) {
            pthread_mutex_unlock(config->mutex);
            break;
        }
        eth_hdr = config->eth_hdr;
        pcap_pkthdr = config->pcap_pkthdr;
        config->ispktpassing = FALSE;
        pthread_cond_signal(config->thr_cond);
        pthread_mutex_unlock(config->mutex);

        pcap_callback((u_char *)config, &pcap_pkthdr, (u_char *)eth_hdr);
    }

    return NULL;
#undef config
}

void pcap_callback_mt(u_char *config, const struct pcap_pkthdr *pcap_pkthdr, const u_char *eth_hdr)
{
#define config ((config_t *)config)
    pthread_mutex_lock(config->mutex);
    while (!config->nthr_idle || config->ispktpassing)
        pthread_cond_wait(config->thr_cond, config->mutex);
    config->eth_hdr = (struct eth_hdr *)eth_hdr;
    config->pcap_pkthdr = *pcap_pkthdr;
    config->ispktpassing = TRUE;
    pthread_cond_signal(config->pkt_cond);
    pthread_mutex_unlock(config->mutex);
#undef config
}

int iploop(config_t *config)
{
    struct pollfd pollfd;

    if ((pollfd.fd = pcap_get_selectable_fd(config->pcap)) == -1) {
        syslogx(LOG_CRIT, "pcap_get_selectable_fd: poll: %s", strerror(ENOTSUP));
        return ENOTSUP;
    }
    pollfd.events = POLLRDNORM;

    while (!isreconfigure && !isterminate) {
        if (isdumpstat)
            dumpstat(config);
        switch (poll(&pollfd, 1, -1)) {
        case -1:
            if (errno != EINTR)
                syslogx(LOG_ERR, "poll: %m");
        case 0:
            break;
        default:
            if (config->nthreads) {
                pthread_mutex_lock(config->mutex);
                while (config->nthr_idle < config->nthreads)
                    pthread_cond_wait(config->thr_cond, config->mutex);
                pthread_mutex_unlock(config->mutex);
            }
            pcap_dispatch(config->pcap, -1, config->nthreads ? pcap_callback_mt : pcap_callback, (u_char *)config);
        }
    }

    return 0;
}

int thrctl(config_t *config)
{
    int i;
    pthread_t *thrs;
    sigset_t oset;

    if (!(thrs = malloc(config->nthreads * sizeof *thrs))) {
        syslogx(LOG_CRIT, "malloc: %m");
        return errno;
    }

    config->isterminate = FALSE;
    pthread_sigmask(SIG_SETMASK, &filledsigset, &oset);
    for (i = 0; i < config->nthreads; i++)
        if ((errno = pthread_create(thrs + i, NULL, workerloop, config))) {
            pthread_sigmask(SIG_SETMASK, &oset, NULL);
            syslogx(LOG_CRIT, "pthread_create: %m");
            pthread_mutex_lock(config->mutex);
            config->isterminate = TRUE;
            pthread_cond_broadcast(config->pkt_cond);
            pthread_mutex_unlock(config->mutex);
            while (i--)
                pthread_join(thrs[i], NULL);
            free(thrs);
            return errno;
        }
    pthread_sigmask(SIG_SETMASK, &oset, NULL);

    errno = iploop(config);

    pthread_mutex_lock(config->mutex);
    config->isterminate = TRUE;
    pthread_cond_broadcast(config->pkt_cond);
    pthread_mutex_unlock(config->mutex);
    for (i = 0; i < config->nthreads; i++)
        pthread_join(thrs[i], NULL);

    free(thrs);

    return errno;
}

int main(int argc, char *const *argv)
{
    config_t config;

    if ((errno = initialize(&config, argc, argv))
        || (!config.isforeground && (errno = daemonize()))
        || (errno = readconfig(&config)))
        return errno;

    while (!isterminate) {
        if (config.nthreads ? thrctl(&config) : iploop(&config))
            break;

        if (isreconfigure) {
            config_t new_config = config;

            if (!readconfig(&new_config)) {
                freeconfig(&config);
                config = new_config;
            }
            isreconfigure = FALSE;
        }
    }

    freeconfig(&config);

    return 0;
}