dhcp_protect/src/dp_dhcpv4.c

#include <stdio.h>
#include <stdint.h>

#include "dp_dhcpv4.h"
#include "dp_helpers.h"

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

	// jump DHCP header
	offset += 28 + 16 + 64 + 128;

	// minimum packet size, fixed header + magic cookie (4 octets)
	if ( pktlen < offset + 4 ) {
		if ( conf->debug ) printf("packet too small\n");
		return;
	}

	// check magic cookie
	if ( pkt[offset] != 99 || pkt[offset+1] != 130 || pkt[offset+2] != 83 || pkt[offset+3] != 99 ) {
		if ( conf->debug )
			printf(
				"invalid magic cookie %02x%02x%02x%02x\n",
				pkt[offset], pkt[offset+1],
				pkt[offset+2], pkt[offset+3]);
		return;
	}
	offset+=4;


	// parse TLV options
	while(offset<pktlen && remoteidlen==0) {
		uint8_t type = pkt[offset];
		uint8_t len;

		offset++;

		// padding of 1 octet
		if ( type == 0 ) {
			continue;
		}

		// end of options
		if ( type == 255 )
			break;

		// make sure we can read len
		if ( offset>=pktlen )
			break;

		len = pkt[offset];
		offset++;

		// can the value be read
		if ( offset+len>pktlen )
			break;

		// option 82 parser
		if ( type == 82 ) {
			unsigned char *o82 = pkt+offset;
			int o82off = 0;

			// loop until the end, +2 to ensure we can read type and length
			while(o82off+2<len && remoteidlen==0) {
				uint8_t otype = o82[o82off];
				uint8_t olen  = o82[o82off+1];
				o82off+=2;

				// printf("o82 type=%i len=%i\n", otype, olen);

				// remoteid
				if ( otype == 2 ) {
					// make sure we don't overflow and can read all data
					if ( o82off + olen > len ) {
						if ( conf->debug) printf("option 82.2 data too long\n");
						break;
					}
					else {
						*remoteid = o82 + o82off;
						*remoteidlen = olen;
					}
				}

				o82off+=olen;
			}
		}

		offset+=len;
	}

	// if we didn't find opt82.2, we fallback to the hw addr
	if ( *remoteidlen == 0 ) {
		// nope, we won't overflow
		if ( (uint8_t)pkt[hwlenpos] <= 16 ) {
			*remoteid = pkt+hwaddrpos;
			*remoteidlen = (uint8_t)pkt[hwlenpos];
		}
	}
}