COM32: add DHCP pack/unpack functions

Signed-off-by: Gene Cumm <gene.cumm@gmail.com>

4 files changed, 323 insertions, 1 deletions

diff --git a/com32/include/dhcp.h b/com32/include/dhcp.h
new file mode 100644
index 00000000..afef9242
--- /dev/null
+++ b/com32/include/dhcp.h
@@ -0,0 +1,40 @@
+#ifndef DHCP_H
+#define DHCP_H
+
+#include <inttypes.h>
+
+struct dhcp_option {
+	void *data;
+	int len;
+};
+
+struct dhcp_packet {
+	uint8_t op;		/*   0 */
+	uint8_t htype;		/*   1 */
+	uint8_t hlen;		/*   2 */
+	uint8_t hops;		/*   3 */
+	uint32_t xid;		/*   4 */
+	uint16_t secs;		/*   8 */
+	uint16_t flags;		/*  10 */
+	uint32_t ciaddr;	/*  12 */
+	uint32_t yiaddr;	/*  16 */
+	uint32_t siaddr;	/*  20 */
+	uint32_t giaddr;	/*  24 */
+	uint8_t chaddr[16];	/*  28 */
+	uint8_t sname[64];	/*  44 */
+	uint8_t file[128];	/* 108 */
+	uint32_t magic;		/* 236 */
+	uint8_t options[4];	/* 240 */
+};
+
+#define DHCP_VENDOR_MAGIC	0x63825363
+
+int dhcp_pack_packet(void *packet, size_t *len,
+		     const struct dhcp_option opt[256]);
+
+int dhcp_unpack_packet(const void *packet, size_t len,
+		       struct dhcp_option opt[256]);
+
+#endif /* DHCP_H */
+  
+
diff --git a/com32/lib/Makefile b/com32/lib/Makefile
index 62a322ab..eace321b 100644
--- a/com32/lib/Makefile
+++ b/com32/lib/Makefile
@@ -31,7 +31,7 @@ LIBOBJS = \
 	skipspace.o							\
 	chrreplace.o							\
 	bufprintf.o							\
-	inet.o								\
+	inet.o dhcppack.o dhcpunpack.o					\
 	strreplace.o								\
 	\
 	lmalloc.o lstrdup.o						\
diff --git a/com32/lib/dhcppack.c b/com32/lib/dhcppack.c
new file mode 100644
index 00000000..a08583c4
--- /dev/null
+++ b/com32/lib/dhcppack.c
@@ -0,0 +1,166 @@
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+// #include <arpa/inet.h>
+#include <netinet/in.h>
+
+// #include "dhcp.h"
+#include <dhcp.h>
+
+/*
+ * Pack DHCP options into an option field, without overload support.
+ * On return, len contains the number of active bytes, and the full
+ * field is zero-padded.
+ *
+ * Options which are successfully placed have their length zeroed out.
+ */
+static int dhcp_pack_field_zero(void *field, size_t *len,
+				struct dhcp_option opt[256])
+{
+	int i;
+	size_t xlen, plen;
+	const uint8_t *p;
+	uint8_t *q = field;
+	size_t spc = *len;
+	int err = 0;
+
+	if (!*len)
+		return ENOSPC;
+
+	for (i = 1; i < 255; i++) {
+		if (opt[i].len < 0)
+			continue;
+
+		/* We need to handle the 0 case as well as > 255 */
+		if (opt[i].len <= 255)
+			xlen = opt[i].len + 2;
+		else
+			xlen = opt[i].len + 2*((opt[i].len+254)/255);
+
+		p = opt[i].data;
+
+		if (xlen >= spc) {
+			/* This option doesn't fit... */
+			err++;
+			continue;
+		}
+
+		xlen = opt[i].len;
+		do {
+			*q++ = i;
+			*q++ = plen = xlen > 255 ? 255 : xlen;
+			if (plen)
+				memcpy(q, p, plen);
+			q += plen;
+			p += plen;
+			spc -= plen+2;
+			xlen -= plen;
+		} while (xlen);
+
+		opt[i].len = -1;
+	}
+
+	*q++ = 255;		/* End marker */
+	memset(q, 0, spc);	/* Zero-pad the rest of the field */
+	
+	*len = xlen = q - (uint8_t *)field;
+	return err;
+}
+
+/*
+ * Pack DHCP options into an option field, without overload support.
+ * On return, len contains the number of active bytes, and the full
+ * field is zero-padded.
+ *
+ * Use this to encode encapsulated option fields.
+ */
+int dhcp_pack_field(void *field, size_t *len,
+		    struct dhcp_option opt[256])
+{
+	struct dhcp_option ox[256];
+	
+	memcpy(ox, opt, sizeof ox);
+	return dhcp_pack_field_zero(field, len, ox);
+}
+
+/*
+ * Pack DHCP options into a packet.
+ * Apply overloading if (and only if) the "file" or "sname" option
+ * doesn't fit in the respective dedicated fields.
+ */
+int dhcp_pack_packet(void *packet, size_t *len,
+		     const struct dhcp_option opt[256])
+{
+	struct dhcp_packet *pkt = packet;
+	size_t spc = *len;
+	uint8_t overload;
+	struct dhcp_option ox[256];
+	uint8_t *q;
+	int err;
+
+	if (spc < sizeof(struct dhcp_packet))
+		return ENOSPC;	/* Buffer impossibly small */
+	
+	pkt->magic = htonl(DHCP_VENDOR_MAGIC);
+
+	memcpy(ox, opt, sizeof ox);
+
+	/* Figure out if we should do overloading or not */
+	overload = 0;
+
+	if (opt[67].len > 128)
+		overload |= 1;
+	else
+		ox[67].len = -1;
+
+	if (opt[66].len > 64)
+		overload |= 2;
+	else
+		ox[66].len = -1;
+
+	/* Kill any passed-in overload option */
+	ox[52].len = -1;
+
+	q = pkt->options;
+	spc -= 240;
+
+	/* Force option 53 (DHCP packet type) first */
+	if (ox[53].len == 1) {
+		*q++ = 53;
+		*q++ = 1;
+		*q++ = *(uint8_t *)ox[53].data;
+		spc -= 3;
+		ox[53].len = -1;
+	}
+
+	/* Follow with the overload option, if applicable */
+	if (overload) {
+		*q++ = 52;
+		*q++ = 1;
+		*q++ = overload;
+		spc -= 3;
+	}
+
+	err = dhcp_pack_field_zero(q, &spc, ox);
+	*len = spc + (q-(uint8_t *)packet);
+
+	if (overload & 1) {
+		spc = 128;
+		err = dhcp_pack_field_zero(pkt->file, &spc, ox);
+	} else {
+		memset(pkt->file, 0, 128);
+		if (opt[67].len > 0)
+			memcpy(pkt->file, opt[67].data, opt[67].len);
+	}
+
+	if (overload & 2) {
+		spc = 64;
+		err = dhcp_pack_field_zero(pkt->sname, &spc, ox);
+	} else {
+		memset(pkt->sname, 0, 64);
+		if (opt[66].len > 0)
+			memcpy(pkt->sname, opt[66].data, opt[66].len);
+	}
+
+	return err;
+}
diff --git a/com32/lib/dhcpunpack.c b/com32/lib/dhcpunpack.c
new file mode 100644
index 00000000..248173a8
--- /dev/null
+++ b/com32/lib/dhcpunpack.c
@@ -0,0 +1,116 @@
+#define _GNU_SOURCE		/* For strnlen() */
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+// #include <arpa/inet.h>
+#include <netinet/in.h>
+
+// #include "dhcp.h"
+#include <dhcp.h>
+
+/*
+ * Unpack DHCP options from a field.  Assumes opt is pre-initalized
+ * (to all zero in the common case.)
+ */
+int dhcp_unpack_field(const void *field, size_t len,
+		      struct dhcp_option opt[256])
+{
+	const uint8_t *p = field;
+	int err = 0;
+
+	while (len > 1) {
+		uint8_t op;
+		size_t xlen;
+
+		op = *p++; len--;
+		if (op == 0)
+			continue;
+		else if (op == 255)
+			break;
+		
+		xlen = *p++; len--;
+		if (xlen > len)
+			break;
+		if (opt[op].len < 0)
+			opt[op].len = 0;
+		if (xlen) {
+			opt[op].data = realloc(opt[op].data,
+					       opt[op].len + xlen + 1);
+			if (!opt[op].data) {
+				err = ENOMEM;
+				continue;
+			}
+			memcpy((char *)opt[op].data + opt[op].len, p, xlen);
+			opt[op].len += xlen;
+			/* Null-terminate as a courtesy to users */
+			*((char *)opt[op].data + opt[op].len) = 0;
+			p += xlen;
+			len -= xlen;
+		}
+	}
+
+	return err;
+}
+
+/*
+ * Unpack a DHCP packet, with overload support.  Do not use this
+ * to unpack an encapsulated option set.
+ */
+int dhcp_unpack_packet(const void *packet, size_t len,
+		       struct dhcp_option opt[256])
+{
+	const struct dhcp_packet *pkt = packet;
+	int err;
+	uint8_t overload;
+	int i;
+
+	if (len < 240 || pkt->magic != htonl(DHCP_VENDOR_MAGIC))
+		return EINVAL;	/* Bogus packet */
+	
+	for (i = 0; i < 256; i++) {
+		opt[i].len = -1; /* Option not present */
+		opt[i].data = NULL;
+	}
+	
+	err = dhcp_unpack_field(pkt->options, len-240, opt);
+
+	overload = 0;
+	if (opt[52].len == 1) {
+		overload = *(uint8_t *)opt[52].data;
+		free(opt[52].data);
+		opt[52].len = -1;
+		opt[52].data = NULL;
+	}
+
+	if (overload & 1) {
+		err |= dhcp_unpack_field(pkt->file, 128, opt);
+	} else {
+		opt[67].len  = strnlen((const char *)pkt->file, 128);
+		if (opt[67].len) {
+			opt[67].data = malloc(opt[67].len + 1);
+			if (opt[67].data) {
+				memcpy(opt[67].data, pkt->file, opt[67].len);
+				*((char *)opt[67].data + opt[67].len) = 0;
+			} else {
+				err |= ENOMEM;
+			}
+		}
+	}
+
+	if (overload & 2) {
+		err |= dhcp_unpack_field(pkt->sname, 64, opt);
+	} else {
+		opt[66].len  = strnlen((const char *)pkt->sname, 64);
+		if (opt[66].len) {
+			opt[66].data = malloc(opt[66].len + 1);
+			if (opt[66].data) {
+				memcpy(opt[66].data, pkt->file, opt[66].len);
+				*((char *)opt[66].data + opt[66].len) = 0;
+			} else {
+				err |= ENOMEM;
+			}
+		}
+	}
+
+	return err;
+}