From 212bde0b612f34919522d7a748b2bc14c028ea0b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 16 Jan 2024 08:33:55 +1100 Subject: [PATCH] AP_Bootloader: use IP address from periph if available --- Tools/AP_Bootloader/AP_Bootloader.cpp | 4 ++ Tools/AP_Bootloader/app_comms.h | 5 ++- Tools/AP_Bootloader/can.cpp | 3 +- Tools/AP_Bootloader/network.cpp | 54 +++++++++++++++++++++------ Tools/AP_Bootloader/network.h | 6 +++ Tools/AP_Bootloader/support.cpp | 3 ++ 6 files changed, 61 insertions(+), 14 deletions(-) diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 60555c213f..7f9964d3b2 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -97,6 +97,10 @@ int main(void) stm32_flash_unprotect_flash(); #endif +#if AP_BOOTLOADER_NETWORK_ENABLED + network.save_comms_ip(); +#endif + #if AP_FASTBOOT_ENABLED enum rtc_boot_magic m = check_fast_reboot(); bool was_watchdog = stm32_was_watchdog_reset(); diff --git a/Tools/AP_Bootloader/app_comms.h b/Tools/AP_Bootloader/app_comms.h index 04c0dcb0b9..2a03f9cc27 100644 --- a/Tools/AP_Bootloader/app_comms.h +++ b/Tools/AP_Bootloader/app_comms.h @@ -10,7 +10,10 @@ struct app_bootloader_comms { uint32_t magic; - uint32_t reserved[4]; + uint32_t ip; + uint32_t netmask; + uint32_t gateway; + uint32_t reserved; uint8_t server_node_id; uint8_t my_node_id; uint8_t path[201]; diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 24ed583284..8e1aaea200 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -725,9 +725,8 @@ bool can_check_update(void) fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data); } memcpy(fw_update.path, comms->path, sizeof(uavcan_protocol_file_Path::path.data)+1); + memset(comms, 0, sizeof(*comms)); ret = true; - // clear comms region - memset(comms, 0, sizeof(struct app_bootloader_comms)); } #endif #if defined(CAN1_BASE) && defined(RCC_APB1ENR_CAN1EN) diff --git a/Tools/AP_Bootloader/network.cpp b/Tools/AP_Bootloader/network.cpp index 243d0b83f6..9b6c433f2e 100644 --- a/Tools/AP_Bootloader/network.cpp +++ b/Tools/AP_Bootloader/network.cpp @@ -24,9 +24,22 @@ #include "support.h" #include "bl_protocol.h" #include +#include "app_comms.h" -#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR -#define AP_NETWORKING_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46" +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR +#define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_IP +#define AP_NETWORKING_BOOTLOADER_DEFAULT_IP "192.168.1.100" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY +#define AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY "192.168.1.1" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK +#define AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK "255.255.255.0" #endif #ifndef AP_BOOTLOADER_NETWORK_USE_DHCP @@ -186,21 +199,27 @@ void BL_Network::net_thread() thread_sleep_ms(100); - AP_Networking::convert_str_to_macaddr(AP_NETWORKING_DEFAULT_MAC_ADDR, thisif->hwaddr); + AP_Networking::convert_str_to_macaddr(AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR, thisif->hwaddr); - struct { - ip4_addr_t ip, gateway, netmask; - } addr {}; - - addr.ip.addr = htonl(SocketAPM::inet_str_to_addr("192.168.2.201")); - addr.gateway.addr = htonl(SocketAPM::inet_str_to_addr("192.168.2.10")); - addr.netmask.addr = htonl(SocketAPM::inet_str_to_addr("255.255.255.0")); + ip4_addr_t ip, gateway, netmask; + if (addr.ip == 0) { + // no IP from AP_Periph, use defaults + ip.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_IP)); + gateway.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY)); + netmask.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK)); + } else { + // use addresses from AP_Periph + ip.addr = htonl(addr.ip); + netmask.addr = htonl(addr.netmask); + gateway.addr = htonl(addr.gateway); + } const MACConfig mac_config = {thisif->hwaddr}; macStart(ÐD1, &mac_config); /* Add interface. */ - auto result = netifapi_netif_add(thisif, &addr.ip, &addr.netmask, &addr.gateway, NULL, ethernetif_init, tcpip_input); + + auto result = netifapi_netif_add(thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input); if (result != ERR_OK) { AP_HAL::panic("Failed to initialise netif"); } @@ -505,5 +524,18 @@ void BL_Network::init() this); } +/* + save IP address from AP_Periph + */ +void BL_Network::save_comms_ip(void) +{ + struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; + if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC && comms->ip != 0) { + addr.ip = comms->ip; + addr.netmask = comms->netmask; + addr.gateway = comms->gateway; + } +} + #endif // AP_BOOTLOADER_NETWORK_ENABLED diff --git a/Tools/AP_Bootloader/network.h b/Tools/AP_Bootloader/network.h index e7df5c4f35..a3f46a9c3e 100644 --- a/Tools/AP_Bootloader/network.h +++ b/Tools/AP_Bootloader/network.h @@ -13,6 +13,8 @@ class SocketAPM; class BL_Network { public: void init(void); + void save_comms_ip(void); + private: struct netif *thisif; thread_t *net_thread_ctx; @@ -37,6 +39,10 @@ private: const char *name; const char *value; } variables[]; + + struct { + uint32_t ip, gateway, netmask; + } addr; }; #endif // AP_BOOTLOADER_NETWORK_ENABLED diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index 0954f7b753..7f75e68494 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -150,6 +150,9 @@ bool flash_func_is_erased(uint32_t sector) bool flash_func_erase_sector(uint32_t sector, bool force_erase) { + if (sector >= num_pages) { + return false; + } #if AP_BOOTLOADER_ALWAYS_ERASE return stm32_flash_erasepage(flash_base_page+sector); #else