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 */
dhcp_pack_field_zero(void * field,size_t * len,struct dhcp_option opt[256])17 static 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 */
dhcp_pack_field(void * field,size_t * len,struct dhcp_option opt[256])77 int 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 */
dhcp_pack_packet(void * packet,size_t * len,const struct dhcp_option opt[256])91 int 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