com32/lib/dhcppack.c

   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 }