Capture packets with classic BPF

capture.c

#include <errno.h>
#include <fcntl.h>
#include <net/bpf.h>
#include <net/if.h>
#include <netinet/in.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <unistd.h>

#define LOG(fmt, ...)         printf("INFO: " fmt "\n", ##__VA_ARGS__)
#define ERROR(fmt, ...)       printf("ERROR: " fmt "\n", ##__VA_ARGS__)
#define PERROR(msg)           ERROR(msg ": %s", strerror(errno))

#define ARRAY_LENGTH(x) (sizeof(x) / sizeof((x)[0]))

struct link {
    int fd;
    unsigned char *buffer;
    unsigned int buffer_size;
};
#define LINK_INIT \
    {             \
        .fd = -1  \
    }

// Capture all IP packets from 8.8.8.8.
struct bpf_insn google_source_filter[] = {
    BPF_STMT(BPF_LD + BPF_W + BPF_LEN, 0),                  // Load packet length into accumulator.
    BPF_JUMP(BPF_JMP + BPF_JGE + BPF_K, 34, 0, 7),          // Make sure that we have enough bytes for an IPv4 header.
    BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),                 // Load the Ethernet type field into the accumulator.
    BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x800, 0, 5),       // Make sure this is an IPv4 header.
    BPF_STMT(BPF_LDX + BPF_W + BPF_IMM, 14),                // Set the index register to the start of the IP header.
    BPF_STMT(BPF_LD + BPF_W + BPF_IND, 12),                 // Load source IP address into accumulator.
    BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x08080808, 0, 2),  // Compare accumulator to target IP address.
    BPF_STMT(BPF_LD + BPF_W + BPF_LEN, 0),                  // Load packet length into accumulator.
    BPF_STMT(BPF_RET + BPF_A, 0),                           // Accept packet.
    BPF_STMT(BPF_RET + BPF_K, 0),                           // Reject packet.
};

void
close_link(struct link *l)
{
    close(l->fd);
    l->fd = -1;

    free(l->buffer);
    l->buffer = NULL;
}

int
open_bpf(void)
{
    for (int k = 0; k < 256; k++) {
        int fd;
        char device[12];

        snprintf(device, sizeof(device), "/dev/bpf%i", k);
        fd = open(device, O_RDWR);
        if (fd < 0) {
            int local_errno = errno;

            if (local_errno == EBUSY) {
                continue;
            }

            ERROR("open (%s): %s", device, strerror(local_errno));
            return -1;
        }

        return fd;
    }

    ERROR("Failed to find available BPF device");
    return -1;
}

int
create_link(struct link *l, const char *device)
{
    int link_type;
    struct ifreq ifr;

    l->fd = open_bpf();
    if (l->fd < 0) {
        return -1;
    }

    snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", device);
    if (ioctl(l->fd, BIOCSETIF, &ifr) == -1) {
        PERROR("ioctl (BIOCSETIF)");
        goto error;
    }

    if (ioctl(l->fd, BIOCGDLT, &link_type) == -1) {
        PERROR("ioctl (BIOCGDLT)");
        goto error;
    }
    if (link_type != DLT_EN10MB) {
        ERROR("Unexpected link layer type.  Got %i but expected %i", link_type, DLT_EN10MB);
        goto error;
    }

    if (ioctl(l->fd, BIOCGBLEN, &l->buffer_size) == -1) {
        PERROR("ioctl (BIOCGBLEN)");
        goto error;
    }
    l->buffer = malloc(l->buffer_size);
    if (!l->buffer) {
        ERROR("Failed to allocate capture buffer");
        goto error;
    }

    if (ioctl(l->fd, BIOCIMMEDIATE, &(unsigned int){1}) == -1) {
        PERROR("ioctl (BIOCIMMEDIATE)");
        goto error;
    }

    return 0;

error:
    close_link(l);
    return -1;
}

int
set_filter(const struct link *l, struct bpf_insn *instructions, unsigned int num_instructions)
{
    struct bpf_program prog = {
        .bf_insns = instructions,
        .bf_len = num_instructions,
    };

    if (ioctl(l->fd, BIOCSETF, &prog) == -1) {
        PERROR("ioctl (BIOCSETF)");
        return -1;
    }

    return 0;
}

int
flush_buffer(const struct link *l)
{
    if (ioctl(l->fd, BIOCFLUSH) == -1) {
        PERROR("ioctl (BIOCFLUSH)");
        return -1;
    }

    return 0;
}

int
send_packet(const struct link *l, const unsigned char *packet, size_t size)
{
    ssize_t transmitted;

    transmitted = write(l->fd, packet, size);
    if (transmitted < 0) {
        PERROR("write");
        return -1;
    }
    if ((size_t)transmitted < size) {
        ERROR("Only transmitted %zi bytes of %zu-byte packet", transmitted, size);
        return -1;
    }
    return 0;
}

int
receive_and_print_packets(const struct link *l)
{
    int transmitted;
    unsigned char *data = l->buffer;

    transmitted = read(l->fd, l->buffer, l->buffer_size);
    if (transmitted < 0) {
        PERROR("read");
        return -1;
    }
    LOG("Read %i bytes from BPF device", transmitted);

    while ((unsigned int)transmitted >= sizeof(struct bpf_hdr)) {
        unsigned int advance;
        const struct bpf_hdr *header = (struct bpf_hdr *)data;
        const unsigned char *packet = data + header->bh_hdrlen;

        LOG("Captured %u bytes of %u-byte packet", header->bh_caplen, header->bh_datalen);

        for (unsigned int k = 0; k < header->bh_caplen; k++) {
            if (k % 16 == 0) {
                printf("\n\t%04x\t", k);
            }

            printf("%02x ", packet[k]);
        }
        printf("\n\n");

        advance = BPF_WORDALIGN(header->bh_hdrlen + header->bh_caplen);
        if (advance > (unsigned int)transmitted) {
            break;
        }
        data += advance;
        transmitted -= advance;
    }

    return 0;
}

int
main()
{
    int ret = 1;
    struct link l = LINK_INIT;

    LOG("Creating link");
    if (create_link(&l, "en0") != 0) {
        return 1;
    }

    LOG("Setting filter");
    if (set_filter(&l, google_source_filter, ARRAY_LENGTH(google_source_filter)) != 0) {
        goto done;
    }

    LOG("Flushing packet buffer");
    if (flush_buffer(&l) != 0) {
        goto done;
    }

    LOG("Waiting for packets");
    while (1) {
        if (receive_and_print_packets(&l) != 0) {
            goto done;
        }
    }

    ret = 0;

done:
    close_link(&l);
    return ret;
}