diff --git a/flashstub/stm32f1.c b/flashstub/stm32f1.c
index 8370114..2268eee 100644
--- a/flashstub/stm32f1.c
+++ b/flashstub/stm32f1.c
@@ -18,6 +18,9 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include "libopencm3/stm32/flash.h"
+#include "stub.h"
+
+#define SR_ERROR_MASK 0x14
 
 void __attribute__((naked))
 stm32f1_flash_write_stub(uint16_t *dest, uint16_t *src, uint32_t size)
@@ -29,6 +32,10 @@ stm32f1_flash_write_stub(uint16_t *dest, uint16_t *src, uint32_t size)
 		while (FLASH_SR & FLASH_SR_BSY)
 			;
 	}
-	asm("bkpt");
+
+	if (FLASH_SR & SR_ERROR_MASK)
+		stub_exit(1);
+
+	stub_exit(0);
 }
 
diff --git a/flashstub/stm32f1.stub b/flashstub/stm32f1.stub
index 97d2a5e..428dbb0 100644
--- a/flashstub/stm32f1.stub
+++ b/flashstub/stm32f1.stub
@@ -1 +1 @@
-0x4613, 0xE010, 0x4A09, 0x2401, 0x6014, 0x4602, 0x1C90, 0x460C, 0x1CA1, 0x8824, 0x8014, 0x3B02, 0xBF00, 0x4A05, 0x6812, 0xF002, 0x0201, 0x2A00, 0xD1F9, 0x2B00, 0xD1EC, 0xBE00, 0x2010, 0x4002, 0x200C, 0x4002, 
\ No newline at end of file
+0x4613, 0xE010, 0x4A0D, 0x2401, 0x6014, 0x4602, 0x1C90, 0x460C, 0x1CA1, 0x8824, 0x8014, 0x3B02, 0xBF00, 0x4A09, 0x6812, 0xF002, 0x0201, 0x2A00, 0xD1F9, 0x2B00, 0xD1EC, 0x4B05, 0x681B, 0xF003, 0x0314, 0x2B00, 0xD000, 0xBE01, 0xBE00, 0xBF00, 0x2010, 0x4002, 0x200C, 0x4002, 
\ No newline at end of file
diff --git a/flashstub/stub.h b/flashstub/stub.h
new file mode 100644
index 0000000..d279cf3
--- /dev/null
+++ b/flashstub/stub.h
@@ -0,0 +1,30 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __STUB_H
+#define __STUB_H
+
+static inline __attribute__((always_inline))
+stub_exit(const int code)
+{
+	asm("bkpt %0"::"i"(code));
+}
+
+#endif
+
diff --git a/src/cortexm.c b/src/cortexm.c
index 0327edc..a129a3c 100644
--- a/src/cortexm.c
+++ b/src/cortexm.c
@@ -60,6 +60,7 @@ const struct command_s cortexm_cmd_list[] = {
 static int cortexm_regs_read(struct target_s *target, void *data);
 static int cortexm_regs_write(struct target_s *target, const void *data);
 static int cortexm_pc_write(struct target_s *target, const uint32_t val);
+static uint32_t cortexm_pc_read(struct target_s *target);
 
 static void cortexm_reset(struct target_s *target);
 static int cortexm_halt_wait(struct target_s *target);
@@ -216,6 +217,7 @@ cortexm_probe(struct target_s *target)
 	target->regs_read = cortexm_regs_read;
 	target->regs_write = cortexm_regs_write;
 	target->pc_write = cortexm_pc_write;
+	target->pc_read = cortexm_pc_read;
 
 	target->reset = cortexm_reset;
 	target->halt_request = cortexm_halt_request;
@@ -629,7 +631,12 @@ int cortexm_run_stub(struct target_s *target, uint32_t loadaddr,
 	while (!cortexm_halt_wait(target))
 		;
 
-	return 0;
+	uint32_t pc = cortexm_pc_read(target);
+	uint16_t bkpt_instr = target_mem_read16(target, pc);
+	if (bkpt_instr >> 8 != 0xbe)
+		return -2;
+
+	return bkpt_instr & 0xff;
 }
 
 /* The following routines implement hardware breakpoints.
diff --git a/src/stm32f1.c b/src/stm32f1.c
index 96faf1f..8f7a7d7 100644
--- a/src/stm32f1.c
+++ b/src/stm32f1.c
@@ -256,16 +256,11 @@ static int stm32f1_flash_write(struct target_s *target, uint32_t dest,
 	memcpy((uint8_t *)data + offset, src, len);
 
 	/* Write stub and data to target ram and set PC */
-	target_mem_write(target, STUB_BUFFER_BASE, data, sizeof(data));
-	cortexm_run_stub(target, SRAM_BASE, stm32f1_flash_write_stub,
-	                sizeof(stm32f1_flash_write_stub),
-	                dest - offset, STUB_BUFFER_BASE, sizeof(data), 0);
-
-	/* Check for error */
-	if (target_mem_read32(target, FLASH_SR) & SR_ERROR_MASK)
-		return -1;
-
-	return 0;
+	target_mem_write(target, STUB_BUFFER_BASE, (void*)data, sizeof(data));
+	return cortexm_run_stub(target, SRAM_BASE, stm32f1_flash_write_stub,
+	                        sizeof(stm32f1_flash_write_stub),
+	                        dest - offset, STUB_BUFFER_BASE, sizeof(data),
+	                        0);
 }
 
 static bool stm32f1_cmd_erase_mass(target *t)