Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

Fork of PicoTCP by Daniele Lacamera

Committer:
tass
Date:
Fri May 17 12:09:59 2013 +0000
Revision:
1:cfe8984a32b4
Parent:
libraries/picotcp/modules/pico_dhcp_server.c@0:d7f2341ab245
Update for smaller SOCKETQ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniele 0:d7f2341ab245 1 /*********************************************************************
daniele 0:d7f2341ab245 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
daniele 0:d7f2341ab245 3 See LICENSE and COPYING for usage.
daniele 0:d7f2341ab245 4
daniele 0:d7f2341ab245 5
daniele 0:d7f2341ab245 6 Authors: Frederik Van Slycken, Kristof Roelants
daniele 0:d7f2341ab245 7 *********************************************************************/
daniele 0:d7f2341ab245 8
daniele 0:d7f2341ab245 9 #ifdef PICO_SUPPORT_DHCPD
daniele 0:d7f2341ab245 10
daniele 0:d7f2341ab245 11 #include "pico_dhcp_server.h"
daniele 0:d7f2341ab245 12 #include "pico_stack.h"
daniele 0:d7f2341ab245 13 #include "pico_config.h"
daniele 0:d7f2341ab245 14 #include "pico_addressing.h"
daniele 0:d7f2341ab245 15 #include "pico_socket.h"
daniele 0:d7f2341ab245 16 #include "pico_arp.h"
daniele 0:d7f2341ab245 17 #include <stdlib.h>
daniele 0:d7f2341ab245 18
daniele 0:d7f2341ab245 19 # define dhcpd_dbg(...) do{}while(0)
daniele 0:d7f2341ab245 20 //# define dhcpd_dbg dbg
daniele 0:d7f2341ab245 21
daniele 0:d7f2341ab245 22 #define dhcpd_make_offer(x) dhcpd_make_reply(x, PICO_DHCP_MSG_OFFER)
daniele 0:d7f2341ab245 23 #define dhcpd_make_ack(x) dhcpd_make_reply(x, PICO_DHCP_MSG_ACK)
daniele 0:d7f2341ab245 24 #define ip_inrange(x) ((long_be(x) >= long_be(dn->settings->pool_start)) && (long_be(x) <= long_be(dn->settings->pool_end)))
daniele 0:d7f2341ab245 25
daniele 0:d7f2341ab245 26 static int dhcp_settings_cmp(void *ka, void *kb)
daniele 0:d7f2341ab245 27 {
daniele 0:d7f2341ab245 28 struct pico_dhcpd_settings *a = ka, *b = kb;
daniele 0:d7f2341ab245 29 if (a->dev < b->dev)
daniele 0:d7f2341ab245 30 return -1;
daniele 0:d7f2341ab245 31 else if (a->dev > b->dev)
daniele 0:d7f2341ab245 32 return 1;
daniele 0:d7f2341ab245 33 else
daniele 0:d7f2341ab245 34 return 0;
daniele 0:d7f2341ab245 35 }
daniele 0:d7f2341ab245 36 PICO_TREE_DECLARE(DHCPSettings, dhcp_settings_cmp);
daniele 0:d7f2341ab245 37
daniele 0:d7f2341ab245 38 static int dhcp_negotiations_cmp(void *ka, void *kb)
daniele 0:d7f2341ab245 39 {
daniele 0:d7f2341ab245 40 struct pico_dhcp_negotiation *a = ka, *b = kb;
daniele 0:d7f2341ab245 41 if (a->xid < b->xid)
daniele 0:d7f2341ab245 42 return -1;
daniele 0:d7f2341ab245 43 else if (a->xid > b->xid)
daniele 0:d7f2341ab245 44 return 1;
daniele 0:d7f2341ab245 45 else
daniele 0:d7f2341ab245 46 return 0;
daniele 0:d7f2341ab245 47 }
daniele 0:d7f2341ab245 48 PICO_TREE_DECLARE(DHCPNegotiations, dhcp_negotiations_cmp);
daniele 0:d7f2341ab245 49
daniele 0:d7f2341ab245 50 static struct pico_dhcp_negotiation *get_negotiation_by_xid(uint32_t xid)
daniele 0:d7f2341ab245 51 {
daniele 0:d7f2341ab245 52 struct pico_dhcp_negotiation test = { }, *neg = NULL;
daniele 0:d7f2341ab245 53
daniele 0:d7f2341ab245 54 test.xid = xid;
daniele 0:d7f2341ab245 55 neg = pico_tree_findKey(&DHCPNegotiations, &test);
daniele 0:d7f2341ab245 56 if (!neg)
daniele 0:d7f2341ab245 57 return NULL;
daniele 0:d7f2341ab245 58 else
daniele 0:d7f2341ab245 59 return neg;
daniele 0:d7f2341ab245 60 }
daniele 0:d7f2341ab245 61
daniele 0:d7f2341ab245 62 static void dhcpd_make_reply(struct pico_dhcp_negotiation *dn, uint8_t reply_type)
daniele 0:d7f2341ab245 63 {
daniele 0:d7f2341ab245 64 uint8_t buf_out[DHCPD_DATAGRAM_SIZE] = {0};
daniele 0:d7f2341ab245 65 struct pico_dhcphdr *dh_out = (struct pico_dhcphdr *) buf_out;
daniele 0:d7f2341ab245 66 struct pico_ip4 destination = { };
daniele 0:d7f2341ab245 67 uint32_t bcast = dn->settings->my_ip.addr | ~(dn->settings->netmask.addr);
daniele 0:d7f2341ab245 68 uint32_t dns_server = OPENDNS;
daniele 0:d7f2341ab245 69 uint16_t port = PICO_DHCP_CLIENT_PORT;
daniele 0:d7f2341ab245 70 int sent = 0;
daniele 0:d7f2341ab245 71
daniele 0:d7f2341ab245 72 memcpy(dh_out->hwaddr, dn->eth.addr, PICO_HLEN_ETHER);
daniele 0:d7f2341ab245 73 dh_out->op = PICO_DHCP_OP_REPLY;
daniele 0:d7f2341ab245 74 dh_out->htype = PICO_HTYPE_ETHER;
daniele 0:d7f2341ab245 75 dh_out->hlen = PICO_HLEN_ETHER;
daniele 0:d7f2341ab245 76 dh_out->xid = dn->xid;
daniele 0:d7f2341ab245 77 dh_out->yiaddr = dn->ipv4.addr;
daniele 0:d7f2341ab245 78 dh_out->siaddr = dn->settings->my_ip.addr;
daniele 0:d7f2341ab245 79 dh_out->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
daniele 0:d7f2341ab245 80
daniele 0:d7f2341ab245 81 /* Option: msg type, len 1 */
daniele 0:d7f2341ab245 82 dh_out->options[0] = PICO_DHCPOPT_MSGTYPE;
daniele 0:d7f2341ab245 83 dh_out->options[1] = 1;
daniele 0:d7f2341ab245 84 dh_out->options[2] = reply_type;
daniele 0:d7f2341ab245 85
daniele 0:d7f2341ab245 86 /* Option: server id, len 4 */
daniele 0:d7f2341ab245 87 dh_out->options[3] = PICO_DHCPOPT_SERVERID;
daniele 0:d7f2341ab245 88 dh_out->options[4] = 4;
daniele 0:d7f2341ab245 89 memcpy(dh_out->options + 5, &dn->settings->my_ip.addr, 4);
daniele 0:d7f2341ab245 90
daniele 0:d7f2341ab245 91 /* Option: Lease time, len 4 */
daniele 0:d7f2341ab245 92 dh_out->options[9] = PICO_DHCPOPT_LEASETIME;
daniele 0:d7f2341ab245 93 dh_out->options[10] = 4;
daniele 0:d7f2341ab245 94 memcpy(dh_out->options + 11, &dn->settings->lease_time, 4);
daniele 0:d7f2341ab245 95
daniele 0:d7f2341ab245 96 /* Option: Netmask, len 4 */
daniele 0:d7f2341ab245 97 dh_out->options[15] = PICO_DHCPOPT_NETMASK;
daniele 0:d7f2341ab245 98 dh_out->options[16] = 4;
daniele 0:d7f2341ab245 99 memcpy(dh_out->options + 17, &dn->settings->netmask.addr, 4);
daniele 0:d7f2341ab245 100
daniele 0:d7f2341ab245 101 /* Option: Router, len 4 */
daniele 0:d7f2341ab245 102 dh_out->options[21] = PICO_DHCPOPT_ROUTER;
daniele 0:d7f2341ab245 103 dh_out->options[22] = 4;
daniele 0:d7f2341ab245 104 memcpy(dh_out->options + 23, &dn->settings->my_ip.addr, 4);
daniele 0:d7f2341ab245 105
daniele 0:d7f2341ab245 106 /* Option: Broadcast, len 4 */
daniele 0:d7f2341ab245 107 dh_out->options[27] = PICO_DHCPOPT_BCAST;
daniele 0:d7f2341ab245 108 dh_out->options[28] = 4;
daniele 0:d7f2341ab245 109 memcpy(dh_out->options + 29, &bcast, 4);
daniele 0:d7f2341ab245 110
daniele 0:d7f2341ab245 111 /* Option: DNS, len 4 */
daniele 0:d7f2341ab245 112 dh_out->options[33] = PICO_DHCPOPT_DNS;
daniele 0:d7f2341ab245 113 dh_out->options[34] = 4;
daniele 0:d7f2341ab245 114 memcpy(dh_out->options + 35, &dns_server, 4);
daniele 0:d7f2341ab245 115
daniele 0:d7f2341ab245 116 dh_out->options[40] = PICO_DHCPOPT_END;
daniele 0:d7f2341ab245 117
daniele 0:d7f2341ab245 118 destination.addr = dh_out->yiaddr;
daniele 0:d7f2341ab245 119
daniele 0:d7f2341ab245 120 sent = pico_socket_sendto(dn->settings->s, buf_out, DHCPD_DATAGRAM_SIZE, &destination, port);
daniele 0:d7f2341ab245 121 if (sent < 0) {
daniele 0:d7f2341ab245 122 dhcpd_dbg("DHCPD: sendto failed with code %d!\n", pico_err);
daniele 0:d7f2341ab245 123 }
daniele 0:d7f2341ab245 124 }
daniele 0:d7f2341ab245 125
daniele 0:d7f2341ab245 126 static void dhcp_recv(struct pico_socket *s, uint8_t *buffer, int len)
daniele 0:d7f2341ab245 127 {
daniele 0:d7f2341ab245 128 struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) buffer;
daniele 0:d7f2341ab245 129 struct pico_dhcp_negotiation *dn = get_negotiation_by_xid(dhdr->xid);
daniele 0:d7f2341ab245 130 struct pico_ip4* ipv4 = NULL;
daniele 0:d7f2341ab245 131 struct pico_dhcpd_settings test, *settings = NULL;
daniele 0:d7f2341ab245 132 uint8_t *nextopt, opt_data[20], opt_type;
daniele 0:d7f2341ab245 133 int opt_len = 20;
daniele 0:d7f2341ab245 134
daniele 0:d7f2341ab245 135 if (!is_options_valid(dhdr->options, len - sizeof(struct pico_dhcphdr))) {
daniele 0:d7f2341ab245 136 dhcpd_dbg("DHCPD WARNING: invalid options in dhcp message\n");
daniele 0:d7f2341ab245 137 return;
daniele 0:d7f2341ab245 138 }
daniele 0:d7f2341ab245 139
daniele 0:d7f2341ab245 140 if (!dn) {
daniele 0:d7f2341ab245 141 dn = pico_zalloc(sizeof(struct pico_dhcp_negotiation));
daniele 0:d7f2341ab245 142 if (!dn) {
daniele 0:d7f2341ab245 143 pico_err = PICO_ERR_ENOMEM;
daniele 0:d7f2341ab245 144 return;
daniele 0:d7f2341ab245 145 }
daniele 0:d7f2341ab245 146 dn->xid = dhdr->xid;
daniele 0:d7f2341ab245 147 dn->state = DHCPSTATE_DISCOVER;
daniele 0:d7f2341ab245 148 memcpy(dn->eth.addr, dhdr->hwaddr, PICO_HLEN_ETHER);
daniele 0:d7f2341ab245 149
daniele 0:d7f2341ab245 150 test.dev = pico_ipv4_link_find(&s->local_addr.ip4);
daniele 0:d7f2341ab245 151 settings = pico_tree_findKey(&DHCPSettings, &test);
daniele 0:d7f2341ab245 152 if (settings) {
daniele 0:d7f2341ab245 153 dn->settings = settings;
daniele 0:d7f2341ab245 154 } else {
daniele 0:d7f2341ab245 155 dhcpd_dbg("DHCPD WARNING: received DHCP message on unconfigured link %s\n", test.dev->name);
daniele 0:d7f2341ab245 156 pico_free(dn);
daniele 0:d7f2341ab245 157 return;
daniele 0:d7f2341ab245 158 }
daniele 0:d7f2341ab245 159
daniele 0:d7f2341ab245 160 ipv4 = pico_arp_reverse_lookup(&dn->eth);
daniele 0:d7f2341ab245 161 if (!ipv4) {
daniele 0:d7f2341ab245 162 dn->ipv4.addr = settings->pool_next;
daniele 0:d7f2341ab245 163 pico_arp_create_entry(dn->eth.addr, dn->ipv4, settings->dev);
daniele 0:d7f2341ab245 164 settings->pool_next = long_be(long_be(settings->pool_next) + 1);
daniele 0:d7f2341ab245 165 } else {
daniele 0:d7f2341ab245 166 dn->ipv4.addr = ipv4->addr;
daniele 0:d7f2341ab245 167 }
daniele 0:d7f2341ab245 168
daniele 0:d7f2341ab245 169 if (pico_tree_insert(&DHCPNegotiations, dn)) {
daniele 0:d7f2341ab245 170 dhcpd_dbg("DHCPD WARNING: tried creating new negotation for existing xid %u\n", dn->xid);
daniele 0:d7f2341ab245 171 pico_free(dn);
daniele 0:d7f2341ab245 172 return; /* Element key already exists */
daniele 0:d7f2341ab245 173 }
daniele 0:d7f2341ab245 174 }
daniele 0:d7f2341ab245 175
daniele 0:d7f2341ab245 176 if (!ip_inrange(dn->ipv4.addr))
daniele 0:d7f2341ab245 177 return;
daniele 0:d7f2341ab245 178
daniele 0:d7f2341ab245 179 opt_type = dhcp_get_next_option(dhdr->options, opt_data, &opt_len, &nextopt);
daniele 0:d7f2341ab245 180 while (opt_type != PICO_DHCPOPT_END) {
daniele 0:d7f2341ab245 181 /* parse interesting options here */
daniele 0:d7f2341ab245 182 if (opt_type == PICO_DHCPOPT_MSGTYPE) {
daniele 0:d7f2341ab245 183 /* server simple state machine */
daniele 0:d7f2341ab245 184 uint8_t msg_type = opt_data[0];
daniele 0:d7f2341ab245 185 if (msg_type == PICO_DHCP_MSG_DISCOVER) {
daniele 0:d7f2341ab245 186 dhcpd_make_offer(dn);
daniele 0:d7f2341ab245 187 dn->state = DHCPSTATE_OFFER;
daniele 0:d7f2341ab245 188 return;
daniele 0:d7f2341ab245 189 } else if ((msg_type == PICO_DHCP_MSG_REQUEST)&&( dn->state == DHCPSTATE_OFFER)) {
daniele 0:d7f2341ab245 190 dhcpd_make_ack(dn);
daniele 0:d7f2341ab245 191 dn->state = DHCPSTATE_BOUND;
daniele 0:d7f2341ab245 192 return;
daniele 0:d7f2341ab245 193 }
daniele 0:d7f2341ab245 194 }
daniele 0:d7f2341ab245 195 opt_len = 20;
daniele 0:d7f2341ab245 196 opt_type = dhcp_get_next_option(NULL, opt_data, &opt_len, &nextopt);
daniele 0:d7f2341ab245 197 }
daniele 0:d7f2341ab245 198 }
daniele 0:d7f2341ab245 199
daniele 0:d7f2341ab245 200 static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s)
daniele 0:d7f2341ab245 201 {
daniele 0:d7f2341ab245 202 uint8_t buf[DHCPD_DATAGRAM_SIZE] = { };
daniele 0:d7f2341ab245 203 int r = 0;
daniele 0:d7f2341ab245 204 uint32_t peer = 0;
daniele 0:d7f2341ab245 205 uint16_t port = 0;
daniele 0:d7f2341ab245 206
daniele 0:d7f2341ab245 207 dhcpd_dbg("DHCPD: called dhcpd_wakeup\n");
daniele 0:d7f2341ab245 208 if (ev == PICO_SOCK_EV_RD) {
daniele 0:d7f2341ab245 209 do {
daniele 0:d7f2341ab245 210 r = pico_socket_recvfrom(s, buf, DHCPD_DATAGRAM_SIZE, &peer, &port);
daniele 0:d7f2341ab245 211 if (r > 0 && port == PICO_DHCP_CLIENT_PORT) {
daniele 0:d7f2341ab245 212 dhcp_recv(s, buf, r);
daniele 0:d7f2341ab245 213 }
daniele 0:d7f2341ab245 214 } while(r>0);
daniele 0:d7f2341ab245 215 }
daniele 0:d7f2341ab245 216 }
daniele 0:d7f2341ab245 217
daniele 0:d7f2341ab245 218 int pico_dhcp_server_initiate(struct pico_dhcpd_settings *setting)
daniele 0:d7f2341ab245 219 {
daniele 0:d7f2341ab245 220 struct pico_dhcpd_settings *settings = NULL;
daniele 0:d7f2341ab245 221 struct pico_ipv4_link *link = NULL;
daniele 0:d7f2341ab245 222 uint16_t port = PICO_DHCPD_PORT;
daniele 0:d7f2341ab245 223
daniele 0:d7f2341ab245 224 if (!setting) {
daniele 0:d7f2341ab245 225 pico_err = PICO_ERR_EINVAL;
daniele 0:d7f2341ab245 226 return -1;
daniele 0:d7f2341ab245 227 }
daniele 0:d7f2341ab245 228
daniele 0:d7f2341ab245 229 if (!setting->my_ip.addr) {
daniele 0:d7f2341ab245 230 pico_err = PICO_ERR_EINVAL;
daniele 0:d7f2341ab245 231 dhcpd_dbg("DHCPD: IP address of interface was not supplied\n");
daniele 0:d7f2341ab245 232 return -1;
daniele 0:d7f2341ab245 233 }
daniele 0:d7f2341ab245 234
daniele 0:d7f2341ab245 235 link = pico_ipv4_link_get(&setting->my_ip);
daniele 0:d7f2341ab245 236 if (!link) {
daniele 0:d7f2341ab245 237 pico_err = PICO_ERR_EINVAL;
daniele 0:d7f2341ab245 238 dhcpd_dbg("DHCPD: no link with IP %X found\n", setting->my_ip.addr);
daniele 0:d7f2341ab245 239 return -1;
daniele 0:d7f2341ab245 240 }
daniele 0:d7f2341ab245 241
daniele 0:d7f2341ab245 242 settings = pico_zalloc(sizeof(struct pico_dhcpd_settings));
daniele 0:d7f2341ab245 243 if (!settings) {
daniele 0:d7f2341ab245 244 pico_err = PICO_ERR_ENOMEM;
daniele 0:d7f2341ab245 245 return -1;
daniele 0:d7f2341ab245 246 }
daniele 0:d7f2341ab245 247 memcpy(settings, setting, sizeof(struct pico_dhcpd_settings));
daniele 0:d7f2341ab245 248
daniele 0:d7f2341ab245 249 settings->dev = link->dev;
daniele 0:d7f2341ab245 250 dhcpd_dbg("DHCPD: configuring DHCP server for link %s\n", link->dev->name);
daniele 0:d7f2341ab245 251 settings->my_ip.addr = link->address.addr;
daniele 0:d7f2341ab245 252 dhcpd_dbg("DHCPD: using server addr %X\n", long_be(settings->my_ip.addr));
daniele 0:d7f2341ab245 253 settings->netmask.addr = link->netmask.addr;
daniele 0:d7f2341ab245 254 dhcpd_dbg("DHCPD: using netmask %X\n", long_be(settings->netmask.addr));
daniele 0:d7f2341ab245 255
daniele 0:d7f2341ab245 256 /* default values if not provided */
daniele 0:d7f2341ab245 257 if (settings->pool_start == 0)
daniele 0:d7f2341ab245 258 settings->pool_start = (settings->my_ip.addr & settings->netmask.addr) | POOL_START;
daniele 0:d7f2341ab245 259 dhcpd_dbg("DHCPD: using pool_start %X\n", long_be(settings->pool_start));
daniele 0:d7f2341ab245 260 if (settings->pool_end == 0)
daniele 0:d7f2341ab245 261 settings->pool_end = (settings->my_ip.addr & settings->netmask.addr) | POOL_END;
daniele 0:d7f2341ab245 262 dhcpd_dbg("DHCPD: using pool_end %x\n", long_be(settings->pool_end));
daniele 0:d7f2341ab245 263 if (settings->lease_time == 0)
daniele 0:d7f2341ab245 264 settings->lease_time = LEASE_TIME;
daniele 0:d7f2341ab245 265 dhcpd_dbg("DHCPD: using lease time %x\n", long_be(settings->lease_time));
daniele 0:d7f2341ab245 266 settings->pool_next = settings->pool_start;
daniele 0:d7f2341ab245 267
daniele 0:d7f2341ab245 268 settings->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup);
daniele 0:d7f2341ab245 269 if (!settings->s) {
daniele 0:d7f2341ab245 270 dhcpd_dbg("DHCP: could not open client socket\n");
daniele 0:d7f2341ab245 271 pico_free(settings);
daniele 0:d7f2341ab245 272 return -1;
daniele 0:d7f2341ab245 273 }
daniele 0:d7f2341ab245 274 if (pico_socket_bind(settings->s, &settings->my_ip, &port) != 0) {
daniele 0:d7f2341ab245 275 dhcpd_dbg("DHCP: could not bind server socket (%s)\n", strerror(pico_err));
daniele 0:d7f2341ab245 276 pico_free(settings);
daniele 0:d7f2341ab245 277 return -1;
daniele 0:d7f2341ab245 278 }
daniele 0:d7f2341ab245 279
daniele 0:d7f2341ab245 280 if (pico_tree_insert(&DHCPSettings, settings)) {
daniele 0:d7f2341ab245 281 dhcpd_dbg("DHCPD ERROR: link %s already configured\n", link->dev->name);
daniele 0:d7f2341ab245 282 pico_err = PICO_ERR_EINVAL;
daniele 0:d7f2341ab245 283 pico_free(settings);
daniele 0:d7f2341ab245 284 return -1; /* Element key already exists */
daniele 0:d7f2341ab245 285 }
daniele 0:d7f2341ab245 286 dhcpd_dbg("DHCPD: configured DHCP server for link %s\n", link->dev->name);
daniele 0:d7f2341ab245 287
daniele 0:d7f2341ab245 288 return 0;
daniele 0:d7f2341ab245 289 }
daniele 0:d7f2341ab245 290 #endif /* PICO_SUPPORT_DHCP */