diff --git a/src/include/platform_support.h b/src/include/platform_support.h
index 7d8443c..2bfbe65 100644
--- a/src/include/platform_support.h
+++ b/src/include/platform_support.h
@@ -32,6 +32,8 @@ void platform_init(void);
 
 const char *platform_target_voltage(void);
 int platform_hwversion(void);
+void platform_timeout_set(uint32_t ms);
+bool platform_timeout_is_expired(void);
 void platform_delay(uint32_t delay);
 void platform_srst_set_val(bool assert);
 bool platform_target_get_power(void);
diff --git a/src/platforms/f4discovery/Makefile.inc b/src/platforms/f4discovery/Makefile.inc
index 21b4078..d8f7754 100644
--- a/src/platforms/f4discovery/Makefile.inc
+++ b/src/platforms/f4discovery/Makefile.inc
@@ -20,6 +20,7 @@ SRC += 	cdcacm.c	\
 	traceswo.c	\
 	usbuart.c	\
 	serialno.c	\
+	timing.c	\
 
 all:	blackmagic.bin
 
diff --git a/src/platforms/f4discovery/platform.c b/src/platforms/f4discovery/platform.c
index 6e10ff4..e85ebd5 100644
--- a/src/platforms/f4discovery/platform.c
+++ b/src/platforms/f4discovery/platform.c
@@ -28,7 +28,6 @@
 #include "morse.h"
 
 #include <libopencm3/stm32/f4/rcc.h>
-#include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/scb.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/exti.h>
@@ -36,9 +35,6 @@
 #include <libopencm3/stm32/syscfg.h>
 #include <libopencm3/usb/usbd.h>
 
-uint8_t running_status;
-volatile uint32_t timeout_counter;
-
 jmp_buf fatal_error_jmpbuf;
 
 void platform_init(void)
@@ -77,35 +73,11 @@ void platform_init(void)
 			GPIO_PUPD_NONE,
 			LED_UART | LED_IDLE_RUN | LED_ERROR | LED_BOOTLOADER);
 
-	/* Setup heartbeat timer */
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(168000000/(10*8));	/* Interrupt us at 10 Hz */
-	SCB_SHPR(11) &= ~((15 << 4) & 0xff);
-	SCB_SHPR(11) |= ((14 << 4) & 0xff);
-	systick_interrupt_enable();
-	systick_counter_enable();
-
+	platform_timing_init();
 	usbuart_init();
 	cdcacm_init();
 }
 
-void platform_delay(uint32_t delay)
-{
-	timeout_counter = delay;
-	while(timeout_counter);
-}
-
-void sys_tick_handler(void)
-{
-	if(running_status)
-		gpio_toggle(LED_PORT, LED_IDLE_RUN);
-
-	if(timeout_counter)
-		timeout_counter--;
-
-	SET_ERROR_STATE(morse_update());
-}
-
 const char *platform_target_voltage(void)
 {
 	return "ABSENT!";
diff --git a/src/platforms/f4discovery/platform.h b/src/platforms/f4discovery/platform.h
index 61753af..3c5c582 100644
--- a/src/platforms/f4discovery/platform.h
+++ b/src/platforms/f4discovery/platform.h
@@ -27,6 +27,7 @@
 #include "gdb_packet.h"
 #include "gpio.h"
 #include "morse.h"
+#include "timing.h"
 
 #include <setjmp.h>
 
@@ -140,9 +141,6 @@
 
 #define DEBUG(...)
 
-extern uint8_t running_status;
-extern volatile uint32_t timeout_counter;
-
 extern jmp_buf fatal_error_jmpbuf;
 
 #define gpio_set_val(port, pin, val) do {	\
diff --git a/src/platforms/native/Makefile.inc b/src/platforms/native/Makefile.inc
index 2270ff5..8b76a53 100644
--- a/src/platforms/native/Makefile.inc
+++ b/src/platforms/native/Makefile.inc
@@ -19,6 +19,7 @@ SRC += 	cdcacm.c	\
 	traceswo.c	\
 	usbuart.c	\
 	serialno.c	\
+	timing.c	\
 
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 
diff --git a/src/platforms/native/platform.c b/src/platforms/native/platform.c
index 5c28747..ac54b4b 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -28,7 +28,6 @@
 #include "morse.h"
 
 #include <libopencm3/stm32/f1/rcc.h>
-#include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/scb.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/exti.h>
@@ -36,9 +35,6 @@
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/stm32/f1/adc.h>
 
-uint8_t running_status;
-volatile uint32_t timeout_counter;
-
 jmp_buf fatal_error_jmpbuf;
 
 static void adc_init(void);
@@ -115,14 +111,6 @@ void platform_init(void)
 		              GPIO_CNF_INPUT_PULL_UPDOWN, PWR_BR_PIN);
 	}
 
-	/* Setup heartbeat timer */
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(900000);	/* Interrupt us at 10 Hz */
-	SCB_SHPR(11) &= ~((15 << 4) & 0xff);
-	SCB_SHPR(11) |= ((14 << 4) & 0xff);
-	systick_interrupt_enable();
-	systick_counter_enable();
-
 	if (platform_hwversion() > 0) {
 		adc_init();
 	} else {
@@ -133,6 +121,7 @@ void platform_init(void)
 	/* Relocate interrupt vector table here */
 	SCB_VTOR = 0x2000;
 
+	platform_timing_init();
 	cdcacm_init();
 	usbuart_init();
 	setup_vbus_irq();
@@ -161,22 +150,6 @@ void platform_target_set_power(bool power)
 		gpio_set_val(PWR_BR_PORT, PWR_BR_PIN, !power);
 	}
 }
-void platform_delay(uint32_t delay)
-{
-	timeout_counter = delay;
-	while(timeout_counter);
-}
-
-void sys_tick_handler(void)
-{
-	if(running_status)
-		gpio_toggle(LED_PORT, LED_IDLE_RUN);
-
-	if(timeout_counter)
-		timeout_counter--;
-
-	SET_ERROR_STATE(morse_update());
-}
 
 static void adc_init(void)
 {
diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index c0b4dba..ae9bcef 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -27,6 +27,7 @@
 #include "gdb_packet.h"
 #include "gpio.h"
 #include "morse.h"
+#include "timing.h"
 
 #include <setjmp.h>
 
@@ -146,9 +147,6 @@
 
 #define DEBUG(...)
 
-extern uint8_t running_status;
-extern volatile uint32_t timeout_counter;
-
 extern jmp_buf fatal_error_jmpbuf;
 
 #define SET_RUN_STATE(state)	{running_status = (state);}
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index 52ad33b..f2ebce5 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -18,6 +18,7 @@ SRC += 	cdcacm.c	\
 	platform.c	\
 	usbuart.c 	\
 	serialno.c	\
+	timing.c	\
 
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade.bin dfu_upgrade.hex
 
diff --git a/src/platforms/stlink/platform.c b/src/platforms/stlink/platform.c
index 446fb47..fb10faa 100644
--- a/src/platforms/stlink/platform.c
+++ b/src/platforms/stlink/platform.c
@@ -27,7 +27,6 @@
 #include "usbuart.h"
 
 #include <libopencm3/stm32/rcc.h>
-#include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/scb.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
@@ -104,25 +103,11 @@ void platform_init(void)
 	gpio_set_mode(LED_PORT, GPIO_MODE_OUTPUT_2_MHZ,
 	              GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
 
-	/* Setup heartbeat timer */
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(900000);	/* Interrupt us at 10 Hz */
-	SCB_SHPR(11) &= ~((15 << 4) & 0xff);
-	SCB_SHPR(11) |= ((14 << 4) & 0xff);
-	systick_interrupt_enable();
-	systick_counter_enable();
-
-	usbuart_init();
-
 	SCB_VTOR = 0x2000; /* Relocate interrupt vector table here */
 
+	platform_timing_init();
 	cdcacm_init();
-}
-
-void platform_delay(uint32_t delay)
-{
-	timeout_counter = delay;
-	while (timeout_counter);
+	usbuart_init();
 }
 
 void platform_srst_set_val(bool assert)
@@ -135,15 +120,6 @@ void platform_srst_set_val(bool assert)
 		gpio_set(SRST_PORT, pin);
 }
 
-void sys_tick_handler(void)
-{
-	if(running_status)
-		gpio_toggle(LED_PORT, led_idle_run);
-
-	if(timeout_counter)
-		timeout_counter--;
-}
-
 const char *platform_target_voltage(void)
 {
 	return "unknown";
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index fcbe259..f026db3 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -26,6 +26,7 @@
 
 #include "gdb_packet.h"
 #include "gpio.h"
+#include "timing.h"
 
 #include <libopencm3/cm3/common.h>
 #include <libopencm3/stm32/f1/memorymap.h>
@@ -129,14 +130,13 @@
 
 #define DEBUG(...)
 
-extern uint8_t running_status;
-extern volatile uint32_t timeout_counter;
-
 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 { 		\
diff --git a/src/platforms/stm32/gdb_if.c b/src/platforms/stm32/gdb_if.c
index 286c66b..52922e7 100644
--- a/src/platforms/stm32/gdb_if.c
+++ b/src/platforms/stm32/gdb_if.c
@@ -113,7 +113,7 @@ unsigned char gdb_if_getchar(void)
 
 unsigned char gdb_if_getchar_to(int timeout)
 {
-	timeout_counter = timeout/100;
+	platform_timeout_set(timeout);
 
 	if (!(out_ptr < count_out)) do {
 		/* Detach if port closed */
@@ -121,7 +121,7 @@ unsigned char gdb_if_getchar_to(int timeout)
 			return 0x04;
 
 		gdb_if_update_buf();
-	} while(timeout_counter && !(out_ptr < count_out));
+	} while (!platform_timeout_is_expired() && !(out_ptr < count_out));
 
 	if(out_ptr < count_out)
 		return gdb_if_getchar();
diff --git a/src/platforms/stm32/timing.c b/src/platforms/stm32/timing.c
new file mode 100644
index 0000000..cac22ca
--- /dev/null
+++ b/src/platforms/stm32/timing.c
@@ -0,0 +1,65 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015 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 <libopencm3/cm3/systick.h>
+#include <libopencm3/cm3/scb.h>
+
+uint8_t running_status;
+
+static volatile uint32_t timeout_counter;
+
+void platform_timing_init(void)
+{
+	/* Setup heartbeat timer */
+	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
+	systick_set_reload(900000);	/* Interrupt us at 10 Hz */
+	SCB_SHPR(11) &= ~((15 << 4) & 0xff);
+	SCB_SHPR(11) |= ((14 << 4) & 0xff);
+	systick_interrupt_enable();
+	systick_counter_enable();
+}
+
+void platform_timeout_set(uint32_t ms)
+{
+	timeout_counter = ms / 100;
+}
+
+bool platform_timeout_is_expired(void)
+{
+	return timeout_counter == 0;
+}
+
+void platform_delay(uint32_t delay)
+{
+	platform_timeout_set(delay);
+	while (platform_timeout_is_expired());
+}
+
+void sys_tick_handler(void)
+{
+	if(running_status)
+		gpio_toggle(LED_PORT, LED_IDLE_RUN);
+
+	if(timeout_counter)
+		timeout_counter--;
+
+	SET_ERROR_STATE(morse_update());
+}
+
diff --git a/src/platforms/stm32/timing.h b/src/platforms/stm32/timing.h
new file mode 100644
index 0000000..0178ff5
--- /dev/null
+++ b/src/platforms/stm32/timing.h
@@ -0,0 +1,27 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015 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 __TIMING_H
+#define __TIMING_H
+
+extern uint8_t running_status;
+
+void platform_timing_init(void);
+
+#endif
+
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index b68b1ab..f891cb0 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -18,6 +18,7 @@ SRC += 	cdcacm.c	\
 	platform.c	\
 	usbuart.c 	\
 	serialno.c	\
+	timing.c	\
 
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index 1b9a76e..cbdae0c 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -27,16 +27,12 @@
 #include "usbuart.h"
 
 #include <libopencm3/stm32/f1/rcc.h>
-#include <libopencm3/cm3/systick.h>
 #include <libopencm3/cm3/scb.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/stm32/f1/adc.h>
 
-uint8_t running_status;
-volatile uint32_t timeout_counter;
-
 jmp_buf fatal_error_jmpbuf;
 
 void platform_init(void)
@@ -72,7 +68,7 @@ void platform_init(void)
 			GPIO_CNF_INPUT_PULL_UPDOWN, NRST_PIN);
 
 	gpio_set_mode(LED_PORT, GPIO_MODE_OUTPUT_2_MHZ,
-			GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
+			GPIO_CNF_OUTPUT_PUSHPULL, LED_IDLE_RUN);
 
 	/* Remap TIM2 TIM2_REMAP[1]
 	 * TIM2_CH1_ETR -> PA15 (TDI, set as output above)
@@ -83,34 +79,11 @@ void platform_init(void)
 	data |=  AFIO_MAPR_TIM2_REMAP_PARTIAL_REMAP1;
 	AFIO_MAPR = data;
 
-	/* Setup heartbeat timer */
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(900000);	/* Interrupt us at 10 Hz */
-	SCB_SHPR(11) &= ~((15 << 4) & 0xff);
-	SCB_SHPR(11) |= ((14 << 4) & 0xff);
-	systick_interrupt_enable();
-	systick_counter_enable();
-
-	usbuart_init();
-
 	SCB_VTOR = 0x2000;	// Relocate interrupt vector table here
 
+	platform_timing_init();
 	cdcacm_init();
-}
-
-void platform_delay(uint32_t delay)
-{
-	timeout_counter = delay;
-	while(timeout_counter);
-}
-
-void sys_tick_handler(void)
-{
-	if(running_status)
-		gpio_toggle(LED_PORT, led_idle_run);
-
-	if(timeout_counter)
-		timeout_counter--;
+	usbuart_init();
 }
 
 const char *platform_target_voltage(void)
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index 6ee4a9c..27cc299 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -26,6 +26,7 @@
 
 #include "gdb_packet.h"
 #include "gpio.h"
+#include "timing.h"
 
 #include <setjmp.h>
 
@@ -69,6 +70,7 @@
 #define SWCLK_PIN	TCK_PIN
 
 #define LED_PORT	GPIOA
+#define LED_IDLE_RUN    GPIO8
 /* Use PC14 for a "dummy" uart led. So we can observere at least with scope*/
 #define LED_PORT_UART	GPIOC
 #define LED_UART	GPIO14
@@ -123,14 +125,11 @@
 
 #define DEBUG(...)
 
-extern uint8_t running_status;
-extern volatile uint32_t timeout_counter;
-
 extern jmp_buf fatal_error_jmpbuf;
 
-#define led_idle_run GPIO8
 #define SET_RUN_STATE(state)	{running_status = (state);}
-#define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, led_idle_run, 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)	{ 		\