diff --git a/bootloader/targets/f5/target.c b/bootloader/targets/f5/target.c
index d515e9851..1250a07e6 100644
--- a/bootloader/targets/f5/target.c
+++ b/bootloader/targets/f5/target.c
@@ -15,6 +15,7 @@
 // Boot request enum
 #define BOOT_REQUEST_NONE 0x00000000
 #define BOOT_REQUEST_DFU 0xDF00B000
+#define BOOT_REQUEST_TAINTED 0xDF00F000
 // Boot to DFU pin
 #define BOOT_DFU_PORT GPIOB
 #define BOOT_DFU_PIN LL_GPIO_PIN_11
@@ -136,8 +137,12 @@ void target_init() {
 
 int target_is_dfu_requested() {
     if(LL_RTC_BAK_GetRegister(RTC, LL_RTC_BKP_DR0) == BOOT_REQUEST_DFU) {
-        LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_NONE);
         return 1;
+    } else if(LL_RTC_BAK_GetRegister(RTC, LL_RTC_BKP_DR0) == BOOT_REQUEST_TAINTED) {
+        // We came here directly from STM bootloader and chip is unusable
+        // One more reset required to fix it
+        LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_NONE);
+        NVIC_SystemReset();
     }
     LL_mDelay(100);
     if(!LL_GPIO_IsInputPinSet(BOOT_DFU_PORT, BOOT_DFU_PIN)) {
@@ -159,6 +164,8 @@ void target_switch(void* offset) {
 
 void target_switch2dfu() {
     target_led_control("B");
+    // Mark system as tainted, it will be soon
+    LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_TAINTED);
     // Remap memory to system bootloader
     LL_SYSCFG_SetRemapMemory(LL_SYSCFG_REMAP_SYSTEMFLASH);
     target_switch(0x0);
@@ -166,6 +173,6 @@ void target_switch2dfu() {
 
 void target_switch2os() {
     target_led_control("G");
-    SCB->VTOR = BOOT_ADDRESS + OS_OFFSET;
+    SCB->VTOR = OS_OFFSET;
     target_switch((void*)(BOOT_ADDRESS + OS_OFFSET));
 }
diff --git a/bootloader/targets/f5/target.mk b/bootloader/targets/f5/target.mk
index d8ba9c2a2..13170b8e8 100644
--- a/bootloader/targets/f5/target.mk
+++ b/bootloader/targets/f5/target.mk
@@ -5,7 +5,7 @@ FW_ADDRESS		= 0x08008000
 OS_OFFSET		= 0x00008000
 FLASH_ADDRESS	= 0x08000000
 
-OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f ../debug/stm32wbx.cfg -c "stm32wbx.cpu configure -rtos auto" -c "init"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f ../debug/stm32wbx.cfg -c "init"
 BOOT_CFLAGS		= -DBOOT_ADDRESS=$(BOOT_ADDRESS) -DFW_ADDRESS=$(FW_ADDRESS) -DOS_OFFSET=$(OS_OFFSET)
 MCU_FLAGS		= -mcpu=cortex-m4 -mthumb -mfpu=fpv4-sp-d16 -mfloat-abi=hard
 
@@ -44,3 +44,5 @@ C_SOURCES		+= ../lib/version/version.c
 ASM_SOURCES		+= $(wildcard $(TARGET_DIR)/*.s)
 C_SOURCES		+= $(wildcard $(TARGET_DIR)/*.c)
 CPP_SOURCES		+= $(wildcard $(TARGET_DIR)/*.cpp)
+
+SVD_FILE = ../debug/STM32WB55_CM4.svd
diff --git a/bootloader/targets/f6/target.c b/bootloader/targets/f6/target.c
index d515e9851..1250a07e6 100644
--- a/bootloader/targets/f6/target.c
+++ b/bootloader/targets/f6/target.c
@@ -15,6 +15,7 @@
 // Boot request enum
 #define BOOT_REQUEST_NONE 0x00000000
 #define BOOT_REQUEST_DFU 0xDF00B000
+#define BOOT_REQUEST_TAINTED 0xDF00F000
 // Boot to DFU pin
 #define BOOT_DFU_PORT GPIOB
 #define BOOT_DFU_PIN LL_GPIO_PIN_11
@@ -136,8 +137,12 @@ void target_init() {
 
 int target_is_dfu_requested() {
     if(LL_RTC_BAK_GetRegister(RTC, LL_RTC_BKP_DR0) == BOOT_REQUEST_DFU) {
-        LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_NONE);
         return 1;
+    } else if(LL_RTC_BAK_GetRegister(RTC, LL_RTC_BKP_DR0) == BOOT_REQUEST_TAINTED) {
+        // We came here directly from STM bootloader and chip is unusable
+        // One more reset required to fix it
+        LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_NONE);
+        NVIC_SystemReset();
     }
     LL_mDelay(100);
     if(!LL_GPIO_IsInputPinSet(BOOT_DFU_PORT, BOOT_DFU_PIN)) {
@@ -159,6 +164,8 @@ void target_switch(void* offset) {
 
 void target_switch2dfu() {
     target_led_control("B");
+    // Mark system as tainted, it will be soon
+    LL_RTC_BAK_SetRegister(RTC, LL_RTC_BKP_DR0, BOOT_REQUEST_TAINTED);
     // Remap memory to system bootloader
     LL_SYSCFG_SetRemapMemory(LL_SYSCFG_REMAP_SYSTEMFLASH);
     target_switch(0x0);
@@ -166,6 +173,6 @@ void target_switch2dfu() {
 
 void target_switch2os() {
     target_led_control("G");
-    SCB->VTOR = BOOT_ADDRESS + OS_OFFSET;
+    SCB->VTOR = OS_OFFSET;
     target_switch((void*)(BOOT_ADDRESS + OS_OFFSET));
 }
diff --git a/bootloader/targets/f6/target.mk b/bootloader/targets/f6/target.mk
index d8ba9c2a2..13170b8e8 100644
--- a/bootloader/targets/f6/target.mk
+++ b/bootloader/targets/f6/target.mk
@@ -5,7 +5,7 @@ FW_ADDRESS		= 0x08008000
 OS_OFFSET		= 0x00008000
 FLASH_ADDRESS	= 0x08000000
 
-OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f ../debug/stm32wbx.cfg -c "stm32wbx.cpu configure -rtos auto" -c "init"
+OPENOCD_OPTS	= -f interface/stlink.cfg -c "transport select hla_swd" -f ../debug/stm32wbx.cfg -c "init"
 BOOT_CFLAGS		= -DBOOT_ADDRESS=$(BOOT_ADDRESS) -DFW_ADDRESS=$(FW_ADDRESS) -DOS_OFFSET=$(OS_OFFSET)
 MCU_FLAGS		= -mcpu=cortex-m4 -mthumb -mfpu=fpv4-sp-d16 -mfloat-abi=hard
 
@@ -44,3 +44,5 @@ C_SOURCES		+= ../lib/version/version.c
 ASM_SOURCES		+= $(wildcard $(TARGET_DIR)/*.s)
 C_SOURCES		+= $(wildcard $(TARGET_DIR)/*.c)
 CPP_SOURCES		+= $(wildcard $(TARGET_DIR)/*.cpp)
+
+SVD_FILE = ../debug/STM32WB55_CM4.svd