2 files changed, 94 insertions, 8 deletions

diff --git a/serial_link/protocol/triple_buffered_object.c b/serial_link/protocol/triple_buffered_object.c
index f5600364f..a82cef5a6 100644
--- a/serial_link/protocol/triple_buffered_object.c
+++ b/serial_link/protocol/triple_buffered_object.c
@@ -24,10 +24,60 @@ SOFTWARE.
 
 #include "protocol/triple_buffered_object.h"
 
+#define GET_READ_INDEX() object->state & 3
+#define GET_WRITE1_INDEX() (object->state >> 2) & 3
+#define GET_WRITE2_INDEX() (object->state >> 4) & 3
+#define GET_FREE_INDEX() (object->state >> 6) & 3
+
+#define SET_READ_INDEX(i) object->state = ((object->state & ~3) | i)
+#define SET_WRITE1_INDEX(i) object->state = ((object->state & ~(3 << 2)) | (i << 2))
+#define SET_WRITE2_INDEX(i) object->state = ((object->state & ~(3 << 4)) | (i << 4))
+#define SET_FREE_INDEX(i) object->state = ((object->state & ~(3 << 6)) | (i << 6))
+
+void triple_buffer_init(triple_buffer_object_t* object) {
+    object->state = 0;
+    SET_WRITE1_INDEX(0);
+    SET_WRITE2_INDEX(0);
+    SET_READ_INDEX(1);
+    SET_FREE_INDEX(2);
+}
+
+static void triple_buffer_begin_read(uint16_t object_size, triple_buffer_object_t* object) {
+    uint8_t newest = GET_WRITE2_INDEX();
+    uint8_t free_index = GET_READ_INDEX();
+    SET_READ_INDEX(newest);
+    SET_FREE_INDEX(free_index);
+}
+
+static void triple_buffer_actual_read(uint16_t object_size, triple_buffer_object_t* object, void* dst) {
+    uint8_t read_index = GET_READ_INDEX();
+    memcpy(dst, object->buffer + object_size*read_index, object_size);
+}
+
+static void triple_buffer_end_read(uint16_t object_size, triple_buffer_object_t* object) {
+}
+
 void triple_buffer_write(uint16_t object_size, triple_buffer_object_t* object, void* src) {
-    memcpy(object->buffer, src, object_size);
+    uint8_t write1_index = GET_WRITE1_INDEX();
+    uint8_t write2_index = GET_WRITE2_INDEX();
+    uint8_t read_index = GET_READ_INDEX();
+    uint8_t free_index = GET_FREE_INDEX();
+
+    if (write2_index == read_index) {
+        // We are reading from the other write index
+        SET_WRITE1_INDEX(free_index);
+        memcpy(object->buffer + object_size * free_index, src, object_size);
+        SET_WRITE2_INDEX(free_index);
+    }
+    else {
+        SET_WRITE1_INDEX(write2_index);
+        memcpy(object->buffer + object_size * write2_index, src, object_size);
+        SET_WRITE2_INDEX(write2_index);
+    }
 }
 
 void triple_buffer_read(uint16_t object_size, triple_buffer_object_t* object, void* dst) {
-    memcpy(dst, object->buffer, object_size);
+    triple_buffer_begin_read(object_size, object);
+    triple_buffer_actual_read(object_size, object, dst);
+    triple_buffer_end_read(object_size, object);
 }
diff --git a/serial_link/tests/triple_buffered_object_tests.c b/serial_link/tests/triple_buffered_object_tests.c
index 66dcefa7f..bf0e3ca67 100644
--- a/serial_link/tests/triple_buffered_object_tests.c
+++ b/serial_link/tests/triple_buffered_object_tests.c
@@ -25,20 +25,56 @@ SOFTWARE.
 #include <cgreen/cgreen.h>
 #include "protocol/triple_buffered_object.c"
 
-Describe(TripleBufferedObject);
-BeforeEach(TripleBufferedObject) {}
-AfterEach(TripleBufferedObject) {}
-
 typedef struct {
     uint8_t state;
     uint32_t buffer[3];
 }test_object_t;
 
-Ensure(TripleBufferedObject, writes_and_and_reads_object) {
-    test_object_t test_object;
+test_object_t test_object;
+
+Describe(TripleBufferedObject);
+BeforeEach(TripleBufferedObject) {
+    triple_buffer_init((triple_buffer_object_t*)&test_object);
+}
+AfterEach(TripleBufferedObject) {}
+
+
+Ensure(TripleBufferedObject, writes_and_reads_object) {
+    uint32_t src = 0x3456ABCC;
+    uint32_t dst;
+    triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
+    triple_buffer_read(4, (triple_buffer_object_t*)&test_object, &dst);
+    assert_that(dst, is_equal_to(src));
+}
+
+Ensure(TripleBufferedObject, writes_and_reads_object_decomposed) {
+    uint32_t src = 0x3456ABCC;
+    uint32_t dst;
+    triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
+    triple_buffer_begin_read(4, (triple_buffer_object_t*)&test_object);
+    triple_buffer_actual_read(4, (triple_buffer_object_t*)&test_object, &dst);
+    triple_buffer_end_read(4, (triple_buffer_object_t*)&test_object);
+    assert_that(dst, is_equal_to(src));
+}
+
+Ensure(TripleBufferedObject, writes_twice_and_reads_object) {
     uint32_t src = 0x3456ABCC;
     uint32_t dst;
     triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
+    src = 0x44778899;
+    triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
     triple_buffer_read(4, (triple_buffer_object_t*)&test_object, &dst);
     assert_that(dst, is_equal_to(src));
 }
+
+Ensure(TripleBufferedObject, performs_another_write_in_the_middle_of_read) {
+    uint32_t src = 1;
+    uint32_t dst;
+    triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
+    src = 2;
+    triple_buffer_begin_read(4, (triple_buffer_object_t*)&test_object);
+    triple_buffer_write(4, (triple_buffer_object_t*)&test_object, &src);
+    triple_buffer_actual_read(4, (triple_buffer_object_t*)&test_object, &dst);
+    triple_buffer_end_read(4, (triple_buffer_object_t*)&test_object);
+    assert_that(dst, is_equal_to(1));
+}