diff --git a/extras/dhcpserver/component.mk b/extras/dhcpserver/component.mk new file mode 100644 index 0000000..79a6dc9 --- /dev/null +++ b/extras/dhcpserver/component.mk @@ -0,0 +1,11 @@ +# Component makefile for extras/dhcpserver + +#error IWASERROR + +INC_DIRS += $(ROOT)extras/dhcpserver/include + +# args for passing into compile rule generation +extras/dhcpserver_INC_DIR = $(ROOT)extras/dhcpserver +extras/dhcpserver_SRC_DIR = $(ROOT)extras/dhcpserver + +$(eval $(call component_compile_rules,extras/dhcpserver)) diff --git a/extras/dhcpserver/dhcpserver.c b/extras/dhcpserver/dhcpserver.c new file mode 100644 index 0000000..106a9bd --- /dev/null +++ b/extras/dhcpserver/dhcpserver.c @@ -0,0 +1,279 @@ +/* Very basic LWIP & FreeRTOS-based DHCP server + + Based on RFC2131 http://www.ietf.org/rfc/rfc2131.txt + ... although not fully RFC compliant yet. + */ +#include + +#include +#include + +#include +#include +#include +#include + +#include "dhcpserver.h" + +typedef struct { + uint8_t hwaddr[NETIF_MAX_HWADDR_LEN]; + uint32_t expires; +} dhcp_lease_t; + +typedef struct { + struct netconn *nc; + dhcp_lease_t leases[DHCPSERVER_MAXCLIENTS]; + struct netif *server_if; +} server_state_t; + +/* Handlers for various kinds of incoming DHCP messages */ +static void handle_dhcp_discover(server_state_t *state, struct dhcp_msg *received); +static void handle_dhcp_request(server_state_t *state, struct dhcp_msg *dhcpmsg); + +/* Utility functions */ +static uint8_t *find_dhcp_option(struct dhcp_msg *msg, uint8_t option_num, uint8_t min_length, uint8_t *length); +static uint8_t *add_dhcp_option_byte(uint8_t *opt, uint8_t type, uint8_t value); +static uint8_t *add_dhcp_option_bytes(uint8_t *opt, uint8_t type, void *value, uint8_t len); +static dhcp_lease_t *find_lease_slot(dhcp_lease_t *leases, uint8_t *hwaddr); + +static void dhcpserver_task(void *pxParameter) +{ + server_state_t state = { + /* TODO: allow server interface to be specified as argument to dhcpserver_start() */ + .server_if = netif_list, + }; + + state.nc = netconn_new (NETCONN_UDP); + if(!state.nc) { + printf("OTA TFTP: Failed to allocate socket.\r\n"); + return; + } + + /* Seems in LWIP we need to bind to IP_ADDR_ANY to receive broadcasts. + + No way I can find to either bind only on a particular interface (server_if), + or to filter incoming broadcasts to only accept those on a single interface, + when the REQUEST arrives the from address is 0.0.0.0 and to is 255.255.255.255, + and the pbuf doesn't know about the interface it came in on... :/ + */ + netconn_bind(state.nc, IP_ADDR_ANY, DHCP_SERVER_PORT); + + while(1) + { + struct netbuf *netbuf; + struct dhcp_msg received = { 0 }; + err_t err = netconn_recv(state.nc, &netbuf); + if(err != ERR_OK) { + printf("DHCP Server Error: Failed to receive DHCP packet. err=%d\r\n", err); + continue; + } + + ip_addr_t received_ip; + u16_t port; + netconn_addr(state.nc, &received_ip, &port); + + if(netbuf_len(netbuf) < offsetof(struct dhcp_msg, options)) { + /* too short to be a valid DHCP client message */ + netbuf_delete(netbuf); + continue; + } + if(netbuf_len(netbuf) >= sizeof(struct dhcp_msg)) { + printf("DHCP Server Warning: Client sent more options than we know how to parse. len=%d\r\n", netbuf_len(netbuf)); + } + + //netconn_connect(nc, netbuf_fromaddr(netbuf), netbuf_fromport(netbuf)); + netbuf_copy(netbuf, &received, sizeof(struct dhcp_msg)); + netbuf_delete(netbuf); + + uint8_t *message_type = find_dhcp_option(&received, DHCP_OPTION_MESSAGE_TYPE, + DHCP_OPTION_MESSAGE_TYPE_LEN, NULL); + if(!message_type) { + printf("DHCP Server Error: No message type field found"); + continue; + } + switch(*message_type) { + case DHCP_DISCOVER: + handle_dhcp_discover(&state, &received); + break; + case DHCP_REQUEST: + handle_dhcp_request(&state, &received); + break; + default: + printf("DHCP Server Error: Unsupported message type %d\r\n", *message_type); + break; + } + } +} + +static void handle_dhcp_discover(server_state_t *state, struct dhcp_msg *dhcpmsg) +{ + if(dhcpmsg->htype != DHCP_HTYPE_ETH) + return; + if(dhcpmsg->hlen > NETIF_MAX_HWADDR_LEN) + return; + + dhcp_lease_t *freelease = find_lease_slot(state->leases, dhcpmsg->chaddr); + if(!freelease) { + printf("DHCP Server: All leases taken.\r\n"); + return; /* Nothing available, so do nothing */ + } + + /* Reuse the DISCOVER buffer for the OFFER response */ + dhcpmsg->op = DHCP_BOOTREPLY; + bzero(dhcpmsg->options, DHCP_OPTIONS_LEN); + + DHCPSERVER_FIRST_CLIENT_IP(&(dhcpmsg->yiaddr)); + ip4_addr4(&(dhcpmsg->yiaddr)) += (freelease - state->leases); + + uint8_t *opt = (uint8_t *)&dhcpmsg->options; + opt = add_dhcp_option_byte(opt, DHCP_OPTION_MESSAGE_TYPE, DHCP_OFFER); + + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SERVER_ID, &state->server_if->ip_addr, 4); + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SUBNET_MASK, &state->server_if->netmask, 4); + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_END, NULL, 0); + + printf("Sending discover response...\r\n"); + struct netbuf *netbuf = netbuf_new(); + netbuf_alloc(netbuf, sizeof(struct dhcp_msg)); + netbuf_take(netbuf, dhcpmsg, sizeof(struct dhcp_msg)); + netconn_sendto(state->nc, netbuf, IP_ADDR_BROADCAST, 68); + netbuf_delete(netbuf); +} + +static void handle_dhcp_request(server_state_t *state, struct dhcp_msg *dhcpmsg) +{ + if(dhcpmsg->htype != DHCP_HTYPE_ETH) + return; + if(dhcpmsg->hlen > NETIF_MAX_HWADDR_LEN) + return; + + ip_addr_t requested_ip; + uint8_t *requested_ip_opt = find_dhcp_option(dhcpmsg, DHCP_OPTION_REQUESTED_IP, 4, NULL); + if(requested_ip_opt) { + memcpy(&requested_ip.addr, requested_ip_opt, 4); + } else if(ip_addr_cmp(&requested_ip, IP_ADDR_ANY)) { + ip_addr_copy(requested_ip, dhcpmsg->ciaddr); + } else { + printf("DHCP Server Error: No requested IP\r\n"); + return; + } + ip_addr_t first_client_ip; + DHCPSERVER_FIRST_CLIENT_IP(&first_client_ip); + + /* Test the first 4 octets match */ + if(ip4_addr1(&requested_ip) != ip4_addr1(&first_client_ip) + || ip4_addr2(&requested_ip) != ip4_addr2(&first_client_ip) + || ip4_addr3(&requested_ip) != ip4_addr3(&first_client_ip)) { + printf("DHCP Server Error: 0x%08lx Not an allowed IP\r\n", requested_ip.addr); + return; + } + /* Test the last octet is in the MAXCLIENTS range */ + int16_t octet_offs = ip4_addr4(&requested_ip) - ip4_addr4(&first_client_ip); + if(octet_offs < 0 || octet_offs >= DHCPSERVER_MAXCLIENTS) { + printf("DHCP Server Error: Address out of range\r\n"); + return; + } + + dhcp_lease_t *requested_lease = state->leases + octet_offs; + if(requested_lease->expires != 0 && memcmp(requested_lease->hwaddr, dhcpmsg->chaddr,dhcpmsg->hlen)) + { + printf("DHCP Server Error: Lease for address already taken\r\n"); + return; + } + + memcpy(requested_lease->hwaddr, dhcpmsg->chaddr, dhcpmsg->hlen); + printf("DHCP lease addr 0x%08lx assigned to MAC %02x:%02x:%02x:%02x:%02x:%02x\r\n", requested_ip.addr, requested_lease->hwaddr[0], + requested_lease->hwaddr[1], requested_lease->hwaddr[2], requested_lease->hwaddr[3], requested_lease->hwaddr[4], + requested_lease->hwaddr[5]); + requested_lease->expires = 1; + + /* Reuse the REQUEST message as the ACK message */ + dhcpmsg->op = DHCP_BOOTREPLY; + bzero(dhcpmsg->options, DHCP_OPTIONS_LEN); + + ip_addr_copy(dhcpmsg->yiaddr, requested_ip); + + uint8_t *opt = (uint8_t *)&dhcpmsg->options; + opt = add_dhcp_option_byte(opt, DHCP_OPTION_MESSAGE_TYPE, DHCP_ACK); + uint32_t expiry = htonl(DHCPSERVER_LEASE_TIME); + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_LEASE_TIME, &expiry, 4); + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SERVER_ID, &state->server_if->ip_addr, 4); + opt = add_dhcp_option_bytes(opt, DHCP_OPTION_END, NULL, 0); + + struct netbuf *netbuf = netbuf_new(); + netbuf_alloc(netbuf, sizeof(struct dhcp_msg)); + netbuf_take(netbuf, dhcpmsg, sizeof(struct dhcp_msg)); + netconn_sendto(state->nc, netbuf, IP_ADDR_BROADCAST, 68); + netbuf_delete(netbuf); +} + +static uint8_t *find_dhcp_option(struct dhcp_msg *msg, uint8_t option_num, uint8_t min_length, uint8_t *length) +{ + uint8_t *start = (uint8_t *)&msg->options; + uint8_t *msg_end = (uint8_t *)msg + sizeof(struct dhcp_msg); + + for(uint8_t *p = start; p < msg_end-2;) { + uint8_t type = *p++; + uint8_t len = *p++; + if(type == DHCP_OPTION_END) + return NULL; + if(p+len >= msg_end) + break; /* We've overrun our valid DHCP message size, or this isn't a valid option */ + if(type == option_num) { + if(len < min_length) + break; + if(length) + *length = len; + return p; /* start of actual option data */ + } + p += len; + } + return NULL; /* Not found */ +} + +static uint8_t *add_dhcp_option_byte(uint8_t *opt, uint8_t type, uint8_t value) +{ + *opt++ = type; + *opt++ = 1; + *opt++ = value; + return opt; +} + +static uint8_t *add_dhcp_option_bytes(uint8_t *opt, uint8_t type, void *value, uint8_t len) +{ + *opt++ = type; + if(len) { + *opt++ = len; + memcpy(opt, value, len); + } + return opt+len; +} + +/* Find a free DHCP lease, or a lease already assigned to 'hwaddr' */ +static dhcp_lease_t *find_lease_slot(dhcp_lease_t *leases, uint8_t *hwaddr) +{ + dhcp_lease_t *empty_lease = NULL; + for(int i = 0; i < DHCPSERVER_MAXCLIENTS; i++) { + if(leases->expires == 0 && !empty_lease) + empty_lease = &leases[i]; + else if (memcmp(hwaddr, leases[i].hwaddr, 6) == 0) + return &leases[i]; + + } + return empty_lease; +} + +static xTaskHandle dhcpserver_task_handle; + +void dhcpserver_start(void) +{ + xTaskCreate(dhcpserver_task, (signed char *)"DHCPServer", 768, NULL, 8, &dhcpserver_task_handle); +} + +void dhcpserver_stop(void) +{ + if(dhcpserver_task_handle) { + vTaskDelete(dhcpserver_task_handle); + dhcpserver_task_handle = NULL; + } +} diff --git a/extras/dhcpserver/include/dhcpserver.h b/extras/dhcpserver/include/dhcpserver.h new file mode 100644 index 0000000..5219b49 --- /dev/null +++ b/extras/dhcpserver/include/dhcpserver.h @@ -0,0 +1,34 @@ +/* Very basic LWIP & FreeRTOS-based DHCP server + + */ +#ifndef _DHCPSERVER_H +#define _DHCPSERVER_H + +#ifndef DHCPSERVER_MAXCLIENTS +#define DHCPSERVER_MAXCLIENTS 4 +#endif + +/* First client IP to hand out. + + IP assignment routine is very simple - Fourth octet in IP will be incremented + from this value to (value+DHCPSERVER_MAXCLIENTS-1). +*/ +#ifndef DHCPSERVER_FIRST_CLIENT_IP +#define DHCPSERVER_FIRST_CLIENT_IP(DST) IP4_ADDR(DST, 192, 168, 3, 5) +#endif + +#ifndef DHCPSERVER_LEASE_TIME +#define DHCPSERVER_LEASE_TIME 30 +#endif + +/* Start DHCP server. + + Static IP of server should already be set and network interface enabled. +*/ +void dhcpserver_start(void); + +/* Stop DHCP server. + */ +void dhcpserver_stop(void); + +#endif