Skip to content

Instantly share code, notes, and snippets.

@manuelvonthron-opalrt
Last active December 14, 2016 17:50
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save manuelvonthron-opalrt/8559997 to your computer and use it in GitHub Desktop.
Raw network traffic filtering/writing with BPF for QNX/BSD
/**
* 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(&ethhdr, 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