diff --git a/src/common.c b/src/common.c
index d04d100..e3c4fdd 100644
--- a/src/common.c
+++ b/src/common.c
@@ -1490,8 +1490,14 @@ int stlink_run(stlink_t *sl) {
     struct stlink_reg rr;
     DLOG("*** stlink_run ***\n");
 
+    /* Make sure we are in Thumb mode
+     * Cortex-M chips don't support ARM mode instructions
+     * xPSR may be incorrect if the vector table has invalid data */
     stlink_read_reg(sl, 16, &rr);
-    DLOG("Run in %s mode\n", (rr.xpsr & (1 << 24))?"ARM":"THUMB");
+    if ((rr.xpsr & (1 << 24)) == 0) {
+        ILOG("Go to Thumb mode\n");
+        stlink_write_reg(sl, rr.xpsr | (1 << 24), 16);
+    }
 
     return(sl->backend->run(sl));
 }
diff --git a/src/stlink-lib/flash_loader.c b/src/stlink-lib/flash_loader.c
index c86aefb..b7726e8 100644
--- a/src/stlink-lib/flash_loader.c
+++ b/src/stlink-lib/flash_loader.c
@@ -356,13 +356,6 @@ 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
 
-    /* Make sure we are in Thumb mode */
-    stlink_read_reg(sl, 16, &rr);
-    if ((rr.xpsr & (1 << 24)) == 0) {
-        ILOG("Go to Thumb mode\n");
-        stlink_write_reg(sl, rr.xpsr | (1 << 24), 16);
-    }
-
     /* Run loader */
     stlink_run(sl);