com32/lib/dhcppack.c

#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;
}