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 */ 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 */ 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 */ 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
