hellen81 util changes

(cherry picked from commit 85ec5aa8e7)

firmware/util/containers/cyclic_buffer.h

@@ -69,12 +69,16 @@ void cyclic_buffer<T, maxSize>::baseC(int size) {
 
 template<typename T, size_t maxSize>
 void cyclic_buffer<T, maxSize>::add(T value) {
-	elements[currentIndex] = value;
+	// Too lazy to make this thread safe, but at the very least let's never let currentIndex
+	// become invalid.  And yes I did see a crash due to an overrun here.
+	uint16_t idx = currentIndex;
 
-	++currentIndex;
-	if (currentIndex == size) {
-		currentIndex = 0;
+	((T &)elements[idx]) = value;
+
+	if (++idx == size) {
+		idx = 0;
 	}
+	currentIndex = idx;
 
 	++count;
 }

firmware/util/containers/fifo_buffer.h

@@ -18,15 +18,20 @@
 // todo: this is not a thread-safe version!
 template<typename T, size_t maxSize = CB_MAX_SIZE>
 class fifo_buffer : public cyclic_buffer<T, maxSize> {
+	using cyclic_buffer<T, maxSize>::add;
+	using cyclic_buffer<T, maxSize>::getSize;
+	using cyclic_buffer<T, maxSize>::elements;
+	using cyclic_buffer<T, maxSize>::size, cyclic_buffer<T, maxSize>::count;
+
 public:
 	fifo_buffer() : currentIndexRead(0) {
 	}
 
-	void put(T item);
+	virtual bool put(T item);
 	T get();
 	void clear() /*override*/;
 
-	void put(const T *items, int numItems);
+	virtual bool put(const T *items, int numItems);
 	
 	bool isEmpty() const {
 		return getCount() == 0;
@@ -36,28 +41,40 @@ public:
 		return getCount() >= getSize();
 	}
 
+    int getCount() const {
+        return cyclic_buffer<T, maxSize>::getCount();
+    }
+
+    const volatile T* getElements() const {
+        return elements;
+    }
+
 public:
     volatile int currentIndexRead;	// FIFO "tail"
 };
 
 template<typename T, size_t maxSize>
-void fifo_buffer<T, maxSize>::put(T item) {
+bool fifo_buffer<T, maxSize>::put(T item) {
 	// check if full
 	if (!isFull()) {
-		cyclic_buffer::add(item);
+		add(item);
+		return true;
 	}
+	return false;
 }
 
 template<typename T, size_t maxSize>
-void fifo_buffer<T, maxSize>::put(const T *items, int numItems) {
+bool fifo_buffer<T, maxSize>::put(const T *items, int numItems) {
 	for (int i = 0; i < numItems; i++) {
-		put(items[i]);
+		if (!put(items[i]))
+			return false;
 	}
+	return true;
 }
 
 template<typename T, size_t maxSize>
 T fifo_buffer<T, maxSize>::get() {
-	auto ret = elements[currentIndexRead];
+	T &ret = (T &)elements[currentIndexRead];
 	if (!isEmpty()) {
 		currentIndexRead = (currentIndexRead + 1) % size;
 		count--;
@@ -67,9 +84,60 @@ T fifo_buffer<T, maxSize>::get() {
 
 template<typename T, size_t maxSize>
 void fifo_buffer<T, maxSize>::clear() {
-	cyclic_buffer::clear();
+	cyclic_buffer<T, maxSize>::clear();
 	currentIndexRead = 0;
 }
 
+template<typename T, size_t maxSize = CB_MAX_SIZE>
+class fifo_buffer_sync : public fifo_buffer<T, maxSize> {
+public:
+	fifo_buffer_sync() {
+		osalThreadQueueObjectInit(&q_waiting);
+	}
+
+	bool put(T item) override {
+		chSysLock();
+		if (fifo_buffer<T, maxSize>::isFull()) {
+			chSysUnlock();
+			return false;
+		}
+		fifo_buffer<T, maxSize>::put(item);
+		osalThreadDequeueNextI(&q_waiting, MSG_OK);
+		chSysUnlock();
+		return true;
+	}
+
+	bool put(const T *items, int numItems) override {
+		for (int i = 0; i < numItems; i++) {
+			if (!put(items[i]))
+				return false;
+		}
+		return true;
+	}
+
+	bool get(T &item, int timeout) {
+    	chSysLock();
+		while (fifo_buffer<T, maxSize>::isEmpty()) {
+			msg_t msg = osalThreadEnqueueTimeoutS(&q_waiting, timeout);
+			if (msg != MSG_OK) {
+				chSysUnlock();
+				return false;
+			}
+		}
+		item = fifo_buffer<T, maxSize>::get();
+		chSysUnlock();
+		return true;
+    }
+
+	void clear() {
+		chSysLock();
+		fifo_buffer<T, maxSize>::clear();
+		osalThreadDequeueAllI(&q_waiting, MSG_RESET);
+		chSysUnlock();
+	}
+
+protected:
+	threads_queue_t q_waiting;
+};
 
 #endif /* FIFO_BUFFER_H */