dhcp_protect/src/dp_nfqueue.c

#include <stdio.h>
#include <string.h>
#include <arpa/inet.h>
#include <errno.h>

#include "dp_nfqueue.h"
#include "dp_accounting.h"
#include "dp_blacklist.h"
#include "dp_helpers.h"
#include "dp_dhcpv4.h"
#include "dp_dhcpv6.h"

// start netfilter queue
void dp_nfq_start(dp_conf *conf) {
	struct nfq_handle *h;
	struct nfq_q_handle *qh;
	int fd;

	if ( ( h = nfq_open() ) == NULL ) {
		fprintf(stderr,"error during nfq_open() %s\n", strerror(errno));
		return;
	}

	if ( ( qh = nfq_create_queue(h, conf->queue, &dp_callback, conf) ) == NULL ) {
		fprintf(stderr, "error during nfq_create_queue() %s\n", strerror(errno));
		return;
	}

	if ( nfq_set_mode(qh, NFQNL_COPY_PACKET, 1500) < 0 ) {
		fprintf(stderr,"error during nfq_set_mode() %s\n", strerror(errno));
		return;
	}

	if ( nfq_set_queue_flags(qh, NFQA_CFG_F_FAIL_OPEN, NFQA_CFG_F_FAIL_OPEN) < 0 ) {
		fprintf(stderr,"error during nfq_set_queue_flags() %s\n", strerror(errno));
		return;
	}

	fd = nfq_fd(h);

	while(1) {
		static char buf[65536];
		int rv;

		rv = recv(fd, buf, sizeof(buf), 0);

		switch(rv) {
			case -1:
				fprintf(stderr, "recv() error: %s\n", strerror(errno));
				return;
			case 0:
				fprintf(stderr,"socket is closed!?\n");
				return;
			default:
				// send packet to callback
				nfq_handle_packet(h, buf, rv);
				break;
		}
	}
}

// decode dhcp packet
int dp_dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
	unsigned char *pkt;
	int pktlen;
	int offset = 0;
	unsigned char *remoteid = NULL;
	int remoteidlen = 0;
	int rv = NF_ACCEPT;

	pktlen = nfq_get_payload(nfa, &pkt);

	if ( conf->debug ) printf("got a packet, len = %i\n", pktlen);

	// can we read the IP proto and IP header length ?
	if ( pktlen > 0 ) {
		uint8_t ipver;
		uint8_t ihl;

		ipver = pkt[offset];
		ipver >>= 4;
		ihl   = pkt[offset];
		ihl  &= 0x0f;

		if ( ipver == 4 ) {
			// jump to DHCPv4
			offset += ( ihl * 4 ) + 8;
			dp_dhcpv4_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
		}
		else if ( ipver == 6 ) {
			// jump to DHCPv6
			offset += 40 + 8;
			dp_dhcpv6_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
		}
		else {		
			if ( conf->debug ) printf("not an IPv4 packet\n");
			goto end;
		}
	}

	if ( conf->debug ) printf("remoteidlen=%i\n",remoteidlen);

	if ( remoteidlen>0 ) {
		if ( conf->debug ) {
			int i;
			printf("remoteid: ");
			for(i=0; i<remoteidlen; i++)
				printf("%02x", remoteid[i]);
			printf("\n");
		}
		// count the packet, even when blacklisted.
		dp_accounting_add(conf, remoteid, remoteidlen);

		// check if already in the blacklist
		if ( dp_blacklist_check(conf, remoteid, remoteidlen) == NF_DROP )
			rv = NF_DROP;

		// check if it must be added to the blacklist
		else if ( dp_accounting_check(conf, remoteid, remoteidlen) == NF_DROP ) {
			dp_blacklist_add(conf, remoteid, remoteidlen);
			rv = NF_DROP;
		}
	}

	end:

	dp_accounting_cleanup(conf);
	dp_blacklist_cleanup(conf);

	return rv;
}

// netfilter queue callback
int dp_callback(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg, struct nfq_data *nfa, void *data) {
	struct nfqnl_msg_packet_hdr *ph = nfq_get_msg_packet_hdr(nfa);
	dp_conf *conf = (dp_conf*)data;
	int id = -1;
	int verdict;

	if ( ph ) {
		id = ntohl (ph->packet_id);
		if ( conf->debug ) printf ("received packet with id %d\n", id);
	}

	verdict = dp_dhcp_check(nfa, conf); /* Treat packet */

	// override decision for dryrun
	if ( conf->dryrun )
		verdict = NF_ACCEPT;

	return nfq_set_verdict(qh, id, verdict, 0, NULL); /* Verdict packet */
}