refs #45 Add ringbuffer implementation.

Adapted from old control firmware, so written in C. Pretty sure this one works,
however, unlike my previous "new" attempt.

src/filesystem/ringbuffer.cpp

@@ -0,0 +1,91 @@
+#include <filesystem/ringbuffer.hpp>
+
+#include "util/time.hpp"
+#include "ch.hpp"
+#include "hal.h"
+#include <chprintf.h>
+
+static uint16_t overflowCount = 0;
+
+void rb_init(rb_t *buf, std::size_t size, uint8_t *elems)
+{
+  buf->size = size;
+  buf->count = 0;
+  buf->head = 0;
+  buf->elems = elems;
+}
+
+std::size_t rb_add(rb_t *buf, std::size_t num_bytes, uint8_t *input)
+{
+  BaseSequentialStream *chp = (BaseSequentialStream*)&SD4;
+  /* Check if buffer is too full. */
+  if (buf->size - buf->count < num_bytes) {
+    overflowCount++;
+    chprintf(chp, "RB %d %d %d %d ###\r\n", ST2MS(chibios_rt::System::getTime()), overflowCount, num_bytes, buf->count);
+
+    // Drop a few bytes
+    int dropCount = 10000;
+    buf->head = (buf->head + dropCount) % buf->size;
+    buf->count -= dropCount;
+  }
+
+  /* Copy data. */
+  static uint16_t i;
+  static uint32_t tail;
+  tail = (buf->head + buf->count) % buf->size;
+  for (i=0; i<num_bytes; i++) {
+    buf->elems[tail++] = input[i];
+    if (tail == buf->size) tail = 0;
+  }
+
+  buf->count += num_bytes;
+
+  return num_bytes;
+}
+
+std::size_t rb_remove(rb_t *buf, std::size_t num_bytes, uint8_t *output)
+{
+  BaseSequentialStream *chp = (BaseSequentialStream*)&SD4;
+  /* Check if buffer does not contain enough data. */
+  if (buf->count < num_bytes) {
+    num_bytes = buf->count;
+  }
+
+  /* Copy data. */
+  static uint16_t i;
+  for (i=0; i<num_bytes; i++) {
+    output[i] = buf->elems[buf->head++];
+    if (buf->head == buf->size) buf->head = 0;
+  }
+
+  // Crudely calculate average throughput
+  static uint32_t avgBps = 0;
+  static systime_t lastTime = 0;
+  avgBps = 0.01*num_bytes*1000/(ST2MS(chibios_rt::System::getTime()-lastTime)) + 0.99*avgBps;
+  lastTime = chibios_rt::System::getTime();
+  chprintf(chp, "RB<%d> %d %d %d\r\n", ST2MS(chibios_rt::System::getTime()), overflowCount, avgBps, buf->count);
+
+
+  buf->count -= num_bytes;
+
+  return num_bytes;
+}
+
+std::size_t rb_peek(rb_t *buf, std::size_t num_bytes, uint8_t *output)
+{
+  /* Check if buffer does not contain enough data. */
+  if (buf->count < num_bytes) {
+    num_bytes = buf->count;
+  }
+
+  /* Copy data. */
+  static uint16_t i;
+  uint32_t idx = buf->head;
+  for (i=0; i<num_bytes; i++) {
+    output[i] = buf->elems[idx++];
+    if (idx == buf->size) idx = 0;
+  }
+
+  return num_bytes;
+}
+

src/filesystem/ringbuffer.hpp

@@ -0,0 +1,58 @@
+#ifndef RINGBUFFER_HPP_
+#define RINGBUFFER_HPP_
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <array>
+
+typedef struct {
+	uint32_t size;   /* I.e., capacity */
+	uint32_t count;   /* Number of slots filled */
+	uint32_t head;   /* Head */
+	uint8_t *elems;   /* Elements */
+} rb_t;
+
+/*
+ * Initializer
+ *
+ * @param buf Ringbuffer
+ * @param size Size
+ * @param elems Buffer to use to store elements
+ */
+void rb_init(rb_t *buf, std::size_t size, uint8_t *elems);
+
+/*
+ * Adder
+ *
+ * If there is not enough room in buffer to accomodate all data, no data will
+ * be written.
+ *
+ * @param buf Ringbuffer
+ * @param num_bytes Number of bytes to add
+ * @param input Data buffer to add
+ *
+ * @return Number of bytes actually added
+ */
+std::size_t rb_add(rb_t *buf, std::size_t num_bytes, uint8_t *input);
+
+/*
+ * Remover
+ *
+ * @param buf Ringbuffer
+ * @param num_bytes Number of bytes to remove
+ *
+ * @return Number of bytes actually removed
+ */
+std::size_t rb_remove(rb_t *buf, std::size_t num_bytes, uint8_t *output);
+
+/*
+ * Peeker
+ *
+ * @param buf Ringbuffer
+ * @param num_bytes Number of bytes to fetch
+ *
+ * @return Number of bytes actually fetched
+ */
+std::size_t rb_peek(rb_t *buf, std::size_t num_bytes, uint8_t *output);
+
+#endif