diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 7e4b80cad1..60555c213f 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -37,6 +37,7 @@ #include #endif #include +#include "network.h" extern "C" { int main(void); @@ -61,10 +62,20 @@ struct boardinfo board_info = { AP_FlashIface_JEDEC ext_flash; #endif +#if AP_BOOTLOADER_NETWORK_ENABLED +static BL_Network network; +#endif + int main(void) { custom_startup(); + flash_init(); + +#ifdef STM32H7 + check_ecc_errors(); +#endif + #ifdef STM32F427xx if (BOARD_FLASH_SIZE > 1024 && check_limit_flash_1M()) { board_info.fw_size = (1024 - (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB))*1024; @@ -183,8 +194,10 @@ int main(void) #if HAL_USE_CAN == TRUE || HAL_NUM_CAN_IFACES can_start(); #endif - flash_init(); +#if AP_BOOTLOADER_NETWORK_ENABLED + network.init(); +#endif #if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED if (flash_from_sd()) { @@ -198,7 +211,7 @@ int main(void) jump_to_app(); } #else - // CAN only + // CAN and network only while (true) { uint32_t t0 = AP_HAL::millis(); while (timeout == 0 || AP_HAL::millis() - t0 <= timeout) { diff --git a/Tools/AP_Bootloader/AP_Bootloader_config.h b/Tools/AP_Bootloader/AP_Bootloader_config.h index 459fca73cd..869ebf1630 100644 --- a/Tools/AP_Bootloader/AP_Bootloader_config.h +++ b/Tools/AP_Bootloader/AP_Bootloader_config.h @@ -1,7 +1,12 @@ #pragma once #include "hwdef.h" +#include #ifndef AP_BOOTLOADER_FLASH_FROM_SD_ENABLED #define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 0 #endif + +#ifndef AP_BOOTLOADER_NETWORK_ENABLED +#define AP_BOOTLOADER_NETWORK_ENABLED AP_NETWORKING_ENABLED +#endif diff --git a/Tools/AP_Bootloader/Web/index.html b/Tools/AP_Bootloader/Web/index.html new file mode 100644 index 0000000000..44ace6fa0a --- /dev/null +++ b/Tools/AP_Bootloader/Web/index.html @@ -0,0 +1,22 @@ + + + AP_Bootloader + + +

Bootloader

+ + + + + +
Board Type{BOARD_NAME}
Board ID{BOARD_ID}
Flash Size{FLASH_SIZE}
+ +

Firmware Update

+ +
+ + +
+ + + diff --git a/Tools/AP_Bootloader/network.cpp b/Tools/AP_Bootloader/network.cpp new file mode 100644 index 0000000000..243d0b83f6 --- /dev/null +++ b/Tools/AP_Bootloader/network.cpp @@ -0,0 +1,509 @@ +/* + support for networking enabled bootloader + */ + +#include "network.h" + +#if AP_BOOTLOADER_NETWORK_ENABLED + +#include +#include + +#include +#include +#include +#include +#if LWIP_DHCP +#include +#endif +#include +#include "../../modules/ChibiOS/os/various/evtimer.h" +#include +#include +#include +#include "support.h" +#include "bl_protocol.h" +#include + +#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR +#define AP_NETWORKING_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46" +#endif + +#ifndef AP_BOOTLOADER_NETWORK_USE_DHCP +#define AP_BOOTLOADER_NETWORK_USE_DHCP 0 +#endif + +#define LWIP_SEND_TIMEOUT_MS 50 +#define LWIP_NETIF_MTU 1500 +#define LWIP_LINK_POLL_INTERVAL TIME_S2I(5) + +#define PERIODIC_TIMER_ID 1 +#define FRAME_RECEIVED_ID 2 + +#define MIN(a,b) ((a)<(b)?(a):(b)) + +void BL_Network::link_up_cb(void *p) +{ +#if AP_BOOTLOADER_NETWORK_USE_DHCP + auto *driver = (AP_Networking_ChibiOS *)p; + dhcp_start(driver->thisif); +#endif +} + +void BL_Network::link_down_cb(void *p) +{ +#if AP_BOOTLOADER_NETWORK_USE_DHCP + auto *driver = (BL_Network *)p; + dhcp_stop(driver->thisif); +#endif +} + +/* + * This function does the actual transmission of the packet. The packet is + * contained in the pbuf that is passed to the function. This pbuf + * might be chained. + * + * @param netif the lwip network interface structure for this ethernetif + * @param p the MAC packet to send (e.g. IP packet including MAC addresses and type) + * @return ERR_OK if the packet could be sent + * an err_t value if the packet couldn't be sent + * + * @note Returning ERR_MEM here if a DMA queue of your MAC is full can lead to + * strange results. You might consider waiting for space in the DMA queue + * to become available since the stack doesn't retry to send a packet + * dropped because of memory failure (except for the TCP timers). + */ +int8_t BL_Network::low_level_output(struct netif *netif, struct pbuf *p) +{ + struct pbuf *q; + MACTransmitDescriptor td; + (void)netif; + + if (macWaitTransmitDescriptor(ÐD1, &td, TIME_MS2I(LWIP_SEND_TIMEOUT_MS)) != MSG_OK) { + return ERR_TIMEOUT; + } + +#if ETH_PAD_SIZE + pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = p; q != NULL; q = q->next) { + macWriteTransmitDescriptor(&td, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseTransmitDescriptorX(&td); + +#if ETH_PAD_SIZE + pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + + return ERR_OK; +} + +/* + * Receives a frame. + * Allocates a pbuf and transfers the bytes of the incoming + * packet from the interface into the pbuf. + * + * @param netif the lwip network interface structure for this ethernetif + * @return a pbuf filled with the received packet (including MAC header) + * NULL on memory error + */ +bool BL_Network::low_level_input(struct netif *netif, struct pbuf **pbuf) +{ + MACReceiveDescriptor rd; + struct pbuf *q; + u16_t len; + + (void)netif; + + if (macWaitReceiveDescriptor(ÐD1, &rd, TIME_IMMEDIATE) != MSG_OK) { + return false; + } + + len = (u16_t)rd.size; + +#if ETH_PAD_SIZE + len += ETH_PAD_SIZE; /* allow room for Ethernet padding */ +#endif + + /* We allocate a pbuf chain of pbufs from the pool. */ + *pbuf = pbuf_alloc(PBUF_RAW, len, PBUF_POOL); + + if (*pbuf != nullptr) { +#if ETH_PAD_SIZE + pbuf_header(*pbuf, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = *pbuf; q != NULL; q = q->next) { + macReadReceiveDescriptor(&rd, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseReceiveDescriptorX(&rd); + +#if ETH_PAD_SIZE + pbuf_header(*pbuf, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + } else { + macReleaseReceiveDescriptorX(&rd); // Drop packet + } + + return true; +} + +int8_t BL_Network::ethernetif_init(struct netif *netif) +{ + netif->state = NULL; + netif->name[0] = 'm'; + netif->name[1] = 's'; + netif->output = etharp_output; + netif->linkoutput = low_level_output; + + /* set MAC hardware address length */ + netif->hwaddr_len = ETHARP_HWADDR_LEN; + + /* maximum transfer unit */ + netif->mtu = LWIP_NETIF_MTU; + + /* device capabilities */ + netif->flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP; + +#if LWIP_IGMP + // also enable multicast + netif->flags |= NETIF_FLAG_IGMP; +#endif + + return ERR_OK; +} + +/* + networking thread +*/ +void BL_Network::net_thread() +{ + /* start tcpip thread */ + tcpip_init(NULL, NULL); + + thread_sleep_ms(100); + + AP_Networking::convert_str_to_macaddr(AP_NETWORKING_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")); + + 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); + if (result != ERR_OK) { + AP_HAL::panic("Failed to initialise netif"); + } + + netifapi_netif_set_default(thisif); + netifapi_netif_set_up(thisif); + + /* Setup event sources.*/ + event_timer_t evt; + event_listener_t el0, el1; + + evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL); + evtStart(&evt); + chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID); + chEvtRegisterMaskWithFlags(macGetEventSource(ÐD1), &el1, + FRAME_RECEIVED_ID, MAC_FLAGS_RX); + chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID); + + while (true) { + eventmask_t mask = chEvtWaitAny(ALL_EVENTS); + if (mask & PERIODIC_TIMER_ID) { + bool current_link_status = macPollLinkStatus(ÐD1); + if (current_link_status != netif_is_link_up(thisif)) { + if (current_link_status) { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up, thisif, 0); + tcpip_callback_with_block(link_up_cb, this, 0); + } + else { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down, thisif, 0); + tcpip_callback_with_block(link_down_cb, this, 0); + } + } + } + + if (mask & FRAME_RECEIVED_ID) { + struct pbuf *p; + while (low_level_input(thisif, &p)) { + if (p != NULL) { + struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload; + switch (htons(ethhdr->type)) { + /* IP or ARP packet? */ + case ETHTYPE_IP: + case ETHTYPE_ARP: + /* full packet send to tcpip_thread to process */ + if (thisif->input(p, thisif) == ERR_OK) { + break; + } + /* Falls through */ + default: + pbuf_free(p); + } + } + } + } + } +} + +void BL_Network::net_thread_trampoline(void *ctx) +{ + auto *net = (BL_Network *)ctx; + net->net_thread(); +} + +void BL_Network::web_server_trampoline(void *ctx) +{ + auto *net = (BL_Network *)ctx; + net->web_server(); +} + +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +BL_Network::web_var BL_Network::variables[] = { + { "BOARD_NAME", CHIBIOS_BOARD_NAME }, + { "BOARD_ID", STR(APJ_BOARD_ID) }, + { "FLASH_SIZE", STR(BOARD_FLASH_SIZE) }, +}; + +/* + substitute variables of the form {VARNAME} + */ +char *BL_Network::substitute_vars(const char *str, uint32_t size) +{ + // assume 1024 is enough room for new variables + char *result = (char *)malloc(strlen(str) + 1024); + if (result == nullptr) { + return nullptr; + } + char *p = result; + const char *str0 = str; + while (*str && str-str0recv(&c, 1, 100); + if (n != 1) { + break; + } + *p++ = c; + if (p-ret >= 4 && strcmp(p-4, "\r\n\r\n") == 0) { + break; + } + } + return ret; +} + +/* + handle an incoming HTTP POST request + */ +void BL_Network::handle_post(SocketAPM *sock, uint32_t content_length) +{ + /* + skip over form boundary. We don't care about the filename as we + only support one file + */ + uint8_t state = 0; + while (true) { + char c; + if (sock->recv(&c, 1, 100) != 1) { + return; + } + content_length--; + // absorb up to \r\n\r\n + if (c == '\r') { + if (state == 2) { + state = 3; + } else { + state = 1; + } + } else if (c == '\n') { + if (state == 1 || state == 3) { + state++; + } else { + state = 0; + } + if (state == 4) { + break; + } + } else { + state = 0; + } + } + /* + erase all of flash + */ + flash_set_keep_unlocked(true); + uint32_t sec=0; + while (flash_func_erase_sector(sec)) { + sec++; + } + /* + receive file and write to flash + */ + uint32_t buf[128]; + uint32_t ofs = 0; + + // must be multiple of 4 + content_length &= ~3; + + const uint32_t max_ofs = MIN(BOARD_FLASH_SIZE*1024, content_length); + while (ofs < max_ofs) { + const uint32_t needed = MIN(sizeof(buf), max_ofs-ofs); + auto n = sock->recv((void*)buf, needed, 10000); + if (n <= 0) { + break; + } + // we need a whole number of words + if (n % 4 != 0 && n < needed) { + auto n2 = sock->recv(((uint8_t*)buf)+n, 4 - n%4, 10000); + if (n2 > 0) { + n += n2; + } + } + flash_write_buffer(ofs, buf, n/4); + ofs += n; + } + if (ofs % 32 != 0) { + // pad to 32 bytes + memset(buf, 0xff, sizeof(buf)); + flash_write_buffer(ofs, buf, (32 - ofs%32)/4); + } + flash_write_flush(); + flash_set_keep_unlocked(false); + const auto ok = check_good_firmware(); + if (ok == check_fw_result_t::CHECK_FW_OK) { + jump_to_app(); + } +} + +/* + handle an incoming HTTP request + */ +void BL_Network::handle_request(SocketAPM *sock) +{ + /* + read HTTP headers + */ + char *headers = read_headers(sock); + + if (strncmp(headers, "POST / ", 7) == 0) { + const char *clen = "\r\nContent-Length:"; + const char *p = strstr(headers, clen); + if (p != nullptr) { + p += strlen(clen); + const uint32_t content_length = atoi(p); + handle_post(sock, content_length); + } + } + + uint32_t size = 0; + /* + we only need one URL in the bootloader + */ + const char *header = "HTTP/1.1 200 OK\r\n" + "Content-Type: text/html\r\n" + "Connection: close\r\n" + "\r\n"; + const auto *msg = AP_ROMFS::find_decompress("index.html", size); + sock->send(header, strlen(header)); + char *msg2 = substitute_vars((const char *)msg, size); + sock->send(msg2, strlen(msg2)); + delete msg2; + delete headers; + AP_ROMFS::free(msg); +} + +/* + web server thread + */ +void BL_Network::web_server(void) +{ + auto *listen_socket = new SocketAPM(0); + listen_socket->bind("0.0.0.0", 80); + listen_socket->listen(20); + + while (true) { + auto *sock = listen_socket->accept(20); + if (sock == nullptr) { + continue; + } + handle_request(sock); + delete sock; + } +} + +/* + initialise bootloader networking + */ +void BL_Network::init() +{ + AP_Networking_ChibiOS::allocate_buffers(); + + macInit(); + + thisif = new netif; + + net_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + "network", + 60, + net_thread_trampoline, + this); + + thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + "web_server", + 60, + web_server_trampoline, + this); +} + +#endif // AP_BOOTLOADER_NETWORK_ENABLED + diff --git a/Tools/AP_Bootloader/network.h b/Tools/AP_Bootloader/network.h new file mode 100644 index 0000000000..e7df5c4f35 --- /dev/null +++ b/Tools/AP_Bootloader/network.h @@ -0,0 +1,43 @@ +/* + support for networking enabled bootloader + */ + +#include "AP_Bootloader_config.h" + +#if AP_BOOTLOADER_NETWORK_ENABLED + +#include + +class SocketAPM; + +class BL_Network { +public: + void init(void); +private: + struct netif *thisif; + thread_t *net_thread_ctx; + + static void net_thread_trampoline(void*); + static void web_server_trampoline(void*); + + void net_thread(void); + void web_server(void); + void handle_request(SocketAPM *); + void handle_post(SocketAPM *, uint32_t content_length); + char *read_headers(SocketAPM *); + + static void link_up_cb(void *p); + static void link_down_cb(void *p); + static int8_t low_level_output(struct netif *netif, struct pbuf *p); + static bool low_level_input(struct netif *netif, struct pbuf **pbuf); + static int8_t ethernetif_init(struct netif *netif); + + static char *substitute_vars(const char *msg, uint32_t size); + static struct web_var { + const char *name; + const char *value; + } variables[]; +}; + +#endif // AP_BOOTLOADER_NETWORK_ENABLED + diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index c9f37b2b60..0954f7b753 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -353,7 +353,7 @@ void uprintf(const char *fmt, ...) #endif } -static void thread_sleep_ms(uint32_t ms) +void thread_sleep_ms(uint32_t ms) { while (ms > 0) { // don't sleep more than 65 at a time, to cope with 16 bit @@ -486,3 +486,36 @@ void port_setbaud(uint32_t baudrate) } #endif // BOOTLOADER_DEV_LIST +#ifdef STM32H7 +/* + check if flash has any ECC errors and if it does then erase all of + flash + */ +void check_ecc_errors(void) +{ + __disable_fault_irq(); + auto *dma = dmaStreamAlloc(STM32_DMA_STREAM_ID(1, 1), 0, nullptr, nullptr); + uint32_t buf[32]; + uint32_t ofs = 0; + while (ofs < BOARD_FLASH_SIZE*1024 && FLASH->SR1 == 0 && FLASH->SR2 == 0) { + dmaStartMemCopy(dma, + STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE | + STM32_DMA_CR_MSIZE_BYTE, + ofs+(uint8_t*)FLASH_BASE, buf, sizeof(buf)); + dmaWaitCompletion(dma); + ofs += sizeof(buf); + } + dmaStreamFree(dma); + + if (ofs < BOARD_FLASH_SIZE*1024) { + // we must have ECC errors in flash + flash_set_keep_unlocked(true); + for (uint32_t i=0; i