From 8c8749accde8a87c0c558b0fb37ec5408bd27eed Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jan=20K=C3=A4berich?= <kaeberic@ibr.cs.tu-bs.de>
Date: Sun, 30 Aug 2020 16:20:00 +0200
Subject: [PATCH] Flash/FPGA driver for configuration and firmware update from
 uC

---
 Software/VNA_embedded/.cproject               |   6 +-
 Software/VNA_embedded/.mxproject              |   2 +-
 Software/VNA_embedded/Application/App.cpp     |  19 +-
 .../Application/Communication/Protocol.cpp    |   2 +-
 .../Application/Communication/Protocol.hpp    |   1 +
 .../VNA_embedded/Application/Drivers/FPGA.cpp | 127 +++++++++----
 .../VNA_embedded/Application/Drivers/FPGA.hpp |   3 +
 .../Application/Drivers/Flash.cpp             | 100 ++++++++++
 .../Application/Drivers/Flash.hpp             |  44 +++++
 .../VNA_embedded/Application/Drivers/Log.cpp  |   4 +
 .../VNA_embedded/Application/Drivers/Log.h    |   1 +
 .../VNA_embedded/Application/Firmware.cpp     | 176 ++++++++++++++++++
 .../VNA_embedded/Application/Firmware.hpp     |  30 +++
 Software/VNA_embedded/Inc/FreeRTOSConfig.h    |  13 +-
 Software/VNA_embedded/Inc/main.h              |   2 +-
 .../VNA_embedded/Inc/stm32g4xx_hal_conf.h     |   2 +-
 Software/VNA_embedded/Src/app_freertos.c      |  16 ++
 Software/VNA_embedded/Src/main.c              |   4 +-
 Software/VNA_embedded/Src/stm32g4xx_hal_msp.c |   2 +-
 Software/VNA_embedded/VNA_embedded.ioc        |  28 ++-
 20 files changed, 520 insertions(+), 62 deletions(-)
 create mode 100644 Software/VNA_embedded/Application/Drivers/Flash.cpp
 create mode 100644 Software/VNA_embedded/Application/Drivers/Flash.hpp
 create mode 100644 Software/VNA_embedded/Application/Firmware.cpp
 create mode 100644 Software/VNA_embedded/Application/Firmware.hpp

diff --git a/Software/VNA_embedded/.cproject b/Software/VNA_embedded/.cproject
index 7380b47..486deae 100644
--- a/Software/VNA_embedded/.cproject
+++ b/Software/VNA_embedded/.cproject
@@ -133,7 +133,7 @@
                                 								
                                 <option id="gnu.c.compiler.option.dialect.std.295832639" name="Language standard" superClass="gnu.c.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.c.compiler.dialect.default" valueType="enumerated"/>
                                 								
-                                <option id="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.fdata.915472139" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.fdata" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+                                <option id="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.fdata.915472139" name="Place the data in their own section (-fdata-sections)" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.fdata" useByScannerDiscovery="false" value="true" valueType="boolean"/>
                                 								
                                 <inputType id="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c.1184125608" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c"/>
                                 								
@@ -205,7 +205,7 @@
                                 								
                                 <option id="fr.ac6.managedbuild.gnu.cpp.compiler.option.misc.other.1734449160" name="Other flags" superClass="fr.ac6.managedbuild.gnu.cpp.compiler.option.misc.other" useByScannerDiscovery="false" value="-fmessage-length=0" valueType="string"/>
                                 								
-                                <option id="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.fdata.2099053827" superClass="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.fdata" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+                                <option id="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.fdata.2099053827" name="Place the data in their own section (-fdata-sections)" superClass="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.fdata" useByScannerDiscovery="false" value="true" valueType="boolean"/>
                                 								
                                 <inputType id="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.input.cpp.1826809538" superClass="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.input.cpp"/>
                                 								
@@ -775,7 +775,7 @@
         		
         <configuration configurationName="Debug">
             			
-            <resource resourceType="PROJECT" workspacePath="VNA_embedded"/>
+            <resource resourceType="PROJECT" workspacePath="/VNA_embedded"/>
             		
         </configuration>
         		
diff --git a/Software/VNA_embedded/.mxproject b/Software/VNA_embedded/.mxproject
index 412af7b..9f6af63 100644
--- a/Software/VNA_embedded/.mxproject
+++ b/Software/VNA_embedded/.mxproject
@@ -10,5 +10,5 @@ LibFiles=Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_gpio.h;Drivers/STM32G4xx
 [PreviousUsedSW4STM32Files]
 SourceFiles=../Src/main.c;../Src/app_freertos.c;../Src/usbpd.c;../Src/usbpd_dpm_user.c;../Src/usbpd_pwr_user.c;../Src/usbpd_pwr_if.c;../Src/usbpd_vdm_user.c;../Src/usbpd_dpm_core.c;../Src/stm32g4xx_it.c;../Src/stm32g4xx_hal_msp.c;../Src/stm32g4xx_hal_timebase_tim.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_utils.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_exti.c;../Middlewares/ST/STM32_USBPD_Library/Core/lib/USBPDCORE_PD3_FULL_CM4_wc32.a;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_gpio.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_spi.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_spi_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_ucpd.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_gpio.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_dma.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_uart.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_uart_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pcd.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pcd_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_usb.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ramfunc.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_cortex.c;../Middlewares/Third_Party/FreeRTOS/Source/croutine.c;../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;../Middlewares/Third_Party/FreeRTOS/Source/list.c;../Middlewares/Third_Party/FreeRTOS/Source/queue.c;../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;../Middlewares/Third_Party/FreeRTOS/Source/tasks.c;../Middlewares/Third_Party/FreeRTOS/Source/timers.c;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;../Middlewares/ST/STM32_USBPD_Library/Core/src/usbpd_trace.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_cad_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw_if_it.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_pwr_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_timersserver.c;../Middlewares/ST/STM32_USBPD_Library/../../../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_tim.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_utils.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_exti.c;../Middlewares/ST/STM32_USBPD_Library/Core/lib/USBPDCORE_PD3_FULL_CM4_wc32.a;..//Src/system_stm32g4xx.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_utils.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_exti.c;../Middlewares/ST/STM32_USBPD_Library/Core/lib/USBPDCORE_PD3_FULL_CM4_wc32.a;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_gpio.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_i2c_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_spi.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_spi_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_ucpd.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_gpio.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_dma.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_uart.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_uart_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pcd.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pcd_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_usb.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ramfunc.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr_ex.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_cortex.c;../Middlewares/Third_Party/FreeRTOS/Source/croutine.c;../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;../Middlewares/Third_Party/FreeRTOS/Source/list.c;../Middlewares/Third_Party/FreeRTOS/Source/queue.c;../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;../Middlewares/Third_Party/FreeRTOS/Source/tasks.c;../Middlewares/Third_Party/FreeRTOS/Source/timers.c;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;../Middlewares/ST/STM32_USBPD_Library/Core/src/usbpd_trace.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_cad_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw_if_it.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_pwr_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_timersserver.c;../Middlewares/ST/STM32_USBPD_Library/../../../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_tim.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_utils.c;../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_exti.c;../Middlewares/ST/STM32_USBPD_Library/Core/lib/USBPDCORE_PD3_FULL_CM4_wc32.a;..//Src/system_stm32g4xx.c;../Drivers/CMSIS/Device/ST/STM32G4xx/Source/Templates/system_stm32g4xx.c;null;../Middlewares/Third_Party/FreeRTOS/Source/croutine.c;../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;../Middlewares/Third_Party/FreeRTOS/Source/list.c;../Middlewares/Third_Party/FreeRTOS/Source/queue.c;../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;../Middlewares/Third_Party/FreeRTOS/Source/tasks.c;../Middlewares/Third_Party/FreeRTOS/Source/timers.c;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;../Middlewares/ST/STM32_USBPD_Library/Core/lib/USBPDCORE_PD3_FULL_CM4_wc32.a;../Middlewares/ST/STM32_USBPD_Library/Core/src/usbpd_trace.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_cad_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_hw_if_it.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_phy_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_pwr_hw_if.c;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/src/usbpd_timersserver.c;../Middlewares/ST/STM32_USBPD_Library/../../../Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_ll_tim.c;
 HeaderPath=../Drivers/STM32G4xx_HAL_Driver/Inc;../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F;../Middlewares/ST/STM32_USBPD_Library/Core/inc;../Middlewares/ST/STM32_USBPD_Library/Devices/STM32G4XX/inc;../Middlewares/ST/STM32_USBPD_Library/../../../Drivers/STM32G4xx_HAL_Driver/Inc;../Drivers/CMSIS/Device/ST/STM32G4xx/Include;../Drivers/CMSIS/Include;../Inc;
-CDefines=USE_FULL_LL_DRIVER;__weak:"__attribute__((weak))";__packed:"__attribute__((__packed__))";USBPD_PORT_COUNT:1;USBPDCORE_LIB_PD3_FULL;_RTOS;_SNK;USE_FULL_LL_DRIVER;__weak:"__attribute__((weak))";__packed:"__attribute__((__packed__))";USE_HAL_DRIVER;STM32G431xx;USE_FULL_LL_DRIVER;USE_HAL_DRIVER;USE_HAL_DRIVER;STM32G431xx;
+CDefines=USE_FULL_LL_DRIVER;__weak:"__attribute__((weak))";__packed:"__attribute__((__packed__))";USBPD_PORT_COUNT:1;USBPDCORE_LIB_PD3_FULL;_RTOS;_SNK;USE_FULL_LL_DRIVER;__weak:"__attribute__((weak))";__packed:"__attribute__((__packed__))";USE_HAL_DRIVER;STM32G431xx;USE_HAL_DRIVER;STM32G431xx;
 
diff --git a/Software/VNA_embedded/Application/App.cpp b/Software/VNA_embedded/Application/App.cpp
index d816c00..314a8ac 100644
--- a/Software/VNA_embedded/Application/App.cpp
+++ b/Software/VNA_embedded/Application/App.cpp
@@ -9,6 +9,8 @@
 #include <complex>
 #include <cstring>
 #include "USB/usb.h"
+#include "Flash.hpp"
+#include "Firmware.hpp"
 
 #define LOG_LEVEL	LOG_LEVEL_INFO
 #define LOG_MODULE	"App"
@@ -29,6 +31,9 @@ static Protocol::ManualControl manual;
 #define FW_MAJOR			0
 #define FW_MINOR			01
 
+extern SPI_HandleTypeDef hspi1;
+static Flash flash = Flash(&hspi1, FLASH_CS_GPIO_Port, FLASH_CS_Pin);
+
 void VNACallback(Protocol::Datapoint res) {
 	result = res;
 	newResult = true;
@@ -45,9 +50,21 @@ void App_Start() {
 	Log_SetRedirect(usb_log);
 	LOG_INFO("Start");
 	Exti::Init();
+	if(!flash.isPresent()) {
+		LOG_CRIT("Failed to detect onboard FLASH");
+	}
+	auto fw_info = Firmware::GetFlashContentInfo(&flash);
+	if(fw_info.valid) {
+		if(fw_info.CPU_need_update) {
+			// Function will not return, the device will reboot with the new firmware instead
+			Firmware::PerformUpdate(&flash);
+		}
+		FPGA::Configure(&flash, fw_info.FPGA_bitstream_address, fw_info.FPGA_bitstream_size);
+	} else {
+		LOG_CRIT("Invalid bitstream/firmware, not configuring FPGA");
+	}
 	if (!VNA::Init()) {
 		LOG_CRIT("Initialization failed, unable to start");
-		return;
 	}
 	// Allow USB enumeration
 //	USB_EN_GPIO_Port->BSRR = USB_EN_Pin;
diff --git a/Software/VNA_embedded/Application/Communication/Protocol.cpp b/Software/VNA_embedded/Application/Communication/Protocol.cpp
index 164cb7e..64b21ce 100644
--- a/Software/VNA_embedded/Application/Communication/Protocol.cpp
+++ b/Software/VNA_embedded/Application/Communication/Protocol.cpp
@@ -15,7 +15,7 @@ static constexpr uint8_t header = 0x5A;
 static constexpr uint8_t header_size = 4;
 
 #define CRC32_POLYGON 0xEDB88320
-uint32_t CRC32(uint32_t crc, const void *data, uint32_t len) {
+uint32_t Protocol::CRC32(uint32_t crc, const void *data, uint32_t len) {
 	uint8_t *u8buf = (uint8_t*) data;
 	int k;
 
diff --git a/Software/VNA_embedded/Application/Communication/Protocol.hpp b/Software/VNA_embedded/Application/Communication/Protocol.hpp
index 65b8d46..75a43fd 100644
--- a/Software/VNA_embedded/Application/Communication/Protocol.hpp
+++ b/Software/VNA_embedded/Application/Communication/Protocol.hpp
@@ -103,6 +103,7 @@ using PacketInfo = struct _packetinfo {
 	};
 };
 
+uint32_t CRC32(uint32_t crc, const void *data, uint32_t len);
 uint16_t DecodeBuffer(uint8_t *buf, uint16_t len, PacketInfo *info);
 uint16_t EncodePacket(PacketInfo packet, uint8_t *dest, uint16_t destsize);
 
diff --git a/Software/VNA_embedded/Application/Drivers/FPGA.cpp b/Software/VNA_embedded/Application/Drivers/FPGA.cpp
index d5cd1d9..14d6179 100644
--- a/Software/VNA_embedded/Application/Drivers/FPGA.cpp
+++ b/Software/VNA_embedded/Application/Drivers/FPGA.cpp
@@ -7,14 +7,31 @@
 #define LOG_MODULE	"FPGA"
 #include "Log.h"
 
-#define FPGA_SPI		hspi1
-extern SPI_HandleTypeDef FPGA_SPI;
+#define FPGA_SPI			hspi1
+#define CONFIGURATION_SPI	hspi2
+extern SPI_HandleTypeDef FPGA_SPI, CONFIGURATION_SPI;
 
-static inline void Low(GPIO_TypeDef *gpio, uint16_t pin) {
-	gpio->BSRR = pin << 16;
+using GPIO = struct {
+	GPIO_TypeDef *gpio;
+	uint16_t pin;
+};
+static constexpr GPIO CS = {.gpio = FPGA_CS_GPIO_Port, .pin = FPGA_CS_Pin};
+static constexpr GPIO PROGRAM_B = {.gpio = FPGA_PROGRAM_B_GPIO_Port, .pin = FPGA_PROGRAM_B_Pin};
+static constexpr GPIO INIT_B = {.gpio = FPGA_INIT_B_GPIO_Port, .pin = FPGA_INIT_B_Pin};
+static constexpr GPIO DONE = {.gpio = FPGA_DONE_GPIO_Port, .pin = FPGA_DONE_Pin};
+static constexpr GPIO FPGA_RESET = {.gpio = FPGA_RESET_GPIO_Port, .pin = FPGA_RESET_Pin};
+static constexpr GPIO AUX1 = {.gpio = FPGA_AUX1_GPIO_Port, .pin = FPGA_AUX1_Pin};
+static constexpr GPIO AUX2 = {.gpio = FPGA_AUX2_GPIO_Port, .pin = FPGA_AUX2_Pin};
+static constexpr GPIO AUX3 = {.gpio = FPGA_AUX3_GPIO_Port, .pin = FPGA_AUX3_Pin};
+
+static inline void Low(GPIO g) {
+	g.gpio->BSRR = g.pin << 16;
 }
-static inline void High(GPIO_TypeDef *gpio, uint16_t pin) {
-	gpio->BSRR = pin;
+static inline void High(GPIO g) {
+	g.gpio->BSRR = g.pin;
+}
+bool isHigh(GPIO g) {
+	return g.gpio->IDR & g.pin;
 }
 
 static FPGA::HaltedCallback halted_cb;
@@ -23,29 +40,64 @@ static uint16_t ISRMaskReg = 0x0000;
 
 void WriteRegister(FPGA::Reg reg, uint16_t value) {
 	uint16_t cmd[2] = {(uint16_t) (0x8000 | (uint16_t) reg), value};
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) cmd, 2, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
+}
+
+bool FPGA::Configure(Flash *f, uint32_t start_address, uint32_t bitstream_size) {
+	LOG_INFO("Loading bitstream of size %lu...", bitstream_size);
+	Low(PROGRAM_B);
+	while(isHigh(INIT_B));
+	High(PROGRAM_B);
+	while(!isHigh(INIT_B));
+
+	uint8_t buf[256];
+	while(bitstream_size > 0) {
+		uint16_t size = sizeof(buf);
+		if(size > bitstream_size) {
+			size = bitstream_size;
+		}
+		// TODO this part might be doable with the DMA instead of the buffer
+		// get chunk of bitstream from flash...
+		f->read(start_address, size, buf);
+		// ... and pass it on to FPGA
+		HAL_SPI_Transmit(&CONFIGURATION_SPI, buf, size, 100);
+		bitstream_size -= size;
+		start_address += size;
+	}
+	Delay::ms(1);
+	if(!isHigh(INIT_B)) {
+		LOG_CRIT("INIT_B asserted after configuration, CRC error occurred");
+		return false;
+	}
+	if(!isHigh(DONE)) {
+		LOG_CRIT("DONE still low after configuration");
+		return false;
+	}
+	LOG_INFO("...configured");
+	return true;
 }
 
 bool FPGA::Init(HaltedCallback cb) {
 	halted_cb = cb;
 	// Reset FPGA
-	High(FPGA_RESET_GPIO_Port, FPGA_RESET_Pin);
+	High(FPGA_RESET);
 	SetMode(Mode::FPGA);
 	Delay::us(1);
-	Low(FPGA_RESET_GPIO_Port, FPGA_RESET_Pin);
+	Low(FPGA_RESET);
 	Delay::ms(10);
 
 	// Check if FPGA response is as expected
 	uint16_t cmd[2] = {0x4000, 0x0000};
 	uint16_t recv[2];
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_TransmitReceive(&FPGA_SPI, (uint8_t*) cmd, (uint8_t*) recv, 2, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 
 	if(recv[1] != 0xF0A5) {
 		LOG_ERR("Initialization failed, got 0x%04x instead of 0xF0A5", recv[1]);
+		return false;
 	}
 
 	LOG_DEBUG("Initialized, status register: 0x%04x", recv[0]);
@@ -148,9 +200,9 @@ void FPGA::WriteSweepConfig(uint16_t pointnum, bool lowband, uint32_t *SourceReg
 	send[5] = (SourceRegs[4] & 0x00300000) >> 6 | (SourceRegs[3] & 0xFC000000) >> 18 | (SourceRegs[1] & 0x00007F80) >> 7;
 	send[6] = (SourceRegs[1] & 0x00000078) << 9 | (SourceRegs[0] & 0x00007FF8) >> 3;
 	send[7] = (SourceRegs[0] & 0x7FFF8000) >> 15;
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) send, 8, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 }
 
 static inline int64_t sign_extend_64(int64_t x, uint16_t bits) {
@@ -167,7 +219,7 @@ bool FPGA::InitiateSampleRead(ReadCallback cb) {
 	uint16_t cmd = 0xC000;
 	uint16_t status;
 
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_TransmitReceive(&FPGA_SPI, (uint8_t*) &cmd, (uint8_t*) &status, 1,
 			100);
 
@@ -179,7 +231,7 @@ bool FPGA::InitiateSampleRead(ReadCallback cb) {
 
 	if (!(status & 0x0004)) {
 		// no new data available yet
-		High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+		High(CS);
 
 		if (halted) {
 			if (halted_cb) {
@@ -199,7 +251,7 @@ bool FPGA::InitiateSampleRead(ReadCallback cb) {
 extern "C" {
 void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
 	FPGA::SamplingResult result;
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 	// Assemble data from words
 	result.P1I = sign_extend_64(
 			(uint64_t) raw[17] << 32 | (uint32_t) raw[16] << 16 | raw[15], 48);
@@ -223,35 +275,35 @@ void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
 }
 
 void FPGA::StartSweep() {
-	Low(FPGA_AUX3_GPIO_Port, FPGA_AUX3_Pin);
+	Low(AUX3);
 	Delay::us(1);
-	High(FPGA_AUX3_GPIO_Port, FPGA_AUX3_Pin);
+	High(AUX3);
 }
 
 void FPGA::AbortSweep() {
-	Low(FPGA_AUX3_GPIO_Port, FPGA_AUX3_Pin);
+	Low(AUX3);
 }
 
 void FPGA::SetMode(Mode mode) {
 	switch(mode) {
 	case Mode::FPGA:
 		// Both AUX1/2 low
-		Low(FPGA_AUX1_GPIO_Port, FPGA_AUX1_Pin);
-		Low(FPGA_AUX2_GPIO_Port, FPGA_AUX2_Pin);
+		Low(AUX1);
+		Low(AUX2);
 		Delay::us(1);
-		High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+		High(CS);
 		break;
 	case Mode::SourcePLL:
-		Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
-		Low(FPGA_AUX2_GPIO_Port, FPGA_AUX2_Pin);
+		Low(CS);
+		Low(AUX2);
 		Delay::us(1);
-		High(FPGA_AUX1_GPIO_Port, FPGA_AUX1_Pin);
+		High(AUX1);
 		break;
 	case Mode::LOPLL:
-		Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
-		Low(FPGA_AUX1_GPIO_Port, FPGA_AUX1_Pin);
+		Low(CS);
+		Low(AUX1);
 		Delay::us(1);
-		High(FPGA_AUX2_GPIO_Port, FPGA_AUX2_Pin);
+		High(AUX2);
 		break;
 	}
 	Delay::us(1);
@@ -260,34 +312,35 @@ void FPGA::SetMode(Mode mode) {
 uint16_t FPGA::GetStatus() {
 	uint16_t cmd = 0x4000;
 	uint16_t status;
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_TransmitReceive(&FPGA_SPI, (uint8_t*) &cmd, (uint8_t*) &status, 1,
 			100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 	return status;
 }
 
 FPGA::ADCLimits FPGA::GetADCLimits() {
 	uint16_t cmd = 0xE000;
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) &cmd, 1, 100);
 	ADCLimits limits;
 	HAL_SPI_Receive(&FPGA_SPI, (uint8_t*) &limits, 6, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 	return limits;
 }
 
 void FPGA::ResetADCLimits() {
 	uint16_t cmd = 0x6000;
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) &cmd, 1, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 }
 
 void FPGA::ResumeHaltedSweep() {
 	uint16_t cmd = 0x2000;
-	Low(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	Low(CS);
 	HAL_SPI_Transmit(&FPGA_SPI, (uint8_t*) &cmd, 1, 100);
-	High(FPGA_CS_GPIO_Port, FPGA_CS_Pin);
+	High(CS);
 }
 
+
diff --git a/Software/VNA_embedded/Application/Drivers/FPGA.hpp b/Software/VNA_embedded/Application/Drivers/FPGA.hpp
index c193eb3..df877cf 100644
--- a/Software/VNA_embedded/Application/Drivers/FPGA.hpp
+++ b/Software/VNA_embedded/Application/Drivers/FPGA.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include "Flash.hpp"
 
 namespace FPGA {
 
@@ -66,6 +67,8 @@ enum class LowpassFilter {
 	Auto = 0xFF,
 };
 
+bool Configure(Flash *f, uint32_t start_address, uint32_t bitstream_size);
+
 using HaltedCallback = void(*)(void);
 bool Init(HaltedCallback cb = nullptr);
 void SetNumberOfPoints(uint16_t npoints);
diff --git a/Software/VNA_embedded/Application/Drivers/Flash.cpp b/Software/VNA_embedded/Application/Drivers/Flash.cpp
new file mode 100644
index 0000000..bff80c6
--- /dev/null
+++ b/Software/VNA_embedded/Application/Drivers/Flash.cpp
@@ -0,0 +1,100 @@
+#include "Flash.hpp"
+
+bool Flash::isPresent() {
+	CS(false);
+	// read JEDEC ID
+	uint8_t send[4] = {0x9F};
+	uint8_t recv[4];
+	HAL_SPI_TransmitReceive(spi, send, recv, 4, 100);
+	CS(true);
+	if(recv[1] != 0xEF) {
+		// wrong manufacturer ID, communication with flash not working
+		return false;
+	}
+	return true;
+}
+
+void Flash::read(uint32_t address, uint16_t length, void *dest) {
+	initiateRead(address);
+	// read data
+	HAL_SPI_Receive(spi, (uint8_t*) dest, length, 1000);
+	CS(true);
+}
+
+bool Flash::write(uint32_t address, uint16_t length, uint8_t *src) {
+	if((address & 0xFF) != 0 || length%256 != 0) {
+		// only writes to complete pages allowed
+		return false;
+	}
+	address &= 0x00FFFFFF;
+	while(length > 0) {
+		EnableWrite();
+		CS(false);
+		uint8_t cmd[4] = {
+			0x02,
+			((uint8_t) address >> 16) & 0xFF,
+			((uint8_t) address >> 8) & 0xFF,
+			(uint8_t) (address & 0xFF),
+		};
+		// issue read command
+		HAL_SPI_Transmit(spi, cmd, 4, 100);
+		// write data
+		HAL_SPI_Transmit(spi, src, 256, 1000);
+		CS(true);
+		if(!WaitBusy(5)) {
+			return false;
+		}
+		length -= 256;
+		src += 256;
+	}
+	return true;
+}
+
+void Flash::EnableWrite() {
+	CS(false);
+	// enable write latch
+	uint8_t wel = 0x06;
+	HAL_SPI_Transmit(spi, &wel, 1, 100);
+	CS(true);
+}
+
+bool Flash::eraseChip() {
+	EnableWrite();
+	CS(false);
+	// enable write latch
+	uint8_t chip_erase = 0x60;
+	HAL_SPI_Transmit(spi, &chip_erase, 1, 100);
+	CS(true);
+	return WaitBusy(25000);
+}
+
+void Flash::initiateRead(uint32_t address) {
+	address &= 0x00FFFFFF;
+	CS(false);
+	uint8_t cmd[4] = {
+		0x03,
+		((uint8_t) address >> 16) & 0xFF,
+		((uint8_t) address >> 8) & 0xFF,
+		(uint8_t) (address & 0xFF),
+	};
+	// issue read command
+	HAL_SPI_Transmit(spi, cmd, 4, 100);
+}
+
+bool Flash::WaitBusy(uint32_t timeout) {
+	uint32_t starttime = HAL_GetTick();
+	CS(false);
+	uint8_t readStatus1 = 0x05;
+	HAL_SPI_Transmit(spi, &readStatus1, 1, 100);
+	while(HAL_GetTick() - starttime > timeout) {
+		uint8_t status1;
+		HAL_SPI_Receive(spi, &status1, 1, 100);
+		if(!(status1 & 0x01)) {
+			CS(true);
+			return true;
+		}
+	}
+	// timed out
+	CS(true);
+	return false;
+}
diff --git a/Software/VNA_embedded/Application/Drivers/Flash.hpp b/Software/VNA_embedded/Application/Drivers/Flash.hpp
new file mode 100644
index 0000000..b7dbdf3
--- /dev/null
+++ b/Software/VNA_embedded/Application/Drivers/Flash.hpp
@@ -0,0 +1,44 @@
+/*
+ * Flash.hpp
+ *
+ *  Created on: Aug 26, 2020
+ *      Author: jan
+ */
+
+#ifndef DRIVERS_FLASH_HPP_
+#define DRIVERS_FLASH_HPP_
+
+#include "stm.hpp"
+
+class Flash {
+public:
+	constexpr Flash(SPI_HandleTypeDef *spi, GPIO_TypeDef *CS_gpio, uint16_t CS_pin)
+	: spi(spi),CS_gpio(CS_gpio),CS_pin(CS_pin){};
+
+	bool isPresent();
+	void read(uint32_t address, uint16_t length, void *dest);
+	bool write(uint32_t address, uint16_t length, uint8_t *src);
+	bool eraseChip();
+	// Starts the reading process without actually reading any bytes
+	void initiateRead(uint32_t address);
+	const SPI_HandleTypeDef* const& getSpi() const {
+		return spi;
+	}
+
+private:
+	void CS(bool high) {
+		if(high) {
+			CS_gpio->BSRR = CS_pin;
+		} else {
+			CS_gpio->BSRR = CS_pin << 16;
+		}
+	}
+	void EnableWrite();
+	bool WaitBusy(uint32_t timeout);
+	SPI_HandleTypeDef * const spi;
+	GPIO_TypeDef * const CS_gpio;
+	const uint16_t CS_pin;
+};
+
+
+#endif /* DRIVERS_FLASH_HPP_ */
diff --git a/Software/VNA_embedded/Application/Drivers/Log.cpp b/Software/VNA_embedded/Application/Drivers/Log.cpp
index 2c7b893..1f368bd 100644
--- a/Software/VNA_embedded/Application/Drivers/Log.cpp
+++ b/Software/VNA_embedded/Application/Drivers/Log.cpp
@@ -161,3 +161,7 @@ void HANDLER(void) {
 	}
 }
 }
+
+void Log_Flush() {
+	while(USART_BASE->CR1 & USART_CR1_TCIE);
+}
diff --git a/Software/VNA_embedded/Application/Drivers/Log.h b/Software/VNA_embedded/Application/Drivers/Log.h
index 4ae96a1..33cbdbf 100644
--- a/Software/VNA_embedded/Application/Drivers/Log.h
+++ b/Software/VNA_embedded/Application/Drivers/Log.h
@@ -58,6 +58,7 @@ void Log_Init();
 typedef void (*log_redirect_t)(const char *line, uint16_t length);
 void Log_SetRedirect(log_redirect_t redirect_function);
 void _log_write(const char *module, const char *level, const char *fmt, ...);
+void Log_Flush();
 
 #ifdef __cplusplus
 }
diff --git a/Software/VNA_embedded/Application/Firmware.cpp b/Software/VNA_embedded/Application/Firmware.cpp
new file mode 100644
index 0000000..827c577
--- /dev/null
+++ b/Software/VNA_embedded/Application/Firmware.cpp
@@ -0,0 +1,176 @@
+#include "Firmware.hpp"
+
+#include "Protocol.hpp"
+#include <cstring>
+
+#define LOG_LEVEL	LOG_LEVEL_INFO
+#define LOG_MODULE	"FW"
+#include "Log.h"
+
+#define FPGA_MAXSIZE	512000
+#define CPU_MAXSIZE		131072
+
+using Header = struct {
+	char magic[4];
+	uint32_t FPGA_start;
+	uint32_t FPGA_size;
+	uint32_t CPU_start;
+	uint32_t CPU_size;
+	uint32_t crc;
+} __attribute__((packed));
+
+Firmware::Info Firmware::GetFlashContentInfo(Flash *f) {
+	Info ret;
+	memset(&ret, 0, sizeof(ret));
+	Header h;
+	f->read(0, sizeof(h), &h);
+	// sanity check values
+	if (memcmp(&h.magic, "VNA!",
+			4) || h.FPGA_start == UINT32_MAX || h.FPGA_size > FPGA_MAXSIZE
+			|| h.CPU_start == UINT32_MAX || h.CPU_size > CPU_MAXSIZE) {
+		LOG_WARN("Invalid content, probably empty FLASH");
+		return ret;
+	}
+	LOG_DEBUG("Checking FPGA bitstream...");
+	uint32_t crc = UINT32_MAX;
+	uint8_t buf[128];
+	uint32_t checked_size = 0;
+	while (checked_size < h.FPGA_size + h.CPU_size) {
+		uint16_t read_size = sizeof(buf);
+		if (h.FPGA_size + h.CPU_size - checked_size < read_size) {
+			read_size = h.FPGA_size + h.CPU_size - checked_size;
+		}
+		f->read(h.FPGA_start + checked_size, read_size, buf);
+		crc = Protocol::CRC32(crc, buf, read_size);
+	}
+	if (crc != h.crc) {
+		LOG_ERR("CRC mismatch, invalid FPGA bitstream/CPU firmware");
+		return ret;
+	}
+	// Compare CPU firmware in external Flash to the one currently running in the MCU
+	checked_size = 0;
+	while (checked_size < h.CPU_size) {
+		uint16_t read_size = sizeof(buf);
+		if (h.CPU_size - checked_size < read_size) {
+			read_size = h.CPU_size - checked_size;
+		}
+		f->read(h.FPGA_start + checked_size, read_size, buf);
+		if(memcmp(buf, (void*)(0x8000000+checked_size), read_size)) {
+			LOG_WARN("Difference to CPU firmware in external FLASH detected, update required");
+			ret.CPU_need_update = true;
+			break;
+		}
+	}
+	ret.valid = true;
+	ret.FPGA_bitstream_address = h.FPGA_start;
+	ret.FPGA_bitstream_size = h.FPGA_size;
+	ret.CPU_image_address = h.CPU_start;
+	ret.CPU_image_size = h.CPU_size;
+
+	return ret;
+}
+
+static void copy_flash(uint32_t size, SPI_TypeDef *spi) __attribute__ ((noinline, section (".data")));
+
+/* This function is executed from RAM as it possibly overwrites the whole FLASH.
+ *
+ * It assumes that the flash has already be unlocked and the SPI interface to the
+ * external flash has already initiated a read command. At the end of the copy
+ * process it initiates a software reset.
+ *
+ * !NO FUNCTION CALLS AT ALL ARE ALLOWED IN HERE!
+ */
+static void copy_flash(uint32_t size, SPI_TypeDef *spi) {
+	/* First, erase internal flash */
+	/* disable caches */
+	__HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
+	__HAL_FLASH_DATA_CACHE_DISABLE();
+	/* Erase FLASH */
+	// Erase the only flash bank
+	SET_BIT(FLASH->CR, FLASH_CR_MER1);
+	// Start erase process
+	SET_BIT(FLASH->CR, FLASH_CR_STRT);
+	// Wait for operation to finish
+	while (FLASH->SR & FLASH_SR_BSY)
+	;
+	// Clear bank1 erase request
+	CLEAR_BIT(FLASH->CR, (FLASH_CR_MER1));
+
+	/* The complete FLASH has been erased. Copy the external FLASH memory
+	 * content into the internal MCU flash */
+	uint32_t written = 0;
+
+	/* Enable FLASH write */
+	FLASH->CR |= FLASH_CR_PG;
+	uint32_t to = 0x80000000;
+	while (written < size) {
+		uint8_t buf[8];
+		// Get 64bit from external flash
+		for(uint8_t i=0;i<8;i++) {
+			// wait for SPI ready to transmit dummy data
+			while(!(spi->SR & SPI_FLAG_TXE));
+			// send dummy byte
+			*(__IO uint8_t *)spi->DR = 0x00;
+			// wait for received byte to be ready
+			while(!(spi->SR & SPI_FLAG_RXNE));
+			// get received byte
+			buf[i] = *(__IO uint8_t *)spi->DR;
+		}
+		// program received data into flash
+		*(__IO uint32_t*) to++ = *(uint32_t*)&buf[0];
+		*(__IO uint32_t*) to++ = *(uint32_t*)&buf[4];
+		/* Wait for it to finish */
+		while (FLASH->SR & FLASH_SR_BSY)
+			;
+		// clear possible error flags (there is no way to respond to errors at this point)
+		uint32_t error = (FLASH->SR & FLASH_FLAG_SR_ERRORS);
+		error |= (FLASH->ECCR & FLASH_FLAG_ECCD);
+		if (error != 0u) {
+			/* Clear error programming flags */
+			__HAL_FLASH_CLEAR_FLAG(error);
+		}
+		/* Check FLASH End of Operation flag  */
+		if (__HAL_FLASH_GET_FLAG(FLASH_FLAG_EOP)) {
+			/* Clear FLASH End of Operation pending bit */
+			__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP);
+		}
+		written += 8;
+	}
+	/* Write operation completed, disable the PG Bit */
+	FLASH->CR &= (~FLASH_CR_PG);
+
+	/* The new firmware is in place. This function can not return as the return
+	 * address might be anywhere in the new firmware. Instead perform a software reset
+	 * here */
+	__DSB();
+	SCB->AIRCR = ((0x5FAUL << SCB_AIRCR_VECTKEY_Pos) |
+	SCB_AIRCR_SYSRESETREQ_Msk);
+	__DSB();
+
+	for (;;) {
+		__NOP();
+	}
+}
+
+void Firmware::PerformUpdate(Flash *f) {
+	auto info = GetFlashContentInfo(f);
+	if(!info.valid) {
+		LOG_ERR("Invalid firmware data, not performing update");
+		return;
+	}
+
+	LOG_INFO("Loading new firmware...");
+	Log_Flush();
+
+	__disable_irq();
+
+	/* Flash must be unlocked */
+	HAL_FLASH_Unlock();
+	FLASH_WaitForLastOperation(50000);
+
+	// Initiate readback from flash at CPU firmware start address
+	f->initiateRead(info.CPU_image_address);
+
+	copy_flash(info.CPU_image_size, f->getSpi()->Instance);
+	__builtin_unreachable();
+}
diff --git a/Software/VNA_embedded/Application/Firmware.hpp b/Software/VNA_embedded/Application/Firmware.hpp
new file mode 100644
index 0000000..6147f0d
--- /dev/null
+++ b/Software/VNA_embedded/Application/Firmware.hpp
@@ -0,0 +1,30 @@
+/*
+ * Firmware.hpp
+ *
+ *  Created on: Aug 26, 2020
+ *      Author: jan
+ */
+
+#ifndef COMMUNICATION_FIRMWARE_HPP_
+#define COMMUNICATION_FIRMWARE_HPP_
+
+#include "Flash.hpp"
+
+namespace Firmware {
+
+using Info = struct info {
+	uint32_t FPGA_bitstream_address;
+	uint32_t FPGA_bitstream_size;
+	uint32_t CPU_image_address;
+	uint32_t CPU_image_size;
+	bool valid;
+	bool CPU_need_update;
+};
+
+Info GetFlashContentInfo(Flash *f);
+void PerformUpdate(Flash *f);
+
+}
+
+
+#endif /* COMMUNICATION_FIRMWARE_HPP_ */
diff --git a/Software/VNA_embedded/Inc/FreeRTOSConfig.h b/Software/VNA_embedded/Inc/FreeRTOSConfig.h
index ad06b36..1de0802 100644
--- a/Software/VNA_embedded/Inc/FreeRTOSConfig.h
+++ b/Software/VNA_embedded/Inc/FreeRTOSConfig.h
@@ -53,7 +53,7 @@
 #endif
 
 #define configUSE_PREEMPTION                     1
-#define configSUPPORT_STATIC_ALLOCATION          0
+#define configSUPPORT_STATIC_ALLOCATION          1
 #define configSUPPORT_DYNAMIC_ALLOCATION         1
 #define configUSE_IDLE_HOOK                      0
 #define configUSE_TICK_HOOK                      0
@@ -61,7 +61,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 7 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)3072)
+#define configTOTAL_HEAP_SIZE                    ((size_t)4096)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
@@ -74,14 +74,15 @@
 
 /* Set the following definitions to 1 to include the API function, or zero
 to exclude the API function. */
-#define INCLUDE_vTaskPrioritySet            1
-#define INCLUDE_uxTaskPriorityGet           1
-#define INCLUDE_vTaskDelete                 1
+#define INCLUDE_vTaskPrioritySet            0
+#define INCLUDE_uxTaskPriorityGet           0
+#define INCLUDE_vTaskDelete                 0
 #define INCLUDE_vTaskCleanUpResources       0
-#define INCLUDE_vTaskSuspend                1
+#define INCLUDE_vTaskSuspend                0
 #define INCLUDE_vTaskDelayUntil             0
 #define INCLUDE_vTaskDelay                  1
 #define INCLUDE_xTaskGetSchedulerState      1
+#define INCLUDE_xTaskResumeFromISR          0
 
 /* Cortex-M specific definitions. */
 #ifdef __NVIC_PRIO_BITS
diff --git a/Software/VNA_embedded/Inc/main.h b/Software/VNA_embedded/Inc/main.h
index 6d35d31..d9c09d5 100644
--- a/Software/VNA_embedded/Inc/main.h
+++ b/Software/VNA_embedded/Inc/main.h
@@ -89,7 +89,7 @@ void Error_Handler(void);
 #define EN_6V_GPIO_Port GPIOB
 #define LED1_Pin GPIO_PIN_15
 #define LED1_GPIO_Port GPIOA
-#define FPGA_RESET_Pin GPIO_PIN_7
+#define FPGA_RESET_Pin GPIO_PIN_5
 #define FPGA_RESET_GPIO_Port GPIOB
 #define FPGA_DONE_Pin GPIO_PIN_9
 #define FPGA_DONE_GPIO_Port GPIOB
diff --git a/Software/VNA_embedded/Inc/stm32g4xx_hal_conf.h b/Software/VNA_embedded/Inc/stm32g4xx_hal_conf.h
index d729806..01b0fe4 100644
--- a/Software/VNA_embedded/Inc/stm32g4xx_hal_conf.h
+++ b/Software/VNA_embedded/Inc/stm32g4xx_hal_conf.h
@@ -180,7 +180,7 @@ The real value may vary depending on the variations in voltage and temperature.*
   */     
 
 #define  VDD_VALUE                   (3300UL) /*!< Value of VDD in mv */
-#define  TICK_INT_PRIORITY           (5UL)    /*!< tick interrupt priority (lowest by default)  */            
+#define  TICK_INT_PRIORITY           (0UL)    /*!< tick interrupt priority (lowest by default)  */            
 #define  USE_RTOS                     0U
 #define  PREFETCH_ENABLE              0U
 #define  INSTRUCTION_CACHE_ENABLE     1U
diff --git a/Software/VNA_embedded/Src/app_freertos.c b/Software/VNA_embedded/Src/app_freertos.c
index d795b05..b4c7fb4 100644
--- a/Software/VNA_embedded/Src/app_freertos.c
+++ b/Software/VNA_embedded/Src/app_freertos.c
@@ -53,6 +53,22 @@
    
 /* USER CODE END FunctionPrototypes */
 
+/* GetIdleTaskMemory prototype (linked to static allocation support) */
+void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );
+
+/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
+static StaticTask_t xIdleTaskTCBBuffer;
+static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];
+  
+void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize )
+{
+  *ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
+  *ppxIdleTaskStackBuffer = &xIdleStack[0];
+  *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
+  /* place for user code */
+}                   
+/* USER CODE END GET_IDLE_TASK_MEMORY */
+
 /* Private application code --------------------------------------------------*/
 /* USER CODE BEGIN Application */
      
diff --git a/Software/VNA_embedded/Src/main.c b/Software/VNA_embedded/Src/main.c
index 43728be..aa44bc2 100644
--- a/Software/VNA_embedded/Src/main.c
+++ b/Software/VNA_embedded/Src/main.c
@@ -56,6 +56,8 @@ UART_HandleTypeDef huart3;
 PCD_HandleTypeDef hpcd_USB_FS;
 
 osThreadId defaultTaskHandle;
+uint32_t defaultTaskBuffer[ 4096 ];
+osStaticThreadDef_t defaultTaskControlBlock;
 /* USER CODE BEGIN PV */
 
 /* USER CODE END PV */
@@ -143,7 +145,7 @@ int main(void)
 
   /* Create the thread(s) */
   /* definition and creation of defaultTask */
-  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
+  osThreadStaticDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 4096, defaultTaskBuffer, &defaultTaskControlBlock);
   defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
 
   /* USER CODE BEGIN RTOS_THREADS */
diff --git a/Software/VNA_embedded/Src/stm32g4xx_hal_msp.c b/Software/VNA_embedded/Src/stm32g4xx_hal_msp.c
index 1459f7f..cffd407 100644
--- a/Software/VNA_embedded/Src/stm32g4xx_hal_msp.c
+++ b/Software/VNA_embedded/Src/stm32g4xx_hal_msp.c
@@ -279,7 +279,7 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
     /* Peripheral clock enable */
     __HAL_RCC_TIM1_CLK_ENABLE();
     /* TIM1 interrupt Init */
-    HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 5, 0);
+    HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(TIM1_TRG_COM_TIM17_IRQn);
   /* USER CODE BEGIN TIM1_MspInit 1 */
 
diff --git a/Software/VNA_embedded/VNA_embedded.ioc b/Software/VNA_embedded/VNA_embedded.ioc
index 2935f3d..3bccd46 100644
--- a/Software/VNA_embedded/VNA_embedded.ioc
+++ b/Software/VNA_embedded/VNA_embedded.ioc
@@ -36,8 +36,18 @@ Dma.UCPD1_TX.1.SyncEnable=DISABLE
 Dma.UCPD1_TX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.UCPD1_TX.1.SyncRequestNumber=1
 Dma.UCPD1_TX.1.SyncSignalID=NONE
-FREERTOS.IPParameters=Tasks01
-FREERTOS.Tasks01=defaultTask,0,128,StartDefaultTask,Default,NULL
+FREERTOS.FootprintOK=true
+FREERTOS.INCLUDE_uxTaskPriorityGet=0
+FREERTOS.INCLUDE_vTaskDelete=0
+FREERTOS.INCLUDE_vTaskPrioritySet=0
+FREERTOS.INCLUDE_vTaskSuspend=0
+FREERTOS.INCLUDE_xTaskResumeFromISR=0
+FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelete,INCLUDE_vTaskPrioritySet,INCLUDE_uxTaskPriorityGet,INCLUDE_xTaskResumeFromISR,INCLUDE_vTaskSuspend,MEMORY_ALLOCATION,configTOTAL_HEAP_SIZE,configENABLE_BACKWARD_COMPATIBILITY,configUSE_MUTEXES,FootprintOK
+FREERTOS.MEMORY_ALLOCATION=2
+FREERTOS.Tasks01=defaultTask,0,4096,StartDefaultTask,Default,NULL,Static,defaultTaskBuffer,defaultTaskControlBlock
+FREERTOS.configENABLE_BACKWARD_COMPATIBILITY=1
+FREERTOS.configTOTAL_HEAP_SIZE=4096
+FREERTOS.configUSE_MUTEXES=1
 File.Version=6
 I2C2.IPParameters=Timing
 I2C2.Timing=0x20B0D9FF
@@ -78,8 +88,8 @@ Mcu.Pin22=PA14
 Mcu.Pin23=PA15
 Mcu.Pin24=PC10
 Mcu.Pin25=PB4
-Mcu.Pin26=PB6
-Mcu.Pin27=PB7
+Mcu.Pin26=PB5
+Mcu.Pin27=PB6
 Mcu.Pin28=PB9
 Mcu.Pin29=VP_FREERTOS_VS_CMSIS_V1
 Mcu.Pin3=PA2
@@ -110,7 +120,7 @@ NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true
-NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true
+NVIC.TIM1_TRG_COM_TIM17_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true
 NVIC.TimeBase=TIM1_TRG_COM_TIM17_IRQn
 NVIC.TimeBaseIP=TIM17
 NVIC.UCPD1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:false
@@ -192,13 +202,13 @@ PB2.Signal=GPIO_Output
 PB4.Locked=true
 PB4.Mode=Sink_AllSignals
 PB4.Signal=UCPD1_CC2
+PB5.GPIOParameters=GPIO_Label
+PB5.GPIO_Label=FPGA_RESET
+PB5.Locked=true
+PB5.Signal=GPIO_Output
 PB6.Locked=true
 PB6.Mode=Sink_AllSignals
 PB6.Signal=UCPD1_CC1
-PB7.GPIOParameters=GPIO_Label
-PB7.GPIO_Label=FPGA_RESET
-PB7.Locked=true
-PB7.Signal=GPIO_Output
 PB9.GPIOParameters=GPIO_Label
 PB9.GPIO_Label=FPGA_DONE
 PB9.Locked=true