read packets with Berkeley Packet Filter

bpf-read.c

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <net/bpf.h>
#include <net/if.h>
#include <net/if_arp.h>
#include <net/ethernet.h>
#include <netinet/in.h>
#include <netinet/ip.h>

int  openBpf(void);
void closeBpf(int fd);
int  setupBpf(int fd, const char *ifname);
int  readPackets(int fd);

void printEther(const struct ether_header* ether);
void printIP(const struct ip* ip);
void printARP(const struct arphdr* arp);
 
static void printMACAddress(const char* str, const u_char* address);
static void printIPAddress(const char* str, const u_char* address);


static void
printMACAddress(const char* str, const u_char* address)
{
    printf("%s: %02x:%02x:%02x:%02x:%02x:%02x\n",
            str,
            address[0], address[1], address[2],
            address[3], address[4], address[5]);
}

  
static void
printIPAddress(const char* str, const u_char* address)
{
    printf("%s: %d.%d.%d.%d\n",
            str,
            address[0], address[1], address[2], address[3]);
}
  

int
openBpf(void)
{
    return open("/dev/bpf", O_RDWR);
}
 

void
closeBpf(int fd)
{
    close(fd);
}


int
setupBpf(int fd, const char *ifname)
{
    struct ifreq request;
    u_int tstamp;
    u_int type;
    u_int enable;
         
    strlcpy(request.ifr_name, ifname, sizeof(request.ifr_name) - 1);
    if (ioctl(fd, BIOCSETIF, &request) < 0) {
        perror("BIOCSETIF failed: ");
        return -1;
    }

    if (ioctl(fd, BIOCGDLT, &type) < 0) {
        perror("BIOCGDLT failed: ");
        return -1;
    }
    if (type != DLT_EN10MB) {
        printf("unsupported datalink type\n");
        return -1;
    }

    tstamp = BPF_T_NANOTIME;
    if (ioctl(fd, BIOCSTSTAMP, &tstamp) < 0) {
        perror("BIOCSTSTAMP faild: ");
        return -1;
    }

    enable = 1;
    if (ioctl(fd, BIOCIMMEDIATE, &enable) < 0) {
        perror("BIOCSIMMEDIATE failed: ");
        return -1;
    }
    return 0;
}


int
readPackets(int fd)
{
    u_int bufSize;
    char *buf;
    ssize_t n;

    if (ioctl(fd, BIOCGBLEN, &bufSize) < 0) {
        perror("BIOCGBLEN failed: ");
        return -1;
    }
    buf = malloc(bufSize);
    if (buf == NULL) {
        printf("memory alloation failed\n");
        return -1;
    }

    for (;;) {
        const char *packet;

        if ((n = read(fd, buf, bufSize)) < 0) {
            perror("read failure: ");
            return -1;
        }
        if (n == 0) {
            break;
        }

        packet = buf;
        while ((packet - buf) < n) {
            const struct bpf_xhdr *bpfHeader;

            bpfHeader = (struct bpf_xhdr*)packet;
            if (bpfHeader->bh_caplen >= sizeof(struct ether_header)) {
                const struct ether_header* ether;

                ether = (struct ether_header*)(packet + bpfHeader->bh_hdrlen);
                printEther(ether);
                printf("\n");
            }
            packet += BPF_WORDALIGN(bpfHeader->bh_hdrlen + bpfHeader->bh_caplen);
        }
    }

    free(buf);
    return 0;
}


void
printEther(const struct ether_header* ether)
{
    u_short ether_type = ntohs(ether->ether_type);
    const u_char* payload;

    printf("Ether\n");

    printMACAddress("src address", ether->ether_shost);
    printMACAddress("dst address", ether->ether_dhost);
    
    payload = (((const u_char*)ether) + sizeof(struct ether_header));
    switch (ether_type)
    {
    case ETHERTYPE_IP:
        printIP((struct ip*)payload);
        break;
    case ETHERTYPE_IPV6:
        printf("IPv6\n");
        break;
    case ETHERTYPE_ARP:
        printARP((struct arphdr*)payload);
        break;
    default:
        printf("Unknown ether type: %d\n", ether_type);
    }
}


void
printIP(const struct ip* ip)
{
    printf("IP\n");

    printIPAddress("src", (u_char*)&ip->ip_src.s_addr);
    printIPAddress("dst", (u_char*)&ip->ip_dst.s_addr);
}


void
printARP(const struct arphdr* arp)
{
    printf("ARP\n");

    if (ntohs(arp->ar_hrd) != ARPHRD_ETHER) {
        printf("not Ethernet\n");
        return;
    }
    if (ntohs(arp->ar_pro) != ETHERTYPE_IP) {
        printf("Unknown protocol\n");
        return;
    }

    printMACAddress("sender hard", (u_char*)ar_sha(arp));
    printIPAddress("sender proto", (u_char*)ar_spa(arp));
    printMACAddress("target hard", (u_char*)ar_tha(arp));
    printIPAddress("target proto", (u_char*)ar_tpa(arp));
}


int
main(int argc, char **argv)
{
    int fdBpf = -1;

    if (argc != 2) {
        printf("usage: %s <interface>\n", argv[0]);
        return 1;
    }

    if ((fdBpf = openBpf()) < 0) {
        printf("BPF cannot be opened.\n");
        return 1;
    }

    if (setupBpf(fdBpf, argv[1]) < 0) {
        close(fdBpf);
        return 1;
    }

    if (readPackets(fdBpf) < 0) {
        close(fdBpf);
        return 1;
    }

    closeBpf(fdBpf);
    return 0;
}