Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Networking: split ChibiOS code into its own backend

Browse Source
This commit is contained in:
Andrew Tridgell 2023-08-05 16:46:13 +10:00
parent 5705c68954
commit fd70f5c7d7
5 changed files with 306 additions and 237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
libraries/AP_Networking/AP_Networking.cpp
View File

@ -4,34 +4,31 @@
#if AP_NETWORKING_ENABLED #if AP_NETWORKING_ENABLED
#include "AP_Networking.h" #include "AP_Networking.h"
#include "AP_Math/definitions.h" #include "AP_Networking_ChibiOS.h"
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <hal_mii.h> #include <hal_mii.h>
#include <lwip/sockets.h> #include <lwip/sockets.h>
#else #else
#include <arpa/inet.h> #include <arpa/inet.h>
#include <sys/socket.h>
#endif #endif
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_Networking::var_info[] = { const AP_Param::GroupInfo AP_Networking::var_info[] = {
// @Param: ENABLED // @Param: ENABLED
// @DisplayName: Networking Enable // @DisplayName: Networking Enable
// @Description: Networking Enable // @Description: Networking Enable
// @Values: 0:Disable,1:Enable // @Values: 0:Disable,1:Enable
// @RebootRequired: True // @RebootRequired: True
// @User: Advanced // @User: Advanced
AP_GROUPINFO_FLAGS("ENABLED", 0, AP_Networking, _param.enabled, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("ENABLED", 1, AP_Networking, param.enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Group: IPADDR // @Group: IPADDR
// @Path: AP_Networking_address.cpp // @Path: AP_Networking_address.cpp
AP_SUBGROUPINFO(_param.ipaddr, "IPADDR", 1, AP_Networking, AP_Networking_IPV4), AP_SUBGROUPINFO(param.ipaddr, "IPADDR", 2, AP_Networking, AP_Networking_IPV4),
// @Param: NETMASK // @Param: NETMASK
// @DisplayName: IP Subnet mask // @DisplayName: IP Subnet mask
@ -39,7 +36,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
// @Range: 0 32 // @Range: 0 32
// @RebootRequired: True // @RebootRequired: True
// @User: Advanced // @User: Advanced
AP_GROUPINFO("NETMASK", 2, AP_Networking, _param.netmask, AP_NETWORKING_DEFAULT_NETMASK), AP_GROUPINFO("NETMASK", 3, AP_Networking, param.netmask, AP_NETWORKING_DEFAULT_NETMASK),
// @Param: DHCP // @Param: DHCP
// @DisplayName: DHCP client // @DisplayName: DHCP client
@ -47,101 +44,39 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = {
// @Values: 0:Disable, 1:Enable // @Values: 0:Disable, 1:Enable
// @RebootRequired: True // @RebootRequired: True
// @User: Advanced // @User: Advanced
AP_GROUPINFO("DHCP", 3, AP_Networking, _param.dhcp, AP_NETWORKING_DEFAULT_DHCP_ENABLE), AP_GROUPINFO("DHCP", 4, AP_Networking, param.dhcp, AP_NETWORKING_DEFAULT_DHCP_ENABLE),
// @Group: GWADDR // @Group: GWADDR
// @Path: AP_Networking_address.cpp // @Path: AP_Networking_address.cpp
AP_SUBGROUPINFO(_param.gwaddr, "GWADDR", 4, AP_Networking, AP_Networking_IPV4), AP_SUBGROUPINFO(param.gwaddr, "GWADDR", 5, AP_Networking, AP_Networking_IPV4),
// @Group: MACADDR // @Group: MACADDR
// @Path: AP_Networking_address.cpp // @Path: AP_Networking_address.cpp
AP_SUBGROUPINFO(_param.macaddr, "MACADDR", 5, AP_Networking, AP_Networking_MAC), AP_SUBGROUPINFO(param.macaddr, "MACADDR", 6, AP_Networking, AP_Networking_MAC),
AP_GROUPEND AP_GROUPEND
}; };
/*
constructor
*/
AP_Networking::AP_Networking(void) AP_Networking::AP_Networking(void)
{ {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) { if (singleton != nullptr) {
AP_HAL::panic("AP_Networking must be singleton"); AP_HAL::panic("AP_Networking must be singleton");
} }
#endif #endif
singleton = this;
_singleton = this;
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
#if defined(STM32_ETH_BUFFERS_EXTERN) && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) /*
stm32_eth_rx_descriptor_t *__eth_rd; initialise networking subsystem
stm32_eth_tx_descriptor_t *__eth_td; */
uint32_t *__eth_rb[STM32_MAC_RECEIVE_BUFFERS];
uint32_t *__eth_tb[STM32_MAC_TRANSMIT_BUFFERS];
static bool allocate_buffers()
{
#define AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE ((((STM32_MAC_BUFFERS_SIZE - 1) | 3) + 1) / 4) // typically == 381
// check total size of buffers
const uint32_t total_size = sizeof(stm32_eth_rx_descriptor_t)*STM32_MAC_RECEIVE_BUFFERS +
sizeof(stm32_eth_tx_descriptor_t)*STM32_MAC_TRANSMIT_BUFFERS +
sizeof(uint32_t)*STM32_MAC_RECEIVE_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE +
sizeof(uint32_t)*STM32_MAC_TRANSMIT_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE; // typically == 9240
// ensure that we allocate 32-bit aligned memory, and mark it non-cacheable
uint32_t size = 2;
uint8_t rasr = 0;
// find size closest to power of 2
while (size < total_size) {
size = size << 1;
rasr++;
}
void *mem = malloc_eth_safe(size);
if (mem == nullptr) {
return false;
}
// ensure our memory is aligned
// ref. Cortex-M7 peripherals PM0253, section 4.6.4 MPU region base address register
if (((uint32_t)mem) % size) {
AP_HAL::panic("Bad alignment of ETH memory");
}
// for total_size == 9240, size should be 16384 and (rasr-1) should be 13 (MPU_RASR_SIZE_16K)
const uint32_t rasr_size = MPU_RASR_SIZE(rasr-1);
// set up MPU region for buffers
mpuConfigureRegion(STM32_NOCACHE_MPU_REGION_ETH,
(uint32_t)mem,
MPU_RASR_ATTR_AP_RW_RW |
MPU_RASR_ATTR_NON_CACHEABLE |
MPU_RASR_ATTR_S |
rasr_size |
MPU_RASR_ENABLE);
mpuEnable(MPU_CTRL_PRIVDEFENA);
SCB_CleanInvalidateDCache();
// assign buffers
__eth_rd = (stm32_eth_rx_descriptor_t *)mem;
__eth_td = (stm32_eth_tx_descriptor_t *)&__eth_rd[STM32_MAC_RECEIVE_BUFFERS];
__eth_rb[0] = (uint32_t*)&__eth_td[STM32_MAC_TRANSMIT_BUFFERS];
for (uint16_t i = 1; i < STM32_MAC_RECEIVE_BUFFERS; i++) {
__eth_rb[i] = &(__eth_rb[i-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
}
__eth_tb[0] = &(__eth_rb[STM32_MAC_RECEIVE_BUFFERS-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
for (uint16_t i = 1; i < STM32_MAC_TRANSMIT_BUFFERS; i++) {
__eth_tb[i] = &(__eth_tb[i-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
}
return true;
}
#endif // STM32_ETH_BUFFERS_EXTERN
void AP_Networking::init() void AP_Networking::init()
{ {
#ifdef HAL_GPIO_ETH_ENABLE if (!param.enabled || backend != nullptr) {
hal.gpio->pinMode(HAL_GPIO_ETH_ENABLE, HAL_GPIO_OUTPUT);
hal.gpio->write(HAL_GPIO_ETH_ENABLE, _param.enabled ? 1 : 0);
#endif
if (!_param.enabled || _init.done) {
return; return;
} }
@ -151,110 +86,57 @@ void AP_Networking::init()
const bool udid_is_ok = hal.util->get_system_id_unformatted(uuid, uuid_len) && uuid_len >= 3; const bool udid_is_ok = hal.util->get_system_id_unformatted(uuid, uuid_len) && uuid_len >= 3;
if (udid_is_ok) { if (udid_is_ok) {
for (uint8_t i=0; i<3; i++) { for (uint8_t i=0; i<3; i++) {
_param.macaddr.set_default_address_byte(i, uuid[uuid_len-3+i]); param.macaddr.set_default_address_byte(i, uuid[uuid_len-3+i]);
} }
} }
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#ifdef STM32_ETH_BUFFERS_EXTERN backend = new AP_Networking_ChibiOS(*this);
if (!allocate_buffers()) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "NET: Failed to allocate buffers");
return;
}
#endif #endif
if (!macInit()) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "NET: macInit failed"); if (backend == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: backend failed");
return; return;
} }
_param.macaddr.get_address(_activeSettings.macaddr); if (!backend->init()) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: backend init failed");
#if !AP_NETWORKING_DHCP_AVAILABLE backend = nullptr;
set_dhcp_enable(false);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: DHCP Not Supported");
#endif
net_addr_mode_t addrMode;
if (get_dhcp_enabled()) {
_activeSettings.ip = 0;
_activeSettings.nm = 0;
_activeSettings.gw = 0;
addrMode = NET_ADDRESS_DHCP;
} else {
_activeSettings.ip = get_ip_param();
_activeSettings.nm = get_netmask_param();
_activeSettings.gw = get_gateway_param();
addrMode = NET_ADDRESS_STATIC;
} }
lwip_options.macaddress = _activeSettings.macaddr; announce_address_changes();
lwip_options.address = _activeSettings.ip;
lwip_options.netmask = _activeSettings.nm;
lwip_options.gateway = _activeSettings.gw;
lwip_options.addrMode = addrMode;
lwipInit(&lwip_options);
#endif
#if AP_NETWORKING_DHCP_AVAILABLE
if (get_dhcp_enabled()) {
// give DHCP a chance to get an address before we show the boot-up address
_activeSettings.announce_ms = AP_HAL::millis();
}
#endif
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"NET: Initialized"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"NET: Initialized");
_init.done = true;
} }
/*
check if we should announce changes to IP addresses
*/
void AP_Networking::announce_address_changes() void AP_Networking::announce_address_changes()
{ {
const uint32_t now_ms = AP_HAL::millis(); auto &as = backend->activeSettings;
if (now_ms - _activeSettings.announce_ms < 1000) {
// Never announce changes any faster than 1 sec
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (as.last_change_ms == 0 || as.last_change_ms == announce_ms) {
const uint32_t ip = lwipGetIp();
const uint32_t nm = lwipGetNetmask();
const uint32_t gw = lwipGetGateway();
#else
const uint32_t ip = 0;
const uint32_t nm = 0;
const uint32_t gw = 0;
#endif
if (_activeSettings.announce_at_boot_done &&
ip == _activeSettings.ip &&
nm == _activeSettings.nm &&
gw == _activeSettings.gw) {
// nothing changed and we've already printed it at least once. Nothing to do. // nothing changed and we've already printed it at least once. Nothing to do.
return; return;
} }
_activeSettings.ip = ip;
_activeSettings.nm = nm;
_activeSettings.gw = gw;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: IP %s", get_ip_active_str()); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: IP %s", get_ip_active_str());
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Mask %s", get_netmask_active_str()); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Mask %s", get_netmask_active_str());
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Gateway %s", get_gateway_active_str()); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Gateway %s", get_gateway_active_str());
if (!_activeSettings.announce_at_boot_done && ip == 0 && nm == 0 && gw == 0 && get_dhcp_enabled()) { announce_ms = as.last_change_ms;
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"NET: DHCP enabled, waiting for IP");
}
_activeSettings.announce_ms = now_ms;
_activeSettings.announce_at_boot_done = true;
} }
/*
update called at 10Hz
*/
void AP_Networking::update() void AP_Networking::update()
{ {
if (!is_healthy()) { if (!is_healthy()) {
return; return;
} }
backend->update();
announce_address_changes(); announce_address_changes();
} }
@ -263,7 +145,6 @@ uint32_t AP_Networking::convert_netmask_bitcount_to_ip(const uint32_t netmask_bi
if (netmask_bitcount > 32) { if (netmask_bitcount > 32) {
return 0; return 0;
} }
uint32_t netmask_ip = 0; uint32_t netmask_ip = 0;
for (uint32_t i=0; i<netmask_bitcount; i++) { for (uint32_t i=0; i<netmask_bitcount; i++) {
netmask_ip |= (1UL << i); netmask_ip |= (1UL << i);
@ -299,6 +180,7 @@ const char* AP_Networking::convert_ip_to_str(const uint8_t ip[4])
} }
return _str_buffer; return _str_buffer;
} }
const char* AP_Networking::convert_ip_to_str(const uint32_t ip) const char* AP_Networking::convert_ip_to_str(const uint32_t ip)
{ {
uint8_t ip_array[4]; uint8_t ip_array[4];
@ -337,39 +219,8 @@ bool AP_Networking::convert_str_to_macaddr(const char *mac_str, uint8_t addr[6])
return true; return true;
} }
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS AP_Networking *AP_Networking::singleton;
int32_t AP_Networking::send_udp(struct udp_pcb *pcb, const ip4_addr_t &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len)
{
if (!AP::network().is_healthy()) {
return ERR_IF;
}
if (pcb == nullptr) {
return ERR_ARG;
}
data_len = (data == nullptr) ? 0 : data_len;
struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, data_len, PBUF_RAM);
if (p == nullptr) {
return ERR_MEM;
}
ip_addr_t dst;
ip_addr_copy_from_ip4(dst, ip4_addr);
if (data_len > 0) {
memcpy(p->payload, data, data_len);
}
const err_t err = udp_sendto(pcb, p, &dst, port);
pbuf_free(p);
return err == ERR_OK ? data_len : err;
}
#endif // #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
AP_Networking *AP_Networking::_singleton;
namespace AP namespace AP
{ {
AP_Networking &network() AP_Networking &network()

74
libraries/AP_Networking/AP_Networking.h
View File

@ -7,17 +7,8 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "lwipthread.h"
#include "lwip/udp.h"
#include <lwip/ip_addr.h>
#else
#include <AP_Common/missing/byteswap.h>
#define IP4_ADDR_VALUE(a,b,c,d) be32toh(UINT32_VALUE(a,b,c,d))
#endif
#include "AP_Networking_address.h" #include "AP_Networking_address.h"
#include "AP_Networking_backend.h"
/* /*
Note! all uint32_t IPv4 addresses are in network byte order (big-endian) Note! all uint32_t IPv4 addresses are in network byte order (big-endian)
@ -26,6 +17,8 @@
class AP_Networking class AP_Networking
{ {
public: public:
friend class AP_Networking_ChibiOS;
AP_Networking(); AP_Networking();
/* Do not allow copies */ /* Do not allow copies */
@ -34,42 +27,42 @@ public:
// initialize the library. This should only be run once // initialize the library. This should only be run once
void init(); void init();
// update task. For most applications this should run as fast as possible. Default is 1000Hz for lowest latency // update task, called at 10Hz
void update(); void update();
static AP_Networking *get_singleton(void) static AP_Networking *get_singleton(void)
{ {
return _singleton; return singleton;
} }
// Networking interface is enabled and initialized // Networking interface is enabled and initialized
bool is_healthy() const bool is_healthy() const
{ {
return _param.enabled && _init.done; return param.enabled && backend != nullptr;
} }
// returns true if DHCP is enabled // returns true if DHCP is enabled
bool get_dhcp_enabled() const bool get_dhcp_enabled() const
{ {
return _param.dhcp; return param.dhcp;
} }
// Sets DHCP to be enabled or disabled // Sets DHCP to be enabled or disabled
void set_dhcp_enable(const bool enable) void set_dhcp_enable(const bool enable)
{ {
_param.dhcp.set(enable); param.dhcp.set(enable);
} }
// returns the 32bit value of the active IP address that is currently in use // returns the 32bit value of the active IP address that is currently in use
uint32_t get_ip_active() const uint32_t get_ip_active() const
{ {
return _activeSettings.ip; return backend?backend->activeSettings.ip:0;
} }
// returns the 32bit value of the user-parameter static IP address // returns the 32bit value of the user-parameter static IP address
uint32_t get_ip_param() const uint32_t get_ip_param() const
{ {
return _param.ipaddr.get_uint32(); return param.ipaddr.get_uint32();
} }
/* /*
@ -84,19 +77,19 @@ public:
// sets the user-parameter static IP address from a 32bit value // sets the user-parameter static IP address from a 32bit value
void set_ip_param(const uint32_t ip) void set_ip_param(const uint32_t ip)
{ {
_param.ipaddr.set_uint32(ip); param.ipaddr.set_uint32(ip);
} }
// returns the 32bit value of the active Netmask that is currently in use // returns the 32bit value of the active Netmask that is currently in use
uint32_t get_netmask_active() const uint32_t get_netmask_active() const
{ {
return _activeSettings.nm; return backend?backend->activeSettings.nm:0;
} }
// returns the 32bit value of the of the user-parameter static Netmask // returns the 32bit value of the of the user-parameter static Netmask
uint32_t get_netmask_param() const uint32_t get_netmask_param() const
{ {
return convert_netmask_bitcount_to_ip(_param.netmask.get()); return convert_netmask_bitcount_to_ip(param.netmask.get());
} }
// returns a null terminated string of the active Netmask address. Example: "192.168.12.13" // returns a null terminated string of the active Netmask address. Example: "192.168.12.13"
@ -117,17 +110,17 @@ public:
void set_netmask_param(const uint32_t nm) void set_netmask_param(const uint32_t nm)
{ {
_param.netmask.set(convert_netmask_ip_to_bitcount(nm)); param.netmask.set(convert_netmask_ip_to_bitcount(nm));
} }
uint32_t get_gateway_active() const uint32_t get_gateway_active() const
{ {
return _activeSettings.gw; return backend?backend->activeSettings.gw:0;
} }
uint32_t get_gateway_param() const uint32_t get_gateway_param() const
{ {
return _param.gwaddr.get_uint32(); return param.gwaddr.get_uint32();
} }
const char *get_gateway_active_str() const char *get_gateway_active_str()
@ -147,10 +140,9 @@ public:
void set_gateway_param(const uint32_t gw) void set_gateway_param(const uint32_t gw)
{ {
_param.gwaddr.set_uint32(gw); param.gwaddr.set_uint32(gw);
} }
// helper functions to convert between 32bit IP addresses and null terminated strings and back // helper functions to convert between 32bit IP addresses and null terminated strings and back
static uint32_t convert_str_to_ip(const char* ip_str); static uint32_t convert_str_to_ip(const char* ip_str);
static const char* convert_ip_to_str(const uint8_t ip[4]); static const char* convert_ip_to_str(const uint8_t ip[4]);
@ -163,24 +155,13 @@ public:
static uint32_t convert_netmask_bitcount_to_ip(const uint32_t netmask_bitcount); static uint32_t convert_netmask_bitcount_to_ip(const uint32_t netmask_bitcount);
static uint8_t convert_netmask_ip_to_bitcount(const uint32_t netmask_ip); static uint8_t convert_netmask_ip_to_bitcount(const uint32_t netmask_ip);
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static int32_t send_udp(struct udp_pcb *pcb, const ip4_addr_t &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len);
#endif
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
static AP_Networking *_singleton; static AP_Networking *singleton;
uint8_t _num_instances; // number of feature instances
void announce_address_changes(); void announce_address_changes();
struct {
bool done;
} _init;
struct { struct {
AP_Networking_IPV4 ipaddr{AP_NETWORKING_DEFAULT_STATIC_IP_ADDR}; AP_Networking_IPV4 ipaddr{AP_NETWORKING_DEFAULT_STATIC_IP_ADDR};
AP_Int8 netmask; // bits to mask. example: (16 == 255.255.0.0) and (24 == 255.255.255.0) AP_Int8 netmask; // bits to mask. example: (16 == 255.255.0.0) and (24 == 255.255.255.0)
@ -190,24 +171,19 @@ private:
AP_Networking_MAC macaddr{AP_NETWORKING_DEFAULT_MAC_ADDR}; AP_Networking_MAC macaddr{AP_NETWORKING_DEFAULT_MAC_ADDR};
AP_Int8 enabled; AP_Int8 enabled;
AP_Int32 options; AP_Int32 options;
} _param; } param;
struct { AP_Networking_backend *backend;
uint32_t ip;
uint32_t nm;
uint32_t gw;
uint32_t announce_ms;
uint8_t macaddr[6];
bool announce_at_boot_done;
} _activeSettings;
struct lwipthread_opts lwip_options;
HAL_Semaphore _sem; HAL_Semaphore sem;
private:
uint32_t announce_ms;
}; };
namespace AP namespace AP
{ {
AP_Networking &network(); AP_Networking &network();
}; };
#endif // AP_NETWORKING_ENABLED #endif // AP_NETWORKING_ENABLED

178
libraries/AP_Networking/AP_Networking_ChibiOS.cpp Normal file
View File

@ -0,0 +1,178 @@
#include "AP_Networking_Config.h"
#if AP_NETWORKING_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "AP_Networking.h"
#include "AP_Networking_ChibiOS.h"
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include <lwipthread.h>
#include <lwip/udp.h>
#include <lwip/ip_addr.h>
extern const AP_HAL::HAL& hal;
#ifndef STM32_ETH_BUFFERS_EXTERN
#error "Must use external ethernet buffers"
#endif
/*
these are referenced as globals inside lwip
*/
stm32_eth_rx_descriptor_t *__eth_rd;
stm32_eth_tx_descriptor_t *__eth_td;
uint32_t *__eth_rb[STM32_MAC_RECEIVE_BUFFERS];
uint32_t *__eth_tb[STM32_MAC_TRANSMIT_BUFFERS];
/*
allocate buffers for LWIP
*/
bool AP_Networking_ChibiOS::allocate_buffers()
{
#define AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE ((((STM32_MAC_BUFFERS_SIZE - 1) | 3) + 1) / 4) // typically == 381
// check total size of buffers
const uint32_t total_size = sizeof(stm32_eth_rx_descriptor_t)*STM32_MAC_RECEIVE_BUFFERS +
sizeof(stm32_eth_tx_descriptor_t)*STM32_MAC_TRANSMIT_BUFFERS +
sizeof(uint32_t)*STM32_MAC_RECEIVE_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE +
sizeof(uint32_t)*STM32_MAC_TRANSMIT_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE; // typically == 9240
// ensure that we allocate 32-bit aligned memory, and mark it non-cacheable
uint32_t size = 2;
uint8_t rasr = 0;
// find size closest to power of 2
while (size < total_size) {
size = size << 1;
rasr++;
}
void *mem = malloc_eth_safe(size);
if (mem == nullptr) {
return false;
}
// ensure our memory is aligned
// ref. Cortex-M7 peripherals PM0253, section 4.6.4 MPU region base address register
if (((uint32_t)mem) % size) {
AP_HAL::panic("Bad alignment of ETH memory");
}
// for total_size == 9240, size should be 16384 and (rasr-1) should be 13 (MPU_RASR_SIZE_16K)
const uint32_t rasr_size = MPU_RASR_SIZE(rasr-1);
// set up MPU region for buffers
mpuConfigureRegion(STM32_NOCACHE_MPU_REGION_ETH,
(uint32_t)mem,
MPU_RASR_ATTR_AP_RW_RW |
MPU_RASR_ATTR_NON_CACHEABLE |
MPU_RASR_ATTR_S |
rasr_size |
MPU_RASR_ENABLE);
mpuEnable(MPU_CTRL_PRIVDEFENA);
SCB_CleanInvalidateDCache();
// assign buffers
__eth_rd = (stm32_eth_rx_descriptor_t *)mem;
__eth_td = (stm32_eth_tx_descriptor_t *)&__eth_rd[STM32_MAC_RECEIVE_BUFFERS];
__eth_rb[0] = (uint32_t*)&__eth_td[STM32_MAC_TRANSMIT_BUFFERS];
for (uint16_t i = 1; i < STM32_MAC_RECEIVE_BUFFERS; i++) {
__eth_rb[i] = &(__eth_rb[i-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
}
__eth_tb[0] = &(__eth_rb[STM32_MAC_RECEIVE_BUFFERS-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
for (uint16_t i = 1; i < STM32_MAC_TRANSMIT_BUFFERS; i++) {
__eth_tb[i] = &(__eth_tb[i-1][AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE]);
}
return true;
}
/*
initialise ChibiOS network backend using LWIP
*/
bool AP_Networking_ChibiOS::init()
{
#ifdef HAL_GPIO_ETH_ENABLE
hal.gpio->pinMode(HAL_GPIO_ETH_ENABLE, HAL_GPIO_OUTPUT);
hal.gpio->write(HAL_GPIO_ETH_ENABLE, _param.enabled ? 1 : 0);
#endif
if (!allocate_buffers()) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "NET: Failed to allocate buffers");
return false;
}
if (!macInit()) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "NET: macInit failed");
return false;
}
#if !AP_NETWORKING_DHCP_AVAILABLE
frontend.set_dhcp_enable(false);
#endif
lwip_options = new lwipthread_opts;
if (frontend.get_dhcp_enabled()) {
lwip_options->addrMode = NET_ADDRESS_DHCP;
} else {
lwip_options->addrMode = NET_ADDRESS_STATIC;
lwip_options->address = frontend.get_ip_param();
lwip_options->netmask = frontend.get_netmask_param();
lwip_options->gateway = frontend.get_gateway_param();
}
frontend.param.macaddr.get_address(macaddr);
lwip_options->macaddress = macaddr;
lwipInit(lwip_options);
return true;
}
/*
update called at 10Hz
*/
void AP_Networking_ChibiOS::update()
{
const uint32_t ip = lwipGetIp();
const uint32_t nm = lwipGetNetmask();
const uint32_t gw = lwipGetGateway();
if (ip != activeSettings.ip ||
nm != activeSettings.nm ||
gw != activeSettings.gw) {
activeSettings.ip = ip;
activeSettings.gw = gw;
activeSettings.nm = nm;
activeSettings.last_change_ms = AP_HAL::millis();
}
}
/*
send a UDP packet
*/
int32_t AP_Networking_ChibiOS::send_udp(struct udp_pcb *pcb, const ip4_addr_t &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len)
{
if (pcb == nullptr) {
return ERR_ARG;
}
data_len = (data == nullptr) ? 0 : data_len;
struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, data_len, PBUF_RAM);
if (p == nullptr) {
return ERR_MEM;
}
ip_addr_t dst;
ip_addr_copy_from_ip4(dst, ip4_addr);
if (data_len > 0) {
memcpy(p->payload, data, data_len);
}
const err_t err = udp_sendto(pcb, p, &dst, port);
pbuf_free(p);
return err == ERR_OK ? data_len : err;
}
#endif // AP_NETWORKING_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

29
libraries/AP_Networking/AP_Networking_ChibiOS.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
#include "AP_Networking_Config.h"
#if AP_NETWORKING_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include "AP_Networking_backend.h"
class AP_Networking_ChibiOS : public AP_Networking_backend
{
public:
using AP_Networking_backend::AP_Networking_backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Networking_ChibiOS);
bool init() override;
void update() override;
private:
bool allocate_buffers(void);
int32_t send_udp(struct udp_pcb *pcb, const struct ip4_addr &ip4_addr, const uint16_t port, const uint8_t* data, uint16_t data_len);
private:
struct lwipthread_opts *lwip_options;
uint8_t macaddr[6];
};
#endif // AP_NETWORKING_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

35
libraries/AP_Networking/AP_Networking_backend.h Normal file
View File

@ -0,0 +1,35 @@
#pragma once
#include "AP_Networking_Config.h"
#if AP_NETWORKING_ENABLED
class AP_Networking;
class AP_Networking_backend
{
public:
friend class AP_Networking;
AP_Networking_backend(AP_Networking &_frontend) : frontend(_frontend) {}
/* Do not allow copies */
CLASS_NO_COPY(AP_Networking_backend);
virtual bool init() = 0;
virtual void update() = 0;
protected:
AP_Networking &frontend;
struct {
uint32_t ip;
uint32_t nm;
uint32_t gw;
uint32_t announce_ms;
uint8_t macaddr[6];
uint32_t last_change_ms;
} activeSettings;
};
#endif // AP_NETWORKING_ENABLED