Added DHCP server. Robot succefully connected to controller !
This commit is contained in:
parent
2c4a35e154
commit
5e7dd2af1c
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,8 +2,13 @@
|
|||
#define ROBOT_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,290 @@
|
|||
#include "headers/dhcp_server.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <cyw43_config.h>
|
||||
#include <lwip/udp.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -1,10 +1,11 @@
|
|||
#ifndef UDP_CLIENT_H
|
||||
#define UDP_CLIENT_H
|
||||
#ifndef UDP_SERVER_H
|
||||
#define UDP_SERVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <lwip/udp.h>
|
||||
|
||||
#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
|
||||
#endif // UDP_SERVER_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
|
||||
|
|
@ -1,39 +1,58 @@
|
|||
#include "headers/udp_server.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
#include <lwip/netif.h>
|
||||
#include <lwip/ip4_addr.h>
|
||||
|
||||
int init_wifi_operator(void)
|
||||
int wifi_operator_init(void)
|
||||
{
|
||||
if(cyw43_wifi_pm(&cyw43_state, CYW43_NO_POWERSAVE_MODE))
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue