1 /*
2 * Copyright (C) 2022 Beken Corporation
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15 #include <string.h>
16 #include <os/os.h>
17 #include "dhcp-bootp.h"
18 #include "dns.h"
19 #include "dhcp-priv.h"
20 #include <os/str.h>
21 #include <os/mem.h>
22 #include "lwip/etharp.h"
23 #include "lwip/sockets.h"
24 #include "opt.h"
25
26 #define os_mem_alloc os_malloc
27 #define os_mem_free os_free
28 #define SEND_RESPONSE(w,x,y,z) send_response(w,x,y,z)
29
30 #define DEFAULT_DHCP_ADDRESS_TIMEOUT (60U*60U*1U) /* 1 hour */
31 #define CLIENT_IP_NOT_FOUND 0x00000000
32
33 uint32_t dhcp_address_timeout = DEFAULT_DHCP_ADDRESS_TIMEOUT;
34 static beken_mutex_t dhcpd_mutex;
35 static int (*dhcp_nack_dns_server_handler)(char *msg, int len,
36 struct sockaddr_in *fromaddr);
37
38 struct dhcp_server_data dhcps;
39
40 extern int net_get_if_macaddr(void *macaddr, void *intrfc_handle);
41 extern int net_get_if_ip_addr(uint32_t *ip, void *intrfc_handle);
42 extern int net_get_if_ip_mask(uint32_t *nm, void *intrfc_handle);
43 extern int net_get_if_gw_addr(uint32_t *ip, void *intrfc_handle);
44 static void get_broadcast_addr(struct sockaddr_in *addr);
45 static int get_ip_addr_from_interface(uint32_t *ip, void *interface_handle);
46 static int get_netmask_from_interface(uint32_t *nm, void *interface_handle);
47 static int get_mac_addr_from_interface(void *mac, void *interface_handle);
48 static int get_gateway_from_interface(uint32_t *gw, void *interface_handle);
49 static int send_gratuitous_arp(uint32_t ip);
50 static bool ac_add(uint8_t *chaddr, uint32_t client_ip);
51 static uint32_t ac_lookup_mac(uint8_t *chaddr);
52 static uint8_t *ac_lookup_ip(uint32_t client_ip);
53 static bool ac_not_full();
54
ac_add(uint8_t * chaddr,uint32_t client_ip)55 static bool ac_add(uint8_t *chaddr, uint32_t client_ip)
56 {
57 /* adds ip-mac mapping in cache */
58 if (ac_not_full()) {
59 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[0]
60 = chaddr[0];
61 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[1]
62 = chaddr[1];
63 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[2]
64 = chaddr[2];
65 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[3]
66 = chaddr[3];
67 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[4]
68 = chaddr[4];
69 dhcps.ip_mac_mapping[dhcps.count_clients].client_mac[5]
70 = chaddr[5];
71 dhcps.ip_mac_mapping[dhcps.count_clients].client_ip = client_ip;
72 dhcps.count_clients++;
73 return 0;
74 }
75 return -1;
76 }
77
ac_lookup_mac(uint8_t * chaddr)78 static uint32_t ac_lookup_mac(uint8_t *chaddr)
79 {
80 /* returns ip address, if mac address is present in cache */
81 int i;
82 for (i = 0; i < dhcps.count_clients && i < MAC_IP_CACHE_SIZE; i++) {
83 if ((dhcps.ip_mac_mapping[i].client_mac[0] == chaddr[0]) &&
84 (dhcps.ip_mac_mapping[i].client_mac[1] == chaddr[1]) &&
85 (dhcps.ip_mac_mapping[i].client_mac[2] == chaddr[2]) &&
86 (dhcps.ip_mac_mapping[i].client_mac[3] == chaddr[3]) &&
87 (dhcps.ip_mac_mapping[i].client_mac[4] == chaddr[4]) &&
88 (dhcps.ip_mac_mapping[i].client_mac[5] == chaddr[5])) {
89 return dhcps.ip_mac_mapping[i].client_ip;
90 }
91 }
92 return CLIENT_IP_NOT_FOUND;
93 }
94
ac_lookup_ip(uint32_t client_ip)95 static uint8_t *ac_lookup_ip(uint32_t client_ip)
96 {
97 /* returns mac address, if ip address is present in cache */
98 int i;
99 for (i = 0; i < dhcps.count_clients && i < MAC_IP_CACHE_SIZE; i++) {
100 if ((dhcps.ip_mac_mapping[i].client_ip) == client_ip) {
101 return dhcps.ip_mac_mapping[i].client_mac;
102 }
103 }
104 return NULL;
105 }
106
ac_not_full()107 static bool ac_not_full()
108 {
109 /* returns true if cache is not full */
110 return (dhcps.count_clients < MAC_IP_CACHE_SIZE);
111 }
112
ac_valid_ip(uint32_t requested_ip)113 static bool ac_valid_ip(uint32_t requested_ip)
114 {
115 /* skip over our own address, the network address or the
116 * broadcast address
117 */
118 if (requested_ip == ntohl(dhcps.my_ip) ||
119 (requested_ip == ntohl(dhcps.my_ip &
120 dhcps.netmask)) ||
121 (requested_ip == ntohl((dhcps.my_ip |
122 (0xffffffff & ~dhcps.netmask))))) {
123 return false;
124 }
125 if (ac_lookup_ip(htonl(requested_ip)) != NULL)
126 return false;
127 return true;
128 }
129
write_u32(char * dest,uint32_t be_value)130 static void write_u32(char *dest, uint32_t be_value)
131 {
132 *dest++ = be_value & 0xFF;
133 *dest++ = (be_value >> 8) & 0xFF;
134 *dest++ = (be_value >> 16) & 0xFF;
135 *dest = be_value >> 24;
136 }
137
138 /* Configure the DHCP dynamic IP lease time*/
dhcp_server_lease_timeout(uint32_t val)139 int dhcp_server_lease_timeout(uint32_t val)
140 {
141 if ((val == 0) || (val > (60U*60U*24U*49700U))) {
142 return -EINVAL;
143 } else {
144 dhcp_address_timeout = val;
145 return 0;
146 }
147 }
148
149 /* calculate the address to give out to the next DHCP DISCOVER request
150 *
151 * DHCP clients will be assigned addresses in sequence in the subnet's address space.
152 */
next_yiaddr()153 static unsigned int next_yiaddr()
154 {
155 #ifdef CONFIG_DHCP_SERVER_DEBUG
156 struct in_addr ip;
157 #endif
158 uint32_t new_ip;
159 struct bootp_header *hdr = (struct bootp_header *)dhcps.msg;
160
161 /* if device requesting for ip address is already registered,
162 * if yes, assign previous ip address to it
163 */
164 new_ip = ac_lookup_mac(hdr->chaddr);
165 if (new_ip == (CLIENT_IP_NOT_FOUND)) {
166 /* next IP address in the subnet */
167 dhcps.current_ip = ntohl(dhcps.my_ip & dhcps.netmask) |
168 ((dhcps.current_ip + 1) & ntohl(~dhcps.netmask));
169 while (!ac_valid_ip(dhcps.current_ip)) {
170 dhcps.current_ip = ntohl(dhcps.my_ip & dhcps.netmask) |
171 ((dhcps.current_ip + 1) &
172 ntohl(~dhcps.netmask));
173 }
174
175 new_ip = htonl(dhcps.current_ip);
176
177 if (ac_add(hdr->chaddr, new_ip) !=
178 0)
179 dhcp_w("No space to store new mapping..\r\n");
180 }
181
182 #ifdef CONFIG_DHCP_SERVER_DEBUG
183 ip.s_addr = new_ip;
184 dhcp_d("New client IP will be %s\r\n", inet_ntoa(ip));
185 ip.s_addr = dhcps.my_ip & dhcps.netmask;
186 #endif
187
188 return new_ip;
189 }
190
make_response(char * msg,enum dhcp_message_type type)191 static unsigned int make_response(char *msg, enum dhcp_message_type type)
192 {
193 struct bootp_header *hdr;
194 struct bootp_option *opt;
195 char *offset = msg;
196
197 hdr = (struct bootp_header *)offset;
198 hdr->op = BOOTP_OP_RESPONSE;
199 hdr->htype = 1;
200 hdr->hlen = 6;
201 hdr->hops = 0;
202 hdr->ciaddr = 0;
203 hdr->yiaddr = (type == DHCP_MESSAGE_ACK) ? dhcps.client_ip : 0;
204 hdr->yiaddr = (type == DHCP_MESSAGE_OFFER) ?
205 next_yiaddr() : hdr->yiaddr;
206 hdr->siaddr = 0;
207 hdr->riaddr = 0;
208 offset += sizeof(struct bootp_header);
209
210 opt = (struct bootp_option *)offset;
211 opt->type = BOOTP_OPTION_DHCP_MESSAGE;
212 *(uint8_t *) opt->value = type;
213 opt->length = 1;
214 offset += sizeof(struct bootp_option) + opt->length;
215
216 if (type == DHCP_MESSAGE_NAK)
217 return (unsigned int)(offset - msg);
218
219 opt = (struct bootp_option *)offset;
220 opt->type = BOOTP_OPTION_SUBNET_MASK;
221 write_u32(opt->value, dhcps.netmask);
222 opt->length = 4;
223 offset += sizeof(struct bootp_option) + opt->length;
224
225 opt = (struct bootp_option *)offset;
226 opt->type = BOOTP_OPTION_ADDRESS_TIME;
227 write_u32(opt->value, htonl(dhcp_address_timeout));
228 opt->length = 4;
229 offset += sizeof(struct bootp_option) + opt->length;
230
231 opt = (struct bootp_option *)offset;
232 opt->type = BOOTP_OPTION_DHCP_SERVER_ID;
233 write_u32(opt->value, dhcps.my_ip);
234 opt->length = 4;
235 offset += sizeof(struct bootp_option) + opt->length;
236
237 opt = (struct bootp_option *)offset;
238 opt->type = BOOTP_OPTION_ROUTER;
239 write_u32(opt->value, dhcps.router_ip);
240 opt->length = 4;
241 offset += sizeof(struct bootp_option) + opt->length;
242
243 opt = (struct bootp_option *)offset;
244 opt->type = BOOTP_OPTION_NAMESERVER;
245 if (dhcp_nack_dns_server_handler)
246 write_u32(opt->value, dhcps.router_ip);
247 else
248 write_u32(opt->value, 0);
249 opt->length = 4;
250 offset += sizeof(struct bootp_option) + opt->length;
251
252 opt = (struct bootp_option *)offset;
253 opt->type = BOOTP_END_OPTION;
254 offset++;
255
256 return (unsigned int)(offset - msg);
257 }
258
dhcp_get_ip_from_mac(uint8_t * client_mac,uint32_t * client_ip)259 int dhcp_get_ip_from_mac(uint8_t *client_mac, uint32_t *client_ip)
260 {
261 *client_ip = ac_lookup_mac(client_mac);
262 if (*client_ip == CLIENT_IP_NOT_FOUND) {
263 return -1;
264 }
265 return 0;
266 }
267
268 extern void ap_set_default_netif(void);
269 extern void reset_default_netif(void);
270
send_response(int sock,struct sockaddr * addr,char * msg,int len)271 static int send_response(int sock, struct sockaddr *addr, char *msg, int len)
272 {
273 int nb;
274 unsigned int sent = 0;
275 // // in AP+STA mode, should set ap-netif as default
276 int dest_ip = ((struct sockaddr_in *)addr)->sin_addr.s_addr;
277 if ((dest_ip == IPADDR_ANY) || (dest_ip == IPADDR_BROADCAST))
278 ap_set_default_netif();
279
280 while (sent < len) {
281 nb = lwip_sendto(sock, msg + sent, len - sent, 0, addr,
282 sizeof(struct sockaddr_in));
283 if (nb < 0) {
284 dhcp_e("failed to send response, addr %p:%d\r\n",
285 ((struct sockaddr_in *)addr)->sin_addr,
286 ((struct sockaddr_in *)addr)->sin_port);
287 return -1;
288 }
289 sent += nb;
290 }
291
292 // rest default netif to sta's netif
293 //if((dest_ip == IPADDR_ANY) || (dest_ip == IPADDR_BROADCAST))
294 //reset_default_netif();
295
296 dhcp_d("sent response, %d bytes %s\r\n", sent,
297 inet_ntoa(((struct sockaddr_in *)addr)->sin_addr));
298 return 0;
299 }
300
301 #define ERROR_REFUSED 5
process_dns_message(char * msg,int len,struct sockaddr_in * fromaddr)302 static int process_dns_message(char *msg, int len, struct sockaddr_in *fromaddr)
303 {
304 struct dns_header *hdr;
305 char *endp = msg + len;
306 int nq;
307
308 if (len < sizeof(struct dns_header)) {
309 dhcp_e("DNS request is not complete, hence ignoring it\r\n");
310 return -1;
311 }
312
313 hdr = (struct dns_header *)msg;
314
315 dhcp_d("DNS transaction id: 0x%x\r\n", htons(hdr->id));
316
317 if (hdr->flags.fields.qr) {
318 dhcp_e("ignoring this dns message (not a query)\r\n");
319 return -1;
320 }
321
322 nq = ntohs(hdr->num_questions);
323 dhcp_d("we were asked %d questions\r\n", nq);
324
325 if (nq <= 0) {
326 dhcp_e("ignoring this dns msg (not a query or 0 questions)\r\n");
327 return -1;
328 }
329
330 /* make the header represent a response */
331 hdr->flags.fields.qr = 1;
332 hdr->flags.fields.opcode = 0;
333 /* Errors are never authoritative (unless they are
334 NXDOMAINS, which this is not) */
335 hdr->flags.fields.aa = 0;
336 hdr->flags.fields.tc = 0;
337 hdr->flags.fields.rd = 1;
338 hdr->flags.fields.ra = 0;
339 hdr->flags.fields.rcode = ERROR_REFUSED;
340 hdr->flags.num = htons(hdr->flags.num);
341 /* number of entries in questions section */
342 hdr->num_questions = htons(0x01);
343 hdr->answer_rrs = 0; /* number of resource records in answer section */
344 hdr->authority_rrs = 0;
345 hdr->additional_rrs = 0;
346 SEND_RESPONSE(dhcps.dnssock, (struct sockaddr *)fromaddr,
347 msg, endp - msg);
348
349 return -1;
350 }
351
process_dhcp_message(char * msg,int len)352 static int process_dhcp_message(char *msg, int len)
353 {
354 struct bootp_header *hdr;
355 struct bootp_option *opt;
356 uint8_t response_type = DHCP_NO_RESPONSE;
357 unsigned int consumed = 0;
358 bool got_ip = 0;
359 bool need_ip = 0;
360 bool got_client_ip = 0;
361 uint32_t new_ip;
362
363 if (!msg ||
364 len < sizeof(struct bootp_header) + sizeof(struct bootp_option) + 1)
365 return -1;
366
367 hdr = (struct bootp_header *)msg;
368
369 switch (hdr->op) {
370 case BOOTP_OP_REQUEST:
371 dhcp_d("bootp request\r\n");
372 break;
373 case BOOTP_OP_RESPONSE:
374 dhcp_d("bootp response\r\n");
375 break;
376 default:
377 dhcp_e("invalid op code: %d\r\n", hdr->op);
378 return -1;
379 }
380
381 if (hdr->htype != 1 || hdr->hlen != 6) {
382 dhcp_e("invalid htype or hlen\r\n");
383 return -1;
384 }
385
386 dhcp_d("client MAC: %02X:%02X:%02X:%02X:%02X:%02X\r\n", hdr->chaddr[0],
387 hdr->chaddr[1], hdr->chaddr[2], hdr->chaddr[3], hdr->chaddr[4],
388 hdr->chaddr[5]);
389
390 dhcp_d("magic cookie: 0x%X\r\n", hdr->cookie);
391
392 len -= sizeof(struct bootp_header);
393 opt = (struct bootp_option *)(msg + sizeof(struct bootp_header));
394 while (len > 0 && opt->type != BOOTP_END_OPTION) {
395 if (opt->type == BOOTP_OPTION_DHCP_MESSAGE && opt->length == 1) {
396 dhcp_d("found DHCP message option\r\n");
397 switch (*(uint8_t *) opt->value) {
398 case DHCP_MESSAGE_DISCOVER:
399 LWIP_LOGI("DHCP discover\r\n");
400 response_type = DHCP_MESSAGE_OFFER;
401 break;
402
403 case DHCP_MESSAGE_REQUEST:
404 LWIP_LOGI("DHCP request\r\n");
405 need_ip = 1;
406 if (hdr->ciaddr != 0x0000000) {
407 dhcps.client_ip = hdr->ciaddr;
408 got_client_ip = 1;
409 }
410 break;
411
412 default:
413 LWIP_LOGI("ignoring message type %d\r\n",
414 *(uint8_t *) opt->value);
415 break;
416 }
417 }
418 if (opt->type == BOOTP_OPTION_REQUESTED_IP && opt->length == 4) {
419 dhcp_d("found REQUESTED IP option %hhu.%hhu.%hhu.%hhu\r\n",
420 (uint8_t)opt->value[0],
421 (uint8_t)opt->value[1],
422 (uint8_t)opt->value[2],
423 (uint8_t)opt->value[3]);
424 memcpy((uint8_t *) &dhcps.client_ip, (uint8_t *) opt->value, 4);
425 got_client_ip = 1;
426 }
427
428 if (got_client_ip) {
429 /* requested address outside of subnet */
430 if ((dhcps.client_ip & dhcps.netmask) ==
431 (dhcps.my_ip & dhcps.netmask)) {
432
433 /* When client requests an IP address,
434 * DHCP-server checks if the valid
435 * IP-MAC entry is present in the
436 * ip-mac cache, if yes, also checks
437 * if the requested IP is same as the
438 * IP address present in IP-MAC entry,
439 * if yes, it allows the device to
440 * continue with the requested IP
441 * address.
442 */
443 new_ip = ac_lookup_mac(hdr->chaddr);
444 if (new_ip != (CLIENT_IP_NOT_FOUND)) {
445 /* if new_ip is equal to requested ip */
446 if (new_ip == dhcps.client_ip) {
447 got_ip = 1;
448 } else {
449 got_ip = 0;
450 }
451 } else if (ac_valid_ip
452 (ntohl(dhcps.client_ip))) {
453 /* When client requests with an IP
454 * address that is within subnet range
455 * and not assigned to any other client,
456 * then dhcp-server allows that device
457 * to continue with that IP address.
458 * And if IP-MAC cache is not full then
459 * adds this entry in cache.
460 */
461 if (ac_not_full()) {
462 ac_add(hdr->chaddr,
463 dhcps.client_ip);
464 } else {
465 dhcp_w("No space to store new \r\n"
466 "mapping..\r\n");
467 }
468 got_ip = 1;
469 }
470 }
471 }
472
473 /* look at the next option (if any) */
474 consumed = sizeof(struct bootp_option) + opt->length;
475 len -= consumed;
476 opt = (struct bootp_option *)((char *)opt + consumed);
477 if (need_ip)
478 response_type = got_ip ? DHCP_MESSAGE_ACK :
479 DHCP_MESSAGE_NAK;
480 }
481
482 if (response_type != DHCP_NO_RESPONSE) {
483 uint32_t dst_ip = 0, retry = 0;
484 int send_byte;
485 struct bootp_header *hdr;
486 if (response_type == DHCP_MESSAGE_OFFER)
487 LWIP_LOGI("DHCP should send offer\r\n");
488 else if (response_type == DHCP_MESSAGE_ACK)
489 LWIP_LOGI("DHCP should send ack\r\n");
490 send_byte = make_response(msg, (enum dhcp_message_type)response_type);
491 hdr = (struct bootp_header *)msg;
492
493 dst_ip = hdr->yiaddr;
494
495 if(dst_ip != 0) {
496 dhcps.uaddr.sin_addr.s_addr = dst_ip;
497
498 //dhcps.baddr.sin_addr.s_addr = dst_ip;
499 dhcp_d("change dhcps.uaddr: %s\r\n", inet_ntoa(dhcps.uaddr.sin_addr));
500
501 etharp_add_static_entry((ip4_addr_t *)&dst_ip, (struct eth_addr *)hdr->chaddr);
502
503 retry = 0;
504
505 dhcp_d("this pkt resp is unicast\r\n");
506 hdr->flags &= ~(htons(1<<15));
507 while(1) {
508 SEND_RESPONSE(dhcps.sock,
509 (struct sockaddr *)&dhcps.uaddr, msg, send_byte);
510
511 if(++retry == 1)
512 break;
513 }
514 etharp_remove_static_entry((ip4_addr_t *)&dst_ip);
515 }
516
517 dhcp_d("this pkt resp is broadcast\r\n");
518 hdr->flags |= (htons(1<<15));
519 retry = 0;
520 while(1) {
521 SEND_RESPONSE(dhcps.sock,
522 (struct sockaddr *)&dhcps.baddr, msg, send_byte);
523
524 if(++retry == 3)
525 break;
526 }
527
528 if (response_type == DHCP_MESSAGE_ACK)
529 send_gratuitous_arp(dhcps.my_ip);
530 return 0;
531 }
532
533 dhcp_d("ignoring DHCP packet\r\n");
534 return 0;
535 }
536
dhcp_clean_sockets(void)537 static void dhcp_clean_sockets(void)
538 {
539 int ret;
540
541 if (dhcps.sock != -1)
542 {
543 ret = lwip_close(dhcps.sock);
544 if (ret != 0) {
545 dhcp_w("Failed to close dhcp socket\r\n");
546 }
547 dhcps.sock = -1;
548 }
549
550 if ( dhcp_nack_dns_server_handler )
551 {
552 if ( dhcps.dnssock != -1 )
553 {
554 ret = lwip_close(dhcps.dnssock);
555 if ( ret != 0 )
556 {
557 dhcp_w("Failed to close dns socket\r\n");
558 }
559 dhcps.dnssock = -1;
560 }
561 }
562
563 if ( dhcps.ctrlsock != -1 )
564 {
565 ret = lwip_close(dhcps.ctrlsock);
566 if ( ret != 0 )
567 {
568 dhcp_w("Failed to close ctrol port socket\r\n");
569 }
570 dhcps.ctrlsock = -1;
571 }
572 }
573
dhcp_server(void * data)574 void dhcp_server(void* data)
575 {
576 int ret, len;
577 struct sockaddr_in caddr;
578 int max_sock;
579 socklen_t flen = sizeof(caddr);
580 fd_set rfds;
581
582 rtos_lock_mutex(&dhcpd_mutex);
583
584 while (1) {
585 FD_ZERO(&rfds);
586 FD_SET(dhcps.sock, &rfds);
587 max_sock = dhcps.sock;
588
589 if (dhcp_nack_dns_server_handler) {
590 FD_SET(dhcps.dnssock, &rfds);
591 max_sock = (dhcps.sock > dhcps.dnssock ?
592 dhcps.sock : dhcps.dnssock);
593 }
594
595 FD_SET(dhcps.ctrlsock, &rfds);
596 max_sock = (dhcps.sock > dhcps.ctrlsock ?
597 dhcps.sock : dhcps.ctrlsock);
598
599 ret = lwip_select(max_sock + 1, &rfds, NULL, NULL, NULL);
600
601 /* Error in select? */
602 if (ret < 0) {
603 dhcp_e("select failed\r\n", -1);
604 goto done;
605 }
606
607 if (FD_ISSET(dhcps.sock, &rfds)) {
608 len = lwip_recvfrom(dhcps.sock, dhcps.msg,
609 SERVER_BUFFER_SIZE,
610 0, (struct sockaddr *)&caddr, &flen);
611 if (len > 0) {
612 dhcp_d("recved msg on dhcp sock len: %d\r\n", len);
613 process_dhcp_message(dhcps.msg, len);
614 }
615 }
616
617 if ( dhcp_nack_dns_server_handler )
618 {
619 if (FD_ISSET(dhcps.dnssock, &rfds)) {
620 len = lwip_recvfrom(dhcps.dnssock, dhcps.msg,
621 SERVER_BUFFER_SIZE,
622 0, (struct sockaddr *)&caddr, &flen);
623 if (len > 0) {
624 dhcp_d("recved msg on dns sock len: %d\r\n", len);
625 dhcp_nack_dns_server_handler(dhcps.msg, len,
626 &caddr);
627 }
628 }
629 }
630
631 if ( FD_ISSET(dhcps.ctrlsock, &rfds) )
632 {
633 len = lwip_recvfrom(dhcps.ctrlsock, dhcps.msg,
634 SERVER_BUFFER_SIZE,
635 0, (struct sockaddr *)&caddr, &flen);
636 if ( len > 0 )
637 {
638 if ( os_strcmp(dhcps.msg, "HALT") == 0 )
639 {
640 goto done;
641 }
642 }
643 }
644 }
645
646 done:
647 dhcp_clean_sockets();
648 rtos_unlock_mutex(&dhcpd_mutex);
649 rtos_delete_thread(NULL);
650 }
651
create_and_bind_udp_socket(struct sockaddr_in * address,void * intrfc_handle)652 static int create_and_bind_udp_socket(struct sockaddr_in *address,
653 void *intrfc_handle)
654 {
655 int one = 1;
656 int ret;
657
658 int sock = lwip_socket(PF_INET, SOCK_DGRAM, 0);
659 if (sock == -1) {
660 dhcp_e("failed to create a socket\r\n");
661 return -1;
662 }
663
664 ret = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *)&one,
665 sizeof(int));
666 if (ret == -1) {
667 /* This is unimplemented in lwIP, hence do not return */
668 dhcp_e("failed to set SO_REUSEADDR\r\n");
669 }
670
671 if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST,
672 (char *)&one, sizeof(one)) == -1) {
673 dhcp_e("failed to set SO_BROADCAST\r\n");
674 lwip_close(sock);
675 return -1;
676 }
677
678 ret = lwip_bind(sock, (struct sockaddr *)address,
679 sizeof(struct sockaddr));
680
681 if (ret) {
682 dhcp_e("failed to bind server socket\r\n");
683 dhcp_e("socket err: %d\r\n", errno);
684 lwip_close(sock);
685 return -1;
686 }
687 return sock;
688 }
689
dhcp_server_init(void * intrfc_handle)690 int dhcp_server_init(void *intrfc_handle)
691 {
692 int ret = 0;
693
694 memset(&dhcps, 0, sizeof(dhcps));
695 dhcps.msg = (char*)os_mem_alloc(SERVER_BUFFER_SIZE);
696 if (dhcps.msg == NULL)
697 return -1;
698 ret = rtos_init_mutex(&dhcpd_mutex);
699 if (ret != 0)
700 return -1;
701
702 get_broadcast_addr(&dhcps.baddr);
703 dhcps.baddr.sin_port = htons(DHCP_CLIENT_PORT);
704
705 get_broadcast_addr(&dhcps.uaddr);
706 dhcps.uaddr.sin_port = htons(DHCP_CLIENT_PORT);
707
708 if (get_ip_addr_from_interface(&dhcps.my_ip, intrfc_handle) < 0) {
709 dhcp_e("failed to look up our IP address from interface\r\n");
710 ret = -1;
711 goto out;
712 }
713
714 if (get_netmask_from_interface(&dhcps.netmask, intrfc_handle) < 0) {
715 dhcp_e("failed to look up our netmask from interface\r\n");
716 ret = -1;
717 goto out;
718 }
719
720 if (get_gateway_from_interface(&dhcps.router_ip, intrfc_handle) < 0) {
721 dhcp_e("failed to look up our netmask from interface\r\n");
722 ret = -1;
723 goto out;
724 }
725
726
727 dhcps.saddr.sin_family = AF_INET;
728 dhcps.saddr.sin_addr.s_addr = INADDR_ANY;
729 dhcps.saddr.sin_port = htons(DHCP_SERVER_PORT);
730 dhcps.sock = create_and_bind_udp_socket(&dhcps.saddr, intrfc_handle);
731
732 if (dhcps.sock < 0) {
733 ret = -1;
734 goto out;
735 }
736
737 if (dhcp_nack_dns_server_handler) {
738 dhcps.dnsaddr.sin_family = AF_INET;
739 dhcps.dnsaddr.sin_addr.s_addr = INADDR_ANY;
740 dhcps.dnsaddr.sin_port = htons(NAMESERVER_PORT);
741 dhcps.dnssock = create_and_bind_udp_socket(&dhcps.dnsaddr,
742 intrfc_handle);
743 if (dhcps.dnssock < 0)
744 return -1;
745 }
746
747 dhcps.ctrladdr.sin_family = AF_INET;
748 dhcps.ctrladdr.sin_addr.s_addr = inet_addr("127.0.0.1");
749 dhcps.ctrladdr.sin_port = htons(CTRL_PORT);
750 dhcps.ctrlsock = create_and_bind_udp_socket(&dhcps.ctrladdr,
751 intrfc_handle);
752 if ( dhcps.ctrlsock < 0 )
753 {
754 ret = -1;
755 goto out;
756 }
757
758 dhcps.prv = intrfc_handle;
759
760 dhcps.current_ip = ntohl(dhcps.my_ip & dhcps.netmask) | ((99) & ntohl(~dhcps.netmask));
761 //LWIP_LOGI("[abc] %x, %x, %x\r\n", dhcps.current_ip, dhcps.my_ip, dhcps.router_ip);
762
763 return 0;
764
765 out:
766 os_mem_free(dhcps.msg);
767 dhcps.msg = NULL;
768 rtos_deinit_mutex(&dhcpd_mutex);
769 return ret;
770 }
771
dhcp_enable_nack_dns_server(void)772 void dhcp_enable_nack_dns_server(void)
773 {
774 dhcp_nack_dns_server_handler = process_dns_message;
775 }
776
send_ctrl_msg(const char * msg)777 static int send_ctrl_msg(const char *msg)
778 {
779 int ret;
780
781 ret = lwip_sendto(dhcps.ctrlsock, msg, os_strlen(msg)+1, 0,
782 (struct sockaddr *)&dhcps.ctrladdr, sizeof(struct sockaddr_in));
783
784 return ret;
785 }
786
dhcp_send_halt(void)787 int dhcp_send_halt(void)
788 {
789 int ret;
790
791 ret = send_ctrl_msg("HALT");
792 if ( ret < 0 )
793 {
794 dhcp_w("Failed to send HALT: %d.\r\n", ret);
795 return -1;
796 }
797
798 ret = dhcp_free_allocations();
799
800 return ret;
801 }
802
dhcp_free_allocations(void)803 int dhcp_free_allocations(void)
804 {
805 /* Wait for 10 seconds */
806 rtos_lock_mutex(&dhcpd_mutex);
807
808 dhcp_clean_sockets();
809
810 rtos_unlock_mutex(&dhcpd_mutex);
811
812 if (dhcps.msg) {
813 os_mem_free(dhcps.msg);
814 dhcps.msg = NULL;
815 }
816 return rtos_deinit_mutex(&dhcpd_mutex);
817 }
818
send_gratuitous_arp(uint32_t ip)819 static int send_gratuitous_arp(uint32_t ip)
820 {
821 int sock;
822 struct arp_packet pkt;
823 struct sockaddr_in to_addr;
824 to_addr.sin_family = AF_INET;
825 to_addr.sin_addr.s_addr = ip;
826 pkt.frame_type = htons(ARP_FRAME_TYPE);
827 pkt.hw_type = htons(ETHER_HW_TYPE);
828 pkt.prot_type = htons(IP_PROTO_TYPE);
829 pkt.hw_addr_size = ETH_HW_ADDR_LEN;
830 pkt.prot_addr_size = IP_ADDR_LEN;
831 pkt.op = htons(OP_ARP_REQUEST);
832
833 write_u32(pkt.sndr_ip_addr, ip);
834 write_u32(pkt.rcpt_ip_addr, ip);
835
836 memset(pkt.targ_hw_addr, 0xff, ETH_HW_ADDR_LEN);
837 memset(pkt.rcpt_hw_addr, 0xff, ETH_HW_ADDR_LEN);
838 get_mac_addr_from_interface((void*)pkt.sndr_hw_addr, dhcps.prv);
839 get_mac_addr_from_interface((void*)pkt.src_hw_addr, dhcps.prv);
840
841 sock = lwip_socket(AF_INET, SOCK_DGRAM, 0);
842 if (sock < 0) {
843 dhcp_e("Could not open socket to send Gratuitous ARP\r\n");
844 return -1;
845 }
846 memset(pkt.padding, 0, sizeof(pkt.padding));
847
848 if (lwip_sendto(sock, (char *)&pkt, sizeof(pkt), 0,
849 (struct sockaddr *)&to_addr, sizeof(to_addr)) < 0) {
850 dhcp_e("Failed to send Gratuitous ARP\r\n");
851 lwip_close(sock);
852 return -1;
853 }
854 dhcp_d("Gratuitous ARP sent\r\n");
855 lwip_close(sock);
856 return 0;
857 }
858
get_broadcast_addr(struct sockaddr_in * addr)859 static void get_broadcast_addr(struct sockaddr_in *addr)
860 {
861 addr->sin_family = AF_INET;
862 /* limited broadcast addr (255.255.255.255) */
863 addr->sin_addr.s_addr = 0xffffffff;
864 addr->sin_len = sizeof(struct sockaddr_in);
865 }
866
get_mac_addr_from_interface(void * mac,void * interface_handle)867 static int get_mac_addr_from_interface(void *mac, void *interface_handle)
868 {
869 return net_get_if_macaddr(mac, interface_handle);
870 }
871
get_ip_addr_from_interface(uint32_t * ip,void * interface_handle)872 static int get_ip_addr_from_interface(uint32_t *ip, void *interface_handle)
873 {
874 return net_get_if_ip_addr(ip, interface_handle);
875 }
876
get_netmask_from_interface(uint32_t * nm,void * interface_handle)877 static int get_netmask_from_interface(uint32_t *nm, void *interface_handle)
878 {
879 return net_get_if_ip_mask(nm, interface_handle);
880 }
881
get_gateway_from_interface(uint32_t * gw,void * interface_handle)882 static int get_gateway_from_interface(uint32_t *gw, void *interface_handle)
883 {
884 return net_get_if_gw_addr(gw, interface_handle);
885 }
886
dhcp_lookup_mac(uint8_t * chaddr)887 uint8_t* dhcp_lookup_mac(uint8_t *chaddr)
888 {
889 /* returns ip address, if mac address is present in cache */
890 int i;
891 struct in_addr ip;
892 for (i = 0; i < dhcps.count_clients && i < MAC_IP_CACHE_SIZE; i++) {
893 if ((dhcps.ip_mac_mapping[i].client_mac[0] == chaddr[0]) &&
894 (dhcps.ip_mac_mapping[i].client_mac[1] == chaddr[1]) &&
895 (dhcps.ip_mac_mapping[i].client_mac[2] == chaddr[2]) &&
896 (dhcps.ip_mac_mapping[i].client_mac[3] == chaddr[3]) &&
897 (dhcps.ip_mac_mapping[i].client_mac[4] == chaddr[4]) &&
898 (dhcps.ip_mac_mapping[i].client_mac[5] == chaddr[5])) {
899
900 ip.s_addr = dhcps.ip_mac_mapping[i].client_ip;
901
902 return (uint8_t*)inet_ntoa(ip);
903 }
904 }
905 return 0;
906 }
907