diff --git a/inc/stlink.h b/inc/stlink.h
index ec119c145..f70060b05 100644
--- a/inc/stlink.h
+++ b/inc/stlink.h
@@ -25,7 +25,7 @@ extern "C" {
 // #define Q_BUF_LEN    96
 #define Q_BUF_LEN (1024 * 100)
 
-// STLINK_DEBUG_RESETSYS, etc:
+/* Statuses of core */
 enum target_state {
     TARGET_UNKNOWN = 0,
     TARGET_RUNNING = 1,
@@ -37,38 +37,15 @@ enum target_state {
 #define STLINK_CORE_RUNNING             0x80
 #define STLINK_CORE_HALTED              0x81
 
-#define STLINK_GET_VERSION              0xF1
-#define STLINK_GET_CURRENT_MODE         0xF5
-#define STLINK_GET_TARGET_VOLTAGE       0xF7
-
-#define STLINK_DEBUG_COMMAND            0xF2
-#define STLINK_DFU_COMMAND              0xF3
-#define STLINK_DFU_EXIT                 0x07
-
-// STLINK_GET_CURRENT_MODE
+/* STLINK modes */
 #define STLINK_DEV_DFU_MODE             0x00
 #define STLINK_DEV_MASS_MODE            0x01
 #define STLINK_DEV_DEBUG_MODE           0x02
 #define STLINK_DEV_UNKNOWN_MODE           -1
 
-// TODO - possible poor names...
-#define STLINK_SWD_ENTER                0x30
-#define STLINK_SWD_READCOREID           0x32 // TBD
-#define STLINK_JTAG_WRITEDEBUG_32BIT    0x35
-#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
-#define STLINK_APIV3_GET_COM_FREQ       0x62
-
-#define STLINK_APIV3_GET_VERSION_EX     0xFB
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW  0x00
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01
 
 /* Baud rate divisors for SWDCLK */
 #define STLINK_SWDCLK_4MHZ_DIVISOR        0
@@ -106,6 +83,23 @@ enum target_state {
 #define STLINK_F_HAS_DPBANKSEL          (1 << 8)
 #define STLINK_F_HAS_RW8_512BYTES       (1 << 9)
 
+/* Error code */
+#define STLINK_DEBUG_ERR_OK              0x80
+#define STLINK_DEBUG_ERR_FAULT           0x81
+#define STLINK_DEBUG_ERR_WRITE           0x0c
+#define STLINK_DEBUG_ERR_WRITE_VERIFY    0x0d
+#define STLINK_DEBUG_ERR_AP_WAIT         0x10
+#define STLINK_DEBUG_ERR_AP_FAULT        0x11
+#define STLINK_DEBUG_ERR_AP_ERROR        0x12
+#define STLINK_DEBUG_ERR_DP_WAIT         0x14
+#define STLINK_DEBUG_ERR_DP_FAULT        0x15
+#define STLINK_DEBUG_ERR_DP_ERROR        0x16
+
+#define CMD_CHECK_NO         0
+#define CMD_CHECK_REP_LEN    1
+#define CMD_CHECK_STATUS     2
+#define CMD_CHECK_RETRY      3 /* check status and retry if wait error */
+
 #define C_BUF_LEN 32
 
 enum stlink_flash_type {
diff --git a/src/common.c b/src/common.c
index 80190f756..6d81bf743 100644
--- a/src/common.c
+++ b/src/common.c
@@ -1275,8 +1275,8 @@ static void stop_wdg_in_debug(stlink_t *sl) {
   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);
+    set = (1 << STM32F0_DBGMCU_CR_IWDG_STOP) | 
+          (1 << STM32F0_DBGMCU_CR_WWDG_STOP);
     break;
   case STLINK_FLASH_TYPE_F4:
   case STLINK_FLASH_TYPE_F7:
@@ -1441,13 +1441,12 @@ void stlink_close(stlink_t *sl) {
 }
 
 int stlink_exit_debug_mode(stlink_t *sl) {
-  int ret;
-
   DLOG("*** stlink_exit_debug_mode ***\n");
-  ret = stlink_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY);
 
-  if (ret == -1) {
-    return (ret);
+  if (sl->flash_type != STLINK_FLASH_TYPE_UNKNOWN && 
+      sl->core_stat != TARGET_RESET) {
+    // stop debugging if the target has been identified
+    stlink_write_debug32(sl, STLINK_REG_DHCSR, STLINK_REG_DHCSR_DBGKEY);
   }
 
   return (sl->backend->exit_debug_mode(sl));
@@ -1462,9 +1461,12 @@ int stlink_enter_swd_mode(stlink_t *sl) {
 int stlink_force_debug(stlink_t *sl) {
   DLOG("*** stlink_force_debug_mode ***\n");
   int res = sl->backend->force_debug(sl);
+  if (res) {
+     return (res);
+  }
   // Stop the watchdogs in the halted state for suppress target reboot
   stop_wdg_in_debug(sl);
-  return (res);
+  return (0);
 }
 
 int stlink_exit_dfu_mode(stlink_t *sl) {
@@ -1540,6 +1542,7 @@ int stlink_chip_id(stlink_t *sl, uint32_t *chip_id) {
 
   if (ret || !(*chip_id)) {
     *chip_id = 0;
+    ret = ret?:-1;
     ELOG("Could not find chip id!\n");
   } else {
     *chip_id = (*chip_id) & 0xfff;
@@ -1679,7 +1682,7 @@ int stlink_load_device_params(stlink_t *sl) {
 }
 
 int stlink_jtag_reset(stlink_t *sl, int value) {
-  DLOG("*** stlink_jtag_reset ***\n");
+  DLOG("*** stlink_jtag_reset %d ***\n", value);
   return (sl->backend->jtag_reset(sl, value));
 }
 
@@ -1763,6 +1766,8 @@ int stlink_reset(stlink_t *sl, enum reset_type type) {
 
   DLOG("*** stlink_reset ***\n");
 
+  sl->core_stat = TARGET_RESET;
+
   if (type == RESET_AUTO) {
     // clear S_RESET_ST in DHCSR register for reset state detection
     stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
@@ -1771,14 +1776,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_LOW);
+      stlink_jtag_reset(sl, STLINK_DEBUG_APIV2_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);
+      stlink_jtag_reset(sl, STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
     }
+    sl->backend->reset(sl);
     usleep(10000);
   }
 
@@ -1788,8 +1791,8 @@ int stlink_reset(stlink_t *sl, enum reset_type type) {
      * 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) {
+    int res = stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+    if ((dhcsr & STLINK_REG_DHCSR_S_RESET_ST) == 0 && !res) {
       // reset not done yet
       // try reset through AIRCR so that NRST does not need to be connected
 
@@ -1803,8 +1806,9 @@ int stlink_reset(stlink_t *sl, enum reset_type type) {
     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)
+      if ((dhcsr & STLINK_REG_DHCSR_S_RESET_ST) == 0) {
         return (0);
+      }
     }
 
     return (-1);
@@ -1926,11 +1930,7 @@ int stlink_version(stlink_t *sl) {
   DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 
   if (sl->version.jtag_v == 0) {
-    DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
-  }
-
-  if (sl->version.swim_v == 0) {
-    DLOG("    notice: the firmware doesn't support a swim interface\n");
+    WLOG("    warning: stlink doesn't support JTAG/SWD interface\n");
   }
 
   return (0);
@@ -1994,13 +1994,6 @@ int stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 
 int stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
   DLOG("*** stlink_write_mem8 ***\n");
-
-  if (len > 0x40) { // !!! never ever: Writing more then 0x40 bytes gives
-                    // unexpected behaviour
-    ELOG("Data length > 64: +%d byte.\n", len);
-    return (-1);
-  }
-
   return (sl->backend->write_mem8(sl, addr, len));
 }
 
@@ -2301,9 +2294,6 @@ static void stlink_checksum(mapped_file_t *mp) {
 
 static void stlink_fwrite_finalize(stlink_t *sl, stm32_addr_t addr) {
   unsigned int val;
-  // set stack
-  stlink_read_debug32(sl, addr, &val);
-  stlink_write_reg(sl, val, 13);
   // set PC to the reset routine
   stlink_read_debug32(sl, addr + 4, &val);
   stlink_write_reg(sl, val, 15);
@@ -2646,20 +2636,21 @@ int stlink_fread(stlink_t *sl, const char *path, bool is_ihex,
 int write_buffer_to_sram(stlink_t *sl, flash_loader_t *fl, const uint8_t *buf,
                          size_t size) {
   // write the buffer right after the loader
+  int ret = 0;
   size_t chunk = size & ~0x3;
   size_t rem = size & 0x3;
 
   if (chunk) {
     memcpy(sl->q_buf, buf, chunk);
-    stlink_write_mem32(sl, fl->buf_addr, chunk);
+    ret = stlink_write_mem32(sl, fl->buf_addr, chunk);
   }
 
-  if (rem) {
+  if (rem && !ret) {
     memcpy(sl->q_buf, buf + chunk, rem);
-    stlink_write_mem8(sl, (fl->buf_addr) + (uint32_t)chunk, rem);
+    ret = stlink_write_mem8(sl, (fl->buf_addr) + (uint32_t)chunk, rem);
   }
 
-  return (0);
+  return (ret);
 }
 
 uint32_t calculate_F4_sectornum(uint32_t flashaddr) {
@@ -3083,42 +3074,48 @@ int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data,
 
 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t *base,
                              uint32_t len, uint32_t pagesize) {
-  unsigned int count;
+  unsigned int count, off;
   unsigned int num_half_pages = len / pagesize;
   uint32_t val;
   uint32_t flash_regs_base = get_stm32l0_flash_base(sl);
   flash_loader_t fl;
+  bool use_loader = true;
+  int ret = 0;
 
-  ILOG("Starting Half page flash write for STM32L core id\n");
-
-  /* Flash loader initialisation */
-  if (stlink_flash_loader_init(sl, &fl) == -1) {
-    WLOG("stlink_flash_loader_init() == -1\n");
-    return (-1);
-  }
-
-  // unlock already done
+  // enable half page write
   stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
   val |= (1 << FLASH_L1_FPRG);
   stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
-
   val |= (1 << FLASH_L1_PROG);
   stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
 
   wait_flash_busy(sl);
 
   for (count = 0; count < num_half_pages; count++) {
-    if (stlink_flash_loader_run(sl, &fl, addr + count * pagesize,
-                                base + count * pagesize, pagesize) == -1) {
+    if (use_loader) {
+      ret = stlink_flash_loader_run(sl, &fl, addr + count * pagesize,
+                                base + count * pagesize, pagesize);
+      if (ret && count == 0) {
+        /* It seems that stm32lx devices have a problem when it is blank */
+        WLOG("Failed to use flash loader, fallback to soft write\n");
+        use_loader = false;
+      }
+    }
+    if (!use_loader) {
+      ret = 0;
+      for (off = 0; off < pagesize && !ret; off += 64) {
+        size_t chunk = (pagesize - off > 64) ? 64 : pagesize - off;
+        memcpy(sl->q_buf, base + count * pagesize + off, chunk);
+        ret = stlink_write_mem32(sl, addr + count * pagesize + off, chunk);
+      }
+    }
+
+    if (ret) {
       WLOG("l1_stlink_flash_loader_run(%#x) failed! == -1\n",
            addr + count * pagesize);
-      stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
-      val &= ~((1 << FLASH_L1_FPRG) | (1 << FLASH_L1_PROG));
-      stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
-      return (-1);
+      break;
     }
 
-    // wait for sr.busy to be cleared
     if (sl->verbose >= 1) {
       // show progress; writing procedure is slow and previous errors are
       // misleading
@@ -3126,16 +3123,15 @@ int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t *base,
       fflush(stdout);
     }
 
+    // wait for sr.busy to be cleared
     wait_flash_busy(sl);
   }
 
+  // disable half page write
   stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
-  val &= ~(1 << FLASH_L1_PROG);
-  stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
-  stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
-  val &= ~(1 << FLASH_L1_FPRG);
+  val &= ~((1 << FLASH_L1_FPRG) | (1 << FLASH_L1_PROG));
   stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
-  return (0);
+  return (ret);
 }
 
 int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
@@ -3239,6 +3235,12 @@ int stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
       ELOG("pecr.prglock not clear\n");
       return (-1);
     }
+
+    /* Flash loader initialisation */
+    if (stlink_flash_loader_init(sl, fl) == -1) {
+      // L0/L1 have fallback to soft write
+      WLOG("stlink_flash_loader_init() == -1\n");
+    }
   } else if ((sl->flash_type == STLINK_FLASH_TYPE_F0) ||
              (sl->flash_type == STLINK_FLASH_TYPE_F1_XL)) {
     ILOG("Starting Flash write for VL/F0/F3/F1_XL\n");
@@ -3341,12 +3343,13 @@ int stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl,
       pagesize = L1_WRITE_BLOCK_SIZE;
     }
 
+    DLOG("Starting %3u page write\r\n", (unsigned int)(len / sl->flash_pgsz));
+
     off = 0;
 
     if (len > pagesize) {
-      if (stm32l1_write_half_pages(sl, addr, base, len, pagesize) == -1) {
-        // this may happen on a blank device!
-        WLOG("\nwrite_half_pages failed == -1\n");
+      if (stm32l1_write_half_pages(sl, addr, base, len, pagesize)) {
+        return (-1);
       } else {
         off = (size_t)(len / pagesize) * pagesize;
       }
@@ -4899,27 +4902,29 @@ int stlink_fwrite_option_bytes(stlink_t *sl, const char *path,
 }
 
 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);
+    stlink_enter_swd_mode(sl);
+
+    stlink_jtag_reset(sl, STLINK_DEBUG_APIV2_DRIVE_NRST_LOW);
+
+    // try to halt the core before reset
+    // this is useful if the NRST pin is not connected
+    sl->backend->force_debug(sl);
 
     // 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);
-
-    // clear S_RESET_ST in DHCSR register
-    stlink_read_debug32(sl, STLINK_REG_DHCSR, &dhcsr);
+    stlink_jtag_reset(sl, STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
 
-    stlink_jtag_reset(sl, STLINK_JTAG_DRIVE_NRST_HIGH);
-    usleep(10000);
+    // try to halt the core after reset
+    unsigned timeout = time_ms() + 10;
+    while (time_ms() < timeout) {
+      sl->backend->force_debug(sl);
+      usleep(100);
+    }
 
     // check NRST connection
-    dhcsr = 0;
+    uint32_t 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");
@@ -4929,8 +4934,10 @@ int stlink_target_connect(stlink_t *sl, enum connect_type connect) {
     stlink_soft_reset(sl, 1 /* halt on reset */);
   }
 
-  if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
-    stlink_enter_swd_mode(sl);
+  if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE &&
+        stlink_enter_swd_mode(sl)) {
+    printf("Failed to enter SWD mode\n");
+    return -1;
   }
 
   if (connect == CONNECT_NORMAL) {
diff --git a/src/st-flash/flash.c b/src/st-flash/flash.c
index a94d78254..be1ef60c2 100644
--- a/src/st-flash/flash.c
+++ b/src/st-flash/flash.c
@@ -68,7 +68,7 @@ int main(int ac, char** av) {
 
     if (sl->flash_type == STLINK_FLASH_TYPE_UNKNOWN) {
         printf("Failed to connect to target\n");
-        return(-1);
+        goto on_error;
     }
 
     if ( o.flash_size != 0u && o.flash_size != sl->flash_size ) {
@@ -84,20 +84,6 @@ int main(int ac, char** av) {
     signal(SIGTERM, &cleanup);
     signal(SIGSEGV, &cleanup);
 
-    if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
-        if (stlink_exit_dfu_mode(sl)) {
-            printf("Failed to exit DFU mode\n");
-            goto on_error;
-        }
-    }
-
-    if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
-        if (stlink_enter_swd_mode(sl)) {
-            printf("Failed to enter SWD mode\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");
diff --git a/src/st-info/info.c b/src/st-info/info.c
index a9551cd49..7eda5d6ff 100644
--- a/src/st-info/info.c
+++ b/src/st-info/info.c
@@ -42,7 +42,6 @@ static void stlink_print_info(stlink_t *sl) {
     printf("  chipid:     0x%.4x\n", sl->chip_id);
 
     params = stlink_chipid_get_params(sl->chip_id);
-
     if (params) { printf("  descr:      %s\n", params->description); }
 }
 
@@ -103,15 +102,8 @@ static int print_data(int ac, char **av) {
 
     // open first st-link device
     sl = stlink_open_usb(0, connect, NULL, freq);
-
     if (sl == NULL) { return(-1); }
 
-    sl->verbose = 0;
-
-    if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) { stlink_exit_dfu_mode(sl); }
-
-    if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) { stlink_enter_swd_mode(sl); }
-
     if (strcmp(av[1], "--serial") == 0) {
         printf("%s\n", sl->serial);
     } else if (strcmp(av[1], "--flash") == 0) {
@@ -124,7 +116,6 @@ static int print_data(int ac, char **av) {
         printf("0x%.4x\n", sl->chip_id);
     } else if (strcmp(av[1], "--descr") == 0) {
         const struct stlink_chipid_params *params = stlink_chipid_get_params(sl->chip_id);
-
         if (params == NULL) { return(-1); }
 
         printf("%s\n", params->description);
diff --git a/src/stlink-gui/gui.c b/src/stlink-gui/gui.c
index 03e999b7f..653428735 100644
--- a/src/stlink-gui/gui.c
+++ b/src/stlink-gui/gui.c
@@ -495,24 +495,13 @@ static void connect_button_cb(GtkWidget *widget, gpointer data) {
 
     if (gui->sl != NULL) { return; }
 
-    gui->sl = stlink_v1_open(0, 1); // try version 1 then version 2
-
-    if (gui->sl == NULL) { gui->sl = stlink_open_usb(0, 1, NULL, 0); }
+    gui->sl = stlink_open_usb(0, 1, NULL, 0);
 
     if (gui->sl == NULL) {
         stlink_gui_set_info_error_message(gui, "Failed to connect to STLink.");
         return;
     }
 
-    // code below taken from flash/main.c, refactoring might be in order
-    if (stlink_current_mode(gui->sl) == STLINK_DEV_DFU_MODE) {
-        stlink_exit_dfu_mode(gui->sl);
-    }
-
-    if (stlink_current_mode(gui->sl) != STLINK_DEV_DEBUG_MODE) {
-        stlink_enter_swd_mode(gui->sl);
-    }
-
     stlink_gui_set_connected(gui);
 }
 
diff --git a/src/stlink-lib/commands.h b/src/stlink-lib/commands.h
index dac82b8e6..136adf80e 100644
--- a/src/stlink-lib/commands.h
+++ b/src/stlink-lib/commands.h
@@ -1,8 +1,17 @@
 #ifndef STLINK_COMMANDS_H_
 #define STLINK_COMMANDS_H_
 
+enum stlink_commands {
+    STLINK_GET_VERSION                   = 0xF1,
+    STLINK_DEBUG_COMMAND                 = 0xF2,
+    STLINK_DFU_COMMAND                   = 0xF3,
+    STLINK_GET_CURRENT_MODE              = 0xF5,
+    STLINK_GET_TARGET_VOLTAGE            = 0xF7,
+    STLINK_GET_VERSION_APIV3             = 0xFB
+};
+
 enum stlink_debug_commands {
-    STLINK_DEBUG_ENTER_JTAG              = 0x00,
+    STLINK_DEBUG_ENTER_JTAG_RESET        = 0x00,
     STLINK_DEBUG_GETSTATUS               = 0x01,
     STLINK_DEBUG_FORCEDEBUG              = 0x02,
     STLINK_DEBUG_APIV1_RESETSYS          = 0x03,
@@ -29,11 +38,20 @@ enum stlink_debug_commands {
     STLINK_DEBUG_APIV2_READDEBUGREG      = 0x36,
     STLINK_DEBUG_APIV2_READALLREGS       = 0x3A,
     STLINK_DEBUG_APIV2_GETLASTRWSTATUS   = 0x3B,
+    STLINK_DEBUG_APIV2_DRIVE_NRST        = 0x3C,
     STLINK_DEBUG_APIV2_GETLASTRWSTATUS2  = 0x3E,
     STLINK_DEBUG_APIV2_START_TRACE_RX    = 0x40,
     STLINK_DEBUG_APIV2_STOP_TRACE_RX     = 0x41,
     STLINK_DEBUG_APIV2_GET_TRACE_NB      = 0x42,
-    STLINK_DEBUG_ENTER_SWD               = 0xa3
+    STLINK_DEBUG_APIV2_SWD_SET_FREQ      = 0x43,
+    STLINK_DEBUG_APIV3_SET_COM_FREQ      = 0x61,
+    STLINK_DEBUG_APIV3_GET_COM_FREQ      = 0x62,
+    STLINK_DEBUG_ENTER_SWD               = 0xa3,
+    STLINK_DEBUG_ENTER_JTAG_NO_RESET     = 0xa4,
+};
+
+enum stlink_dfu_commands {
+    STLINK_DFU_EXIT                      = 0x07
 };
 
 #endif // STLINK_COMMANDS_H_
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index d492716db..355223146 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -14,7 +14,13 @@
 
 #define STM32F0_WDG_KR_KEY_RELOAD 0xAAAA
 
-/* DO NOT MODIFY SOURCECODE DIRECTLY, EDIT ASSEMBLY FILES INSTEAD */
+/* !!!
+ * !!! DO NOT MODIFY FLASH LOADERS DIRECTLY!
+ * !!!
+ * 
+ * Edit assembly files in the '/flashloaders' instead. The sizes of binary 
+ * flash loaders must be aligned by 4 (it's written by stlink_write_mem32)
+ */
 
 /* flashloaders/stm32f0.s -- compiled with thumb2 */
 static const uint8_t loader_code_stm32vl[] = {
@@ -322,9 +328,7 @@ int stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t targe
 
     DLOG("Running flash loader, write address:%#x, size: %u\n", target, (unsigned int)size);
 
-    // TODO: This can never return -1
     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
-        // IMPOSSIBLE!
         ELOG("write_buffer_to_sram() == -1\n");
         return(-1);
     }
diff --git a/src/stlink-lib/sg.c b/src/stlink-lib/sg.c
index 07971284f..18792c89f 100644
--- a/src/stlink-lib/sg.c
+++ b/src/stlink-lib/sg.c
@@ -470,7 +470,7 @@ int _stlink_sg_enter_jtag_mode(stlink_t *sl) {
     DLOG("\n*** stlink_enter_jtag_mode ***\n");
     clear_cdb(sg);
     sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_ENTER;
-    sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
+    sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG_RESET;
     sl->q_len = 0;
     return(stlink_q(sl));
 }
@@ -570,7 +570,7 @@ int _stlink_sg_reset(stlink_t *sl) {
 int _stlink_sg_jtag_reset(stlink_t *sl, int value) {
     struct stlink_libsg *sg = sl->backend_data;
     clear_cdb(sg);
-    sg->cdb_cmd_blk[1] = STLINK_JTAG_DRIVE_NRST;
+    sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV2_DRIVE_NRST;
     sg->cdb_cmd_blk[2] = (value) ? 0 : 1;
     sl->q_len = 3;
     sg->q_addr = 2;
@@ -876,7 +876,7 @@ int _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 int _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
     struct stlink_libsg *sg = sl->backend_data;
     clear_cdb(sg);
-    sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
+    sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
     // 2-5: addr
     write_uint32(sg->cdb_cmd_blk + 2, addr);
     write_uint32(sg->cdb_cmd_blk + 6, data);
@@ -888,7 +888,7 @@ int _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 int _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
     struct stlink_libsg *sg = sl->backend_data;
     clear_cdb(sg);
-    sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
+    sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV2_READDEBUGREG;
     // 2-5: addr
     write_uint32(sg->cdb_cmd_blk + 2, addr);
     sl->q_len = 8;
diff --git a/src/stlink-lib/usb.c b/src/stlink-lib/usb.c
index 2754db06f..5e5f042d5 100644
--- a/src/stlink-lib/usb.c
+++ b/src/stlink-lib/usb.c
@@ -84,50 +84,87 @@ void _stlink_usb_close(stlink_t* sl) {
 }
 
 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
-                  unsigned char* txbuf, size_t txsize, unsigned char* rxbuf, size_t rxsize) {
+                  unsigned char* txbuf, size_t txsize, unsigned char* rxbuf, 
+                  size_t rxsize, int check_error, const char *cmd) {
     // Note: txbuf and rxbuf can point to the same area
-    int res = 0;
-    int t;
+    int res, t, retry = 0;
 
-    t = libusb_bulk_transfer(handle->usb_handle, handle->ep_req, txbuf, (int)txsize, &res, 3000);
-
-    if (t) {
-        printf("[!] send_recv send request failed: %s\n", libusb_error_name(t));
-        return(-1);
-    } else if ((size_t)res != txsize) {
-        printf("[!] send_recv send request wrote %u bytes (instead of %u).\n",
-               (unsigned int)res, (unsigned int)txsize);
-    }
-
-    if (rxsize != 0) {
-        t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, rxbuf, (int)rxsize, &res, 3000);
+    while (1) {
+        res = 0;
+        t = libusb_bulk_transfer(handle->usb_handle, handle->ep_req, txbuf, (int)txsize, &res, 3000);
 
         if (t) {
-            printf("[!] send_recv read reply failed: %s\n", libusb_error_name(t));
+            ELOG("%s send request failed: %s\n", cmd, libusb_error_name(t));
             return(-1);
+        } else if ((size_t)res != txsize) {
+            ELOG("%s send request wrote %u bytes, instead of %u\n",
+                   cmd, (unsigned int)res, (unsigned int)txsize);
         }
-    }
 
-    if ((handle->protocoll == 1) && terminate) {
-        // read the SG reply
-        unsigned char sg_buf[13];
-        t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, sg_buf, 13, &res, 3000);
+        if (rxsize != 0) {
+            t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, rxbuf, (int)rxsize, &res, 3000);
 
-        if (t) {
-            printf("[!] send_recv read storage failed: %s\n", libusb_error_name(t));
-            return(-1);
+            if (t) {
+                ELOG("%s read reply failed: %s\n", cmd, libusb_error_name(t));
+                return(-1);
+            }
+
+            /* Checking the command execution status stored in the first byte of the response */
+            if (handle->protocoll != 1 && check_error >= CMD_CHECK_STATUS && 
+                        rxbuf[0] != STLINK_DEBUG_ERR_OK) {
+                switch(rxbuf[0]) {
+                case STLINK_DEBUG_ERR_AP_WAIT:
+                case STLINK_DEBUG_ERR_DP_WAIT:
+                    if (check_error == CMD_CHECK_RETRY && retry < 3) {
+                        unsigned int delay_us = (1<<retry) * 1000;
+                        DLOG("%s wait error (0x%02X), delaying %u us and retry\n", cmd, rxbuf[0], delay_us);
+                        usleep(delay_us);
+                        retry++;
+                        continue;
+                    }
+                    DLOG("%s wait error (0x%02X)\n", cmd, rxbuf[0]);
+                    break;
+                case STLINK_DEBUG_ERR_FAULT: DLOG("%s response fault\n", cmd); break;
+                case STLINK_DEBUG_ERR_AP_FAULT: DLOG("%s access port fault\n", cmd); break;
+                case STLINK_DEBUG_ERR_DP_FAULT: DLOG("%s debug port fault\n", cmd); break;
+                case STLINK_DEBUG_ERR_AP_ERROR: DLOG("%s access port error\n", cmd); break;
+                case STLINK_DEBUG_ERR_DP_ERROR: DLOG("%s debug port error\n", cmd); break;
+                case STLINK_DEBUG_ERR_WRITE_VERIFY:  DLOG("%s verification error\n", cmd); break;
+                case STLINK_DEBUG_ERR_WRITE:  DLOG("%s write error\n", cmd); break;
+                default: DLOG("%s error (0x%02X)\n", cmd, rxbuf[0]); break;
+                }
+
+                return(-1);
+            }
+
+            if (check_error == CMD_CHECK_REP_LEN && res != (int)rxsize) {
+                ELOG("%s wrong reply length\n", cmd);
+                res = -1;
+            }
         }
 
-        // The STLink doesn't seem to evaluate the sequence number.
-        handle->sg_transfer_idx++;
-    }
+        if ((handle->protocoll == 1) && terminate) {
+            // read the SG reply
+            unsigned char sg_buf[13];
+            t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, sg_buf, 13, &res, 3000);
+
+            if (t) {
+                ELOG("%s read storage failed: %s\n", cmd, libusb_error_name(t));
+                return(-1);
+            }
 
-    return(res);
+            // The STLink doesn't seem to evaluate the sequence number.
+            handle->sg_transfer_idx++;
+        }
+
+        return(res);
+    }
 }
 
 static inline int send_only(struct stlink_libusb* handle, int terminate,
-                            unsigned char* txbuf, size_t txsize) {
-    return((int)send_recv(handle, terminate, txbuf, txsize, NULL, 0));
+                            unsigned char* txbuf, size_t txsize,
+                            const char *cmd) {
+    return((int)send_recv(handle, terminate, txbuf, txsize, NULL, 0, CMD_CHECK_NO, cmd));
 }
 
 
@@ -149,7 +186,6 @@ static int fill_command(stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t
         cmd[i++] = 0;   // logical unit
         cmd[i++] = 0xa; // command length
     }
-
     return(i);
 }
 
@@ -158,33 +194,23 @@ int _stlink_usb_version(stlink_t *sl) {
     unsigned char* const data = sl->q_buf;
     unsigned char* const cmd  = sl->c_buf;
     ssize_t size;
-    uint32_t rep_len = 6;
-    int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
-
-    cmd[i++] = STLINK_GET_VERSION;
-
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_GET_VERSION\n");
-        return((int)size);
-    }
+    uint32_t rep_len;
+    int i;
 
-    /* STLINK-V3 requires a specific command */
     if (sl->version.stlink_v == 3) {
+        // STLINK-V3 version is determined by another command 
         rep_len = 12;
         i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
-        cmd[i++] = STLINK_APIV3_GET_VERSION_EX;
-
-        size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-        if (size != (ssize_t)rep_len) {
-            printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
-            return((int)size);
-        }
+        cmd[i++] = STLINK_GET_VERSION_APIV3;
+    } else {
+        rep_len = 6;
+        i = fill_command(sl, SG_DXFER_FROM_DEV, 6);
+        cmd[i++] = STLINK_GET_VERSION;
     }
 
-    return(0);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_REP_LEN, "GET_VERSION");
+
+    return(size<0?-1:0);
 }
 
 int32_t _stlink_usb_target_voltage(stlink_t *sl) {
@@ -199,13 +225,9 @@ int32_t _stlink_usb_target_voltage(stlink_t *sl) {
 
     cmd[i++] = STLINK_GET_TARGET_VOLTAGE;
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len, CMD_CHECK_REP_LEN, "GET_TARGET_VOLTAGE");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_GET_TARGET_VOLTAGE\n");
-        return(-1);
-    } else if (size != 8) {
-        printf("[!] wrong length STLINK_GET_TARGET_VOLTAGE\n");
+    if (size < 0) {
         return(-1);
     }
 
@@ -225,13 +247,12 @@ int _stlink_usb_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
 
     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
     cmd[i++] = STLINK_DEBUG_COMMAND;
-    cmd[i++] = STLINK_JTAG_READDEBUG_32BIT;
+    cmd[i++] = STLINK_DEBUG_APIV2_READDEBUGREG;
     write_uint32(&cmd[i], addr);
-    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len, CMD_CHECK_RETRY, "READDEBUGREG");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_JTAG_READDEBUG_32BIT\n");
-        return((int)size);
+    if (size < 0) {
+        return(-1);
     }
 
     *data = read_uint32(rdata, 4);
@@ -248,17 +269,12 @@ int _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 
     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
     cmd[i++] = STLINK_DEBUG_COMMAND;
-    cmd[i++] = STLINK_JTAG_WRITEDEBUG_32BIT;
+    cmd[i++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
     write_uint32(&cmd[i], addr);
     write_uint32(&cmd[i + 4], data);
-    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_JTAG_WRITEDEBUG_32BIT\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len, CMD_CHECK_RETRY, "WRITEDEBUGREG");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_get_rw_status(stlink_t *sl) {
@@ -275,15 +291,13 @@ int _stlink_usb_get_rw_status(stlink_t *sl) {
 
     if (sl->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
         cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
-        ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 12);
+        ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 12, CMD_CHECK_STATUS, "GETLASTRWSTATUS2");
     } else {
         cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
-        ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 2);
+        ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 2, CMD_CHECK_STATUS, "GETLASTRWSTATUS");
     }
 
-    if (ret < 0) { return(-1); }
-
-    return(0);
+    return(ret<0?-1:0);
 }
 
 int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
@@ -292,16 +306,22 @@ int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     unsigned char* const cmd  = sl->c_buf;
     int i, ret;
 
+    if ((sl->version.jtag_api < STLINK_JTAG_API_V3 && len > 64) ||
+        (sl->version.jtag_api >= STLINK_JTAG_API_V3 && len > 512)) {
+        ELOG("WRITEMEM_32BIT: bulk packet limits exceeded (data len %d byte)\n", len);
+        return (-1);
+    }
+
     i = fill_command(sl, SG_DXFER_TO_DEV, len);
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
     write_uint32(&cmd[i], addr);
     write_uint16(&cmd[i + 4], len);
-    ret = send_only(slu, 0, cmd, slu->cmd_len);
+    ret = send_only(slu, 0, cmd, slu->cmd_len, "WRITEMEM_32BIT");
 
     if (ret == -1) { return(ret); }
 
-    ret = send_only(slu, 1, data, len);
+    ret = send_only(slu, 1, data, len, "WRITEMEM_32BIT");
 
     if (ret == -1) { return(ret); }
 
@@ -314,16 +334,22 @@ int _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
     unsigned char* const cmd  = sl->c_buf;
     int i, ret;
 
+    if ((sl->version.jtag_api < STLINK_JTAG_API_V3 && len > 64) ||
+        (sl->version.jtag_api >= STLINK_JTAG_API_V3 && len > 512)) {
+        ELOG("WRITEMEM_8BIT: bulk packet limits exceeded (data len %d byte)\n", len);
+        return (-1);
+    }
+
     i = fill_command(sl, SG_DXFER_TO_DEV, 0);
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
     write_uint32(&cmd[i], addr);
     write_uint16(&cmd[i + 4], len);
-    ret = send_only(slu, 0, cmd, slu->cmd_len);
+    ret = send_only(slu, 0, cmd, slu->cmd_len, "WRITEMEM_8BIT");
 
     if (ret == -1) { return(ret); }
 
-    ret = send_only(slu, 1, data, len);
+    ret = send_only(slu, 1, data, len, "WRITEMEM_8BIT");
 
     if (ret == -1) { return(ret); }
 
@@ -340,10 +366,9 @@ int _stlink_usb_current_mode(stlink_t * sl) {
     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
     cmd[i++] = STLINK_GET_CURRENT_MODE;
-    size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len, CMD_CHECK_NO, "GET_CURRENT_MODE");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_GET_CURRENT_MODE\n");
+    if (size < 0) {
         return(-1);
     }
 
@@ -368,10 +393,9 @@ int _stlink_usb_core_id(stlink_t * sl) {
         offset = 4;
     }
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_STATUS, "READ_IDCODES");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_READCOREID\n");
+    if (size < 0) {
         return(-1);
     }
 
@@ -414,16 +438,9 @@ int _stlink_usb_status(stlink_t * sl) {
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_GETSTATUS;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_NO, "GETSTATUS");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_GETSTATUS\n");
-        return((int)size);
-    }
-
-    sl->q_len = (int)size;
-
-    if (sl->q_len > 1) {
+    if (size > 1) {
         if (sl->q_buf[0] == STLINK_CORE_RUNNING) {
             sl->core_stat = TARGET_RUNNING;
         } else if (sl->q_buf[0] == STLINK_CORE_HALTED) {
@@ -435,7 +452,7 @@ int _stlink_usb_status(stlink_t * sl) {
         sl->core_stat = TARGET_UNKNOWN;
     }
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_force_debug(stlink_t *sl) {
@@ -456,14 +473,9 @@ int _stlink_usb_force_debug(stlink_t *sl) {
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_FORCEDEBUG\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "FORCEDEBUG");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_enter_swd_mode(stlink_t * sl) {
@@ -478,14 +490,9 @@ int _stlink_usb_enter_swd_mode(stlink_t * sl) {
     // select correct API-Version for entering SWD mode: V1 API (0x20) or V2 API (0x30).
     cmd[i++] = sl->version.jtag_api == STLINK_JTAG_API_V1 ? STLINK_DEBUG_APIV1_ENTER : STLINK_DEBUG_APIV2_ENTER;
     cmd[i++] = STLINK_DEBUG_ENTER_SWD;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_ENTER\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "ENTER_SWD");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_exit_dfu_mode(stlink_t* sl) {
@@ -496,14 +503,9 @@ int _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 
     cmd[i++] = STLINK_DFU_COMMAND;
     cmd[i++] = STLINK_DFU_EXIT;
-    size = send_only(slu, 1, cmd, slu->cmd_len);
+    size = send_only(slu, 1, cmd, slu->cmd_len, "DFU_EXIT");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DFU_EXIT\n");
-        return((int)size);
-    }
-
-    return(0);
+    return(size<0?-1:0);
 }
 
 
@@ -524,14 +526,9 @@ int _stlink_usb_reset(stlink_t * sl) {
         cmd[i++] = STLINK_DEBUG_APIV2_RESETSYS;
     }
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "RESETSYS");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_RESETSYS\n");
-        return((int)size);
-    }
-
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_jtag_reset(stlink_t * sl, int value) {
@@ -543,16 +540,11 @@ int _stlink_usb_jtag_reset(stlink_t * sl, int value) {
     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
-    cmd[i++] = STLINK_JTAG_DRIVE_NRST;
+    cmd[i++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
     cmd[i++] = value;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_JTAG_DRIVE_NRST\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "DRIVE_NRST");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 
@@ -577,14 +569,9 @@ int _stlink_usb_step(stlink_t* sl) {
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_STEPCORE;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_STEPCORE\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "STEPCORE");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 /**
@@ -613,14 +600,9 @@ int _stlink_usb_run(stlink_t* sl, enum run_type type) {
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_RUNCORE;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_RUNCORE\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "RUNCORE");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) {
@@ -661,27 +643,21 @@ int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) {
         cmd[i++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
         cmd[i++] = clk_divisor & 0xFF;
         cmd[i++] = (clk_divisor >> 8) & 0xFF;
-        size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+        size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "SWD_SET_FREQ");
 
-        if (size == -1) {
-            printf("[!] send_recv STLINK_DEBUG_APIV2_SWD_SET_FREQ\n");
-            return((int)size);
-        }
-
-        return(0);
+        return(size<0?-1:0);
     } else if (sl->version.stlink_v == 3) {
         int speed_index;
         uint32_t map[STLINK_V3_MAX_FREQ_NB];
         i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
 
         cmd[i++] = STLINK_DEBUG_COMMAND;
-        cmd[i++] = STLINK_APIV3_GET_COM_FREQ;
+        cmd[i++] = STLINK_DEBUG_APIV3_GET_COM_FREQ;
         cmd[i++] = 0; // SWD mode
-        size = send_recv(slu, 1, cmd, slu->cmd_len, data, 52);
+        size = send_recv(slu, 1, cmd, slu->cmd_len, data, 52, CMD_CHECK_STATUS, "GET_COM_FREQ");
 
-        if (size == -1) {
-            printf("[!] send_recv STLINK_APIV3_GET_COM_FREQ\n");
-            return((int)size);
+        if (size < 0) {
+            return(-1);
         }
 
         int speeds_size = data[8];
@@ -694,13 +670,13 @@ int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) {
         // Set to zero all the next entries
         for (i = speeds_size; i < STLINK_V3_MAX_FREQ_NB; i++) map[i] = 0;
 
-        if (!clk_freq) clk_freq = 1800; // set default frequency
+        if (!clk_freq) clk_freq = 1000; // set default frequency
         speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), clk_freq);
 
         i = fill_command(sl, SG_DXFER_FROM_DEV, 16);
 
         cmd[i++] = STLINK_DEBUG_COMMAND;
-        cmd[i++] = STLINK_APIV3_SET_COM_FREQ;
+        cmd[i++] = STLINK_DEBUG_APIV3_SET_COM_FREQ;
         cmd[i++] = 0; // SWD mode
         cmd[i++] = 0;
         cmd[i++] = (uint8_t)((map[speed_index] >> 0) & 0xFF);
@@ -708,14 +684,9 @@ int _stlink_usb_set_swdclk(stlink_t* sl, int clk_freq) {
         cmd[i++] = (uint8_t)((map[speed_index] >> 16) & 0xFF);
         cmd[i++] = (uint8_t)((map[speed_index] >> 24) & 0xFF);
 
-        size = send_recv(slu, 1, cmd, slu->cmd_len, data, 8);
-
-        if (size == -1) {
-            printf("[!] send_recv STLINK_APIV3_SET_COM_FREQ\n");
-            return((int)size);
-        }
+        size = send_recv(slu, 1, cmd, slu->cmd_len, data, 8, CMD_CHECK_STATUS, "SET_COM_FREQ");
 
-        return(0);
+        return(size<0?-1:0);
     } else if (clk_freq) {
         WLOG("ST-Link firmware does not support frequency setup\n");
     }
@@ -732,14 +703,9 @@ int _stlink_usb_exit_debug_mode(stlink_t *sl) {
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_EXIT;
 
-    size = send_only(slu, 1, cmd, slu->cmd_len);
+    size = send_only(slu, 1, cmd, slu->cmd_len, "DEBUG_EXIT");
 
-    if (size == -1) {
-        printf("[!] send_only STLINK_DEBUG_EXIT\n");
-        return((int)size);
-    }
-
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
@@ -753,11 +719,10 @@ int _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
     write_uint32(&cmd[i], addr);
     write_uint16(&cmd[i + 4], len);
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, len, CMD_CHECK_NO, "READMEM_32BIT");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_READMEM_32BIT\n");
-        return((int)size);
+    if (size < 0) {
+        return(-1);
     }
 
     sl->q_len = (int)size;
@@ -782,11 +747,10 @@ int _stlink_usb_read_all_regs(stlink_t *sl, struct stlink_reg *regp) {
         cmd[i++] = STLINK_DEBUG_APIV2_READALLREGS;
     }
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_STATUS, "READALLREGS");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_READALLREGS\n");
-        return((int)size);
+    if (size < 0) {
+        return(-1);
     }
 
     /* V1: regs data from offset 0 */
@@ -833,11 +797,10 @@ int _stlink_usb_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) {
     }
 
     cmd[i++] = (uint8_t)r_idx;
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "READREG");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_READREG\n");
-        return((int)size);
+    if (size < 0) {
+        return(-1);
     }
 
     sl->q_len = (int)size;
@@ -1000,17 +963,9 @@ int _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 
     cmd[i++] = idx;
     write_uint32(&cmd[i], reg);
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_RETRY, "WRITEREG");
 
-    if (size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_WRITEREG\n");
-        return((int)size);
-    }
-
-    sl->q_len = (int)size;
-    stlink_print_data(sl);
-
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_enable_trace(stlink_t* sl, uint32_t frequency) {
@@ -1026,17 +981,9 @@ int _stlink_usb_enable_trace(stlink_t* sl, uint32_t frequency) {
     write_uint16(&cmd[i + 0], 2 * STLINK_TRACE_BUF_LEN);
     write_uint32(&cmd[i + 2], frequency);
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_only STLINK_DEBUG_APIV2_START_TRACE_RX\n");
-        return((int)size);
-    }
-
-    sl->q_len = (int)size;
-    stlink_print_data(sl);
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_STATUS, "START_TRACE_RX");
 
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_disable_trace(stlink_t* sl) {
@@ -1050,17 +997,9 @@ int _stlink_usb_disable_trace(stlink_t* sl) {
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
 
-    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
-
-    if (size == -1) {
-        printf("[!] send_only STLINK_DEBUG_APIV2_STOP_TRACE_RX\n");
-        return((int)size);
-    }
+    size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_STATUS, "STOP_TRACE_RX");
 
-    sl->q_len = (int)size;
-    stlink_print_data(sl);
-
-    return(0);
+    return(size<0?-1:0);
 }
 
 int _stlink_usb_read_trace(stlink_t* sl, uint8_t* buf, size_t size) {
@@ -1072,15 +1011,13 @@ int _stlink_usb_read_trace(stlink_t* sl, uint8_t* buf, size_t size) {
 
     cmd[i++] = STLINK_DEBUG_COMMAND;
     cmd[i++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
-    ssize_t send_size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
+    ssize_t send_size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len, CMD_CHECK_NO, "GET_TRACE_NB");
 
-    if (send_size == -1) {
-        printf("[!] send_recv STLINK_DEBUG_APIV2_GET_TRACE_NB\n");
-        return((int)send_size);
-    }
-    if (send_size != 2) {
-        printf("[!] send_recv STLINK_DEBUG_APIV2_GET_TRACE_NB %d\n", (int)send_size);
-        return -1;
+    if (send_size < 0) {
+        return(-1);
+    } else if (send_size != 2) {
+        ELOG("STLINK_DEBUG_APIV2_GET_TRACE_NB reply size %d\n", (int)send_size);
+        return(-1);
     }
 
     uint16_t trace_count = read_uint16(sl->q_buf, 0);
@@ -1180,10 +1117,9 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect,
     int config;
 
     sl = calloc(1, sizeof(stlink_t));
-    slu = calloc(1, sizeof(struct stlink_libusb));
-
     if (sl == NULL) { goto on_malloc_error; }
 
+    slu = calloc(1, sizeof(struct stlink_libusb));
     if (slu == NULL) { goto on_malloc_error; }
 
     ugly_init(verbose);
@@ -1343,11 +1279,22 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect,
     // initialize stlink version (sl->version)
     stlink_version(sl);
 
-    if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
-        // this seems to work, and is unnecessary information for the user.
-        // demoted to debug -- REW
+    int mode = stlink_current_mode(sl);
+    if (mode == STLINK_DEV_DFU_MODE) {
         DLOG("-- exit_dfu_mode\n");
-        stlink_exit_dfu_mode(sl);
+        _stlink_usb_exit_dfu_mode(sl);
+    }
+
+    if (connect == CONNECT_UNDER_RESET) {
+        // for the connect under reset only
+        // OpenOСD says (official documentation is not available) that
+        // the NRST pin must be pull down before selecting the SWD/JTAG mode
+        if (mode == STLINK_DEV_DEBUG_MODE) {
+            DLOG("-- exit_debug_mode\n");
+            _stlink_usb_exit_dfu_mode(sl);
+        }
+
+        _stlink_usb_jtag_reset(sl, STLINK_DEBUG_APIV2_DRIVE_NRST_LOW);
     }
 
     sl->freq = freq;
@@ -1355,7 +1302,7 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect,
     // should be done at this speed too
     // set the stlink clock speed (default is 1800kHz)
     DLOG("JTAG/SWD freq set to %d\n", freq);
-    stlink_set_swdclk(sl, freq);
+    _stlink_usb_set_swdclk(sl, freq);
 
     stlink_target_connect(sl, connect);
     return(sl);
@@ -1365,13 +1312,10 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, enum connect_type connect,
     return(NULL);
 
 on_error:
-
     if (slu->libusb_ctx) { libusb_exit(slu->libusb_ctx); }
 
 on_malloc_error:
-
     if (sl != NULL) { free(sl); }
-
     if (slu != NULL) { free(slu); }
 
     return(NULL);