1#include <stdlib.h>
2#include <errno.h>
3#include <string.h>
4// #include <arpa/inet.h>
5#include <netinet/in.h>
6
7// #include "dhcp.h"
8#include <dhcp.h>
9
10/*
11 * Pack DHCP options into an option field, without overload support.
12 * On return, len contains the number of active bytes, and the full
13 * field is zero-padded.
14 *
15 * Options which are successfully placed have their length zeroed out.
16 */
17static int dhcp_pack_field_zero(void *field, size_t *len,
18				struct dhcp_option opt[256])
19{
20	int i;
21	size_t xlen, plen;
22	const uint8_t *p;
23	uint8_t *q = field;
24	size_t spc = *len;
25	int err = 0;
26
27	if (!*len)
28		return ENOSPC;
29
30	for (i = 1; i < 255; i++) {
31		if (opt[i].len < 0)
32			continue;
33
34		/* We need to handle the 0 case as well as > 255 */
35		if (opt[i].len <= 255)
36			xlen = opt[i].len + 2;
37		else
38			xlen = opt[i].len + 2*((opt[i].len+254)/255);
39
40		p = opt[i].data;
41
42		if (xlen >= spc) {
43			/* This option doesn't fit... */
44			err++;
45			continue;
46		}
47
48		xlen = opt[i].len;
49		do {
50			*q++ = i;
51			*q++ = plen = xlen > 255 ? 255 : xlen;
52			if (plen)
53				memcpy(q, p, plen);
54			q += plen;
55			p += plen;
56			spc -= plen+2;
57			xlen -= plen;
58		} while (xlen);
59
60		opt[i].len = -1;
61	}
62
63	*q++ = 255;		/* End marker */
64	memset(q, 0, spc);	/* Zero-pad the rest of the field */
65
66	*len = xlen = q - (uint8_t *)field;
67	return err;
68}
69
70/*
71 * Pack DHCP options into an option field, without overload support.
72 * On return, len contains the number of active bytes, and the full
73 * field is zero-padded.
74 *
75 * Use this to encode encapsulated option fields.
76 */
77int dhcp_pack_field(void *field, size_t *len,
78		    struct dhcp_option opt[256])
79{
80	struct dhcp_option ox[256];
81
82	memcpy(ox, opt, sizeof ox);
83	return dhcp_pack_field_zero(field, len, ox);
84}
85
86/*
87 * Pack DHCP options into a packet.
88 * Apply overloading if (and only if) the "file" or "sname" option
89 * doesn't fit in the respective dedicated fields.
90 */
91int dhcp_pack_packet(void *packet, size_t *len,
92		     const struct dhcp_option opt[256])
93{
94	struct dhcp_packet *pkt = packet;
95	size_t spc = *len;
96	uint8_t overload;
97	struct dhcp_option ox[256];
98	uint8_t *q;
99	int err;
100
101	if (spc < sizeof(struct dhcp_packet))
102		return ENOSPC;	/* Buffer impossibly small */
103
104	pkt->magic = htonl(DHCP_VENDOR_MAGIC);
105
106	memcpy(ox, opt, sizeof ox);
107
108	/* Figure out if we should do overloading or not */
109	overload = 0;
110
111	if (opt[67].len > 128)
112		overload |= 1;
113	else
114		ox[67].len = -1;
115
116	if (opt[66].len > 64)
117		overload |= 2;
118	else
119		ox[66].len = -1;
120
121	/* Kill any passed-in overload option */
122	ox[52].len = -1;
123
124	q = pkt->options;
125	spc -= 240;
126
127	/* Force option 53 (DHCP packet type) first */
128	if (ox[53].len == 1) {
129		*q++ = 53;
130		*q++ = 1;
131		*q++ = *(uint8_t *)ox[53].data;
132		spc -= 3;
133		ox[53].len = -1;
134	}
135
136	/* Follow with the overload option, if applicable */
137	if (overload) {
138		*q++ = 52;
139		*q++ = 1;
140		*q++ = overload;
141		spc -= 3;
142	}
143
144	err = dhcp_pack_field_zero(q, &spc, ox);
145	*len = spc + (q-(uint8_t *)packet);
146
147	if (overload & 1) {
148		spc = 128;
149		err = dhcp_pack_field_zero(pkt->file, &spc, ox);
150	} else {
151		memset(pkt->file, 0, 128);
152		if (opt[67].len > 0)
153			memcpy(pkt->file, opt[67].data, opt[67].len);
154	}
155
156	if (overload & 2) {
157		spc = 64;
158		err = dhcp_pack_field_zero(pkt->sname, &spc, ox);
159	} else {
160		memset(pkt->sname, 0, 64);
161		if (opt[66].len > 0)
162			memcpy(pkt->sname, opt[66].data, opt[66].len);
163	}
164
165	return err;
166}
167