1 /*
2 * Copyright (c) 2016, Ari Suutari <ari@stonepile.fi>.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 * 3. The name of the author may not be used to endorse or promote
15 * products derived from this software without specific prior written
16 * permission.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
19 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
22 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
26 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 * OF THE POSSIBILITY OF SUCH DAMAGE.
29 */
30
31 /*
32 * Loosely Based on simple LwIP dhcp server by Mark Lakata, posted
33 * on lwip mailing list.
34 * https://lists.gnu.org/archive/html/lwip-users/2012-12/msg00016.html
35 */
36
37 #include <stdio.h>
38 #include <string.h>
39
40 #include "lwip/api.h"
41 #include "lwip/inet.h"
42 #include "lwip/sockets.h"
43 #include "lwip/tcpip.h"
44 #include "lwip/ip_addr.h"
45 #include "lwip/udp.h"
46 #include "lwip/ip.h"
47 #include "netif/etharp.h"
48 // chipsea
49 #include "lwip/prot/dhcp.h"
50 //end
51 #include "flash_api.h"
52 #include "wifi_mac.h"
53 #include "wifi_host_cntrl.h"
54 #include "dbg.h"
55
56 /*
57 * DHCP message types.
58 */
59 #define DHCPDISCOVER 1
60 #define DHCPOFFER 2
61 #define DHCPREQUEST 3
62 #define DHCPDECLINE 4
63 #define DHCPACK 5
64 #define DHCPNAK 6
65 #define DHCPRELEASE 7
66 #define DHCPINFORM 8
67
68 /*
69 * DHCP option codes that are used.
70 */
71 #define OPT_NETMASK 1
72 #define OPT_REQ_IP 50
73 #define OPT_LEASE_TIME 51
74 #define OPT_MSGTYPE 53
75 #define OPT_SERVERID 54
76 #define OPT_RENEW_TIME 58
77 #define OPT_REBIND_TIME 59
78 #define OPT_END 255
79
80
81 #define LWIP_IANA_PORT_DHCP_CLIENT 68
82 #define LWIP_IANA_PORT_DHCP_SERVER 67
83
84 static struct udp_pcb* dhcpPcb;
85 typedef struct
86 {
87 struct eth_addr mac;
88 ip4_addr_t ip;
89 } Lease;
90 static uint16_t lease_time = 24 * 60;
91 static uint32_t dhcps_start = 10;
92 //static uint32_t dhcps_end = 20;
93
94 static uint8_t dhcps_offered = 0;
95
96 #define MAX_LEASES 10
97 static Lease leases[MAX_LEASES];
98 static const uint8_t freeMac[6] = { 0, 0, 0, 0, 0, 0 };
99
allocateAddr(struct eth_addr * mac,ip4_addr_t * ip)100 static int allocateAddr(struct eth_addr* mac, ip4_addr_t* ip)
101 {
102 // search for existing
103 for (int i = 0; i < MAX_LEASES; i++) {
104 if (memcmp(mac->addr, leases[i].mac.addr, 6) == 0) {
105
106 ip4_addr_copy(*ip, leases[i].ip);
107 return 0;
108 }
109 }
110 // create new one
111 for (int i = 0; i < MAX_LEASES; i++) {
112 if (memcmp(leases[i].mac.addr, freeMac, 6) == 0) {
113
114 memcpy(leases[i].mac.addr, mac->addr, 6);
115 ip4_addr_copy(*ip, leases[i].ip);
116 dhcps_offered++;
117 return 0;
118 }
119 }
120 return -1;
121 }
122
getOption(uint8_t * optPtr)123 static uint8_t getOption(uint8_t* optPtr)
124 {
125 return optPtr[0];
126 }
127
getOptionLen(uint8_t * optPtr)128 static uint8_t getOptionLen(uint8_t* optPtr)
129 {
130 return optPtr[1];
131 }
132
getOptionData(uint8_t * optPtr)133 static uint8_t* getOptionData(uint8_t* optPtr)
134 {
135 return optPtr + 2;
136 }
137
nextOption(uint8_t ** optPtrPtr)138 static void nextOption(uint8_t** optPtrPtr)
139 {
140 uint8_t* optPtr =*optPtrPtr;
141
142 *optPtrPtr = optPtr + getOptionLen(optPtr) + 2;
143 }
144
addOptionTime(uint8_t ** optPtrPtr,uint8_t opt,uint32_t t)145 static void addOptionTime(uint8_t** optPtrPtr, uint8_t opt, uint32_t t)
146 {
147 uint8_t* optPtr = *optPtrPtr;
148
149 *optPtr++ = opt;
150 *optPtr++ = 4;
151
152 *optPtr++ = (t / (256*256*256));
153 t %= 256*256*256;
154
155 *optPtr++ = (t / (256*256));
156 t %= 256*256;
157
158 *optPtr++ = (t / (256));
159 t %= 256;
160
161 *optPtr++ = t;
162
163 *optPtrPtr = optPtr;
164 }
165
addOptionIP(uint8_t ** optPtrPtr,uint8_t opt,const ip4_addr_t * ip)166 static void addOptionIP(uint8_t** optPtrPtr, uint8_t opt, const ip4_addr_t* ip)
167 {
168 uint8_t* optPtr = *optPtrPtr;
169
170 *optPtr++ = opt;
171 *optPtr++ = 4;
172 memcpy(optPtr, &ip4_addr_get_u32(ip), 4);
173 optPtr += 4;
174
175 *optPtrPtr = optPtr;
176 }
177
addOptionByte(uint8_t ** optPtrPtr,uint8_t opt,uint8_t val)178 static void addOptionByte(uint8_t** optPtrPtr, uint8_t opt, uint8_t val)
179 {
180 uint8_t* optPtr = *optPtrPtr;
181
182 *optPtr++ = opt;
183 *optPtr++ = 1;
184 *optPtr++ = val;
185 *optPtrPtr = optPtr;
186 }
187
newResp(struct pbuf * p)188 static struct pbuf* newResp(struct pbuf* p)
189 {
190 struct pbuf* rp;
191
192 rp = pbuf_alloc(PBUF_TRANSPORT, sizeof(struct dhcp_msg), PBUF_RAM);
193 if (rp == NULL) {
194
195 dbg("out of mem\n");
196 return NULL;
197 }
198
199 if (rp->len < sizeof(struct dhcp_msg)) {
200
201 dbg("pbuf too small\n");
202 pbuf_free(rp);
203 return NULL;
204 }
205
206 struct dhcp_msg* resp = (struct dhcp_msg*)rp->payload;
207
208 int len = p->len;
209
210 if (rp->len < len)
211 len = rp->len;
212
213 memcpy(rp->payload, p->payload, len);
214 memset(resp->options, 0, sizeof(resp->options));
215 resp->op = 2; // reply
216 resp->cookie = PP_HTONL(DHCP_MAGIC_COOKIE);
217
218 return rp;
219 }
220
handleDiscover(struct pbuf * p,struct netif * netif)221 static void handleDiscover(struct pbuf* p, struct netif* netif)
222 {
223 uint8_t * option_ptr;
224
225 ip4_addr_t addr;
226 struct dhcp_msg* req = (struct dhcp_msg*)p->payload;
227
228 if (allocateAddr((struct eth_addr*) req->chaddr, &addr) == 0) {
229
230 struct pbuf* rp = newResp(p);
231 if (rp == NULL)
232 return;
233
234 struct dhcp_msg* resp = (struct dhcp_msg*)rp->payload;
235
236 ip4_addr_copy(resp->yiaddr, addr);
237 option_ptr = (uint8_t *) &resp->options;
238
239 addOptionByte(&option_ptr, OPT_MSGTYPE, DHCPOFFER);
240 addOptionIP(&option_ptr, OPT_NETMASK, netif_ip4_netmask(netif));
241 addOptionIP(&option_ptr, OPT_SERVERID, netif_ip4_addr(netif));
242 *option_ptr++ = OPT_END;
243
244 udp_sendto_if(dhcpPcb, rp, IP_ADDR_BROADCAST, LWIP_IANA_PORT_DHCP_CLIENT, netif);
245 pbuf_free(rp);
246
247 // dbg("dhcps: offer %02X:%02X:%02X:%02X:%02X:%02X -> %s\n", resp->chaddr[0],
248 // resp->chaddr[1],
249 // resp->chaddr[2],
250 // resp->chaddr[3],
251 // resp->chaddr[4],
252 // resp->chaddr[5],
253 // inet_ntoa(addr));
254 }
255 else
256 dbg("dhcps: no free addresses left.\n");
257
258 }
259
260 extern void HandleStaConnect(uint8_t *macAddr, ip_addr_t clientIP);
handleRequest(struct pbuf * p,struct netif * netif)261 static void handleRequest(struct pbuf* p, struct netif* netif)
262 {
263 uint8_t * option_ptr;
264 ip4_addr_t newAddr;
265 ip4_addr_t reqAddr;
266
267 ip4_addr_set_zero(&reqAddr);
268 struct dhcp_msg* req = (struct dhcp_msg*)p->payload;
269
270 if (allocateAddr((struct eth_addr*) req->chaddr, &newAddr) == 0) {
271
272 struct pbuf* rp = newResp(p);
273 if (rp == NULL)
274 return;
275
276 struct dhcp_msg* resp = (struct dhcp_msg*)rp->payload;
277
278 // Find out what IP client is requesting.
279 option_ptr = (uint8_t *) &req->options;
280 while (getOption(option_ptr) != OPT_END) {
281
282 int len = getOptionLen(option_ptr);
283
284 switch (getOption(option_ptr)) {
285 case OPT_REQ_IP:
286 if (len == 4)
287 memcpy(&reqAddr, getOptionData(option_ptr), 4);
288
289 break;
290 default:
291 break;
292 }
293
294 nextOption(&option_ptr);
295 }
296
297 if (!memcmp(&reqAddr, &newAddr, 4) != 0) {
298
299 ip4_addr_copy(resp->yiaddr, reqAddr);
300 option_ptr = (uint8_t *) &resp->options;
301
302 addOptionByte(&option_ptr, OPT_MSGTYPE, DHCPACK);
303 addOptionTime(&option_ptr, OPT_RENEW_TIME, lease_time / 2 * 60);
304 addOptionTime(&option_ptr, OPT_REBIND_TIME, lease_time * 7 / 8 * 60);
305 addOptionTime(&option_ptr, OPT_LEASE_TIME, lease_time * 60);
306 addOptionIP(&option_ptr, OPT_NETMASK, netif_ip4_netmask(netif));
307 addOptionIP(&option_ptr, OPT_SERVERID, netif_ip4_addr(netif));
308 *option_ptr++ = OPT_END;
309
310 ip_addr_t dest;
311 ip_addr_t* dp = &dest;
312
313 ip_addr_set_ip4_u32(dp, ip4_addr_get_u32(&resp->yiaddr));
314 etharp_add_static_entry(ip_2_ip4(dp), (struct eth_addr *) resp->chaddr);
315 udp_sendto_if(dhcpPcb, rp, &dest, LWIP_IANA_PORT_DHCP_CLIENT, netif);
316 pbuf_free(rp);
317
318 HandleStaConnect(resp->chaddr, dest);
319 // dbg("dhcps: ack %02X:%02X:%02X:%02X:%02X:%02X -> %s\n", resp->chaddr[0],
320 // resp->chaddr[1],
321 // resp->chaddr[2],
322 // resp->chaddr[3],
323 // resp->chaddr[4],
324 // resp->chaddr[5],
325 // inet_ntoa(dest));
326 }
327 else {
328
329 // dbg("dhcps: Request IP %s is not free.\n", inet_ntoa(reqAddr));
330
331 option_ptr = (uint8_t *) &resp->options;
332
333 addOptionByte(&option_ptr, OPT_MSGTYPE, DHCPNAK);
334 addOptionIP(&option_ptr, OPT_SERVERID, netif_ip4_addr(netif));
335 *option_ptr++ = OPT_END;
336
337 udp_sendto_if(dhcpPcb, rp, IP_ADDR_BROADCAST, LWIP_IANA_PORT_DHCP_CLIENT, netif);
338 pbuf_free(rp);
339
340 }
341
342 }
343 else
344 dbg("dhcps: no free addresses left.\n");
345
346 }
347
348 extern void HandleStaDisconnect(uint8_t *macAddr);
force_dhcp_release(struct eth_addr * mac)349 uint32_t force_dhcp_release(struct eth_addr *mac)
350 {
351 // search for existing
352 for (int i = 0; i < MAX_LEASES; i++) {
353 if (memcmp(mac->addr, leases[i].mac.addr, 6) == 0) {
354 dbg("Dhcps release %d\r\n", i);
355 memset(leases[i].mac.addr, 0, 6);
356 dhcps_offered--;
357 HandleStaDisconnect(mac->addr);
358 return 0;
359 }
360 }
361
362 return 0;
363 }
handleRelese(struct pbuf * p,struct netif * netif)364 static void handleRelese(struct pbuf* p, struct netif* netif)
365 {
366 struct dhcp_msg* req = (struct dhcp_msg*)p->payload;
367 struct eth_addr eth_addr;
368 memcpy(eth_addr.addr, req->chaddr, 6);
369 force_dhcp_release(ð_addr);
370 }
371
dhcpRecv(void * arg,struct udp_pcb * pcb,struct pbuf * p,const ip_addr_t * addr,u16_t port)372 static void dhcpRecv(void* arg, struct udp_pcb* pcb, struct pbuf* p, const ip_addr_t* addr, u16_t port)
373 {
374 struct netif *netif = (struct netif *)arg;
375 struct dhcp_msg* req = (struct dhcp_msg*)p->payload;;
376
377 // dbg(D_INF "DHCP Server request received[%d]\n", req->options[2]);
378
379 switch (req->options[2]) {
380
381 case DHCPDISCOVER:
382 handleDiscover(p, netif);
383 break;
384
385 case DHCPREQUEST:
386 handleRequest(p, netif);
387 break;
388
389 case DHCPRELEASE:
390 handleRelese(p, netif);
391 break;
392
393 default:
394 dbg("DHCP unknown option %d\n", req->options[2]);
395 break;
396 }
397
398 pbuf_free(p);
399 }
400
dhcpServerStartRaw(void * arg)401 static void dhcpServerStartRaw(void* arg)
402 {
403 struct netif *netif = (struct netif *)arg;
404
405 memset(leases, '\0', sizeof(leases));
406 const ip4_addr_t* base = netif_ip4_addr(netif);
407 int i;
408
409 for (i = 0; i < MAX_LEASES; i++) {
410
411 IP4_ADDR(&leases[i].ip, ip4_addr1(base),
412 ip4_addr2(base),
413 ip4_addr3(base),
414 dhcps_start + i);
415 }
416
417 dhcpPcb = udp_new();
418 if (dhcpPcb == NULL) {
419
420 dbg("Cannot allocate dhcps pcb.\n");
421 return;
422 }
423
424 ip_set_option(dhcpPcb, SOF_BROADCAST);
425 udp_bind(dhcpPcb, IP_ADDR_ANY, LWIP_IANA_PORT_DHCP_SERVER);
426
427 udp_recv(dhcpPcb, dhcpRecv, netif);
428 }
429
dhcpServerStopRaw(void * arg)430 static void dhcpServerStopRaw(void* arg)
431 {
432 udp_remove(dhcpPcb);
433 dhcpPcb = NULL;
434 }
435
dhcpServerStart(struct netif * netif)436 err_t dhcpServerStart(struct netif* netif)
437 {
438 unsigned int ap_lease_time = 0;
439 unsigned int ap_dhcp_start = 0;
440 unsigned int ap_dhcp_end = 0;
441 flash_wifi_lwip_dhcps_read(&ap_lease_time, &ap_dhcp_start, &ap_dhcp_end);
442 if(ap_lease_time) {
443 lease_time = ap_lease_time;
444 }
445 if(ap_dhcp_start) {
446 dhcps_start = (ap_dhcp_start>>24)&0xFF;
447 }
448 fhost_cntrl_dhcps_release_register((fhost_cntrl_func_t)force_dhcp_release);
449 dbg("dhcpServerStart %d %x, %x\r\n", ap_lease_time, ap_dhcp_start, ap_dhcp_end);
450 return tcpip_callback(dhcpServerStartRaw, netif);
451 }
452
dhcpServerStop(struct netif * netif)453 err_t dhcpServerStop(struct netif* netif)
454 {
455 return tcpip_callback(dhcpServerStopRaw, netif);
456 }
457
get_client_ip_by_index(uint8_t index)458 uint32_t get_client_ip_by_index(uint8_t index)
459 {
460 if (index < MAX_LEASES) {
461 if (MAC_ADDR_VALID(leases[index].mac.addr) != 0)
462 return leases[index].ip.addr;
463 }
464 dbg("Error: dhcp index[%d] unused! \r\n", index);
465 return 0;
466 }
467
get_client_ip_offered_cnt(void)468 uint8_t get_client_ip_offered_cnt(void)
469 {
470 return dhcps_offered;
471 }
472
473