Raw network traffic filtering/writing with BPF for QNX/BSD
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/** | |
* Created by Manuel Vonthron | |
* Copyright (c) 2014 OPAL-RT Technologies, Inc. | |
*/ | |
#include <stdlib.h> | |
#include <stdio.h> | |
#include <errno.h> | |
#include <string.h> | |
#include <fcntl.h> | |
#include <sys/types.h> | |
#include <sys/ioctl.h> | |
#include <arpa/inet.h> | |
#include <net/bpf.h> | |
#include <net/ethertypes.h> | |
#include <net/if_ether.h> | |
/* @see https://github.com/mvonthron/tiler/blob/master/src/utils.h */ | |
#define D printf | |
#define FATAL printf | |
/* Ethertypes definitions */ | |
#define ETHERTYPE_ETHERCAT 0x88A4 | |
#define ETHERTYPE_IEC61850GO 0x88B8 | |
#define ETHERTYPE_IEC61850SV 0x88BA | |
#define ETHERTYPE_FILTER ETHERTYPE_ETHERCAT | |
struct bpf_insn insns[] = { | |
/* load Ethertype (first 12b are src and dst addresses) */ | |
BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 12), | |
/* compare to 'ETHERTYPE_FILTER' value */ | |
BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, ETHERTYPE_FILTER, 0, 1), | |
/* true: we keep the whole packet */ | |
BPF_STMT(BPF_RET+BPF_K, (u_int)-1), | |
/* false: dropped */ | |
BPF_STMT(BPF_RET+BPF_K, 0), | |
}; | |
struct bpf_program filter = { | |
.bf_insns = insns, | |
.bf_len = (sizeof(insns) / sizeof(struct bpf_insn)) | |
}; | |
static long unsigned int arp_count=0; | |
static long unsigned int ecat_count=0; | |
static long unsigned int unknown_count=0; | |
static uint64_t total_count=0; | |
static uint64_t iec61850_count=0; | |
static uint64_t ip_count=0; | |
void process_packet(struct bpf_hdr *buffer, int size); | |
void write_packet(int bpf); | |
void quit(); | |
void cleanup(); | |
void stats(); | |
/** */ | |
static int bpf = 0; | |
void quit() | |
{ | |
D("Exiting"); | |
stats(); | |
cleanup(); | |
exit(1); | |
} | |
void cleanup() | |
{ | |
close(bpf); | |
return; | |
} | |
void stats() | |
{ | |
struct bpf_stat s; | |
if( ioctl(bpf, BIOCGSTATS , &s) == -1){ | |
FATAL("Could get stats"); | |
} | |
D("-> s.bs_capt: %llu, s.bs_drop: %llu, s.bs_recv: %llu", s.bs_capt, s.bs_drop, s.bs_recv) | |
} | |
int main(int argc, char **argv) | |
{ | |
D("Hello QNX!"); | |
signal (SIGINT, quit); | |
/* using BPF device */ | |
char bpfname[16] = {"/dev/bpf\0"}; | |
int i=0; | |
/* opening autocloning BFP device */ | |
bpf = open(bpfname, O_RDWR); | |
if (bpf < 0){ | |
/* no autocloning BPF found: fall back to iteration */ | |
for(i=0; i<128; i++){ | |
snprintf(bpfname, sizeof(bpfname), "/dev/bpf%d", i); | |
bpf = open(bpfname, O_RDWR); | |
if(bpf != -1) | |
break; | |
} | |
if(bpf < 0){ | |
FATAL("Error: could not open any /dev/bpf device."); | |
} | |
} | |
D("Opened BPF device \"%s\"", bpfname); | |
/* binding with real interface (wm1) */ | |
const char* ifname = "wm0"; | |
struct ifreq iface; | |
strncpy(iface.ifr_name, ifname, sizeof(ifname)); | |
if( ioctl(bpf, BIOCSETIF, &iface) > 0){ | |
FATAL("Could not bind %s to BPF", ifname); | |
} | |
D("Associated with \"%s\"", ifname); | |
/* set immediate: returns on packet arrived instead of when buffer full */ | |
int setimmediate = 1; | |
if( ioctl(bpf, BIOCIMMEDIATE, &setimmediate) == -1){ | |
FATAL("Could set IO immediate"); | |
} | |
/* disable ethernet header completion by BPF */ | |
int hdr_complete = 1; | |
if( ioctl(bpf, BIOCSHDRCMPLT , &hdr_complete) == -1){ | |
FATAL("Could get disable HDRCMPLT"); | |
} | |
/* set promiscuous mode */ | |
// int promiscuous = 1; | |
// if( ioctl(bpf, BIOCPROMISC, &promiscuous) == -1){ | |
// FATAL("Could get disable BIOCPROMISC"); | |
// } | |
struct bpf_version version; | |
if( ioctl(bpf, BIOCVERSION, &version) == -1){ | |
FATAL("Could BPF version"); | |
} | |
D("BPF version %d.%d", version.bv_major, version.bv_minor); | |
/* retrieve internal buffer length */ | |
int buffer_len = 0; | |
if( ioctl(bpf, BIOCGBLEN, &buffer_len) == -1){ | |
FATAL("Could get buffer length"); | |
} | |
D("Buffer length is %d (%dko).", buffer_len, buffer_len/1024); | |
/* set BPF filter */ | |
if( ioctl(bpf, BIOCSETF, &filter) < 0) { | |
FATAL("Could not set BPF filter (type 0x%04x): error %d (%s)", ETHERTYPE_FILTER, errno, strerror(errno)); | |
} | |
D("BPF filter for type 0x%04x set.", ETHERTYPE_FILTER); | |
/** | |
* now let's read | |
*/ | |
struct bpf_hdr *bpf_buffer = (struct bpf_hdr *) calloc(buffer_len, sizeof(struct bpf_hdr *)); | |
struct bpf_hdr* bpf_packet; | |
int readed=0; | |
int stop=0; | |
stats(); | |
while(!stop) | |
{ | |
memset(bpf_buffer, 0, buffer_len); | |
write_packet(bpf); | |
if((readed = read(bpf, bpf_buffer, buffer_len)) > 0) | |
{ | |
char* p = (char*) (bpf_buffer); | |
while(p < ((char*) (bpf_buffer) + readed)) | |
{ | |
bpf_packet = (struct bpf_hdr *) (p); | |
process_packet(bpf_packet, readed); | |
p += BPF_WORDALIGN(bpf_packet->bh_hdrlen + bpf_packet->bh_caplen); | |
} | |
printf("\rTotal: "COLOR_GREEN"%llu"COLOR_CLEAR"\tIP: %llu\tARP: %lu\t61850: %llu\tEtherCAT: "COLOR_RED"%lu"COLOR_CLEAR"\tUnknown: %lu\r", | |
total_count, ip_count, arp_count, iec61850_count, ecat_count, unknown_count); | |
fflush(stdout); | |
if(total_count % 5000 == 0){ | |
// stats(); | |
write_packet(bpf); | |
} | |
} | |
} | |
close(bpf); | |
free(bpf_buffer); | |
return 0; | |
} | |
#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x" | |
#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5] | |
void process_packet(struct bpf_hdr *packet, int size) | |
{ | |
unsigned char* frame = ((unsigned char *) packet + packet->bh_hdrlen); | |
struct ether_header ethhdr; | |
memcpy(ðhdr, frame, sizeof(struct ether_header)); | |
char src[18] = {0}; | |
char dest[18] = {0}; | |
sprintf(src, MACSTR, MAC2STR(ethhdr.ether_shost)); | |
sprintf(dest,MACSTR, MAC2STR(ethhdr.ether_shost)); | |
uint16_t type = ntohs(ethhdr.ether_type) ; | |
total_count++; | |
switch(type){ | |
case ETHERTYPE_IP: | |
ip_count++; | |
printf(COLOR_BLUE "IP " COLOR_CLEAR " %s -> %s (%d)\n", src, dest, packet->bh_caplen); | |
break; | |
case ETHERTYPE_ARP: | |
printf(COLOR_BLUE "ARP " COLOR_CLEAR " %s -> %s (%d)\n", src, dest, packet->bh_caplen); | |
arp_count++; break; | |
case ETHERTYPE_IEC61850GO: | |
case ETHERTYPE_IEC61850SV: | |
printf(COLOR_BLUE "SVGO" COLOR_CLEAR " %s -> %s (%d)\n", src, dest, packet->bh_caplen); | |
iec61850_count++; break; | |
case ETHERTYPE_ETHERCAT: | |
printf(COLOR_GREEN "ECAT" COLOR_CLEAR " %s -> %s (%d)\n", src, dest, packet->bh_caplen); | |
ecat_count++; break; | |
default: | |
printf(COLOR_YELLOW "%04x" COLOR_CLEAR " %s -> %s (%d)\n", type, src, dest, packet->bh_caplen); | |
unknown_count++; | |
} | |
} | |
void write_packet(int bpf) | |
{ | |
const char frame[] = "012345678901234567890blehblehbleh\0"; | |
int n = write(bpf, frame, sizeof(frame)); | |
if(n < sizeof(frame)){ | |
D("Nope!"); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment