common: Added a FIFO type and routines

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

@@ -16,9 +16,9 @@ LDFLAGS = -nostartfiles -lc $(CPU_FLAGS) -nodefaultlibs -T$(LINKER_SCRIPT) -Wl,-
 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/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
-