pico_dhcp_server.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 
00006 Authors: Frederik Van Slycken, Kristof Roelants
00007 *********************************************************************/
00008 
00009 #ifdef PICO_SUPPORT_DHCPD
00010 
00011 #include "pico_dhcp_server.h"
00012 #include "pico_stack.h"
00013 #include "pico_config.h"
00014 #include "pico_addressing.h"
00015 #include "pico_socket.h"
00016 #include "pico_arp.h"
00017 #include <stdlib.h>
00018 
00019 # define dhcpd_dbg(...) do{}while(0)
00020 //# define dhcpd_dbg dbg
00021 
00022 #define dhcpd_make_offer(x) dhcpd_make_reply(x, PICO_DHCP_MSG_OFFER)
00023 #define dhcpd_make_ack(x) dhcpd_make_reply(x, PICO_DHCP_MSG_ACK)
00024 #define ip_inrange(x) ((long_be(x) >= long_be(dn->settings->pool_start)) && (long_be(x) <= long_be(dn->settings->pool_end)))
00025 
00026 static int dhcp_settings_cmp(void *ka, void *kb)
00027 {
00028   struct pico_dhcpd_settings *a = ka, *b = kb;
00029   if (a->dev < b->dev)
00030     return -1; 
00031   else if (a->dev > b->dev)
00032     return 1;
00033   else
00034     return 0;
00035 } 
00036 PICO_TREE_DECLARE(DHCPSettings, dhcp_settings_cmp);
00037 
00038 static int dhcp_negotiations_cmp(void *ka, void *kb)
00039 {
00040   struct pico_dhcp_negotiation *a = ka, *b = kb;
00041   if (a->xid < b->xid)
00042     return -1; 
00043   else if (a->xid > b->xid)
00044     return 1;
00045   else
00046     return 0;
00047 } 
00048 PICO_TREE_DECLARE(DHCPNegotiations, dhcp_negotiations_cmp);
00049 
00050 static struct pico_dhcp_negotiation *get_negotiation_by_xid(uint32_t xid)
00051 {
00052   struct pico_dhcp_negotiation test = { }, *neg = NULL;
00053 
00054   test.xid = xid;
00055   neg = pico_tree_findKey(&DHCPNegotiations, &test);
00056   if (!neg)
00057     return NULL;
00058   else
00059     return neg;
00060 }
00061 
00062 static void dhcpd_make_reply(struct pico_dhcp_negotiation *dn, uint8_t reply_type)
00063 {
00064   uint8_t buf_out[DHCPD_DATAGRAM_SIZE] = {0};
00065   struct pico_dhcphdr *dh_out = (struct pico_dhcphdr *) buf_out;
00066   struct pico_ip4 destination = { };
00067   uint32_t bcast = dn->settings->my_ip.addr | ~(dn->settings->netmask.addr);
00068   uint32_t dns_server = OPENDNS;
00069   uint16_t port = PICO_DHCP_CLIENT_PORT;
00070   int sent = 0;
00071 
00072   memcpy(dh_out->hwaddr, dn->eth.addr, PICO_HLEN_ETHER);
00073   dh_out->op = PICO_DHCP_OP_REPLY;
00074   dh_out->htype = PICO_HTYPE_ETHER;
00075   dh_out->hlen = PICO_HLEN_ETHER;
00076   dh_out->xid = dn->xid;
00077   dh_out->yiaddr = dn->ipv4.addr;
00078   dh_out->siaddr = dn->settings->my_ip.addr;
00079   dh_out->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
00080 
00081   /* Option: msg type, len 1 */
00082   dh_out->options[0] = PICO_DHCPOPT_MSGTYPE;
00083   dh_out->options[1] = 1;
00084   dh_out->options[2] = reply_type;
00085 
00086   /* Option: server id, len 4 */
00087   dh_out->options[3] = PICO_DHCPOPT_SERVERID;
00088   dh_out->options[4] = 4;
00089   memcpy(dh_out->options + 5, &dn->settings->my_ip.addr, 4);
00090 
00091   /* Option: Lease time, len 4 */
00092   dh_out->options[9] = PICO_DHCPOPT_LEASETIME;
00093   dh_out->options[10] = 4;
00094   memcpy(dh_out->options + 11, &dn->settings->lease_time, 4);
00095 
00096   /* Option: Netmask, len 4 */
00097   dh_out->options[15] = PICO_DHCPOPT_NETMASK;
00098   dh_out->options[16] = 4;
00099   memcpy(dh_out->options + 17, &dn->settings->netmask.addr, 4);
00100 
00101   /* Option: Router, len 4 */
00102   dh_out->options[21] = PICO_DHCPOPT_ROUTER;
00103   dh_out->options[22] = 4;
00104   memcpy(dh_out->options + 23, &dn->settings->my_ip.addr, 4);
00105 
00106   /* Option: Broadcast, len 4 */
00107   dh_out->options[27] = PICO_DHCPOPT_BCAST;
00108   dh_out->options[28] = 4;
00109   memcpy(dh_out->options + 29, &bcast, 4);
00110 
00111   /* Option: DNS, len 4 */
00112   dh_out->options[33] = PICO_DHCPOPT_DNS;
00113   dh_out->options[34] = 4;
00114   memcpy(dh_out->options + 35, &dns_server, 4);
00115 
00116   dh_out->options[40] = PICO_DHCPOPT_END;
00117 
00118   destination.addr = dh_out->yiaddr;
00119 
00120   sent = pico_socket_sendto(dn->settings->s, buf_out, DHCPD_DATAGRAM_SIZE, &destination, port);
00121   if (sent < 0) {
00122     dhcpd_dbg("DHCPD: sendto failed with code %d!\n", pico_err);
00123   }
00124 }
00125 
00126 static void dhcp_recv(struct pico_socket *s, uint8_t *buffer, int len)
00127 {
00128   struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) buffer;
00129   struct pico_dhcp_negotiation *dn = get_negotiation_by_xid(dhdr->xid);
00130   struct pico_ip4* ipv4 = NULL;
00131   struct pico_dhcpd_settings test, *settings = NULL;
00132   uint8_t *nextopt, opt_data[20], opt_type;
00133   int opt_len = 20;
00134   uint8_t msg_type;
00135   uint32_t msg_reqIP = 0;
00136   uint32_t msg_servID = 0;
00137 
00138   if (!is_options_valid(dhdr->options, len - sizeof(struct pico_dhcphdr))) {
00139     dhcpd_dbg("DHCPD WARNING: invalid options in dhcp message\n");
00140     return;
00141   }
00142 
00143   if (!dn) {
00144     dn = pico_zalloc(sizeof(struct pico_dhcp_negotiation));
00145     if (!dn) {
00146       pico_err = PICO_ERR_ENOMEM;
00147       return;
00148     }
00149     dn->xid = dhdr->xid;
00150     dn->state = DHCPSTATE_DISCOVER;
00151     memcpy(dn->eth.addr, dhdr->hwaddr, PICO_HLEN_ETHER);
00152 
00153     test.dev = pico_ipv4_link_find(&s->local_addr.ip4);
00154     settings = pico_tree_findKey(&DHCPSettings, &test);
00155     if (settings) {
00156       dn->settings = settings;
00157     } else {
00158       dhcpd_dbg("DHCPD WARNING: received DHCP message on unconfigured link %s\n", test.dev->name);
00159       pico_free(dn);
00160       return;
00161     }
00162 
00163     ipv4 = pico_arp_reverse_lookup(&dn->eth);
00164     if (!ipv4) {
00165       dn->ipv4.addr = settings->pool_next;
00166       pico_arp_create_entry(dn->eth.addr, dn->ipv4, settings->dev);
00167       settings->pool_next = long_be(long_be(settings->pool_next) + 1);
00168     } else {
00169       dn->ipv4.addr = ipv4->addr;
00170     }
00171 
00172     if (pico_tree_insert(&DHCPNegotiations, dn)) {
00173       dhcpd_dbg("DHCPD WARNING: tried creating new negotation for existing xid %u\n", dn->xid);
00174       pico_free(dn);
00175       return; /* Element key already exists */
00176     }
00177   }
00178  
00179   if (!ip_inrange(dn->ipv4.addr))
00180     return;
00181 
00182   opt_type = dhcp_get_next_option(dhdr->options, opt_data, &opt_len, &nextopt);
00183   while (opt_type != PICO_DHCPOPT_END) {
00184     /* parse interesting options here */
00185       //dhcpd_dbg("DHCPD sever: opt_type %x,  opt_data[0]%d\n", opt_type, opt_data[0]);
00186     switch(opt_type){
00187       case PICO_DHCPOPT_MSGTYPE:
00188         msg_type = opt_data[0];
00189         break;
00190       case PICO_DHCPOPT_REQIP:
00191         //dhcpd_dbg("DHCPD sever: opt_type %x,  opt_len%d\n", opt_type, opt_len);
00192         if( opt_len == 4)
00193         {
00194           msg_reqIP =  ( opt_data[0] << 24 );
00195           msg_reqIP |= ( opt_data[1] << 16 );
00196           msg_reqIP |= ( opt_data[2] << 8  );
00197           msg_reqIP |= ( opt_data[3]       );
00198          //dhcpd_dbg("DHCPD sever: msg_reqIP %x, opt_data[0] %x,[1] %x,[2] %x,[3] %x\n", msg_reqIP, opt_data[0],opt_data[1],opt_data[2],opt_data[3]);
00199         };
00200         break;
00201       case PICO_DHCPOPT_SERVERID:
00202         //dhcpd_dbg("DHCPD sever: opt_type %x,  opt_len%d\n", opt_type, opt_len);
00203         if( opt_len == 4)
00204         {
00205           msg_servID =  ( opt_data[0] << 24 );
00206           msg_servID |= ( opt_data[1] << 16 );
00207           msg_servID |= ( opt_data[2] << 8  );
00208           msg_servID |= ( opt_data[3]       );
00209           //dhcpd_dbg("DHCPD sever: msg_servID %x, opt_data[0] %x,[1] %x,[2] %x,[3] %x\n", msg_servID, opt_data[0],opt_data[1],opt_data[2],opt_data[3]);
00210         };
00211         break;        
00212       default:
00213         break;
00214     }
00215         
00216     opt_len = 20;
00217     opt_type = dhcp_get_next_option(NULL, opt_data, &opt_len, &nextopt);
00218   }
00219     
00220   //dhcpd_dbg("DHCPD sever: msg_type %d, dn->state %d\n", msg_type, dn->state);
00221   //dhcpd_dbg("DHCPD sever: msg_reqIP %x, dn->msg_servID %x\n", msg_reqIP, msg_servID);
00222   //dhcpd_dbg("DHCPD sever: dhdr->ciaddr %x, dhdr->yiaddr %x, dn->ipv4.addr %x\n", dhdr->ciaddr,dhdr->yiaddr,dn->ipv4.addr);
00223 
00224   if (msg_type == PICO_DHCP_MSG_DISCOVER)
00225   {
00226     dhcpd_make_offer(dn);
00227     dn->state = DHCPSTATE_OFFER;
00228     return;
00229   }
00230   else if ((msg_type == PICO_DHCP_MSG_REQUEST)&&( dn->state == DHCPSTATE_OFFER))
00231   {
00232     dhcpd_make_ack(dn);
00233     dn->state = DHCPSTATE_BOUND;
00234     return;
00235   }
00236   else if ((msg_type == PICO_DHCP_MSG_REQUEST)&&( dn->state == DHCPSTATE_BOUND))
00237   {
00238     if( ( msg_servID == 0 )
00239       &&( msg_reqIP == 0 )
00240       &&( dhdr->ciaddr == dn->ipv4.addr)
00241       )
00242     { 
00243       dhcpd_make_ack(dn);
00244       return;
00245     }
00246   }  
00247 }
00248 
00249 static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s)
00250 {
00251   uint8_t buf[DHCPD_DATAGRAM_SIZE] = { };
00252   int r = 0;
00253   uint32_t peer = 0;
00254   uint16_t port = 0;
00255 
00256   dhcpd_dbg("DHCPD: called dhcpd_wakeup\n");
00257   if (ev == PICO_SOCK_EV_RD) {
00258     do {
00259       r = pico_socket_recvfrom(s, buf, DHCPD_DATAGRAM_SIZE, &peer, &port);
00260       if (r > 0 && port == PICO_DHCP_CLIENT_PORT) {
00261         dhcp_recv(s, buf, r);
00262       }
00263     } while(r>0);
00264   }
00265 }
00266 
00267 int pico_dhcp_server_initiate(struct pico_dhcpd_settings *setting)
00268 {
00269   struct pico_dhcpd_settings *settings = NULL;
00270   struct pico_ipv4_link *link = NULL;
00271   uint16_t port = PICO_DHCPD_PORT;
00272 
00273   if (!setting) {
00274     pico_err = PICO_ERR_EINVAL;
00275     return -1;
00276   }
00277 
00278   if (!setting->my_ip.addr) {
00279     pico_err = PICO_ERR_EINVAL;
00280     dhcpd_dbg("DHCPD: IP address of interface was not supplied\n");
00281     return -1;
00282   }
00283 
00284   link = pico_ipv4_link_get(&setting->my_ip);
00285   if (!link) {
00286     pico_err = PICO_ERR_EINVAL;
00287     dhcpd_dbg("DHCPD: no link with IP %X found\n", setting->my_ip.addr);
00288     return -1;
00289   }
00290 
00291   settings = pico_zalloc(sizeof(struct pico_dhcpd_settings));
00292   if (!settings) {
00293     pico_err = PICO_ERR_ENOMEM;
00294     return -1;
00295   }
00296   memcpy(settings, setting, sizeof(struct pico_dhcpd_settings));
00297 
00298   settings->dev = link->dev;
00299   dhcpd_dbg("DHCPD: configuring DHCP server for link %s\n", link->dev->name);
00300   settings->my_ip.addr = link->address.addr;
00301   dhcpd_dbg("DHCPD: using server addr %X\n", long_be(settings->my_ip.addr));
00302   settings->netmask.addr = link->netmask.addr;
00303   dhcpd_dbg("DHCPD: using netmask %X\n", long_be(settings->netmask.addr));
00304 
00305   /* default values if not provided */
00306   if (settings->pool_start == 0)
00307     settings->pool_start = (settings->my_ip.addr & settings->netmask.addr) | POOL_START;
00308   dhcpd_dbg("DHCPD: using pool_start %X\n", long_be(settings->pool_start));
00309   if (settings->pool_end == 0)
00310     settings->pool_end = (settings->my_ip.addr & settings->netmask.addr) | POOL_END;
00311   dhcpd_dbg("DHCPD: using pool_end %x\n", long_be(settings->pool_end));
00312   if (settings->lease_time == 0)
00313     settings->lease_time = LEASE_TIME;
00314   dhcpd_dbg("DHCPD: using lease time %x\n", long_be(settings->lease_time));
00315   settings->pool_next = settings->pool_start;
00316 
00317   settings->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup);
00318   if (!settings->s) {
00319     dhcpd_dbg("DHCP: could not open client socket\n");
00320     pico_free(settings);
00321     return -1;
00322   }
00323   if (pico_socket_bind(settings->s, &settings->my_ip, &port) != 0) {
00324     dhcpd_dbg("DHCP: could not bind server socket (%s)\n", strerror(pico_err));
00325     pico_free(settings);
00326     return -1;
00327   }
00328   
00329   if (pico_tree_insert(&DHCPSettings, settings)) {
00330     dhcpd_dbg("DHCPD ERROR: link %s already configured\n", link->dev->name);
00331     pico_err = PICO_ERR_EINVAL;
00332     pico_free(settings);
00333     return -1; /* Element key already exists */
00334   }
00335   dhcpd_dbg("DHCPD: configured DHCP server for link %s\n", link->dev->name);
00336 
00337   return 0;
00338 }
00339 #endif /* PICO_SUPPORT_DHCP */