13 files changed, 680 insertions, 636 deletions

diff --git a/quantum/serial_link/protocol/byte_stuffer.c b/quantum/serial_link/protocol/byte_stuffer.c
index fb4c45a8dc..2c87d64c29 100644
--- a/quantum/serial_link/protocol/byte_stuffer.c
+++ b/quantum/serial_link/protocol/byte_stuffer.c
@@ -31,9 +31,6 @@ SOFTWARE.
 // https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
 // http://www.stuartcheshire.org/papers/COBSforToN.pdf
 
-#define MAX_FRAME_SIZE 1024
-#define NUM_LINKS 2
-
 typedef struct byte_stuffer_state {
     uint16_t next_zero;
     uint16_t data_pos;
diff --git a/quantum/serial_link/protocol/byte_stuffer.h b/quantum/serial_link/protocol/byte_stuffer.h
index 2cc88beb42..97e8968564 100644
--- a/quantum/serial_link/protocol/byte_stuffer.h
+++ b/quantum/serial_link/protocol/byte_stuffer.h
@@ -27,6 +27,9 @@ SOFTWARE.
 
 #include <stdint.h>
 
+#define MAX_FRAME_SIZE 1024
+#define NUM_LINKS 2
+
 void init_byte_stuffer(void);
 void byte_stuffer_recv_byte(uint8_t link, uint8_t data);
 void byte_stuffer_send_frame(uint8_t link, uint8_t* data, uint16_t size);
diff --git a/quantum/serial_link/protocol/transport.c b/quantum/serial_link/protocol/transport.c
index f418d11ceb..ff795fe201 100644
--- a/quantum/serial_link/protocol/transport.c
+++ b/quantum/serial_link/protocol/transport.c
@@ -31,6 +31,10 @@ SOFTWARE.
 static remote_object_t* remote_objects[MAX_REMOTE_OBJECTS];
 static uint32_t num_remote_objects = 0;
 
+void reinitialize_serial_link_transport(void) {
+    num_remote_objects = 0;
+}
+
 void add_remote_objects(remote_object_t** _remote_objects, uint32_t _num_remote_objects) {
     unsigned int i;
     for(i=0;i<_num_remote_objects;i++) {
diff --git a/quantum/serial_link/protocol/transport.h b/quantum/serial_link/protocol/transport.h
index 9a052d8809..2c5d890b21 100644
--- a/quantum/serial_link/protocol/transport.h
+++ b/quantum/serial_link/protocol/transport.h
@@ -82,7 +82,7 @@ typedef struct { \
         remote_object_t* obj = (remote_object_t*)&remote_object_##name; \
         uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size);\
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define MASTER_TO_SINGLE_SLAVE_OBJECT(name, type) \
@@ -112,7 +112,7 @@ typedef struct { \
         remote_object_t* obj = (remote_object_t*)&remote_object_##name; \
         uint8_t* start = obj->buffer + NUM_SLAVES * LOCAL_OBJECT_SIZE(obj->object_size);\
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define SLAVE_TO_MASTER_OBJECT(name, type) \
@@ -139,12 +139,13 @@ typedef struct { \
         uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size);\
         start+=slave * REMOTE_OBJECT_SIZE(obj->object_size); \
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define REMOTE_OBJECT(name) (remote_object_t*)&remote_object_##name
 
 void add_remote_objects(remote_object_t** remote_objects, uint32_t num_remote_objects);
+void reinitialize_serial_link_transport(void);
 void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size);
 void update_transport(void);
 
diff --git a/quantum/serial_link/tests/byte_stuffer_tests.c b/quantum/serial_link/tests/byte_stuffer_tests.cpp
index 64b170e8c1..ff49d727bb 100644
--- a/quantum/serial_link/tests/byte_stuffer_tests.c
+++ b/quantum/serial_link/tests/byte_stuffer_tests.cpp
@@ -22,70 +22,90 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 
-#include <cgreen/cgreen.h>
-#include <cgreen/mocks.h>
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+#include <vector>
+#include <algorithm>
+extern "C" {
 #include "serial_link/protocol/byte_stuffer.h"
-#include "serial_link/protocol/byte_stuffer.c"
 #include "serial_link/protocol/frame_validator.h"
 #include "serial_link/protocol/physical.h"
+}
 
-static uint8_t sent_data[MAX_FRAME_SIZE*2];
-static uint16_t sent_data_size;
+using testing::_;
+using testing::ElementsAreArray;
+using testing::Args;
 
-Describe(ByteStuffer);
-BeforeEach(ByteStuffer) {
-    init_byte_stuffer();
-    sent_data_size = 0;
-}
-AfterEach(ByteStuffer) {}
+class ByteStuffer : public ::testing::Test{
+public:
+    ByteStuffer() {
+        Instance = this;
+        init_byte_stuffer();
+    }
 
-void validator_recv_frame(uint8_t link, uint8_t* data, uint16_t size) {
-    mock(data, size);
-}
+    ~ByteStuffer() {
+        Instance = nullptr;
+    }
+
+    MOCK_METHOD3(validator_recv_frame, void (uint8_t link, uint8_t* data, uint16_t size));
+
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        std::copy(data, data + size, std::back_inserter(sent_data));
+    }
+    std::vector<uint8_t> sent_data;
 
-void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
-    memcpy(sent_data + sent_data_size, data, size);
-    sent_data_size += size;
+    static ByteStuffer* Instance;
+};
+
+ByteStuffer* ByteStuffer::Instance = nullptr;
+
+extern "C" {
+    void validator_recv_frame(uint8_t link, uint8_t* data, uint16_t size) {
+        ByteStuffer::Instance->validator_recv_frame(link, data, size);
+    }
+
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        ByteStuffer::Instance->send_data(link, data, size);
+    }
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_zero_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_zero_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_FF_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_FF_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0xFF);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_random_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_random_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0x4A);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_zero_length_frame) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_zero_length_frame) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_single_byte_valid_frame) {
+TEST_F(ByteStuffer, receives_single_byte_valid_frame) {
     uint8_t expected[] = {0x37};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 0x37);
     byte_stuffer_recv_byte(0, 0);
 }
-
-Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
+TEST_F(ByteStuffer, receives_three_bytes_valid_frame) {
     uint8_t expected[] = {0x37, 0x99, 0xFF};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(3)),
-        when(data, is_equal_to_contents_of(expected, 3))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 4);
     byte_stuffer_recv_byte(0, 0x37);
     byte_stuffer_recv_byte(0, 0x99);
@@ -93,23 +113,19 @@ Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_single_zero_valid_frame) {
+TEST_F(ByteStuffer, receives_single_zero_valid_frame) {
     uint8_t expected[] = {0};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
+TEST_F(ByteStuffer, receives_valid_frame_with_zeroes) {
     uint8_t expected[] = {5, 0, 3, 0};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(expected, 4))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 5);
     byte_stuffer_recv_byte(0, 2);
@@ -118,17 +134,14 @@ Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_two_valid_frames) {
+
+TEST_F(ByteStuffer, receives_two_valid_frames) {
     uint8_t expected1[] = {5, 0};
     uint8_t expected2[] = {3};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected1, 2))
-    );
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected2, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected1)));
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected2)));
     byte_stuffer_recv_byte(1, 2);
     byte_stuffer_recv_byte(1, 5);
     byte_stuffer_recv_byte(1, 1);
@@ -138,12 +151,10 @@ Ensure(ByteStuffer, receives_two_valid_frames) {
     byte_stuffer_recv_byte(1, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
+TEST_F(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
     uint8_t expected[] = {5, 7};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected, 2))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(1, 3);
     byte_stuffer_recv_byte(1, 1);
     byte_stuffer_recv_byte(1, 0);
@@ -153,12 +164,10 @@ Ensure(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
     byte_stuffer_recv_byte(1, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
+TEST_F(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
     uint8_t expected[] = {5, 7};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected, 2))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 9);
     byte_stuffer_recv_byte(0, 4); // This should have been zero
@@ -169,16 +178,14 @@ Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_of_frame) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_of_frame) {
     uint8_t expected[254];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
-    expect(validator_recv_frame,
-        when(size, is_equal_to(254)),
-        when(data, is_equal_to_contents_of(expected, 254))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -186,17 +193,15 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_non_zero) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_non_zero) {
     uint8_t expected[255];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
     expected[254] = 7;
-    expect(validator_recv_frame,
-        when(size, is_equal_to(255)),
-        when(data, is_equal_to_contents_of(expected, 255))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -206,17 +211,15 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_zero) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_zero) {
     uint8_t expected[255];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
     expected[254] = 0;
-    expect(validator_recv_frame,
-        when(size, is_equal_to(255)),
-        when(data, is_equal_to_contents_of(expected, 255))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -226,7 +229,7 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
+TEST_F(ByteStuffer, receives_two_long_frames_and_some_more) {
     uint8_t expected[515];
     int i;
     int j;
@@ -238,10 +241,8 @@ Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
     for (i=0;i<7;i++) {
         expected[254*2+i] = i + 1;
     }
-    expect(validator_recv_frame,
-        when(size, is_equal_to(515)),
-        when(data, is_equal_to_contents_of(expected, 510))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -261,12 +262,10 @@ Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
+TEST_F(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
     uint8_t expected[MAX_FRAME_SIZE] = {};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(MAX_FRAME_SIZE)),
-        when(data, is_equal_to_contents_of(expected, MAX_FRAME_SIZE))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -275,9 +274,10 @@ Ensure(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
+TEST_F(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
     uint8_t expected[1] = {0};
-    never_expect(validator_recv_frame);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -287,12 +287,10 @@ Ensure(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
+TEST_F(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
     uint8_t expected[1] = {1};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -303,76 +301,68 @@ Ensure(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, does_nothing_when_sending_zero_size_frame) {
-    assert_that(sent_data_size, is_equal_to(0));
+TEST_F(ByteStuffer, does_nothing_when_sending_zero_size_frame) {
+    EXPECT_EQ(sent_data.size(), 0);
     byte_stuffer_send_frame(0, NULL, 0);
 }
 
-Ensure(ByteStuffer, send_one_byte_frame) {
+TEST_F(ByteStuffer, send_one_byte_frame) {
     uint8_t data[] = {5};
     byte_stuffer_send_frame(1, data, 1);
     uint8_t expected[] = {2, 5, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame) {
+TEST_F(ByteStuffer, sends_two_byte_frame) {
     uint8_t data[] = {5, 0x77};
     byte_stuffer_send_frame(0, data, 2);
     uint8_t expected[] = {3, 5, 0x77, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_one_byte_frame_with_zero) {
+TEST_F(ByteStuffer, sends_one_byte_frame_with_zero) {
     uint8_t data[] = {0};
     byte_stuffer_send_frame(0, data, 1);
     uint8_t expected[] = {1, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame_starting_with_zero) {
+TEST_F(ByteStuffer, sends_two_byte_frame_starting_with_zero) {
     uint8_t data[] = {0, 9};
     byte_stuffer_send_frame(1, data, 2);
     uint8_t expected[] = {1, 2, 9, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame_starting_with_non_zero) {
+TEST_F(ByteStuffer, sends_two_byte_frame_starting_with_non_zero) {
     uint8_t data[] = {9, 0};
     byte_stuffer_send_frame(1, data, 2);
     uint8_t expected[] = {2, 9, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_zero_in_the_middle) {
+TEST_F(ByteStuffer, sends_three_byte_frame_zero_in_the_middle) {
     uint8_t data[] = {9, 0, 0x68};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {2, 9, 2, 0x68, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_data_in_the_middle) {
+TEST_F(ByteStuffer, sends_three_byte_frame_data_in_the_middle) {
     uint8_t data[] = {0, 0x55, 0};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {1, 2, 0x55, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_with_all_zeroes) {
+TEST_F(ByteStuffer, sends_three_byte_frame_with_all_zeroes) {
     uint8_t data[] = {0, 0, 0};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {1, 1, 1, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_254_non_zeroes) {
+TEST_F(ByteStuffer, sends_frame_with_254_non_zeroes) {
     uint8_t data[254];
     int i;
     for(i=0;i<254;i++) {
@@ -385,11 +375,10 @@ Ensure(ByteStuffer, sends_frame_with_254_non_zeroes) {
         expected[i] = i;
     }
     expected[255] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_255_non_zeroes) {
+TEST_F(ByteStuffer, sends_frame_with_255_non_zeroes) {
     uint8_t data[255];
     int i;
     for(i=0;i<255;i++) {
@@ -404,17 +393,16 @@ Ensure(ByteStuffer, sends_frame_with_255_non_zeroes) {
     expected[255] = 2;
     expected[256] = 255;
     expected[257] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
+TEST_F(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
     uint8_t data[255];
     int i;
     for(i=0;i<254;i++) {
         data[i] = i + 1;
     }
-    data[255] = 0;
+    data[254] = 0;
     byte_stuffer_send_frame(0, data, 255);
     uint8_t expected[258];
     expected[0] = 0xFF;
@@ -424,53 +412,46 @@ Ensure(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
     expected[255] = 1;
     expected[256] = 1;
     expected[257] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_small_packet) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_small_packet) {
     uint8_t original_data[] = { 1, 2, 3};
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
     int i;
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_small_packet_with_zeros) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_small_packet_with_zeros) {
     uint8_t original_data[] = { 1, 0, 3, 0, 0, 9};
     byte_stuffer_send_frame(1, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
     int i;
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(0, sent_data[i]);
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes) {
     uint8_t original_data[254];
     int i;
     for(i=0;i<254;i++) {
         original_data[i] = i + 1;
     }
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
     uint8_t original_data[256];
     int i;
     for(i=0;i<254;i++) {
@@ -479,16 +460,14 @@ Ensure(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
     original_data[254] = 22;
     original_data[255] = 23;
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
     uint8_t original_data[255];
     int i;
     for(i=0;i<254;i++) {
@@ -496,11 +475,9 @@ Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
     }
     original_data[254] = 0;
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
diff --git a/quantum/serial_link/tests/frame_router_tests.c b/quantum/serial_link/tests/frame_router_tests.c
deleted file mode 100644
index 6c806fa939..0000000000
--- a/quantum/serial_link/tests/frame_router_tests.c
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
-The MIT License (MIT)
-
-Copyright (c) 2016 Fred Sundvik
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#include <cgreen/cgreen.h>
-#include <cgreen/mocks.h>
-#include "serial_link/protocol/byte_stuffer.c"
-#include "serial_link/protocol/frame_validator.c"
-#include "serial_link/protocol/frame_router.c"
-#include "serial_link/protocol/transport.h"
-
-static uint8_t received_data[256];
-static uint16_t received_data_size;
-
-typedef struct {
-    uint8_t sent_data[256];
-    uint16_t sent_data_size;
-} receive_buffer_t;
-
-typedef struct {
-    receive_buffer_t send_buffers[2];
-} router_buffer_t;
-
-router_buffer_t router_buffers[8];
-
-router_buffer_t* current_router_buffer;
-
-
-Describe(FrameRouter);
-BeforeEach(FrameRouter) {
-    init_byte_stuffer();
-    memset(router_buffers, 0, sizeof(router_buffers));
-    current_router_buffer = 0;
-}
-AfterEach(FrameRouter) {}
-
-typedef struct {
-    uint32_t data;
-    uint8_t extra[16];
-} frame_buffer_t;
-
-
-void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
-    receive_buffer_t* buffer = &current_router_buffer->send_buffers[link];
-    memcpy(buffer->sent_data + buffer->sent_data_size, data, size);
-    buffer->sent_data_size += size;
-}
-
-static void receive_data(uint8_t link, uint8_t* data, uint16_t size) {
-    int i;
-    for(i=0;i<size;i++) {
-        byte_stuffer_recv_byte(link, data[i]);
-    }
-}
-
-static void activate_router(uint8_t num) {
-    current_router_buffer = router_buffers + num;
-    router_set_master(num==0);
-}
-
-static void simulate_transport(uint8_t from, uint8_t to) {
-   activate_router(to);
-   if (from > to) {
-       receive_data(DOWN_LINK,
-               router_buffers[from].send_buffers[UP_LINK].sent_data,
-               router_buffers[from].send_buffers[UP_LINK].sent_data_size);
-   }
-   else if(to > from) {
-       receive_data(UP_LINK,
-               router_buffers[from].send_buffers[DOWN_LINK].sent_data,
-               router_buffers[from].send_buffers[DOWN_LINK].sent_data_size);
-   }
-}
-
-void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) {
-    mock(from, data, size);
-}
-
-
-Ensure(FrameRouter, master_broadcast_is_received_by_everyone) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(0);
-    router_send_frame(0xFF, (uint8_t*)&data, 4);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(0, 1);
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 2);
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, master_send_is_received_by_targets) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(0);
-    router_send_frame((1 << 1) | (1 << 2), (uint8_t*)&data, 4);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    simulate_transport(0, 1);
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 2);
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(2, 3);
-    assert_that(router_buffers[3].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[3].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, first_link_sends_to_master) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(1);
-    router_send_frame(0, (uint8_t*)&data, 4);
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(1)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 0);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, second_link_sends_to_master) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(2);
-    router_send_frame(0, (uint8_t*)&data, 4);
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    simulate_transport(2, 1);
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(2)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 0);
-    assert_that(router_buffe