Add bootloader and firmware update API (#5728)

* Add F765XG MCU support * Add bootloader and update system * Fix linker files
2025-07-25 17:25:18 +03:00 · 2020-07-20 22:46:15 +02:00 · 2020-07-20 22:46:15 +02:00 · 6f76bd5ad9
commit 6f76bd5ad9
parent 707133c4db
55 changed files with 2103 additions and 98 deletions
--- a/114
+++ b/114
@ -66,7 +66,9 @@ endif

 # Working directories
 SRC_DIR         := $(ROOT)/src/main
+BL_SRC_DIR      := $(ROOT)/src/bl
 OBJECT_DIR      := $(ROOT)/obj/main
+BL_OBJECT_DIR   := $(ROOT)/obj/bl
 BIN_DIR         := $(ROOT)/obj
 CMSIS_DIR       := $(ROOT)/lib/main/CMSIS
 INCLUDE_DIRS    := $(SRC_DIR) \
@ -93,8 +95,9 @@ FATFS_DIR       = $(ROOT)/lib/main/FatFS
 FATFS_SRC       = $(notdir $(wildcard $(FATFS_DIR)/*.c))

 VPATH           := $(SRC_DIR):$(SRC_DIR)/startup
-VPATH 			:= $(VPATH):$(ROOT)/make/mcu
-VPATH 			:= $(VPATH):$(ROOT)/make
+VPATH 		:= $(VPATH):$(ROOT)/make/mcu
+VPATH 		:= $(VPATH):$(ROOT)/make
+VPATH 		:= $(VPATH):$(BL_SRC_DIR)

 CSOURCES        := $(shell find $(SRC_DIR) -name '*.c')

@ -102,6 +105,12 @@ CSOURCES        := $(shell find $(SRC_DIR) -name '*.c')
 include $(ROOT)/make/mcu/STM32.mk
 include $(ROOT)/make/mcu/$(TARGET_MCU_GROUP).mk

+BL_LD_SCRIPT := $(basename $(LD_SCRIPT))_bl.ld
+
+ifneq ($(FOR_BL),)
+    LD_SCRIPT := $(basename $(LD_SCRIPT))_for_bl.ld
+endif
+
 # Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
 ifeq ($(FLASH_SIZE),)
 ifneq ($(TARGET_FLASH),)
@ -159,6 +168,7 @@ include $(ROOT)/make/tools.mk
 CROSS_CC    = $(ARM_SDK_PREFIX)gcc
 OBJCOPY     = $(ARM_SDK_PREFIX)objcopy
 SIZE        = $(ARM_SDK_PREFIX)size
+COMBINE_TOOL := src/utils/combine_tool

 #
 # Tool options.
@ -209,6 +219,12 @@ CFLAGS      += $(ARCH_FLAGS) \
              -save-temps=obj \
              -MMD -MP

+BL_CFLAGS  = -DMSP_FIRMWARE_UPDATE -DBOOTLOADER
+
+ifneq ($(FOR_BL),)
+    CFLAGS += -DMSP_FIRMWARE_UPDATE
+endif
+
 ASFLAGS     = $(ARCH_FLAGS) \
              -x assembler-with-cpp \
              $(addprefix -I,$(INCLUDE_DIRS)) \
@ -232,6 +248,23 @@ LDFLAGS     = -lm \
              -Wl,--print-memory-usage \
              -T$(LD_SCRIPT)

+BL_LDFLAGS     = -lm \
+              -nostartfiles \
+              --specs=nano.specs \
+              -lc \
+              $(SYSLIB) \
+              $(ARCH_FLAGS) \
+              $(LTO_FLAGS) \
+              $(DEBUG_FLAGS) \
+              $(SEMIHOSTING_LDFLAGS) \
+              -static \
+              -Wl,-gc-sections,-Map,$(TARGET_BL_MAP) \
+              -Wl,-L$(LINKER_DIR) \
+              -Wl,--cref \
+              -Wl,--no-wchar-size-warning \
+              -Wl,--print-memory-usage \
+              -T$(BL_LD_SCRIPT)
+
 ###############################################################################
 # No user-serviceable parts below
 ###############################################################################
@ -246,30 +279,41 @@ CPPCHECK        = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
 #
 TARGET_BIN	:= $(BIN_DIR)/$(FORKNAME)_$(FC_VER)
 TARGET_BIN	:= $(TARGET_BIN)_$(TARGET)
+TARGET_BL_BIN	:= $(TARGET_BIN)_bl
 ifneq ($(BUILD_SUFFIX),)
    TARGET_BIN	:= $(TARGET_BIN)_$(BUILD_SUFFIX)
+    TARGET_BL_BIN := $(TARGET_BL_BIN)_$(BUILD_SUFFIX)
 endif
 TARGET_BIN	:= $(TARGET_BIN).bin
+TARGET_BL_BIN	:= $(TARGET_BL_BIN).bin
 TARGET_HEX	= $(TARGET_BIN:.bin=.hex)
+TARGET_BL_HEX	= $(TARGET_BL_BIN:.bin=.hex)
+TARGET_COMBINED_HEX = $(basename $(TARGET_HEX))_combined.hex

 TARGET_OBJ_DIR  = $(OBJECT_DIR)/$(TARGET)
+TARGET_BL_OBJ_DIR  = $(BL_OBJECT_DIR)/$(TARGET)
 TARGET_ELF      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
+TARGET_BL_ELF   = $(BL_OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
 TARGET_OBJS     = $(addsuffix .o,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(TARGET_SRC))))
+TARGET_BL_OBJS  = $(addsuffix .o,$(addprefix $(TARGET_BL_OBJ_DIR)/,$(basename $(TARGET_BL_SRC))))
 TARGET_DEPS     = $(addsuffix .d,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(TARGET_SRC))))
+TARGET_BL_DEPS  = $(addsuffix .d,$(addprefix $(TARGET_BL_OBJ_DIR)/,$(basename $(TARGET_BL_SRC))))
 TARGET_MAP      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map
+TARGET_BL_MAP   = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET)_bl.map


 CLEAN_ARTIFACTS := $(TARGET_BIN)
 CLEAN_ARTIFACTS += $(TARGET_HEX)
 CLEAN_ARTIFACTS += $(TARGET_ELF)
 CLEAN_ARTIFACTS += $(TARGET_OBJS) $(TARGET_MAP)
+CLEAN_ARTIFACTS += $(TARGET_BL_BIN) $(TARGET_BL_HEX) $(TARGET_BL_ELF) $(TARGET_BL_OBJS) $(TARGET_BL_MAP)

 include $(ROOT)/make/stamp.mk
 include $(ROOT)/make/settings.mk
 include $(ROOT)/make/svd.mk

 # Make sure build date and revision is updated on every incremental build
-$(TARGET_OBJ_DIR)/build/version.o : $(TARGET_SRC)
+$(TARGET_OBJ_DIR)/build/version.o $(TARGET_BL_OBJ_DIR)/build/version.o: $(TARGET_SRC)

 # CFLAGS used for ASM generation. These can't include the LTO related options
 # since they prevent proper ASM generation. Since $(LTO_FLAGS) includes the
@ -291,6 +335,17 @@ $(TARGET_ELF): $(TARGET_OBJS)
 	$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(LDFLAGS)
 	$(V0) $(SIZE) $(TARGET_ELF)

+$(TARGET_BL_HEX): $(TARGET_BL_ELF)
+	$(V0) $(OBJCOPY) -O ihex --set-start $(FLASH_ORIGIN) $< $@
+
+$(TARGET_BL_BIN): $(TARGET_BL_ELF)
+	$(V0) $(OBJCOPY) -O binary $< $@
+
+$(TARGET_BL_ELF): $(TARGET_BL_OBJS)
+	$(V1) echo Linking $(TARGET) bl
+	$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(BL_LDFLAGS)
+	$(V0) $(SIZE) $(TARGET_BL_ELF)
+
 OPTIMIZE_FLAG_SPEED := "-Os"
 OPTIMIZE_FLAG_SIZE := "-Os"
 OPTIMIZE_FLAG_NORMAL := "-Os"
@ -306,6 +361,11 @@ define compile_file
 	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
 endef

+define compile_bl_file
+	echo "%% $(1) $(2) $<" "$(STDOUT)" && \
+	$(CROSS_CC) -c -o $@ $(CFLAGS) $(BL_CFLAGS) $(2) $<
+endef
+
 # Compile
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
@ -323,13 +383,29 @@ ifeq ($(GENERATE_ASM), 1)
 	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
 endif

+$(TARGET_BL_OBJ_DIR)/%.o: %.c
+	$(V1) mkdir -p $(dir $@)
+
+	$(V1) $(if $(findstring $<,$(SIZE_OPTIMISED_SRC)), \
+		$(call compile_bl_file,(size),$(OPTIMIZE_FLAG_SIZE)) \
+	, \
+		$(if $(findstring $<,$(SPEED_OPTIMISED_SRC)), \
+			$(call compile_bl_file,(speed),$(OPTIMIZE_FLAG_SPEED)) \
+		, \
+			$(call compile_bl_file,(normal),$(OPTIMIZE_FLAG_NORMAL)) \
+		) \
+	)
+ifeq ($(GENERATE_ASM), 1)
+	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $(BL_CFLAGS) $<
+endif
+
 # Assemble
-$(TARGET_OBJ_DIR)/%.o: %.s
+$(TARGET_OBJ_DIR)/%.o $(TARGET_BL_OBJ_DIR)/%.o: %.s
 	$(V1) mkdir -p $(dir $@)
 	$(V1) echo %% $(notdir $<) "$(STDOUT)"
 	$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<

-$(TARGET_OBJ_DIR)/%.o: %.S
+$(TARGET_OBJ_DIR)/%.o $(TARGET_BL_OBJ_DIR)/%.o: %.S
 	$(V1) mkdir -p $(dir $@)
 	$(V1) echo %% $(notdir $<) "$(STDOUT)"
 	$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<
@ -355,6 +431,31 @@ $(VALID_TARGETS):
 	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$@ && \
 	echo "Building $@ succeeded."

+$(VALID_BL_TARGETS):
+	$(V0) echo "" && \
+	echo "Building $@" && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$(@:%_bl=%) bl_binary bl_hex && \
+	echo "Building $(@:%_bl=%) bl succeeded."
+
+$(VALID_TARGETS_FOR_BL):
+	$(V0) echo "" && \
+	echo "Building $@" && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$(@:%_for_bl=%) FOR_BL=1 binary hex && \
+	echo "Building $(@:%_for_bl=%) for bl succeeded."
+
+$(VALID_TARGETS_WITH_BL):
+	$(V0) echo "" && \
+	echo "Building $@ with bl" && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) TARGET=$(@:%_with_bl=%) combined_hex && \
+	echo "Building $(@:%_with_bl=%) with bl succeeded."
+
+combined_hex:
+	$(V1) echo "Building combined BL+MAIN hex" && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$(TARGET) bl_binary && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$(TARGET) binary FOR_BL=1 && \
+	echo "Combining MAIN+BL in $(TARGET_COMBINED_HEX)" && \
+	$(COMBINE_TOOL) $(TARGET_BL_BIN) $(TARGET_BIN) $(TARGET_COMBINED_HEX)
+
 ## clean             : clean up all temporary / machine-generated files
 clean:
 	$(V0) echo "Cleaning $(TARGET)"
@ -398,6 +499,9 @@ st-flash: $(TARGET_BIN)
 elf:	$(TARGET_ELF)
 binary: $(TARGET_BIN)
 hex:    $(TARGET_HEX)
+bl_elf:	$(TARGET_BL_ELF)
+bl_binary: $(TARGET_BL_BIN)
+bl_hex:    $(TARGET_BL_HEX)

 unbrick_$(TARGET): $(TARGET_HEX)
 	$(V0) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
--- a/make/gdb.mk
+++ b/make/gdb.mk
@ -12,3 +12,6 @@ endif

 gdb-openocd: $(TARGET_ELF)
 	$(GDB) $< -ex "target remote $(GDB_REMOTE)" $(GDB_OPENOCD_INIT_CMDS)
+
+gdb-openocd-bl: $(TARGET_BL_ELF)
+	$(GDB) $< -ex "target remote $(GDB_REMOTE)" $(GDB_OPENOCD_INIT_CMDS)
--- a/make/mcu/STM32F3.mk
+++ b/make/mcu/STM32F3.mk
@ -3,7 +3,7 @@
 #

 ifeq ($(OPBL),yes)
-LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k_opbl.ld
+LD_SCRIPT = $(LINKER_DIR)/stm32_flash_F303_$(FLASH_SIZE)k_opbl.ld
 endif

 TARGET_FLASH   := 256
@ -47,7 +47,7 @@ INCLUDE_DIRS    := $(INCLUDE_DIRS) \
 VPATH           := $(VPATH):$(FATFS_DIR)
 endif

-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F303_$(FLASH_SIZE)k.ld

 ARCH_FLAGS      = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion
 DEVICE_FLAGS    = -DSTM32F303xC -DSTM32F303
@ -74,7 +74,9 @@ MCU_COMMON_SRC = \
            drivers/serial_uart_stm32f30x.c \
            drivers/system_stm32f30x.c \
            drivers/timer_impl_stdperiph.c \
-            drivers/timer_stm32f30x.c
+            drivers/timer_stm32f30x.c \
+	    src/main/drivers/bus_spi.c
+

 DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP
 DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4
--- a/make/mcu/STM32F4.mk
+++ b/make/mcu/STM32F4.mk
@ -141,23 +141,23 @@ endif
 ARCH_FLAGS      = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion

 ifeq ($(TARGET),$(filter $(TARGET),$(F411_TARGETS)))
-DEVICE_FLAGS    = -DSTM32F411xE
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f411.ld
-STARTUP_SRC     = startup_stm32f411xe.s
+    DEVICE_FLAGS    := -DSTM32F411xE
+    LD_SCRIPT       := $(LINKER_DIR)/stm32_flash_F411.ld
+    STARTUP_SRC     := startup_stm32f411xe.s
 else ifeq ($(TARGET),$(filter $(TARGET),$(F405_TARGETS)))
-DEVICE_FLAGS    = -DSTM32F40_41xxx -DSTM32F405xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f405.ld
-STARTUP_SRC     = startup_stm32f40xx.s
+    DEVICE_FLAGS    := -DSTM32F40_41xxx -DSTM32F405xx
+    LD_SCRIPT       := $(LINKER_DIR)/stm32_flash_F405.ld
+    STARTUP_SRC     := startup_stm32f40xx.s
 else ifeq ($(TARGET),$(filter $(TARGET),$(F446_TARGETS)))
-DEVICE_FLAGS    = -DSTM32F446xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f446.ld
-STARTUP_SRC     = startup_stm32f446xx.s
+    DEVICE_FLAGS    := -DSTM32F446xx
+    LD_SCRIPT       := $(LINKER_DIR)/stm32_flash_F446.ld
+    STARTUP_SRC     := startup_stm32f446xx.s
 else ifeq ($(TARGET),$(filter $(TARGET),$(F427_TARGETS)))
-DEVICE_FLAGS    = -DSTM32F427_437xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f427.ld
-STARTUP_SRC     = startup_stm32f427xx.s
+    DEVICE_FLAGS    := -DSTM32F427_437xx
+    LD_SCRIPT       := $(LINKER_DIR)/stm32_flash_F427.ld
+    STARTUP_SRC     := startup_stm32f427xx.s
 else
-$(error Unknown MCU for F4 target)
+    $(error Unknown MCU for F4 target)
 endif

 DEVICE_FLAGS    += -DHSE_VALUE=$(HSE_VALUE)
@ -169,6 +169,7 @@ MCU_COMMON_SRC = \
            drivers/adc_stm32f4xx.c \
            drivers/adc_stm32f4xx.c \
            drivers/bus_i2c_stm32f40x.c \
+            drivers/bus_spi.c \
            drivers/serial_softserial.c \
            drivers/serial_uart_stm32f4xx.c \
            drivers/system_stm32f4xx.c \
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@ -121,25 +121,30 @@ endif
 ARCH_FLAGS      = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion

 DEVICE_FLAGS    = -DUSE_HAL_DRIVER -DUSE_FULL_LL_DRIVER
-ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XI_TARGETS)))
-DEVICE_FLAGS   += -DSTM32F765xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f765.ld
+ifeq ($(TARGET),$(filter $(TARGET),$(F765XI_TARGETS)))
+DEVICE_FLAGS   += -DSTM32F765xx -DSTM32F765xI
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F765xI.ld
 STARTUP_SRC     = startup_stm32f765xx.s
 TARGET_FLASH   := 2048
-else ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS)))
-DEVICE_FLAGS   += -DSTM32F745xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f745.ld
+else ifeq ($(TARGET),$(filter $(TARGET),$(F765XG_TARGETS)))
+DEVICE_FLAGS   += -DSTM32F765xx -DSTM32F765xG
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F7x5xG.ld
+STARTUP_SRC     = startup_stm32f765xx.s
+TARGET_FLASH   := 1024
+else ifeq ($(TARGET),$(filter $(TARGET),$(F745XG_TARGETS)))
+DEVICE_FLAGS   += -DSTM32F745xx -DSTM32F745xG
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F7x5xG.ld
 STARTUP_SRC     = startup_stm32f745xx.s
-TARGET_FLASH   := 2048
+TARGET_FLASH   := 1024
 else ifeq ($(TARGET),$(filter $(TARGET),$(F7X6XG_TARGETS)))
 DEVICE_FLAGS   += -DSTM32F746xx
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f746.ld
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F746.ld
 STARTUP_SRC     = startup_stm32f746xx.s
 TARGET_FLASH   := 2048
 else ifeq ($(TARGET),$(filter $(TARGET),$(F7X2RE_TARGETS)))
 DEVICE_FLAGS   += -DSTM32F722xx
 ifndef LD_SCRIPT
-LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_f722.ld
+LD_SCRIPT       = $(LINKER_DIR)/stm32_flash_F7x2.ld
 endif
 STARTUP_SRC     = startup_stm32f722xx.s
 TARGET_FLASH   := 512
--- a/make/source.mk
+++ b/make/source.mk
@ -1,7 +1,25 @@
 COMMON_SRC = \
+            common/log.c \
+            common/printf.c \
+            common/string_light.c \
+            common/typeconversion.c \
+            drivers/bus.c \
+            drivers/bus_busdev_i2c.c \
+            drivers/bus_busdev_spi.c \
+            drivers/bus_i2c_soft.c \
+            drivers/io.c \
+            drivers/persistent.c \
+            drivers/light_led.c \
+            drivers/rcc.c \
+            drivers/serial.c \
+            drivers/system.c \
+            drivers/time.c \
+            fc/firmware_update_common.c \
+            target/common_hardware.c
+
+MAIN_SRC = \
            $(TARGET_DIR_SRC) \
            main.c \
-            target/common_hardware.c \
            build/assert.c \
            build/build_config.c \
            build/debug.c \
@ -13,15 +31,11 @@ COMMON_SRC = \
            common/encoding.c \
            common/filter.c \
            common/gps_conversion.c \
-            common/log.c \
            common/maths.c \
            common/memory.c \
            common/olc.c \
-            common/printf.c \
            common/streambuf.c \
-            common/string_light.c \
            common/time.c \
-            common/typeconversion.c \
            common/uvarint.c \
            programming/logic_condition.c \
            programming/global_functions.c \
@ -34,21 +48,12 @@ COMMON_SRC = \
            config/general_settings.c \
            drivers/adc.c \
            drivers/buf_writer.c \
-            drivers/bus.c \
-            drivers/bus_busdev_i2c.c \
-            drivers/bus_busdev_spi.c \
-            drivers/bus_i2c_soft.c \
-            drivers/bus_spi.c \
            drivers/display.c \
            drivers/display_canvas.c \
            drivers/display_font_metadata.c \
            drivers/exti.c \
-            drivers/io.c \
            drivers/io_pca9685.c \
-            drivers/irlock.c \
-            drivers/light_led.c \
            drivers/osd.c \
-            drivers/persistent.c \
            drivers/resource.c \
            drivers/rx_nrf24l01.c \
            drivers/rx_spi.c \
@ -59,14 +64,10 @@ COMMON_SRC = \
            drivers/pwm_mapping.c \
            drivers/pwm_output.c \
            drivers/pinio.c \
-            drivers/rcc.c \
            drivers/rx_pwm.c \
-            drivers/serial.c \
            drivers/serial_uart.c \
            drivers/sound_beeper.c \
            drivers/stack_check.c \
-            drivers/system.c \
-            drivers/time.c \
            drivers/timer.c \
            drivers/usb_msc.c \
            drivers/lights_io.c \
@ -85,6 +86,7 @@ COMMON_SRC = \
            fc/fc_hardfaults.c \
            fc/fc_msp.c \
            fc/fc_msp_box.c \
+            fc/firmware_update.c \
            fc/rc_smoothing.c \
            fc/rc_adjustments.c \
            fc/rc_controls.c \
@ -235,25 +237,31 @@ COMMON_SRC = \
            io/vtx_ffpv24g.c \
            io/vtx_control.c

+BL_SRC = \
+	    bl_main.c
+
 COMMON_DEVICE_SRC = \
            $(CMSIS_SRC) \
            $(DEVICE_STDPERIPH_SRC)

-TARGET_SRC := $(STARTUP_SRC) $(COMMON_DEVICE_SRC) $(COMMON_SRC) $(MCU_COMMON_SRC) $(TARGET_SRC)
+TARGET_SRC := $(STARTUP_SRC) $(COMMON_DEVICE_SRC) $(MAIN_SRC) $(COMMON_SRC) $(MCU_COMMON_SRC) $(TARGET_SRC)
+TARGET_BL_SRC := $(STARTUP_SRC) $(COMMON_DEVICE_SRC) $(BL_SRC) $(COMMON_SRC) $(MCU_COMMON_SRC)

 #excludes
 TARGET_SRC   := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC))

 ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
-TARGET_SRC += \
+FLASH_SRC += \
            drivers/flash.c \
            drivers/flash_m25p16.c \
            io/flashfs.c \
            $(MSC_SRC)
+TARGET_SRC += $(FLASH_SRC)
+TARGET_BL_SRC += $(FLASH_SRC)
 endif

 ifneq ($(filter SDCARD,$(FEATURES)),)
-TARGET_SRC += \
+SDCARD_SRC += \
            drivers/sdcard/sdcard.c \
            drivers/sdcard/sdcard_spi.c \
            drivers/sdcard/sdcard_sdio.c \
@ -261,6 +269,8 @@ TARGET_SRC += \
            io/asyncfatfs/asyncfatfs.c \
            io/asyncfatfs/fat_standard.c \
            $(MSC_SRC)
+TARGET_SRC += $(SDCARD_SRC)
+TARGET_BL_SRC += $(SDCARD_SRC)
 endif

 ifneq ($(filter VCP,$(FEATURES)),)
--- a/make/targets.mk
+++ b/make/targets.mk
@ -5,6 +5,10 @@ VALID_TARGETS  := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS)))
 VALID_TARGETS  := $(VALID_TARGETS) $(ALT_TARGETS)
 VALID_TARGETS  := $(sort $(VALID_TARGETS))

+VALID_BL_TARGETS := $(addsuffix _bl,$(VALID_TARGETS))
+VALID_TARGETS_FOR_BL := $(addsuffix _for_bl,$(VALID_TARGETS))
+VALID_TARGETS_WITH_BL := $(addsuffix _with_bl,$(VALID_TARGETS))
+
 CLEAN_TARGETS   = $(addprefix clean_,$(VALID_TARGETS) )
 TARGETS_CLEAN   = $(addsuffix _clean,$(VALID_TARGETS) )
 STFLASH_TARGETS = $(addprefix st-flash_,$(VALID_TARGETS) )
@ -20,7 +24,7 @@ endif
 -include $(ROOT)/src/main/target/$(BASE_TARGET)/target.mk

 F4_TARGETS      = $(F405_TARGETS) $(F411_TARGETS) $(F427_TARGETS) $(F446_TARGETS)
-F7_TARGETS      = $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS)
+F7_TARGETS      = $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) $(F745XG_TARGETS) $(F765XG_TARGETS) $(F765XI_TARGETS) $(F7X6XG_TARGETS)

 ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
 $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
@ -31,34 +35,37 @@ $(error Target '$(TARGET)' has not specified a valid STM group, must be one of F
 endif

 ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
-TARGET_MCU 			:= STM32F303
+TARGET_MCU 		:= STM32F303
 TARGET_MCU_GROUP 	:= STM32F3
 else ifeq ($(TARGET),$(filter $(TARGET), $(F405_TARGETS)))
-TARGET_MCU			:= STM32F405
+TARGET_MCU		:= STM32F405
 TARGET_MCU_GROUP 	:= STM32F4
 else ifeq ($(TARGET),$(filter $(TARGET), $(F411_TARGETS)))
-TARGET_MCU			:= STM32F411
+TARGET_MCU		:= STM32F411
 TARGET_MCU_GROUP 	:= STM32F4
 else ifeq ($(TARGET),$(filter $(TARGET), $(F427_TARGETS)))
-TARGET_MCU			:= STM32F427
+TARGET_MCU		:= STM32F427
 TARGET_MCU_GROUP 	:= STM32F4
 else ifeq ($(TARGET),$(filter $(TARGET), $(F446_TARGETS)))
-TARGET_MCU			:= STM32F446
+TARGET_MCU		:= STM32F446
 TARGET_MCU_GROUP 	:= STM32F4
 else ifeq ($(TARGET),$(filter $(TARGET), $(F7X2RE_TARGETS)))
-TARGET_MCU			:= STM32F7X2RE
+TARGET_MCU		:= STM32F7X2RE
 TARGET_MCU_GROUP 	:= STM32F7
 else ifeq ($(TARGET),$(filter $(TARGET), $(F7X5XE_TARGETS)))
-TARGET_MCU			:= STM32F7X5XE
+TARGET_MCU		:= STM32F7X5XE
 TARGET_MCU_GROUP 	:= STM32F7
-else ifeq ($(TARGET),$(filter $(TARGET), $(F7X5XG_TARGETS)))
-TARGET_MCU			:= STM32F7X5XG
+else ifeq ($(TARGET),$(filter $(TARGET), $(F745XG_TARGETS)))
+TARGET_MCU		:= STM32F745XG
 TARGET_MCU_GROUP 	:= STM32F7
-else ifeq ($(TARGET),$(filter $(TARGET), $(F7X5XI_TARGETS)))
-TARGET_MCU			:= STM32F7X5XI
+else ifeq ($(TARGET),$(filter $(TARGET), $(F765XG_TARGETS)))
+TARGET_MCU		:= STM32F765XG
+TARGET_MCU_GROUP 	:= STM32F7
+else ifeq ($(TARGET),$(filter $(TARGET), $(F765XI_TARGETS)))
+TARGET_MCU		:= STM32F765XI
 TARGET_MCU_GROUP 	:= STM32F7
 else ifeq ($(TARGET),$(filter $(TARGET), $(F7X6XG_TARGETS)))
-TARGET_MCU			:= STM32F7X6XG
+TARGET_MCU		:= STM32F7X6XG
 TARGET_MCU_GROUP 	:= STM32F7
 else
 $(error Unknown target MCU specified.)
--- a/src/bl/bl_main.c
+++ b/src/bl/bl_main.c
@ -0,0 +1,489 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute this software
+ * and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav is distributed in the hope that they will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+/*#include "common/log.h"*/
+#include "common/maths.h"
+/*#include "common/printf.h"*/
+
+#include "drivers/bus.h"
+#include "drivers/flash.h"
+#include "drivers/persistent.h"
+#include "drivers/io.h"
+#include "drivers/light_led.h"
+#include "drivers/sdcard/sdcard.h"
+#include "drivers/system.h"
+#include "drivers/time.h"
+
+#include "fc/firmware_update_common.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
+
+
+#if !(defined(STM32F405xx) || defined(STM32F722xx) || defined(STM32F765XG) || defined(STM32F7X5XI))
+#error Unsupported MCU
+#endif
+
+#if !(defined(USE_FLASHFS) || defined(USE_SDCARD))
+#error No storage backend available
+#endif
+
+
+typedef struct {
+    uint16_t size;
+    uint8_t count;
+} flashSectorDef_t;
+
+#if defined(STM32F405xx)
+#define SECTOR_COUNT 12
+flashSectorDef_t flashSectors[] = { { 16, 4 }, { 64, 1 }, { 128, 7 }, { 0, 0 } };
+
+#elif defined(STM32F722xx)
+#define SECTOR_COUNT 8
+flashSectorDef_t flashSectors[] = { { 16, 4 }, { 64, 1 }, { 128, 3 }, { 0, 0 } };
+
+#elif defined(STM32F765XG)
+#define SECTOR_COUNT 8
+flashSectorDef_t flashSectors[] = { { 32, 4 }, { 128, 1 }, { 256, 3 }, { 0, 0 } };
+
+#elif defined(STM32F7X5XI)
+#define SECTOR_COUNT 8
+flashSectorDef_t flashSectors[] = { { 32, 4 }, { 128, 1 }, { 256, 7 }, { 0, 0 } };
+
+#endif
+
+#if defined(STM32F4)
+    #define flashLock() FLASH_Lock()
+    #define flashUnlock() FLASH_Unlock()
+#elif defined(STM32F7)
+    #define flashLock() HAL_FLASH_Lock()
+    #define flashUnlock() HAL_FLASH_Unlock()
+#endif
+
+static bool dataBackEndInitialized = false;
+
+#ifdef USE_SDCARD
+static afatfsFilePtr_t flashDataFile = NULL;
+
+static void flashDataFileOpenCallback(afatfsFilePtr_t file)
+{
+    flashDataFile = file;
+}
+#endif
+
+static void init(void)
+{
+#ifdef USE_HAL_DRIVER
+    HAL_Init();
+#endif
+
+    /*printfSupportInit();*/
+
+    systemInit();
+
+    __enable_irq();
+
+    // initialize IO (needed for all IO operations)
+    IOInitGlobal();
+
+    ledInit(false);
+
+    LED0_OFF;
+    LED1_OFF;
+
+    for(int x = 0; x < 10; ++x) {
+        LED0_TOGGLE;
+        LED1_TOGGLE;
+        delay(200);
+    }
+
+}
+
+static bool dataBackendInit(void)
+{
+    if (dataBackEndInitialized) return true;
+
+    busInit();
+
+#if defined(USE_SDCARD)
+    sdcardInsertionDetectInit();
+    sdcard_init();
+    afatfs_init();
+
+    afatfsError_e sdError = afatfs_getLastError();
+    while ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) && ((sdError = afatfs_getLastError()) == AFATFS_ERROR_NONE)) {
+        afatfs_poll();
+    }
+
+    if (sdError != AFATFS_ERROR_NONE) {
+        return false;
+    }
+
+#elif defined(USE_FLASHFS) && defined(USE_FLASH_M25P16)
+    if (!flashInit()) {
+        return false;
+    }
+
+#endif
+
+    dataBackEndInitialized = true;
+    return true;
+}
+
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
+static void do_jump(uint32_t address)
+{
+#ifdef STM32F7
+    __DSB();
+    __DMB();
+    __ISB();
+#endif
+
+    volatile isrVector_t *bootloaderVector = (isrVector_t *)address;
+    __set_MSP(bootloaderVector->stackEnd);
+    bootloaderVector->resetHandler();
+}
+
+void bootloader_jump_to_app(void)
+{
+    FLASH->ACR &= (~FLASH_ACR_PRFTEN);
+
+#if defined(STM32F4)
+    RCC_APB1PeriphResetCmd(~0, DISABLE);
+    RCC_APB2PeriphResetCmd(~0, DISABLE);
+#elif defined(STM32F7)
+    RCC->APB1ENR = 0;
+    RCC->APB1LPENR = 0;
+    RCC->APB2ENR = 0;
+    RCC->APB2LPENR = 0;
+#endif
+
+    __disable_irq();
+
+    do_jump(FIRMWARE_START_ADDRESS);
+}
+
+// find sector specified address is in (assume than the config section doesn't span more than 1 sector)
+// returns -1 if not found
+int8_t mcuFlashAddressSectorIndex(uint32_t address)
+{
+    uint32_t sectorStartAddress = FLASH_START_ADDRESS;
+    uint8_t sector = 0;
+    flashSectorDef_t *sectorDef = flashSectors;
+
+    do {
+        for (unsigned j = 0; j < sectorDef->count; ++j) {
+            uint32_t sectorEndAddress = sectorStartAddress + sectorDef->size * 1024;
+            /*if ((CONFIG_START_ADDRESS >= sectorStartAddress) && (CONFIG_START_ADDRESS < sectorEndAddress) && (CONFIG_END_ADDRESS <= sectorEndAddress)) {*/
+            if ((address >= sectorStartAddress) && (address < sectorEndAddress)) {
+                return sector;
+            }
+            sectorStartAddress = sectorEndAddress;
+            sector += 1;
+        }
+        sectorDef += 1;
+    } while (sectorDef->count);
+
+    return -1;
+}
+
+uint32_t mcuFlashSectorID(uint8_t sectorIndex)
+{
+#if defined(STM32F4)
+    if (sectorIndex < 12) {
+        return sectorIndex * 8;
+    } else {
+        return 0x80 + (sectorIndex - 12) * 8;
+    }
+#elif defined(STM32F7)
+    return sectorIndex;
+#endif
+}
+
+bool mcuFlashSectorErase(uint8_t sectorIndex)
+{
+#if defined(STM32F4)
+    return (FLASH_EraseSector(mcuFlashSectorID(sectorIndex), VoltageRange_3) == FLASH_COMPLETE);
+#elif defined(STM32F7)
+    FLASH_EraseInitTypeDef EraseInitStruct = {
+        .TypeErase     = FLASH_TYPEERASE_SECTORS,
+        .VoltageRange  = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
+        .NbSectors     = 1
+    };
+    EraseInitStruct.Sector = mcuFlashSectorID(sectorIndex);
+    uint32_t SECTORError;
+    const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+    return (status == HAL_OK);
+#else
+#error Unsupported MCU
+#endif
+}
+
+bool mcuFirmwareFlashErase(bool includeConfig)
+{
+    int8_t firmwareSectorIndex = mcuFlashAddressSectorIndex(FIRMWARE_START_ADDRESS);
+    int8_t configSectorIndex = mcuFlashAddressSectorIndex(CONFIG_START_ADDRESS);
+
+    if ((firmwareSectorIndex == -1) || (configSectorIndex == -1)) {
+        return false;
+    }
+
+    LED0_OFF;
+    LED1_ON;
+    for (unsigned i = firmwareSectorIndex; i < SECTOR_COUNT; ++i) {
+        if (includeConfig || (!includeConfig && (i != (uint8_t)configSectorIndex))) {
+            if (!mcuFlashSectorErase(i)) {
+                LED1_OFF;
+                return false;
+            }
+            LED0_TOGGLE;
+        }
+    }
+
+    LED1_OFF;
+    return true;
+}
+
+bool mcuFlashWriteWord(uint32_t address, uint32_t data)
+{
+#if defined(STM32F4)
+    const FLASH_Status status = FLASH_ProgramWord(address, data);
+    return (status == FLASH_COMPLETE);
+#elif defined(STM32F7)
+    const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, (uint64_t)data);
+    return (status == HAL_OK);
+#else
+#error Unsupported MCU
+#endif
+}
+
+typedef enum {
+    FLASH_OPERATION_UPDATE,
+    FLASH_OPERATION_ROLLBACK
+} flashOperation_e;
+
+#if defined(USE_SDCARD)
+bool afatfs_fseekWorkAround(afatfsFilePtr_t file, uint32_t forward)
+{
+    uint8_t buffer[256];
+    while (forward > 0) {
+        uint32_t bytesRead = afatfs_freadSync(file, buffer, MIN(forward, (uint16_t)256));
+        if (bytesRead < 256) {
+            return false;
+        }
+        forward -= bytesRead;
+    }
+    return true;
+}
+#endif
+
+bool flash(flashOperation_e flashOperation)
+{
+    if (!dataBackendInit()) {
+        return false;
+    }
+
+    uint32_t buffer;
+    uint32_t flashDstAddress = FIRMWARE_START_ADDRESS + sizeof(buffer); // Write the first bytes last so that we can check that the firmware has been written fully
+
+#if defined(USE_SDCARD)
+    const char * const flashDataFileName = (flashOperation == FLASH_OPERATION_UPDATE ? FIRMWARE_UPDATE_FIRMWARE_FILENAME : FIRMWARE_UPDATE_BACKUP_FILENAME);
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY)
+            || !afatfs_fopen(flashDataFileName, "r", flashDataFileOpenCallback)
+            || (afatfs_fileSize(flashDataFile) > AVAILABLE_FIRMWARE_SPACE)) {
+        return false;
+    }
+
+#elif defined(USE_FLASHFS)
+    flashPartitionType_e srcFlashPartitionType = (flashOperation == FLASH_OPERATION_UPDATE ? FLASH_PARTITION_TYPE_UPDATE_FIRMWARE : FLASH_PARTITION_TYPE_FULL_BACKUP);
+    flashPartition_t *flashDataPartition = flashPartitionFindByType(srcFlashPartitionType);
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    uint32_t flashDataPartitionSize = (flashDataPartition->endSector - flashDataPartition->startSector + 1) * (flashGeometry->sectorSize * flashGeometry->pageSize);
+    firmwareUpdateMetadata_t updateMetadata;
+
+    if (!flashDataPartition || !firmwareUpdateMetadataRead(&updateMetadata)
+            || (updateMetadata.firmwareSize > flashDataPartitionSize)
+            || (updateMetadata.firmwareSize > AVAILABLE_FIRMWARE_SPACE)) {
+        return false;
+    }
+
+#endif
+
+    flashUnlock();
+    bool flashSucceeded = false;
+
+    if (!mcuFirmwareFlashErase(flashOperation != FLASH_OPERATION_UPDATE)) goto flashFailed;
+
+    LED0_OFF;
+    LED1_OFF;
+
+    uint32_t counter = 0;
+
+#if defined(USE_SDCARD)
+
+    if (afatfs_fseekSync(flashDataFile, sizeof(buffer), AFATFS_SEEK_SET) == AFATFS_OPERATION_FAILURE) {
+        goto flashFailed;
+    }
+
+    while (!afatfs_feof(flashDataFile)) {
+
+        if ((flashOperation == FLASH_OPERATION_UPDATE) && (flashDstAddress == CONFIG_START_ADDRESS)) {
+            // skip config region
+            const uint32_t configSize = CONFIG_END_ADDRESS - CONFIG_START_ADDRESS;
+            /*if (afatfs_fseekSync(flashDataFile, configSize, AFATFS_SEEK_CUR) == AFATFS_OPERATION_FAILURE) {*/
+            if (!afatfs_fseekWorkAround(flashDataFile, configSize)) { // workaround fseek bug, should be ^^^^^^^^^
+                goto flashFailed;
+            }
+            flashDstAddress += configSize;
+        }
+
+        afatfs_freadSync(flashDataFile, (uint8_t *)&buffer, sizeof(buffer));
+
+        if (!mcuFlashWriteWord(flashDstAddress, buffer)) {
+            goto flashFailed;
+        }
+
+        flashDstAddress += sizeof(buffer);
+
+        if (++counter % (10*1024/4) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+    if ((afatfs_fseekSync(flashDataFile, 0, AFATFS_SEEK_SET) == AFATFS_OPERATION_FAILURE)
+            || (afatfs_freadSync(flashDataFile, (uint8_t *)&buffer, sizeof(buffer)) != sizeof(buffer))) {
+        goto flashFailed;
+    }
+
+#elif defined(USE_FLASHFS)
+    const uint32_t flashSrcStartAddress = flashDataPartition->startSector * flashGeometry->sectorSize;
+    uint32_t flashSrcAddress = flashSrcStartAddress + sizeof(buffer);
+    const uint32_t flashDstEndAddress = (flashOperation == FLASH_OPERATION_UPDATE ? FIRMWARE_START_ADDRESS + updateMetadata.firmwareSize : FLASH_END);
+
+    while (flashDstAddress < flashDstEndAddress) {
+
+        if ((flashOperation == FLASH_OPERATION_UPDATE) && (flashDstAddress == CONFIG_START_ADDRESS)) {
+            // skip config region
+            const uint32_t configSize = CONFIG_END_ADDRESS - CONFIG_START_ADDRESS;
+            flashSrcAddress += configSize;
+            flashDstAddress += configSize;
+
+            if (flashDstAddress >= flashDstEndAddress) {
+                goto flashFailed;
+            }
+        }
+
+        flashReadBytes(flashSrcAddress, (uint8_t*)&buffer, sizeof(buffer));
+        if (!mcuFlashWriteWord(flashDstAddress, buffer)) {
+            goto flashFailed;
+        }
+
+        flashSrcAddress += sizeof(buffer);
+        flashDstAddress += sizeof(buffer);
+
+        if (++counter % (10*1024/4) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+    flashReadBytes(flashSrcStartAddress, (uint8_t*)&buffer, sizeof(buffer));
+
+#endif
+
+    if (!mcuFlashWriteWord(FIRMWARE_START_ADDRESS, buffer)) {
+        goto flashFailed;
+    }
+
+    flashSucceeded = true;
+
+flashFailed:
+
+    flashLock();
+
+    LED0_OFF;
+    LED1_OFF;
+
+    return flashSucceeded;
+}
+
+#if defined(USE_FLASHFS)
+bool dataflashChipEraseUpdatePartition(void)
+{
+    flashPartition_t *flashDataPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_UPDATE_FIRMWARE);
+    if (!flashDataPartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+
+    LED0_OFF;
+
+    for (unsigned i = flashDataPartition->startSector; i <= flashDataPartition->endSector; i++) {
+        uint32_t flashAddress = flashGeometry->sectorSize * i;
+        flashEraseSector(flashAddress);
+        flashWaitForReady(1000);
+        LED0_TOGGLE;
+    }
+
+    LED0_OFF;
+
+    return true;
+}
+#endif
+
+int main(void)
+{
+    init();
+
+    uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
+    if ((bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_UPDATE) || (bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_ROLLBACK)) {
+        flashOperation_e flashOperation = (bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_UPDATE ? FLASH_OPERATION_UPDATE : FLASH_OPERATION_ROLLBACK);
+        const bool success = flash(flashOperation);
+        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, success ? RESET_BOOTLOADER_FIRMWARE_UPDATE_SUCCESS : RESET_BOOTLOADER_FIRMWARE_UPDATE_FAILED);
+    } else if (*(uint32_t*)FIRMWARE_START_ADDRESS == 0xFFFFFFFF) {
+        if (!flash(FLASH_OPERATION_ROLLBACK)) {
+            LED0_OFF;
+            LED1_OFF;
+
+            while (true) {
+                LED0_TOGGLE;
+                LED1_TOGGLE;
+                delay(2000);
+            }
+        }
+    }
+
+    bootloader_jump_to_app();
+
+    return 0;
+}
--- a/src/main/drivers/flash.c
+++ b/src/main/drivers/flash.c
@ -30,6 +30,9 @@

 #include "flash.h"
 #include "flash_m25p16.h"
+
+#include "common/time.h"
+
 #include "drivers/bus_spi.h"
 #include "drivers/io.h"
 #include "drivers/time.h"
@ -37,6 +40,7 @@
 static flashPartitionTable_t flashPartitionTable;
 static int flashPartitions = 0;

+
 #ifdef USE_SPI
 static bool flashSpiInit(void)
 {
@ -64,7 +68,7 @@ bool flashIsReady(void)
    return false;
 }

-bool flashWaitForReady(uint32_t timeoutMillis)
+bool flashWaitForReady(timeMs_t timeoutMillis)
 {
 #ifdef USE_FLASH_M25P16
    return m25p16_waitForReady(timeoutMillis);
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@ -22,7 +22,7 @@

 #include <stdint.h>

-//#include "drivers/io.h"
+#include "common/time.h"

 //#ifdef USE_FLASHFS

@ -39,7 +39,7 @@ typedef struct flashGeometry_s {
 bool flashInit(void);

 bool flashIsReady(void);
-bool flashWaitForReady(uint32_t timeoutMillis);
+bool flashWaitForReady(timeMs_t timeoutMillis);
 void flashEraseSector(uint32_t address);
 void flashEraseCompletely(void);
 #if 0
--- a/src/main/drivers/persistent.h
+++ b/src/main/drivers/persistent.h
@ -35,8 +35,12 @@ typedef enum {

 // Values for PERSISTENT_OBJECT_RESET_REASON
 #define RESET_NONE                                      0
-#define RESET_BOOTLOADER_REQUEST_ROM                    1
+#define RESET_BOOTLOADER_REQUEST_ROM                    1  // DFU request
 #define RESET_MSC_REQUEST                               2  // MSC invocation was requested
+#define RESET_BOOTLOADER_FIRMWARE_UPDATE                3  // Bootloader request to flash stored firmware update
+#define RESET_BOOTLOADER_FIRMWARE_ROLLBACK              4  // Bootloader request to rollback to stored firmware and config backup
+#define RESET_BOOTLOADER_FIRMWARE_UPDATE_SUCCESS        5
+#define RESET_BOOTLOADER_FIRMWARE_UPDATE_FAILED         6

 void persistentObjectInit(void);
 uint32_t persistentObjectRead(persistentObjectId_e id);
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@ -85,10 +85,15 @@ void systemReset(void)
    NVIC_SystemReset();
 }

+void systemResetRequest(uint32_t requestId)
+{
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, requestId);
+    systemReset();
+}
+
 void systemResetToBootloader(void)
 {
-    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
-    systemReset();
+    systemResetRequest(RESET_BOOTLOADER_REQUEST_ROM);
 }

 typedef void resetHandler_t(void);
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@ -38,6 +38,7 @@ void failureMode(failureMode_e mode);

 // bootloader/IAP
 void systemReset(void);
+void systemResetRequest(uint32_t requestId);
 void systemResetToBootloader(void);
 uint32_t systemBootloaderAddress(void);
 bool isMPUSoftReset(void);
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@ -28,6 +28,8 @@

 #include "drivers/exti.h"

+#include "target/system.h"
+

 void SetSysClock(void);

@ -160,7 +162,6 @@ void systemInit(void)
    cachedRccCsrValue = RCC->CSR;

    /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
-    extern void *isr_vector_table_base;
    NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
    RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE);

--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -3462,11 +3462,7 @@ static void cliMsc(char *cmdline)
        delay(1000);
        waitForSerialPortToFinishTransmitting(cliPort);
        stopPwmAllMotors();
-
-        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST);
-
-        __disable_irq();
-        NVIC_SystemReset();
+        systemResetRequest(RESET_MSC_REQUEST);
    } else {
        cliPrint("\r\nStorage not present or failed to initialize!");
        bufWriterFlush(cliWriter);
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@ -90,6 +90,7 @@
 #include "fc/fc_tasks.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
+#include "fc/firmware_update.h"

 #include "flight/failsafe.h"
 #include "flight/imu.h"
@ -201,6 +202,8 @@ void init(void)
    // Initialize system and CPU clocks to their initial values
    systemInit();

+    __enable_irq();
+
    // initialize IO (needed for all IO operations)
    IOInitGlobal();

--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@ -60,6 +60,7 @@
 #include "fc/controlrate_profile.h"
 #include "fc/fc_msp.h"
 #include "fc/fc_msp_box.h"
+#include "fc/firmware_update.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
@ -2835,9 +2836,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        break;

    case MSP2_INAV_SELECT_BATTERY_PROFILE:
-        if (!ARMING_FLAG(ARMED)) {
-            if (sbufReadU8Safe(&tmp_u8, src))
+        if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) {
                setConfigBatteryProfileAndWriteEEPROM(tmp_u8);
+        } else {
+            return MSP_RESULT_ERROR;
        }
        break;

@ -2861,6 +2863,32 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        break;
 #endif

+#ifdef MSP_FIRMWARE_UPDATE
+    case MSP2_INAV_FWUPDT_PREPARE:
+        if (!firmwareUpdatePrepare(sbufReadU32(src))) {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_FWUPDT_STORE:
+        if (!firmwareUpdateStore(sbufPtr(src), sbufBytesRemaining(src))) {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_FWUPDT_EXEC:
+        firmwareUpdateExec(sbufReadU8(src));
+        return MSP_RESULT_ERROR; // will only be reached if the update is not ready
+        break;
+    case MSP2_INAV_FWUPDT_ROLLBACK_PREPARE:
+        if (!firmwareUpdateRollbackPrepare()) {
+            return MSP_RESULT_ERROR;
+        }
+        break;
+    case MSP2_INAV_FWUPDT_ROLLBACK_EXEC:
+        firmwareUpdateRollbackExec();
+        return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
+        break;
+#endif
+
    default:
        return MSP_RESULT_ERROR;
    }
--- a/src/main/fc/firmware_update.c
+++ b/src/main/fc/firmware_update.c
@ -0,0 +1,318 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute this software
+ * and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav is distributed in the hope that they will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "common/crc.h"
+
+#include "drivers/flash.h"
+#include "drivers/light_led.h"
+#include "drivers/persistent.h"
+#include "drivers/system.h"
+
+#include "fc/firmware_update.h"
+#include "fc/firmware_update_common.h"
+#include "fc/runtime_config.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
+
+
+#ifdef MSP_FIRMWARE_UPDATE
+
+#if !(defined(USE_FLASHFS) || defined(USE_SDCARD))
+#error No storage backend available
+#endif
+
+static firmwareUpdateMetadata_t updateMetadata;
+static uint8_t updateFirmwareCalcCRC = 0;
+static uint32_t receivedSize = 0;
+static bool rollbackPrepared = false;
+
+#if defined(USE_SDCARD)
+static uint32_t firmwareSize;
+static afatfsFilePtr_t updateFile = NULL;
+static afatfsFilePtr_t backupFile = NULL;
+
+static void updateFileOpenCallback(afatfsFilePtr_t file)
+{
+    updateFile = file;
+}
+
+static void backupFileOpenCallback(afatfsFilePtr_t file)
+{
+    backupFile = file;
+}
+
+#elif defined(USE_FLASHFS)
+static uint32_t flashStartAddress, flashOverflowAddress;
+
+#endif
+
+static bool fullBackup(void)
+{
+    const uint8_t *const backupSrcEnd = (const uint8_t*)FLASH_END;
+    uint8_t *backupSrcPtr = (uint8_t*)&__firmware_start;
+    uint32_t counter = 0;
+    updateMetadata.backupCRC = 0;
+
+    LED0_OFF;
+    LED1_OFF;
+
+#if defined(USE_SDCARD)
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) || !afatfs_fopen(FIRMWARE_UPDATE_BACKUP_FILENAME, "w+", backupFileOpenCallback)) return false;
+
+    while (backupSrcPtr < backupSrcEnd) {
+
+        const uint16_t writeBlockSize = 512;
+        uint32_t justWritten = afatfs_fwriteSync(backupFile, backupSrcPtr, writeBlockSize);
+        updateMetadata.backupCRC = crc8_dvb_s2_update(updateMetadata.backupCRC, backupSrcPtr, justWritten);
+
+        afatfs_poll();
+        backupSrcPtr += justWritten;
+
+        if (++counter % (50*1024/512) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+    afatfs_fcloseSync(backupFile);
+
+#elif defined(USE_FLASHFS)
+    flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FULL_BACKUP);
+    if (!flashPartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    const uint32_t flashSectorSize = flashGeometry->sectorSize;
+    const uint32_t flashPartitionSize = (flashPartition->endSector - flashPartition->startSector + 1) * flashSectorSize;
+    const uint32_t backupSize = AVAILABLE_FIRMWARE_SPACE;
+    if (backupSize > flashPartitionSize) return false;
+
+    uint32_t flashAddress = flashPartition->startSector * flashSectorSize;
+
+    const uint32_t flashPageSize = flashGeometry->pageSize;
+    while (backupSrcPtr < backupSrcEnd) {
+
+        if (flashAddress % flashSectorSize == 0) {
+            flashEraseSector(flashAddress);
+            flashWaitForReady(1000);
+        }
+
+        flashPageProgram(flashAddress, backupSrcPtr, flashPageSize);
+        updateMetadata.backupCRC = crc8_dvb_s2_update(updateMetadata.backupCRC, backupSrcPtr, flashPageSize);
+
+        flashAddress += flashPageSize;
+        backupSrcPtr += flashPageSize;
+
+        if (++counter % (10*1024/256) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+#endif
+
+    return true;
+}
+
+static bool backupIsValid(void)
+{
+    if (!firmwareUpdateMetadataRead(&updateMetadata) || (updateMetadata.magic != FIRMWARE_UPDATE_METADATA_MAGIC)) {
+        return false;
+    }
+
+    LED0_OFF;
+    LED1_OFF;
+
+    uint32_t counter = 0;
+    uint8_t calcCRC = 0;
+
+#if defined(USE_SDCARD)
+#define SD_BACKUP_FILE_BLOCK_READ_SIZE 512
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) || !afatfs_fopen(FIRMWARE_UPDATE_BACKUP_FILENAME, "w+", backupFileOpenCallback)) return false;
+
+    uint32_t totalRead = 0;
+
+    uint8_t buffer[SD_BACKUP_FILE_BLOCK_READ_SIZE];
+    while (!afatfs_feof(backupFile)) {
+
+        uint32_t readBytes = afatfs_freadSync(backupFile, buffer, SD_BACKUP_FILE_BLOCK_READ_SIZE);
+        calcCRC = crc8_dvb_s2_update(calcCRC, buffer, readBytes);
+
+        totalRead += readBytes;
+
+        if (++counter % (50*1024/SD_BACKUP_FILE_BLOCK_READ_SIZE) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+    afatfs_fcloseSync(backupFile);
+
+#elif defined(USE_FLASHFS)
+    flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FULL_BACKUP);
+    if (!flashPartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    const uint32_t flashSectorSize = flashGeometry->sectorSize;
+    const uint32_t flashPartitionSize = (flashPartition->endSector - flashPartition->startSector + 1) * flashSectorSize;
+    const uint32_t backupSize = FLASH_END - (uint32_t)&__firmware_start;
+    if (backupSize > flashPartitionSize) return false;
+
+    uint32_t flashAddress = flashPartition->startSector * flashSectorSize;
+    const uint32_t flashEndAddress = flashAddress + backupSize;
+
+    uint8_t buffer[256];
+    while (flashAddress < flashEndAddress) {
+
+        flashReadBytes(flashAddress, buffer, sizeof(buffer));
+        calcCRC = crc8_dvb_s2_update(calcCRC, buffer, sizeof(buffer));
+
+        flashAddress += sizeof(buffer);
+
+        if (++counter % (10*1024/256) == 0) {
+            LED0_TOGGLE;
+            LED1_TOGGLE;
+        }
+
+    }
+
+#endif
+
+    return (calcCRC == updateMetadata.backupCRC);
+}
+
+bool firmwareUpdatePrepare(uint32_t updateSize)
+{
+    if (ARMING_FLAG(ARMED) || (updateSize > AVAILABLE_FIRMWARE_SPACE)) return false;
+
+#if defined(USE_SDCARD)
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) || !afatfs_fopen(FIRMWARE_UPDATE_FIRMWARE_FILENAME, "w+", updateFileOpenCallback)) return false;
+
+    firmwareSize = updateSize;
+
+#elif defined(USE_FLASHFS)
+    flashPartition_t *flashUpdatePartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_UPDATE_FIRMWARE);
+    if (!flashUpdatePartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+
+    flashStartAddress = flashUpdatePartition->startSector * flashGeometry->sectorSize;
+    flashOverflowAddress = ((flashUpdatePartition->endSector + 1) * flashGeometry->sectorSize);
+    receivedSize = 0;
+
+    uint32_t partitionSize = (flashUpdatePartition->endSector - flashUpdatePartition->startSector + 1) * (flashGeometry->sectorSize * flashGeometry->pageSize);
+
+    if (updateSize > partitionSize) {
+        return false;
+    }
+
+    updateMetadata.firmwareSize = updateSize;
+
+#endif
+
+    updateFirmwareCalcCRC = 0;
+
+    return true;
+}
+
+bool firmwareUpdateStore(uint8_t *data, uint16_t length)
+{
+    if (ARMING_FLAG(ARMED)) {
+        return false;
+    }
+
+#if defined(USE_SDCARD)
+
+    if (!updateFile || !firmwareSize || (receivedSize + length > firmwareSize)
+            || (afatfs_fwriteSync(updateFile, data, length) != length)) {
+        return false;
+    }
+
+#elif defined(USE_FLASHFS)
+    if  (!updateMetadata.firmwareSize || (receivedSize + length > updateMetadata.firmwareSize)) return false;
+
+    const uint32_t flashAddress = flashStartAddress + receivedSize;
+
+    if ((flashAddress + length > flashOverflowAddress) || (receivedSize + length > updateMetadata.firmwareSize)) {
+        updateMetadata.firmwareSize = 0;
+        return false;
+    }
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    const uint32_t flashSectorSize = flashGeometry->sectorSize;
+
+    if (flashAddress % flashSectorSize == 0) {
+        flashEraseSector(flashAddress);
+        flashWaitForReady(1000);
+    }
+
+    flashPageProgram(flashAddress, data, length);
+
+#endif
+
+    updateFirmwareCalcCRC = crc8_dvb_s2_update(updateFirmwareCalcCRC, data, length);
+    receivedSize += length;
+
+    return true;
+}
+
+void firmwareUpdateExec(uint8_t expectCRC)
+{
+    if (ARMING_FLAG(ARMED)) return;
+
+#if defined(USE_SDCARD)
+    if (!afatfs_fclose(updateFile, NULL)) return;
+    if (firmwareSize && (receivedSize == firmwareSize) &&
+            (updateFirmwareCalcCRC == expectCRC) && fullBackup() && firmwareUpdateMetadataWrite(&updateMetadata)) {
+        systemResetRequest(RESET_BOOTLOADER_FIRMWARE_UPDATE);
+    }
+#elif defined(USE_FLASHFS)
+    if (updateMetadata.firmwareSize && (receivedSize == updateMetadata.firmwareSize) &&
+            (updateFirmwareCalcCRC == expectCRC) && fullBackup() && firmwareUpdateMetadataWrite(&updateMetadata)) {
+        systemResetRequest(RESET_BOOTLOADER_FIRMWARE_UPDATE);
+    }
+#endif
+
+}
+
+bool firmwareUpdateRollbackPrepare(void)
+{
+    if (ARMING_FLAG(ARMED) || !(rollbackPrepared || backupIsValid())) return false;
+
+    rollbackPrepared = true;
+    return true;
+}
+
+void firmwareUpdateRollbackExec(void)
+{
+    if (ARMING_FLAG(ARMED) || !firmwareUpdateRollbackPrepare()) return;
+
+    systemResetRequest(RESET_BOOTLOADER_FIRMWARE_ROLLBACK);
+}
+
+#endif
--- a/src/main/fc/firmware_update.h
+++ b/src/main/fc/firmware_update.h
@ -0,0 +1,29 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute this software
+ * and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav is distributed in the hope that they will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool firmwareUpdatePrepare(uint32_t firmwareSize);
+bool firmwareUpdateStore(uint8_t *data, uint16_t length);
+void firmwareUpdateExec(uint8_t expectCRC);
+bool firmwareUpdateRollbackPrepare(void);
+void firmwareUpdateRollbackExec(void);
+
+bool writeMeta(void); // XXX temp
--- a/src/main/fc/firmware_update_common.c
+++ b/src/main/fc/firmware_update_common.c
@ -0,0 +1,87 @@
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "common/log.h"
+
+#include "drivers/flash.h"
+
+#include "fc/firmware_update_common.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
+
+#ifdef MSP_FIRMWARE_UPDATE
+
+#if !(defined(USE_FLASHFS) || defined(USE_SDCARD))
+#error No storage backend available
+#endif
+
+#ifdef USE_SDCARD
+static afatfsFilePtr_t metaFile = NULL;
+
+static void metaFileOpenCallback(afatfsFilePtr_t file)
+{
+    metaFile = file;
+}
+#endif
+
+bool firmwareUpdateMetadataRead(firmwareUpdateMetadata_t *updateMetadata)
+{
+#if defined(USE_SDCARD)
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY)
+            || !afatfs_fopen(FIRMWARE_UPDATE_META_FILENAME, "r", metaFileOpenCallback)
+            || (afatfs_freadSync(metaFile, (uint8_t *)updateMetadata, sizeof(*updateMetadata)) != sizeof(*updateMetadata))) {
+        return false;
+    }
+
+#elif defined(USE_FLASHFS)
+    flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META);
+    if (!flashPartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    uint32_t flashAddress = flashPartition->startSector * flashGeometry->sectorSize;
+
+    if (!flashReadBytes(flashAddress, (uint8_t *)updateMetadata, sizeof(*updateMetadata))) {
+        return false;
+    }
+
+#endif
+
+    return true;
+}
+
+bool firmwareUpdateMetadataWrite(firmwareUpdateMetadata_t *updateMetadata)
+{
+    updateMetadata->magic = FIRMWARE_UPDATE_METADATA_MAGIC;
+
+#if defined(USE_SDCARD)
+    if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY)
+            || !afatfs_fopen(FIRMWARE_UPDATE_META_FILENAME, "w+", metaFileOpenCallback)
+            || (afatfs_fwriteSync(metaFile, (uint8_t *)updateMetadata, sizeof(*updateMetadata)) != sizeof(*updateMetadata))) {
+        return false;
+    }
+
+    afatfs_fcloseSync(metaFile);
+
+#elif defined(USE_FLASHFS)
+    flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META);
+    if (!flashPartition) return false;
+
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    const uint32_t flashPartitionSize = (flashPartition->endSector - flashPartition->startSector + 1) * flashGeometry->sectorSize;
+    uint32_t flashAddress = flashPartition->startSector * flashGeometry->sectorSize;
+
+    if (flashPartitionSize < sizeof(*updateMetadata)) return false;
+
+    flashEraseSector(flashAddress);
+    flashWaitForReady(1000);
+    flashPageProgram(flashAddress, (uint8_t *)updateMetadata, sizeof(*updateMetadata));
+
+#endif
+
+    return true;
+}
+
+#endif
--- a/src/main/fc/firmware_update_common.h
+++ b/src/main/fc/firmware_update_common.h
@ -0,0 +1,55 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute this software
+ * and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav is distributed in the hope that they will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#define FIRMWARE_UPDATE_FIRMWARE_FILENAME "firmware.upt"
+#define FIRMWARE_UPDATE_BACKUP_FILENAME "firmware.bak"
+#define FIRMWARE_UPDATE_META_FILENAME "update.mta"
+
+#define FIRMWARE_UPDATE_METADATA_MAGIC 0xAABBCCDD
+
+#undef FLASH_END
+
+#define FIRMWARE_START_ADDRESS ((uint32_t)&__firmware_start)
+#define FLASH_START_ADDRESS 0x08000000UL
+#define FLASH_END (FLASH_START_ADDRESS + FLASH_SIZE * 1024)
+#define CONFIG_START_ADDRESS ((uint32_t)&__config_start)
+#define CONFIG_END_ADDRESS ((uint32_t)&__config_end)
+
+#define AVAILABLE_FIRMWARE_SPACE (FLASH_END - FIRMWARE_START_ADDRESS)
+
+extern uint8_t __firmware_start; // set via linker
+extern uint8_t __config_start;
+extern uint8_t __config_end;
+
+typedef struct {
+    uint32_t magic;
+#ifdef USE_FLASHFS
+    uint32_t firmwareSize;
+    bool dataFlashErased;
+#endif
+    uint8_t backupCRC;
+} firmwareUpdateMetadata_t;
+
+bool firmwareUpdateMetadataRead(firmwareUpdateMetadata_t *updateMetadata);
+bool firmwareUpdateMetadataWrite(firmwareUpdateMetadata_t *updateMetadata);
--- a/src/main/io/asyncfatfs/asyncfatfs.c
+++ b/src/main/io/asyncfatfs/asyncfatfs.c
@ -2107,6 +2107,25 @@ afatfsOperationStatus_e afatfs_fseek(afatfsFilePtr_t file, int32_t offset, afatf
    return afatfs_fseekInternal(file, MIN((uint32_t) offset, file->logicalSize), NULL);
 }

+/**
+ * Attempt to seek the file cursor from the given point (`whence`) by the given offset, just like C's fseek().
+ *
+ * AFATFS_SEEK_SET with offset 0 will always return AFATFS_OPERATION_SUCCESS.
+ *
+ * Returns:
+ *     AFATFS_OPERATION_SUCCESS     - The seek was completed immediately
+ *     AFATFS_OPERATION_IN_PROGRESS - The seek was queued and will complete later. Feel free to attempt read/write
+ *                                    operations on the file, they'll fail until the seek is complete.
+ */
+afatfsOperationStatus_e afatfs_fseekSync(afatfsFilePtr_t file, int32_t offset, afatfsSeek_e whence)
+{
+    while (afatfs_fileIsBusy(file)) {
+        afatfs_poll();
+    }
+
+    return afatfs_fseek(file, offset, whence);
+}
+
 /**
 * Get the byte-offset of the file's cursor from the start of the file.
 *
@ -2536,7 +2555,9 @@ static void afatfs_createFileContinue(afatfsFile_t *file)
    afatfsCreateFile_t *opState = &file->operation.state.createFile;
    fatDirectoryEntry_t *entry;
    afatfsOperationStatus_e status;
+#ifndef BOOTLOADER
    dateTime_t now;
+#endif

    doMore:

@ -2596,13 +2617,17 @@ static void afatfs_createFileContinue(afatfsFile_t *file)

                memcpy(entry->filename, opState->filename, FAT_FILENAME_LENGTH);
                entry->attrib = file->attrib;
+#ifndef BOOTLOADER
                if (rtcGetDateTimeLocal(&now)) {
                    entry->creationDate = FAT_MAKE_DATE(now.year, now.month, now.day);
                    entry->creationTime = FAT_MAKE_TIME(now.hours, now.minutes, now.seconds);
                } else {
+#endif
                    entry->creationDate = AFATFS_DEFAULT_FILE_DATE;
                    entry->creationTime = AFATFS_DEFAULT_FILE_TIME;
+#ifndef BOOTLOADER
                }
+#endif
                entry->lastWriteDate = entry->creationDate;
                entry->lastWriteTime = entry->creationTime;

@ -2867,6 +2892,20 @@ bool afatfs_fclose(afatfsFilePtr_t file, afatfsCallback_t callback)
    }
 }

+/**
+ * Close `file` immediately
+ */
+void afatfs_fcloseSync(afatfsFilePtr_t file)
+{
+    for(unsigned i = 0; i < 1000; ++i) {
+        afatfs_poll();
+    }
+    afatfs_fclose(file, NULL);
+    for(unsigned i = 0; i < 1000; ++i) {
+        afatfs_poll();
+    }
+}
+
 /**
 * Create a new directory with the given name, or open the directory if it already exists.
 *
@ -2998,6 +3037,11 @@ bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t c
    return file != NULL;
 }

+uint32_t afatfs_fileSize(afatfsFilePtr_t file)
+{
+    return file->logicalSize;
+}
+
 /**
 * Write a single character to the file at the current cursor position. If the cache is too busy to accept the write,
 * it is silently dropped.
@ -3087,6 +3131,39 @@ uint32_t afatfs_fwrite(afatfsFilePtr_t file, const uint8_t *buffer, uint32_t len
    return writtenBytes;
 }

+/**
+ * Attempt to write `len` bytes from `buffer` into the `file`.
+ *
+ * Returns the number of bytes actually written
+ *
+ * Fewer bytes than requested will be read when EOF is reached before all the bytes could be read
+ */
+uint32_t afatfs_fwriteSync(afatfsFilePtr_t file, uint8_t *data, uint32_t length)
+{
+    uint32_t written = 0;
+
+    while (true) {
+        uint32_t leftToWrite = length - written;
+        uint32_t justWritten = afatfs_fwrite(file, data + written, leftToWrite);
+        /*if (justWritten != leftToWrite) LOG_E(SYSTEM, "%ld -> %ld", length, written);*/
+        written += justWritten;
+
+        if (written < length) {
+
+            if (afatfs_isFull()) {
+                break;
+            }
+
+            afatfs_poll();
+
+        } else {
+            break;
+        }
+    }
+
+    return written;
+}
+
 /**
 * Attempt to read `len` bytes from `file` into the `buffer`.
 *
@ -3156,6 +3233,37 @@ uint32_t afatfs_fread(afatfsFilePtr_t file, uint8_t *buffer, uint32_t len)
    return readBytes;
 }

+/**
+ * Attempt to read `len` bytes from `file` into the `buffer`.
+ *
+ * Returns the number of bytes actually read.
+ *
+ * Fewer bytes than requested will be read when EOF is reached before all the bytes could be read
+ */
+uint32_t afatfs_freadSync(afatfsFilePtr_t file, uint8_t *buffer, uint32_t length)
+{
+    uint32_t bytesRead = 0;
+
+    while (true) {
+        uint32_t leftToRead = length - bytesRead;
+        uint32_t readNow = afatfs_fread(file, buffer, leftToRead);
+        bytesRead += readNow;
+        if (bytesRead < length) {
+
+            if (afatfs_feof(file)) {
+                break;
+            }
+
+            afatfs_poll();
+
+        } else {
+            break;
+        }
+    }
+
+    return bytesRead;
+}
+
 /**
 * Returns true if the file's pointer position currently lies at the end-of-file point (i.e. one byte beyond the last
 * byte in the file).
--- a/src/main/io/asyncfatfs/asyncfatfs.h
+++ b/src/main/io/asyncfatfs/asyncfatfs.h
@ -63,14 +63,19 @@ typedef void (*afatfsCallback_t)(void);
 bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete);
 bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback);
 bool afatfs_fclose(afatfsFilePtr_t file, afatfsCallback_t callback);
+void afatfs_fcloseSync(afatfsFilePtr_t file);
 bool afatfs_funlink(afatfsFilePtr_t file, afatfsCallback_t callback);

 bool afatfs_feof(afatfsFilePtr_t file);
 void afatfs_fputc(afatfsFilePtr_t file, uint8_t c);
 uint32_t afatfs_fwrite(afatfsFilePtr_t file, const uint8_t *buffer, uint32_t len);
+uint32_t afatfs_fwriteSync(afatfsFilePtr_t file, uint8_t *data, uint32_t length);
 uint32_t afatfs_fread(afatfsFilePtr_t file, uint8_t *buffer, uint32_t len);
+uint32_t afatfs_freadSync(afatfsFilePtr_t file, uint8_t *buffer, uint32_t length);
 afatfsOperationStatus_e afatfs_fseek(afatfsFilePtr_t file, int32_t offset, afatfsSeek_e whence);
+afatfsOperationStatus_e afatfs_fseekSync(afatfsFilePtr_t file, int32_t offset, afatfsSeek_e whence);
 bool afatfs_ftell(afatfsFilePtr_t file, uint32_t *position);
+uint32_t afatfs_fileSize(afatfsFilePtr_t file);

 bool afatfs_mkdir(const char *filename, afatfsFileCallback_t complete);
 bool afatfs_chdir(afatfsFilePtr_t dirHandle);
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@ -66,3 +66,9 @@
 #define MSP2_SET_PID                            0x2031

 #define MSP2_INAV_OPFLOW_CALIBRATION            0x2032
+
+#define MSP2_INAV_FWUPDT_PREPARE                0x2033
+#define MSP2_INAV_FWUPDT_STORE                  0x2034
+#define MSP2_INAV_FWUPDT_EXEC                   0x2035
+#define MSP2_INAV_FWUPDT_ROLLBACK_PREPARE       0x2036
+#define MSP2_INAV_FWUPDT_ROLLBACK_EXEC          0x2037
--- a/src/main/target/ANYFCF7/target.mk
+++ b/src/main/target/ANYFCF7/target.mk
@ -1,4 +1,4 @@
-F7X5XG_TARGETS += $(TARGET)
+F765XG_TARGETS += $(TARGET)
 FEATURES       += SDCARD VCP MSC

 TARGET_SRC = \
--- a/src/main/target/KAKUTEF7/target.mk
+++ b/src/main/target/KAKUTEF7/target.mk
@ -1,4 +1,4 @@
-F7X5XG_TARGETS += $(TARGET)
+F765XG_TARGETS += $(TARGET)
 ifeq ($(TARGET), KAKUTEF7MINI)
 FEATURES       += VCP ONBOARDFLASH
 else
--- a/src/main/target/MATEKF765/target.mk
+++ b/src/main/target/MATEKF765/target.mk
@ -1,4 +1,4 @@
-F7X5XI_TARGETS += $(TARGET)
+F765XI_TARGETS += $(TARGET)
 FEATURES       += SDCARD VCP MSC

 TARGET_SRC = \
--- a/src/main/target/OMNIBUSF7/target.mk
+++ b/src/main/target/OMNIBUSF7/target.mk
@ -1,4 +1,4 @@
-F7X5XG_TARGETS += $(TARGET)
+F765XG_TARGETS += $(TARGET)
 FEATURES       += SDCARD VCP MSC

 TARGET_SRC = \
--- a/src/main/target/link/stm32_flash_F303_128k.ld
+++ b/src/main/target/link/stm32_flash_F303_128k.ld
--- a/src/main/target/link/stm32_flash_F303_256k.ld
+++ b/src/main/target/link/stm32_flash_F303_256k.ld
--- a/src/main/target/link/stm32_flash_F405.ld
+++ b/src/main/target/link/stm32_flash_F405.ld
--- a/src/main/target/link/stm32_flash_F405_bl.ld
+++ b/src/main/target/link/stm32_flash_F405_bl.ld
@ -0,0 +1,43 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f405.ld
+**
+**  Abstract    : Linker script for STM32F405RG Device with
+**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x08000000 to 0x08100000 1024K full flash,
+0x08000000 to 0x080DFFFF 896K firmware,
+0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
+    FIRMWARE (rx)     : ORIGIN = 0x08008000, LENGTH = 864K
+    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
+
+    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
+    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
+    BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", CCM)
+REGION_ALIAS("FASTRAM", CCM)
+
+__firmware_start = ORIGIN(FIRMWARE);
+
+INCLUDE "stm32_flash.ld"
--- a/src/main/target/link/stm32_flash_F405_for_bl.ld
+++ b/src/main/target/link/stm32_flash_F405_for_bl.ld
@ -0,0 +1,42 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f405.ld
+**
+**  Abstract    : Linker script for STM32F405RG Device with
+**                1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x08000000 to 0x08100000 1024K full flash,
+0x08000000 to 0x080DFFFF 896K firmware,
+0x080E0000 to 0x08100000 128K config,       // FLASH_Sector_11
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    FLASH (rx)        : ORIGIN = 0x08008000, LENGTH = 864K
+    FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
+
+    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
+    CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
+    BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", CCM)
+REGION_ALIAS("FASTRAM", CCM)
+
+__firmware_start = ORIGIN(FLASH);
+
+INCLUDE "stm32_flash.ld"
--- a/src/main/target/link/stm32_flash_F405_opbl.ld
+++ b/src/main/target/link/stm32_flash_F405_opbl.ld
--- a/src/main/target/link/stm32_flash_F411.ld
+++ b/src/main/target/link/stm32_flash_F411.ld
--- a/src/main/target/link/stm32_flash_F411_opbl.ld
+++ b/src/main/target/link/stm32_flash_F411_opbl.ld
--- a/src/main/target/link/stm32_flash_F427.ld
+++ b/src/main/target/link/stm32_flash_F427.ld
--- a/src/main/target/link/stm32_flash_F446.ld
+++ b/src/main/target/link/stm32_flash_F446.ld
--- a/src/main/target/link/stm32_flash_F7.ld
+++ b/src/main/target/link/stm32_flash_F7.ld
@ -0,0 +1,184 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash.ld
+**
+**  Abstract    : Common linker script for STM32 devices.
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM);    /* end of RAM */
+
+/* Base address where the config is stored. */
+__config_start = ORIGIN(FLASH_CONFIG);
+__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG);
+
+/* Define output sections */
+SECTIONS
+{
+  /* The startup code goes first into FLASH */
+  .isr_vector :
+  {
+    . = ALIGN(4);
+    PROVIDE (isr_vector_table_base = .);
+    KEEP(*(.isr_vector)) /* Startup code */
+    . = ALIGN(4);
+  } >FLASH
+
+  /* The program code and other data goes into FLASH */
+  .text :
+  {
+    . = ALIGN(4);
+    *(.text)           /* .text sections (code) */
+    *(.text*)          /* .text* sections (code) */
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    *(.glue_7)         /* glue arm to thumb code */
+    *(.glue_7t)        /* glue thumb to arm code */
+    *(.eh_frame)
+
+    KEEP (*(.init))
+    KEEP (*(.fini))
+
+    . = ALIGN(4);
+    _etext = .;        /* define a global symbols at end of code */
+  } >FLASH
+
+  tcm_code = LOADADDR(.tcm_code); 
+  .tcm_code (NOLOAD) :
+  {
+    . = ALIGN(4);
+    tcm_code_start = .; 
+    *(.tcm_code)
+    *(.tcm_code*)
+    . = ALIGN(4);
+    tcm_code_end = .; 
+  } >ITCM_RAM AT >FLASH
+
+   .ARM.extab   : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+    .ARM : {
+    __exidx_start = .;
+      *(.ARM.exidx*)
+      __exidx_end = .;
+    } >FLASH
+
+  .preinit_array     :
+  {
+    PROVIDE_HIDDEN (__preinit_array_start = .);
+    KEEP (*(.preinit_array*))
+    PROVIDE_HIDDEN (__preinit_array_end = .);
+  } >FLASH
+  .init_array :
+  {
+    PROVIDE_HIDDEN (__init_array_start = .);
+    KEEP (*(SORT(.init_array.*)))
+    KEEP (*(.init_array*))
+    PROVIDE_HIDDEN (__init_array_end = .);
+  } >FLASH
+  .fini_array :
+  {
+    PROVIDE_HIDDEN (__fini_array_start = .);
+    KEEP (*(.fini_array*))
+    KEEP (*(SORT(.fini_array.*)))
+    PROVIDE_HIDDEN (__fini_array_end = .);
+  } >FLASH
+  .pg_registry :
+  {
+    PROVIDE_HIDDEN (__pg_registry_start = .);
+    KEEP (*(.pg_registry))
+    KEEP (*(SORT(.pg_registry.*)))
+    PROVIDE_HIDDEN (__pg_registry_end = .);
+  } >FLASH
+  .pg_resetdata :
+  {
+    PROVIDE_HIDDEN (__pg_resetdata_start = .);
+    KEEP (*(.pg_resetdata))
+    PROVIDE_HIDDEN (__pg_resetdata_end = .);
+  } >FLASH
+  .busdev_registry :
+  {
+    PROVIDE_HIDDEN (__busdev_registry_start = .);
+    KEEP (*(.busdev_registry))
+    KEEP (*(SORT(.busdev_registry.*)))
+    PROVIDE_HIDDEN (__busdev_registry_end = .);
+  } >FLASH
+
+  /* used by the startup to initialize data */
+  _sidata = .;
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data :
+  {
+    . = ALIGN(4);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+
+    . = ALIGN(4);
+    _edata = .;        /* define a global symbol at data end */
+  } >RAM AT> FLASH
+
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .bss :
+  {
+    /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.bss)
+    *(SORT_BY_ALIGNMENT(.bss*))
+    *(COMMON)
+
+    . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+  } >RAM
+  
+  .fastram_bss (NOLOAD) :
+  {
+    __fastram_bss_start__ = .;
+    *(.fastram_bss)
+    *(SORT_BY_ALIGNMENT(.fastram_bss*))
+    . = ALIGN(4);
+    __fastram_bss_end__ = .;
+  } >FASTRAM
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */
+  _heap_stack_begin = _heap_stack_end - _Min_Stack_Size  - _Min_Heap_Size;
+  . = _heap_stack_begin;
+  ._user_heap_stack :
+  {
+    . = ALIGN(4);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(4);
+  } >STACKRAM = 0xa5
+
+  /* MEMORY_bank1 section, code must be located here explicitly            */
+  /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
+  .memory_b1_text :
+  {
+    *(.mb1text)        /* .mb1text sections (code) */
+    *(.mb1text*)       /* .mb1text* sections (code)  */
+    *(.mb1rodata)      /* read-only data (constants) */
+    *(.mb1rodata*)
+  } >MEMORY_B1
+
+  /* Remove information from the standard libraries */
+  /DISCARD/ :
+  {
+    libc.a ( * )
+    libm.a ( * )
+    libgcc.a ( * )
+  }
+
+  .ARM.attributes 0 : { *(.ARM.attributes) }
+}
--- a/src/main/target/link/stm32_flash_F746.ld
+++ b/src/main/target/link/stm32_flash_F746.ld
@ -26,22 +26,22 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
-    ITCM_RAM (rx)			: ORIGIN = 0x00000000, LENGTH = 16K
-    ITCM_FLASH (rx)        	: ORIGIN = 0x00200000, LENGTH = 32K
+    ITCM_RAM (rx)	        : ORIGIN = 0x00000000, LENGTH = 16K
+    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
-    ITCM_FLASH_CONFIG (r)  	: ORIGIN = 0x00208000, LENGTH = 32K
-    ITCM_FLASH1 (rx)       	: ORIGIN = 0x00210000, LENGTH = 960K
+    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)	        : ORIGIN = 0x00210000, LENGTH = 960K

-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
-    FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
+    FLASH (rx)                  : ORIGIN = 0x08000000, LENGTH = 32K
+    FLASH_CONFIG (r)            : ORIGIN = 0x08008000, LENGTH = 32K
+    FLASH1 (rx)                 : ORIGIN = 0x08010000, LENGTH = 960K

-    TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
+    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
+    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
 }
 /* note CCM could be used for stack */
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)

-INCLUDE "stm32_flash_f7_split.ld"
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F765xI.ld
+++ b/src/main/target/link/stm32_flash_F765xI.ld
@ -52,4 +52,4 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
 REGION_ALIAS("FASTRAM",  DTCM_RAM)
 REGION_ALIAS("RAM",      SRAM1)

-INCLUDE "stm32_flash_f7_split.ld"
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F765xI_bl.ld
+++ b/src/main/target/link/stm32_flash_F765xI_bl.ld
@ -0,0 +1,56 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f765.ld
+**
+**  Abstract    : Linker script for STM32F765xITx Device with
+**                2048KByte FLASH, 512KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x00000000 to 0x00003FFF   16K TCM RAM,
+
+0x08000000 to 0x081FFFFF 2048K full flash,
+0x08000000 to 0x08007FFF   32K isr vector, startup code,
+0x08008000 to 0x0800FFFF   32K config,
+0x08010000 to 0x081FFFFF  1984K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rx)           : ORIGIN = 0x00000000, LENGTH = 16K
+
+    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 32K
+    ITCM_FLASH_CFG (r)      : ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)        : ORIGIN = 0x00210000, LENGTH = 1984K
+
+    AXIM_FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 32K
+    AXIM_FIRMWARE (rx)      : ORIGIN = 0x08008000, LENGTH = 32K
+    AXIM_FLASH_CFG (r)      : ORIGIN = 0x08010000, LENGTH = 32K
+
+    DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
+    SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
+    SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
+    MEMORY_B1 (rx)          : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("FLASH",        AXIM_FLASH)
+REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG)
+
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM",  DTCM_RAM)
+REGION_ALIAS("RAM",      SRAM1)
+
+__firmware_start = ORIGIN(AXIM_FIRMWARE);
+
+INCLUDE "stm32_flash_F7.ld"
--- a/src/main/target/link/stm32_flash_F765xI_for_bl.ld
+++ b/src/main/target/link/stm32_flash_F765xI_for_bl.ld
@ -0,0 +1,57 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f765.ld
+**
+**  Abstract    : Linker script for STM32F765xITx Device with
+**                2048KByte FLASH, 512KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x00000000 to 0x00003FFF   16K TCM RAM,
+
+0x08000000 to 0x081FFFFF 2048K full flash,
+0x08000000 to 0x08007FFF   32K isr vector, startup code,
+0x08008000 to 0x0800FFFF   32K config,
+0x08010000 to 0x081FFFFF  1984K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rx)           : ORIGIN = 0x00000000, LENGTH = 16K
+
+    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 32K
+    ITCM_FLASH_CFG (r)      : ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)        : ORIGIN = 0x00210000, LENGTH = 1984K
+
+    AXIM_FLASH (rx)         : ORIGIN = 0x08008000, LENGTH = 32K
+    AXIM_FLASH_CFG (r)      : ORIGIN = 0x08010000, LENGTH = 32K
+    AXIM_FLASH1 (rx)        : ORIGIN = 0x08018000, LENGTH = 1984K
+
+    DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
+    SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
+    SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
+    MEMORY_B1 (rx)          : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("FLASH",        AXIM_FLASH)
+REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG)
+REGION_ALIAS("FLASH1",       AXIM_FLASH1)
+
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM",  DTCM_RAM)
+REGION_ALIAS("RAM",      SRAM1)
+
+__firmware_start = ORIGIN(AXIM_FLASH);
+
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F7_split.ld
+++ b/src/main/target/link/stm32_flash_F7_split.ld
--- a/src/main/target/link/stm32_flash_F7x2.ld
+++ b/src/main/target/link/stm32_flash_F7x2.ld
@ -45,4 +45,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)

-INCLUDE "stm32_flash_f7_split.ld"
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F7x2_bl.ld
+++ b/src/main/target/link/stm32_flash_F7x2_bl.ld
@ -0,0 +1,50 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f722.ld
+**
+**  Abstract    : Linker script for STM32F722RETx Device with
+**                512KByte FLASH, 256KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x08000000 to 0x0807FFFF  512K full flash,
+0x08000000 to 0x08003FFF   16K isr vector, startup code,
+0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
+0x08008000 to 0x0807FFFF  480K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
+
+    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
+    ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K
+    ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K
+
+    FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 32K
+    FIRMWARE (rx)      : ORIGIN = 0x08008000, LENGTH = 16K
+    FLASH_CONFIG (r)   : ORIGIN = 0x0800c000, LENGTH = 16K
+
+    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
+    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", TCM)
+REGION_ALIAS("FASTRAM", TCM)
+
+__firmware_start = ORIGIN(FIRMWARE);
+
+INCLUDE "stm32_flash_F7.ld"
--- a/src/main/target/link/stm32_flash_F7x2_for_bl.ld
+++ b/src/main/target/link/stm32_flash_F7x2_for_bl.ld
@ -0,0 +1,50 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f722.ld
+**
+**  Abstract    : Linker script for STM32F722RETx Device with
+**                512KByte FLASH, 256KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x08000000 to 0x0807FFFF  512K full flash,
+0x08000000 to 0x08003FFF   16K isr vector, startup code,
+0x08004000 to 0x08007FFF   16K config,                  // FLASH_Sector_1
+0x08008000 to 0x0807FFFF  480K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
+
+    /*ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K*/
+    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
+    /*ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K*/
+    /*ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K*/
+
+    FLASH (rx)         : ORIGIN = 0x08008000, LENGTH = 16K
+    FLASH_CONFIG (r)   : ORIGIN = 0x0800c000, LENGTH = 16K
+    FLASH1 (rx)        : ORIGIN = 0x08010000, LENGTH = 448K
+
+    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
+    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", TCM)
+REGION_ALIAS("FASTRAM", TCM)
+
+__firmware_start = ORIGIN(FLASH);
+
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F7x5xG.ld
+++ b/src/main/target/link/stm32_flash_F7x5xG.ld
@ -45,4 +45,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)

-INCLUDE "stm32_flash_f7_split.ld"
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/link/stm32_flash_F7x5xG_bl.ld
+++ b/src/main/target/link/stm32_flash_F7x5xG_bl.ld
@ -0,0 +1,50 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f745.ld
+**
+**  Abstract    : Linker script for STM32F745VGTx Device with
+**                1024KByte FLASH, 320KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x00000000 to 0x00003FFF   16K ITCM RAM,
+0x08000000 to 0x080FFFFF 1024K full flash,
+0x08000000 to 0x08007FFF   32K isr vector, startup code,
+0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
+0x08010000 to 0x080FFFFF  960K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
+    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 928K
+
+    FLASH (rx)                  : ORIGIN = 0x08000000, LENGTH = 32K
+    FIRMWARE (rx)               : ORIGIN = 0x08008000, LENGTH = 32K
+    FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
+
+    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
+    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
+}
+/* note CCM could be used for stack */
+REGION_ALIAS("STACKRAM", TCM)
+REGION_ALIAS("FASTRAM", TCM)
+
+__firmware_start = ORIGIN(FIRMWARE);
+
+INCLUDE "stm32_flash_F7.ld"
--- a/src/main/target/link/stm32_flash_F7x5xG_for_bl.ld
+++ b/src/main/target/link/stm32_flash_F7x5xG_for_bl.ld
@ -0,0 +1,50 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_flash_f745.ld
+**
+**  Abstract    : Linker script for STM32F745VGTx Device with
+**                1024KByte FLASH, 320KByte RAM
+**
+*****************************************************************************
+*/
+
+/* Stack & Heap sizes */
+_Min_Heap_Size = 0;
+_Min_Stack_Size = 0x1800;
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x00000000 to 0x00003FFF   16K ITCM RAM,
+0x08000000 to 0x080FFFFF 1024K full flash,
+0x08000000 to 0x08007FFF   32K isr vector, startup code,
+0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
+0x08010000 to 0x080FFFFF  960K firmware,
+*/
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
+    ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 960K
+
+    FLASH (rx)                  : ORIGIN = 0x08008000, LENGTH = 32K
+    FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
+    FLASH1 (rx)                 : ORIGIN = 0x08018000, LENGTH = 928K
+
+    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
+    MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
+}
+/* note CCM could be used for stack */
+REGION_ALIAS("STACKRAM", TCM)
+REGION_ALIAS("FASTRAM", TCM)
+
+__firmware_start = ORIGIN(FLASH);
+
+INCLUDE "stm32_flash_F7_split.ld"
--- a/src/main/target/system.h
+++ b/src/main/target/system.h
@ -0,0 +1,21 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute this software
+ * and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav is distributed in the hope that they will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+extern uint8_t isr_vector_table_base;
--- a/src/main/target/system_stm32f4xx.c
+++ b/src/main/target/system_stm32f4xx.c
@ -315,6 +315,7 @@
  */

 #include "stm32f4xx.h"
+#include "system.h"
 #include "system_stm32f4xx.h"

 uint32_t hse_value = HSE_VALUE;
@ -509,7 +510,7 @@ void SystemInit(void)
 #ifdef VECT_TAB_SRAM
  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
 #else
-  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+  SCB->VTOR = (uint32_t) &isr_vector_table_base; /* Vector Table Relocation in Internal FLASH */
 #endif
 }

--- a/src/main/target/system_stm32f7xx.c
+++ b/src/main/target/system_stm32f7xx.c
@ -298,7 +298,8 @@ void SystemInit(void)
 #ifdef VECT_TAB_SRAM
  SCB->VTOR = RAMDTCM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
 #else
-  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
+  extern uint8_t isr_vector_table_base; /* Vector Table Relocation in Internal FLASH */
+  SCB->VTOR = (uint32_t) &isr_vector_table_base;
 #endif

  /* Enable I-Cache */
--- a/src/utils/combine_tool
+++ b/src/utils/combine_tool
@ -0,0 +1,29 @@
+#!/usr/bin/env ruby
+
+##
+## This file is part of iNav.
+##
+## iNav is free software. You can redistribute this software
+## and/or modify this software under the terms of the
+## GNU General Public License as published by the Free Software
+## Foundation, either version 3 of the License, or (at your option)
+## any later version.
+##
+## iNav is distributed in the hope that they will be
+## useful, but WITHOUT ANY WARRANTY; without even the implied
+## warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+## See the GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this software.
+##
+## If not, see <http://www.gnu.org/licenses/>.
+##
+
+require_relative 'intelhex'
+
+ih = IntelHex.new
+ih.add_section 0x08000000, File.open(ARGV[0], 'rb')
+ih.add_section 0x08008000, File.open(ARGV[1], 'rb')
+ih.start_address = 0x08000000
+ih.write File.open(ARGV[2], 'w')
--- a/src/utils/intelhex.rb
+++ b/src/utils/intelhex.rb
@ -0,0 +1,100 @@
+##
+## This file is part of iNav.
+##
+## iNav is free software. You can redistribute this software
+## and/or modify this software under the terms of the
+## GNU General Public License as published by the Free Software
+## Foundation, either version 3 of the License, or (at your option)
+## any later version.
+##
+## iNav is distributed in the hope that they will be
+## useful, but WITHOUT ANY WARRANTY; without even the implied
+## warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+## See the GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this software.
+##
+## If not, see <http://www.gnu.org/licenses/>.
+##
+
+class IntelHex
+
+    class Section < Struct.new(:address, :io); end
+
+    def initialize address = 0, bin_io = nil, start_address = nil
+        @sections = Array.new
+        @start_address = start_address == true ? address : start_address
+        add_section address, bin_io if bin_io
+    end
+
+    def add_section address, io
+        sections << Section.new(address, io)
+    end
+
+    def write io
+        sections.each { |section| write_section section, io }
+        write_start_linear_address start_address, io if start_address
+        write_end_of_file io
+        nil
+    end
+
+    attr_reader :sections
+    attr_accessor :start_address
+
+    private
+
+    def calc_crc address, record_type, data
+        sum = 0
+        sum += (address & 0xFF00) >> 8
+        sum += (address & 0x00FF)
+        sum += record_type
+
+        if data.is_a? Integer
+            sum += data > 0xFFFF ? 4 : 2
+            sum += (data & 0xFF000000) >> 24
+            sum += (data & 0x00FF0000) >> 16
+            sum += (data & 0x0000FF00) >> 8
+            sum += (data & 0x000000FF)
+        else
+            sum += data.bytesize
+            sum += data.each_byte.sum
+        end
+
+        ((sum ^ 0xFF) + 1) & 0xFF
+    end
+
+    def write_end_of_file io
+        io.puts ":00000001FF"
+    end
+
+    def write_start_linear_address address, io
+        crc = calc_crc 0, 5, address
+        io.printf ":04000005%08X%02X\n", address, crc
+    end
+
+    def write_extended_linear_address address, io
+        address_msw = (address & 0xffff0000) >> 16
+        crc = calc_crc 0, 4, address_msw
+        io.printf ":02000004%04X%02X\n", address_msw, crc
+    end
+
+    def write_data address, data, io
+        address &= 0xFFFF
+        hex_data = data.each_byte.map { |byte| "%02X" % byte }.join
+        crc = calc_crc address, 0, data
+        io.printf ":%02X%04X00%s%02X\n", data.bytesize, address, hex_data, crc
+    end
+
+    def write_section section, io
+        previous_address = 0
+        address = section.address
+        while data = section.io.read(16)
+            write_extended_linear_address address, io if (previous_address & 0xFFFF0000) != (address & 0xFFFF0000)
+            previous_address = address
+            write_data address, data, io
+            address += data.bytesize
+        end
+    end
+
+end