From 5e7dd2af1c3be444e425033a60e2f69636c76537 Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Fri, 6 Feb 2026 21:34:51 +0100 Subject: [PATCH] Added DHCP server. Robot succefully connected to controller ! --- program/CMakeLists.txt | 1 + program/src/controller.c | 9 +- program/src/headers/controller.h | 5 + program/src/wifi/dhcp_server.c | 290 +++++++++++++++++++++++ program/src/wifi/headers/dhcp_server.h | 24 ++ program/src/wifi/headers/udp_server.h | 7 +- program/src/wifi/headers/wifi_operator.h | 2 +- program/src/wifi/udp_server.c | 35 ++- program/src/wifi/wifi_operator.c | 2 +- 9 files changed, 360 insertions(+), 15 deletions(-) create mode 100644 program/src/wifi/dhcp_server.c create mode 100644 program/src/wifi/headers/dhcp_server.h diff --git a/program/CMakeLists.txt b/program/CMakeLists.txt index 407daad..8b50421 100644 --- a/program/CMakeLists.txt +++ b/program/CMakeLists.txt @@ -19,6 +19,7 @@ add_executable(controller src/controller.c src/wifi/wifi_operator.c src/wifi/udp_server.c + src/wifi/dhcp_server.c ) target_include_directories(controller PRIVATE diff --git a/program/src/controller.c b/program/src/controller.c index 351509d..a829c2f 100644 --- a/program/src/controller.c +++ b/program/src/controller.c @@ -18,14 +18,17 @@ void controller_init(void) cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true); - if(init_wifi_operator()) + if(wifi_operator_init()) + controller.is_running = false; + + if(dhcp_server_init()) controller.is_running = false; if(udp_server_init()) controller.is_running = false; // Initialisation ended - for(uint i = 0, led_state = true; i < 5; i++) + for(uint8_t i = 0, led_state = true; i < 5; i++) { cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state); @@ -62,6 +65,8 @@ void controller_handle_inputs_outputs(void) update_time(); + udp_server_send(); + tight_loop_contents(); } diff --git a/program/src/headers/controller.h b/program/src/headers/controller.h index cba8078..b2d4d4c 100644 --- a/program/src/headers/controller.h +++ b/program/src/headers/controller.h @@ -2,8 +2,13 @@ #define ROBOT_H #include +#include "wifi/headers/dhcp_server.h" +#include "wifi/headers/udp_server.h" typedef struct controller_t { + dhcp_server_t dhcp_server; + udp_server_t udp_server; + bool is_running; float delta_time_ms; } controller_t; diff --git a/program/src/wifi/dhcp_server.c b/program/src/wifi/dhcp_server.c new file mode 100644 index 0000000..499aa7d --- /dev/null +++ b/program/src/wifi/dhcp_server.c @@ -0,0 +1,290 @@ +#include "headers/dhcp_server.h" + +#include +#include +#include +#include +#include +#include "headers/controller.h" + +#define DHCPDISCOVER (1) +#define DHCPOFFER (2) +#define DHCPREQUEST (3) +#define DHCPDECLINE (4) +#define DHCPACK (5) +#define DHCPNACK (6) +#define DHCPRELEASE (7) +#define DHCPINFORM (8) + +#define DHCP_OPT_PAD (0) +#define DHCP_OPT_SUBNET_MASK (1) +#define DHCP_OPT_ROUTER (3) +#define DHCP_OPT_DNS (6) +#define DHCP_OPT_HOST_NAME (12) +#define DHCP_OPT_REQUESTED_IP (50) +#define DHCP_OPT_IP_LEASE_TIME (51) +#define DHCP_OPT_MSG_TYPE (53) +#define DHCP_OPT_SERVER_ID (54) +#define DHCP_OPT_PARAM_REQUEST_LIST (55) +#define DHCP_OPT_MAX_MSG_SIZE (57) +#define DHCP_OPT_VENDOR_CLASS_ID (60) +#define DHCP_OPT_CLIENT_ID (61) +#define DHCP_OPT_END (255) + +#define PORT_DHCP_SERVER (67) +#define PORT_DHCP_CLIENT (68) + +#define DEFAULT_LEASE_TIME_S (24 * 60 * 60) // in seconds + +#define MAC_LEN (6) +#define MAKE_IP4(a, b, c, d) ((a) << 24 | (b) << 16 | (c) << 8 | (d)) + +#define DHCP_SERVER_IP PP_HTONL(CYW43_DEFAULT_IP_AP_ADDRESS) +#define DHCP_SERVER_MASK PP_HTONL(CYW43_DEFAULT_IP_MASK) + +typedef struct { + uint8_t op; // message opcode + uint8_t htype; // hardware address type + uint8_t hlen; // hardware address length + uint8_t hops; + uint32_t xid; // transaction id, chosen by client + uint16_t secs; // client seconds elapsed + uint16_t flags; + uint8_t ciaddr[4]; // client IP address + uint8_t yiaddr[4]; // your IP address + uint8_t siaddr[4]; // next server IP address + uint8_t giaddr[4]; // relay agent IP address + uint8_t chaddr[16]; // client hardware address + uint8_t sname[64]; // server host name + uint8_t file[128]; // boot file name + uint8_t options[312]; // optional parameters, variable, starts with magic +} dhcp_msg_t; + +static int dhcp_socket_sendto(struct udp_pcb **pcb, struct netif *nif, const void *buf, size_t len, uint32_t ip, uint16_t port) +{ + if(len > 0xffff) + { + len = 0xffff; + } + + struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM); + if(p == NULL) + { + return -ENOMEM; + } + + memcpy(p->payload, buf, len); + + ip_addr_t dest; + IP4_ADDR(ip_2_ip4(&dest), ip >> 24 & 0xff, ip >> 16 & 0xff, ip >> 8 & 0xff, ip & 0xff); + + err_t err; + + if(nif != NULL) + { + err = udp_sendto_if(*pcb, p, &dest, port, nif); + } + else + { + err = udp_sendto(*pcb, p, &dest, port); + } + + pbuf_free(p); + + if(err != ERR_OK) + { + return err; + } + + return len; +} + +static uint8_t *opt_find(uint8_t *opt, uint8_t cmd) +{ + for (int i = 0; i < 308 && opt[i] != DHCP_OPT_END;) + { + if (opt[i] == cmd) + { + return &opt[i]; + } + + i += 2 + opt[i + 1]; + } + + return NULL; +} + +static void opt_write_n(uint8_t **opt, uint8_t cmd, size_t n, const void *data) +{ + uint8_t *o = *opt; + *o++ = cmd; + *o++ = n; + memcpy(o, data, n); + *opt = o + n; +} + +static void opt_write_u8(uint8_t **opt, uint8_t cmd, uint8_t val) +{ + uint8_t *o = *opt; + *o++ = cmd; + *o++ = 1; + *o++ = val; + *opt = o; +} + +static void opt_write_u32(uint8_t **opt, uint8_t cmd, uint32_t val) +{ + uint8_t *o = *opt; + *o++ = cmd; + *o++ = 4; + *o++ = val >> 24; + *o++ = val >> 16; + *o++ = val >> 8; + *o++ = val; + *opt = o; +} + +static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct pbuf *p, const ip_addr_t *src_addr __unused, u16_t src_port __unused) +{ + dhcp_server_t *d = arg; + + // This is around 548 bytes + dhcp_msg_t dhcp_msg; + + #define DHCP_MIN_SIZE (240 + 3) + if (p->tot_len < DHCP_MIN_SIZE) { + goto ignore_request; + } + + size_t len = pbuf_copy_partial(p, &dhcp_msg, sizeof(dhcp_msg), 0); + if (len < DHCP_MIN_SIZE) { + goto ignore_request; + } + + dhcp_msg.op = DHCPOFFER; + memcpy(&dhcp_msg.yiaddr, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 4); + + uint8_t *opt = (uint8_t *)&dhcp_msg.options; + opt += 4; // assume magic cookie: 99, 130, 83, 99 + + uint8_t *msgtype = opt_find(opt, DHCP_OPT_MSG_TYPE); + if (msgtype == NULL) { + // A DHCP package without MSG_TYPE? + goto ignore_request; + } + + switch (msgtype[2]) { + case DHCPDISCOVER: { + int yi = DHCPS_MAX_IP; + for (int i = 0; i < DHCPS_MAX_IP; ++i) { + if (memcmp(d->lease[i].mac, dhcp_msg.chaddr, MAC_LEN) == 0) { + // MAC match, use this IP address + yi = i; + break; + } + if (yi == DHCPS_MAX_IP) { + // Look for a free IP address + if (memcmp(d->lease[i].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) { + // IP available + yi = i; + } + uint32_t expiry = d->lease[i].expiry << 16 | 0xffff; + if ((int32_t)(expiry - cyw43_hal_ticks_ms()) < 0) { + // IP expired, reuse it + memset(d->lease[i].mac, 0, MAC_LEN); + yi = i; + } + } + } + if (yi == DHCPS_MAX_IP) { + // No more IP addresses left + goto ignore_request; + } + dhcp_msg.yiaddr[3] = DHCPS_BASE_IP + yi; + opt_write_u8(&opt, DHCP_OPT_MSG_TYPE, DHCPOFFER); + break; + } + + case DHCPREQUEST: { + uint8_t *o = opt_find(opt, DHCP_OPT_REQUESTED_IP); + if (o == NULL) { + // Should be NACK + goto ignore_request; + } + if (memcmp(o + 2, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 3) != 0) { + // Should be NACK + goto ignore_request; + } + uint8_t yi = o[5] - DHCPS_BASE_IP; + if (yi >= DHCPS_MAX_IP) { + // Should be NACK + goto ignore_request; + } + if (memcmp(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN) == 0) { + // MAC match, ok to use this IP address + } else if (memcmp(d->lease[yi].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) { + // IP unused, ok to use this IP address + memcpy(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN); + } else { + // IP already in use + // Should be NACK + goto ignore_request; + } + d->lease[yi].expiry = (cyw43_hal_ticks_ms() + DEFAULT_LEASE_TIME_S * 1000) >> 16; + dhcp_msg.yiaddr[3] = DHCPS_BASE_IP + yi; + opt_write_u8(&opt, DHCP_OPT_MSG_TYPE, DHCPACK); + printf("DHCPS: client connected: MAC=%02x:%02x:%02x:%02x:%02x:%02x IP=%u.%u.%u.%u\n", + dhcp_msg.chaddr[0], dhcp_msg.chaddr[1], dhcp_msg.chaddr[2], dhcp_msg.chaddr[3], dhcp_msg.chaddr[4], dhcp_msg.chaddr[5], + dhcp_msg.yiaddr[0], dhcp_msg.yiaddr[1], dhcp_msg.yiaddr[2], dhcp_msg.yiaddr[3]); + break; + } + + default: + goto ignore_request; + } + + opt_write_n(&opt, DHCP_OPT_SERVER_ID, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); + opt_write_n(&opt, DHCP_OPT_SUBNET_MASK, 4, &ip4_addr_get_u32(ip_2_ip4(&d->nm))); + opt_write_n(&opt, DHCP_OPT_ROUTER, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // aka gateway; can have multiple addresses + opt_write_n(&opt, DHCP_OPT_DNS, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // this server is the dns + opt_write_u32(&opt, DHCP_OPT_IP_LEASE_TIME, DEFAULT_LEASE_TIME_S); + *opt++ = DHCP_OPT_END; + struct netif *nif = ip_current_input_netif(); + dhcp_socket_sendto(&d->pcb, nif, &dhcp_msg, opt - (uint8_t *)&dhcp_msg, 0xffffffff, PORT_DHCP_CLIENT); + +ignore_request: + pbuf_free(p); +} + +int dhcp_server_init(void) +{ + controller.dhcp_server.ip.addr = DHCP_SERVER_IP; + controller.dhcp_server.nm.addr = DHCP_SERVER_MASK; + memset(controller.dhcp_server.lease, 0, sizeof(controller.dhcp_server.lease)); + + controller.dhcp_server.pcb = udp_new(); + if (controller.dhcp_server.pcb == NULL) { + return -ENOMEM; + } + + // Register callback + udp_recv(controller.dhcp_server.pcb, dhcp_server_process, (void *)&controller.dhcp_server); + + if(udp_bind(controller.dhcp_server.pcb, IP_ANY_TYPE, PORT_DHCP_SERVER)) + { + return -1; + } + + puts("DHCP server started"); + + return 0; +} + +void dhcp_server_deinit(void) +{ + if (controller.dhcp_server.pcb != NULL) + { + udp_remove(controller.dhcp_server.pcb); + controller.dhcp_server.pcb = NULL; + } +} diff --git a/program/src/wifi/headers/dhcp_server.h b/program/src/wifi/headers/dhcp_server.h new file mode 100644 index 0000000..267764d --- /dev/null +++ b/program/src/wifi/headers/dhcp_server.h @@ -0,0 +1,24 @@ +#ifndef DHCP_SERVER_H +#define DHCP_SERVER_H + +#include "lwip/ip_addr.h" + +#define DHCPS_BASE_IP (16) +#define DHCPS_MAX_IP (8) + +typedef struct _dhcp_server_lease_t { + uint8_t mac[6]; + uint16_t expiry; +} dhcp_server_lease_t; + +typedef struct _dhcp_server_t { + ip_addr_t ip; + ip_addr_t nm; + dhcp_server_lease_t lease[DHCPS_MAX_IP]; + struct udp_pcb *pcb; +} dhcp_server_t; + +int dhcp_server_init(void); +void dhcp_server_deinit(void); + +#endif // DHCP_SERVER_H diff --git a/program/src/wifi/headers/udp_server.h b/program/src/wifi/headers/udp_server.h index 6028e4b..d4a8cbe 100644 --- a/program/src/wifi/headers/udp_server.h +++ b/program/src/wifi/headers/udp_server.h @@ -1,10 +1,11 @@ -#ifndef UDP_CLIENT_H -#define UDP_CLIENT_H +#ifndef UDP_SERVER_H +#define UDP_SERVER_H #include #include #define UDP_SERVER_PORT 4242 +#define UDP_CLIENT_PORT 4243 typedef struct udp_server_t { struct udp_pcb *pcb; @@ -17,4 +18,4 @@ void udp_server_send(void); // Exit udp client void udp_server_deinit(void); -#endif // UDP_CLIENT_H \ No newline at end of file +#endif // UDP_SERVER_H \ No newline at end of file diff --git a/program/src/wifi/headers/wifi_operator.h b/program/src/wifi/headers/wifi_operator.h index 4c10558..c377e7b 100644 --- a/program/src/wifi/headers/wifi_operator.h +++ b/program/src/wifi/headers/wifi_operator.h @@ -5,6 +5,6 @@ #define WIFI_OPERATOR_PASSWORD "x4ptSLpPuJFcpzbLEhDoZ5J7dz" #define WIFI_OPERATOR_CHANNEL 3 -int init_wifi_operator(void); +int wifi_operator_init(void); #endif // WIFI_OPERATOR_H \ No newline at end of file diff --git a/program/src/wifi/udp_server.c b/program/src/wifi/udp_server.c index f5385a5..0631456 100644 --- a/program/src/wifi/udp_server.c +++ b/program/src/wifi/udp_server.c @@ -1,39 +1,58 @@ #include "headers/udp_server.h" #include +#include "headers/controller.h" -udp_server_t udp_server; +#define MSG_LEN 128 int udp_server_init(void) { - udp_server.pcb = udp_new(); - if(udp_server.pcb == NULL) + controller.udp_server.pcb = udp_new(); + if(controller.udp_server.pcb == NULL) { puts("Error creating UDP server"); return -1; } - if(udp_bind(udp_server.pcb, IP_ADDR_ANY, UDP_SERVER_PORT)) + if(udp_bind(controller.udp_server.pcb, IP_ADDR_ANY, UDP_SERVER_PORT)) { printf("Error bind UDP server"); return -1; } - puts("UDP client started"); + puts("UDP server started"); return 0; } void udp_server_send(void) { + static float elapsed_time = 0.0f; + elapsed_time += controller.delta_time_ms; + static uint8_t counter = 0; + + if(elapsed_time >= 1000.0f) + { + elapsed_time = 0.0f; + + struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, MSG_LEN, PBUF_RAM); + uint8_t *payload = (uint8_t *)p->payload; + payload[0] = counter; + + udp_sendto(controller.udp_server.pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT); + + pbuf_free(p); + + counter++; + } } void udp_server_deinit(void) { - if(udp_server.pcb) + if(controller.udp_server.pcb) { - udp_remove(udp_server.pcb); - udp_server.pcb = NULL; + udp_remove(controller.udp_server.pcb); + controller.udp_server.pcb = NULL; } } diff --git a/program/src/wifi/wifi_operator.c b/program/src/wifi/wifi_operator.c index d3052b3..04cee1c 100644 --- a/program/src/wifi/wifi_operator.c +++ b/program/src/wifi/wifi_operator.c @@ -5,7 +5,7 @@ #include #include -int init_wifi_operator(void) +int wifi_operator_init(void) { if(cyw43_wifi_pm(&cyw43_state, CYW43_NO_POWERSAVE_MODE)) {