1 /* dhcpd.c - DHCP server for dynamic network configuration.
2  *
3  * Copyright 2013 Madhur Verma <mad.flexi@gmail.com>
4  * Copyright 2013 Kyungwan Han <asura321@gamil.com>
5  * Copyright 2015 Yeongdeok Suh <skyducks111@gmail.com>
6  *
7  * No Standard
8 USE_DHCPD(NEWTOY(dhcpd, ">1P#<0>65535fi:S46[!46]", TOYFLAG_SBIN|TOYFLAG_ROOTONLY))
9 
10 config DHCPD
11   bool "dhcpd"
12   default n
13   help
14    usage: dhcpd [-46fS] [-i IFACE] [-P N] [CONFFILE]
15 
16     -f    Run in foreground
17     -i Interface to use
18     -S    Log to syslog too
19     -P N  Use port N (default ipv4 67, ipv6 547)
20     -4, -6    Run as a DHCPv4 or DHCPv6 server
21 
22 config DEBUG_DHCP
23   bool "debugging messeges ON/OFF"
24   default n
25   depends on DHCPD
26 */
27 
28 /*
29  * TODO
30  * - Working as an relay agent
31  * - Rapid commit option support
32  * - Additional packet options (commented on the middle of sources)
33  * - Create common modules
34  */
35 
36 #define FOR_dhcpd
37 
38 #include "toys.h"
39 #include <linux/sockios.h>
40 #include <linux/if_ether.h>
41 
42 // Todo: headers not in posix
43 #include <netinet/ip.h>
44 #include <netinet/ip6.h>
45 #include <netinet/udp.h>
46 #include <netpacket/packet.h>
47 
48 #if CFG_DEBUG_DHCP==1
49 # define dbg(fmt, arg...)   printf(fmt, ##arg)
50 #else
51 # define dbg(fmt, arg...)
52 #endif
53 
54 #define LOG_SILENT          0x0
55 #define LOG_CONSOLE         0x1
56 #define LOG_SYSTEM          0x2
57 
58 #define DHCP_MAGIC          0x63825363
59 
60 #define DHCPDISCOVER        1
61 #define DHCPOFFER           2
62 #define DHCPREQUEST         3
63 #define DHCPDECLINE         4
64 #define DHCPACK             5
65 #define DHCPNAK             6
66 #define DHCPRELEASE         7
67 #define DHCPINFORM          8
68 
69 #define DHCP6SOLICIT        1
70 #define DHCP6ADVERTISE      2   // server -> client
71 #define DHCP6REQUEST        3
72 #define DHCP6CONFIRM        4
73 #define DHCP6RENEW          5
74 #define DHCP6REBIND         6
75 #define DHCP6REPLY          7   // server -> client
76 #define DHCP6RELEASE        8
77 #define DHCP6DECLINE        9
78 #define DHCP6RECONFIGURE    10  // server -> client
79 #define DHCP6INFOREQUEST    11
80 #define DHCP6RELAYFLOW      12  // relay -> relay/server
81 #define DHCP6RELAYREPLY     13  // server/relay -> relay
82 
83 #define DHCP_NUM8           (1<<8)
84 #define DHCP_NUM16          (1<<9)
85 #define DHCP_NUM32          DHCP_NUM16 | DHCP_NUM8
86 #define DHCP_STRING         (1<<10)
87 #define DHCP_STRLST         (1<<11)
88 #define DHCP_IP             (1<<12)
89 #define DHCP_IPLIST         (1<<13)
90 #define DHCP_IPPLST         (1<<14)
91 #define DHCP_STCRTS         (1<<15)
92 
93 // DHCP option codes (partial list). See RFC 2132 and
94 #define DHCP_OPT_PADDING                          0x00
95 #define DHCP_OPT_HOST_NAME          DHCP_STRING | 0x0c // either client informs server or server gives name to client
96 #define DHCP_OPT_REQUESTED_IP       DHCP_IP     | 0x32 // sent by client if specific IP is wanted
97 #define DHCP_OPT_LEASE_TIME         DHCP_NUM32  | 0x33
98 #define DHCP_OPT_OPTION_OVERLOAD                  0x34
99 #define DHCP_OPT_MESSAGE_TYPE       DHCP_NUM8   | 0x35
100 #define DHCP_OPT_SERVER_ID          DHCP_IP     | 0x36 // by default server's IP
101 #define DHCP_OPT_PARAM_REQ          DHCP_STRING | 0x37 // list of options client wants
102 #define DHCP_OPT_END                              0xff
103 
104 // DHCPv6 option codes (partial). See RFC 3315
105 #define DHCP6_OPT_CLIENTID      1
106 #define DHCP6_OPT_SERVERID      2
107 #define DHCP6_OPT_IA_NA         3
108 #define DHCP6_OPT_IA_ADDR       5
109 #define DHCP6_OPT_ORO           6
110 #define DHCP6_OPT_PREFERENCE    7
111 #define DHCP6_OPT_ELAPSED_TIME  8
112 #define DHCP6_OPT_RELAY_MSG     9
113 #define DHCP6_OPT_STATUS_CODE   13
114 #define DHCP6_OPT_IA_PD         25
115 #define DHCP6_OPT_IA_PREFIX     26
116 
117 #define DHCP6_STATUS_SUCCESS        0
118 #define DHCP6_STATUS_NOADDRSAVAIL   2
119 
120 #define DHCP6_DUID_LLT    1
121 #define DHCP6_DUID_EN     2
122 #define DHCP6_DUID_LL     3
123 #define DHCP6_DUID_UUID   4
124 
125 GLOBALS(
126     char *iface;
127     long port;
128 );
129 
130 struct config_keyword {
131   char *keyword;
132   int (*handler)(const char *str, void *var);
133   void *var;
134   char *def;
135 };
136 
137 typedef struct __attribute__((packed)) dhcp_msg_s {
138   uint8_t op;
139   uint8_t htype;
140   uint8_t hlen;
141   uint8_t hops;
142   uint32_t xid;
143   uint16_t secs;
144   uint16_t flags;
145   uint32_t ciaddr;
146   uint32_t yiaddr;
147   uint32_t nsiaddr;
148   uint32_t ngiaddr;
149   uint8_t chaddr[16];
150   uint8_t sname[64];
151   uint8_t file[128];
152   uint32_t cookie;
153   uint8_t options[308];
154 } dhcp_msg_t;
155 
156 typedef struct __attribute__((packed)) dhcp6_msg_s {
157   uint8_t msgtype;
158   uint8_t transaction_id[3];
159   uint8_t options[524];
160 } dhcp6_msg_t;
161 
162 typedef struct __attribute__((packed)) dhcp_raw_s {
163   struct iphdr iph;
164   struct udphdr udph;
165   dhcp_msg_t dhcp;
166 } dhcp_raw_t;
167 
168 typedef struct __attribute__((packed)) dhcp6_raw_s {
169   struct ip6_hdr iph;
170   struct udphdr udph;
171   dhcp6_msg_t dhcp6;
172 } dhcp6_raw_t;
173 
174 typedef struct static_lease_s {
175   struct static_lease_s *next;
176   uint32_t nip;
177   int mac[6];
178 } static_lease;
179 
180 typedef struct static_lease6_s {
181   struct static_lease6_s *next;
182   uint16_t duid_len;
183   uint16_t ia_type;
184   uint32_t iaid;
185   uint8_t nip6[16];
186   uint8_t duid[20];
187 } static_lease6;
188 
189 typedef struct {
190   uint32_t expires;
191   uint32_t lease_nip;
192   uint8_t lease_mac[6];
193   char hostname[20];
194   uint8_t pad[2];
195 } dyn_lease;
196 
197 typedef struct {
198   uint16_t duid_len;
199   uint16_t ia_type;
200   uint32_t expires;
201   uint32_t iaid;
202   uint8_t lease_nip6[16];
203   uint8_t duid[20];
204 } dyn_lease6;
205 
206 typedef struct option_val_s {
207   char *key;
208   uint16_t code;
209   void *val;
210   size_t len;
211 } option_val_t;
212 
213 struct __attribute__((packed)) optval_duid_llt {
214   uint16_t type;
215   uint16_t hwtype;
216   uint32_t time;
217   uint8_t lladdr[];   //flexible
218 };
219 
220 struct __attribute__((packed)) optval_ia_na {
221   uint32_t iaid;
222   uint32_t t1, t2;
223   uint8_t optval[];   //flexible
224 };
225 struct __attribute__((packed)) optval_ia_addr {
226   uint8_t ipv6_addr[16];
227   uint32_t pref_lifetime;
228   uint32_t valid_lifetime;
229 };
230 struct __attribute__((packed)) optval_status_code {
231   uint16_t status_code;
232   uint8_t status_msg[]; //flexible
233 };
234 
235 typedef struct __attribute__((__may_alias__)) server_config_s {
236   char *interface;                // interface to use
237   int ifindex;
238   uint8_t server_nip6[16];
239   uint32_t server_nip;
240   uint32_t port;
241   uint8_t server_mac[6];          // our MAC address (used only for ARP probing)
242   void *options[256];             // list of DHCP options loaded from the config file
243   /* start,end are in host order: we need to compare start <= ip <= end*/
244   uint32_t start_ip;              // start address of leases, in host order
245   uint32_t end_ip;                // end of leases, in host order
246   uint8_t start_ip6[16];          // start address of leases, in IPv6 mode
247   uint8_t end_ip6[16];            // end of leases, in IPv6 mode
248   uint32_t max_lease_sec;         // maximum lease time (host order)
249   uint32_t min_lease_sec;         // minimum lease time a client can request
250   uint32_t max_leases;            // maximum number of leases (including reserved addresses)
251   uint32_t auto_time;             // how long should dhcpd wait before writing a config file.
252                                   // if this is zero, it will only write one on SIGUSR1
253   uint32_t decline_time;          // how long an address is reserved if a client returns a
254                                   // decline message
255   uint32_t conflict_time;         // how long an arp conflict offender is leased for
256   uint32_t offer_time;            // how long an offered address is reserved
257   uint32_t siaddr_nip;            // "next server" bootp option
258   char *lease_file;
259   char *lease6_file;
260   char *pidfile;
261   char *notify_file;              // what to run whenever leases are written
262   char *sname;                    // bootp server name
263   char *boot_file;                // bootp boot file option
264   uint32_t pref_lifetime;
265   uint32_t valid_lifetime;
266   uint32_t t1,t2;
267   struct static_lease *static_leases; // List of ip/mac pairs to assign static leases
268 } server_config_t;
269 
270 typedef struct __attribute__((__may_alias__)) server_state_s {
271   uint8_t client_nip6[16];
272   uint32_t client_port;
273   uint8_t rqcode;
274   int listensock;
275   union {
276     dhcp_msg_t rcvd_pkt;
277     dhcp6_msg_t rcvd_pkt6;
278   } rcvd;
279   uint8_t* rqopt;
280   union {
281     dhcp_msg_t send_pkt;
282     dhcp6_msg_t send_pkt6;
283   } send;
284   union {
285     static_lease *sleases;
286     static_lease6 *sleases6;
287   } leases;
288   struct arg_list *dleases;
289 } server_state_t;
290 
291 static option_val_t options_list[] = {
292     {"lease"          , DHCP_NUM32  | 0x33, NULL, 0},
293     {"subnet"         , DHCP_IP     | 0x01, NULL, 0},
294     {"broadcast"      , DHCP_IP     | 0x1c, NULL, 0},
295     {"router"         , DHCP_IP     | 0x03, NULL, 0},
296     {"ipttl"          , DHCP_NUM8   | 0x17, NULL, 0},
297     {"mtu"            , DHCP_NUM16  | 0x1a, NULL, 0},
298     {"hostname"       , DHCP_STRING | 0x0c, NULL, 0},
299     {"domain"         , DHCP_STRING | 0x0f, NULL, 0},
300     {"search"         , DHCP_STRLST | 0x77, NULL, 0},
301     {"nisdomain"      , DHCP_STRING | 0x28, NULL, 0},
302     {"timezone"       , DHCP_NUM32  | 0x02, NULL, 0},
303     {"tftp"           , DHCP_STRING | 0x42, NULL, 0},
304     {"bootfile"       , DHCP_STRING | 0x43, NULL, 0},
305     {"bootsize"       , DHCP_NUM16  | 0x0d, NULL, 0},
306     {"rootpath"       , DHCP_STRING | 0x11, NULL, 0},
307     {"wpad"           , DHCP_STRING | 0xfc, NULL, 0},
308     {"serverid"       , DHCP_IP     | 0x36, NULL, 0},
309     {"message"        , DHCP_STRING | 0x38, NULL, 0},
310     {"vlanid"         , DHCP_NUM32  | 0x84, NULL, 0},
311     {"vlanpriority"   , DHCP_NUM32  | 0x85, NULL, 0},
312     {"dns"            , DHCP_IPLIST | 0x06, NULL, 0},
313     {"wins"           , DHCP_IPLIST | 0x2c, NULL, 0},
314     {"nissrv"         , DHCP_IPLIST | 0x29, NULL, 0},
315     {"ntpsrv"         , DHCP_IPLIST | 0x2a, NULL, 0},
316     {"lprsrv"         , DHCP_IPLIST | 0x09, NULL, 0},
317     {"swapsrv"        , DHCP_IP     | 0x10, NULL, 0},
318     {"routes"         , DHCP_STCRTS | 0x21, NULL, 0},
319     {"staticroutes"   , DHCP_STCRTS | 0x79, NULL, 0},
320     {"msstaticroutes" , DHCP_STCRTS | 0xf9, NULL, 0},
321 };
322 
323 struct fd_pair { int rd; int wr; };
324 static server_config_t gconfig;
325 static server_state_t gstate;
326 static uint8_t infomode;
327 static struct fd_pair sigfd;
328 static int constone = 1;
329 static sa_family_t addr_version = AF_INET;
330 
331 // calculate options size.
dhcp_opt_size(uint8_t * optionptr)332 static int dhcp_opt_size(uint8_t *optionptr)
333 {
334   int i = 0;
335   for(;optionptr[i] != 0xff; i++)
336     if(optionptr[i] != 0x00) i += optionptr[i + 1] + 2 -1;
337   return i;
338 }
339 
340 // calculates checksum for dhcp messeges.
dhcp_checksum(void * addr,int count)341 static uint16_t dhcp_checksum(void *addr, int count)
342 {
343   int32_t sum = 0;
344   uint16_t tmp = 0, *source = (uint16_t *)addr;
345 
346   while (count > 1)  {
347     sum += *source++;
348     count -= 2;
349   }
350   if (count > 0) {
351     *(uint8_t*)&tmp = *(uint8_t*)source;
352     sum += tmp;
353   }
354   while (sum >> 16) sum = (sum & 0xffff) + (sum >> 16);
355   return ~sum;
356 }
357 
358 // gets information of INTERFACE and updates IFINDEX, MAC and IP
get_interface(const char * interface,int * ifindex,void * oip,uint8_t * mac)359 static int get_interface(const char *interface, int *ifindex, void *oip,
360     uint8_t *mac)
361 {
362   struct ifreq req;
363   struct sockaddr_in *ip;
364   struct sockaddr_in6 ip6;
365   int fd = xsocket(addr_version, SOCK_RAW, IPPROTO_RAW);
366   char ipv6_addr[40] = {0,};
367 
368   req.ifr_addr.sa_family = addr_version;
369   xstrncpy(req.ifr_name, (char *)interface, IFNAMSIZ);
370 
371   xioctl(fd, SIOCGIFFLAGS, &req);
372 
373   if (!(req.ifr_flags & IFF_UP)) return -1;
374 
375   if (addr_version == AF_INET6) {
376 
377     FILE *fd6 = fopen("/proc/net/if_inet6", "r");
378     uint8_t *oip6 = (uint8_t*)oip;
379     int i;
380 
381     while(fgets(toybuf, sizeof(toybuf), fd6)) {
382       if (!strstr(toybuf, interface))
383         continue;
384 
385       if (sscanf(toybuf, "%32s \n", ipv6_addr) == 1)
386         break;
387     }
388     fclose(fd6);
389 
390     if (oip6) {
391       char *ptr = ipv6_addr+sizeof(ipv6_addr)-1;
392 
393       // convert giant hex string into colon-spearated ipv6 address by
394       // inserting ':' every 4 characters.
395       for (i = 32; i; i--)
396         if ((*(ptr--) = ipv6_addr[i])) if (!(i&3)) *(ptr--) = ':';
397 
398       dbg("ipv6 %s\n", ipv6_addr);
399       if(inet_pton(AF_INET6, ipv6_addr, &ip6.sin6_addr) <= 0)
400         error_msg("inet : the ipv6 address is not proper");
401       else
402         memcpy(oip6, ip6.sin6_addr.s6_addr32, sizeof(uint32_t)*4);
403     }
404   } else {
405     uint32_t *oip4 = (uint32_t*)oip;
406     if (oip4) {
407       xioctl(fd, SIOCGIFADDR, &req);
408       ip = (struct sockaddr_in*) &req.ifr_addr;
409       dbg("IP %s\n", inet_ntoa(ip->sin_addr));
410       *oip4 = ntohl(ip->sin_addr.s_addr);
411     }
412   }
413 
414   if (ifindex) {
415     xioctl(fd, SIOCGIFINDEX, &req);
416     dbg("Adapter index %d\n", req.ifr_ifindex);
417     *ifindex = req.ifr_ifindex;
418   }
419   if (mac) {
420     xioctl(fd, SIOCGIFHWADDR, &req);
421     memcpy(mac, req.ifr_hwaddr.sa_data, 6);
422     dbg("MAC %02x:%02x:%02x:%02x:%02x:%02x\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
423   }
424 
425   close(fd);
426   return 0;
427 }
428 
429 /*
430  *logs messeges to syslog or console
431  *opening the log is still left with applet.
432  *FIXME: move to more relevent lib. probably libc.c
433  */
infomsg(uint8_t infomode,char * s,...)434 static void infomsg(uint8_t infomode, char *s, ...)
435 {
436   int used;
437   char *msg;
438   va_list p, t;
439 
440   if (infomode == LOG_SILENT) return;
441   va_start(p, s);
442   va_copy(t, p);
443   used = vsnprintf(NULL, 0, s, t);
444   used++;
445   va_end(t);
446 
447   msg = xmalloc(used);
448   vsnprintf(msg, used, s, p);
449   va_end(p);
450 
451   if (infomode & LOG_SYSTEM) syslog(LOG_INFO, "%s", msg);
452   if (infomode & LOG_CONSOLE) printf("%s\n", msg);
453   free(msg);
454 }
455 
456 /*
457  * Writes self PID in file PATH
458  * FIXME: libc implementation only writes in /var/run
459  * this is more generic as some implemenation may provide
460  * arguments to write in specific file. as dhcpd does.
461  */
write_pid(char * path)462 static void write_pid(char *path)
463 {
464   int pidfile = open(path, O_CREAT | O_WRONLY | O_TRUNC, 0666);
465   if (pidfile > 0) {
466     char pidbuf[12];
467 
468     sprintf(pidbuf, "%u", (unsigned)getpid());
469     write(pidfile, pidbuf, strlen(pidbuf));
470     close(pidfile);
471   }
472 }
473 
474 // Generic signal handler real handling is done in main funcrion.
signal_handler(int sig)475 static void signal_handler(int sig)
476 {
477   unsigned char ch = sig;
478   if (write(sigfd.wr, &ch, 1) != 1) dbg("can't send signal\n");
479 }
480 
481 // signal setup for SIGUSR1 SIGTERM
setup_signal()482 static int setup_signal()
483 {
484   if (pipe((int *)&sigfd) < 0) {
485     dbg("signal pipe failed\n");
486     return -1;
487   }
488   fcntl(sigfd.wr , F_SETFD, FD_CLOEXEC);
489   fcntl(sigfd.rd , F_SETFD, FD_CLOEXEC);
490   int flags = fcntl(sigfd.wr, F_GETFL);
491   fcntl(sigfd.wr, F_SETFL, flags | O_NONBLOCK);
492   signal(SIGUSR1, signal_handler);
493   signal(SIGTERM, signal_handler);
494   return 0;
495 }
496 
497 // String STR to UINT32 conversion strored in VAR
strtou32(const char * str,void * var)498 static int strtou32(const char *str, void *var)
499 {
500   char *endptr = NULL;
501   int base = 10;
502   errno=0;
503   *((uint32_t*)(var)) = 0;
504   if (str[0]=='0' && (str[1]=='x' || str[1]=='X')) {
505     base = 16;
506     str+=2;
507   }
508 
509   long ret_val = strtol(str, &endptr, base);
510   if (errno) infomsg(infomode, "config : Invalid num %s",str);
511   else if (endptr && (*endptr!='\0'||endptr == str))
512       infomsg(infomode, "config : Not a valid num %s",str);
513   else *((uint32_t*)(var)) = (uint32_t)ret_val;
514   return 0;
515 }
516 
517 // copy string STR in variable VAR
strinvar(const char * str,void * var)518 static int strinvar(const char *str, void *var)
519 {
520   char **dest = var;
521   if (*dest) free(*dest);
522   *dest = strdup(str);
523   return 0;
524 }
525 
526 // IP String STR to binary data.
striptovar(const char * str,void * var)527 static int striptovar(const char *str, void *var)
528 {
529   *((uint32_t*)(var)) = 0;
530   if(!str) {
531     error_msg("config : NULL address string \n");
532     return -1;
533   }
534   if((inet_pton(AF_INET6, str, var)<=0) && (inet_pton(AF_INET, str, var)<=0)) {
535     error_msg("config : wrong address %s \n", str);
536     return -1;
537   }
538   return 0;
539 }
540 
541 // String to dhcp option conversion
strtoopt(const char * str,void * var)542 static int strtoopt(const char *str, void *var)
543 {
544   char *option, *valstr, *grp, *tp;
545   uint32_t optcode = 0, inf = infomode, convtmp, mask, nip, router;
546   uint16_t flag = 0;
547   int count, size = ARRAY_LEN(options_list);
548 
549   if (!*str) return 0;
550   if (!(option = strtok((char*)str, " \t="))) return -1;
551 
552   infomode = LOG_SILENT;
553   strtou32(option, (uint32_t*)&optcode);
554   infomode = inf;
555 
556   if (optcode > 0 && optcode < 256) { // raw option
557     for (count = 0; count < size; count++) {
558       if ((options_list[count].code & 0X00FF) == optcode) {
559         flag = (options_list[count].code & 0XFF00);
560         break;
561       }
562     }
563   } else { //string option
564     for (count = 0; count < size; count++) {
565       if (!strncmp(options_list[count].key, option, strlen(options_list[count].key))) {
566         flag = (options_list[count].code & 0XFF00);
567         optcode = (options_list[count].code & 0X00FF);
568         break;
569       }
570     }
571   }
572   if (count == size) {
573     infomsg(inf, "config : Obsolete OR Unknown Option : %s", option);
574     return -1;
575   }
576 
577   if (!flag || !optcode) return -1;
578 
579   if (!(valstr = strtok(NULL, " \t"))) {
580     dbg("config : option %s has no value defined.\n", option);
581     return -1;
582   }
583   dbg(" value : %-20s : ", valstr);
584   switch (flag) {
585   case DHCP_NUM32:
586     options_list[count].len = sizeof(uint32_t);
587     options_list[count].val = xmalloc(sizeof(uint32_t));
588     strtou32(valstr, &convtmp);
589     memcpy(options_list[count].val, &convtmp, sizeof(uint32_t));
590     break;
591   case DHCP_NUM16:
592     options_list[count].len = sizeof(uint16_t);
593     options_list[count].val = xmalloc(sizeof(uint16_t));
594     strtou32(valstr, &convtmp);
595     memcpy(options_list[count].val, &convtmp, sizeof(uint16_t));
596     break;
597   case DHCP_NUM8:
598     options_list[count].len = sizeof(uint8_t);
599     options_list[count].val = xmalloc(sizeof(uint8_t));
600     strtou32(valstr, &convtmp);
601     memcpy(options_list[count].val, &convtmp, sizeof(uint8_t));
602     break;
603   case DHCP_IP:
604     options_list[count].len = sizeof(uint32_t);
605     options_list[count].val = xmalloc(sizeof(uint32_t));
606     striptovar(valstr, options_list[count].val);
607     break;
608   case DHCP_STRING:
609     options_list[count].len = strlen(valstr);
610     options_list[count].val = strdup(valstr);
611     break;
612   case DHCP_IPLIST:
613     while(valstr){
614       options_list[count].val = xrealloc(options_list[count].val, options_list[count].len + sizeof(uint32_t));
615       striptovar(valstr, ((uint8_t*)options_list[count].val)+options_list[count].len);
616       options_list[count].len += sizeof(uint32_t);
617       valstr = strtok(NULL," \t");
618     }
619     break;
620   case DHCP_IPPLST:
621     break;
622   case DHCP_STCRTS:
623     /* Option binary format:
624      * mask [one byte, 0..32]
625      * ip [0..4 bytes depending on mask]
626      * router [4 bytes]
627      * may be repeated
628      * staticroutes 10.0.0.0/8 10.127.0.1, 10.11.12.0/24 10.11.12.1
629      */
630     grp = strtok(valstr, ",");;
631     while(grp){
632       while(*grp == ' ' || *grp == '\t') grp++;
633       tp = strchr(grp, '/');
634       if (!tp) error_exit("wrong formated static route option");
635       *tp = '\0';
636       mask = strtol(++tp, &tp, 10);
637       if (striptovar(grp, (uint8_t*)&nip)<0) error_exit("wrong formated static route option");
638       while(*tp == ' ' || *tp == '\t' || *tp == '-') tp++;
639       if (striptovar(tp, (uint8_t*)&router)<0) error_exit("wrong formated static route option");
640       options_list[count].val = xrealloc(options_list[count].val, options_list[count].len + 1 + mask/8 + 4);
641       memcpy(((uint8_t*)options_list[count].val)+options_list[count].len, &mask, 1);
642       options_list[count].len += 1;
643       memcpy(((uint8_t*)options_list[count].val)+options_list[count].len, &nip, mask/8);
644       options_list[count].len += mask/8;
645       memcpy(((uint8_t*)options_list[count].val)+options_list[count].len, &router, 4);
646       options_list[count].len += 4;
647       tp = NULL;
648       grp = strtok(NULL, ",");
649     }
650     break;
651   }
652   return 0;
653 }
654 
655 // Reads Static leases from STR and updates inner structures.
get_staticlease(const char * str,void * var)656 static int get_staticlease(const char *str, void *var)
657 {
658   struct static_lease_s *sltmp;
659   char *tkmac, *tkip;
660   int count;
661 
662   if (!*str) return 0;
663 
664   if (!(tkmac = strtok((char*)str, " \t"))) {
665     infomsg(infomode, "config : static lease : mac not found");
666     return 0;
667   }
668   if (!(tkip = strtok(NULL, " \t"))) {
669     infomsg(infomode, "config : static lease : no ip bind to mac %s", tkmac);
670     return 0;
671   }
672   sltmp = xzalloc(sizeof(struct static_lease_s));
673   for (count = 0; count < 6; count++, tkmac++) {
674     errno = 0;
675     sltmp->mac[count] = strtol(tkmac, &tkmac, 16);
676     if (sltmp->mac[count]>255 || sltmp->mac[count]<0 || (*tkmac && *tkmac!=':') || errno) {
677       infomsg(infomode, "config : static lease : mac address wrong format");
678       free(sltmp);
679       return 0;
680     }
681   }
682   striptovar(tkip, &sltmp->nip);
683   sltmp->next = gstate.leases.sleases;
684   gstate.leases.sleases = sltmp;
685 
686   return 0;
687 }
688 
689 static struct config_keyword keywords[] = {
690 // keyword          handler           variable address                default
691   {"start"        , striptovar      , (void*)&gconfig.start_ip     , "192.168.0.20"},
692   {"end"          , striptovar      , (void*)&gconfig.end_ip       , "192.168.0.254"},
693   {"interface"    , strinvar        , (void*)&gconfig.interface    , "eth0"},
694   {"port"         , strtou32        , (void*)&gconfig.port         , "67"},
695   {"min_lease"    , strtou32        , (void*)&gconfig.min_lease_sec, "60"},
696   {"max_leases"   , strtou32        , (void*)&gconfig.max_leases   , "235"},
697   {"auto_time"    , strtou32        , (void*)&gconfig.auto_time    , "7200"},
698   {"decline_time" , strtou32        , (void*)&gconfig.decline_time , "3600"},
699   {"conflict_time", strtou32        , (void*)&gconfig.conflict_time, "3600"},
700   {"offer_time"   , strtou32        , (void*)&gconfig.offer_time   , "60"},
701   {"lease_file"   , strinvar        , (void*)&gconfig.lease_file   , "/var/lib/misc/dhcpd.leases"}, //LEASES_FILE
702   {"lease6_file"  , strinvar        , (void*)&gconfig.lease6_file  , "/var/lib/misc/dhcpd6.leases"}, //LEASES_FILE
703   {"pidfile"      , strinvar        , (void*)&gconfig.pidfile      , "/var/run/dhcpd.pid"}, //DPID_FILE
704   {"siaddr"       , striptovar      , (void*)&gconfig.siaddr_nip   , "0.0.0.0"},
705   {"option"       , strtoopt        , (void*)&gconfig.options      , ""},
706   {"opt"          , strtoopt        , (void*)&gconfig.options      , ""},
707   {"notify_file"  , strinvar        , (void*)&gconfig.notify_file  , ""},
708   {"sname"        , strinvar        , (void*)&gconfig.sname        , ""},
709   {"boot_file"    , strinvar        , (void*)&gconfig.boot_file    , ""},
710   {"static_lease" , get_staticlease , (void*)&gconfig.static_leases, ""},
711   {"start6"       , striptovar      , (void*)&gconfig.start_ip6    , "2001:620:40b:555::100"},
712   {"end6"         , striptovar      , (void*)&gconfig.end_ip6      , "2001:620:40b:555::200"},
713   {"preferred_lifetime" , strtou32  , (void*)&gconfig.pref_lifetime, "3600"},
714   {"valid_lifetime"     , strtou32  , (void*)&gconfig.valid_lifetime, "7200"},
715   {"t1"           , strtou32        , (void*)&gconfig.t1           , "3600"},
716   {"t2"           , strtou32        , (void*)&gconfig.t2           , "5400"},
717 };
718 
719 // Parses the server config file and updates the global server config accordingly.
parse_server_config(char * config_file,struct config_keyword * confkey)720 static int parse_server_config(char *config_file, struct config_keyword *confkey)
721 {
722   FILE *fs = NULL;
723   char *confline_temp = NULL,*confline = NULL, *tk = NULL, *tokens[2] = {NULL, NULL};
724   int len, linelen, tcount, count, size = ARRAY_LEN(keywords);
725 
726   for (count = 0; count < size; count++)
727     if (confkey[count].handler)
728       confkey[count].handler(confkey[count].def, confkey[count].var);
729 
730   if (!(fs = fopen(config_file, "r"))) perror_msg("%s", config_file);
731   for (len = 0, linelen = 0; fs;) {
732     len = getline(&confline_temp, (size_t*) &linelen, fs);
733     confline = confline_temp;
734     if (len <= 0) break;
735     for (; *confline == ' '; confline++, len--);
736     if ((confline[0] == '#') || (confline[0] == '\n')) goto free_conf_continue;
737     tk = strchr(confline, '#');
738     if (tk) {
739       for (; *(tk-1)==' ' || *(tk-1)=='\t'; tk--);
740       *tk = '\0';
741     }
742     tk = strchr(confline, '\n');
743     if (tk) {
744       for (; *(tk-1)==' ' || *(tk-1)=='\t'; tk--);
745       *tk = '\0';
746     }
747     for (tcount=0, tk=strtok(confline, " \t"); tk && (tcount < 2);
748         tcount++, tk=strtok(NULL,(tcount==1)?"":" \t")) {
749       while ((*tk == '\t') || (*tk == ' ')) tk++;
750       tokens[tcount] = xstrdup(tk);
751     }
752     if (tcount<=1) goto free_tk0_continue;
753     for (count = 0; count < size; count++) {
754       if (!strcmp(confkey[count].keyword,tokens[0])) {
755         dbg("got config : %15s : ", confkey[count].keyword);
756         if (confkey[count].handler(tokens[1], confkey[count].var) == 0)
757           dbg("%s \n", tokens[1]);
758         break;
759       }
760     }
761     if (tokens[1]) { free(tokens[1]); tokens[1] = NULL; }
762 free_tk0_continue:
763     if (tokens[0]) { free(tokens[0]); tokens[0] = NULL; }
764 free_conf_continue:
765     free(confline_temp);
766     confline_temp = NULL;
767   }
768   if (fs) fclose(fs);
769   return 0;
770 }
771 
772 // opens UDP socket for listen ipv6 packets
open_listensock6(void)773 static int open_listensock6(void)
774 {
775   struct sockaddr_in6 addr6;
776   struct ipv6_mreq mreq;
777 
778   if (gstate.listensock > 0) close(gstate.listensock);
779 
780   dbg("Opening listen socket on *:%d %s\n", gconfig.port, gconfig.interface);
781 
782   gstate.listensock = xsocket(PF_INET6, SOCK_DGRAM, 0);
783   setsockopt(gstate.listensock, SOL_SOCKET, SO_REUSEADDR, &constone, sizeof(constone));
784   setsockopt(gstate.listensock, IPPROTO_IPV6, IPV6_CHECKSUM, &constone, sizeof(constone));
785 
786   if (setsockopt(gstate.listensock, IPPROTO_IPV6, IPV6_RECVPKTINFO, &constone,
787         sizeof(constone)) == -1) {
788     error_msg("failed to receive ipv6 packets.\n");
789     close(gstate.listensock);
790     return -1;
791   }
792 
793   setsockopt(gstate.listensock, SOL_SOCKET, SO_BINDTODEVICE, gconfig.interface, strlen(gconfig.interface)+1);
794 
795   memset(&addr6, 0, sizeof(addr6));
796   addr6.sin6_family = AF_INET6;
797   addr6.sin6_port = htons(gconfig.port); //SERVER_PORT
798   addr6.sin6_scope_id = if_nametoindex(gconfig.interface);
799   //Listening for multicast packet
800   inet_pton(AF_INET6, "ff02::1:2", &addr6.sin6_addr);
801 
802   if (bind(gstate.listensock, (struct sockaddr *) &addr6, sizeof(addr6)) == -1) {
803     close(gstate.listensock);
804     perror_exit("bind failed");
805   }
806 
807   memset(&mreq, 0, sizeof(mreq));
808   mreq.ipv6mr_interface = if_nametoindex(gconfig.interface);
809   memcpy(&mreq.ipv6mr_multiaddr, &addr6.sin6_addr, sizeof(addr6.sin6_addr));
810 
811   if(setsockopt(gstate.listensock, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) == -1) {
812     error_msg("failed to join a multicast group.\n");
813     close(gstate.listensock);
814     return -1;
815   }
816 
817   dbg("OPEN : success\n");
818   return 0;
819 }
820 
821 // opens UDP socket for listen
open_listensock(void)822 static int open_listensock(void)
823 {
824   struct sockaddr_in addr;
825   struct ifreq ifr;
826 
827   if (gstate.listensock > 0) close(gstate.listensock);
828 
829   dbg("Opening listen socket on *:%d %s\n", gconfig.port, gconfig.interface);
830   gstate.listensock = xsocket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
831   setsockopt(gstate.listensock, SOL_SOCKET, SO_REUSEADDR, &constone, sizeof(constone));
832   if (setsockopt(gstate.listensock, SOL_SOCKET, SO_BROADCAST, &constone, sizeof(constone)) == -1) {
833     error_msg("failed to receive brodcast packets.\n");
834     close(gstate.listensock);
835     return -1;
836   }
837   memset(&ifr, 0, sizeof(ifr));
838   xstrncpy(ifr.ifr_name, gconfig.interface, IFNAMSIZ);
839   setsockopt(gstate.listensock, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr));
840 
841   memset(&addr, 0, sizeof(addr));
842   addr.sin_family = AF_INET;
843   addr.sin_port = htons(gconfig.port); //SERVER_PORT
844   addr.sin_addr.s_addr = INADDR_ANY ;
845 
846   if (bind(gstate.listensock, (struct sockaddr *) &addr, sizeof(addr))) {
847     close(gstate.listensock);
848     perror_exit("bind failed");
849   }
850   dbg("OPEN : success\n");
851   return 0;
852 }
853 
send_packet6(uint8_t relay,uint8_t * client_lla,uint16_t optlen)854 static int send_packet6(uint8_t relay, uint8_t *client_lla, uint16_t optlen)
855 {
856   struct sockaddr_ll dest_sll;
857   dhcp6_raw_t packet;
858   unsigned padding;
859   int fd, result = -1;
860 
861   memset(&packet, 0, sizeof(dhcp6_raw_t));
862   memcpy(&packet.dhcp6, &gstate.send.send_pkt6, sizeof(dhcp6_msg_t));
863   padding = sizeof(packet.dhcp6.options) - optlen;
864 
865   if ((fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IPV6))) < 0) {
866     dbg("SEND : ipv6 socket failed\n");
867     return -1;
868   }
869   memset(&dest_sll, 0, sizeof(dest_sll));
870   dest_sll.sll_family = AF_PACKET;
871   dest_sll.sll_protocol = htons(ETH_P_IPV6);
872   dest_sll.sll_ifindex = gconfig.ifindex;
873   dest_sll.sll_halen = ETH_ALEN;
874   memcpy(dest_sll.sll_addr, client_lla, sizeof(uint8_t)*6);
875 
876   if (bind(fd, (struct sockaddr *) &dest_sll, sizeof(dest_sll)) < 0) {
877     dbg("SEND : bind failed\n");
878     close(fd);
879     return -1;
880   }
881   memcpy(&packet.iph.ip6_src, &gconfig.server_nip6, sizeof(uint32_t)*4);
882   memcpy(&packet.iph.ip6_dst, &gstate.client_nip6, sizeof(uint32_t)*4);
883 
884   packet.udph.source = htons(gconfig.port); //SERVER_PORT
885   packet.udph.dest = gstate.client_port; //CLIENT_PORT
886   packet.udph.len = htons(sizeof(dhcp6_raw_t) - sizeof(struct ip6_hdr) - padding);
887   packet.iph.ip6_ctlun.ip6_un1.ip6_un1_plen = htons(ntohs(packet.udph.len) + 0x11);
888   packet.udph.check = dhcp_checksum(&packet, sizeof(dhcp6_raw_t) - padding);
889   packet.iph.ip6_ctlun.ip6_un1.ip6_un1_flow = htonl(0x60000000);
890   packet.iph.ip6_ctlun.ip6_un1.ip6_un1_plen = packet.udph.len;
891   packet.iph.ip6_ctlun.ip6_un1.ip6_un1_nxt = IPPROTO_UDP;
892   packet.iph.ip6_ctlun.ip6_un1.ip6_un1_hlim = 0x64;
893 
894   result = sendto(fd, &packet, sizeof(dhcp6_raw_t)-padding,
895       0, (struct sockaddr *) &dest_sll, sizeof(dest_sll));
896 
897   dbg("sendto %d\n", result);
898   close(fd);
899   if (result < 0) dbg("PACKET send error\n");
900   return result;
901 }
902 
903 // Sends data through raw socket.
send_packet(uint8_t broadcast)904 static int send_packet(uint8_t broadcast)
905 {
906   struct sockaddr_ll dest_sll;
907   dhcp_raw_t packet;
908   unsigned padding;
909   int fd, result = -1;
910   uint8_t bmacaddr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
911 
912   memset(&packet, 0, sizeof(dhcp_raw_t));
913   memcpy(&packet.dhcp, &gstate.send.send_pkt, sizeof(dhcp_msg_t));
914 
915   if ((fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP))) < 0) {
916     dbg("SEND : socket failed\n");
917     return -1;
918   }
919   memset(&dest_sll, 0, sizeof(dest_sll));
920   dest_sll.sll_family = AF_PACKET;
921   dest_sll.sll_protocol = htons(ETH_P_IP);
922   dest_sll.sll_ifindex = gconfig.ifindex;
923   dest_sll.sll_halen = 6;
924   memcpy(dest_sll.sll_addr, (broadcast)?bmacaddr:gstate.rcvd.rcvd_pkt.chaddr , 6);
925 
926   if (bind(fd, (struct sockaddr *) &dest_sll, sizeof(dest_sll)) < 0) {
927     dbg("SEND : bind failed\n");
928     close(fd);
929     return -1;
930   }
931   padding = 308 - 1 - dhcp_opt_size(gstate.send.send_pkt.options);
932   packet.iph.protocol = IPPROTO_UDP;
933   packet.iph.saddr = gconfig.server_nip;
934   packet.iph.daddr = (broadcast || (gstate.rcvd.rcvd_pkt.ciaddr == 0))?
935     INADDR_BROADCAST : gstate.rcvd.rcvd_pkt.ciaddr;
936   packet.udph.source = htons(gconfig.port);//SERVER_PORT
937   packet.udph.dest = gstate.client_port; //CLIENT_PORT
938   packet.udph.len = htons(sizeof(dhcp_raw_t) - sizeof(struct iphdr) - padding);
939   packet.iph.tot_len = packet.udph.len;
940   packet.udph.check = dhcp_checksum(&packet, sizeof(dhcp_raw_t) - padding);
941   packet.iph.tot_len = htons(sizeof(dhcp_raw_t) - padding);
942   packet.iph.ihl = sizeof(packet.iph) >> 2;
943   packet.iph.version = IPVERSION;
944   packet.iph.ttl = IPDEFTTL;
945   packet.iph.check = dhcp_checksum(&packet.iph, sizeof(packet.iph));
946 
947   result = sendto(fd, &packet, sizeof(dhcp_raw_t) - padding, 0,
948       (struct sockaddr *) &dest_sll, sizeof(dest_sll));
949 
950   dbg("sendto %d\n", result);
951   close(fd);
952   if (result < 0) dbg("PACKET send error\n");
953   return result;
954 }
955 
read_packet6(void)956 static int read_packet6(void)
957 {
958   int ret;
959   struct sockaddr_in6 c_addr;
960   socklen_t c_addr_size = sizeof(c_addr);
961 
962   memset(&gstate.rcvd.rcvd_pkt6, 0, sizeof(dhcp6_msg_t));
963   ret = recvfrom(gstate.listensock, &gstate.rcvd.rcvd_pkt6, sizeof(dhcp6_msg_t),
964       0, (struct sockaddr*) &c_addr, &c_addr_size);
965   memcpy(gstate.client_nip6, &c_addr.sin6_addr, sizeof(uint32_t)*4);
966   memcpy(&gstate.client_port, &c_addr.sin6_port, sizeof(uint32_t));
967   if (ret < 0) {
968     dbg("Packet read error, ignoring. \n");
969     return ret; // returns -1
970   }
971   if (gstate.rcvd.rcvd_pkt6.msgtype < 1) {
972     dbg("Bad message type, igroning. \n");
973     return -2;
974   }
975 
976   dbg("Received an ipv6 packet. Size : %d \n", ret);
977   return ret;
978 }
979 
980 // Reads from UDP socket
read_packet(void)981 static int read_packet(void)
982 {
983   int ret;
984   struct sockaddr_in c_addr;
985   socklen_t c_addr_size = sizeof(c_addr);
986 
987   memset(&gstate.rcvd.rcvd_pkt, 0, sizeof(dhcp_msg_t));
988   ret = recvfrom(gstate.listensock, &gstate.rcvd.rcvd_pkt, sizeof(dhcp_msg_t),
989       0, (struct sockaddr*) &c_addr, &c_addr_size);
990   memcpy(&gstate.client_port, &c_addr.sin_port, sizeof(uint32_t));
991   /*ret = read(gstate.listensock, &gstate.rcvd.rcvd_pkt, sizeof(dhcp_msg_t));*/
992   if (ret < 0) {
993     dbg("Packet read error, ignoring. \n");
994     return ret; // returns -1
995   }
996   if (gstate.rcvd.rcvd_pkt.cookie != htonl(DHCP_MAGIC)) {
997     dbg("Packet with bad magic, ignoring. \n");
998     return -2;
999   }
1000   if (gstate.rcvd.rcvd_pkt.op != 1) { //BOOTPREQUEST
1001     dbg("Not a BOOT REQUEST ignoring. \n");
1002     return -2;
1003   }
1004   if (gstate.rcvd.rcvd_pkt.hlen != 6) {
1005     dbg("hlen != 6 ignoring. \n");
1006     return -2;
1007   }
1008   dbg("Received a packet. Size : %d \n", ret);
1009   return ret;
1010 }
1011 
1012 // Preapres a dhcp packet with defaults and configs
prepare_send_pkt(void)1013 static uint8_t* prepare_send_pkt(void)
1014 {
1015   memset((void*)&gstate.send.send_pkt, 0, sizeof(gstate.send.send_pkt));
1016   gstate.send.send_pkt.op = 2; //BOOTPREPLY
1017   gstate.send.send_pkt.htype = 1;
1018   gstate.send.send_pkt.hlen = 6;
1019   gstate.send.send_pkt.xid = gstate.rcvd.rcvd_pkt.xid;
1020   gstate.send.send_pkt.cookie = htonl(DHCP_MAGIC);
1021   gstate.send.send_pkt.nsiaddr = gconfig.server_nip;
1022   memcpy(gstate.send.send_pkt.chaddr, gstate.rcvd.rcvd_pkt.chaddr, 16);
1023   gstate.send.send_pkt.options[0] = DHCP_OPT_END;
1024   return gstate.send.send_pkt.options;
1025 }
1026 
prepare_send_pkt6(uint16_t opt)1027 static uint8_t* prepare_send_pkt6(uint16_t opt)
1028 {
1029   memset((void*)&gstate.send.send_pkt6, 0, sizeof(gstate.send.send_pkt6));
1030   gstate.send.send_pkt6.msgtype = opt;
1031   memcpy(gstate.send.send_pkt6.transaction_id, gstate.rcvd.rcvd_pkt6.transaction_id, 3);
1032   return gstate.send.send_pkt6.options;
1033 }
1034 
1035 // Sets a option value in dhcp packet's option field
set_optval(uint8_t * optptr,uint16_t opt,void * var,size_t len)1036 static uint8_t* set_optval(uint8_t *optptr, uint16_t opt, void *var, size_t len)
1037 {
1038   while (*optptr != DHCP_OPT_END) optptr++;
1039   *optptr++ = (uint8_t)(opt & 0x00FF);
1040   *optptr++ = (uint8_t) len;
1041   memcpy(optptr, var, len);
1042   optptr += len;
1043   *optptr = DHCP_OPT_END;
1044   return optptr;
1045 }
1046 
set_optval6(uint8_t * optptr,uint16_t opt,void * var,size_t len)1047 static uint8_t* set_optval6(uint8_t *optptr, uint16_t opt, void *var, size_t len)
1048 {
1049   *((uint16_t*)optptr) = htons(opt);
1050   *(uint16_t*)(optptr+2) = htons(len);
1051   memcpy(optptr+4, var, len);
1052   optptr += len+4;
1053   return optptr;
1054 }
1055 
1056 // Gets a option value from dhcp packet's option field
get_optval(uint8_t * optptr,uint16_t opt,void * var)1057 static uint8_t* get_optval(uint8_t *optptr, uint16_t opt, void *var)
1058 {
1059   size_t len;
1060   uint8_t overloaded = 0;
1061 
1062   while (1) {
1063     while (*optptr == DHCP_OPT_PADDING) optptr++;
1064     if ((*optptr & 0x00FF) == DHCP_OPT_END) break;
1065     if ((*optptr & 0x00FF) == DHCP_OPT_OPTION_OVERLOAD) {
1066       overloaded = optptr[2];
1067       optptr += optptr[1] + 2;
1068     }
1069     len = optptr[1];
1070     if (*optptr == (opt & 0x00FF))
1071       switch (opt & 0xFF00) {
1072         case DHCP_NUM32: // FALLTHROUGH
1073         case DHCP_IP:
1074           memcpy(var, optptr+2, sizeof(uint32_t));
1075           optptr += len + 2;
1076           return optptr;
1077           break;
1078         case DHCP_NUM16:
1079           memcpy(var, optptr+2, sizeof(uint16_t));
1080           optptr += len + 2;
1081           return optptr;
1082           break;
1083         case DHCP_NUM8:
1084           memcpy(var, optptr+2, sizeof(uint8_t));
1085           optptr += len + 2;
1086           return optptr;
1087           break;
1088         case DHCP_STRING:
1089           var = xstrndup((char*) optptr, len);
1090           optptr += len + 2;
1091           return optptr;
1092           break;
1093       }
1094     optptr += len + 2;
1095   }
1096   if ((overloaded == 1) | (overloaded == 3)) get_optval((uint8_t*)&gstate.rcvd.rcvd_pkt.file, opt, var);
1097   if ((overloaded == 2) | (overloaded == 3)) get_optval((uint8_t*)&gstate.rcvd.rcvd_pkt.sname, opt, var);
1098   return optptr;
1099 }
1100 
get_optval6(uint8_t * optptr,uint16_t opt,uint16_t * datalen,void ** var)1101 static uint8_t* get_optval6(uint8_t *optptr, uint16_t opt, uint16_t *datalen, void **var)
1102 {
1103   uint16_t optcode;
1104   uint16_t len;
1105 
1106   memcpy(&optcode, optptr, sizeof(uint16_t));
1107   memcpy(&len, optptr+2, sizeof(uint16_t));
1108   if(!optcode) {
1109     dbg("Option %d is not exist.\n", opt);
1110     return optptr;
1111   }
1112   optcode = ntohs(optcode);
1113   len = ntohs(len);
1114 
1115   if (opt == optcode) {
1116     *var = xmalloc(len);
1117     memcpy(*var, optptr+4, len);
1118     optptr = optptr + len + 4;
1119     memcpy(datalen, &len, sizeof(uint16_t));
1120   }
1121   else {
1122     optptr = get_optval6(optptr+len+4, opt, datalen, var);
1123   }
1124 
1125   return optptr;
1126 }
1127 
1128 // Retrives Requested Parameter list from dhcp req packet.
get_reqparam(uint8_t ** list)1129 static uint8_t get_reqparam(uint8_t **list)
1130 {
1131   uint8_t len, *optptr;
1132   if(*list) free(*list);
1133   for (optptr = gstate.rcvd.rcvd_pkt.options;
1134       *optptr && *optptr!=((DHCP_OPT_PARAM_REQ) & 0x00FF); optptr+=optptr[1]+2);
1135   len = *++optptr;
1136   *list = xzalloc(len+1);
1137   memcpy(*list, ++optptr, len);
1138   return len;
1139 }
1140 
1141 // Sets values of req param in dhcp offer packet.
set_reqparam(uint8_t * optptr,uint8_t * list)1142 static uint8_t* set_reqparam(uint8_t *optptr, uint8_t *list)
1143 {
1144   uint8_t reqcode;
1145   int count, size = ARRAY_LEN(options_list);
1146 
1147   while (*list) {
1148     reqcode = *list++;
1149     for (count = 0; count < size; count++) {
1150       if ((options_list[count].code & 0X00FF)==reqcode) {
1151         if (!(options_list[count].len) || !(options_list[count].val)) break;
1152         for (; *optptr && *optptr!=DHCP_OPT_END; optptr+=optptr[1]+2);
1153         *optptr++ = (uint8_t) (options_list[count].code & 0x00FF);
1154         *optptr++ = (uint8_t) options_list[count].len;
1155         memcpy(optptr, options_list[count].val, options_list[count].len);
1156         optptr += options_list[count].len;
1157         *optptr = DHCP_OPT_END;
1158         break;
1159       }
1160     }
1161   }
1162   return optptr;
1163 }
1164 
run_notify(char ** argv)1165 static void run_notify(char **argv)
1166 {
1167   struct stat sts;
1168   volatile int error = 0;
1169   pid_t pid;
1170 
1171   if (stat(argv[0], &sts) == -1 && errno == ENOENT) {
1172     infomsg(infomode, "notify file: %s : not exist.", argv[0]);
1173     return;
1174   }
1175   fflush(NULL);
1176 
1177   pid = vfork();
1178   if (pid < 0) {
1179     dbg("Fork failed.\n");
1180     return;
1181   }
1182   if (!pid) {
1183     execvp(argv[0], argv);
1184     error = errno;
1185     _exit(111);
1186   }
1187   if (error) {
1188     waitpid(pid, NULL, 0);
1189     errno = error;
1190   }
1191   dbg("script complete.\n");
1192 }
1193 
write_leasefile(void)1194 static void write_leasefile(void)
1195 {
1196   int fd;
1197   uint32_t curr, tmp_time;
1198   int64_t timestamp;
1199   struct arg_list *listdls = gstate.dleases;
1200   dyn_lease *dls;
1201 
1202   if ((fd = open(gconfig.lease_file, O_WRONLY | O_CREAT | O_TRUNC, 0600)) < 0) {
1203     perror_msg("can't open %s ", gconfig.lease_file);
1204   } else {
1205     curr = timestamp = time(NULL);
1206     timestamp = SWAP_BE64(timestamp);
1207     writeall(fd, &timestamp, sizeof(timestamp));
1208 
1209     while (listdls) {
1210       dls = (dyn_lease*)listdls->arg;
1211       tmp_time = dls->expires;
1212       dls->expires -= curr;
1213       if ((int32_t) dls->expires < 0) goto skip;
1214       dls->expires = htonl(dls->expires);
1215       writeall(fd, dls, sizeof(dyn_lease));
1216 skip:
1217       dls->expires = tmp_time;
1218       listdls = listdls->next;
1219     }
1220     close(fd);
1221     if (gconfig.notify_file) {
1222       char *argv[3];
1223       argv[0] = gconfig.notify_file;
1224       argv[1] = gconfig.lease_file;
1225       argv[2] = NULL;
1226       run_notify(argv);
1227     }
1228   }
1229 }
1230 
write_lease6file(void)1231 static void write_lease6file(void)
1232 {
1233   int fd;
1234   uint32_t curr, tmp_time;
1235   int64_t timestamp;
1236   struct arg_list *listdls = gstate.dleases;
1237   dyn_lease6 *dls6;
1238 
1239   if ((fd = open(gconfig.lease6_file, O_WRONLY | O_CREAT | O_TRUNC, 0600)) < 0) {
1240     perror_msg("can't open %s ", gconfig.lease6_file);
1241   } else {
1242     curr = timestamp = time(NULL);
1243     timestamp = SWAP_BE64(timestamp);
1244     writeall(fd, &timestamp, sizeof(timestamp));
1245 
1246     while (listdls) {
1247       dls6 = (dyn_lease6*)listdls->arg;
1248       tmp_time = dls6->expires;
1249       dls6->expires -= curr;
1250       if ((int32_t) dls6->expires < 0) goto skip;
1251       dls6->expires = htonl(dls6->expires);
1252       writeall(fd, dls6, sizeof(dyn_lease6));
1253 skip:
1254       dls6->expires = tmp_time;
1255       listdls = listdls->next;
1256     }
1257     close(fd);
1258     if (gconfig.notify_file) {
1259       char *argv[3];
1260       argv[0] = gconfig.notify_file;
1261       argv[1] = gconfig.lease6_file;
1262       argv[2] = NULL;
1263       run_notify(argv);
1264     }
1265   }
1266 }
1267 
1268 // Update max lease time from options.
set_maxlease(void)1269 static void set_maxlease(void)
1270 {
1271   int count, size = ARRAY_LEN(options_list);
1272   for (count = 0; count < size; count++)
1273     if (options_list[count].val && options_list[count].code == (DHCP_OPT_LEASE_TIME)) {
1274       gconfig.max_lease_sec = *((uint32_t*)options_list[count].val);
1275       break;
1276     }
1277   if (!gconfig.max_lease_sec) gconfig.max_lease_sec = (60*60*24*10);// DEFAULT_LEASE_TIME;
1278 }
1279 
1280 // Returns lease time for client.
get_lease(uint32_t req_exp)1281 static uint32_t get_lease(uint32_t req_exp)
1282 {
1283   uint32_t now = time(NULL);
1284   req_exp = req_exp - now;
1285   if(addr_version == AF_INET6) {
1286     if ((req_exp <= 0) || req_exp > gconfig.pref_lifetime ||
1287         req_exp > gconfig.valid_lifetime) {
1288       if ((gconfig.pref_lifetime > gconfig.valid_lifetime)) {
1289         error_msg("The valid lifetime must be greater than the preferred lifetime, \
1290             setting to valid lifetime", gconfig.valid_lifetime);
1291         return gconfig.valid_lifetime;
1292       }
1293       return gconfig.pref_lifetime;
1294     }
1295   } else {
1296     if ((req_exp <= 0) || (req_exp > gconfig.max_lease_sec))
1297       return gconfig.max_lease_sec;
1298 
1299     if (req_exp < gconfig.min_lease_sec)
1300       return gconfig.min_lease_sec;
1301   }
1302 
1303   return req_exp;
1304 }
1305 
verifyip6_in_lease(uint8_t * nip6,uint8_t * duid,uint16_t ia_type,uint32_t iaid)1306 static int verifyip6_in_lease(uint8_t *nip6, uint8_t *duid, uint16_t ia_type, uint32_t iaid)
1307 {
1308   static_lease6 *sls6;
1309   struct arg_list *listdls;
1310 
1311   for (listdls = gstate.dleases; listdls; listdls = listdls->next) {
1312     if (!memcmp(((dyn_lease6*) listdls->arg)->lease_nip6, nip6, sizeof(uint32_t)*4))
1313       return -1;
1314 
1315     if (!memcmp(((dyn_lease6*) listdls->arg)->duid, duid, ((dyn_lease6*) listdls->arg)->duid_len)
1316         && ((dyn_lease6*) listdls->arg)->ia_type == ia_type)
1317       return -1;
1318   }
1319   for (sls6 = gstate.leases.sleases6; sls6; sls6 = sls6->next)
1320     if (memcmp(sls6->nip6, nip6, sizeof(uint32_t)*4)==0) return -2;
1321 
1322   if (memcmp(nip6, gconfig.start_ip6, sizeof(uint32_t)*4) < 0 ||
1323       memcmp(nip6, gconfig.end_ip6, sizeof(uint32_t)*4) > 0)
1324     return -3;
1325 
1326   return 0;
1327 }
1328 
1329 // Verify ip NIP in current leases ( assigned or not)
verifyip_in_lease(uint32_t nip,uint8_t mac[6])1330 static int verifyip_in_lease(uint32_t nip, uint8_t mac[6])
1331 {
1332   static_lease *sls;
1333   struct arg_list *listdls;
1334 
1335   for (listdls = gstate.dleases; listdls; listdls = listdls->next) {
1336     if (((dyn_lease*) listdls->arg)->lease_nip == nip) {
1337       if (((int32_t)(((dyn_lease*) listdls->arg)->expires) - time(NULL)) < 0)
1338         return 0;
1339       return -1;
1340     }
1341     if (!memcmp(((dyn_lease*) listdls->arg)->lease_mac, mac, 6)) return -1;
1342   }
1343   for (sls = gstate.leases.sleases; sls; sls = sls->next)
1344     if (sls->nip == nip) return -2;
1345 
1346   if ((ntohl(nip) < gconfig.start_ip) || (ntohl(nip) > gconfig.end_ip))
1347     return -3;
1348 
1349   return 0;
1350 }
1351 
1352 // add ip assigned_nip to dynamic lease.
addip_to_lease(uint32_t assigned_nip,uint8_t mac[6],uint32_t * req_exp,char * hostname,uint8_t update)1353 static int addip_to_lease(uint32_t assigned_nip, uint8_t mac[6], uint32_t *req_exp, char *hostname, uint8_t update)
1354 {
1355   dyn_lease *dls;
1356   struct arg_list *listdls = gstate.dleases;
1357   uint32_t now = time(NULL);
1358 
1359   while (listdls) {
1360     if (!memcmp(((dyn_lease*) listdls->arg)->lease_mac, mac, 6)) {
1361       if (update) *req_exp = get_lease(*req_exp + ((dyn_lease*) listdls->arg)->expires);
1362       ((dyn_lease*) listdls->arg)->expires = *req_exp + now;
1363       return 0;
1364     }
1365     listdls = listdls->next;
1366   }
1367 
1368   dls = xzalloc(sizeof(dyn_lease));
1369   memcpy(dls->lease_mac, mac, 6);
1370   dls->lease_nip = assigned_nip;
1371   if (hostname) memcpy(dls->hostname, hostname, 20);
1372 
1373   if (update) *req_exp = get_lease(*req_exp + now);
1374   dls->expires = *req_exp + now;
1375 
1376   listdls = xzalloc(sizeof(struct arg_list));
1377   listdls->next = gstate.dleases;
1378   listdls->arg = (char*)dls;
1379   gstate.dleases = listdls;
1380 
1381   return 0;
1382 }
1383 
addip6_to_lease(uint8_t * assigned_nip,uint8_t * duid,uint16_t duid_len,uint16_t ia_type,uint32_t iaid,uint32_t * lifetime,uint8_t update)1384 static int addip6_to_lease(uint8_t *assigned_nip, uint8_t *duid, uint16_t duid_len, uint16_t ia_type, uint32_t iaid, uint32_t *lifetime, uint8_t update)
1385 {
1386   dyn_lease6 *dls6;
1387   struct arg_list *listdls = gstate.dleases;
1388   uint32_t now = time(NULL);
1389 
1390   while (listdls) {
1391     if (!memcmp(((dyn_lease6*) listdls->arg)->duid, duid, ((dyn_lease6*) listdls->arg)->duid_len)) {
1392       if (update) *lifetime = get_lease(*lifetime + ((dyn_lease6*) listdls->arg)->expires);
1393       ((dyn_lease6*) listdls->arg)->expires = *lifetime + now;
1394       return 0;
1395     }
1396     listdls = listdls->next;
1397   }
1398 
1399   dls6 = xzalloc(sizeof(dyn_lease6));
1400   dls6->duid_len = duid_len;
1401   memcpy(dls6->duid, duid, duid_len);
1402   dls6->ia_type = ia_type;
1403   dls6->iaid = iaid;
1404   memcpy(dls6->lease_nip6, assigned_nip, sizeof(uint32_t)*4);
1405 
1406   if (update) *lifetime = get_lease(*lifetime + now);
1407   dls6->expires = *lifetime + now;
1408 
1409   listdls = xzalloc(sizeof(struct arg_list));
1410   listdls->next = gstate.dleases;
1411   listdls->arg = (char*)dls6;
1412   gstate.dleases = listdls;
1413 
1414   return 0;
1415 }
1416 
1417 // delete ip assigned_nip from dynamic lease.
delip_from_lease(uint32_t assigned_nip,uint8_t mac[6],uint32_t del_time)1418 static int delip_from_lease(uint32_t assigned_nip, uint8_t mac[6], uint32_t del_time)
1419 {
1420   struct arg_list *listdls = gstate.dleases;
1421 
1422   while (listdls) {
1423     if (!memcmp(((dyn_lease*) listdls->arg)->lease_mac, mac, 6)) {
1424       ((dyn_lease*) listdls->arg)->expires = del_time + time(NULL);
1425       return 0;
1426     }
1427     listdls = listdls->next;
1428   }
1429   return -1;
1430 }
1431 
1432 // returns a IP from static, dynamic leases or free ip pool, 0 otherwise.
getip_from_pool(uint32_t req_nip,uint8_t mac[6],uint32_t * req_exp,char * hostname)1433 static uint32_t getip_from_pool(uint32_t req_nip, uint8_t mac[6], uint32_t *req_exp, char *hostname)
1434 {
1435   uint32_t nip = 0;
1436   static_lease *sls = gstate.leases.sleases;
1437   struct arg_list *listdls = gstate.dleases, *tmp = NULL;
1438 
1439   if (req_nip && (!verifyip_in_lease(req_nip, mac))) nip = req_nip;
1440 
1441   if (!nip) {
1442     while (listdls) {
1443       if (!memcmp(((dyn_lease*)listdls->arg)->lease_mac, mac, 6)) {
1444         nip = ((dyn_lease*)listdls->arg)->lease_nip;
1445         if (tmp) tmp->next = listdls->next;
1446         else gstate.dleases = listdls->next;
1447         free(listdls->arg);
1448         free(listdls);
1449         if (verifyip_in_lease(nip, mac) < 0) nip = 0;
1450         break;
1451       }
1452       tmp = listdls;
1453       listdls = listdls->next;
1454     }
1455   }
1456   if (!nip) {
1457     while (sls) {
1458       if (memcmp(sls->mac, mac, 6) == 0) {
1459         nip = sls->nip;
1460         break;
1461       }
1462       sls = sls->next;
1463     }
1464   }
1465   if (!nip) {
1466     for (nip = htonl(gconfig.start_ip); ntohl(nip) <= gconfig.end_ip; ) {
1467       if (!verifyip_in_lease(nip, mac)) break;
1468       nip = ntohl(nip);
1469       nip = htonl(++nip);
1470     }
1471     if (ntohl(nip) > gconfig.end_ip) {
1472       nip = 0;
1473       infomsg(infomode, "can't find free IP in IP Pool.");
1474     }
1475   }
1476   if (nip) addip_to_lease(nip, mac, req_exp, hostname, 1);
1477   return nip;
1478 }
1479 
getip6_from_pool(uint8_t * duid,uint16_t duid_len,uint16_t ia_type,uint32_t iaid,uint32_t * lifetime)1480 static uint8_t *getip6_from_pool(uint8_t *duid, uint16_t duid_len, uint16_t ia_type, uint32_t iaid, uint32_t *lifetime)
1481 {
1482   static uint8_t nip6[16] = {0, };
1483   static_lease6 *sls6 = gstate.leases.sleases6;
1484   struct arg_list *listdls6 = gstate.dleases, *tmp = NULL;
1485 
1486   while(listdls6) {
1487     if (!memcmp(((dyn_lease6*)listdls6->arg)->duid, duid, duid_len)) {
1488       memcpy(nip6, ((dyn_lease6*)listdls6->arg)->lease_nip6, sizeof(nip6));
1489       if(tmp) tmp->next = listdls6->next;
1490       else gstate.dleases = listdls6->next;
1491       free(listdls6->arg);
1492       free(listdls6);
1493 
1494       if(verifyip6_in_lease(nip6, duid, ia_type, iaid) < 0)
1495         memset(nip6, 0, sizeof(nip6));
1496       break;
1497     }
1498     tmp = listdls6;
1499     listdls6 = listdls6->next;
1500   }
1501 
1502   if(!memcmp(nip6, (uint8_t[16]){0}, sizeof(uint32_t)*4)) {
1503     while(sls6) {
1504       if(!memcmp(sls6->duid, duid, 6)) {
1505         memcpy(nip6, sls6->nip6, sizeof(nip6));
1506         break;
1507       }
1508       sls6 = sls6->next;
1509     }
1510   }
1511 
1512   if(!memcmp(nip6, (uint8_t[16]){0}, sizeof(uint32_t)*4)) {
1513     memcpy(nip6, gconfig.start_ip6, sizeof(nip6));
1514     while(memcmp(nip6, gconfig.end_ip6, sizeof(nip6)) < 0) {
1515       if(!verifyip6_in_lease(nip6, duid, ia_type, iaid)) break;
1516       int i=sizeof(nip6);
1517       while(i--) {
1518         ++nip6[i];
1519         if (!nip6[i]) {
1520           if(i==(sizeof(nip6)-1)) ++nip6[i];
1521           ++nip6[i-1];
1522         } else
1523           break;
1524       }
1525     }
1526 
1527     if (memcmp(nip6, gconfig.end_ip6, sizeof(nip6)) > 0) {
1528       memset(nip6, 0, sizeof(nip6));
1529       infomsg(infomode, "can't find free IP in IPv6 Pool.");
1530     }
1531   }
1532 
1533   if(memcmp(nip6, (uint8_t[16]){0}, sizeof(uint32_t)*4)) {
1534     addip6_to_lease(nip6, duid, duid_len, ia_type, iaid, lifetime, 1);
1535     infomsg(infomode, "Assigned IPv6 %02X%02X:%02X%02X:%02X%02X:%02X%02X:%02X%02X:%02X%02X:%02X%02X:%02X%02X",
1536         nip6[0], nip6[1], nip6[2], nip6[3], nip6[4], nip6[5], nip6[6], nip6[7], nip6[8],
1537         nip6[9], nip6[10], nip6[11], nip6[12], nip6[13], nip6[14], nip6[15]);
1538   }
1539   return nip6;
1540 }
1541 
read_leasefile(void)1542 static void read_leasefile(void)
1543 {
1544   uint32_t passed, ip;
1545   int32_t tmp_time;
1546   int64_t timestamp;
1547   dyn_lease *dls;
1548   int fd = open(gconfig.lease_file, O_RDONLY);
1549 
1550   dls = xzalloc(sizeof(dyn_lease));
1551 
1552   if (read(fd, &timestamp, sizeof(timestamp)) != sizeof(timestamp))
1553     goto lease_error_exit;
1554 
1555   timestamp = SWAP_BE64(timestamp);
1556   passed = time(NULL) - timestamp;
1557   if ((uint64_t)passed > 12 * 60 * 60) goto lease_error_exit;
1558 
1559   while (read(fd, dls, sizeof(dyn_lease)) == sizeof(dyn_lease)) {
1560     ip = ntohl(dls->lease_nip);
1561     if (ip >= gconfig.start_ip && ip <= gconfig.end_ip) {
1562       tmp_time = ntohl(dls->expires) - passed;
1563       if (tmp_time < 0) continue;
1564       addip_to_lease(dls->lease_nip, dls->lease_mac,
1565           (uint32_t*)&tmp_time, dls->hostname, 0);
1566     }
1567   }
1568 lease_error_exit:
1569   free(dls);
1570   close(fd);
1571 }
1572 
read_lease6file(void)1573 static void read_lease6file(void)
1574 {
1575   uint32_t passed;
1576   uint32_t tmp_time;
1577   int64_t timestamp;
1578   dyn_lease6 *dls6;
1579   int fd = open(gconfig.lease6_file, O_RDONLY);
1580 
1581   dls6 = xzalloc(sizeof(dyn_lease6));
1582 
1583   if (read(fd, &timestamp, sizeof(timestamp)) != sizeof(timestamp))
1584     goto lease6_error_exit;
1585 
1586   timestamp = SWAP_BE64(timestamp);
1587   passed = time(NULL) - timestamp;
1588   if ((uint64_t)passed > 12 * 60 * 60) goto lease6_error_exit;
1589 
1590   while (read(fd, dls6, sizeof(dyn_lease6)) == sizeof(dyn_lease6)) {
1591     if (memcmp(dls6->lease_nip6, gconfig.start_ip6, sizeof(uint32_t)*4) > 0 &&
1592         memcmp(dls6->lease_nip6, gconfig.end_ip6, sizeof(uint32_t)*4) < 0) {
1593       tmp_time = ntohl(dls6->expires) - passed;
1594       if (tmp_time < 0U) continue;
1595       addip6_to_lease(dls6->lease_nip6, dls6->duid, dls6->duid_len, dls6->ia_type, dls6->iaid,
1596           (uint32_t*)&tmp_time, 0);
1597     }
1598   }
1599 
1600 lease6_error_exit:
1601   free(dls6);
1602   close(fd);
1603 }
1604 
dhcpd_main(void)1605 void dhcpd_main(void)
1606 {
1607   struct timeval tv;
1608   int retval, i;
1609   uint8_t *optptr, msgtype = 0;
1610   uint16_t optlen = 0;
1611   uint32_t waited = 0, serverid = 0, requested_nip = 0;
1612   uint8_t transactionid[3] = {0,};
1613   uint32_t reqested_lease = 0, ip_pool_size = 0;
1614   char *hstname = NULL;
1615   fd_set rfds;
1616 
1617   infomode = LOG_CONSOLE;
1618   if (!(toys.optflags & FLAG_f)) {
1619     daemon(0,0);
1620     infomode = LOG_SILENT;
1621   }
1622   if (toys.optflags & FLAG_S) {
1623         openlog("UDHCPD :", LOG_PID, LOG_DAEMON);
1624         infomode |= LOG_SYSTEM;
1625   }
1626   setlinebuf(stdout);
1627   //DHCPD_CONF_FILE
1628   parse_server_config((toys.optc==1)?toys.optargs[0]:"/etc/dhcpd.conf", keywords);
1629   infomsg(infomode, "toybox dhcpd started");
1630 
1631   if (toys.optflags & FLAG_6){
1632     addr_version = AF_INET6;
1633     gconfig.t1 = ntohl(gconfig.t1);
1634     gconfig.t2 = ntohl(gconfig.t2);
1635     gconfig.pref_lifetime = ntohl(gconfig.pref_lifetime);
1636     gconfig.valid_lifetime = ntohl(gconfig.valid_lifetime);
1637     gconfig.port = 547;
1638     for(i=0;i<4;i++)
1639       ip_pool_size += (gconfig.end_ip6[i]-gconfig.start_ip6[i])<<((3-i)*8);
1640   } else {
1641     gconfig.start_ip = ntohl(gconfig.start_ip);
1642     gconfig.end_ip = ntohl(gconfig.end_ip);
1643     ip_pool_size = gconfig.end_ip - gconfig.start_ip + 1;
1644   }
1645 
1646   if (gconfig.max_leases > ip_pool_size) {
1647     error_msg("max_leases=%u is too big, setting to %u",
1648         (unsigned) gconfig.max_leases, ip_pool_size);
1649     gconfig.max_leases = ip_pool_size;
1650   }
1651   write_pid(gconfig.pidfile);
1652   set_maxlease();
1653   if(TT.iface) gconfig.interface = TT.iface;
1654   if(TT.port) gconfig.port = TT.port;
1655   (addr_version==AF_INET6) ? read_lease6file() : read_leasefile();
1656 
1657 
1658   if (get_interface(gconfig.interface, &gconfig.ifindex,
1659         (addr_version==AF_INET6)? (void*)gconfig.server_nip6 :
1660         (void*)&gconfig.server_nip, gconfig.server_mac) < 0)
1661     perror_exit("Failed to get interface %s", gconfig.interface);
1662   setup_signal();
1663   if (addr_version==AF_INET6) {
1664     open_listensock6();
1665   } else {
1666     gconfig.server_nip = htonl(gconfig.server_nip);
1667     open_listensock();
1668   }
1669 
1670   fcntl(gstate.listensock, F_SETFD, FD_CLOEXEC);
1671 
1672   for (;;) {
1673     uint32_t timestmp = time(NULL);
1674     FD_ZERO(&rfds);
1675     FD_SET(gstate.listensock, &rfds);
1676     FD_SET(sigfd.rd, &rfds);
1677     tv.tv_sec = gconfig.auto_time - waited;
1678     tv.tv_usec = 0;
1679     retval = 0;
1680     serverid = 0;
1681     msgtype = 0;
1682 
1683     int maxfd = (sigfd.rd > gstate.listensock)? sigfd.rd : gstate.listensock;
1684     dbg("select waiting ....\n");
1685     retval = select(maxfd + 1, &rfds, NULL, NULL, (gconfig.auto_time?&tv:NULL));
1686     if (retval < 0) {
1687       if (errno == EINTR) {
1688         waited += (unsigned) time(NULL) - timestmp;
1689         continue;
1690       }
1691       dbg("Error in select wait again...\n");
1692       continue;
1693     }
1694     if (!retval) { // Timed out
1695       dbg("select wait Timed Out...\n");
1696       waited = 0;
1697       (addr_version == AF_INET6)? write_lease6file() : write_leasefile();
1698       if (get_interface(gconfig.interface, &gconfig.ifindex,
1699             (addr_version==AF_INET6)? (void*)gconfig.server_nip6 :
1700             (void*)&gconfig.server_nip, gconfig.server_mac)<0)
1701         perror_exit("Failed to get interface %s", gconfig.interface);
1702       if(addr_version != AF_INET6) {
1703         gconfig.server_nip = htonl(gconfig.server_nip);
1704       }
1705       continue;
1706     }
1707     if (FD_ISSET(sigfd.rd, &rfds)) { // Some Activity on RDFDs : is signal
1708       unsigned char sig;
1709       if (read(sigfd.rd, &sig, 1) != 1) {
1710         dbg("signal read failed.\n");
1711         continue;
1712       }
1713       switch (sig) {
1714         case SIGUSR1:
1715           infomsg(infomode, "Received SIGUSR1");
1716           (addr_version==AF_INET6)? write_lease6file() : write_leasefile();
1717           continue;
1718         case SIGTERM:
1719           infomsg(infomode, "received sigterm");
1720           (addr_version==AF_INET6)? write_lease6file() : write_leasefile();
1721           unlink(gconfig.pidfile);
1722           exit(0);
1723           break;
1724         default: break;
1725       }
1726     }
1727     if (FD_ISSET(gstate.listensock, &rfds)) { // Some Activity on RDFDs : is socket
1728       dbg("select listen sock read\n");
1729       if(addr_version==AF_INET6) {
1730         void *client_duid, *server_duid, *client_ia_na, *server_ia_na,
1731              *client_ia_pd;
1732         uint8_t client_lla[6] = {0,};
1733         uint16_t client_duid_len = 0, server_duid_len = 0, server_ia_na_len = 0,
1734                  client_ia_na_len = 0, client_ia_pd_len = 0;
1735 
1736         if(read_packet6() < 0) {
1737           open_listensock6();
1738           continue;
1739         }
1740         waited += time(NULL) - timestmp;
1741 
1742         memcpy(&gstate.rqcode, &gstate.rcvd.rcvd_pkt6.msgtype, sizeof(uint8_t));
1743         memcpy(&transactionid, &gstate.rcvd.rcvd_pkt6.transaction_id,
1744             sizeof(transactionid));
1745 
1746         if (!gstate.rqcode || gstate.rqcode < DHCP6SOLICIT ||
1747             gstate.rqcode > DHCP6RELAYREPLY) {
1748           dbg("no or bad message type option, ignoring packet.\n");
1749           continue;
1750         }
1751         if (!gstate.rcvd.rcvd_pkt6.transaction_id ||
1752             memcmp(gstate.rcvd.rcvd_pkt6.transaction_id, transactionid, 3)) {
1753           dbg("no or bad transaction id, ignoring packet.\n");
1754           continue;
1755         }
1756 
1757         waited += time(NULL) - timestmp;
1758         switch (gstate.rqcode) {
1759           case DHCP6SOLICIT:
1760             dbg("Message Type: DHCP6SOLICIT\n");
1761             optptr = prepare_send_pkt6(DHCP6ADVERTISE);
1762             optlen = 0;
1763 
1764             //TODO policy check
1765             //TODO Receive: ORO check (e.g. DNS)
1766 
1767             //Receive: Client Identifier (DUID)
1768             get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1769                 DHCP6_OPT_CLIENTID, &client_duid_len, &client_duid);
1770 
1771             //Receive: Identity Association for Non-temporary Address
1772             if(get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1773                   DHCP6_OPT_IA_NA, &client_ia_na_len, &client_ia_na)) {
1774               uint16_t ia_addr_len = sizeof(struct optval_ia_addr);
1775               void *ia_addr, *status_code;
1776               char *status_code_msg;
1777               uint16_t status_code_len = 0;
1778               server_ia_na_len = sizeof(struct optval_ia_na);
1779 
1780               //IA Address
1781               ia_addr = xzalloc(ia_addr_len);
1782               struct optval_ia_addr *ia_addr_p = (struct optval_ia_addr*)ia_addr;
1783               (*ia_addr_p).pref_lifetime = gconfig.pref_lifetime;
1784               (*ia_addr_p).valid_lifetime = gconfig.valid_lifetime;
1785               memcpy(&(*ia_addr_p).ipv6_addr,
1786                   getip6_from_pool(client_duid, client_duid_len,
1787                     DHCP6_OPT_IA_NA, (*(struct optval_ia_na*) client_ia_na).iaid,
1788                     &(*ia_addr_p).pref_lifetime), sizeof(uint32_t)*4);
1789               server_ia_na_len += (ia_addr_len+4);
1790 
1791               //Status Code
1792               if(memcmp((*ia_addr_p).ipv6_addr, (uint8_t[16]){0}, sizeof(uint32_t)*4)) {
1793                 status_code_msg = xstrdup("Assigned an address.");
1794                 status_code_len = strlen(status_code_msg)+1;
1795                 status_code = xzalloc(status_code_len);
1796                 struct optval_status_code *status_code_p =
1797                   (struct optval_status_code*)status_code;
1798                 (*status_code_p).status_code = htons(DHCP6_STATUS_SUCCESS);
1799                 memcpy((*status_code_p).status_msg, status_code_msg,
1800                     status_code_len);
1801                 server_ia_na_len += (status_code_len+4);
1802                 free(status_code_msg);
1803               } else {
1804                 status_code_msg = xstrdup("There's no available address.");
1805                 status_code_len = strlen(status_code_msg)+1;
1806                 status_code = xzalloc(status_code_len);
1807                 struct optval_status_code *status_code_p =
1808                   (struct optval_status_code*)status_code;
1809                 (*status_code_p).status_code = htons(DHCP6_STATUS_NOADDRSAVAIL);
1810                 memcpy((*status_code_p).status_msg, status_code_msg,
1811                     status_code_len);
1812                 server_ia_na_len += (status_code_len+4);
1813                 server_ia_na_len -= (ia_addr_len+4);
1814                 ia_addr_len = 0;
1815                 free(ia_addr);
1816                 free(status_code_msg);
1817                 //TODO send failed status code
1818                 break;
1819               }
1820 
1821               //combine options
1822               server_ia_na = xzalloc(server_ia_na_len);
1823               struct optval_ia_na *ia_na_p = (struct optval_ia_na*)server_ia_na;
1824               (*ia_na_p).iaid = (*(struct optval_ia_na*)client_ia_na).iaid;
1825               (*ia_na_p).t1 = gconfig.t1;
1826               (*ia_na_p).t2 = gconfig.t2;
1827 
1828               uint8_t* ia_na_optptr = (*ia_na_p).optval;
1829               if(ia_addr_len) {
1830                 set_optval6(ia_na_optptr, DHCP6_OPT_IA_ADDR, ia_addr, ia_addr_len);
1831                 ia_na_optptr += (ia_addr_len + 4);
1832                 free(ia_addr);
1833               }
1834               if(status_code_len) {
1835                 set_optval6(ia_na_optptr, DHCP6_OPT_STATUS_CODE, status_code,
1836                     status_code_len);
1837                 ia_na_optptr += (status_code_len);
1838                 free(status_code);
1839               }
1840 
1841               //Response: Identity Association for Non-temporary Address
1842               optptr = set_optval6(optptr, DHCP6_OPT_IA_NA, server_ia_na,
1843                   server_ia_na_len);
1844               optlen += (server_ia_na_len + 4);
1845               free(client_ia_na);free(server_ia_na);
1846             }
1847             //Receive: Identity Association for Prefix Delegation
1848             else if(get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1849                   DHCP6_OPT_IA_PD, &client_ia_pd_len, &client_ia_pd)) {
1850 
1851               //TODO
1852               //Response: Identity Association for Prefix Delegation
1853             }
1854 
1855             //DUID type: link-layer address plus time
1856             if(ntohs((*(struct optval_duid_llt*)client_duid).type) ==
1857                 DHCP6_DUID_LLT) {
1858               server_duid_len = 8+sizeof(gconfig.server_mac);
1859               server_duid = xzalloc(server_duid_len);
1860               struct optval_duid_llt *server_duid_p =
1861                 (struct optval_duid_llt*)server_duid;
1862               (*server_duid_p).type = htons(1);
1863               (*server_duid_p).hwtype = htons(1);
1864               (*server_duid_p).time = htonl((uint32_t)
1865                   (time(NULL) - 946684800) & 0xffffffff);
1866               memcpy((*server_duid_p).lladdr, gconfig.server_mac,
1867                   sizeof(gconfig.server_mac));
1868               memcpy(&client_lla, (*(struct optval_duid_llt*)client_duid).lladdr,
1869                   sizeof(client_lla));
1870 
1871               //Response: Server Identifier (DUID)
1872               optptr = set_optval6(optptr, DHCP6_OPT_SERVERID, server_duid,
1873                   server_duid_len);
1874               optlen += (server_duid_len + 4);
1875               //Response: Client Identifier
1876               optptr = set_optval6(optptr, DHCP6_OPT_CLIENTID, client_duid,
1877                   client_duid_len);
1878               optlen += (client_duid_len + 4);
1879               free(client_duid);free(server_duid);
1880             }
1881 
1882             send_packet6(0, client_lla, optlen);
1883             write_lease6file();
1884             break;
1885           case DHCP6REQUEST:
1886             dbg("Message Type: DHCP6REQUEST\n");
1887             optptr = prepare_send_pkt6(DHCP6REPLY);
1888             optlen = 0;
1889 
1890             //Receive: Client Identifier (DUID)
1891             get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1892                 DHCP6_OPT_CLIENTID, &client_duid_len, &client_duid);
1893             optptr = set_optval6(optptr, DHCP6_OPT_CLIENTID, client_duid,
1894                 client_duid_len);
1895             optlen += (client_duid_len + 4);
1896             memcpy(client_lla, (*(struct optval_duid_llt*)client_duid).lladdr,
1897                 sizeof(client_lla));
1898 
1899             //Receive: Identity Association for Non-temporary Address
1900             if(get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1901                   DHCP6_OPT_IA_NA, &client_ia_na_len, &client_ia_na)) {
1902               uint16_t ia_addr_len = 0, status_code_len = 0;
1903               void *ia_addr, *status_code;
1904               uint16_t server_ia_na_len = sizeof(struct optval_ia_na);
1905               char *status_code_msg;
1906 
1907               //Check IA Address
1908               get_optval6((uint8_t*)(*(struct optval_ia_na*)client_ia_na).optval,
1909                   DHCP6_OPT_IA_ADDR, &ia_addr_len, &ia_addr);
1910               struct optval_ia_addr *ia_addr_p = (struct optval_ia_addr*)ia_addr;
1911               if(verifyip6_in_lease((*ia_addr_p).ipv6_addr, client_duid,
1912                     DHCP6_OPT_IA_NA, (*(struct optval_ia_na*)client_ia_na).iaid)
1913                   == -1) {
1914                 server_ia_na_len += (ia_addr_len + 4);
1915                 //Add Status Code
1916                 status_code_msg = xstrdup("Assigned an address.");
1917                 status_code_len = strlen(status_code_msg) + 1;
1918                 status_code = xzalloc(status_code_len);
1919                 struct optval_status_code *status_code_p =
1920                   (struct optval_status_code*)status_code;
1921                 (*status_code_p).status_code = htons(DHCP6_STATUS_SUCCESS);
1922                 memcpy((*status_code_p).status_msg, status_code_msg,
1923                     status_code_len);
1924                 server_ia_na_len += (status_code_len+4);
1925               } else {
1926                 //TODO send failed status code
1927                 break;
1928               }
1929 
1930               //combine options
1931               server_ia_na = xzalloc(server_ia_na_len);
1932               struct optval_ia_na *ia_na_p = (struct optval_ia_na*)server_ia_na;
1933               (*ia_na_p).iaid = (*(struct optval_ia_na*)client_ia_na).iaid;
1934               (*ia_na_p).t1 = gconfig.t1;
1935               (*ia_na_p).t2 = gconfig.t2;
1936 
1937               uint8_t* ia_na_optptr = (*ia_na_p).optval;
1938               ia_na_optptr = set_optval6(ia_na_optptr, DHCP6_OPT_IA_ADDR,
1939                   ia_addr, ia_addr_len);
1940               free(ia_addr);
1941 
1942               if(status_code_len) {
1943                 ia_na_optptr = set_optval6(ia_na_optptr, DHCP6_OPT_STATUS_CODE,
1944                     status_code, status_code_len);
1945                 free(status_code);
1946               }
1947 
1948               //Response: Identity Association for Non-temporary Address
1949               //(Status Code added)
1950               optptr = set_optval6(optptr, DHCP6_OPT_IA_NA,
1951                   server_ia_na, server_ia_na_len);
1952               optlen += (server_ia_na_len + 4);
1953               free(client_ia_na);free(server_ia_na);
1954             }
1955 
1956             //Receive: Server Identifier (DUID)
1957             get_optval6((uint8_t*)&gstate.rcvd.rcvd_pkt6.options,
1958                 DHCP6_OPT_SERVERID, &server_duid_len, &server_duid);
1959             optptr = set_optval6(optptr, DHCP6_OPT_SERVERID,
1960                 server_duid, server_duid_len);
1961             optlen += (server_duid_len + 4);
1962 
1963             free(client_duid); free(server_duid);
1964 
1965             send_packet6(0, client_lla, optlen);
1966             write_lease6file();
1967             break;
1968           case DHCP6DECLINE:  //TODO
1969           case DHCP6RENEW:    //TODO
1970           case DHCP6REBIND:   //TODO
1971           case DHCP6RELEASE:
1972             dbg("Message Type: DHCP6RELEASE\n");
1973             optptr = prepare_send_pkt6(DHCP6REPLY);
1974             break;
1975           default:
1976             dbg("Message Type : %u\n", gstate.rqcode);
1977             break;
1978         }
1979 
1980       } else {
1981         if(read_packet() < 0) {
1982           open_listensock();
1983           continue;
1984         }
1985         waited += time(NULL) - timestmp;
1986 
1987         get_optval((uint8_t*)&gstate.rcvd.rcvd_pkt.options,
1988             DHCP_OPT_MESSAGE_TYPE, &gstate.rqcode);
1989         if (gstate.rqcode == 0 || gstate.rqcode < DHCPDISCOVER
1990             || gstate.rqcode > DHCPINFORM) {
1991           dbg("no or bad message type option, ignoring packet.\n");
1992           continue;
1993         }
1994         get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
1995             DHCP_OPT_SERVER_ID, &serverid);
1996         if (serverid && (serverid != gconfig.server_nip)) {
1997           dbg("server ID doesn't match, ignoring packet.\n");
1998           continue;
1999         }
2000 
2001         waited += time(NULL) - timestmp;
2002         switch (gstate.rqcode) {
2003           case DHCPDISCOVER:
2004             msgtype = DHCPOFFER;
2005             dbg("Message Type : DHCPDISCOVER\n");
2006             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2007                 DHCP_OPT_REQUESTED_IP, &requested_nip);
2008             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2009                 DHCP_OPT_HOST_NAME, &hstname);
2010             reqested_lease = gconfig.offer_time;
2011             get_reqparam(&gstate.rqopt);
2012             optptr = prepare_send_pkt();
2013             gstate.send.send_pkt.yiaddr = getip_from_pool(requested_nip,
2014                 gstate.rcvd.rcvd_pkt.chaddr, &reqested_lease, hstname);
2015             if(!gstate.send.send_pkt.yiaddr){
2016               msgtype = DHCPNAK;
2017               optptr = set_optval(optptr, DHCP_OPT_MESSAGE_TYPE, &msgtype, 1);
2018               send_packet(1);
2019               break;
2020             }
2021             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2022                 DHCP_OPT_LEASE_TIME, &reqested_lease);
2023             reqested_lease = htonl(get_lease(reqested_lease + time(NULL)));
2024             optptr = set_optval(optptr, DHCP_OPT_MESSAGE_TYPE, &msgtype, 1);
2025             optptr = set_optval(optptr, DHCP_OPT_SERVER_ID, &gconfig.server_nip, 4);
2026             optptr = set_optval(optptr, DHCP_OPT_LEASE_TIME, &reqested_lease, 4);
2027             optptr = set_reqparam(optptr, gstate.rqopt);
2028             send_packet(1);
2029             break;
2030           case DHCPREQUEST:
2031             msgtype = DHCPACK;
2032             dbg("Message Type : DHCPREQUEST\n");
2033             optptr = prepare_send_pkt();
2034             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2035                 DHCP_OPT_REQUESTED_IP, &requested_nip);
2036             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2037                 DHCP_OPT_LEASE_TIME, &reqested_lease);
2038             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2039                 DHCP_OPT_HOST_NAME, &hstname);
2040             gstate.send.send_pkt.yiaddr = getip_from_pool(requested_nip,
2041                 gstate.rcvd.rcvd_pkt.chaddr, &reqested_lease, hstname);
2042             if (!serverid) reqested_lease = gconfig.max_lease_sec;
2043             if (!gstate.send.send_pkt.yiaddr) {
2044               msgtype = DHCPNAK;
2045               optptr = set_optval(optptr, DHCP_OPT_MESSAGE_TYPE, &msgtype, 1);
2046               send_packet(1);
2047               break;
2048             }
2049             optptr = set_optval(optptr, DHCP_OPT_MESSAGE_TYPE, &msgtype, 1);
2050             optptr = set_optval(optptr, DHCP_OPT_SERVER_ID, &gconfig.server_nip, 4);
2051             reqested_lease = htonl(reqested_lease);
2052             optptr = set_optval(optptr, DHCP_OPT_LEASE_TIME, &reqested_lease, 4);
2053             send_packet(1);
2054             write_leasefile();
2055             break;
2056           case DHCPDECLINE:// FALL THROUGH
2057           case DHCPRELEASE:
2058             dbg("Message Type : DHCPDECLINE or DHCPRELEASE \n");
2059             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2060                 DHCP_OPT_SERVER_ID, &serverid);
2061             if (serverid != gconfig.server_nip) break;
2062             get_optval((uint8_t*) &gstate.rcvd.rcvd_pkt.options,
2063                 DHCP_OPT_REQUESTED_IP, &requested_nip);
2064             delip_from_lease(requested_nip, gstate.rcvd.rcvd_pkt.chaddr,
2065                 (gstate.rqcode==DHCPRELEASE)?0:gconfig.decline_time);
2066             break;
2067           default:
2068             dbg("Message Type : %u\n", gstate.rqcode);
2069             break;
2070         }
2071       }
2072     }
2073   }
2074 }
2075