diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp new file mode 100644 index 0000000000..d55c731885 --- /dev/null +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -0,0 +1,393 @@ + +#include "AP_Networking.h" + +#if AP_NETWORKING_ENABLED + +#include "AP_Math/definitions.h" +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + #include + #include +#else + #include + #include + +#endif + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_Networking::var_info[] = { + + // @Param: ENABLED + // @DisplayName: Enable Networking + // @Description: Enable Networking + // @Values: 0:Disable,1:Enable + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLED", 0, AP_Networking, _param.enabled, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: IPADDR0 + // @DisplayName: IPv4 Address MSB + // @Description: Allows setting static IP address. Example: 192.xxx.xxx.xxx + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("IPADDR0", 1, AP_Networking, _param.ipaddr[0], AP_NETWORKING_DEFAULT_STATIC_IP_ADDR0), + + // @Param: IPADDR1 + // @DisplayName: IPv4 Address 2nd byte + // @Description: Allows setting static IP address. Example: xxx.168.xxx.xxx + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("IPADDR1", 2, AP_Networking, _param.ipaddr[1], AP_NETWORKING_DEFAULT_STATIC_IP_ADDR1), + + // @Param: IPADDR2 + // @DisplayName: IPv4 Address 3rd byte + // @Description: Allows setting static IP address. Example: xxx.xxx.13.xxx + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("IPADDR2", 3, AP_Networking, _param.ipaddr[2], AP_NETWORKING_DEFAULT_STATIC_IP_ADDR2), + + // @Param: IPADDR3 + // @DisplayName: IPv4 Address LSB + // @Description: Allows setting static IP address. Example: xxx.xxx.xxx.14 + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("IPADDR3", 4, AP_Networking, _param.ipaddr[3], AP_NETWORKING_DEFAULT_STATIC_IP_ADDR3), + + // @Param: NETMASK + // @DisplayName: IP Subnet mask + // @Description: Allows setting static subnet mask. The value is a count of consecutive bits. Examples: 24 = 255.255.255.0, 16 = 255.255.0.0 + // @Range: 0 32 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("NETMASK", 5, AP_Networking, _param.netmask, AP_NETWORKING_DEFAULT_NETMASK), + + // @Param: DHCP + // @DisplayName: DHCP client + // @Description: Enable/Disable DHCP client + // @Values: 0:Disable, 1:Enable + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("DHCP", 6, AP_Networking, _param.dhcp, AP_NETWORKING_DEFAULT_DHCP_ENABLE), + + // @Param: GWADDR0 + // @DisplayName: Gateway IP Address MSB + // @Description: Allows setting static GW address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("GWADDR0", 7, AP_Networking, _param.gwaddr[0], AP_NETWORKING_DEFAULT_STATIC_GW_ADDR0), + + // @Param: GWADDR1 + // @DisplayName: Gateway IP Address 2nd byte + // @Description: Allows setting static GW address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("GWADDR1", 8, AP_Networking, _param.gwaddr[1], AP_NETWORKING_DEFAULT_STATIC_GW_ADDR1), + + // @Param: GWADDR2 + // @DisplayName: Gateway IP Address 3rd byte + // @Description: Allows setting static GW address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("GWADDR2", 9, AP_Networking, _param.gwaddr[2], AP_NETWORKING_DEFAULT_STATIC_GW_ADDR2), + + // @Param: GWADDR3 + // @DisplayName: Gateway IP Address LSB + // @Description: Allows setting static GW address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("GWADDR3", 10, AP_Networking, _param.gwaddr[3], AP_NETWORKING_DEFAULT_STATIC_GW_ADDR3), + + // @Param: MACADDR0 + // @DisplayName: MAC Address MSbyte + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR0", 11, AP_Networking, _param.macaddr[0], AP_NETWORKING_DEFAULT_MAC_ADDR0), + + // @Param: MACADDR1 + // @DisplayName: MAC Address 2nd byte + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR1", 12, AP_Networking, _param.macaddr[1], AP_NETWORKING_DEFAULT_MAC_ADDR1), + + // @Param: MACADDR2 + // @DisplayName: MAC Address 3rd byte + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR2", 13, AP_Networking, _param.macaddr[2], AP_NETWORKING_DEFAULT_MAC_ADDR2), + + // @Param: MACADDR3 + // @DisplayName: MAC Address 4th byte + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR3", 14, AP_Networking, _param.macaddr[3], AP_NETWORKING_DEFAULT_MAC_ADDR3), + + // @Param: MACADDR4 + // @DisplayName: MAC Address 5th byte + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR4", 15, AP_Networking, _param.macaddr[4], AP_NETWORKING_DEFAULT_MAC_ADDR4), + + // @Param: MACADDR5 + // @DisplayName: MAC Address LSb + // @Description: Allows setting MAC address + // @Range: 0 255 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("MACADDR5", 16, AP_Networking, _param.macaddr[5], AP_NETWORKING_DEFAULT_MAC_ADDR5), + + AP_GROUPEND +}; + +AP_Networking::AP_Networking(void) +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_singleton != nullptr) { + AP_HAL::panic("AP_Networking must be singleton"); + } +#endif + + _singleton = this; + AP_Param::setup_object_defaults(this, var_info); +} + + +void AP_Networking::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 (!_param.enabled || _init.done) { + return; + } + + // set default MAC Address lower 3 bytes to UUID if possible + uint8_t uuid[12]; + uint8_t uuid_len = sizeof(uuid); + const bool udid_is_ok = hal.util->get_system_id_unformatted(uuid, uuid_len) && uuid_len >= 3; + if (udid_is_ok) { + _param.macaddr[3].set_default(uuid[uuid_len-2]); + _param.macaddr[4].set_default(uuid[uuid_len-1]); + _param.macaddr[5].set_default(uuid[uuid_len-0]); + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + if (!macInit()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "NET: macInit failed"); + return; + } + + const uint8_t localMACAddress[6] = {(uint8_t)_param.macaddr[0].get(), + (uint8_t)_param.macaddr[1].get(), + (uint8_t)_param.macaddr[2].get(), + (uint8_t)_param.macaddr[3].get(), + (uint8_t)_param.macaddr[4].get(), + (uint8_t)_param.macaddr[5].get() }; + +#if !AP_NETWORKING_DHCP_AVAILABLE + 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; + } + + struct lwipthread_opts netOptions = { (uint8_t *) localMACAddress, + _activeSettings.ip, + _activeSettings.nm, + _activeSettings.gw, + addrMode }; + + lwipInit(&netOptions); +#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"); + _init.done = true; +} + +void AP_Networking::announce_address_changes() +{ + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _activeSettings.announce_ms < 1000) { + // Never announce changes any faster than 1 sec + return; + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + const uint32_t ip = lwipGetIp(); + const uint32_t nm = lwipGetNetmask(); + 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 + 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. + 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: Mask %s", get_netmask_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()) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"NET: DHCP enabled, waiting for IP"); + } + + _activeSettings.announce_ms = now_ms; + _activeSettings.announce_at_boot_done = true; +} + +void AP_Networking::update() +{ + if (!is_healthy()) { + return; + } + + announce_address_changes(); +} + +uint32_t AP_Networking::convert_netmask_bitcount_to_ip(const uint32_t netmask_bitcount) +{ + if (netmask_bitcount > 32) { + return 0; + } + + uint32_t netmask_ip = 0; + for (uint32_t i=0; isnprintf(_str_buffer, sizeof(_str_buffer), "%u.%u.%u.%u", (unsigned)ip[0], (unsigned)ip[1], (unsigned)ip[2], (unsigned)ip[3]) == 0) { + _str_buffer[0] = '\0'; + } + return _str_buffer; +} +char* AP_Networking::convert_ip_to_str(const uint32_t ip) +{ + uint8_t ip_array[4]; + ip_array[3] = ((ip >> 24) & 0xff); + ip_array[2] = ((ip >> 16) & 0xff); + ip_array[1] = ((ip >> 8) & 0xff); + ip_array[0] = (ip & 0xff); + + return convert_ip_to_str(ip_array); +} + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +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 { + AP_Networking &network() { + return *AP_Networking::get_singleton(); + } +} + +#endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h new file mode 100644 index 0000000000..37c8ea3dbb --- /dev/null +++ b/libraries/AP_Networking/AP_Networking.h @@ -0,0 +1,128 @@ + +#pragma once + +#include "AP_Networking_Config.h" + +#if AP_NETWORKING_ENABLED +#include +#include + + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + #include "lwipthread.h" + #include "lwip/udp.h" + #include +#else + #include + #define IP4_ADDR_VALUE(a,b,c,d) be32toh(UINT32_VALUE(a,b,c,d)) +#endif + +#define IP4_ADDR_VALUE_FROM_ARRAY(array) IP4_ADDR_VALUE(array[0],array[1],array[2],array[3]) +#define IP4_ADDR_FROM_ARRAY(dest_ip, array) IP4_ADDR(dest_ip, array[0],array[1],array[2],array[3]) +#define IP_ADDR_FROM_ARRAY(dest_ip, array) IP4_ADDR_FROM_ARRAY(dest_ip, array) + +class AP_Networking +{ +public: + AP_Networking(); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Networking); + + void init(); + + void update(); + + static AP_Networking *get_singleton(void) { return _singleton; } + + bool is_healthy() const { return _param.enabled && _init.done; } + bool get_dhcp_enabled() const { return _param.dhcp; } + void set_dhcp_enable(const bool enable) { _param.dhcp.set(enable); } + + uint32_t get_ip_active() const { return _activeSettings.ip; } + uint32_t get_ip_param() const { return IP4_ADDR_VALUE_FROM_ARRAY(_param.ipaddr); } + char* get_ip_active_str() const { return convert_ip_to_str(get_ip_active()); } + char* get_ip_param_str() const { return convert_ip_to_str(get_ip_param()); } + void set_ip_param_str(const char* ip_str) { set_ip_param(convert_str_to_ip((char*)ip_str)); } + void set_ip_param(const uint32_t ip) { + //put_le32_ptr(_param.ipaddr->get(), ip); + _param.ipaddr[3].set_and_save((ip >> 24) & 0xff); + _param.ipaddr[2].set_and_save((ip >> 16) & 0xff); + _param.ipaddr[1].set_and_save((ip >> 8) & 0xff); + _param.ipaddr[0].set_and_save(ip & 0xff); + } + + uint32_t get_netmask_active() const { return _activeSettings.nm; } + uint32_t get_netmask_param() const { return convert_netmask_bitcount_to_ip(_param.netmask.get()); } + char* get_netmask_active_str() { return convert_ip_to_str(get_netmask_active()); } + char* get_netmask_param_str() { return convert_ip_to_str(get_netmask_param()); } + void set_netmask_param_str(const char* nm_str) { set_netmask_param(convert_str_to_ip((char*)nm_str)); } + void set_netmask_param(const uint32_t nm) { _param.netmask.set(convert_netmask_ip_to_bitcount(nm)); } + + uint32_t get_gateway_active() const { return _activeSettings.gw; } + uint32_t get_gateway_param() const { return IP4_ADDR_VALUE_FROM_ARRAY(_param.gwaddr); } + char* get_gateway_active_str() { return convert_ip_to_str(get_gateway_active()); } + char* get_gateway_param_str() { return convert_ip_to_str(get_gateway_param()); } + void set_gateway_param_str(const char* gw_str) { set_gateway_param(convert_str_to_ip((char*)gw_str)); } + void set_gateway_param(const uint32_t gw) { + //put_le32_ptr(_param.gwaddr->get(), gw); + _param.gwaddr[3].set_and_save((gw >> 24) & 0xff); + _param.gwaddr[2].set_and_save((gw >> 16) & 0xff); + _param.gwaddr[1].set_and_save((gw >> 8) & 0xff); + _param.gwaddr[0].set_and_save(gw & 0xff); + } + + + static uint32_t convert_str_to_ip(char* ip_str); + static char* convert_ip_to_str(const uint8_t ip[4]); + static char* convert_ip_to_str(const uint32_t ip); + + 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); + + +#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[]; + +private: + static int tftp_fd; + static AP_Networking *_singleton; + + uint8_t _num_instances; // number of feature instances + + void announce_address_changes(); + + struct { + bool done; + } _init; + + struct { + AP_Int16 ipaddr[4]; + AP_Int8 netmask; // bits to mask. example: (16 == 255.255.0.0) and (24 == 255.255.255.0) + AP_Int16 gwaddr[4]; + + AP_Int8 dhcp; + AP_Int16 macaddr[6]; + AP_Int8 enabled; + AP_Int32 options; + } _param; + + struct { + uint32_t ip; + uint32_t nm; + uint32_t gw; + uint32_t announce_ms; + bool announce_at_boot_done; + } _activeSettings; + + HAL_Semaphore _sem; +}; + +namespace AP { + AP_Networking &network(); +}; + +#endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h new file mode 100644 index 0000000000..e7444a9743 --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -0,0 +1,84 @@ +#include + +#ifndef AP_NETWORKING_ENABLED +#define AP_NETWORKING_ENABLED 0 +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + #define AP_NETWORKING_DHCP_AVAILABLE LWIP_DHCP +#else + #define AP_NETWORKING_DHCP_AVAILABLE 1 // for non-ChibiOS, assume it's available +#endif + + +// --------------------------- +// Below are default params +// --------------------------- + +#ifndef AP_NETWORKING_DEFAULT_OPTIONS + #define AP_NETWORKING_DEFAULT_OPTIONS 0 +#endif + +// Default DHCP +#ifndef AP_NETWORKING_DEFAULT_DHCP_ENABLE +#define AP_NETWORKING_DEFAULT_DHCP_ENABLE 0 +#endif + +// Default Static IP Address: 192.168.13.13 +#ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR0 + #define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR0 192 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR1 + #define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR1 168 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR2 + #define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR2 13 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR3 + #define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR3 14 +#endif + +// Default Netmask: 24 +// Note, the netmask is the number of consecutive bits +#ifndef AP_NETWORKING_DEFAULT_NETMASK + #define AP_NETWORKING_DEFAULT_NETMASK 24 // 255.255.255.0 + // #define AP_NETWORKING_DEFAULT_NETMASK 16 // 255.255.0.0 +#endif + + +// Default Static IP Address: 192.168.13.1 +#ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR0 + #define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR0 192 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR1 + #define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR1 168 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR2 + #define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR2 13 +#endif +#ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR3 + #define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR3 1 +#endif + + +// Default MAC Address: 2C:AF:51:03:CF:46 +// Note, lower 3 bytes (ADDR3,4,5) will be replaced with the platform UUID +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR0 + #define AP_NETWORKING_DEFAULT_MAC_ADDR0 0xC2 //LWIP_ETHADDR_0 +#endif +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR1 + #define AP_NETWORKING_DEFAULT_MAC_ADDR1 0xAF //LWIP_ETHADDR_1 +#endif +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR2 + #define AP_NETWORKING_DEFAULT_MAC_ADDR2 0x51 //LWIP_ETHADDR_2 +#endif +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR3 + #define AP_NETWORKING_DEFAULT_MAC_ADDR3 0x03 //LWIP_ETHADDR_3 +#endif +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR4 + #define AP_NETWORKING_DEFAULT_MAC_ADDR4 0xCF //LWIP_ETHADDR_4 +#endif +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR5 + #define AP_NETWORKING_DEFAULT_MAC_ADDR5 0x46 //LWIP_ETHADDR_5 +#endif +