traceswo: Made use of the new FIFO type to improve stability

src/platforms/96b_carbon/Makefile.inc

@@ -16,6 +16,7 @@ LDFLAGS = -lopencm3_stm32f4 -Wl,--defsym,_stack=0x20006000 \
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	traceswodecode.c	\
 	traceswo.c	\
 	usbuart.c	\

src/platforms/blackpillv2/Makefile.inc

@@ -28,6 +28,7 @@ endif
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	traceswodecode.c	\
 	traceswo.c	\
 	usbuart.c	\

src/platforms/common/fifo.c

@@ -0,0 +1,117 @@
+/*
+ * CircularBuffer.c
+ *
+ *  Created on: 26.01.2018
+ *      Author: Daniel
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "fifo.h"
+
+bool fifo_push(fifo_t *const f, const uint8_t byte)
+{
+	if (f->is_full)
+		return false;
+
+	f->buffer[f->head++] = byte;
+	f->head %= f->size;
+
+	if (f->head == f->tail)
+		f->is_full = true;
+
+	return true;
+}
+
+uint8_t fifo_pop(fifo_t *const f)
+{
+	if (f->head == f->tail && !f->is_full)
+		return 0;
+
+	const uint8_t result = f->buffer[f->tail++];
+	f->tail %= f->size;
+
+	if (f->is_full)
+		f->is_full = false;
+
+	return result;
+}
+
+size_t fifo_write(fifo_t *const f, const uint8_t *data, size_t size)
+{
+	if (size > fifo_get_free(f))
+		size = fifo_get_free(f);
+
+	for (size_t idx = 0; idx < size; ++idx) {
+		if (!fifo_push(f, data[idx]))
+			return idx;
+	}
+
+	return size;
+}
+
+size_t fifo_read(fifo_t *const f, uint8_t *const data, size_t size)
+{
+	if (size > fifo_get_used(f))
+		size = fifo_get_used(f);
+
+	for (size_t idx = 0; idx < size; ++idx)
+		data[idx] = fifo_pop(f);
+
+	return size;
+}
+
+uint8_t *fifo_get_pointer(fifo_t *const f, const size_t size)
+{
+	size_t tailsize = f->size - f->tail;
+
+	if ((size <= tailsize || f->tail <= f->head) && !f->is_full)
+		return f->buffer + f->tail;
+
+	uint8_t *tailstart = f->buffer + f->tail;
+
+	uint8_t *overlap = NULL;
+	size_t overlapsize = 0;
+
+	if (tailsize + f->head > f->tail) // used size overlapping tail? save it!
+	{
+		overlapsize = tailsize + f->head - f->tail;
+		overlap = malloc(overlapsize);
+		memcpy(overlap, &f->buffer[f->tail], overlapsize);
+
+		// % prevents tailstart pointing outside array
+		tailstart = f->buffer + ((f->tail + overlapsize) % f->size);
+		tailsize -= overlapsize;
+	}
+
+	memmove(f->buffer + overlapsize + tailsize, f->buffer, f->head);
+	memcpy(f->buffer + overlapsize, tailstart, tailsize);
+
+	if (overlap) {
+		memcpy(f->buffer, overlap, overlapsize);
+		free(overlap);
+	}
+
+	f->tail = 0;
+	f->head = (overlapsize + tailsize + f->head) % f->size;
+
+	return f->buffer;
+}
+
+size_t fifo_discard(fifo_t *const f, size_t amount)
+{
+	if (amount > fifo_get_used(f))
+		amount = fifo_get_used(f);
+
+	if (amount && f->is_full)
+		f->is_full = false;
+	f->tail = (f->tail + amount) % f->size;
+	return amount;
+}
+
+void fifo_reset(fifo_t *const f)
+{
+	f->head = 0;
+	f->tail = 0;
+	f->is_full = false;
+}

src/platforms/common/fifo.h

@@ -0,0 +1,49 @@
+/*
+ * CircularBuffer.h
+ *
+ *  Created on: 26.01.2018
+ *      Author: Daniel
+ */
+
+#ifndef FIFO_H_
+#define FIFO_H_
+
+#include <stdint.h>
+#include <stddef.h>
+#include <stdbool.h>
+
+#define FIFO_STATIC(name, fifo_size) \
+	uint8_t name##_data[(fifo_size)];  \
+	fifo_t name = {.buffer = name##_data, .head = 0, .tail = 0, .size = (fifo_size), .is_full = 0}
+
+typedef struct fifo_s {
+	uint8_t *buffer;
+	size_t head;
+	size_t tail;
+	const size_t size;
+	bool is_full;
+} fifo_t;
+
+bool fifo_push(fifo_t *f, uint8_t byte);
+uint8_t fifo_pop(fifo_t *f);
+size_t fifo_write(fifo_t *f, const uint8_t *data, size_t size);
+size_t fifo_read(fifo_t *f, uint8_t *data, size_t size);
+uint8_t *fifo_get_pointer(fifo_t *f, size_t size);
+size_t fifo_discard(fifo_t *f, size_t del);
+void fifo_reset(fifo_t *f);
+
+static inline size_t fifo_get_used(fifo_t *f)
+{
+	if (f->is_full)
+		return f->size;
+	if (f->tail > f->head)
+		return f->size + f->head - f->tail;
+	return f->head - f->tail;
+}
+
+static inline size_t fifo_get_free(fifo_t *f)
+{
+	return f->size - fifo_get_used(f);
+}
+
+#endif /* FIFO_H_ */

src/platforms/f072/Makefile.inc

@@ -14,6 +14,7 @@ LDFLAGS = --specs=nano.specs -lopencm3_stm32f0 \
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	traceswodecode.c\
 	traceswo.c	\
 	usbuart.c	\

src/platforms/f3/Makefile.inc

@@ -16,6 +16,7 @@ LDFLAGS =  --specs=nano.specs -lopencm3_stm32f3 \
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	traceswodecode.c\
 	traceswo.c	\
 	usbuart.c	\

src/platforms/f4discovery/Makefile.inc

@@ -29,6 +29,7 @@ VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
 	traceswodecode.c	\
+	fifo.c      \
 	traceswo.c	\
 	usbuart.c	\
 	serialno.c	\

src/platforms/hydrabus/Makefile.inc

@@ -17,6 +17,7 @@ VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
 	traceswodecode.c	\
+	fifo.c      \
 	traceswo.c	\
 	usbuart.c	\
 	serialno.c	\

src/platforms/launchpad-icdi/Makefile.inc

@@ -12,13 +12,13 @@ CFLAGS += -DDFU_SERIAL_LENGTH=9
 LINKER_SCRIPT="platforms/tm4c/tm4c.ld"
 LDFLAGS = -nostartfiles -lc $(CPU_FLAGS) -nodefaultlibs -T$(LINKER_SCRIPT) -Wl,--gc-sections \
 	-L../libopencm3/lib -lopencm3_lm4f -lnosys -lm -lgcc
-	
+
 VPATH += platforms/tm4c
 
 SRC +=	cdcacm.c	\
+	fifo.c      \
 	usbuart.c	\
 	timing.c        \
 	traceswo.o
 
 all: blackmagic.bin
-

src/platforms/native/Makefile.inc

@@ -22,6 +22,7 @@ VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
 	traceswodecode.c	\
+	fifo.c      \
 	traceswo.c	\
 	usbuart.c	\
 	serialno.c	\

src/platforms/stlink/Makefile.inc

@@ -32,6 +32,7 @@ endif
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	usbuart.c 	\
 	serialno.c	\
 	timing.c	\

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;
 
-	if(!(sr & TIM_SR_CC1IF)) notstart = 1;
+	/* 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));
 }

src/platforms/swlink/Makefile.inc

@@ -20,6 +20,7 @@ endif
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
+	fifo.c      \
 	usbuart.c 	\
 	serialno.c	\
 	timing.c	\
@@ -36,4 +37,3 @@ blackmagic_dfu.elf: usbdfu.o dfucore.o dfu_f1.o platform_common.o serialno.o
 
 host_clean:
 	-$(Q)$(RM) blackmagic.bin blackmagic_dfu blackmagic_dfu.bin blackmagic_dfu.hex
-