diff --git a/src/Makefile b/src/Makefile
index 02b5690..73fb8e9 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -24,6 +24,7 @@ SRC =			\
 	command.c	\
 	cortexm.c	\
 	crc32.c		\
+	exception.c	\
 	gdb_if.c	\
 	gdb_main.c	\
 	gdb_packet.c	\
diff --git a/src/adiv5_jtagdp.c b/src/adiv5_jtagdp.c
index 5049e3c..d97bc99 100644
--- a/src/adiv5_jtagdp.c
+++ b/src/adiv5_jtagdp.c
@@ -23,6 +23,7 @@
  */
 
 #include "general.h"
+#include "exception.h"
 #include "adiv5.h"
 #include "jtag_scan.h"
 #include "jtagtap.h"
@@ -84,19 +85,17 @@ static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 	jtag_dev_write_ir(dp->dev, APnDP ? IR_APACC : IR_DPACC);
 
-	int tries = 1000;
+	platform_timeout_set(2000);
 	do {
 		jtag_dev_shift_dr(dp->dev, (uint8_t*)&response, (uint8_t*)&request, 35);
 		ack = response & 0x07;
-	} while(--tries && (ack == JTAGDP_ACK_WAIT));
+	} while(!platform_timeout_is_expired() && (ack == JTAGDP_ACK_WAIT));
 
-	if (dp->allow_timeout && (ack == JTAGDP_ACK_WAIT))
-		return 0;
+	if (ack == JTAGDP_ACK_WAIT)
+		raise_exception(EXCEPTION_TIMEOUT, "JTAG-DP ACK timeout");
 
-	if((ack != JTAGDP_ACK_OK)) {
-		/* Fatal error if invalid ACK response */
-		PLATFORM_FATAL_ERROR(1);
-	}
+	if((ack != JTAGDP_ACK_OK))
+		raise_exception(EXCEPTION_ERROR, "JTAG-DP invalid ACK");
 
 	return (uint32_t)(response >> 3);
 }
diff --git a/src/adiv5_swdp.c b/src/adiv5_swdp.c
index 9ac791a..b53fcb5 100644
--- a/src/adiv5_swdp.c
+++ b/src/adiv5_swdp.c
@@ -23,6 +23,7 @@
  */
 
 #include "general.h"
+#include "exception.h"
 #include "adiv5.h"
 #include "swdptap.h"
 #include "jtagtap.h"
@@ -129,28 +130,26 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	if((addr == 4) || (addr == 8))
 		request ^= 0x20;
 
-	size_t tries = 1000;
+	platform_timeout_set(2000);
 	do {
 		swdptap_seq_out(request, 8);
 		ack = swdptap_seq_in(3);
-	} while(--tries && ack == SWDP_ACK_WAIT);
+	} while (!platform_timeout_is_expired() && ack == SWDP_ACK_WAIT);
 
-	if (dp->allow_timeout && (ack == SWDP_ACK_WAIT))
-		return 0;
+	if (ack == SWDP_ACK_WAIT)
+		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
 
 	if(ack == SWDP_ACK_FAULT) {
 		dp->fault = 1;
 		return 0;
 	}
 
-	if(ack != SWDP_ACK_OK) {
-		/* Fatal error if invalid ACK response */
-		PLATFORM_FATAL_ERROR(1);
-	}
+	if(ack != SWDP_ACK_OK)
+		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
 
 	if(RnW) {
 		if(swdptap_seq_in_parity(&response, 32))  /* Give up on parity error */
-			PLATFORM_FATAL_ERROR(1);
+			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
 		swdptap_seq_out_parity(value, 32);
 	}
diff --git a/src/command.c b/src/command.c
index f271085..2c2a198 100644
--- a/src/command.c
+++ b/src/command.c
@@ -23,6 +23,7 @@
  */
 
 #include "general.h"
+#include "exception.h"
 #include "command.h"
 #include "gdb_packet.h"
 #include "jtag_scan.h"
@@ -138,19 +139,30 @@ bool cmd_help(target *t)
 static bool cmd_jtag_scan(target *t, int argc, char **argv)
 {
 	(void)t;
-	uint8_t *irlens = NULL;
+	uint8_t irlens[argc];
 
 	gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
 	if (argc > 1) {
 		/* Accept a list of IR lengths on command line */
-		irlens = alloca(argc);
 		for (int i = 1; i < argc; i++)
 			irlens[i-1] = atoi(argv[i]);
 		irlens[argc-1] = 0;
 	}
 
-	int devs = jtag_scan(irlens);
+	int devs = -1;
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_ALL) {
+		devs = jtag_scan(argc > 1 ? irlens : NULL);
+	}
+	switch (e.type) {
+	case EXCEPTION_TIMEOUT:
+		gdb_outf("Timeout during scan. Is target stuck in WFI?\n");
+		break;
+	case EXCEPTION_ERROR:
+		gdb_outf("Exception: %s\n", e.msg);
+		break;
+	}
 
 	if(devs < 0) {
 		gdb_out("JTAG device scan failed!\n");
@@ -174,13 +186,25 @@ bool cmd_swdp_scan(void)
 {
 	gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
-	if(adiv5_swdp_scan() < 0) {
+	int devs = -1;
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_ALL) {
+		devs = adiv5_swdp_scan();
+	}
+	switch (e.type) {
+	case EXCEPTION_TIMEOUT:
+		gdb_outf("Timeout during scan. Is target stuck in WFI?\n");
+		break;
+	case EXCEPTION_ERROR:
+		gdb_outf("Exception: %s\n", e.msg);
+		break;
+	}
+
+	if(devs < 0) {
 		gdb_out("SW-DP scan failed!\n");
 		return false;
 	}
 
-	//gdb_outf("SW-DP detected IDCODE: 0x%08X\n", adiv5_dp_list->idcode);
-
 	cmd_targets(NULL);
 	return true;
 
diff --git a/src/cortexm.c b/src/cortexm.c
index a129a3c..f6a0ea3 100644
--- a/src/cortexm.c
+++ b/src/cortexm.c
@@ -29,6 +29,7 @@
  * There are way too many magic numbers used here.
  */
 #include "general.h"
+#include "exception.h"
 #include "jtagtap.h"
 #include "jtag_scan.h"
 #include "adiv5.h"
@@ -36,6 +37,7 @@
 #include "command.h"
 #include "gdb_packet.h"
 #include "cortexm.h"
+#include "morse.h"
 
 #include <unistd.h>
 
@@ -56,6 +58,7 @@ const struct command_s cortexm_cmd_list[] = {
 #define SIGINT 2
 #define SIGTRAP 5
 #define SIGSEGV 11
+#define SIGLOST 29
 
 static int cortexm_regs_read(struct target_s *target, void *data);
 static int cortexm_regs_write(struct target_s *target, const void *data);
@@ -467,12 +470,15 @@ cortexm_reset(struct target_s *target)
 static void
 cortexm_halt_request(struct target_s *target)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(target);
-
-	ap->dp->allow_timeout = false;
-	target_mem_write32(target, CORTEXM_DHCSR,
-	                   CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_C_HALT |
-	                   CORTEXM_DHCSR_C_DEBUGEN);
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_TIMEOUT) {
+		target_mem_write32(target, CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY |
+		                                          CORTEXM_DHCSR_C_HALT |
+		                                          CORTEXM_DHCSR_C_DEBUGEN);
+	}
+	if (e.type) {
+		gdb_out("Timeout sending interrupt, is target in WFI?\n");
+	}
 }
 
 static int
@@ -480,10 +486,27 @@ cortexm_halt_wait(struct target_s *target)
 {
 	ADIv5_AP_t *ap = adiv5_target_ap(target);
 	struct cortexm_priv *priv = ap->priv;
-	if (!(target_mem_read32(target, CORTEXM_DHCSR) & CORTEXM_DHCSR_S_HALT))
-		return 0;
 
-	ap->dp->allow_timeout = false;
+	uint32_t dhcsr = 0;
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_ALL) {
+		/* If this times out because the target is in WFI then
+		 * the target is still running. */
+		dhcsr = target_mem_read32(target, CORTEXM_DHCSR);
+	}
+	switch (e.type) {
+	case EXCEPTION_ERROR:
+		/* Oh crap, there's no recovery from this... */
+		target_list_free();
+		morse("TARGET LOST.", 1);
+		return SIGLOST;
+	case EXCEPTION_TIMEOUT:
+		/* Timeout isn't a problem, target could be in WFI */
+		return 0;
+	}
+
+	if (!(dhcsr & CORTEXM_DHCSR_S_HALT))
+		return 0;
 
 	/* We've halted.  Let's find out why. */
 	uint32_t dfsr = target_mem_read32(target, CORTEXM_DFSR);
@@ -543,7 +566,6 @@ void cortexm_halt_resume(struct target_s *target, bool step)
 	}
 
 	target_mem_write32(target, CORTEXM_DHCSR, dhcsr);
-	ap->dp->allow_timeout = true;
 }
 
 static int cortexm_fault_unwind(struct target_s *target)
diff --git a/src/exception.c b/src/exception.c
new file mode 100644
index 0000000..3d43f99
--- /dev/null
+++ b/src/exception.c
@@ -0,0 +1,39 @@
+/*
+ * 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/>.
+ */
+
+#include "general.h"
+#include "exception.h"
+
+struct exception *innermost_exception;
+
+void raise_exception(uint32_t type, const char *msg)
+{
+	struct exception *e;
+	for (e = innermost_exception; e; e = e->outer) {
+		if (e->mask & type) {
+			e->type = type;
+			e->msg = msg;
+			innermost_exception = e->outer;
+			longjmp(e->jmpbuf, type);
+		}
+	}
+	abort();
+}
+
diff --git a/src/gdb_main.c b/src/gdb_main.c
index 7bb788b..750dfe2 100644
--- a/src/gdb_main.c
+++ b/src/gdb_main.c
@@ -158,6 +158,12 @@ gdb_main(void)
 			if (sig < 0)
 				break;
 
+			/* Target disappeared */
+			if (cur_target == NULL) {
+				gdb_putpacket_f("X%02X", sig);
+				break;
+			}
+
 			/* Report reason for halt */
 			if(target_check_hw_wp(cur_target, &watch_addr)) {
 				/* Watchpoint hit */
diff --git a/src/include/adiv5.h b/src/include/adiv5.h
index 6896f6d..12d3bf4 100644
--- a/src/include/adiv5.h
+++ b/src/include/adiv5.h
@@ -107,12 +107,8 @@ typedef struct ADIv5_DP_s {
 
 	uint32_t idcode;
 
-	bool allow_timeout;
-
 	uint32_t (*dp_read)(struct ADIv5_DP_s *dp, uint16_t addr);
-
 	uint32_t (*error)(struct ADIv5_DP_s *dp);
-
 	uint32_t (*low_access)(struct ADIv5_DP_s *dp, uint8_t RnW,
                                uint16_t addr, uint32_t value);
 
diff --git a/src/include/exception.h b/src/include/exception.h
new file mode 100644
index 0000000..180398d
--- /dev/null
+++ b/src/include/exception.h
@@ -0,0 +1,74 @@
+/*
+ * 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/>.
+ */
+
+/* Exception handling to escape deep nesting.
+ * Used for the case of communicaiton failure and timeouts.
+ */
+
+/* Example usage:
+ *
+ * volatile struct exception e;
+ * TRY_CATCH (e, EXCEPTION_TIMEOUT) {
+ *    ...
+ *    raise_exception(EXCEPTION_TIMEOUT, "Timeout occurred");
+ *    ...
+ * }
+ * if (e.type == EXCEPTION_TIMEOUT) {
+ *    printf("timeout: %s\n", e.msg);
+ * }
+ */
+
+/* Limitations:
+ * Can't use break, return, goto, etc from inside the TRY_CATCH block.
+ */
+
+#ifndef __EXCEPTION_H
+#define __EXCEPTION_H
+
+#include <setjmp.h>
+#include <stdint.h>
+
+#define EXCEPTION_ERROR   0x01
+#define EXCEPTION_TIMEOUT 0x02
+#define EXCEPTION_ALL     -1
+
+struct exception {
+	uint32_t type;
+	const char *msg;
+	/* private */
+	uint32_t mask;
+	jmp_buf jmpbuf;
+	struct exception *outer;
+};
+
+extern struct exception *innermost_exception;
+
+#define TRY_CATCH(e, type_mask) \
+	(e).type = 0; \
+	(e).mask = (type_mask); \
+	(e).outer = innermost_exception; \
+	innermost_exception = (void*)&(e); \
+	if (setjmp(innermost_exception->jmpbuf) == 0) \
+		for (;innermost_exception == &(e); innermost_exception = (e).outer)
+
+void raise_exception(uint32_t type, const char *msg);
+
+#endif
+
diff --git a/src/main.c b/src/main.c
index 60db15c..d1b3c86 100644
--- a/src/main.c
+++ b/src/main.c
@@ -28,6 +28,9 @@
 #include "jtagtap.h"
 #include "jtag_scan.h"
 #include "target.h"
+#include "exception.h"
+#include "gdb_packet.h"
+#include "morse.h"
 
 int
 main(int argc, char **argv)
@@ -39,9 +42,18 @@ main(int argc, char **argv)
 	(void) argv;
 	platform_init();
 #endif
-	PLATFORM_SET_FATAL_ERROR_RECOVERY();
 
-	gdb_main();
+	while (true) {
+		volatile struct exception e;
+		TRY_CATCH(e, EXCEPTION_ALL) {
+			gdb_main();
+		}
+		if (e.type == EXCEPTION_ERROR) {
+			gdb_putpacketz("EFF");
+			target_list_free();
+			morse("TARGET LOST.", 1);
+		}
+	}
 
 	/* Should never get here */
 	return 0;
diff --git a/src/platforms/f4discovery/platform.h b/src/platforms/f4discovery/platform.h
index 3c5c582..92ae046 100644
--- a/src/platforms/f4discovery/platform.h
+++ b/src/platforms/f4discovery/platform.h
@@ -24,9 +24,7 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
-#include "gdb_packet.h"
 #include "gpio.h"
-#include "morse.h"
 #include "timing.h"
 
 #include <setjmp.h>
@@ -141,8 +139,6 @@
 
 #define DEBUG(...)
 
-extern jmp_buf fatal_error_jmpbuf;
-
 #define gpio_set_val(port, pin, val) do {	\
 	if(val)					\
 		gpio_set((port), (pin));	\
@@ -154,16 +150,6 @@ extern jmp_buf fatal_error_jmpbuf;
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
 #define SET_ERROR_STATE(state)	{gpio_set_val(LED_PORT, LED_ERROR, state);}
 
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
-#define PLATFORM_FATAL_ERROR(error)	{ 		\
-	if(running_status) gdb_putpacketz("X1D");	\
-		else gdb_putpacketz("EFF");		\
-	running_status = 0;				\
-	target_list_free();				\
-	morse("TARGET LOST.", 1);			\
-	longjmp(fatal_error_jmpbuf, (error));		\
-}
-
 static inline int platform_hwversion(void)
 {
 	return 0;
diff --git a/src/platforms/launchpad-icdi/platform.c b/src/platforms/launchpad-icdi/platform.c
index e689f0c..ff67fa0 100644
--- a/src/platforms/launchpad-icdi/platform.c
+++ b/src/platforms/launchpad-icdi/platform.c
@@ -33,7 +33,6 @@
 
 extern void trace_tick(void);
 
-jmp_buf fatal_error_jmpbuf;
 uint8_t running_status;
 volatile uint32_t timeout_counter;
 
@@ -86,10 +85,20 @@ platform_init(void)
 	cdcacm_init();
 }
 
+void platform_timeout_set(uint32_t ms)
+{
+	timeout_counter = ms / 10;
+}
+
+bool platform_timeout_is_expired(void)
+{
+	return timeout_counter == 0;
+}
+
 void platform_delay(uint32_t delay)
 {
-	timeout_counter = delay * 10;
-	while(timeout_counter);
+	platform_timeout_set(delay);
+	while (platform_timeout_is_expired());
 }
 
 const char *platform_target_voltage(void)
diff --git a/src/platforms/launchpad-icdi/platform.h b/src/platforms/launchpad-icdi/platform.h
index eb3ac6e..c13172f 100644
--- a/src/platforms/launchpad-icdi/platform.h
+++ b/src/platforms/launchpad-icdi/platform.h
@@ -17,10 +17,6 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
-#include "gdb_packet.h"
-
-#include <setjmp.h>
-
 #include <libopencm3/lm4f/gpio.h>
 #include <libopencm3/usb/usbd.h>
 
@@ -29,7 +25,6 @@
 #define DFU_IDENT               "Black Magic Firmware Upgrade (Launchpad)"
 #define DFU_IFACE_STRING	"lolwut"
 
-extern jmp_buf fatal_error_jmpbuf;
 extern uint8_t running_status;
 extern volatile uint32_t timeout_counter;
 
@@ -106,16 +101,6 @@ extern usbd_driver lm4f_usb_driver;
 #define SET_IDLE_STATE(state)	{}
 #define SET_ERROR_STATE(state)	SET_IDLE_STATE(state)
 
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
-#define PLATFORM_FATAL_ERROR(error) {			\
-	if( running_status ) gdb_putpacketz("X1D");	\
-		else gdb_putpacketz("EFF");		\
-	running_status = 0;				\
-	target_list_free();				\
-	morse("TARGET LOST.", 1);			\
-	longjmp(fatal_error_jmpbuf, (error));		\
-}
-
 #define PLATFORM_HAS_TRACESWO
 
 inline static void gpio_set_val(uint32_t port, uint8_t pin, uint8_t val) {
diff --git a/src/platforms/libftdi/platform.c b/src/platforms/libftdi/platform.c
index 634090d..3062698 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/libftdi/platform.c
@@ -21,6 +21,7 @@
 #include "gdb_if.h"
 
 #include <assert.h>
+#include <sys/time.h>
 
 struct ftdi_context *ftdic;
 
@@ -258,3 +259,21 @@ void platform_delay(uint32_t delay)
 	usleep(delay * 100000);
 }
 
+static uint32_t timeout_time;
+static uint32_t time_ms(void)
+{
+	struct timeval tv;
+	gettimeofday(&tv, NULL);
+	return (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
+}
+
+void platform_timeout_set(uint32_t ms)
+{
+	timeout_time = time_ms() + ms;
+}
+
+bool platform_timeout_is_expired(void)
+{
+	return time_ms() > timeout_time;
+}
+
diff --git a/src/platforms/libftdi/platform.h b/src/platforms/libftdi/platform.h
index 135b2e3..8611674 100644
--- a/src/platforms/libftdi/platform.h
+++ b/src/platforms/libftdi/platform.h
@@ -36,9 +36,6 @@
 #define SET_IDLE_STATE(state)
 #define SET_ERROR_STATE(state)
 
-#define PLATFORM_FATAL_ERROR(error)	abort()
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()
-
 extern struct ftdi_context *ftdic;
 
 void platform_buffer_flush(void);
diff --git a/src/platforms/native/platform.c b/src/platforms/native/platform.c
index ac54b4b..babb498 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -35,8 +35,6 @@
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/stm32/f1/adc.h>
 
-jmp_buf fatal_error_jmpbuf;
-
 static void adc_init(void);
 static void setup_vbus_irq(void);
 
diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index ae9bcef..6958585 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -24,13 +24,9 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
-#include "gdb_packet.h"
 #include "gpio.h"
-#include "morse.h"
 #include "timing.h"
 
-#include <setjmp.h>
-
 #define PLATFORM_HAS_TRACESWO
 #define PLATFORM_HAS_POWER_SWITCH
 #define BOARD_IDENT             "Black Magic Probe"
@@ -147,22 +143,10 @@
 
 #define DEBUG(...)
 
-extern jmp_buf fatal_error_jmpbuf;
-
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
 #define SET_ERROR_STATE(state)	{gpio_set_val(LED_PORT, LED_ERROR, state);}
 
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
-#define PLATFORM_FATAL_ERROR(error)	do { 		\
-	if(running_status) gdb_putpacketz("X1D");	\
-		else gdb_putpacketz("EFF");		\
-	running_status = 0;				\
-	target_list_free();				\
-	morse("TARGET LOST.", 1);			\
-	longjmp(fatal_error_jmpbuf, (error));		\
-} while (0)
-
 /* Use newlib provided integer only stdio functions */
 #define sscanf siscanf
 #define sprintf siprintf
diff --git a/src/platforms/stlink/platform.c b/src/platforms/stlink/platform.c
index fb10faa..dd501a4 100644
--- a/src/platforms/stlink/platform.c
+++ b/src/platforms/stlink/platform.c
@@ -36,8 +36,6 @@
 uint8_t running_status;
 volatile uint32_t timeout_counter;
 
-jmp_buf fatal_error_jmpbuf;
-
 uint16_t led_idle_run;
 /* Pins PC[14:13] are used to detect hardware revision. Read
  * 11 for STLink V1 e.g. on VL Discovery, tag as hwversion 0
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index f026db3..aa02ac0 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -24,7 +24,6 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
-#include "gdb_packet.h"
 #include "gpio.h"
 #include "timing.h"
 
@@ -32,8 +31,6 @@
 #include <libopencm3/stm32/f1/memorymap.h>
 #include <libopencm3/usb/usbd.h>
 
-#include <setjmp.h>
-
 #define BOARD_IDENT       "Black Magic Probe (STLINK), (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
 #define BOARD_IDENT_DFU   "Black Magic (Upgrade) for STLink/Discovery, (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
 #define BOARD_IDENT_UPD   "Black Magic (DFU Upgrade) for STLink/Discovery, (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
@@ -130,23 +127,12 @@
 
 #define DEBUG(...)
 
-extern jmp_buf fatal_error_jmpbuf;
-
 extern uint16_t led_idle_run;
 #define LED_IDLE_RUN            led_idle_run
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, led_idle_run, state);}
 #define SET_ERROR_STATE(x)
 
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
-#define PLATFORM_FATAL_ERROR(error)	do { 		\
-	if(running_status) gdb_putpacketz("X1D");	\
-		else gdb_putpacketz("EFF");		\
-	running_status = 0;				\
-	target_list_free();				\
-	longjmp(fatal_error_jmpbuf, (error));		\
-} while (0)
-
 /* Use newlib provided integer only stdio functions */
 #define sscanf siscanf
 #define sprintf siprintf
diff --git a/src/platforms/stm32/timing.c b/src/platforms/stm32/timing.c
index cac22ca..55a217a 100644
--- a/src/platforms/stm32/timing.c
+++ b/src/platforms/stm32/timing.c
@@ -17,6 +17,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include "general.h"
+#include "morse.h"
 
 #include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/scb.h>
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index ca793d4..0ca2a3b 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -2,6 +2,7 @@ CROSS_COMPILE ?= arm-none-eabi-
 CC = $(CROSS_COMPILE)gcc
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
+OPT_FLAGS = -Os
 CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DDISCOVERY_SWLINK -I../libopencm3/include \
 	-I platforms/stm32
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index cbdae0c..02cde19 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -33,8 +33,6 @@
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/stm32/f1/adc.h>
 
-jmp_buf fatal_error_jmpbuf;
-
 void platform_init(void)
 {
 	uint32_t data;
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index 27cc299..2c6fb76 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -24,12 +24,9 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
-#include "gdb_packet.h"
 #include "gpio.h"
 #include "timing.h"
 
-#include <setjmp.h>
-
 #define BOARD_IDENT            "Black Magic Probe (SWLINK), (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
 #define BOARD_IDENT_DFU	       "Black Magic (Upgrade), STM8S Discovery, (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
 #define BOARD_IDENT_UPD	       "Black Magic (DFU Upgrade), STM8S Discovery, (Firmware 1.5" VERSION_SUFFIX ", build " BUILDDATE ")"
@@ -125,21 +122,10 @@
 
 #define DEBUG(...)
 
-extern jmp_buf fatal_error_jmpbuf;
-
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
 #define SET_ERROR_STATE(x)
 
-#define PLATFORM_SET_FATAL_ERROR_RECOVERY()	{setjmp(fatal_error_jmpbuf);}
-#define PLATFORM_FATAL_ERROR(error)	{ 		\
-	if(running_status) gdb_putpacketz("X1D");	\
-		else gdb_putpacketz("EFF");		\
-	running_status = 0;				\
-	target_list_free();				\
-	longjmp(fatal_error_jmpbuf, (error));		\
-}
-
 /* Use newlib provided integer only stdio functions */
 #define sscanf siscanf
 #define sprintf siprintf