From 7cfab876e73f994c95697ba1b09a0a18c2602597 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Sat, 27 Mar 2021 19:14:37 +0500
Subject: [PATCH 01/18] flash_loader: fix typo in the stm32l4 loader

---
 flashloaders/stm32l4.s        | 6 +++---
 src/stlink-lib/flash_loader.c | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/flashloaders/stm32l4.s b/flashloaders/stm32l4.s
index 21e926f49..50676be9a 100644
--- a/flashloaders/stm32l4.s
+++ b/flashloaders/stm32l4.s
@@ -12,7 +12,7 @@
     .global copy
 copy:
     ldr r12, flash_base
-    ldr r10, flash_off_bsy
+    ldr r10, flash_off_sr
     add r10, r10, r12
 
 loop:
@@ -44,5 +44,5 @@ exit:
     .align 2
 flash_base:
     .word 0x40022000
-flash_off_bsy:
-    .word 0x12
+flash_off_sr:
+    .word 0x10
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index 9fa897ccc..20fd6aeed 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -114,7 +114,7 @@ static const uint8_t loader_code_stm32l4[] = {
     0x08, 0x3a, 0xf0, 0xdc,
     0x00, 0xbe, 0x00, 0xbf,
     0x00, 0x20, 0x02, 0x40,
-    0x12, 0x00, 0x00, 0x00
+    0x10, 0x00, 0x00, 0x00
 };
 
 static const uint8_t loader_code_stm32f7[] = {

From c0653f6595e1c564f5a762982ef0c23a00ff3dea Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Sat, 27 Mar 2021 19:17:13 +0500
Subject: [PATCH 02/18] flash_loader: improved handling errors

---
 flashloaders/Makefile         |  2 +-
 src/stlink-lib/flash_loader.c | 19 +++++++++++++++++--
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/flashloaders/Makefile b/flashloaders/Makefile
index d4ca4d22a..8b01d5293 100644
--- a/flashloaders/Makefile
+++ b/flashloaders/Makefile
@@ -31,7 +31,7 @@ stm32vl.o: stm32f0.s
 	$(CC) stm32f0.s $(CFLAGS_ARMV7_M) -o stm32vl.o
 
 # generic rule for all other ARMv7-M
-%.o: *.s
+%.o: %.s
 	$(CC) $< $(CFLAGS_ARMV7_M) -o $@
 
 clean:
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index 20fd6aeed..aea60f258 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -154,6 +154,7 @@ static const uint8_t loader_code_stm32f7_lv[] = {
 
 int stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl) {
     size_t size = 0;
+    uint32_t dfsr, cfsr, hfsr;
 
     // allocate the loader in SRAM
     if (stlink_flash_loader_write_to_sram(sl, &fl->loader_addr, &size) == -1) {
@@ -165,6 +166,20 @@ int stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl) {
     fl->buf_addr = fl->loader_addr + (uint32_t)size;
     ILOG("Successfully loaded flash loader in sram\n");
 
+    /* Clear Fault Status Register for handling flash loader error */
+    if (!stlink_read_debug32(sl, STLINK_REG_DFSR, &dfsr) && dfsr) {
+        ILOG("Clear DFSR\n");
+        stlink_write_debug32(sl, STLINK_REG_DFSR, dfsr);
+    }
+    if (!stlink_read_debug32(sl, STLINK_REG_CFSR, &cfsr) && cfsr) {
+        ILOG("Clear CFSR\n");
+        stlink_write_debug32(sl, STLINK_REG_CFSR, cfsr);
+    }
+    if (!stlink_read_debug32(sl, STLINK_REG_HFSR, &hfsr) && hfsr) {
+        ILOG("Clear HFSR\n");
+        stlink_write_debug32(sl, STLINK_REG_HFSR, hfsr);
+    }
+
     return(0);
 }
 
@@ -348,7 +363,7 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
     // check written byte count
     stlink_read_reg(sl, 2, &rr);
 
-    if (rr.r[2] != 0) {
+    if ((int32_t)rr.r[2] > 0) {
         ELOG("Write error\n");
         goto error;
     }
@@ -364,7 +379,7 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
     stlink_read_all_regs(sl, &rr);
 
     WLOG("Loader state: R2 0x%X R15 0x%X\n", rr.r[2], rr.r[15]);
-    if (dhcsr != 0x3000B || dfsr != 0x3 || cfsr || hfsr) {
+    if (dhcsr != 0x3000B || dfsr || cfsr || hfsr) {
         WLOG("MCU state: DHCSR 0x%X DFSR 0x%X CFSR 0x%X HFSR 0x%X\n",
             dhcsr, dfsr, cfsr, hfsr);
     }

From bb96d613dd78f34b112dcbc91af0363315b7d0ff Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Sat, 27 Mar 2021 19:37:20 +0500
Subject: [PATCH 03/18] Fix is_flash_busy on STM32F1 XL device

---
 src/common.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/common.c b/src/common.c
index a17392cf1..640f37799 100644
--- a/src/common.c
+++ b/src/common.c
@@ -993,7 +993,7 @@ static inline unsigned int is_flash_busy(stlink_t *sl) {
     unsigned int res;
 
     if ((sl->flash_type == STLINK_FLASH_TYPE_F0) ||
-        (sl->flash_type == STLINK_FLASH_TYPE_F0) ||
+        (sl->flash_type == STLINK_FLASH_TYPE_F1_XL) ||
         (sl->flash_type == STLINK_FLASH_TYPE_L0)) {
         sr_busy_shift = FLASH_SR_BSY;
     } else if (sl->flash_type == STLINK_FLASH_TYPE_F4) {
@@ -1010,7 +1010,7 @@ static inline unsigned int is_flash_busy(stlink_t *sl) {
     } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
         sr_busy_shift = FLASH_H7_SR_QW;
     } else {
-        ELOG("unsupported flash method, abort\n");
+        ELOG("unsupported flash method\n");
         return(-1);
     }
 

From be9c9dec7661f2b9e30fb26b85808bc132ee2c80 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Sun, 28 Mar 2021 21:36:12 +0500
Subject: [PATCH 04/18] flash_loader: fixed check of the flash loader results

---
 src/stlink-lib/flash_loader.c | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index aea60f258..992f0a827 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -363,7 +363,13 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
     // check written byte count
     stlink_read_reg(sl, 2, &rr);
 
-    if ((int32_t)rr.r[2] > 0) {
+    /* The chunk size for loading is not rounded. The flash loader 
+     * subtracts the size of the written block (1-8 bytes) from 
+     * the remaining size each time. A negative value may mean that 
+     * several bytes garbage has been written due to the unaligned 
+     * firmware size.
+     */
+    if ((int32_t)rr.r[2] > 0 || (int32_t)rr.r[2] < -7) {
         ELOG("Write error\n");
         goto error;
     }

From d0d52690d680d603264b90f6328e54aec4be793c Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 29 Mar 2021 19:51:55 +0500
Subject: [PATCH 05/18] Added error checking in the FLASH_SR register after
 write and erase operations

---
 src/common.c | 214 +++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 175 insertions(+), 39 deletions(-)

diff --git a/src/common.c b/src/common.c
index 640f37799..7ebbb8d11 100644
--- a/src/common.c
+++ b/src/common.c
@@ -159,6 +159,9 @@
 
 // G0/G4 FLASH status register
 #define STM32Gx_FLASH_SR_ERROR_MASK (0x3fa)
+#define STM32Gx_FLASH_SR_PROGERR    (3)
+#define STM32Gx_FLASH_SR_WRPERR     (4)
+#define STM32Gx_FLASH_SR_PGAERR     (5)
 #define STM32Gx_FLASH_SR_BSY        (16)      /* FLASH_SR Busy */
 #define STM32Gx_FLASH_SR_EOP        (0)       /* FLASH_EOP End of Operation */
 
@@ -188,11 +191,15 @@
 #define STM32WB_FLASH_SRRVR     (STM32WB_FLASH_REGS_ADDR + 0x84)
 
 // WB Flash control register.
-#define STM32WB_FLASH_CR_STRT       (16) /* FLASH_CR Start */
-#define STM32WB_FLASH_CR_OPTLOCK    (30) /* FLASH_CR Option Lock */
-#define STM32WB_FLASH_CR_LOCK       (31) /* FLASH_CR Lock */
+#define STM32WB_FLASH_CR_STRT       (16) /* Start */
+#define STM32WB_FLASH_CR_OPTLOCK    (30) /* Option Lock */
+#define STM32WB_FLASH_CR_LOCK       (31) /* Lock */
 // WB Flash status register.
-#define STM32WB_FLASH_SR_BSY        (16) /* FLASH_SR Busy */
+#define STM32WB_FLASH_SR_ERROR_MASK (0x3f8) /* SR [9:3] */
+#define STM32WB_FLASH_SR_PROGERR    (3)  /* Programming alignment error */
+#define STM32WB_FLASH_SR_WRPERR     (4)  /* Write protection error */
+#define STM32WB_FLASH_SR_PGAERR     (5)  /* Programming error */
+#define STM32WB_FLASH_SR_BSY        (16) /* Busy */
 
 // 32L4 register base is at FLASH_REGS_ADDR (0x40022000)
 #define STM32L4_FLASH_KEYR      (FLASH_REGS_ADDR + 0x08)
@@ -201,8 +208,11 @@
 #define STM32L4_FLASH_CR        (FLASH_REGS_ADDR + 0x14)
 #define STM32L4_FLASH_OPTR      (FLASH_REGS_ADDR + 0x20)
 
+#define STM32L4_FLASH_SR_ERROR_MASK 0x3f8 /* SR [9:3] */
+#define STM32L4_FLASH_SR_PROGERR     3
+#define STM32L4_FLASH_SR_WRPERR      4
+#define STM32L4_FLASH_SR_PGAERR      5
 #define STM32L4_FLASH_SR_BSY        16
-#define STM32L4_FLASH_SR_ERRMASK 0x3f8 /* SR [9:3] */
 
 #define STM32L4_FLASH_CR_LOCK       31 /* Lock control register */
 #define STM32L4_FLASH_CR_OPTLOCK    30 /* Lock option bytes */
@@ -234,7 +244,10 @@
 #define STM32L0_FLASH_OPTLOCK (2)
 #define STM32L0_FLASH_OBL_LAUNCH (18)
 
-#define STM32L0_FLASH_SR_ERROR_MASK 0x00003F00
+#define STM32L0_FLASH_SR_ERROR_MASK 0x00013F00
+#define STM32L0_FLASH_SR_WRPERR     8
+#define STM32L0_FLASH_SR_PGAERR     9
+#define STM32L0_FLASH_SR_NOTZEROERR 16
 
 #define FLASH_ACR_OFF     ((uint32_t) 0x00)
 #define FLASH_PECR_OFF    ((uint32_t) 0x04)
@@ -287,7 +300,10 @@
 #define FLASH_F4_CR_SER 1
 #define FLASH_F4_CR_SNB 3
 #define FLASH_F4_CR_SNB_MASK 0xf8
-#define FLASH_F4_SR_BSY 16
+#define FLASH_F4_SR_ERROR_MASK 0x000000F0
+#define FLASH_F4_SR_PGAERR     5
+#define FLASH_F4_SR_WRPERR     4
+#define FLASH_F4_SR_BSY        16
 
 // STM32F2
 #define FLASH_F2_REGS_ADDR ((uint32_t)0x40023c00)
@@ -980,7 +996,7 @@ static inline uint32_t read_flash_sr(stlink_t *sl, unsigned bank) {
     } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
         sr_reg = (bank==BANK_1)?FLASH_H7_SR1:FLASH_H7_SR2;
     } else {
-        ELOG("unsupported flash method, abort\n");
+        ELOG("method 'read_flash_sr' is unsupported\n");
         return(-1);
     }
 
@@ -988,6 +1004,35 @@ static inline uint32_t read_flash_sr(stlink_t *sl, unsigned bank) {
     return(res);
 }
 
+static inline int write_flash_sr(stlink_t *sl, unsigned bank, uint32_t val) {
+    uint32_t sr_reg;
+
+    if ((sl->flash_type == STLINK_FLASH_TYPE_F0) ||
+        (sl->flash_type == STLINK_FLASH_TYPE_F1_XL)) {
+        sr_reg = (bank==BANK_1)?FLASH_SR:FLASH_SR2;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_L0) {
+        sr_reg = get_stm32l0_flash_base(sl) + FLASH_SR_OFF;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_F4) {
+        sr_reg = FLASH_F4_SR;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_F7) {
+        sr_reg = FLASH_F7_SR;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_L4) {
+        sr_reg = STM32L4_FLASH_SR;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_G0 ||
+               sl->flash_type == STLINK_FLASH_TYPE_G4) {
+        sr_reg = STM32Gx_FLASH_SR;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_WB) {
+        sr_reg = STM32WB_FLASH_SR;
+    } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
+        sr_reg = (bank==BANK_1)?FLASH_H7_SR1:FLASH_H7_SR2;
+    } else {
+        ELOG("method 'write_flash_sr' is unsupported\n");
+        return(-1);
+    }
+
+    return stlink_write_debug32(sl, sr_reg, val);
+}
+
 static inline unsigned int is_flash_busy(stlink_t *sl) {
     uint32_t sr_busy_shift;
     unsigned int res;
@@ -1010,7 +1055,7 @@ static inline unsigned int is_flash_busy(stlink_t *sl) {
     } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
         sr_busy_shift = FLASH_H7_SR_QW;
     } else {
-        ELOG("unsupported flash method\n");
+        ELOG("method 'is_flash_busy' is unsupported\n");
         return(-1);
     }
 
@@ -1048,43 +1093,120 @@ static void wait_flash_busy_progress(stlink_t *sl) {
     fprintf(stdout, "\n");
 }
 
+static void clear_flash_error(stlink_t *sl)
+{
+    switch (sl->flash_type) {
+    case STLINK_FLASH_TYPE_F0:
+        write_flash_sr(sl, BANK_1, FLASH_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_F4:
+        write_flash_sr(sl, BANK_1, FLASH_F4_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_F7:
+        write_flash_sr(sl, BANK_1, FLASH_F7_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_G0:
+    case STLINK_FLASH_TYPE_G4:
+        write_flash_sr(sl, BANK_1, STM32Gx_FLASH_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_L0:
+        write_flash_sr(sl, BANK_1, STM32L0_FLASH_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_L4:
+        write_flash_sr(sl, BANK_1, STM32L4_FLASH_SR_ERROR_MASK);
+        break;
+    case STLINK_FLASH_TYPE_H7:
+        write_flash_sr(sl, BANK_1, FLASH_H7_SR_ERROR_MASK);
+        if (sl->chip_flags & CHIP_F_HAS_DUAL_BANK) {
+            write_flash_sr(sl, BANK_2, FLASH_H7_SR_ERROR_MASK);
+        }
+        break;
+    case STLINK_FLASH_TYPE_WB:
+        write_flash_sr(sl, BANK_1, STM32WB_FLASH_SR_ERROR_MASK);
+        break;
+    default:
+        break;
+    }
+}
+
 static int check_flash_error(stlink_t *sl)
 {
     uint32_t res = 0;
+    uint32_t WRPERR, PROGERR, PGAERR;
+
+    WRPERR = PROGERR = PGAERR = 0;
 
     switch (sl->flash_type) {
     case STLINK_FLASH_TYPE_F0:
+    case STLINK_FLASH_TYPE_F1_XL:
         res = read_flash_sr(sl, BANK_1) & FLASH_SR_ERROR_MASK;
+        if (sl->flash_type == STLINK_FLASH_TYPE_F1_XL) {
+            res |= read_flash_sr(sl, BANK_2) & FLASH_SR_ERROR_MASK;
+        }
+        WRPERR  = (1 << FLASH_SR_WRPRT_ERR);
+        PROGERR = (1 << FLASH_SR_PG_ERR);
+        break;
+    case STLINK_FLASH_TYPE_F4:
+        res = read_flash_sr(sl, BANK_1) & FLASH_F4_SR_ERROR_MASK;
+        WRPERR  = (1 << FLASH_F4_SR_WRPERR);
+        PGAERR  = (1 << FLASH_F4_SR_PGAERR);
         break;
     case STLINK_FLASH_TYPE_F7:
         res = read_flash_sr(sl, BANK_1) & FLASH_F7_SR_ERROR_MASK;
+        WRPERR  = (1 << FLASH_F7_SR_WRP_ERR);
+        PROGERR = (1 << FLASH_F7_SR_PGP_ERR);
         break;
     case STLINK_FLASH_TYPE_G0:
     case STLINK_FLASH_TYPE_G4:
         res = read_flash_sr(sl, BANK_1) & STM32Gx_FLASH_SR_ERROR_MASK;
+        WRPERR  = (1 << STM32Gx_FLASH_SR_WRPERR);
+        PROGERR = (1 << STM32Gx_FLASH_SR_PROGERR);
+        PGAERR  = (1 << STM32Gx_FLASH_SR_PGAERR);
         break;
     case STLINK_FLASH_TYPE_L0:
         res = read_flash_sr(sl, BANK_1) & STM32L0_FLASH_SR_ERROR_MASK;
+        WRPERR  = (1 << STM32L0_FLASH_SR_WRPERR);
+        PROGERR = (1 << STM32L0_FLASH_SR_NOTZEROERR);
+        PGAERR  = (1 << STM32L0_FLASH_SR_PGAERR);
+        break;
+    case STLINK_FLASH_TYPE_L4:
+        res = read_flash_sr(sl, BANK_1) & STM32L4_FLASH_SR_ERROR_MASK;
+        WRPERR  = (1 << STM32L4_FLASH_SR_WRPERR);
+        PROGERR = (1 << STM32L4_FLASH_SR_PROGERR);
+        PGAERR  = (1 << STM32L4_FLASH_SR_PGAERR);
         break;
     case STLINK_FLASH_TYPE_H7:
         res = read_flash_sr(sl, BANK_1) & FLASH_H7_SR_ERROR_MASK;
         if (sl->chip_flags & CHIP_F_HAS_DUAL_BANK) {
             res |= read_flash_sr(sl, BANK_2) & FLASH_H7_SR_ERROR_MASK;
         }
-        if (res) {
-            // Clear errors
-            stlink_write_debug32(sl, FLASH_H7_CCR1, res);
-            if (sl->chip_flags & CHIP_F_HAS_DUAL_BANK) {
-                stlink_write_debug32(sl, FLASH_H7_CCR2, res);
-            }
-        }
+        WRPERR  = (1 << FLASH_H7_SR_WRPERR);
+        break;
+    case STLINK_FLASH_TYPE_WB:
+        res = read_flash_sr(sl, BANK_1) & STM32WB_FLASH_SR_ERROR_MASK;
+        WRPERR  = (1 << STM32WB_FLASH_SR_WRPERR);
+        PROGERR = (1 << STM32WB_FLASH_SR_PROGERR);
+        PGAERR  = (1 << STM32WB_FLASH_SR_PGAERR);
         break;
     default:
         break;
     }
 
     if (res) {
-        ELOG("Flash programming error : %#010x\n", res);
+        if (WRPERR && (WRPERR&res)==WRPERR) {
+            ELOG("Flash memory is write protected\n");
+            res &= ~WRPERR;
+        } else if (PROGERR && (PROGERR&res)==PROGERR) {
+            ELOG("Flash memory contains a non-erased value\n");
+            res &= ~PROGERR;
+        } else if (PGAERR && (PGAERR&res)==PGAERR) {
+            ELOG("Invalid flash address\n");
+            res &= ~PGAERR;
+        }
+
+        if (res) {
+            ELOG("Flash programming error: %#010x\n", res);
+        }
         return(-1);
     }
 
@@ -2372,12 +2494,14 @@ uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr) {
  * @return 0 on success -ve on failure
  */
 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
+    // wait for ongoing op to finish
+    wait_flash_busy(sl);
+    // clear flash IO errors
+    clear_flash_error(sl);
+
     if (sl->flash_type == STLINK_FLASH_TYPE_F4 ||
         sl->flash_type == STLINK_FLASH_TYPE_F7 ||
         sl->flash_type == STLINK_FLASH_TYPE_L4) {
-        // wait for ongoing op to finish
-        wait_flash_busy(sl);
-
         // unlock if locked
         unlock_flash_if(sl);
 
@@ -2498,7 +2622,6 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
                sl->flash_type == STLINK_FLASH_TYPE_G0 ||
                sl->flash_type == STLINK_FLASH_TYPE_G4) {
         uint32_t val;
-        wait_flash_busy(sl);  // wait for any ongoing Flash operation to finish
         unlock_flash_if(sl);
         set_flash_cr_per(sl, BANK_1); // set the 'enable Flash erase' bit
 
@@ -2535,7 +2658,6 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
     } else if (sl->flash_type == STLINK_FLASH_TYPE_F0 ||
                sl->flash_type == STLINK_FLASH_TYPE_F1_XL) {
         unsigned bank = (flashaddr < STM32_F1_FLASH_BANK2_BASE)?BANK_1:BANK_2;
-        wait_flash_busy(sl);
         unlock_flash_if(sl);
         clear_flash_cr_pg(sl, bank);         // clear the pg bit
         set_flash_cr_per(sl, bank);          // set the page erase bit
@@ -2545,7 +2667,6 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
         lock_flash(sl);
     } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
         unsigned bank = (flashaddr < STM32_H7_FLASH_BANK2_BASE)?BANK_1:BANK_2;
-        wait_flash_busy(sl); // wait for ongoing op to finish
         unlock_flash_if(sl); // unlock if locked
         uint32_t sector = calculate_H7_sectornum(sl, flashaddr, bank);// calculate the actual page from the address
         write_flash_cr_snb(sl, sector, bank); // select the page to erase
@@ -2557,8 +2678,7 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
         return(-1);
     }
 
-    // TODO: verify the erased page
-    return(0);
+    return check_flash_error(sl);
 }
 
 int stlink_erase_flash_mass(stlink_t *sl) {
@@ -2574,7 +2694,7 @@ int stlink_erase_flash_mass(stlink_t *sl) {
             // addr must be an addr inside the page
             stm32_addr_t addr = (stm32_addr_t)sl->flash_base + i * (stm32_addr_t)sl->flash_pgsz;
 
-            if (stlink_erase_flash_page(sl, addr) == -1) {
+            if (stlink_erase_flash_page(sl, addr)) {
                 WLOG("Failed to erase_flash_page(%#x) == -1\n", addr);
                 return(-1);
             }
@@ -2586,6 +2706,7 @@ int stlink_erase_flash_mass(stlink_t *sl) {
         fprintf(stdout, "\n");
     } else {
         wait_flash_busy(sl);
+        clear_flash_error(sl);
         unlock_flash_if(sl);
 
         if (sl->flash_type == STLINK_FLASH_TYPE_H7 && sl->chip_id != STLINK_CHIPID_STM32_H7AX) {
@@ -2596,7 +2717,7 @@ int stlink_erase_flash_mass(stlink_t *sl) {
             }
         }
 
-        set_flash_cr_mer(sl, 1, BANK_1);      // set the mass erase bit
+        set_flash_cr_mer(sl, 1, BANK_1); // set the mass erase bit
         set_flash_cr_strt(sl, BANK_1);   // start erase operation, reset by hw with busy bit
 
         if (sl->flash_type == STLINK_FLASH_TYPE_F1_XL ||
@@ -2606,7 +2727,6 @@ int stlink_erase_flash_mass(stlink_t *sl) {
         }
 
         wait_flash_busy_progress(sl);
-        err = check_flash_error(sl);
         lock_flash(sl);
 
         // reset the mass erase bit
@@ -2616,7 +2736,7 @@ int stlink_erase_flash_mass(stlink_t *sl) {
             set_flash_cr_mer(sl, 0, BANK_2);
         }
 
-        // TODO: verify the erased memory
+        err = check_flash_error(sl);
     }
 
     return(err);
@@ -2751,6 +2871,11 @@ int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
                                         STLINK_REG_DHCSR_C_HALT |
                                         STLINK_REG_DHCSR_C_MASKINTS);
 
+    // wait for ongoing op to finish
+    wait_flash_busy(sl);
+    // Clear errors
+    clear_flash_error(sl);
+
     if ((sl->flash_type == STLINK_FLASH_TYPE_F4) ||
         (sl->flash_type == STLINK_FLASH_TYPE_F7) ||
         (sl->flash_type == STLINK_FLASH_TYPE_L4)) {
@@ -2801,7 +2926,6 @@ int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
                sl->flash_type == STLINK_FLASH_TYPE_G4) {
         ILOG("Starting Flash write for WB/G0/G4\n");
 
-        wait_flash_busy(sl);
         unlock_flash_if(sl); // unlock flash if necessary
         set_flash_cr_pg(sl, BANK_1); // set PG 'allow programming' bit
     } else if (sl->flash_type == STLINK_FLASH_TYPE_L0) {
@@ -2882,6 +3006,7 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr
             size_t size = len - off > buf_size ? buf_size : len - off;
             if (stlink_flash_loader_run(sl, fl, addr + (uint32_t)off, base + off, size) == -1) {
                 ELOG("stlink_flash_loader_run(%#x) failed! == -1\n", (unsigned)(addr + off));
+                check_flash_error(sl);
                 return(-1);
             }
 
@@ -2977,6 +3102,7 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr
 
             if (stlink_flash_loader_run(sl, fl, addr + (uint32_t)off, base + off, size) == -1) {
                 ELOG("stlink_flash_loader_run(%#x) failed! == -1\n", (unsigned)(addr + off));
+                check_flash_error(sl);
                 return(-1);
             }
 
@@ -3015,7 +3141,7 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr
         return(-1);
     }
 
-    return(0);
+    return check_flash_error(sl);
 }
 
 int stlink_flashloader_stop(stlink_t *sl) {
@@ -3405,6 +3531,8 @@ static int stlink_write_option_bytes_gx(
     (void)len;
     uint32_t data;
 
+    clear_flash_error(sl);
+
     write_uint32((unsigned char*)&data, *(uint32_t*)(base));
     WLOG("Writing option bytes %#10x to %#10x\n", data, addr);
     stlink_write_debug32(sl, STM32Gx_FLASH_OPTR, data);
@@ -3441,7 +3569,7 @@ static int stlink_write_option_bytes_l0(
     int ret = 0;
 
     // Clear errors
-    stlink_write_debug32(sl, flash_base + FLASH_SR_OFF, STM32L0_FLASH_REGS_ADDR);
+    clear_flash_error(sl);
 
     while (len != 0) {
         write_uint32((unsigned char*)&data, *(uint32_t*)(base)); // write options bytes
@@ -3480,6 +3608,9 @@ static int stlink_write_option_bytes_l4(
     (void)addr;
     (void)len;
 
+    // Clear errors
+    clear_flash_error(sl);
+
     // write options bytes
     uint32_t data;
     write_uint32((unsigned char*)&data, *(uint32_t*)(base));
@@ -3516,6 +3647,9 @@ static int stlink_write_option_bytes_f4(
     (void)addr;
     (void)len;
 
+    // Clear errors
+    clear_flash_error(sl);
+
     write_uint32((unsigned char*)&option_byte, *(uint32_t*)(base));
 
     // write option byte, ensuring we dont lock opt, and set strt bit
@@ -3539,8 +3673,8 @@ static int stlink_write_option_bytes_f7(stlink_t *sl, uint8_t* base, stm32_addr_
     uint32_t option_byte;
     int ret = 0;
 
-    //(void) addr;
-    //(void) len;
+    // Clear errors
+    clear_flash_error(sl);
 
     ILOG("Asked to write option byte %#10x to %#010x.\n", *(uint32_t*) (base), addr);
     write_uint32((unsigned char*) &option_byte, *(uint32_t*) (base));
@@ -3999,9 +4133,10 @@ int stlink_write_option_bytes(stlink_t *sl, stm32_addr_t addr, uint8_t* base, ui
 static int stlink_write_option_control_register_f7(stlink_t *sl, uint32_t option_control_register) {
     int ret = 0;
 
+    // Clear errors
+    clear_flash_error(sl);
+
     ILOG("Asked to write option control register 1 %#10x to %#010x.\n", option_control_register, FLASH_F7_OPTCR);
-    //write_uint32((unsigned char*) &option_byte, *(uint32_t*) (base));
-    //ILOG("Write %d option bytes %#010x to %#010x!\n", len, option_byte, addr);
 
     /* write option byte, ensuring we dont lock opt, and set strt bit */
     stlink_write_debug32(sl, FLASH_F7_OPTCR, (option_control_register & ~(1 << FLASH_F7_OPTCR_LOCK)) | (1 << FLASH_F7_OPTCR_START));
@@ -4010,7 +4145,7 @@ static int stlink_write_option_control_register_f7(stlink_t *sl, uint32_t option
 
     ret = check_flash_error(sl);
     if (!ret)
-        ILOG("Wrote option bytes %#010x to %#010x!\n", option_control_register, FLASH_F7_OPTCR1);
+        ILOG("Wrote option bytes %#010x to %#010x!\n", option_control_register, FLASH_F7_OPTCR);
 
     return ret;
 }
@@ -4024,9 +4159,10 @@ static int stlink_write_option_control_register_f7(stlink_t *sl, uint32_t option
 static int stlink_write_option_control_register1_f7(stlink_t *sl, uint32_t option_control_register1) {
     int ret = 0;
 
+    // Clear errors
+    clear_flash_error(sl);
+
     ILOG("Asked to write option control register 1 %#010x to %#010x.\n", option_control_register1, FLASH_F7_OPTCR1);
-    //write_uint32((unsigned char*) &option_byte, *(uint32_t*) (base));
-    //ILOG("Write %d option bytes %#010x to %#010x!\n", len, option_byte, addr);
 
     /* write option byte, ensuring we dont lock opt, and set strt bit */
     uint32_t current_control_register_value;

From 817c8ad436eada97294fa93e6cc8e57fb1be9504 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 29 Mar 2021 20:31:57 +0500
Subject: [PATCH 06/18] flash_loader: fixed setup PG flag in F0/VL flash
 loaders, code cleared

---
 flashloaders/stm32f0.s        |  6 ++--
 src/common.c                  | 58 ++++++++++-------------------------
 src/stlink-lib/flash_loader.c | 18 +++++------
 3 files changed, 26 insertions(+), 56 deletions(-)

diff --git a/flashloaders/stm32f0.s b/flashloaders/stm32f0.s
index e2a4b0f00..23f1c43fe 100644
--- a/flashloaders/stm32f0.s
+++ b/flashloaders/stm32f0.s
@@ -35,10 +35,8 @@ copy:
     ldr r5, flash_off_sr
     add r5, r5, r7
 
-    # FLASH_CR |= 0x01 (set PG)
-    ldr r7, =0x1
-    ldr r4, [r6]
-    orrs r4, r4, r7
+    # FLASH_CR = 0x01 (set PG)
+    ldr r4, =0x1
     str r4, [r6]
 
 loop:
diff --git a/src/common.c b/src/common.c
index 7ebbb8d11..8585ff881 100644
--- a/src/common.c
+++ b/src/common.c
@@ -394,13 +394,21 @@ uint16_t read_uint16(const unsigned char *c, const int pt) {
 static uint32_t get_stm32l0_flash_base(stlink_t *sl)
 {
     switch (sl->chip_id) {
+    case STLINK_CHIPID_STM32_L0:
+    case STLINK_CHIPID_STM32_L0_CAT5:
+    case STLINK_CHIPID_STM32_L0_CAT2:
+    case STLINK_CHIPID_STM32_L011:
+		return(STM32L0_FLASH_REGS_ADDR);
+
     case STLINK_CHIPID_STM32_L1_CAT2:
     case STLINK_CHIPID_STM32_L1_MEDIUM:
     case STLINK_CHIPID_STM32_L1_MEDIUM_PLUS:
     case STLINK_CHIPID_STM32_L1_HIGH:
         return(STM32L1_FLASH_REGS_ADDR);
+
     default:
-        return(STM32L0_FLASH_REGS_ADDR);
+		WLOG("Flash base use default L0 address");
+		return(STM32L0_FLASH_REGS_ADDR);
     }
 }
 
@@ -2549,16 +2557,7 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
     } else if (sl->flash_type == STLINK_FLASH_TYPE_L0) {
 
         uint32_t val;
-        uint32_t flash_regs_base;
-
-        if (sl->chip_id == STLINK_CHIPID_STM32_L0 ||
-            sl->chip_id == STLINK_CHIPID_STM32_L0_CAT5 ||
-            sl->chip_id == STLINK_CHIPID_STM32_L0_CAT2 ||
-            sl->chip_id == STLINK_CHIPID_STM32_L011) {
-            flash_regs_base = STM32L0_FLASH_REGS_ADDR;
-        } else {
-            flash_regs_base = STM32L_FLASH_REGS_ADDR;
-        }
+        uint32_t flash_regs_base = get_stm32l0_flash_base(sl);
 
         // check if the locks are set
         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
@@ -2592,15 +2591,6 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
         // set pecr.{erase,prog}
         val |= (1 << 9) | (1 << 3);
         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
-#if 0
-        /* Wait for sr.busy to be cleared
-         * MP: Test shows that busy bit is not set here. Perhaps, PM0062 is wrong
-         * and we do not need to wait here for clearing the busy bit.
-         */
-        do {
-            stlink_read_debug32(sl, STM32L_FLASH_SR, &val)
-        } while ((val & (1 << 0)) != 0);
-#endif
 
         // write 0 to the first word of the page to be erased
         stlink_write_debug32(sl, flashaddr, 0);
@@ -2610,9 +2600,7 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
          * Test shows that a few iterations is performed in the following loop
          * before busy bit is cleared.
          */
-        do
-            stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
-        while ((val & (1 << 0)) != 0);
+        wait_flash_busy(sl);
 
         // reset lock bits
         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
@@ -2652,7 +2640,7 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
         }
 
         set_flash_cr_strt(sl, BANK_1);  // set the 'start operation' bit
-        wait_flash_busy(sl);    // wait for the 'busy' bit to clear
+        wait_flash_busy(sl);            // wait for the 'busy' bit to clear
         clear_flash_cr_per(sl, BANK_1); // clear the 'enable page erase' bit
         lock_flash(sl);
     } else if (sl->flash_type == STLINK_FLASH_TYPE_F0 ||
@@ -2664,6 +2652,7 @@ int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr) {
         write_flash_ar(sl, flashaddr, bank); // select the page to erase
         set_flash_cr_strt(sl, bank);         // start erase operation, reset by hw with busy bit
         wait_flash_busy(sl);
+        clear_flash_cr_per(sl, bank);        // clear the page erase bit
         lock_flash(sl);
     } else if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
         unsigned bank = (flashaddr < STM32_H7_FLASH_BANK2_BASE)?BANK_1:BANK_2;
@@ -2796,18 +2785,9 @@ int stm32l1_write_half_pages(
     unsigned int count;
     unsigned int num_half_pages = len / pagesize;
     uint32_t val;
-    uint32_t flash_regs_base;
+    uint32_t flash_regs_base = get_stm32l0_flash_base(sl);
     flash_loader_t fl;
 
-    if (sl->chip_id == STLINK_CHIPID_STM32_L0 ||
-        sl->chip_id == STLINK_CHIPID_STM32_L0_CAT5 ||
-        sl->chip_id == STLINK_CHIPID_STM32_L0_CAT2 ||
-        sl->chip_id == STLINK_CHIPID_STM32_L011) {
-        flash_regs_base = STM32L0_FLASH_REGS_ADDR;
-    } else {
-        flash_regs_base = STM32L_FLASH_REGS_ADDR;
-    }
-
     ILOG("Starting Half page flash write for STM32L core id\n");
 
     /* Flash loader initialisation */
@@ -2824,9 +2804,7 @@ int stm32l1_write_half_pages(
     val |= (1 << FLASH_L1_PROG);
     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
 
-    do {
-        stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
-    } while ((val & (1 << 0)) != 0);
+    wait_flash_busy(sl);
 
     for (count = 0; count  < num_half_pages; count++) {
         if (stlink_flash_loader_run(
@@ -2845,9 +2823,7 @@ int stm32l1_write_half_pages(
             fflush(stdout);
         }
 
-        do {
-            stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
-        } while ((val & (1 << 0)) != 0);
+        wait_flash_busy(sl);
     }
 
     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
@@ -3096,8 +3072,6 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr
             // unlock and set programming mode
             unlock_flash_if(sl);
 
-            if (sl->flash_type != STLINK_FLASH_TYPE_F1_XL) { set_flash_cr_pg(sl, BANK_1); }
-
             DLOG("Finished unlocking flash, running loader!\n");
 
             if (stlink_flash_loader_run(sl, fl, addr + (uint32_t)off, base + off, size) == -1) {
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index 992f0a827..e3b1a12ea 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -14,11 +14,10 @@
 /* flashloaders/stm32f0.s -- compiled with thumb2 */
 static const uint8_t loader_code_stm32vl[] = {
     0x00, 0xbf, 0x00, 0xbf,
-    0x0f, 0x4f, 0x1f, 0x44,
-    0x0f, 0x4e, 0x3e, 0x44,
-    0x0f, 0x4d, 0x3d, 0x44,
-    0x4f, 0xf0, 0x01, 0x07,
-    0x34, 0x68, 0x3c, 0x43,
+    0x0e, 0x4f, 0x1f, 0x44,
+    0x0e, 0x4e, 0x3e, 0x44,
+    0x0e, 0x4d, 0x3d, 0x44,
+    0x4f, 0xf0, 0x01, 0x04,
     0x34, 0x60, 0x04, 0x88,
     0x0c, 0x80, 0x02, 0x30,
     0x02, 0x31, 0x4f, 0xf0,
@@ -38,11 +37,10 @@ static const uint8_t loader_code_stm32vl[] = {
 /* flashloaders/stm32f0.s -- thumb1 only, same sequence as for STM32VL, bank ignored */
 static const uint8_t loader_code_stm32f0[] = {
     0xc0, 0x46, 0xc0, 0x46,
-    0x0d, 0x4f, 0x1f, 0x44,
-    0x0d, 0x4e, 0x3e, 0x44,
-    0x0d, 0x4d, 0x3d, 0x44,
-    0x0d, 0x4f, 0x34, 0x68,
-    0x3c, 0x43, 0x34, 0x60,
+    0x0c, 0x4f, 0x1f, 0x44,
+    0x0c, 0x4e, 0x3e, 0x44,
+    0x0c, 0x4d, 0x3d, 0x44,
+    0x0c, 0x4c, 0x34, 0x60,
     0x04, 0x88, 0x0c, 0x80,
     0x02, 0x30, 0x02, 0x31,
     0x09, 0x4f, 0x2c, 0x68,

From b62cdcfab70e362b34b5875ad86b31c9e6fd6c73 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 29 Mar 2021 20:40:22 +0500
Subject: [PATCH 07/18] Added disabling the IWDG and WWDG in halt state

---
 src/common.c | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 67 insertions(+), 1 deletion(-)

diff --git a/src/common.c b/src/common.c
index 8585ff881..26a3b3859 100644
--- a/src/common.c
+++ b/src/common.c
@@ -362,6 +362,25 @@
 #define FLASH_H7_OPTSR_CUR (FLASH_H7_REGS_ADDR + 0x1c)
 #define FLASH_H7_OPTCCR (FLASH_H7_REGS_ADDR + 0x24)
 
+#define STM32F0_DBGMCU_CR                 0xE0042004
+#define STM32F0_DBGMCU_CR_IWDG_STOP       8
+#define STM32F0_DBGMCU_CR_WWDG_STOP       9
+
+#define STM32F4_DBGMCU_APB1FZR1           0xE0042008
+#define STM32F4_DBGMCU_APB1FZR1_WWDG_STOP 11
+#define STM32F4_DBGMCU_APB1FZR1_IWDG_STOP 12
+
+#define STM32L0_DBGMCU_APB1_FZ            0x40015808
+#define STM32L0_DBGMCU_APB1_FZ_WWDG_STOP  11
+#define STM32L0_DBGMCU_APB1_FZ_IWDG_STOP  12
+
+#define STM32H7_DBGMCU_APB1HFZ            0x5C001054
+#define STM32H7_DBGMCU_APB1HFZ_IWDG_STOP  18
+
+#define STM32WB_DBGMCU_APB1FZR1           0xE004203C
+#define STM32WB_DBGMCU_APB1FZR1_WWDG_STOP 11
+#define STM32WB_DBGMCU_APB1FZR1_IWDG_STOP 12
+
 #define L1_WRITE_BLOCK_SIZE 0x80
 #define L0_WRITE_BLOCK_SIZE 0x40
 
@@ -1221,6 +1240,50 @@ static int check_flash_error(stlink_t *sl)
     return(0);
 }
 
+static void stop_wdg_in_debug(stlink_t *sl) {
+    uint32_t dbgmcu_cr;
+    uint32_t set;
+    uint32_t value;
+
+    switch (sl->flash_type) {
+    case STLINK_FLASH_TYPE_F0:
+    case STLINK_FLASH_TYPE_F1_XL:
+    case STLINK_FLASH_TYPE_G4:
+        dbgmcu_cr = STM32F0_DBGMCU_CR;
+        set = (1<<STM32F0_DBGMCU_CR_IWDG_STOP) |
+              (1<<STM32F0_DBGMCU_CR_WWDG_STOP);
+        break;
+    case STLINK_FLASH_TYPE_F4:
+    case STLINK_FLASH_TYPE_F7:
+    case STLINK_FLASH_TYPE_L4:
+        dbgmcu_cr = STM32F4_DBGMCU_APB1FZR1;
+        set = (1<<STM32F4_DBGMCU_APB1FZR1_IWDG_STOP) |
+              (1<<STM32F4_DBGMCU_APB1FZR1_WWDG_STOP);
+        break;
+    case STLINK_FLASH_TYPE_L0:
+    case STLINK_FLASH_TYPE_G0:
+        dbgmcu_cr = STM32L0_DBGMCU_APB1_FZ;
+        set = (1<<STM32L0_DBGMCU_APB1_FZ_IWDG_STOP) |
+              (1<<STM32L0_DBGMCU_APB1_FZ_WWDG_STOP);
+        break;
+    case STLINK_FLASH_TYPE_H7:
+        dbgmcu_cr = STM32H7_DBGMCU_APB1HFZ;
+        set = (1<<STM32H7_DBGMCU_APB1HFZ_IWDG_STOP);
+        break;
+    case STLINK_FLASH_TYPE_WB:
+        dbgmcu_cr = STM32WB_DBGMCU_APB1FZR1;
+        set = (1<<STM32WB_DBGMCU_APB1FZR1_IWDG_STOP) |
+              (1<<STM32WB_DBGMCU_APB1FZR1_WWDG_STOP);
+        break;
+    default:
+        return;
+    }
+
+    if (!stlink_read_debug32(sl, dbgmcu_cr, &value)) {
+        stlink_write_debug32(sl, dbgmcu_cr, value | set);
+    }
+}
+
 static inline void write_flash_ar(stlink_t *sl, uint32_t n, unsigned bank) {
     stlink_write_debug32(sl, (bank==BANK_1)?FLASH_AR:FLASH_AR2, n);
 }
@@ -1319,7 +1382,10 @@ int stlink_enter_swd_mode(stlink_t *sl) {
 // Force the core into the debug mode -> halted state.
 int stlink_force_debug(stlink_t *sl) {
     DLOG("*** stlink_force_debug_mode ***\n");
-    return(sl->backend->force_debug(sl));
+    int res = sl->backend->force_debug(sl);
+    // Stop the watchdogs in the halted state for suppress target reboot
+    stop_wdg_in_debug(sl);
+    return(res);
 }
 
 int stlink_exit_dfu_mode(stlink_t *sl) {

From b0192478c8262476ead421890b1550d6a7b72ecc Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 29 Mar 2021 20:56:03 +0500
Subject: [PATCH 08/18] Added disabling the DMA channels

---
 inc/stlink.h             |  3 +-
 src/common.c             | 83 +++++++++++++++++++++++++++++++++++++++-
 src/st-flash/flash.c     | 13 -------
 src/st-util/gdb-server.c |  2 +-
 src/stlink-gui/gui.c     | 12 ------
 5 files changed, 84 insertions(+), 29 deletions(-)

diff --git a/inc/stlink.h b/inc/stlink.h
index 7bb1bc51e..61b8472f1 100644
--- a/inc/stlink.h
+++ b/inc/stlink.h
@@ -137,6 +137,7 @@ typedef uint32_t stm32_addr_t;
 typedef struct flash_loader {
     stm32_addr_t loader_addr; // loader sram addr
     stm32_addr_t buf_addr; // buffer sram address
+    uint32_t rcc_dma_bkp; // backup RCC DMA enable state
 } flash_loader_t;
 
 typedef struct _cortex_m3_cpuid_ {
@@ -297,7 +298,7 @@ int stlink_fwrite_option_bytes(stlink_t *sl, const char* path, stm32_addr_t addr
 
 int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl);
 int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr, uint8_t* base, uint32_t len);
-int stlink_flashloader_stop(stlink_t *sl);
+int stlink_flashloader_stop(stlink_t *sl, flash_loader_t *fl);
 
 #include <sg.h>
 #include <usb.h>
diff --git a/src/common.c b/src/common.c
index 26a3b3859..60d7ff145 100644
--- a/src/common.c
+++ b/src/common.c
@@ -381,6 +381,27 @@
 #define STM32WB_DBGMCU_APB1FZR1_WWDG_STOP 11
 #define STM32WB_DBGMCU_APB1FZR1_IWDG_STOP 12
 
+#define STM32F1_RCC_AHBENR       0x40021014
+#define STM32F1_RCC_DMAEN        0x00000003 // DMA2EN | DMA1EN
+
+#define STM32F4_RCC_AHB1ENR      0x40023830
+#define STM32F4_RCC_DMAEN        0x00600000 // DMA2EN | DMA1EN
+
+#define STM32G0_RCC_AHBENR       0x40021038
+#define STM32G0_RCC_DMAEN        0x00000003 // DMA2EN | DMA1EN
+
+#define STM32G4_RCC_AHB1ENR      0x40021048
+#define STM32G4_RCC_DMAEN        0x00000003 // DMA2EN | DMA1EN
+
+#define STM32L0_RCC_AHBENR       0x40021030
+#define STM32L0_RCC_DMAEN        0x00000001 // DMAEN
+
+#define STM32H7_RCC_AHB1ENR      0x58024538
+#define STM32H7_RCC_DMAEN        0x00000003 // DMA2EN | DMA1EN
+
+#define STM32WB_RCC_AHB1ENR      0x58000048
+#define STM32WB_RCC_DMAEN        0x00000003 // DMA2EN | DMA1EN
+
 #define L1_WRITE_BLOCK_SIZE 0x80
 #define L0_WRITE_BLOCK_SIZE 0x40
 
@@ -1284,6 +1305,58 @@ static void stop_wdg_in_debug(stlink_t *sl) {
     }
 }
 
+static void set_dma_state(stlink_t *sl, flash_loader_t* fl, int bckpRstr) {
+    uint32_t rcc, rcc_dma_mask, value;
+
+    rcc = rcc_dma_mask = value = 0;
+
+    switch (sl->flash_type) {
+    case STLINK_FLASH_TYPE_F0:
+    case STLINK_FLASH_TYPE_F1_XL:
+        rcc = STM32F1_RCC_AHBENR;
+        rcc_dma_mask = STM32F1_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_F4:
+    case STLINK_FLASH_TYPE_F7:
+        rcc = STM32F4_RCC_AHB1ENR;
+        rcc_dma_mask = STM32F4_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_G0:
+        rcc = STM32G0_RCC_AHBENR;
+        rcc_dma_mask = STM32G0_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_G4:
+    case STLINK_FLASH_TYPE_L4:
+        rcc = STM32G4_RCC_AHB1ENR;
+        rcc_dma_mask = STM32G4_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_L0:
+        rcc = STM32L0_RCC_AHBENR;
+        rcc_dma_mask = STM32L0_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_H7:
+        rcc = STM32H7_RCC_AHB1ENR;
+        rcc_dma_mask = STM32H7_RCC_DMAEN;
+        break;
+    case STLINK_FLASH_TYPE_WB:
+        rcc = STM32WB_RCC_AHB1ENR;
+        rcc_dma_mask = STM32WB_RCC_DMAEN;
+        break;
+    default:
+        return;
+    }
+
+    if (!stlink_read_debug32(sl, rcc, &value)) {
+        if (bckpRstr) {
+            value = (value&(~rcc_dma_mask)) | fl->rcc_dma_bkp;
+        } else {
+            fl->rcc_dma_bkp = value&rcc_dma_mask;
+            value &= ~rcc_dma_mask;
+        }
+        stlink_write_debug32(sl, rcc, value);
+    }
+}
+
 static inline void write_flash_ar(stlink_t *sl, uint32_t n, unsigned bank) {
     stlink_write_debug32(sl, (bank==BANK_1)?FLASH_AR:FLASH_AR2, n);
 }
@@ -2913,6 +2986,9 @@ int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
                                         STLINK_REG_DHCSR_C_HALT |
                                         STLINK_REG_DHCSR_C_MASKINTS);
 
+    // disable DMA
+    set_dma_state(sl, fl, 0);
+
     // wait for ongoing op to finish
     wait_flash_busy(sl);
     // Clear errors
@@ -3184,7 +3260,7 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr
     return check_flash_error(sl);
 }
 
-int stlink_flashloader_stop(stlink_t *sl) {
+int stlink_flashloader_stop(stlink_t *sl, flash_loader_t *fl) {
     uint32_t dhcsr;
 
     if ((sl->flash_type == STLINK_FLASH_TYPE_F4) ||
@@ -3224,6 +3300,9 @@ int stlink_flashloader_stop(stlink_t *sl) {
                                 (dhcsr&(~STLINK_REG_DHCSR_C_MASKINTS)));
     }
 
+    // restore DMA state
+    set_dma_state(sl, fl, 1);
+
     return(0);
 }
 
@@ -3286,7 +3365,7 @@ int stlink_write_flash(
     ret = stlink_flashloader_write(sl, &fl, addr, base, len);
     if (ret)
         return ret;
-    ret = stlink_flashloader_stop(sl);
+    ret = stlink_flashloader_stop(sl, &fl);
     if (ret)
         return ret;
 
diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index 5a226d57b..e870e116c 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -112,19 +112,6 @@ int main(int ac, char** av) {
         }
     }
 
-    // disable DMA - Set All DMA CCR Registers to zero. - AKS 1/7/2013
-    if (sl->chip_id == STLINK_CHIPID_STM32_F4) {
-        memset(sl->q_buf, 0, 4);
-        
-        for (int i = 0; i < 8; i++) {
-            stlink_write_mem32(sl, 0x40026000 + 0x10 + 0x18 * i, 4);
-            stlink_write_mem32(sl, 0x40026400 + 0x10 + 0x18 * i, 4);
-            stlink_write_mem32(sl, 0x40026000 + 0x24 + 0x18 * i, 4);
-            stlink_write_mem32(sl, 0x40026400 + 0x24 + 0x18 * i, 4);
-
-        }
-    }
-
     // core must be halted to use RAM based flashloaders
     if (stlink_force_debug(sl)) {
         printf("Failed to halt the core\n");
diff --git a/src/st-util/gdb-server.c b/src/st-util/gdb-server.c
index 3dc3073e5..8d63d9ee0 100644
--- a/src/st-util/gdb-server.c
+++ b/src/st-util/gdb-server.c
@@ -908,7 +908,7 @@ static int flash_go(stlink_t *sl) {
         }
     }
 
-    stlink_flashloader_stop(sl);
+    stlink_flashloader_stop(sl, &fl);
     stlink_soft_reset(sl, 1 /* halt on reset */);
     error = 0;
 
diff --git a/src/stlink-gui/gui.c b/src/stlink-gui/gui.c
index ed38fcc7c..d264cf9a5 100644
--- a/src/stlink-gui/gui.c
+++ b/src/stlink-gui/gui.c
@@ -514,18 +514,6 @@ static void connect_button_cb(GtkWidget *widget, gpointer data) {
         stlink_enter_swd_mode(gui->sl);
     }
 
-    // disable DMA - Set All DMA CCR Registers to zero. - AKS 1/7/2013
-    if (gui->sl->chip_id == STLINK_CHIPID_STM32_F4) {
-        memset(gui->sl->q_buf, 0, 4);
-
-        for (i = 0; i < 8; i++) {
-            stlink_write_mem32(gui->sl, 0x40026000 + 0x10 + 0x18 * i, 4);
-            stlink_write_mem32(gui->sl, 0x40026400 + 0x10 + 0x18 * i, 4);
-            stlink_write_mem32(gui->sl, 0x40026000 + 0x24 + 0x18 * i, 4);
-            stlink_write_mem32(gui->sl, 0x40026400 + 0x24 + 0x18 * i, 4);
-        }
-    }
-
     stlink_gui_set_connected(gui);
 }
 

From c793ca348fab43711cc3d24e8b9956ba90ca03d3 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 29 Mar 2021 22:19:48 +0500
Subject: [PATCH 09/18] Fixed GUI compilation

---
 src/stlink-gui/gui.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/stlink-gui/gui.c b/src/stlink-gui/gui.c
index d264cf9a5..03e999b7f 100644
--- a/src/stlink-gui/gui.c
+++ b/src/stlink-gui/gui.c
@@ -489,7 +489,6 @@ static void stlink_gui_set_connected(STlinkGUI *gui) {
 
 static void connect_button_cb(GtkWidget *widget, gpointer data) {
     STlinkGUI *gui;
-    gint i;
     (void)widget;
 
     gui = STLINK_GUI(data);

From 4334ce48b2c03497a6d415f8982fdb986409398c Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Sun, 4 Apr 2021 11:45:21 +0500
Subject: [PATCH 10/18] flash_loader: added reset of IWDG If IWDG is enabled by
 hardware, then IWDG cannot be disabled by software

---
 inc/stlink.h                  |  1 +
 src/stlink-lib/flash_loader.c | 17 +++++++++++++++++
 2 files changed, 18 insertions(+)

diff --git a/inc/stlink.h b/inc/stlink.h
index 61b8472f1..855e5126d 100644
--- a/inc/stlink.h
+++ b/inc/stlink.h
@@ -138,6 +138,7 @@ typedef struct flash_loader {
     stm32_addr_t loader_addr; // loader sram addr
     stm32_addr_t buf_addr; // buffer sram address
     uint32_t rcc_dma_bkp; // backup RCC DMA enable state
+    uint32_t iwdg_kr; // IWDG key register address
 } flash_loader_t;
 
 typedef struct _cortex_m3_cpuid_ {
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index e3b1a12ea..80262cf07 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -9,6 +9,11 @@
 #define FLASH_REGS_BANK2_OFS 0x40
 #define FLASH_BANK2_START_ADDR 0x08080000
 
+#define STM32F0_WDG_KR            0x40003000
+#define STM32H7_WDG_KR            0x58004800
+
+#define STM32F0_WDG_KR_KEY_RELOAD 0xAAAA
+
 /* DO NOT MODIFY SOURCECODE DIRECTLY, EDIT ASSEMBLY FILES INSTEAD */
 
 /* flashloaders/stm32f0.s -- compiled with thumb2 */
@@ -164,6 +169,13 @@ int stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl) {
     fl->buf_addr = fl->loader_addr + (uint32_t)size;
     ILOG("Successfully loaded flash loader in sram\n");
 
+    // set address of IWDG key register for reset it
+    if (sl->flash_type == STLINK_FLASH_TYPE_H7) {
+        fl->iwdg_kr = STM32H7_WDG_KR;
+    } else {
+        fl->iwdg_kr = STM32F0_WDG_KR;
+    }
+
     /* Clear Fault Status Register for handling flash loader error */
     if (!stlink_read_debug32(sl, STLINK_REG_DFSR, &dfsr) && dfsr) {
         ILOG("Clear DFSR\n");
@@ -329,6 +341,11 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
                                                // only used on VL/F1_XL, but harmless for others
     stlink_write_reg(sl, fl->loader_addr, 15); // pc register
 
+    /* Reset IWDG */
+    if (fl->iwdg_kr) {
+        stlink_write_debug32(sl, fl->iwdg_kr, STM32F0_WDG_KR_KEY_RELOAD);
+    }
+
     /* Run loader */
     stlink_run(sl);
 

From 9a8e0aa6457640e27a0dbee5d994c3f8249fddf6 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 5 Apr 2021 21:15:18 +0500
Subject: [PATCH 11/18] Added the hot plug option to st-flash

---
 doc/tutorial.md           | 2 +-
 src/st-flash/flash.c      | 4 ++--
 src/st-flash/flash_opts.c | 2 ++
 3 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/doc/tutorial.md b/doc/tutorial.md
index 37c29ab6a..256513bc4 100644
--- a/doc/tutorial.md
+++ b/doc/tutorial.md
@@ -9,7 +9,7 @@
 | --opt                 | st-flash                           | Optimisation can be enabled in order to skip flashing empty (0x00 or 0xff) bytes at the end of binary file.<br />This may cause some garbage data left after a flash operation. This option was enabled by default in earlier releases.                                                                                                                                                                                  | v1.6.1               |
 | --reset               | st-flash                           | Trigger a reset both before and after flashing. The default uses the hardware reset through `NRST` pin.<br />A software reset (via `AIRCR`; since v1.5.1) is used, if the hardware reset failed (`NRST` pin not connected).                                                                                                                                                                                              | v1.0.0               |
 | --connect-under-reset | st-info<br />st-flash              | Connect under reset. Option makes it possible to connect to the device before code execution. This is useful<br />when the target contains code that lets the device go to sleep, disables debug pins or other special code.                                                                                                                                                                                             | v1.6.1               |
-| --hot-plug            | st-info<br />st-util               | Connect to the target without reset.                                                                                                                                                                                                                                                                                                                                                                                     | v1.6.2               |
+| --hot-plug            | st-info<br />st-flash<br />st-util | Connect to the target without reset.                                                                                                                                                                                                                                                                                                                                                                                     | v1.6.2               |
 | --probe               | st-info                            | Display hardware information about the connected programmer and target MCU.                                                                                                                                                                                                                                                                                                                                              | v1.2.0               |
 | --version             | st-info<br />st-flash<br />st-util | Print version information.                                                                                                                                                                                                                                                                                                                                                                                               | v1.3.0               |
 | --help                | st-flash<br />st-util              | Print list of available commands.                                                                                                                                                                                                                                                                                                                                                                                        |                      |
diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index e870e116c..356515ac4 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -26,8 +26,8 @@ static void cleanup(int signum) {
 }
 
 static void usage(void) {
-    puts("command line:   ./st-flash [--debug] [--reset] [--connect-under-reset] [--opt] [--serial <serial>] [--format <format>] [--flash=<fsize>] [--freq=<KHz>] [--area=<area>] {read|write} [path] [addr] [size]");
-    puts("command line:   ./st-flash [--debug] [--connect-under-reset] [--freq=<KHz>] [--serial <serial>] erase");
+    puts("command line:   ./st-flash [--debug] [--reset] [--connect-under-reset] [--hot-plug] [--opt] [--serial <serial>] [--format <format>] [--flash=<fsize>] [--freq=<KHz>] [--area=<area>] {read|write} [path] [addr] [size]");
+    puts("command line:   ./st-flash [--debug] [--connect-under-reset] [--hot-plug] [--freq=<KHz>] [--serial <serial>] erase");
     puts("command line:   ./st-flash [--debug] [--freq=<KHz>] [--serial <serial>] reset");
     puts("   <addr>, <serial> and <size>: Use hex format.");
     puts("   <fsize>: Use decimal, octal or hex (prefix 0xXXX) format, optionally followed by k=KB, or m=MB (eg. --flash=128k)");
diff --git a/src/st-flash/flash_opts.c b/src/st-flash/flash_opts.c
index 63774ea25..172a24468 100644
--- a/src/st-flash/flash_opts.c
+++ b/src/st-flash/flash_opts.c
@@ -193,6 +193,8 @@ int flash_get_opts(struct flash_opts* o, int ac, char** av) {
             }
         } else if (strcmp(av[0], "--connect-under-reset") == 0) {
             o->connect = CONNECT_UNDER_RESET;
+        } else if (strcmp(av[0], "--hot-plug") == 0) {
+            o->connect = CONNECT_HOT_PLUG;
         } else {
             break; // non-option found
 

From 696ea923ce7517a604d43adfe017c1cdda3b7339 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 5 Apr 2021 21:23:16 +0500
Subject: [PATCH 12/18] Changed the behavior of the st-flash reset option. The
 reset option conflicted with the connection type options (connect under
 reset, hot plug). Now reset of target is performed only after the completion
 of the firmware if the reset options is used.

---
 src/st-flash/flash.c | 14 --------------
 1 file changed, 14 deletions(-)

diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index 356515ac4..edb54e195 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -98,20 +98,6 @@ int main(int ac, char** av) {
         }
     }
 
-    if (o.reset) {
-        if (sl->version.stlink_v > 1) {
-            if (stlink_jtag_reset(sl, 2)) {
-                printf("Failed to reset JTAG\n");
-                goto on_error;
-            }
-        }
-
-        if (stlink_reset(sl)) {
-            printf("Failed to reset device\n");
-            goto on_error;
-        }
-    }
-
     // core must be halted to use RAM based flashloaders
     if (stlink_force_debug(sl)) {
         printf("Failed to halt the core\n");

From d3bc853209a6785a1f87a2359839e80c15735563 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 5 Apr 2021 21:53:57 +0500
Subject: [PATCH 13/18] Update tutorial

---
 doc/tutorial.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/doc/tutorial.md b/doc/tutorial.md
index 256513bc4..21f1498ee 100644
--- a/doc/tutorial.md
+++ b/doc/tutorial.md
@@ -7,7 +7,7 @@
 | --flash=n[k][m]       | st-flash                           | One can specify `--flash=128k` for example, to override the default value of 64k for the STM32F103C8T6<br />to assume 128k of flash being present. This option accepts decimal (128k), octal 0200k, or hex 0x80k values.<br />Leaving the multiplier out is equally valid, e.g.: `--flash=0x20000`. The size may be followed by an optional<br />"k" or "m" to multiply the given value by 1k (1024) or 1M respectively. | v1.4.0               |
 | --freq=n[k][m]        | st-info<br />st-flash<br />st-util | The frequency of the SWD/JTAG interface can be specified, to override the default 1800 kHz configuration.<br />This option solely accepts decimal values (5K or 1.8M) with the unit `Hz` being left out. Valid frequencies are:<br />`5K, 15K, 25K, 50K, 100K, 125K, 240K, 480K, 950K, 1200K, 1800K, 4000K(4M)`.                                                                                                         | v1.6.1               |
 | --opt                 | st-flash                           | Optimisation can be enabled in order to skip flashing empty (0x00 or 0xff) bytes at the end of binary file.<br />This may cause some garbage data left after a flash operation. This option was enabled by default in earlier releases.                                                                                                                                                                                  | v1.6.1               |
-| --reset               | st-flash                           | Trigger a reset both before and after flashing. The default uses the hardware reset through `NRST` pin.<br />A software reset (via `AIRCR`; since v1.5.1) is used, if the hardware reset failed (`NRST` pin not connected).                                                                                                                                                                                              | v1.0.0               |
+| --reset               | st-flash                           | Trigger a reset after flashing. The default uses the hardware reset through `NRST` pin.<br />A software reset (via `AIRCR`; since v1.5.1) is used, if the hardware reset failed (`NRST` pin not connected).                                                                                                                                                                                                              | v1.0.0               |
 | --connect-under-reset | st-info<br />st-flash              | Connect under reset. Option makes it possible to connect to the device before code execution. This is useful<br />when the target contains code that lets the device go to sleep, disables debug pins or other special code.                                                                                                                                                                                             | v1.6.1               |
 | --hot-plug            | st-info<br />st-flash<br />st-util | Connect to the target without reset.                                                                                                                                                                                                                                                                                                                                                                                     | v1.6.2               |
 | --probe               | st-info                            | Display hardware information about the connected programmer and target MCU.                                                                                                                                                                                                                                                                                                                                              | v1.2.0               |

From 4fc6344d44681a0add3bdce69ecf2d0e4e70db49 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Mon, 5 Apr 2021 22:13:17 +0500
Subject: [PATCH 14/18] flash_loader: fixed alignment of size of the lx loader

---
 src/stlink-lib/flash_loader.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index 80262cf07..cd7096490 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -68,7 +68,7 @@ static const uint8_t loader_code_stm32lx[] = {
     0x00, 0xf1, 0x04, 0x00,
     0x01, 0xf1, 0x04, 0x01,
     0x04, 0x3a, 0xf7, 0xdc,
-    0x00, 0xbe
+    0x00, 0xbe, 0x00, 0x00
 };
 
 static const uint8_t loader_code_stm32f4[] = {

From ee43e3b3571c8c7d253b158bbab2d330b3ef19cb Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Tue, 6 Apr 2021 21:13:54 +0500
Subject: [PATCH 15/18] Unification of the reset function

---
 inc/stlink.h             | 24 +++++++++--
 src/common.c             | 91 +++++++++++++++++++++++++++++++++++++--
 src/st-flash/flash.c     | 13 +-----
 src/st-trace/trace.c     |  2 +-
 src/st-util/gdb-server.c | 92 +++++++++++++---------------------------
 src/stlink-lib/sg.c      |  2 +-
 src/stlink-lib/usb.c     | 51 +---------------------
 src/stlink-lib/usb.h     |  6 ---
 tests/sg.c               |  2 +-
 tests/usb.c              |  2 +-
 10 files changed, 145 insertions(+), 140 deletions(-)

diff --git a/inc/stlink.h b/inc/stlink.h
index 855e5126d..8fe6033d6 100644
--- a/inc/stlink.h
+++ b/inc/stlink.h
@@ -58,6 +58,11 @@ enum target_state {
 #define STLINK_JTAG_READDEBUG_32BIT     0x36
 #define STLINK_JTAG_DRIVE_NRST          0x3C
 
+/* NRST pin states */
+#define STLINK_JTAG_DRIVE_NRST_LOW      0x00
+#define STLINK_JTAG_DRIVE_NRST_HIGH     0x01
+#define STLINK_JTAG_DRIVE_NRST_PULSE    0x02
+
 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43
 
 #define STLINK_APIV3_SET_COM_FREQ       0x61
@@ -173,6 +178,19 @@ enum transport_type {
     TRANSPORT_TYPE_INVALID
 };
 
+enum connect_type {
+    CONNECT_HOT_PLUG = 0,
+    CONNECT_NORMAL = 1,
+    CONNECT_UNDER_RESET = 2,
+};
+
+enum reset_type {
+    RESET_AUTO = 0,
+    RESET_HARD = 1,
+    RESET_SOFT = 2,
+    RESET_SOFT_AND_HALT = 3,
+};
+
 typedef struct _stlink stlink_t;
 
 #include <backend.h>
@@ -231,9 +249,7 @@ int stlink_exit_debug_mode(stlink_t *sl);
 int stlink_exit_dfu_mode(stlink_t *sl);
 void stlink_close(stlink_t *sl);
 int stlink_core_id(stlink_t *sl);
-int stlink_reset(stlink_t *sl);
-int stlink_jtag_reset(stlink_t *sl, int value);
-int stlink_soft_reset(stlink_t *sl, int halt_on_reset);
+int stlink_reset(stlink_t *sl, enum reset_type type);
 int stlink_run(stlink_t *sl);
 int stlink_status(stlink_t *sl);
 int stlink_version(stlink_t *sl);
@@ -301,6 +317,8 @@ int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl);
 int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr, uint8_t* base, uint32_t len);
 int stlink_flashloader_stop(stlink_t *sl, flash_loader_t *fl);
 
+int stlink_target_connect(stlink_t *sl, enum connect_type connect);
+
 #include <sg.h>
 #include <usb.h>
 #include <reg.h>
diff --git a/src/common.c b/src/common.c
index 60d7ff145..928091eb4 100644
--- a/src/common.c
+++ b/src/common.c
@@ -1663,10 +1663,6 @@ int stlink_load_device_params(stlink_t *sl) {
     return(0);
 }
 
-int stlink_reset(stlink_t *sl) {
-    DLOG("*** stlink_reset ***\n");
-    return(sl->backend->reset(sl));
-}
 
 int stlink_jtag_reset(stlink_t *sl, int value) {
     DLOG("*** stlink_jtag_reset ***\n");
@@ -1742,6 +1738,59 @@ int stlink_soft_reset(stlink_t *sl, int halt_on_reset) {
     return(0);
 }
 
+int stlink_reset(stlink_t *sl, enum reset_type type) {
+    uint32_t dhcsr;
+    unsigned timeout;
+
+    DLOG("*** stlink_reset ***\n");
+
+    if (type == RESET_AUTO) {
+        // clear S_RESET_ST in DHCSR register for reset state detection
+        stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+    }
+
+    if (type == RESET_HARD || type == RESET_AUTO) {
+        // hardware target reset
+        if (sl->version.stlink_v > 1) { stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_PULSE); }
+        if (sl->backend->reset(sl)) { return(-1); }
+        usleep(10000);
+    }
+
+    if (type == RESET_AUTO) {
+        /* Check if the S_RESET_ST bit is set in DHCSR
+         * This means that a reset has occurred
+         * DDI0337E, p. 10-4, Debug Halting Control and Status Register */
+
+        dhcsr = 0;
+        stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+        if ((dhcsr & STLINK_REG_DHCSR_S_RESET_ST) == 0) {
+            // reset not done yet
+            // try reset through AIRCR so that NRST does not need to be connected
+
+            WLOG("NRST is not connected\n");
+            DLOG("Using reset through SYSRESETREQ\n");
+            return stlink_soft_reset(sl, 0);
+        }
+
+        // waiting for reset the S_RESET_ST bit within 500ms
+        timeout = time_ms() + 500;
+        while (time_ms() < timeout) {
+            dhcsr = STLINK_REG_DHCSR_S_RESET_ST;
+            stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+            if ((dhcsr&STLINK_REG_DHCSR_S_RESET_ST) == 0)
+                return(0);
+        }
+
+        return(-1);
+    }
+
+    if (type == RESET_SOFT || type == RESET_SOFT_AND_HALT) {
+        return stlink_soft_reset(sl, (type==RESET_SOFT_AND_HALT));
+    }
+
+    return(0);
+}
+
 int stlink_run(stlink_t *sl) {
     struct stlink_reg rr;
     DLOG("*** stlink_run ***\n");
@@ -4467,3 +4516,37 @@ int stlink_fwrite_option_bytes(stlink_t *sl, const char* path, stm32_addr_t addr
     return(err);
 }
 
+int stlink_target_connect(stlink_t *sl, enum connect_type connect) {
+    uint32_t dhcsr;
+
+    if (connect == CONNECT_UNDER_RESET) {
+        stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_LOW);
+
+        if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
+        stlink_force_debug(sl);
+
+        // clear S_RESET_ST in DHCSR register
+        stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+
+        stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_HIGH);
+        usleep(10000);
+
+        // check NRST connection
+        dhcsr = 0;
+        stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+        if ((dhcsr & STLINK_REG_DHCSR_S_RESET_ST) == 0) {
+            WLOG("NRST is not connected\n");
+        }
+
+        // addition soft reset for halt before the first instruction
+        stlink_soft_reset(sl, 1 /* halt on reset */);
+    }
+
+    if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
+
+    if (connect == CONNECT_NORMAL) {
+        stlink_reset(sl, RESET_AUTO);
+    }
+
+    return stlink_load_device_params(sl);
+}
diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index edb54e195..5eb607758 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -188,14 +188,7 @@ int main(int ac, char** av) {
             goto on_error;
         }
     } else if (o.cmd == CMD_RESET) {
-        if (sl->version.stlink_v > 1) {
-            if (stlink_jtag_reset(sl, 2)) {
-                printf("Failed to reset JTAG\n");
-                goto on_error;
-            }
-        }
-
-        if (stlink_reset(sl)) {
+        if (stlink_reset(sl, RESET_AUTO)) {
             printf("Failed to reset device\n");
             goto on_error;
         }
@@ -263,9 +256,7 @@ int main(int ac, char** av) {
     }
 
     if (o.reset) {
-        if (sl->version.stlink_v > 1) { stlink_jtag_reset(sl, 2); }
-
-        stlink_reset(sl);
+        stlink_reset(sl, RESET_AUTO);
     }
 
     err = 0; // success
diff --git a/src/st-trace/trace.c b/src/st-trace/trace.c
index 3180335a0..f40a9d683 100644
--- a/src/st-trace/trace.c
+++ b/src/st-trace/trace.c
@@ -203,7 +203,7 @@ static bool enable_trace(stlink_t* stlink, const st_settings_t* settings, uint32
             return false;
     }
 
-    if (settings->reset_board && stlink_jtag_reset(stlink, 2)) {
+    if (settings->reset_board && stlink_reset(stlink, RESET_AUTO)) {
         ELOG("Unable to reset device\n");
         if (!settings->force)
             return false;
diff --git a/src/st-util/gdb-server.c b/src/st-util/gdb-server.c
index 8d63d9ee0..c2a220df3 100644
--- a/src/st-util/gdb-server.c
+++ b/src/st-util/gdb-server.c
@@ -42,9 +42,6 @@
 #define FLASH_PAGE (sl->flash_pgsz)
 
 static stlink_t *connected_stlink = NULL;
-static bool semihosting = false;
-static bool serial_specified = false;
-static char serialnumber[STLINK_SERIAL_BUFFER_SIZE] = {0};
 
 #if defined(_WIN32)
 #define close_socket win32_close_socket
@@ -57,8 +54,6 @@ static char serialnumber[STLINK_SERIAL_BUFFER_SIZE] = {0};
 
 static const char hex[] = "0123456789abcdef";
 
-static const char* current_memory_map = NULL;
-
 typedef struct _st_state_t {
     // things from command line, bleh
     int logging_level;
@@ -66,6 +61,9 @@ typedef struct _st_state_t {
     int persistent;
     enum connect_type connect_mode;
     int freq;
+    char serialnumber[STLINK_SERIAL_BUFFER_SIZE];
+    bool semihosting;
+    const char* current_memory_map;
 } st_state_t;
 
 
@@ -97,20 +95,6 @@ BOOL WINAPI CtrlHandler(DWORD fdwCtrlType) {
 }
 #endif
 
-
-static stlink_t* do_connect(st_state_t *st) {
-    stlink_t *sl = NULL;
-
-    if (serial_specified) {
-        sl = stlink_open_usb(st->logging_level, st->connect_mode, serialnumber, st->freq);
-    } else {
-        sl = stlink_open_usb(st->logging_level, st->connect_mode, NULL, st->freq);
-    }
-
-    return(sl);
-}
-
-
 int parse_options(int argc, char** argv, st_state_t *st) {
     static struct option long_options[] = {
         {"help", no_argument, NULL, 'h'},
@@ -119,6 +103,7 @@ int parse_options(int argc, char** argv, st_state_t *st) {
         {"multi", optional_argument, NULL, 'm'},
         {"no-reset", optional_argument, NULL, 'n'},
         {"hot-plug", optional_argument, NULL, 'n'},
+        {"connect-under-reset", optional_argument, NULL, 'u'},
         {"freq", optional_argument, NULL, 'F'},
         {"version", no_argument, NULL, 'V'},
         {"semihosting", no_argument, NULL, SEMIHOSTING_OPTION},
@@ -138,6 +123,8 @@ int parse_options(int argc, char** argv, st_state_t *st) {
                             "\t\t\tst-util will continue listening for connections after disconnect.\n"
                             "  -n, --no-reset, --hot-plug\n"
                             "\t\t\tDo not reset board on connection.\n"
+                            "  -u, --connect-under-reset\n"
+                            "\t\t\tConnect to the board before executing any instructions.\n"                            
                             "  -F 1800K, --freq=1M\n"
                             "\t\t\tSet the frequency of the SWD/JTAG interface.\n"
                             "  --semihosting\n"
@@ -182,11 +169,14 @@ int parse_options(int argc, char** argv, st_state_t *st) {
             break;
             
         case 'm':
-            st->persistent = 1;
+            st->persistent = true;
             break;
         case 'n':
             st->connect_mode = CONNECT_HOT_PLUG;
             break;
+        case 'u':
+            st->connect_mode = CONNECT_UNDER_RESET;
+            break;
         case 'F':
             st->freq = arg_parse_freq(optarg);
             if (st->freq < 0) {
@@ -198,12 +188,11 @@ int parse_options(int argc, char** argv, st_state_t *st) {
             printf("v%s\n", STLINK_VERSION);
             exit(EXIT_SUCCESS);
         case SEMIHOSTING_OPTION:
-            semihosting = true;
+            st->semihosting = true;
             break;
         case SERIAL_OPTION:
             printf("use serial %s\n", optarg);
-            memcpy(serialnumber, optarg, STLINK_SERIAL_BUFFER_SIZE);
-            serial_specified = true;
+            memcpy(st->serialnumber, optarg, STLINK_SERIAL_BUFFER_SIZE);
             break;
         }
 
@@ -232,8 +221,7 @@ int main(int argc, char** argv) {
 
     printf("st-util\n");
 
-    sl = do_connect(&state);
-
+    sl = stlink_open_usb(state.logging_level, state.connect_mode, state.serialnumber, state.freq);
     if (sl == NULL) { return(1); }
 
     if (sl->chip_id == STLINK_CHIPID_UNKNOWN) {
@@ -241,7 +229,9 @@ int main(int argc, char** argv) {
         return(1);
     }
 
+    sl->verbose = 0;
     connected_stlink = sl;
+
 #if defined(_WIN32)
     SetConsoleCtrlHandler((PHANDLER_ROUTINE) CtrlHandler, TRUE);
 #else
@@ -250,18 +240,14 @@ int main(int argc, char** argv) {
     signal(SIGSEGV, &cleanup);
 #endif
 
-    if (state.connect_mode != CONNECT_HOT_PLUG) { stlink_reset(sl); }
-
     DLOG("Chip ID is %#010x, Core ID is %#08x.\n", sl->chip_id, sl->core_id);
 
-    sl->verbose = 0;
-    current_memory_map = make_memory_map(sl);
+    state.current_memory_map = make_memory_map(sl);
 
 #if defined(_WIN32)
     WSADATA wsadata;
 
     if (WSAStartup(MAKEWORD(2, 2), &wsadata) != 0) { goto winsock_error; }
-
 #endif
 
     init_cache(sl);
@@ -865,16 +851,13 @@ static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
     return(0);
 }
 
-static int flash_go(stlink_t *sl) {
+static int flash_go(stlink_t *sl, st_state_t *st) {
     int error = -1;
     int ret;
     flash_loader_t fl;
 
-    // some kinds of clock settings do not allow writing to flash.
-    stlink_reset(sl);
+    stlink_target_connect(sl, st->connect_mode);
     stlink_force_debug(sl);
-    // delay to ensure that STM32 HSI clock and others have started up fully
-    usleep(10000);
 
     for (struct flash_block* fb = flash_root; fb; fb = fb->next) {
         ILOG("flash_erase: block %08x -> %04x\n", fb->addr, fb->length);
@@ -909,7 +892,7 @@ static int flash_go(stlink_t *sl) {
     }
 
     stlink_flashloader_stop(sl, &fl);
-    stlink_soft_reset(sl, 1 /* halt on reset */);
+    stlink_reset(sl, RESET_SOFT_AND_HALT);
     error = 0;
 
 error:
@@ -1119,10 +1102,9 @@ int serve(stlink_t *sl, st_state_t *st) {
 
     close_socket(sock);
 
+    stlink_target_connect(sl, st->connect_mode);
     stlink_force_debug(sl);
 
-    if (st->connect_mode != CONNECT_HOT_PLUG) { stlink_reset(sl); }
-
     init_code_breakpoints(sl);
     init_data_watchpoints(sl);
 
@@ -1198,7 +1180,7 @@ int serve(stlink_t *sl, st_state_t *st) {
                 if (strcmp(op, "read")) {
                     data = NULL;
                 } else if (!strcmp(type, "memory-map")) {
-                    data = current_memory_map;
+                    data = st->current_memory_map;
                 } else if (!strcmp(type, "features")) {
                     data = target_description;
                 } else {
@@ -1269,22 +1251,14 @@ int serve(stlink_t *sl, st_state_t *st) {
 
                 } else if (!strncmp(cmd, "jtag_reset", 10)) {                   // jtag_reset
                     reply = strdup("OK");
-                    ret = stlink_jtag_reset(sl, 0);
-
-                    if (ret) {
-                        DLOG("Rcmd: jtag_reset failed with jtag_reset\n");
-                        reply = strdup("E00");
-                    }
-
-                    ret = stlink_jtag_reset(sl, 1);
 
+                    ret = stlink_reset(sl, RESET_HARD);
                     if (ret) {
                         DLOG("Rcmd: jtag_reset failed with jtag_reset\n");
                         reply = strdup("E00");
                     }
 
                     ret = stlink_force_debug(sl);
-
                     if (ret) {
                         DLOG("Rcmd: jtag_reset failed with force_debug\n");
                         reply = strdup("E00");
@@ -1297,14 +1271,12 @@ int serve(stlink_t *sl, st_state_t *st) {
                 } else if (!strncmp(cmd, "reset", 5)) {     // reset
 
                     ret = stlink_force_debug(sl);
-
                     if (ret) {
                         DLOG("Rcmd: reset failed with force_debug\n");
                         reply = strdup("E00");
                     }
 
-                    ret = stlink_reset(sl);
-
+                    ret = stlink_reset(sl, RESET_AUTO);
                     if (ret) {
                         DLOG("Rcmd: reset failed with reset\n");
                         reply = strdup("E00");
@@ -1325,10 +1297,10 @@ int serve(stlink_t *sl, st_state_t *st) {
                     while (isspace(*arg)) { arg++; } // skip whitespaces
 
                     if (!strncmp(arg, "enable", 6) || !strncmp(arg, "1", 1)) {
-                        semihosting = true;
+                        st->semihosting = true;
                         reply = strdup("OK");
                     } else if (!strncmp(arg, "disable", 7) || !strncmp(arg, "0", 1)) {
-                        semihosting = false;
+                        st->semihosting = false;
                         reply = strdup("OK");
                     } else {
                         DLOG("Rcmd: unknown semihosting arg: '%s'\n", arg);
@@ -1407,7 +1379,7 @@ int serve(stlink_t *sl, st_state_t *st) {
 
                 free(decoded);
             } else if (!strcmp(cmdName, "FlashDone")) {
-                if (flash_go(sl)) {
+                if (flash_go(sl, st)) {
                     reply = strdup("E08");
                 } else {
                     reply = strdup("OK");
@@ -1453,7 +1425,7 @@ int serve(stlink_t *sl, st_state_t *st) {
                     int offset = 0;
                     uint16_t insn;
 
-                    if (!semihosting) { break; }
+                    if (!st->semihosting) { break; }
 
                     ret = stlink_read_all_regs (sl, &reg);
 
@@ -1821,7 +1793,7 @@ int serve(stlink_t *sl, st_state_t *st) {
 
         case 'R': {
             // reset the core.
-            ret = stlink_reset(sl);
+            ret = stlink_reset(sl, RESET_AUTO);
             if (ret) { DLOG("R packet : stlink_reset failed\n"); }
 
             init_code_breakpoints(sl);
@@ -1835,25 +1807,19 @@ int serve(stlink_t *sl, st_state_t *st) {
         case 'k':
             // kill request - reset the connection itself
             ret = stlink_run(sl);
-
             if (ret) { DLOG("Kill: stlink_run failed\n"); }
 
             ret = stlink_exit_debug_mode(sl);
-
             if (ret) { DLOG("Kill: stlink_exit_debug_mode failed\n"); }
 
             stlink_close(sl);
 
-            sl = do_connect(st);
-
+            sl = stlink_open_usb(st->logging_level, st->connect_mode, st->serialnumber, st->freq);
             if (sl == NULL || sl->chip_id == STLINK_CHIPID_UNKNOWN) { cleanup(0); }
 
             connected_stlink = sl;
 
-            if (st->connect_mode != CONNECT_HOT_PLUG) { stlink_reset(sl); }
-
             ret = stlink_force_debug(sl);
-
             if (ret) { DLOG("Kill: stlink_force_debug failed\n"); }
 
             init_cache(sl);
diff --git a/src/stlink-lib/sg.c b/src/stlink-lib/sg.c
index 840466e75..c5946ed19 100644
--- a/src/stlink-lib/sg.c
+++ b/src/stlink-lib/sg.c
@@ -1114,7 +1114,7 @@ stlink_t* stlink_v1_open(const int verbose, int reset) {
     stlink_enter_swd_mode(sl);
 
     // now we are ready to read the parameters
-    if (reset) { stlink_reset(sl); }
+    if (reset) { stlink_reset(sl, RESET_AUTO); }
 
     stlink_load_device_params(sl);
     ILOG("Successfully opened a stlink v1 debugger\n");
diff --git a/src/stlink-lib/usb.c b/src/stlink-lib/usb.c
index ce0d672c8..99b5f51ef 100644
--- a/src/stlink-lib/usb.c
+++ b/src/stlink-lib/usb.c
@@ -512,13 +512,8 @@ int _stlink_usb_reset(stlink_t * sl) {
     unsigned char* const data = sl->q_buf;
     unsigned char* const cmd = sl->c_buf;
     ssize_t size;
-    uint32_t dhcsr;
-    unsigned timeout;
     int i, rep_len = 2;
 
-    // clear S_RESET_ST in DHCSR registr
-    _stlink_usb_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
-
     // send reset command
     i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
     cmd[i++] = STLINK_DEBUG_COMMAND;
@@ -536,30 +531,7 @@ int _stlink_usb_reset(stlink_t * sl) {
         return((int)size);
     }
 
-    usleep(10000);
-
-    dhcsr = 0;
-    _stlink_usb_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
-    if ((dhcsr & STLINK_REG_DHCSR_S_RESET_ST) == 0) {
-        // reset not done yet
-        // try reset through AIRCR so that NRST does not need to be connected
-
-        WLOG("NRST is not connected\n");
-        DLOG("Using reset through SYSRESETREQ\n");
-        return stlink_soft_reset(sl, 0);
-    }
-
-    // waiting for a reset within 500ms
-    timeout = time_ms() + 500;
-    while (time_ms() < timeout) {
-        // DDI0337E, p. 10-4, Debug Halting Control and Status Register
-        dhcsr = STLINK_REG_DHCSR_S_RESET_ST;
-        _stlink_usb_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
-        if ((dhcsr&STLINK_REG_DHCSR_S_RESET_ST) == 0)
-            return(0);
-    }
-
-    return(-1);
+    return(0);
 }
 
 int _stlink_usb_jtag_reset(stlink_t * sl, int value) {
@@ -1382,26 +1354,7 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect,
     DLOG("JTAG/SWD freq set to %d\n", freq);
     stlink_set_swdclk(sl, freq);
 
-    if (connect == CONNECT_UNDER_RESET) {
-        stlink_jtag_reset(sl, 0);
-
-        if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
-
-        stlink_force_debug(sl);
-        stlink_jtag_reset(sl, 1);
-        usleep(10000);
-    }
-
-    if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
-
-    if (connect == CONNECT_NORMAL) {
-        if ( sl->version.stlink_v > 1) { stlink_jtag_reset(sl, 2); }
-
-        stlink_reset(sl);
-        usleep(10000);
-    }
-
-    stlink_load_device_params(sl);
+    stlink_target_connect(sl, connect);
     return(sl);
 
 on_libusb_error:
diff --git a/src/stlink-lib/usb.h b/src/stlink-lib/usb.h
index f48ef225d..27de32700 100644
--- a/src/stlink-lib/usb.h
+++ b/src/stlink-lib/usb.h
@@ -48,12 +48,6 @@ extern "C" {
 #define STLINK_SG_SIZE 31
 #define STLINK_CMD_SIZE 16
 
-enum connect_type {
-    CONNECT_HOT_PLUG = 0,
-    CONNECT_NORMAL = 1,
-    CONNECT_UNDER_RESET = 2,
-};
-
 struct stlink_libusb {
     libusb_context* libusb_ctx;
     libusb_device_handle* usb_handle;
diff --git a/tests/sg.c b/tests/sg.c
index 745df845f..7584ab5d4 100644
--- a/tests/sg.c
+++ b/tests/sg.c
@@ -46,7 +46,7 @@ int main(void) { // main() ripped out of old stlink-hw.c
     stlink_core_id(sl);
     stlink_status(sl);
     // stlink_force_debug(sl);
-    stlink_reset(sl);
+    stlink_reset(sl, RESET_AUTO);
     stlink_status(sl);
     // core system control block
     stlink_read_mem32(sl, 0xe000ed00, 4);
diff --git a/tests/usb.c b/tests/usb.c
index 03c135f00..5c5c2077d 100644
--- a/tests/usb.c
+++ b/tests/usb.c
@@ -93,7 +93,7 @@ int main(int ac, char** av) {
         stlink_status(sl);
 
         printf("-- reset\n");
-        stlink_reset(sl);
+        stlink_reset(sl, RESET_AUTO);
         stlink_force_debug(sl);
         /* Test reg write */
         stlink_write_reg(sl, 0x01234567, 3);

From 6bd99d4b55cd33c7f3773eda8aadea10e1288b5e Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Tue, 6 Apr 2021 22:03:52 +0500
Subject: [PATCH 16/18] flash_loader: fixed interrupt masking

---
 doc/dev/developer.txt         |  7 ++++---
 doc/tutorial.md               |  2 +-
 inc/backend.h                 |  2 +-
 inc/stlink.h                  |  8 +++++++-
 src/common.c                  |  8 ++++----
 src/st-flash/flash.c          |  2 +-
 src/st-trace/trace.c          |  2 +-
 src/st-util/gdb-server.c      | 16 ++++++++--------
 src/stlink-lib/flash_loader.c |  2 +-
 src/stlink-lib/sg.c           |  3 ++-
 src/stlink-lib/usb.c          |  7 +++++--
 tests/sg.c                    |  4 ++--
 tests/usb.c                   |  2 +-
 13 files changed, 38 insertions(+), 27 deletions(-)

diff --git a/doc/dev/developer.txt b/doc/dev/developer.txt
index 9c4da22f7..e3091c003 100644
--- a/doc/dev/developer.txt
+++ b/doc/dev/developer.txt
@@ -244,18 +244,19 @@ Description:
 Backend:		"jtag_reset"
 Arguments:
   sl:		Pointer to the stlink data structure, returned by stlink_v1_open() or stlink_open_usb()
-  value: 	0: drive low, 1: drive high, 2: ????
+  value: 	0: drive low, 1: drive high, 2: pulse
 Return:		-1 for error. 0 for success.
 
 
 Include:		inc/stlink.h
-Prototype:	int stlink_run(stlink_t *sl);
+Prototype:	int stlink_run(stlink_t *sl, enum run_type type);
 Definition: 	src/common.c
 Description:
   Just calls the backend "run" procedure.
 Backend:		"run"
 Arguments:
-  sl:	Pointer to the stlink data structure, returned by stlink_v1_open() or stlink_open_usb()
+  sl:	    Pointer to the stlink data structure, returned by stlink_v1_open() or stlink_open_usb()
+  type: 	RUN_NORMAL - run target, RUN_FLASH_LOADER - run target with masking interrupts
 Return:		-1 for error. 0 for success.
 
 Include:		inc/stlink.h
diff --git a/doc/tutorial.md b/doc/tutorial.md
index 21f1498ee..71af3d45e 100644
--- a/doc/tutorial.md
+++ b/doc/tutorial.md
@@ -8,7 +8,7 @@
 | --freq=n[k][m]        | st-info<br />st-flash<br />st-util | The frequency of the SWD/JTAG interface can be specified, to override the default 1800 kHz configuration.<br />This option solely accepts decimal values (5K or 1.8M) with the unit `Hz` being left out. Valid frequencies are:<br />`5K, 15K, 25K, 50K, 100K, 125K, 240K, 480K, 950K, 1200K, 1800K, 4000K(4M)`.                                                                                                         | v1.6.1               |
 | --opt                 | st-flash                           | Optimisation can be enabled in order to skip flashing empty (0x00 or 0xff) bytes at the end of binary file.<br />This may cause some garbage data left after a flash operation. This option was enabled by default in earlier releases.                                                                                                                                                                                  | v1.6.1               |
 | --reset               | st-flash                           | Trigger a reset after flashing. The default uses the hardware reset through `NRST` pin.<br />A software reset (via `AIRCR`; since v1.5.1) is used, if the hardware reset failed (`NRST` pin not connected).                                                                                                                                                                                                              | v1.0.0               |
-| --connect-under-reset | st-info<br />st-flash              | Connect under reset. Option makes it possible to connect to the device before code execution. This is useful<br />when the target contains code that lets the device go to sleep, disables debug pins or other special code.                                                                                                                                                                                             | v1.6.1               |
+| --connect-under-reset | st-info<br />st-flash<br />st-util | Connect under reset. Option makes it possible to connect to the device before code execution. This is useful<br />when the target contains code that lets the device go to sleep, disables debug pins or other special code.                                                                                                                                                                                             | v1.6.1               |
 | --hot-plug            | st-info<br />st-flash<br />st-util | Connect to the target without reset.                                                                                                                                                                                                                                                                                                                                                                                     | v1.6.2               |
 | --probe               | st-info                            | Display hardware information about the connected programmer and target MCU.                                                                                                                                                                                                                                                                                                                                              | v1.2.0               |
 | --version             | st-info<br />st-flash<br />st-util | Print version information.                                                                                                                                                                                                                                                                                                                                                                                               | v1.3.0               |
diff --git a/inc/backend.h b/inc/backend.h
index ebd438127..a45dccd82 100644
--- a/inc/backend.h
+++ b/inc/backend.h
@@ -10,7 +10,7 @@
         int (*core_id) (stlink_t * stl);
         int (*reset) (stlink_t * stl);
         int (*jtag_reset) (stlink_t * stl, int value);
-        int (*run) (stlink_t * stl);
+        int (*run) (stlink_t * stl, enum run_type type);
         int (*status) (stlink_t * stl);
         int (*version) (stlink_t *sl);
         int (*read_debug32) (stlink_t *sl, uint32_t addr, uint32_t *data);
diff --git a/inc/stlink.h b/inc/stlink.h
index 8fe6033d6..ec119c145 100644
--- a/inc/stlink.h
+++ b/inc/stlink.h
@@ -191,6 +191,12 @@ enum reset_type {
     RESET_SOFT_AND_HALT = 3,
 };
 
+enum run_type {
+    RUN_NORMAL = 0,
+    RUN_FLASH_LOADER = 1,
+};
+
+
 typedef struct _stlink stlink_t;
 
 #include <backend.h>
@@ -250,7 +256,7 @@ int stlink_exit_dfu_mode(stlink_t *sl);
 void stlink_close(stlink_t *sl);
 int stlink_core_id(stlink_t *sl);
 int stlink_reset(stlink_t *sl, enum reset_type type);
-int stlink_run(stlink_t *sl);
+int stlink_run(stlink_t *sl, enum run_type type);
 int stlink_status(stlink_t *sl);
 int stlink_version(stlink_t *sl);
 int stlink_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data);
diff --git a/src/common.c b/src/common.c
index 928091eb4..63086ab22 100644
--- a/src/common.c
+++ b/src/common.c
@@ -1791,7 +1791,7 @@ int stlink_reset(stlink_t *sl, enum reset_type type) {
     return(0);
 }
 
-int stlink_run(stlink_t *sl) {
+int stlink_run(stlink_t *sl, enum run_type type) {
     struct stlink_reg rr;
     DLOG("*** stlink_run ***\n");
 
@@ -1804,7 +1804,7 @@ int stlink_run(stlink_t *sl) {
         stlink_write_reg(sl, rr.xpsr | (1 << 24), 16);
     }
 
-    return(sl->backend->run(sl));
+    return(sl->backend->run(sl, type));
 }
 
 int stlink_set_swdclk(stlink_t *sl, int freq_khz) {
@@ -2096,7 +2096,7 @@ int stlink_trace_read(stlink_t* sl, uint8_t* buf, size_t size) {
 
 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
     stlink_write_reg(sl, addr, 15); /* pc register */
-    stlink_run(sl);
+    stlink_run(sl, RUN_NORMAL);
 
     while (stlink_is_core_halted(sl)) { usleep(3000000); }
 }
@@ -2258,7 +2258,7 @@ static void stlink_fwrite_finalize(stlink_t *sl, stm32_addr_t addr) {
     // set PC to the reset routine
     stlink_read_debug32(sl, addr + 4, &val);
     stlink_write_reg(sl, val, 15);
-    stlink_run(sl);
+    stlink_run(sl, RUN_NORMAL);
 }
 
 int stlink_mwrite_sram(stlink_t * sl, uint8_t* data, uint32_t length, stm32_addr_t addr) {
diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index 5eb607758..ede0f1179 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -17,7 +17,7 @@ static void cleanup(int signum) {
     (void)signum;
 
     if (connected_stlink) { // switch back to mass storage mode before closing
-        stlink_run(connected_stlink);
+        stlink_run(connected_stlink, RUN_NORMAL);
         stlink_exit_debug_mode(connected_stlink);
         stlink_close(connected_stlink);
     }
diff --git a/src/st-trace/trace.c b/src/st-trace/trace.c
index f40a9d683..f28624475 100644
--- a/src/st-trace/trace.c
+++ b/src/st-trace/trace.c
@@ -539,7 +539,7 @@ int main(int argc, char** argv)
     memset(&trace, 0, sizeof(trace));
     trace.start_time = time(NULL);
 
-    if (stlink_run(stlink)) {
+    if (stlink_run(stlink, RUN_NORMAL)) {
         ELOG("Unable to run device\n");
         if (!settings.force)
             return APP_RESULT_STLINK_STATE_ERROR;
diff --git a/src/st-util/gdb-server.c b/src/st-util/gdb-server.c
index c2a220df3..c6d329456 100644
--- a/src/st-util/gdb-server.c
+++ b/src/st-util/gdb-server.c
@@ -74,7 +74,7 @@ static void init_cache(stlink_t *sl);
 static void _cleanup() {
     if (connected_stlink) {
         // Switch back to mass storage mode before closing
-        stlink_run(connected_stlink);
+        stlink_run(connected_stlink, RUN_NORMAL);
         stlink_exit_debug_mode(connected_stlink);
         stlink_close(connected_stlink);
     }
@@ -252,11 +252,11 @@ int main(int argc, char** argv) {
 
     init_cache(sl);
 
-    do {                       // don't go beserk if serve() returns with error
+    do {                            // don't go beserk if serve() returns with error
         if (serve(sl, &state)) { usleep (1 * 1000); }
 
-        sl = connected_stlink; // in case serve() changed the connection
-        stlink_run(sl);        // continue
+        sl = connected_stlink;      // in case serve() changed the connection
+        stlink_run(sl, RUN_NORMAL); // continue
     } while (state.persistent);
 
 #if defined(_WIN32)
@@ -1229,7 +1229,7 @@ int serve(stlink_t *sl, st_state_t *st) {
                 if (!strncmp(cmd, "resume", 6)) {                               // resume
                     DLOG("Rcmd: resume\n");
                     cache_sync(sl);
-                    ret = stlink_run(sl);
+                    ret = stlink_run(sl, RUN_NORMAL);
 
                     if (ret) {
                         DLOG("Rcmd: resume failed\n");
@@ -1396,7 +1396,7 @@ int serve(stlink_t *sl, st_state_t *st) {
 
         case 'c':
             cache_sync(sl);
-            ret = stlink_run(sl);
+            ret = stlink_run(sl, RUN_NORMAL);
 
             if (ret) { DLOG("Semihost: run failed\n"); }
 
@@ -1466,7 +1466,7 @@ int serve(stlink_t *sl, st_state_t *st) {
 
                         // continue execution
                         cache_sync(sl);
-                        ret = stlink_run(sl);
+                        ret = stlink_run(sl, RUN_NORMAL);
 
                         if (ret) { DLOG("Semihost: continue execution failed with stlink_run\n"); }
                     } else {
@@ -1806,7 +1806,7 @@ int serve(stlink_t *sl, st_state_t *st) {
         }
         case 'k':
             // kill request - reset the connection itself
-            ret = stlink_run(sl);
+            ret = stlink_run(sl, RUN_NORMAL);
             if (ret) { DLOG("Kill: stlink_run failed\n"); }
 
             ret = stlink_exit_debug_mode(sl);
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index cd7096490..ba5efdb8e 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -347,7 +347,7 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
     }
 
     /* Run loader */
-    stlink_run(sl);
+    stlink_run(sl, RUN_FLASH_LOADER);
 
 /* This piece of code used to try to spin for .1 second by waiting doing 10000 rounds of 10 µs.
  * But because this usually runs on Unix-like OSes, the 10 µs get rounded up to the "tick"
diff --git a/src/stlink-lib/sg.c b/src/stlink-lib/sg.c
index c5946ed19..07971284f 100644
--- a/src/stlink-lib/sg.c
+++ b/src/stlink-lib/sg.c
@@ -731,8 +731,9 @@ void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 }
 
 // force the core exit the debug mode.
-int _stlink_sg_run(stlink_t *sl) {
+int _stlink_sg_run(stlink_t *sl, enum run_type type) {
     struct stlink_libsg *sg = sl->backend_data;
+    (void)(type); //unused
     clear_cdb(sg);
     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
     sl->q_len = 2;
diff --git a/src/stlink-lib/usb.c b/src/stlink-lib/usb.c
index 99b5f51ef..2754db06f 100644
--- a/src/stlink-lib/usb.c
+++ b/src/stlink-lib/usb.c
@@ -590,14 +590,17 @@ int _stlink_usb_step(stlink_t* sl) {
 /**
  * This seems to do a good job of restarting things from the beginning?
  * @param sl
+ * @param type
  */
-int _stlink_usb_run(stlink_t* sl) {
+int _stlink_usb_run(stlink_t* sl, enum run_type type) {
     struct stlink_libusb * const slu = sl->backend_data;
 
     int res;
 
     if (sl->version.jtag_api != STLINK_JTAG_API_V1) {
-        res = _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_DEBUGEN);
+        res = _stlink_usb_write_debug32(sl, STLINK_REG_DHCSR, 
+                    STLINK_REG_DHCSR_DBGKEY | STLINK_REG_DHCSR_C_DEBUGEN |
+                    ((type==RUN_FLASH_LOADER)?STLINK_REG_DHCSR_C_MASKINTS:0));
         return(res);
     }
 
diff --git a/tests/sg.c b/tests/sg.c
index 7584ab5d4..d88fbe760 100644
--- a/tests/sg.c
+++ b/tests/sg.c
@@ -159,7 +159,7 @@ int main(void) { // main() ripped out of old stlink-hw.c
 #endif
 
 #if 0
-    stlink_run(sl);
+    stlink_run(sl, RUN_NORMAL);
     stlink_status(sl);
     stlink_force_debug(sl);
     stlink_status(sl);
@@ -196,7 +196,7 @@ int main(void) { // main() ripped out of old stlink-hw.c
 #endif
 
 #if 0
-    stlink_run(sl);
+    stlink_run(sl, RUN_NORMAL);
     stlink_status(sl);
     // back to mass mode, just in case ...
     stlink_exit_debug_mode(sl);
diff --git a/tests/usb.c b/tests/usb.c
index 5c5c2077d..742f5c578 100644
--- a/tests/usb.c
+++ b/tests/usb.c
@@ -111,7 +111,7 @@ int main(int ac, char** av) {
         stlink_step(sl);
 
         printf("-- run\n");
-        stlink_run(sl);
+        stlink_run(sl, RUN_NORMAL);
 
         printf("-- exit_debug_mode\n");
         stlink_exit_debug_mode(sl);

From c2e898c9228e2b13378987354f602745e1ca7b1c Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Tue, 6 Apr 2021 22:43:15 +0500
Subject: [PATCH 17/18] Manual NRST pulse shaping Automatic generation via
 argument two of the jtag_reset function seem doesn't to work

---
 src/common.c | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/common.c b/src/common.c
index 63086ab22..1cb5c6db0 100644
--- a/src/common.c
+++ b/src/common.c
@@ -1751,7 +1751,12 @@ int stlink_reset(stlink_t *sl, enum reset_type type) {
 
     if (type == RESET_HARD || type == RESET_AUTO) {
         // hardware target reset
-        if (sl->version.stlink_v > 1) { stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_PULSE); }
+        if (sl->version.stlink_v > 1) {
+            stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_LOW);
+            // minimum reset pulse duration of 20 us (RM0008, 8.1.2 Power reset)
+            usleep(100);
+            stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_HIGH);
+        }
         if (sl->backend->reset(sl)) { return(-1); }
         usleep(10000);
     }
@@ -4522,6 +4527,9 @@ int stlink_target_connect(stlink_t *sl, enum connect_type connect) {
     if (connect == CONNECT_UNDER_RESET) {
         stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_LOW);
 
+        // minimum reset pulse duration of 20 us (RM0008, 8.1.2 Power reset)
+        usleep(20);
+
         if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
         stlink_force_debug(sl);
 

From 31738583967b89cf6416fd45a17ca8a7adfe12e2 Mon Sep 17 00:00:00 2001
From: anton <prozanton@gmail.com>
Date: Wed, 7 Apr 2021 20:57:17 +0500
Subject: [PATCH 18/18] gdb-server: set target configuration after client
 connects

---
 src/st-util/gdb-server.c | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/st-util/gdb-server.c b/src/st-util/gdb-server.c
index c6d329456..006a0e3aa 100644
--- a/src/st-util/gdb-server.c
+++ b/src/st-util/gdb-server.c
@@ -242,16 +242,12 @@ int main(int argc, char** argv) {
 
     DLOG("Chip ID is %#010x, Core ID is %#08x.\n", sl->chip_id, sl->core_id);
 
-    state.current_memory_map = make_memory_map(sl);
-
 #if defined(_WIN32)
     WSADATA wsadata;
 
     if (WSAStartup(MAKEWORD(2, 2), &wsadata) != 0) { goto winsock_error; }
 #endif
 
-    init_cache(sl);
-
     do {                            // don't go beserk if serve() returns with error
         if (serve(sl, &state)) { usleep (1 * 1000); }
 
@@ -1102,12 +1098,22 @@ int serve(stlink_t *sl, st_state_t *st) {
 
     close_socket(sock);
 
+    uint32_t chip_id = sl->chip_id;
+
     stlink_target_connect(sl, st->connect_mode);
     stlink_force_debug(sl);
 
+    if (sl->chip_id != chip_id) {
+        WLOG("Target has changed!\n");
+    }
+
     init_code_breakpoints(sl);
     init_data_watchpoints(sl);
 
+    init_cache(sl);
+
+    st->current_memory_map = make_memory_map(sl);
+
     ILOG("GDB connected.\n");
 
     /*