diff --git a/src/platforms/stm32/traceswo.c b/src/platforms/stm32/traceswo.c
index 555f104d..9b1bec57 100644
--- a/src/platforms/stm32/traceswo.c
+++ b/src/platforms/stm32/traceswo.c
@@ -34,18 +34,36 @@
 #include "general.h"
 #include "cdcacm.h"
 #include "traceswo.h"
+#include "platform.h"
+#include "fifo.h"
 
+#include <stdatomic.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/timer.h>
 #include <libopencm3/stm32/rcc.h>
 
+#define FULL_SWO_PACKET (CDCACM_PACKET_SIZE) //maximum is 255! Best is same as usb ep buffer size
+#ifndef NUM_TRACE_PACKETS
+#define NUM_TRACE_PACKETS (128)
+#endif
+#define FIFO_SIZE (NUM_TRACE_PACKETS * FULL_SWO_PACKET)
+
 /* SWO decoding */
 static bool decoding = false;
 
-void traceswo_init(uint32_t swo_chan_bitmask)
+FIFO_STATIC(trace_fifo, FIFO_SIZE);
+static bool active_transfer = false;
+
+void traceswo_init(const uint32_t swo_chan_bitmask)
 {
 	TRACE_TIM_CLK_EN();
 
+	/* Refer to ST doc RM0008 - STM32F10xx Reference Manual.
+	 * Section 14.3.4 - 14.3.6 (General Purpose Timer - Input Capture)
+	 *
+	 * CCR1 captures cycle time, CCR2 captures high time
+	 */
+
 	/* Refer to ST doc RM0008 - STM32F10xx Reference Manual.
 	 * Section 14.3.4 - 14.3.6 (General Purpose Timer - Input Capture)
 	 *
@@ -81,44 +99,38 @@ void traceswo_init(uint32_t swo_chan_bitmask)
 	decoding = (swo_chan_bitmask != 0);
 }
 
-static uint8_t trace_usb_buf[64];
-static uint8_t trace_usb_buf_size;
-
-void trace_buf_push(uint8_t *buf, int len)
+void trace_buf_drain(usbd_device *const dev, const uint8_t ep)
 {
-	if (decoding)
-		traceswo_decode(usbdev, CDCACM_UART_ENDPOINT, buf, len);
-	else if (usbd_ep_write_packet(usbdev, 0x85, buf, len) != len) {
-		if (trace_usb_buf_size + len > 64) {
-			/* Stall if upstream to too slow. */
-			usbd_ep_stall_set(usbdev, 0x85, 1);
-			trace_usb_buf_size = 0;
-			return;
-		}
-		memcpy(trace_usb_buf + trace_usb_buf_size, buf, len);
-		trace_usb_buf_size += len;
-	}
-}
-
-void trace_buf_drain(usbd_device *dev, uint8_t ep)
-{
-	if (!trace_usb_buf_size)
+	static atomic_flag draining = ATOMIC_FLAG_INIT;
+	/* If we are already in this routine then we don't need to come in again */
+	if (atomic_flag_test_and_set(&draining))
 		return;
 
-	if (decoding)
-		traceswo_decode(dev, CDCACM_UART_ENDPOINT, trace_usb_buf, trace_usb_buf_size);
-	else
-		usbd_ep_write_packet(dev, ep, trace_usb_buf, trace_usb_buf_size);
-	trace_usb_buf_size = 0;
-}
+	/* Attempt to write to ep */
+	size_t available = FULL_SWO_PACKET;
+	if (fifo_get_used(&trace_fifo) < FULL_SWO_PACKET)
+		available = fifo_get_used(&trace_fifo);
+
+	if (available) {
+		uint8_t *buffer = fifo_get_pointer(&trace_fifo, available);
+		uint16_t consumed = 0;
+		if (decoding)
+			consumed = traceswo_decode(dev, CDCACM_UART_ENDPOINT, buffer, available);
+		else
+			consumed = usbd_ep_write_packet(dev, ep, buffer, available);
+		fifo_discard(&trace_fifo, consumed);
+	} else {
+		active_transfer = false;
+	}
 
-#define ALLOWED_DUTY_ERROR 5
+	atomic_flag_clear(&draining);
+}
 
 void TRACE_ISR(void)
 {
 	uint16_t sr = TIM_SR(TRACE_TIM);
 	uint16_t duty, cycle;
-	static uint16_t bt;
+	static uint16_t half_period;
 	static uint8_t lastbit;
 	static uint8_t decbuf[17];
 	static uint8_t decbuf_pos;
@@ -135,23 +147,24 @@ void TRACE_ISR(void)
 	cycle = TIM_CCR1(TRACE_TIM);
 	duty = TIM_CCR2(TRACE_TIM);
 
-	/* Reset decoder state if crazy shit happened */
-	if ((bt && (((duty / bt) > 2) || ((duty / bt) == 0))) || (duty == 0))
+	/* Reset decoder state if anything too crazy happened */
+	if (duty == 0 || (half_period && (((duty + half_period / 2) / half_period) > 2 ||
+										 ((duty + half_period / 2) / half_period) == 0)))
 		goto flush_and_reset;
 
+	/* No rising edge caught, cycle not finished yet! */
 	if (!(sr & TIM_SR_CC1IF))
 		notstart = 1;
 
-	if (!bt) {
+	if (!half_period) {
 		if (notstart) {
 			notstart = 0;
 			return;
 		}
 		/* First bit, sync decoder */
-		duty -= ALLOWED_DUTY_ERROR;
-		if (((cycle / duty) != 2) && ((cycle / duty) != 3))
+		if (((cycle + duty / 2) / duty) != 2 && ((cycle + duty / 2) / duty) != 3)
 			return;
-		bt = duty;
+		half_period = duty;
 		lastbit = 1;
 		halfbit = 0;
 		timer_set_period(TRACE_TIM, duty * 6);
@@ -159,20 +172,22 @@ void TRACE_ISR(void)
 		timer_enable_irq(TRACE_TIM, TIM_DIER_UIE);
 	} else {
 		/* If high time is extended we need to flip the bit */
-		if ((duty / bt) > 1) {
+		if (((duty + half_period / 2) / half_period) > 1) {
 			if (!halfbit) /* lost sync somehow */
 				goto flush_and_reset;
 			halfbit = 0;
 			lastbit ^= 1;
-		}
+			half_period = half_period / 2 + duty / 4; //update with EMA IIR filter alpha 0.5 (duty is 2 halfperiods)
+		} else
+			half_period = half_period / 2 + duty / 2; //update with EMA IIR filter alpha 0.5
 		decbuf[decbuf_pos >> 3] |= lastbit << (decbuf_pos & 7);
 		decbuf_pos++;
 	}
 
-	if (!(sr & TIM_SR_CC1IF) || (((cycle - duty) / bt) > 2))
+	if (!(sr & TIM_SR_CC1IF) || ((cycle - duty + half_period / 2) / half_period) > 2)
 		goto flush_and_reset;
 
-	if (((cycle - duty) / bt) > 1) {
+	if (((cycle - duty + half_period / 2) / half_period) > 1) {
 		/* If low time extended we need to pack another bit. */
 		if (halfbit) /* this is a valid stop-bit or we lost sync */
 			goto flush_and_reset;
@@ -188,8 +203,14 @@ void TRACE_ISR(void)
 flush_and_reset:
 	timer_set_period(TRACE_TIM, -1);
 	timer_disable_irq(TRACE_TIM, TIM_DIER_UIE);
-	trace_buf_push(decbuf, decbuf_pos >> 3);
-	bt = 0;
+
+	fifo_write(&trace_fifo, decbuf, decbuf_pos >> 3);
+	if (!active_transfer) {
+		active_transfer = true;
+		trace_buf_drain(usbdev, 0x85);
+	}
+
+	half_period = 0;
 	decbuf_pos = 0;
 	memset(decbuf, 0, sizeof(decbuf));
 }