AP_Bootloader: use IP address from periph if available

This commit is contained in:
Andrew Tridgell 2024-01-16 08:33:55 +11:00
parent 82dea9c037
commit 212bde0b61
6 changed files with 61 additions and 14 deletions

View File

@ -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();

View File

@ -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];

View File

@ -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)

View File

@ -24,9 +24,22 @@
#include "support.h"
#include "bl_protocol.h"
#include <AP_CheckFirmware/AP_CheckFirmware.h>
#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(&ETHD1, &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

View File

@ -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

View File

@ -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