Added bootloader.c files

crazyflie_software/crazyflie-firmware-2021.06/Makefile

@@ -166,6 +166,7 @@ PROJ_OBJ += vl53l1_register_funcs.o vl53l1_wait.o vl53l1_core_support.o
 
 # Modules
 PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o
+PROJ_OBJ += bootloader.o
 PROJ_OBJ += log.o worker.o queuemonitor.o msp.o
 PROJ_OBJ += platformservice.o sound_cf2.o extrx.o sysload.o mem.o
 PROJ_OBJ += range.o app_handler.o static_mem.o app_channel.o

crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c

@@ -50,6 +50,8 @@
 
 #include "debug.h"
 
+#include "bootloader.h"
+
 
 NO_DMA_CCM_SAFE_ZERO_INIT __ALIGN_BEGIN USB_OTG_CORE_HANDLE    USB_OTG_dev __ALIGN_END ;
 
@@ -180,23 +182,10 @@ static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ  *req)
       rxStopped = false;
     }
   } else if(command == 0x02){
-    //transition to DFU bootloader mode
-
-    //shutdown any tasks running
-    RCC_DeInit();
-    SysTick->CTRL = 0;  //reset the Systick timer
-    SysTick->LOAD = 0;
-    SysTick->VAL  = 0;
-
-    __set_PRIMASK(1);   //disable interrupts
-
-    SYSCFG_MemoryRemapConfig(SYSCFG_MemoryRemap_SystemFlash);
-
-    //set main stack pointer to its default value
-    __set_MSP(*((uint32_t*) 0x00000000));
+    //restart system and transition to DFU bootloader mode
 
-    //jump to boot loader (start of system memory plus 4)
-    ((void (*)(void)) *((uint32_t*) 0x00000004))();
+    //enter bootloader specific to STM32f4xx
+    enter_bootloader(0, 0x00000000);
 
     while(1);
 

crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c

@@ -42,8 +42,12 @@
 /* ST includes */
 #include "stm32fxxx.h"
 
+#include "bootloader.h"
+
 int main() 
 {
+  check_enter_bootloader();
+
   //Initialize the platform.
   int err = platformInit();
   if (err != 0) {

crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/bootloader.h

+
+/* FreeRtos includes */
+#include "FreeRTOS.h"
+/* ST includes */
+#include "stm32f4xx.h"
+
+
+void branch_to_bootloader(uint32_t r0, uint32_t bl_addr);
+
+void check_enter_bootloader();
+
+void enter_bootloader(uint32_t r0, uint32_t bl_addr);
\ No newline at end of file

crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/bootloader.c

+#include "bootloader.h"
+
+//bootloader code used from micropython machine_bootloader
+
+// Location in RAM of bootloader state (just after the top of the stack).
+// STM32H7 has ECC and writes to RAM must be 64-bit so they are fully committed
+// to actual SRAM before a system reset occurs.
+#define BL_STATE_PTR                ((uint64_t *)&_estack)
+#define BL_STATE_KEY                (0x5a5) //arbitrary bit pattern used as marker
+#define BL_STATE_KEY_MASK           (0xfff)
+#define BL_STATE_KEY_SHIFT          (32)
+#define BL_STATE_INVALID            (0)
+#define BL_STATE_VALID(reg, addr)   ((uint64_t)(reg) | ((uint64_t)((addr) | BL_STATE_KEY)) << BL_STATE_KEY_SHIFT)
+#define BL_STATE_GET_REG(s)         ((s) & 0xffffffff)
+#define BL_STATE_GET_KEY(s)         (((s) >> BL_STATE_KEY_SHIFT) & BL_STATE_KEY_MASK)
+#define BL_STATE_GET_ADDR(s)        (((s) >> BL_STATE_KEY_SHIFT) & ~BL_STATE_KEY_MASK)
+extern uint64_t _estack[];
+
+
+void branch_to_bootloader(uint32_t r0, uint32_t bl_addr){
+    __asm volatile (
+        "ldr r2, [r1, #0]\n"    // get address of stack pointer
+        "msr msp, r2\n"         // get stack pointer
+        "ldr r2, [r1, #4]\n"    // get address of destination
+        "bx r2\n"               // branch to bootloader
+        );
+    //unreachable code
+    while(1);
+}
+
+//check if memory is set after software reboot indicating we need to branch to the bootloader
+void check_enter_bootloader(){
+    uint64_t bl_state = *BL_STATE_PTR;
+    //set to invalid for next boot
+    *BL_STATE_PTR = BL_STATE_INVALID;
+
+    //TODO test if statement
+    if(BL_STATE_GET_KEY(bl_state) == BL_STATE_KEY && (RCC->CSR & RCC_CSR_SFTRSTF)){
+        //botloader data valid and was just reset with NVIC_SystemReset
+
+        //remap memory to system flash
+        SYSCFG_MemoryRemapConfig(SYSCFG_MemoryRemap_SystemFlash);
+        branch_to_bootloader(BL_STATE_GET_REG(bl_state), BL_STATE_GET_ADDR(bl_state));
+    }
+}
+
+void enter_bootloader(uint32_t r0, uint32_t bl_addr){
+    //set bootloader state values    
+    *BL_STATE_PTR = BL_STATE_VALID(r0, bl_addr);
+
+    NVIC_SystemReset();
+}
\ No newline at end of file