AP_Networking: cleanup

This commit is contained in:
Tom Pittenger 2023-08-02 17:53:32 -07:00 committed by Andrew Tridgell
parent 88cc72f0ee
commit 1e9d00fecb
2 changed files with 43 additions and 22 deletions

View File

@ -1,8 +1,9 @@
#include "AP_Networking.h" #include "AP_Networking_Config.h"
#if AP_NETWORKING_ENABLED #if AP_NETWORKING_ENABLED
#include "AP_Networking.h"
#include "AP_Math/definitions.h" #include "AP_Math/definitions.h"
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
@ -20,8 +21,8 @@ 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: Enable Networking // @DisplayName: Networking Enable
// @Description: Enable Networking // @Description: Networking Enable
// @Values: 0:Disable,1:Enable // @Values: 0:Disable,1:Enable
// @RebootRequired: True // @RebootRequired: True
// @User: Advanced // @User: Advanced
@ -194,14 +195,14 @@ static bool allocate_buffers()
rasr++; rasr++;
} }
void *mem = malloc_eth_safe(size); void *mem = malloc_eth_safe(size);
if (mem == nullptr) {
return false;
}
// ensure our memory is aligned // ensure our memory is aligned
// ref. Cortex-M7 peripherals PM0253, section 4.6.4 MPU region base address register // ref. Cortex-M7 peripherals PM0253, section 4.6.4 MPU region base address register
if (((uint32_t)mem) % size) { if (((uint32_t)mem) % size) {
AP_HAL::panic("Bad alignment of ETH memory"); AP_HAL::panic("Bad alignment of ETH memory");
} }
if (mem == nullptr) {
return false;
}
// for total_size == 9240, size should be 16384 and (rasr-1) should be 13 (MPU_RASR_SIZE_16K) // 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); const uint32_t rasr_size = MPU_RASR_SIZE(rasr-1);
@ -230,7 +231,7 @@ static bool allocate_buffers()
} }
return true; return true;
} }
#endif #endif // STM32_ETH_BUFFERS_EXTERN
void AP_Networking::init() void AP_Networking::init()
{ {
@ -324,10 +325,6 @@ void AP_Networking::announce_address_changes()
const uint32_t ip = lwipGetIp(); const uint32_t ip = lwipGetIp();
const uint32_t nm = lwipGetNetmask(); const uint32_t nm = lwipGetNetmask();
const uint32_t gw = lwipGetGateway(); const uint32_t gw = lwipGetGateway();
// struct netif thisif = lwipGetNetIf();
// const uint32_t ip = thisif.ip_addr.u_addr.ip4.addr;
// const uint32_t nm = thisif.netmask.u_addr.ip4.addr;
// const uint32_t gw = thisif.gw.u_addr.ip4.addr;
#else #else
const uint32_t ip = 0; const uint32_t ip = 0;
const uint32_t nm = 0; const uint32_t nm = 0;
@ -400,7 +397,7 @@ uint32_t AP_Networking::convert_str_to_ip(char* ip_str)
return ip; return ip;
} }
char* AP_Networking::convert_ip_to_str(const uint8_t ip[4]) const char* AP_Networking::convert_ip_to_str(const uint8_t ip[4])
{ {
static char _str_buffer[20]; static char _str_buffer[20];
if (hal.util->snprintf(_str_buffer, sizeof(_str_buffer), "%u.%u.%u.%u", (unsigned)ip[0], (unsigned)ip[1], (unsigned)ip[2], (unsigned)ip[3]) == 0) { if (hal.util->snprintf(_str_buffer, sizeof(_str_buffer), "%u.%u.%u.%u", (unsigned)ip[0], (unsigned)ip[1], (unsigned)ip[2], (unsigned)ip[3]) == 0) {
@ -408,7 +405,7 @@ char* AP_Networking::convert_ip_to_str(const uint8_t ip[4])
} }
return _str_buffer; return _str_buffer;
} }
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];
ip_array[3] = ((ip >> 24) & 0xff); ip_array[3] = ((ip >> 24) & 0xff);

View File

@ -29,8 +29,10 @@ public:
/* Do not allow copies */ /* Do not allow copies */
CLASS_NO_COPY(AP_Networking); CLASS_NO_COPY(AP_Networking);
// 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
void update(); void update();
static AP_Networking *get_singleton(void) static AP_Networking *get_singleton(void)
@ -38,39 +40,55 @@ public:
return _singleton; return _singleton;
} }
// Networking interface is enabled and initialized
bool is_healthy() const bool is_healthy() const
{ {
return _param.enabled && _init.done; return _param.enabled && _init.done;
} }
// 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
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
uint32_t get_ip_active() const uint32_t get_ip_active() const
{ {
return _activeSettings.ip; return _activeSettings.ip;
} }
// returns the 32bit value of the user-parameter static IP address
uint32_t get_ip_param() const uint32_t get_ip_param() const
{ {
return IP4_ADDR_VALUE_FROM_ARRAY(_param.ipaddr); return IP4_ADDR_VALUE_FROM_ARRAY(_param.ipaddr);
} }
char* get_ip_active_str() const
// returns a null terminated string of the active IP address. Example: "192.168.12.13"
const char* get_ip_active_str() const
{ {
return convert_ip_to_str(get_ip_active()); return convert_ip_to_str(get_ip_active());
} }
char* get_ip_param_str() const
// returns a null terminated string of the user-parameter static IP address. Example: "192.168.12.13"
const char* get_ip_param_str() const
{ {
return convert_ip_to_str(get_ip_param()); return convert_ip_to_str(get_ip_param());
} }
// sets the user-parameter static IP address from a null terminated string.
void set_ip_param_str(const char* ip_str) void set_ip_param_str(const char* ip_str)
{ {
set_ip_param(convert_str_to_ip((char*)ip_str)); set_ip_param(convert_str_to_ip((char*)ip_str));
} }
// 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)
{ {
//put_le32_ptr(_param.ipaddr->get(), ip); //put_le32_ptr(_param.ipaddr->get(), ip);
@ -80,19 +98,24 @@ public:
_param.ipaddr[0].set_and_save(ip & 0xff); _param.ipaddr[0].set_and_save(ip & 0xff);
} }
// 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 _activeSettings.nm;
} }
// 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());
} }
char* get_netmask_active_str()
// returns a null terminated string of the active Netmask address. Example: "192.168.12.13"
const char* get_netmask_active_str()
{ {
return convert_ip_to_str(get_netmask_active()); return convert_ip_to_str(get_netmask_active());
} }
char* get_netmask_param_str() const char* get_netmask_param_str()
{ {
return convert_ip_to_str(get_netmask_param()); return convert_ip_to_str(get_netmask_param());
} }
@ -113,11 +136,11 @@ public:
{ {
return IP4_ADDR_VALUE_FROM_ARRAY(_param.gwaddr); return IP4_ADDR_VALUE_FROM_ARRAY(_param.gwaddr);
} }
char* get_gateway_active_str() const char* get_gateway_active_str()
{ {
return convert_ip_to_str(get_gateway_active()); return convert_ip_to_str(get_gateway_active());
} }
char* get_gateway_param_str() const char* get_gateway_param_str()
{ {
return convert_ip_to_str(get_gateway_param()); return convert_ip_to_str(get_gateway_param());
} }
@ -135,10 +158,12 @@ public:
} }
// helper functions to convert between 32bit IP addresses and null terminated strings and back
static uint32_t convert_str_to_ip(char* ip_str); static uint32_t convert_str_to_ip(char* ip_str);
static char* convert_ip_to_str(const uint8_t ip[4]); static const char* convert_ip_to_str(const uint8_t ip[4]);
static char* convert_ip_to_str(const uint32_t ip); static const char* convert_ip_to_str(const uint32_t ip);
// helper functions to convert between 32bit Netmask and counting consecutive bits and back
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);
@ -150,7 +175,6 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
static int tftp_fd;
static AP_Networking *_singleton; static AP_Networking *_singleton;
uint8_t _num_instances; // number of feature instances uint8_t _num_instances; // number of feature instances