support for DHCPv6

dhcp_protect.c

@@ -27,16 +27,16 @@ int main(int argc, char **argv) {
 		configfile = argv[1];
 	}
 	else {
-		usage(argv[0]);
+		dp_usage(argv[0]);
 		return EXIT_FAILURE;
 	}
 
-	if ( load_config(&conf, configfile) == NULL )
+	if ( dp_load_config(&conf, configfile) == NULL )
 		return EXIT_FAILURE;
 
 	openlog("dhcp_protect", LOG_PID, LOG_DAEMON);
 
-	nfq_start(&conf);
+	dp_nfq_start(&conf);
 
 	return 0;
 }
@@ -62,7 +62,7 @@ void dp_log(unsigned char *remoteid, int remoteidlen, char *fmt, ...) {
 }
 
 // start netfilter queue
-void nfq_start(dp_conf *conf) {
+void dp_nfq_start(dp_conf *conf) {
 	struct nfq_handle *h;
 	struct nfq_q_handle *qh;
 	int fd;
@@ -100,13 +100,13 @@ void nfq_start(dp_conf *conf) {
 }
 
 // display usage
-void usage(char *prog) {
+void dp_usage(char *prog) {
 	fprintf(stderr,"Usage: %s <configuration file>\n",prog);
 }
 
 
 // load configuration file
-dp_conf *load_config(dp_conf *conf, char *file) {
+dp_conf *dp_load_config(dp_conf *conf, char *file) {
 	FILE *fh;
 	char *line = NULL;
 	size_t len = 0;
@@ -189,16 +189,12 @@ dp_conf *load_config(dp_conf *conf, char *file) {
 }
 
 // decode dhcp packet
-int dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
+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 found = 0;
-	uint8_t ipver = 0;
-	uint8_t ihl = 0;
-	//int i;
 	int rv = NF_ACCEPT;
 
 	pktlen = nfq_get_payload(nfa, &pkt);
@@ -207,6 +203,9 @@ int dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
 
 	// 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];
@@ -215,12 +214,12 @@ int dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
 		if ( ipver == 4 ) {
 			// jump to DHCPv4
 			offset += ( ihl * 4 ) + 8;
-			dhcpv4_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
+			dp_dhcpv4_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
 		}
 		else if ( ipver == 6 ) {
 			// jump to DHCPv6
-			offset += 48 + 8;
-			dhcpv6_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
+			offset += 40 + 8;
+			dp_dhcpv6_check(conf, pkt, pktlen, offset, &remoteid, &remoteidlen);
 		}
 		else {		
 			if ( conf->debug ) printf("not an IPv4 packet\n");
@@ -229,6 +228,13 @@ int dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
 	}
 
 	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);
 
@@ -250,7 +256,7 @@ int dhcp_check(struct nfq_data *nfa, dp_conf *conf) {
 	return rv;
 }
 
-void dhcpv6_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, unsigned char *remoteid, int *remoteidlen) {
+void dp_dhcpv6_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, unsigned char **remoteid, int *remoteidlen) {
 
 	while(offset<pktlen) {
 		uint8_t msgtype = (uint8_t)pkt[offset];
@@ -268,12 +274,15 @@ void dhcpv6_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 			uint8_t code = (uint8_t)pkt[offset];
 			uint8_t len  = (uint8_t)pkt[offset+1];
 
-			offset+2;
+			offset+=2;
 
 			if ( code == 1 ) { // Client identifier / DUID
-				remoteid = pkt+offset;
-				*remoteidlen = len;
-				break;
+				// make sure there's enough space
+				if ( offset + len <= pktlen ) {
+					*remoteid = pkt+offset;
+					*remoteidlen = len;
+					break;
+				}
 			}
 			offset+=len;
 		}
@@ -284,7 +293,7 @@ void dhcpv6_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 }
 
 
-void dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, unsigned char *remoteid, int *remoteidlen) {
+void dp_dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, unsigned char **remoteid, int *remoteidlen) {
 	int hwaddrpos = offset + 28; // remember where the hw addr is if we need to fallback to it
 	int hwlenpos = offset + 2;   // remember where the hw addr len is if we need to fallback to it
 
@@ -310,7 +319,7 @@ void dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 
 
 	// parse TLV options
-	while(offset<pktlen && !found) {
+	while(offset<pktlen && remoteidlen==0) {
 		uint8_t type = pkt[offset];
 		uint8_t len;
 
@@ -342,7 +351,7 @@ void dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 			int o82off = 0;
 
 			// loop until the end, +2 to ensure we can read type and length
-			while(o82off+2<len && !found) {
+			while(o82off+2<len && remoteidlen==0) {
 				uint8_t otype = o82[o82off];
 				uint8_t olen  = o82[o82off+1];
 				o82off+=2;
@@ -357,7 +366,7 @@ void dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 						break;
 					}
 					else {
-						remoteid = o82 + o82off;
+						*remoteid = o82 + o82off;
 						*remoteidlen = olen;
 					}
 				}
@@ -373,8 +382,8 @@ void dhcpv4_check(dp_conf *conf, unsigned char *pkt, int pktlen, int offset, uns
 	if ( *remoteidlen == 0 ) {
 		// nope, we won't overflow
 		if ( (uint8_t)pkt[hwlenpos] <= 16 ) {
-			remoteid = pkt+hwaddrpos;
-			remoteidlen = (uint8_t)pkt[hwlenpos];
+			*remoteid = pkt+hwaddrpos;
+			*remoteidlen = (uint8_t)pkt[hwlenpos];
 		}
 	}
 }
@@ -391,7 +400,7 @@ static int dp_callback(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg, struct n
 		if ( conf->debug ) printf ("received packet with id %d\n", id);
 	}
 
-	verdict = dhcp_check(nfa, conf); /* Treat packet */
+	verdict = dp_dhcp_check(nfa, conf); /* Treat packet */
 
 	// override decision for dryrun
 	if ( conf->dryrun )

dhcp_protect.h

@@ -35,13 +35,29 @@ static int dp_callback (
 	struct nfq_data*,
 	void*);
 
-void       usage               (char*);
-int        dhcp_check          (struct nfq_data*, dp_conf*);
-dp_conf   *load_config         (dp_conf*, char*);
-void       nfq_start           (dp_conf*);
+void       dp_usage               (char*);
+dp_conf   *dp_load_config         (dp_conf*, char*);
+void       dp_nfq_start           (dp_conf*);
 void       dp_accounting_add   (dp_conf*, unsigned char *, int);
 int        dp_accounting_check (dp_conf*, unsigned char *, int);
 void       dp_blacklist_add    (dp_conf*, unsigned char *, int);
 int        dp_blacklist_check  (dp_conf*, unsigned char *, int);
 void       dp_hash_cleanup     (dp_conf*);
 void       dp_log              (unsigned char *, int, char *, ...);
+int        dp_dhcp_check       (struct nfq_data*, dp_conf*);
+
+void dp_dhcpv4_check (
+	dp_conf*,
+	unsigned char*,
+	int,
+	int,
+	unsigned char**,
+	int*);
+
+void dp_dhcpv6_check (
+	dp_conf*,
+	unsigned char*,
+	int,
+	int,
+	unsigned char**,
+	int*);