Merge branch 'master'

Conflicts:
	keyboards/planck/keymaps/bone2planck/keymap.c

29 files changed, 1972 insertions, 1175 deletions

diff --git a/quantum/audio/song_list.h b/quantum/audio/song_list.h
index fc6fcdeef..8022ca672 100644
--- a/quantum/audio/song_list.h
+++ b/quantum/audio/song_list.h
@@ -28,6 +28,14 @@
         Q__NOTE(_E4), Q__NOTE(_C4), \
         Q__NOTE(_E4),
 
+/* Requires: PLAY_NOTE_ARRAY(..., ..., STACCATO); */
+#define IN_LIKE_FLINT \
+    E__NOTE(_AS4), E__NOTE(_AS4), QD_NOTE(_B4),  \
+    E__NOTE(_AS4), E__NOTE(_B4),  QD_NOTE(_CS4), \
+    E__NOTE(_B4),  E__NOTE(_CS4), QD_NOTE(_DS4), \
+    E__NOTE(_CS4), E__NOTE(_B4),  QD_NOTE(_AS4), \
+    E__NOTE(_AS4), E__NOTE(_AS4), QD_NOTE(_B4),
+
 #define GOODBYE_SOUND \
     E__NOTE(_E7),     \
     E__NOTE(_A6),     \
diff --git a/quantum/config_common.h b/quantum/config_common.h
index 09a4fe701..8ed5f4a10 100644
--- a/quantum/config_common.h
+++ b/quantum/config_common.h
@@ -45,6 +45,15 @@
 #define F5 0xF5
 #define F6 0xF6
 #define F7 0xF7
+#define A0 0x00
+#define A1 0x01
+#define A2 0x02
+#define A3 0x03
+#define A4 0x04
+#define A5 0x05
+#define A6 0x06
+#define A7 0x07
+
 
 /* USART configuration */
 #ifdef BLUETOOTH_ENABLE
diff --git a/quantum/dynamic_macro.h b/quantum/dynamic_macro.h
new file mode 100644
index 000000000..a3ad61bc7
--- /dev/null
+++ b/quantum/dynamic_macro.h
@@ -0,0 +1,226 @@
+/* Author: Wojciech Siewierski < wojciech dot siewierski at onet dot pl > */
+#ifndef DYNAMIC_MACROS_H
+#define DYNAMIC_MACROS_H
+
+#include "action_layer.h"
+
+#ifndef DYNAMIC_MACRO_SIZE
+/* May be overridden with a custom value. Be aware that the effective
+ * macro length is half of this value: each keypress is recorded twice
+ * because of the down-event and up-event. This is not a bug, it's the
+ * intended behavior. */
+#define DYNAMIC_MACRO_SIZE 256
+#endif
+
+/* DYNAMIC_MACRO_RANGE must be set as the last element of user's
+ * "planck_keycodes" enum prior to including this header. This allows
+ * us to 'extend' it.
+ */
+enum dynamic_macro_keycodes {
+    DYN_REC_START1 = DYNAMIC_MACRO_RANGE,
+    DYN_REC_START2,
+    DYN_MACRO_PLAY1,
+    DYN_MACRO_PLAY2,
+};
+
+/* Blink the LEDs to notify the user about some event. */
+void dynamic_macro_led_blink(void)
+{
+    backlight_toggle();
+    _delay_ms(100);
+    backlight_toggle();
+}
+
+/**
+ * Start recording of the dynamic macro.
+ *
+ * @param[out] macro_pointer The new macro buffer iterator.
+ * @param[in]  macro_buffer  The macro buffer used to initialize macro_pointer.
+ */
+void dynamic_macro_record_start(
+    keyrecord_t **macro_pointer, keyrecord_t *macro_buffer)
+{
+    dynamic_macro_led_blink();
+
+    clear_keyboard();
+    layer_clear();
+    *macro_pointer = macro_buffer;
+}
+
+/**
+ * Play the dynamic macro.
+ *
+ * @param macro_buffer[in] The beginning of the macro buffer being played.
+ * @param macro_end[in]    The element after the last macro buffer element.
+ * @param direction[in]    Either +1 or -1, which way to iterate the buffer.
+ */
+void dynamic_macro_play(
+    keyrecord_t *macro_buffer, keyrecord_t *macro_end, int8_t direction)
+{
+    uint32_t saved_layer_state = layer_state;
+
+    clear_keyboard();
+    layer_clear();
+
+    while (macro_buffer != macro_end) {
+        process_record(macro_buffer);
+        macro_buffer += direction;
+    }
+
+    clear_keyboard();
+
+    layer_state = saved_layer_state;
+}
+
+/**
+ * Record a single key in a dynamic macro.
+ *
+ * @param macro_pointer[in,out] The current buffer position.
+ * @param macro_end2[in] The end of the other macro which shouldn't be overwritten.
+ * @param direction[in]  Either +1 or -1, which way to iterate the buffer.
+ * @param record[in]     The current keypress.
+ */
+void dynamic_macro_record_key(
+    keyrecord_t **macro_pointer,
+    keyrecord_t *macro_end2,
+    int8_t direction,
+    keyrecord_t *record)
+{
+    if (*macro_pointer + direction != macro_end2) {
+        **macro_pointer = *record;
+        *macro_pointer += direction;
+    } else {
+        /* Notify about the end of buffer. The blinks are paired
+         * because they should happen on both down and up events. */
+        backlight_toggle();
+    }
+}
+
+/**
+ * End recording of the dynamic macro. Essentially just update the
+ * pointer to the end of the macro.
+ */
+void dynamic_macro_record_end(keyrecord_t *macro_pointer, keyrecord_t **macro_end)
+{
+    dynamic_macro_led_blink();
+
+    *macro_end = macro_pointer;
+}
+
+/* Handle the key events related to the dynamic macros. Should be
+ * called from process_record_user() like this:
+ *
+ *   bool process_record_user(uint16_t keycode, keyrecord_t *record) {
+ *       if (!process_record_dynamic_macro(keycode, record)) {
+ *           return false;
+ *       }
+ *       <...THE REST OF THE FUNCTION...>
+ *   }
+ */
+bool process_record_dynamic_macro(uint16_t keycode, keyrecord_t *record)
+{
+    /* Both macros use the same buffer but read/write on different
+     * ends of it.
+     *
+     * Macro1 is written left-to-right starting from the beginning of
+     * the buffer.
+     *
+     * Macro2 is written right-to-left starting from the end of the
+     * buffer.
+     *
+     * &macro_buffer   macro_end
+     *  v                   v
+     * +------------------------------------------------------------+
+     * |>>>>>> MACRO1 >>>>>>|    |<<<<<<<<<<<<< MACRO2 <<<<<<<<<<<<<|
+     * +------------------------------------------------------------+
+     *                           ^                                 ^
+     *                         r_macro_end                  r_macro_buffer
+     *
+     * During the recording when one macro encounters the end of the
+     * other macro, the recording is stopped. Apart from this, there
+     * are no arbitrary limits for the macros' length in relation to
+     * each other: for example one can either have two medium sized
+     * macros or one long macro and one short macro. Or even one empty
+     * and one using the whole buffer.
+     */
+    static keyrecord_t macro_buffer[DYNAMIC_MACRO_SIZE];
+
+    /* Pointer to the first buffer element after the first macro.
+     * Initially points to the very beginning of the buffer since the
+     * macro is empty. */
+    static keyrecord_t *macro_end = macro_buffer;
+
+    /* The other end of the macro buffer. Serves as the beginning of
+     * the second macro. */
+    static keyrecord_t *const r_macro_buffer = macro_buffer + DYNAMIC_MACRO_SIZE - 1;
+
+    /* Like macro_end but for the second macro. */
+    static keyrecord_t *r_macro_end = r_macro_buffer;
+
+    /* A persistent pointer to the current macro position (iterator)
+     * used during the recording. */
+    static keyrecord_t *macro_pointer = NULL;
+
+    /* 0   - no macro is being recorded right now
+     * 1,2 - either macro 1 or 2 is being recorded */
+    static uint8_t macro_id = 0;
+
+    if (macro_id == 0) {
+        /* No macro recording in progress. */
+        if (!record->event.pressed) {
+            switch (keycode) {
+            case DYN_REC_START1:
+                dynamic_macro_record_start(&macro_pointer, macro_buffer);
+                macro_id = 1;
+                return false;
+            case DYN_REC_START2:
+                dynamic_macro_record_start(&macro_pointer, r_macro_buffer);
+                macro_id = 2;
+                return false;
+            case DYN_MACRO_PLAY1:
+                dynamic_macro_play(macro_buffer, macro_end, +1);
+                return false;
+            case DYN_MACRO_PLAY2:
+                dynamic_macro_play(r_macro_buffer, r_macro_end, -1);
+                return false;
+            }
+        }
+    } else {
+        /* A macro is being recorded right now. */
+        switch (keycode) {
+        case MO(_DYN):
+            /* Use the layer key used to access the macro recording as
+             * a stop button. */
+            if (record->event.pressed) { /* Ignore the initial release
+                                          * just after the recoding
+                                          * starts. */
+                switch (macro_id) {
+                case 1:
+                    dynamic_macro_record_end(macro_pointer, &macro_end);
+                    break;
+                case 2:
+                    dynamic_macro_record_end(macro_pointer, &r_macro_end);
+                    break;
+                }
+                macro_id = 0;
+            }
+            return false;
+        default:
+            /* Store the key in the macro buffer and process it normally. */
+            switch (macro_id) {
+            case 1:
+                dynamic_macro_record_key(&macro_pointer, r_macro_end, +1, record);
+                break;
+            case 2:
+                dynamic_macro_record_key(&macro_pointer, macro_end, -1, record);
+                break;
+            }
+            return true;
+            break;
+        }
+    }
+
+    return true;
+}
+
+#endif
diff --git a/quantum/keymap.h b/quantum/keymap.h
index a15865183..f2d94d75c 100644
--- a/quantum/keymap.h
+++ b/quantum/keymap.h
@@ -156,16 +156,16 @@ enum quantum_keycodes {
     BL_INC,
     BL_TOGG,
     BL_STEP,
-        
-        // RGB functionality
-        RGB_TOG,
-        RGB_MOD,
-        RGB_HUI,
-        RGB_HUD,
-        RGB_SAI,
-        RGB_SAD,
-        RGB_VAI,
-        RGB_VAD,
+
+    // RGB functionality
+    RGB_TOG,
+    RGB_MOD,
+    RGB_HUI,
+    RGB_HUD,
+    RGB_SAI,
+    RGB_SAD,
+    RGB_VAI,
+    RGB_VAD,
 
     // Left shift, open paren
     KC_LSPO,
@@ -309,7 +309,7 @@ enum quantum_keycodes {
 #define OSL(layer) (layer | QK_ONE_SHOT_LAYER)
 
 // One-shot mod
-#define OSM(layer) (layer | QK_ONE_SHOT_MOD)
+#define OSM(mod) (mod | QK_ONE_SHOT_MOD)
 
 // M-od, T-ap - 256 keycode max
 #define MT(mod, kc) (kc | QK_MOD_TAP | ((mod & 0xF) << 8))
diff --git a/quantum/keymap_extras/keymap_russian.h b/quantum/keymap_extras/keymap_russian.h
new file mode 100644
index 000000000..237e9abde
--- /dev/null
+++ b/quantum/keymap_extras/keymap_russian.h
@@ -0,0 +1,77 @@
+#ifndef KEYMAP_RUSSIAN_H
+#define KEYMAP_RUSSIAN_H
+
+#include "keymap.h"
+
+// Normal Chracters      // reg   SHIFT
+#define RU_A     KC_F    // а and А
+#define RU_BE    KC_COMM // б and Б
+#define RU_VE    KC_D    // в and В
+#define RU_GHE   KC_U    // г and Г
+#define RU_DE    KC_L    // д and Д
+#define RU_IE    KC_T    // е and Е
+#define RU_IO    KC_GRV  // ё and Ё
+#define RU_ZHE   KC_SCLN // ж and Ж
+#define RU_ZE    KC_P    // з and З
+#define RU_I     KC_B    // и and И
+#define RU_SRT_I KC_Q    // й and Й
+#define RU_KA    KC_R    // к and К
+#define RU_EL    KC_K    // л and Л
+#define RU_EM    KC_V    // м and М
+#define RU_EN    KC_Y    // н and Н
+#define RU_O     KC_J    // о and О
+#define RU_PE    KC_G    // п and П
+#define RU_ER    KC_H    // р and Р
+#define RU_ES    KC_C    // с and С
+#define RU_TE    KC_N    // т and Т
+#define RU_U     KC_E    // у and У
+#define RU_EF    KC_A    // ф and Ф
+#define RU_HA    KC_LBRC // х and Х
+#define RU_TSE   KC_W    // ц and Ц
+#define RU_CHE   KC_X    // ч and Ч
+#define RU_SHA   KC_I    // ш and Ш
+#define RU_SHCHA KC_O    // щ and Щ
+#define RU_HSIGN KC_RBRC // ъ and Ъ
+#define RU_YERU  KC_S    // ы and Ы
+#define RU_SSIGN KC_M    // ь and Ь
+#define RU_E     KC_QUOT // э and Э
+#define RU_YU    KC_DOT  // ю and Ю
+#define RU_YA    KC_Z    // я and Я
+
+#define RU_1     KC_1    // 1 and !
+#define RU_2     KC_2    // 2 and "
+#define RU_3     KC_3    // 3 and №
+#define RU_4     KC_4    // 4 and ;
+#define RU_5     KC_5    // 5 and %
+#define RU_6     KC_6    // 6 and :
+#define RU_7     KC_7    // 7 and ?
+#define RU_8     KC_8    // 8 and *
+#define RU_9     KC_9    // 9 and (
+#define RU_0     KC_0    // 0 and )
+
+#define RU_MINS  KC_MINS // - and _
+#define RU_EQL   KC_EQL  // = and +
+#define RU_BSLS  KC_BSLS // \ and /
+#define RU_DOT   KC_SLSH // . and ,
+
+// Shifted Chracters
+#define RU_EXLM LSFT(RU_1) // !
+#define RU_DQUT LSFT(RU_2) // "
+#define RU_NMRO LSFT(RU_3) // №
+#define RU_SCLN LSFT(RU_4) // ;
+#define RU_PERC LSFT(RU_5) // %
+#define RU_COLN LSFT(RU_6) // :
+#define RU_QUES LSFT(RU_7) // ?
+#define RU_ASTR LSFT(RU_8) // *
+#define RU_LPRN LSFT(RU_9) // (
+#define RU_RPRN LSFT(RU_0) // )
+
+#define RU_UNDR LSFT(RU_MINS) // _
+#define RU_PLUS LSFT(RU_EQL)  // +
+#define RU_SLSH LSFT(RU_BSLS) // /
+#define RU_COMM LSFT(RU_DOT)  // ,
+
+// Alt Gr-ed characters
+#define RU_RUBL RALT(RU_8) // ₽
+
+#endif
diff --git a/quantum/keymap_extras/keymap_unicode_cyrillic.h b/quantum/keymap_extras/keymap_unicode_cyrillic.h
new file mode 100644
index 000000000..a40626d91
--- /dev/null
+++ b/quantum/keymap_extras/keymap_unicode_cyrillic.h
@@ -0,0 +1,163 @@
+#ifndef KEYMAP_CYRILLIC_H
+#define KEYMAP_CYRILLIC_H
+
+#include "keymap.h"
+
+/*
+ * This is based off of
+ * https://en.wikipedia.org/wiki/Cyrillic_script
+ *
+ * Unicode is iffy, a software implementation is preferred
+ */
+
+// Capital                   Char russian/ukrainian/bulgarian
+#define CY_A     UC(0x0410) // А  rus ukr bul
+#define CY_BE    UC(0x0411) // Б  rus ukr bul
+#define CY_VE    UC(0x0412) // В  rus ukr bul
+#define CY_GHE   UC(0x0413) // Г  rus ukr bul
+#define CY_GHEUP UC(0x0490) // Ґ      ukr
+#define CY_DE    UC(0x0414) // Д  rus ukr bul
+#define CY_DJE   UC(0x0402) // Ђ
+#define CY_GJE   UC(0x0403) // Ѓ
+#define CY_IE    UC(0x0415) // Е  rus ukr bul
+#define CY_IO    UC(0x0401) // Ё  rus
+#define CY_UIE   UC(0x0404) // Є      ukr
+#define CY_ZHE   UC(0x0416) // Ж  rus ukr bul
+#define CY_ZE    UC(0x0417) // З  rus ukr bul
+#define CY_DZE   UC(0x0405) // Ѕ
+#define CY_I     UC(0x0418) // И  rus ukr bul
+#define CY_B_U_I UC(0x0406) // І      ukr
+#define CY_YI    UC(0x0407) // Ї      ukr
+#define CY_SRT_I UC(0x0419) // Й  rus ukr bul
+#define CY_JE    UC(0x0408) // Ј
+#define CY_KA    UC(0x041a) // К  rus ukr bul
+#define CY_EL    UC(0x041b) // Л  rus ukr bul
+#define CY_LJE   UC(0x0409) // Љ
+#define CY_EM    UC(0x041c) // М  rus ukr bul
+#define CY_EN    UC(0x041d) // Н  rus ukr bul
+#define CY_NJE   UC(0x040a) // Њ
+#define CY_O     UC(0x041e) // О  rus ukr bul
+#define CY_PE    UC(0x041f) // П  rus ukr bul
+#define CY_ER    UC(0x0420) // Р  rus ukr bul
+#define CY_ES    UC(0x0421) // С  rus ukr bul
+#define CY_TE    UC(0x0422) // Т  rus ukr bul
+#define CY_TSHE  UC(0x040b) // Ћ
+#define CY_KJE   UC(0x040c) // Ќ
+#define CY_U     UC(0x0423) // У  rus ukr bul
+#define CY_SRT_U UC(0x040e) // Ў
+#define CY_EF    UC(0x0424) // Ф  rus ukr bul
+#define CY_HA    UC(0x0425) // Х  rus     bul
+#define CY_TSE   UC(0x0426) // Ц  rus ukr bul
+#define CY_CHE   UC(0x0427) // Ч  rus ukr bul
+#define CY_DZHE  UC(0x040f) // Џ
+#define CY_SHA   UC(0x0428) // Ш  rus ukr bul
+#define CY_SHCHA UC(0x0429) // Щ  rus ukr bul
+#define CY_HSIGN UC(0x042a) // Ъ  rus     bul
+#define CY_YERU  UC(0x042b) // Ы  rus
+#define CY_SSIGN UC(0x042c) // Ь  rus ukr bul
+#define CY_E     UC(0x042d) // Э  rus
+#define CY_YU    UC(0x042e) // Ю  rus ukr bul
+#define CY_YA    UC(0x042f) // Я  rus ukr bul
+// Important Cyrillic non-Slavic letters
+#define CY_PALOCHKA UC(0x04c0) // Ӏ
+#define CY_SCHWA UC(0x04d8) // Ә
+#define CY_GHE_S UC(0x0492) // Ғ
+#define CY_ZE_D  UC(0x0498) // Ҙ
+#define CY_ES_D  UC(0x04aa) // Ҫ
+#define CY_BR_KA UC(0x04a0) // Ҡ
+#define CY_ZHE_D UC(0x0496) // Җ
+#define CY_KA_D  UC(0x049a) // Қ
+#define CY_EN_D  UC(0x04a2) // Ң
+#define CY_ENGHE UC(0x04a4) // Ҥ
+#define CY_BRD_O UC(0x04e8) // Ө
+#define CY_STR_U UC(0x04ae) // Ү
+#define CY_S_U_S UC(0x04b0) // Ұ
+#define CY_SHHA  UC(0x04ba) // Һ
+#define CY_HA_D  UC(0x04b2) // Ҳ
+
+
+// Small
+#define CY_a     UC(0x0430) // a  rus ukr bul
+#define CY_be    UC(0x0431) // б  rus ukr bul
+#define CY_ve    UC(0x0432) // в  rus ukr bul
+#define CY_ghe   UC(0x0433) // г  rus ukr bul
+#define CY_gheup UC(0x0491) // ґ      ukr
+#define CY_de    UC(0x0434) // д  rus ukr bul
+#define CY_dje   UC(0x0452) // ђ
+#define CY_gje   UC(0x0453) // ѓ
+#define CY_ie    UC(0x0435) // е  rus ukr bul
+#define CY_io    UC(0x0451) // ё  rus
+#define CY_uie   UC(0x0454) // є      ukr
+#define CY_zhe   UC(0x0436) // ж  rus ukr bul
+#define CY_ze    UC(0x0437) // з  rus ukr bul
+#define CY_dze   UC(0x0455) // ѕ
+#define CY_i     UC(0x0438) // и  rus ukr bul
+#define CY_b_u_i UC(0x0456) // і      ukr
+#define CY_yi    UC(0x0457) // ї      ukr
+#define CY_srt_i UC(0x0439) // й  rus ukr bul
+#define CY_je    UC(0x0458) // ј
+#define CY_ka    UC(0x043a) // к  rus ukr bul
+#define CY_el    UC(0x043b) // л  rus ukr bul
+#define CY_lje   UC(0x0459) // љ
+#define CY_em    UC(0x043c) // м  rus ukr bul
+#define CY_en    UC(0x043d) // н  rus ukr bul
+#define CY_nje   UC(0x045a) // њ
+#define CY_o     UC(0x043e) // о  rus ukr bul
+#define CY_pe    UC(0x043f) // п  rus ukr bul
+#define CY_er    UC(0x0440) // р  rus ukr bul
+#define CY_es    UC(0x0441) // с  rus ukr bul
+#define CY_te    UC(0x0442) // т  rus ukr bul
+#define CY_tshe  UC(0x045b) // ћ
+#define CY_kje   UC(0x045c) // ќ
+#define CY_u     UC(0x0443) // у  rus ukr bul
+#define CY_srt_u UC(0x045e) // ў
+#define CY_ef    UC(0x0444) // ф  rus ukr bul
+#define CY_ha    UC(0x0445) // х  rus ukr bul
+#define CY_tse   UC(0x0446) // ц  rus ukr bul
+#define CY_che   UC(0x0447) // ч  rus ukr bul
+#define CY_dzhe  UC(0x045f) // џ
+#define CY_sha   UC(0x0448) // ш  rus ukr bul
+#define CY_shcha UC(0x0449) // щ  rus ukr bul
+#define CY_hsign UC(0x044a) // ъ  rus     bul
+#define CY_yeru  UC(0x044b) // ы  rus
+#define CY_ssign UC(0x044c) // ь  rus ukr bul
+#define CY_e     UC(0x044d) // э  rus
+#define CY_yu    UC(0x044e) // ю  rus ukr bul
+#define CY_ya    UC(0x044f) // я  rus ukr bul
+// Important Cyrillic non-Slavic letters
+#define CY_palochka UC(0x04cf) // ӏ
+#define CY_schwa UC(0x04d9) // ә
+#define CY_ghe_s UC(0x0493) // ғ
+#define CY_ze_d  UC(0x0499) // ҙ
+#define CY_es_d  UC(0x04ab) // ҫ
+#define CY_br_ka UC(0x04a1) // ҡ
+#define CY_zhe_d UC(0x0497) // җ
+#define CY_ka_d  UC(0x049b) // қ
+#define CY_en_d  UC(0x04a3) // ң
+#define CY_enghe UC(0x04a5) // ҥ
+#define CY_brd_o UC(0x04e9) // ө
+#define CY_str_u UC(0x04af) // ү
+#define CY_s_u_s UC(0x04b1) // ұ
+#define CY_shha  UC(0x04bb) // һ
+#define CY_ha_d  UC(0x04b3) // ҳ
+
+
+// Extra
+#define CY_slr_ve UC(0x1c80) // ᲀ CYRILLIC SMALL LETTER ROUNDED VE
+#define CY_ll_de  UC(0x1c81) // ᲁ CYRILLIC SMALL LETTER LONG-LEGGED DE
+#define CY_ZEMLYA UC(0xa640) // Ꙁ CYRILLIC CAPITAL LETTER ZEMLYA
+#define CY_zemlya UC(0xa641) // ꙁ CYRILLIC SMALL LETTER ZEMLYA
+#define CY_RV_DZE UC(0xa644) // Ꙅ CYRILLIC CAPITAL LETTER REVERSED DZE
+#define CY_rv_DZE UC(0xa645) // ꙅ CYRILLIC SMALL LETTER REVERSED DZE
+#define CY_slw_es UC(0x1c83) // ᲃ CYRILLIC SMALL LETTER WIDE ES
+#define CY_st_te  UC(0x1c84) // ᲄ CYRILLIC SMALL LETTER TALL TE
+#define CY_3l_te  UC(0x1c85) // ᲅ CYRILLIC SMALL LETTER THREE-LEGGED TE
+#define CY_thsign UC(0x1c86) // ᲆ CYRILLIC SMALL LETTER TALL HARD SIGN
+#define CY_YERUBY UC(0xa650) // Ꙑ CYRILLIC CAPITAL LETTER YERU WITH BACK YER
+#define CY_yeruby UC(0xa651) // ꙑ CYRILLIC SMALL LETTER YERU WITH BACK YER
+#define CY_RUBL   UC(0x20bd) // ₽
+#define CY_NMRO   UC(0x2116) // №
+
+// The letters Zje and Sje are made for other letters and accent marks
+
+#endif
diff --git a/quantum/process_keycode/process_tap_dance.c b/quantum/process_keycode/process_tap_dance.c
index d240dc2e6..07de3ecb8 100644
--- a/quantum/process_keycode/process_tap_dance.c
+++ b/quantum/process_keycode/process_tap_dance.c
@@ -1,14 +1,16 @@
 #include "quantum.h"
+#include "action_tapping.h"
 
-static qk_tap_dance_state_t qk_tap_dance_state;
+static uint16_t last_td;
+static int8_t highest_td = -1;
 
 void qk_tap_dance_pair_finished (qk_tap_dance_state_t *state, void *user_data) {
   qk_tap_dance_pair_t *pair = (qk_tap_dance_pair_t *)user_data;
 
   if (state->count == 1) {
-    register_code (pair->kc1);
+    register_code16 (pair->kc1);
   } else if (state->count == 2) {
-    register_code (pair->kc2);
+    register_code16 (pair->kc2);
   }
 }
 
@@ -16,102 +18,117 @@ void qk_tap_dance_pair_reset (qk_tap_dance_state_t *state, void *user_data) {
   qk_tap_dance_pair_t *pair = (qk_tap_dance_pair_t *)user_data;
 
   if (state->count == 1) {
-    unregister_code (pair->kc1);
+    unregister_code16 (pair->kc1);
   } else if (state->count == 2) {
-    unregister_code (pair->kc2);
+    unregister_code16 (pair->kc2);
   }
 }
 
 static inline void _process_tap_dance_action_fn (qk_tap_dance_state_t *state,
-                                          void *user_data,
-                                          qk_tap_dance_user_fn_t fn)
+                                                 void *user_data,
+                                                 qk_tap_dance_user_fn_t fn)
 {
   if (fn) {
     fn(state, user_data);
   }
 }
 
-static inline void process_tap_dance_action_on_each_tap (qk_tap_dance_action_t action)
+static inline void process_tap_dance_action_on_each_tap (qk_tap_dance_action_t *action)
 {
-  _process_tap_dance_action_fn (&qk_tap_dance_state, action.user_data, action.fn.on_each_tap);
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_each_tap);
 }
 
-static inline void process_tap_dance_action_on_dance_finished (qk_tap_dance_action_t action)
+static inline void process_tap_dance_action_on_dance_finished (qk_tap_dance_action_t *action)
 {
-  _process_tap_dance_action_fn (&qk_tap_dance_state, action.user_data, action.fn.on_dance_finished);
+  if (action->state.finished)
+    return;
+  action->state.finished = true;
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_dance_finished);
 }
 
-static inline void process_tap_dance_action_on_reset (qk_tap_dance_action_t action)
+static inline void process_tap_dance_action_on_reset (qk_tap_dance_action_t *action)
 {
-  _process_tap_dance_action_fn (&qk_tap_dance_state, action.user_data, action.fn.on_reset);
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_reset);
 }
 
 bool process_tap_dance(uint16_t keycode, keyrecord_t *record) {
-  bool r = true;
   uint16_t idx = keycode - QK_TAP_DANCE;
-  qk_tap_dance_action_t action;
+  qk_tap_dance_action_t *action;
+
+  if (last_td && last_td != keycode) {
+    (&tap_dance_actions[last_td - QK_TAP_DANCE])->state.interrupted = true;
+  }
 
   switch(keycode) {
   case QK_TAP_DANCE ... QK_TAP_DANCE_MAX:
-    action = tap_dance_actions[idx];
+    if ((int16_t)idx > highest_td)
+      highest_td = idx;
+    action = &tap_dance_actions[idx];
 
-    process_tap_dance_action_on_each_tap (action);
-    if (qk_tap_dance_state.keycode && qk_tap_dance_state.keycode != keycode) {
-      process_tap_dance_action_on_dance_finished (action);
-    } else if (qk_tap_dance_state.active && qk_tap_dance_state.pressed) {
-      reset_tap_dance (&qk_tap_dance_state);
-    } else {
-      r = false;
-    }
-
-    qk_tap_dance_state.active = true;
-    qk_tap_dance_state.pressed = record->event.pressed;
+    action->state.keycode = keycode;
+    action->state.pressed = record->event.pressed;
     if (record->event.pressed) {
-      qk_tap_dance_state.keycode = keycode;
-      qk_tap_dance_state.timer = timer_read ();
-      qk_tap_dance_state.count++;
+      action->state.count++;
+      action->state.timer = timer_read();
+
+      if (last_td && last_td != keycode) {
+        qk_tap_dance_action_t *paction = &tap_dance_actions[last_td - QK_TAP_DANCE];
+        paction->state.interrupted = true;
+        process_tap_dance_action_on_dance_finished (paction);
+        reset_tap_dance (&paction->state);
+      }
     }
+    last_td = keycode;
+
     break;
 
   default:
-    if (qk_tap_dance_state.keycode) {
-      // if we are here, the tap dance was interrupted by a different key
-      idx = qk_tap_dance_state.keycode - QK_TAP_DANCE;
-      action = tap_dance_actions[idx];
+    if (!record->event.pressed)
+      return true;
 
-      process_tap_dance_action_on_each_tap (action);
+    if (highest_td == -1)
+      return true;
+
+    for (int i = 0; i <= highest_td; i++) {
+      action = &tap_dance_actions[i];
+      if (action->state.count == 0)
+        continue;
+      action->state.interrupted = true;
       process_tap_dance_action_on_dance_finished (action);
-      reset_tap_dance (&qk_tap_dance_state);
-      qk_tap_dance_state.active = false;
+      reset_tap_dance (&action->state);
     }
     break;
   }
 
-  return r;
+  return true;
 }
 
 void matrix_scan_tap_dance () {
-  if (qk_tap_dance_state.active && timer_elapsed (qk_tap_dance_state.timer) > TAPPING_TERM) {
-    // if we are here, the tap dance was timed out
-    uint16_t idx = qk_tap_dance_state.keycode - QK_TAP_DANCE;
-    qk_tap_dance_action_t action = tap_dance_actions[idx];
+  if (highest_td == -1)
+    return;
+
+  for (int i = 0; i <= highest_td; i++) {
+    qk_tap_dance_action_t *action = &tap_dance_actions[i];
 
-    process_tap_dance_action_on_dance_finished (action);
-    reset_tap_dance (&qk_tap_dance_state);
+    if (action->state.count && timer_elapsed (action->state.timer) > TAPPING_TERM) {
+      process_tap_dance_action_on_dance_finished (action);
+      reset_tap_dance (&action->state);
+    }
   }
 }
 
 void reset_tap_dance (qk_tap_dance_state_t *state) {
-  uint16_t idx = state->keycode - QK_TAP_DANCE;
-  qk_tap_dance_action_t action;
+  qk_tap_dance_action_t *action;
 
   if (state->pressed)
     return;
 
-  action = tap_dance_actions[idx];
+  action = &tap_dance_actions[state->keycode - QK_TAP_DANCE];
+
   process_tap_dance_action_on_reset (action);
 
-  state->keycode = 0;
   state->count = 0;
-  state->active = false;
+  state->interrupted = false;
+  state->finished = false;
+  last_td = 0;
 }
diff --git a/quantum/process_keycode/process_tap_dance.h b/quantum/process_keycode/process_tap_dance.h
index e2c74efe9..d7b857bdc 100644
--- a/quantum/process_keycode/process_tap_dance.h
+++ b/quantum/process_keycode/process_tap_dance.h
@@ -11,8 +11,9 @@ typedef struct
   uint8_t count;
   uint16_t keycode;
   uint16_t timer;
-  bool active:1;
-  bool pressed:1;
+  bool interrupted;
+  bool pressed;
+  bool finished;
 } qk_tap_dance_state_t;
 
 #define TD(n) (QK_TAP_DANCE + n)
@@ -26,6 +27,7 @@ typedef struct
     qk_tap_dance_user_fn_t on_dance_finished;
     qk_tap_dance_user_fn_t on_reset;
   } fn;
+  qk_tap_dance_state_t state;
   void *user_data;
 } qk_tap_dance_action_t;
 
@@ -48,7 +50,7 @@ typedef struct
     .fn = { user_fn_on_each_tap, user_fn_on_dance_finished, user_fn_on_reset } \
   }
 
-extern const qk_tap_dance_action_t tap_dance_actions[];
+extern qk_tap_dance_action_t tap_dance_actions[];
 
 /* To be used internally */
 
diff --git a/quantum/process_keycode/process_unicode.c b/quantum/process_keycode/process_unicode.c
index ad5d7f86b..06c1694f2 100644
--- a/quantum/process_keycode/process_unicode.c
+++ b/quantum/process_keycode/process_unicode.c
@@ -13,45 +13,196 @@ uint16_t hex_to_keycode(uint8_t hex)
   }
 }
 
-void set_unicode_mode(uint8_t os_target)
+void set_unicode_input_mode(uint8_t os_target)
 {
   input_mode = os_target;
 }
 
+__attribute__((weak))
+void unicode_input_start (void) {
+  switch(input_mode) {
+  case UC_OSX:
+    register_code(KC_LALT);
+    break;
+  case UC_LNX:
+    register_code(KC_LCTL);
+    register_code(KC_LSFT);
+    register_code(KC_U);
+    unregister_code(KC_U);
+    unregister_code(KC_LSFT);
+    unregister_code(KC_LCTL);
+    break;
+  case UC_WIN:
+    register_code(KC_LALT);
+    register_code(KC_PPLS);
+    unregister_code(KC_PPLS);
+    break;
+  }
+  wait_ms(UNICODE_TYPE_DELAY);
+}
+
+__attribute__((weak))
+void unicode_input_finish (void) {
+  switch(input_mode) {
+  case UC_OSX:
+  case UC_WIN:
+    unregister_code(KC_LALT);
+    break;
+  case UC_LNX:
+    register_code(KC_SPC);
+    unregister_code(KC_SPC);
+    break;
+  }
+}
+
+void register_hex(uint16_t hex) {
+  for(int i = 3; i >= 0; i--) {
+    uint8_t digit = ((hex >> (i*4)) & 0xF);
+    register_code(hex_to_keycode(digit));
+    unregister_code(hex_to_keycode(digit));
+  }
+}
+
 bool process_unicode(uint16_t keycode, keyrecord_t *record) {
   if (keycode > QK_UNICODE && record->event.pressed) {
     uint16_t unicode = keycode & 0x7FFF;
-    switch(input_mode) {
-      case UC_OSX:
-        register_code(KC_LALT);
-        break;
-      case UC_LNX:
-        register_code(KC_LCTL);
-        register_code(KC_LSFT);
-        register_code(KC_U);
-        unregister_code(KC_U);
-        break;
-      case UC_WIN:
-        register_code(KC_LALT);
-        register_code(KC_PPLS);
-        unregister_code(KC_PPLS);
-        break;
+    unicode_input_start();
+    register_hex(unicode);
+    unicode_input_finish();
+  }
+  return true;
+}
+
+#ifdef UCIS_ENABLE
+qk_ucis_state_t qk_ucis_state;
+
+void qk_ucis_start(void) {
+  qk_ucis_state.count = 0;
+  qk_ucis_state.in_progress = true;
+
+  qk_ucis_start_user();
+}
+
+__attribute__((weak))
+void qk_ucis_start_user(void) {
+  unicode_input_start();
+  register_hex(0x2328);
+  unicode_input_finish();
+}
+
+static bool is_uni_seq(char *seq) {
+  uint8_t i;
+
+  for (i = 0; seq[i]; i++) {
+    uint16_t code;
+    if (('1' <= seq[i]) && (seq[i] <= '0'))
+      code = seq[i] - '1' + KC_1;
+    else
+      code = seq[i] - 'a' + KC_A;
+
+    if (i > qk_ucis_state.count || qk_ucis_state.codes[i] != code)
+      return false;
+  }
+
+  return (qk_ucis_state.codes[i] == KC_ENT ||
+          qk_ucis_state.codes[i] == KC_SPC);
+}
+
+__attribute__((weak))
+void qk_ucis_symbol_fallback (void) {
+  for (uint8_t i = 0; i < qk_ucis_state.count - 1; i++) {
+    uint8_t code = qk_ucis_state.codes[i];
+    register_code(code);
+    unregister_code(code);
+    wait_ms(UNICODE_TYPE_DELAY);
+  }
+}
+
+void register_ucis(const char *hex) {
+  for(int i = 0; hex[i]; i++) {
+    uint8_t kc = 0;
+    char c = hex[i];
+
+    switch (c) {
+    case '0':
+      kc = KC_0;
+      break;
+    case '1' ... '9':
+      kc = c - '1' + KC_1;
+      break;
+    case 'a' ... 'f':
+      kc = c - 'a' + KC_A;
+      break;
+    case 'A' ... 'F':
+      kc = c - 'A' + KC_A;
+      break;
     }
-    for(int i = 3; i >= 0; i--) {
-        uint8_t digit = ((unicode >> (i*4)) & 0xF);
-        register_code(hex_to_keycode(digit));
-        unregister_code(hex_to_keycode(digit));
+
+    if (kc) {
+      register_code (kc);
+      unregister_code (kc);
+      wait_ms (UNICODE_TYPE_DELAY);
     }
-    switch(input_mode) {
-      case UC_OSX:
-      case UC_WIN:
-        unregister_code(KC_LALT);
-        break;
-      case UC_LNX:
-        unregister_code(KC_LCTL);
-        unregister_code(KC_LSFT);
+  }
+}
+
+bool process_ucis (uint16_t keycode, keyrecord_t *record) {
+  uint8_t i;
+
+  if (!qk_ucis_state.in_progress)
+    return true;
+
+  if (qk_ucis_state.count >= UCIS_MAX_SYMBOL_LENGTH &&
+      !(keycode == KC_BSPC || keycode == KC_ESC || keycode == KC_SPC || keycode == KC_ENT)) {
+    return false;
+  }
+
+  if (!record->event.pressed)
+    return true;
+
+  qk_ucis_state.codes[qk_ucis_state.count] = keycode;
+  qk_ucis_state.count++;
+
+  if (keycode == KC_BSPC) {
+    if (qk_ucis_state.count >= 2) {
+      qk_ucis_state.count -= 2;
+      return true;
+    } else {
+      qk_ucis_state.count--;
+      return false;
+    }
+  }
+
+  if (keycode == KC_ENT || keycode == KC_SPC || keycode == KC_ESC) {
+    bool symbol_found = false;
+
+    for (i = qk_ucis_state.count; i > 0; i--) {
+      register_code (KC_BSPC);
+      unregister_code (KC_BSPC);
+      wait_ms(UNICODE_TYPE_DELAY);
+    }
+
+    if (keycode == KC_ESC) {
+      qk_ucis_state.in_progress = false;
+      return false;
+    }
+
+    unicode_input_start();
+    for (i = 0; ucis_symbol_table[i].symbol; i++) {
+      if (is_uni_seq (ucis_symbol_table[i].symbol)) {
+        symbol_found = true;
+        register_ucis(ucis_symbol_table[i].code + 2);
         break;
+      }
     }
+    if (!symbol_found) {
+      qk_ucis_symbol_fallback();
+    }
+    unicode_input_finish();
+
+    qk_ucis_state.in_progress = false;
+    return false;
   }
   return true;
-}
\ No newline at end of file
+}
+#endif
diff --git a/quantum/process_keycode/process_unicode.h b/quantum/process_keycode/process_unicode.h
index ca17f8f66..02ce3dd7e 100644
--- a/quantum/process_keycode/process_unicode.h
+++ b/quantum/process_keycode/process_unicode.h
@@ -8,10 +8,48 @@
 #define UC_WIN 2
 #define UC_BSD 3
 
+#ifndef UNICODE_TYPE_DELAY
+#define UNICODE_TYPE_DELAY 10
+#endif
+
 void set_unicode_input_mode(uint8_t os_target);
+void unicode_input_start(void);
+void unicode_input_finish(void);
+void register_hex(uint16_t hex);
 
 bool process_unicode(uint16_t keycode, keyrecord_t *record);
 
+#ifdef UCIS_ENABLE
+#ifndef UCIS_MAX_SYMBOL_LENGTH
+#define UCIS_MAX_SYMBOL_LENGTH 32
+#endif
+
+typedef struct {
+  char *symbol;
+  char *code;
+} qk_ucis_symbol_t;
+
+typedef struct {
+  uint8_t count;
+  uint16_t codes[UCIS_MAX_SYMBOL_LENGTH];
+  bool in_progress:1;
+} qk_ucis_state_t;
+
+extern qk_ucis_state_t qk_ucis_state;
+
+#define UCIS_TABLE(...) {__VA_ARGS__, {NULL, NULL}}
+#define UCIS_SYM(name, code) {name, #code}
+
+extern const qk_ucis_symbol_t ucis_symbol_table[];
+
+void qk_ucis_start(void);
+void qk_ucis_start_user(void);
+void qk_ucis_symbol_fallback (void);
+void register_ucis(const char *hex);
+bool process_ucis (uint16_t keycode, keyrecord_t *record);
+
+#endif
+
 #define UC_BSPC UC(0x0008)
 
 #define UC_SPC  UC(0x0020)
@@ -119,4 +157,4 @@ bool process_unicode(uint16_t keycode, keyrecord_t *record);
 #define UC_TILD UC(0x007E)
 #define UC_DEL  UC(0x007F)
 
-#endif
\ No newline at end of file
+#endif
diff --git a/quantum/quantum.c b/quantum/quantum.c
index bc2da510f..e3a20f43e 100644
--- a/quantum/quantum.c
+++ b/quantum/quantum.c
@@ -1,5 +1,42 @@
 #include "quantum.h"
 
+static void do_code16 (uint16_t code, void (*f) (uint8_t)) {
+  switch (code) {
+  case QK_MODS ... QK_MODS_MAX:
+    break;
+  default:
+    return;
+  }
+
+  if (code & QK_LCTL)
+    f(KC_LCTL);
+  if (code & QK_LSFT)
+    f(KC_LSFT);
+  if (code & QK_LALT)
+    f(KC_LALT);
+  if (code & QK_LGUI)
+    f(KC_LGUI);
+
+  if (code & QK_RCTL)
+    f(KC_RCTL);
+  if (code & QK_RSFT)
+    f(KC_RSFT);
+  if (code & QK_RALT)
+    f(KC_RALT);
+  if (code & QK_RGUI)
+    f(KC_RGUI);
+}
+
+void register_code16 (uint16_t code) {
+  do_code16 (code, register_code);
+  register_code (code);
+}
+
+void unregister_code16 (uint16_t code) {
+  unregister_code (code);
+  do_code16 (code, unregister_code);
+}
+
 __attribute__ ((weak))
 bool process_action_kb(keyrecord_t *record) {
   return true;
@@ -46,18 +83,20 @@ bool process_record_quantum(keyrecord_t *record) {
   uint16_t keycode;
 
   #if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
-    uint8_t layer;
+    /* TODO: Use store_or_get_action() or a similar function. */
+    if (!disable_action_cache) {
+      uint8_t layer;
 
-    if (record->event.pressed) {
-      layer = layer_switch_get_layer(key);
-      update_source_layers_cache(key, layer);
-    } else {
-      layer = read_source_layers_cache(key);
-    }
-    keycode = keymap_key_to_keycode(layer, key);
-  #else
-    keycode = keymap_key_to_keycode(layer_switch_get_layer(key), key);
+      if (record->event.pressed) {
+        layer = layer_switch_get_layer(key);
+        update_source_layers_cache(key, layer);
+      } else {
+        layer = read_source_layers_cache(key);
+      }
+      keycode = keymap_key_to_keycode(layer, key);
+    } else
   #endif
+    keycode = keymap_key_to_keycode(layer_switch_get_layer(key), key);
 
     // This is how you use actions here
     // if (keycode == KC_LEAD) {
@@ -87,6 +126,9 @@ bool process_record_quantum(keyrecord_t *record) {
   #ifdef UNICODE_ENABLE
     process_unicode(keycode, record) &&
   #endif
+  #ifdef UCIS_ENABLE
+    process_ucis(keycode, record) &&
+  #endif
       true)) {
     return false;
   }
diff --git a/quantum/quantum.h b/quantum/quantum.h
index 7ebfb24e3..0c6046649 100644
--- a/quantum/quantum.h
+++ b/quantum/quantum.h
@@ -25,6 +25,7 @@
 #include "led.h"
 #include "action_util.h"
 #include <stdlib.h>
+#include "print.h"
 
 
 extern uint32_t default_layer_state;
@@ -82,6 +83,9 @@ void reset_keyboard(void);
 void startup_user(void);
 void shutdown_user(void);
 
+void register_code16 (uint16_t code);
+void unregister_code16 (uint16_t code);
+
 #ifdef BACKLIGHT_ENABLE
 void backlight_init_ports(void);
 
diff --git a/quantum/rgblight.c b/quantum/rgblight.c
index b1b0f035d..f82e3ec55 100644
--- a/quantum/rgblight.c
+++ b/quantum/rgblight.c
@@ -7,24 +7,41 @@
 #include "debug.h"
 
 const uint8_t DIM_CURVE[] PROGMEM = {
-        0, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3,
-        3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4,
-        4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 6, 6, 6,
-        6, 6, 6, 6, 6, 7, 7, 7, 7, 7, 7, 7, 8, 8, 8, 8,
-        8, 8, 9, 9, 9, 9, 9, 9, 10, 10, 10, 10, 10, 11, 11, 11,
-        11, 11, 12, 12, 12, 12, 12, 13, 13, 13, 13, 14, 14, 14, 14, 15,
-        15, 15, 16, 16, 16, 16, 17, 17, 17, 18, 18, 18, 19, 19, 19, 20,
-        20, 20, 21, 21, 22, 22, 22, 23, 23, 24, 24, 25, 25, 25, 26, 26,
-        27, 27, 28, 28, 29, 29, 30, 30, 31, 32, 32, 33, 33, 34, 35, 35,
-        36, 36, 37, 38, 38, 39, 40, 40, 41, 42, 43, 43, 44, 45, 46, 47,
-        48, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62,
-        63, 64, 65, 66, 68, 69, 70, 71, 73, 74, 75, 76, 78, 79, 81, 82,
-        83, 85, 86, 88, 90, 91, 93, 94, 96, 98, 99, 101, 103, 105, 107, 109,
-        110, 112, 114, 116, 118, 121, 123, 125, 127, 129, 132, 134, 136, 139, 141, 144,
-        146, 149, 151, 154, 157, 159, 162, 165, 168, 171, 174, 177, 180, 183, 186, 190,
-        193, 196, 200, 203, 207, 211, 214, 218, 222, 226, 230, 234, 238, 242, 248, 255,
+  0, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3,
+  3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4,
+  4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 6, 6, 6,
+  6, 6, 6, 6, 6, 7, 7, 7, 7, 7, 7, 7, 8, 8, 8, 8,
+  8, 8, 9, 9, 9, 9, 9, 9, 10, 10, 10, 10, 10, 11, 11, 11,
+  11, 11, 12, 12, 12, 12, 12, 13, 13, 13, 13, 14, 14, 14, 14, 15,
+  15, 15, 16, 16, 16, 16, 17, 17, 17, 18, 18, 18, 19, 19, 19, 20,
+  20, 20, 21, 21, 22, 22, 22, 23, 23, 24, 24, 25, 25, 25, 26, 26,
+  27, 27, 28, 28, 29, 29, 30, 30, 31, 32, 32, 33, 33, 34, 35, 35,
+  36, 36, 37, 38, 38, 39, 40, 40, 41, 42, 43, 43, 44, 45, 46, 47,
+  48, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62,
+  63, 64, 65, 66, 68, 69, 70, 71, 73, 74, 75, 76, 78, 79, 81, 82,
+  83, 85, 86, 88, 90, 91, 93, 94, 96, 98, 99, 101, 103, 105, 107, 109,
+  110, 112, 114, 116, 118, 121, 123, 125, 127, 129, 132, 134, 136, 139, 141, 144,
+  146, 149, 151, 154, 157, 159, 162, 165, 168, 171, 174, 177, 180, 183, 186, 190,
+  193, 196, 200, 203, 207, 211, 214, 218, 222, 226, 230, 234, 238, 242, 248, 255
+};
+const uint8_t RGBLED_BREATHING_TABLE[] PROGMEM = {
+  0, 0, 0, 0, 1, 1, 1, 2, 2, 3, 4, 5, 5, 6, 7, 9,
+  10, 11, 12, 14, 15, 17, 18, 20, 21, 23, 25, 27, 29, 31, 33, 35,
+  37, 40, 42, 44, 47, 49, 52, 54, 57, 59, 62, 65, 67, 70, 73, 76,
+  79, 82, 85, 88, 90, 93, 97, 100, 103, 106, 109, 112, 115, 118, 121, 124,
+  127, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 162, 165, 167, 170, 173,
+  176, 179, 182, 185, 188, 190, 193, 196, 198, 201, 203, 206, 208, 211, 213, 215,
+  218, 220, 222, 224, 226, 228, 230, 232, 234, 235, 237, 238, 240, 241, 243, 244,
+  245, 246, 248, 249, 250, 250, 251, 252, 253, 253, 254, 254, 254, 255, 255, 255,
+  255, 255, 255, 255, 254, 254, 254, 253, 253, 252, 251, 250, 250, 249, 248, 246,
+  245, 244, 243, 241, 240, 238, 237, 235, 234, 232, 230, 228, 226, 224, 222, 220,
+  218, 215, 213, 211, 208, 206, 203, 201, 198, 196, 193, 190, 188, 185, 182, 179,
+  176, 173, 170, 167, 165, 162, 158, 155, 152, 149, 146, 143, 140, 137, 134, 131,
+  128, 124, 121, 118, 115, 112, 109, 106, 103, 100, 97, 93, 90, 88, 85, 82,
+  79, 76, 73, 70, 67, 65, 62, 59, 57, 54, 52, 49, 47, 44, 42, 40,
+  37, 35, 33, 31, 29, 27, 25, 23, 21, 20, 18, 17, 15, 14, 12, 11,
+  10, 9, 7, 6, 5, 5, 4, 3, 2, 2, 1, 1, 1, 0, 0, 0
 };
-const uint8_t RGBLED_BREATHING_TABLE[] PROGMEM = {0,0,0,0,1,1,1,2,2,3,4,5,5,6,7,9,10,11,12,14,15,17,18,20,21,23,25,27,29,31,33,35,37,40,42,44,47,49,52,54,57,59,62,65,67,70,73,76,79,82,85,88,90,93,97,100,103,106,109,112,115,118,121,124,127,131,134,137,140,143,146,149,152,155,158,162,165,167,170,173,176,179,182,185,188,190,193,196,198,201,203,206,208,211,213,215,218,220,222,224,226,228,230,232,234,235,237,238,240,241,243,244,245,246,248,249,250,250,251,252,253,253,254,254,254,255,255,255,255,255,255,255,254,254,254,253,253,252,251,250,250,249,248,246,245,244,243,241,240,238,237,235,234,232,230,228,226,224,222,220,218,215,213,211,208,206,203,201,198,196,193,190,188,185,182,179,176,173,170,167,165,162,158,155,152,149,146,143,140,137,134,131,128,124,121,118,115,112,109,106,103,100,97,93,90,88,85,82,79,76,73,70,67,65,62,59,57,54,52,49,47,44,42,40,37,35,33,31,29,27,25,23,21,20,18,17,15,14,12,11,10,9,7,6,5,5,4,3,2,2,1,1,1,0,0,0};
 const uint8_t RGBLED_BREATHING_INTERVALS[] PROGMEM = {30, 20, 10, 5};
 const uint8_t RGBLED_RAINBOW_MOOD_INTERVALS[] PROGMEM = {120, 60, 30};
 const uint8_t RGBLED_RAINBOW_SWIRL_INTERVALS[] PROGMEM = {100, 50, 20};
@@ -38,63 +55,56 @@ uint8_t rgblight_inited = 0;
 
 
 void sethsv(uint16_t hue, uint8_t sat, uint8_t val, struct cRGB *led1) {
-        /* convert hue, saturation and brightness ( HSB/HSV ) to RGB
-        The DIM_CURVE is used only on brightness/value and on saturation (inverted).
-        This looks the most natural.
-        */
-  uint8_t r = 0, g = 0, b = 0;
+  // Convert hue, saturation, and value (HSV/HSB) to RGB. DIM_CURVE is used only
+  // on value and saturation (inverted). This looks the most natural.
+  uint8_t r = 0, g = 0, b = 0, base, color;
 
   val = pgm_read_byte(&DIM_CURVE[val]);
-        sat = 255 - pgm_read_byte(&DIM_CURVE[255 - sat]);
-
-        uint8_t base;
-
-        if (sat == 0) { // Acromatic color (gray). Hue doesn't mind.
-                r = val;
-                g = val;
-                b = val;
-        } else  {
-                base = ((255 - sat) * val) >> 8;
-
-                switch (hue / 60) {
-                case 0:
-                        r = val;
-                        g = (((val - base)*hue) / 60) + base;
-                        b = base;
-                        break;
-
-                case 1:
-                        r = (((val - base)*(60 - (hue % 60))) / 60) + base;
-                        g = val;
-                        b = base;
-                        break;
-
-                case 2:
-                        r = base;
-                        g = val;
-                        b = (((val - base)*(hue % 60)) / 60) + base;
-                        break;
-
-                case 3:
-                        r = base;
-                        g = (((val - base)*(60 - (hue % 60))) / 60) + base;
-                        b = val;
-                        break;
-
-                case 4:
-                        r = (((val - base)*(hue % 60)) / 60) + base;
-                        g = base;
-                        b = val;
-                        break;
-
-                case 5:
-                        r = val;
-                        g = base;
-                        b = (((val - base)*(60 - (hue % 60))) / 60) + base;
-                        break;
-                }
-        }
-  setrgb(r,g,b, led1);
+  sat = 255 - pgm_read_byte(&DIM_CURVE[255 - sat]);
+
+  if (sat == 0) { // Acromatic color (gray). Hue doesn't mind.
+    r = val;
+    g = val;
+    b = val;
+  } else {
+    base = ((255 - sat) * val) >> 8;
+    color = (val - base) * (hue % 60) / 60;
+
+    switch (hue / 60) {
+      case 0:
+        r = val;
+        g = base + color;
+        b = base;
+        break;
+      case 1:
+        r = val - color;
+        g = val;
+        b = base;
+        break;
+      case 2:
+        r = base;
+        g = val;
+        b = base + color;
+        break;
+      case 3:
+        r = base;
+        g = val - color;
+        b = val;
+        break;
+      case 4:
+        r = base + color;
+        g = base;
+        b = val;
+        break;
+      case 5:
+        r = val;
+        g = base;
+        b = val - color;
+        break;
+    }
+  }
+
+  setrgb(r, g, b, led1);
 }
 
 void setrgb(uint8_t r, uint8_t g, uint8_t b, struct cRGB *led1) {
@@ -111,44 +121,44 @@ void eeconfig_update_rgblight(uint32_t val) {
   eeprom_update_dword(EECONFIG_RGBLIGHT, val);
 }
 void eeconfig_update_rgblight_default(void) {
-        dprintf("eeconfig_update_rgblight_default\n");
-        rgblight_config.enable = 1;
-        rgblight_config.mode = 1;
-        rgblight_config.hue = 200;
-        rgblight_config.sat = 204;
-        rgblight_config.val = 204;
-        eeconfig_update_rgblight(rgblight_config.raw);
+  dprintf("eeconfig_update_rgblight_default\n");
+  rgblight_config.enable = 1;
+  rgblight_config.mode = 1;
+  rgblight_config.hue = 200;
+  rgblight_config.sat = 204;
+  rgblight_config.val = 204;
+  eeconfig_update_rgblight(rgblight_config.raw);
 }
 void eeconfig_debug_rgblight(void) {
-        dprintf("rgblight_config eprom\n");
-        dprintf("rgblight_config.enable = %d\n", rgblight_config.enable);
-        dprintf("rghlight_config.mode = %d\n", rgblight_config.mode);
-        dprintf("rgblight_config.hue = %d\n", rgblight_config.hue);
-        dprintf("rgblight_config.sat = %d\n", rgblight_config.sat);
-        dprintf("rgblight_config.val = %d\n", rgblight_config.val);
+  dprintf("rgblight_config eprom\n");
+  dprintf("rgblight_config.enable = %d\n", rgblight_config.enable);
+  dprintf("rghlight_config.mode = %d\n", rgblight_config.mode);
+  dprintf("rgblight_config.hue = %d\n", rgblight_config.hue);
+  dprintf("rgblight_config.sat = %d\n", rgblight_config.sat);
+  dprintf("rgblight_config.val = %d\n", rgblight_config.val);
 }
 
 void rgblight_init(void) {
   debug_enable = 1; // Debug ON!
-        dprintf("rgblight_init called.\n");
+  dprintf("rgblight_init called.\n");
   rgblight_inited = 1;
-        dprintf("rgblight_init start!\n");
+  dprintf("rgblight_init start!\n");
   if (!eeconfig_is_enabled()) {
-                dprintf("rgblight_init eeconfig is not enabled.\n");
+    dprintf("rgblight_init eeconfig is not enabled.\n");
     eeconfig_init();
-                eeconfig_update_rgblight_default();
+    eeconfig_update_rgblight_default();
   }
   rgblight_config.raw = eeconfig_read_rgblight();
-        if (!rgblight_config.mode) {
-                dprintf("rgblight_init rgblight_config.mode = 0. Write default values to EEPROM.\n");
-                eeconfig_update_rgblight_default();
-                rgblight_config.raw = eeconfig_read_rgblight();
-        }
-        eeconfig_debug_rgblight(); // display current eeprom values
+  if (!rgblight_config.mode) {
+    dprintf("rgblight_init rgblight_config.mode = 0. Write default values to EEPROM.\n");
+    eeconfig_update_rgblight_default();
+    rgblight_config.raw = eeconfig_read_rgblight();
+  }
+  eeconfig_debug_rgblight(); // display current eeprom values
 
-        #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-                rgblight_timer_init(); // setup the timer
-        #endif
+  #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+    rgblight_timer_init(); // setup the timer
+  #endif
 
   if (rgblight_config.enable) {
     rgblight_mode(rgblight_config.mode);
@@ -156,58 +166,57 @@ void rgblight_init(void) {
 }
 
 void rgblight_increase(void) {
-        uint8_t mode = 0;
+  uint8_t mode = 0;
   if (rgblight_config.mode < RGBLIGHT_MODES) {
     mode = rgblight_config.mode + 1;
   }
-        rgblight_mode(mode);
+  rgblight_mode(mode);
 }
-
 void rgblight_decrease(void) {
-        uint8_t mode = 0;
-  if (rgblight_config.mode > 1) { //mode will never < 1, if mode is less than 1, eeprom need to be initialized.
-    mode = rgblight_config.mode-1;
+  uint8_t mode = 0;
+  // Mode will never be < 1. If it ever is, eeprom needs to be initialized.
+  if (rgblight_config.mode > 1) {
+    mode = rgblight_config.mode - 1;
   }
-        rgblight_mode(mode);
+  rgblight_mode(mode);
 }
-
 void rgblight_step(void) {
-        uint8_t mode = 0;
+  uint8_t mode = 0;
   mode = rgblight_config.mode + 1;
   if (mode > RGBLIGHT_MODES) {
     mode = 1;
   }
-        rgblight_mode(mode);
+  rgblight_mode(mode);
 }
 
 void rgblight_mode(uint8_t mode) {
-        if (!rgblight_config.enable) {
-                return;
-        }
-  if (mode<1) {
-                rgblight_config.mode = 1;
-        } else if (mode > RGBLIGHT_MODES) {
-                rgblight_config.mode = RGBLIGHT_MODES;
-        } else {
-                rgblight_config.mode = mode;
-        }
+  if (!rgblight_config.enable) {
+    return;
+  }
+  if (mode < 1) {
+    rgblight_config.mode = 1;
+  } else if (mode > RGBLIGHT_MODES) {
+    rgblight_config.mode = RGBLIGHT_MODES;
+  } else {
+    rgblight_config.mode = mode;
+  }
   eeconfig_update_rgblight(rgblight_config.raw);
   xprintf("rgblight mode: %u\n", rgblight_config.mode);
-        if (rgblight_config.mode == 1) {
-                #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-                        rgblight_timer_disable();
-                #endif
-        } else if (rgblight_config.mode >=2 && rgblight_config.mode <=23) {
-                // MODE 2-5, breathing
-                // MODE 6-8, rainbow mood
-                // MODE 9-14, rainbow swirl
-                // MODE 15-20, snake
-                // MODE 21-23, knight
-
-                #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-                        rgblight_timer_enable();
-                #endif
-        }
+  if (rgblight_config.mode == 1) {
+    #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+      rgblight_timer_disable();
+    #endif
+  } else if (rgblight_config.mode >= 2 && rgblight_config.mode <= 23) {
+    // MODE 2-5, breathing
+    // MODE 6-8, rainbow mood
+    // MODE 9-14, rainbow swirl
+    // MODE 15-20, snake
+    // MODE 21-23, knight
+
+    #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+      rgblight_timer_enable();
+    #endif
+  }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
 }
 
@@ -215,306 +224,318 @@ void rgblight_toggle(void) {
   rgblight_config.enable ^= 1;
   eeconfig_update_rgblight(rgblight_config.raw);
   xprintf("rgblight toggle: rgblight_config.enable = %u\n", rgblight_config.enable);
-        if (rgblight_config.enable) {
-                rgblight_mode(rgblight_config.mode);
-        } else {
-
-                #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-                        rgblight_timer_disable();
-                #endif
-                _delay_ms(50);
-                rgblight_set();
-        }
+  if (rgblight_config.enable) {
+    rgblight_mode(rgblight_config.mode);
+  } else {
+    #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+      rgblight_timer_disable();
+    #endif
+    _delay_ms(50);
+    rgblight_set();
+  }
 }
 
 
-void rgblight_increase_hue(void){
-        uint16_t hue;
+void rgblight_increase_hue(void) {
+  uint16_t hue;
   hue = (rgblight_config.hue+RGBLIGHT_HUE_STEP) % 360;
   rgblight_sethsv(hue, rgblight_config.sat, rgblight_config.val);
 }
-void rgblight_decrease_hue(void){
-        uint16_t hue;
-        if (rgblight_config.hue-RGBLIGHT_HUE_STEP <0 ) {
-                hue = (rgblight_config.hue+360-RGBLIGHT_HUE_STEP) % 360;
-        } else {
-                hue = (rgblight_config.hue-RGBLIGHT_HUE_STEP) % 360;
-        }
+void rgblight_decrease_hue(void) {
+  uint16_t hue;
+  if (rgblight_config.hue-RGBLIGHT_HUE_STEP < 0) {
+    hue = (rgblight_config.hue + 360 - RGBLIGHT_HUE_STEP) % 360;
+  } else {
+    hue = (rgblight_config.hue - RGBLIGHT_HUE_STEP) % 360;
+  }
   rgblight_sethsv(hue, rgblight_config.sat, rgblight_config.val);
 }
 void rgblight_increase_sat(void) {
-        uint8_t sat;
+  uint8_t sat;
   if (rgblight_config.sat + RGBLIGHT_SAT_STEP > 255) {
     sat = 255;
   } else {
-    sat = rgblight_config.sat+RGBLIGHT_SAT_STEP;
+    sat = rgblight_config.sat + RGBLIGHT_SAT_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, sat, rgblight_config.val);
 }
-void rgblight_decrease_sat(void){
-        uint8_t sat;
+void rgblight_decrease_sat(void) {
+  uint8_t sat;
   if (rgblight_config.sat - RGBLIGHT_SAT_STEP < 0) {
     sat = 0;
   } else {
-    sat = rgblight_config.sat-RGBLIGHT_SAT_STEP;
+    sat = rgblight_config.sat - RGBLIGHT_SAT_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, sat, rgblight_config.val);
 }
-void rgblight_increase_val(void){
-        uint8_t val;
+void rgblight_increase_val(void) {
+  uint8_t val;
   if (rgblight_config.val + RGBLIGHT_VAL_STEP > 255) {
     val = 255;
   } else {
-    val = rgblight_config.val+RGBLIGHT_VAL_STEP;
+    val = rgblight_config.val + RGBLIGHT_VAL_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, val);
 }
 void rgblight_decrease_val(void) {
-        uint8_t val;
+  uint8_t val;
   if (rgblight_config.val - RGBLIGHT_VAL_STEP < 0) {
     val = 0;
   } else {
-    val = rgblight_config.val-RGBLIGHT_VAL_STEP;
+    val = rgblight_config.val - RGBLIGHT_VAL_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, val);
 }
 
-void rgblight_sethsv_noeeprom(uint16_t hue, uint8_t sat, uint8_t val){
-        inmem_config.raw = rgblight_config.raw;
+void rgblight_sethsv_noeeprom(uint16_t hue, uint8_t sat, uint8_t val) {
+  inmem_config.raw = rgblight_config.raw;
   if (rgblight_config.enable) {
     struct cRGB tmp_led;
     sethsv(hue, sat, val, &tmp_led);
-                inmem_config.hue = hue;
-                inmem_config.sat = sat;
-                inmem_config.val = val;
+    inmem_config.hue = hue;
+    inmem_config.sat = sat;
+    inmem_config.val = val;
     // dprintf("rgblight set hue [MEMORY]: %u,%u,%u\n", inmem_config.hue, inmem_config.sat, inmem_config.val);
     rgblight_setrgb(tmp_led.r, tmp_led.g, tmp_led.b);
   }
 }
-void rgblight_sethsv(uint16_t hue, uint8_t sat, uint8_t val){
+void rgblight_sethsv(uint16_t hue, uint8_t sat, uint8_t val) {
   if (rgblight_config.enable) {
-                if (rgblight_config.mode == 1) {
-                        // same static color
-                        rgblight_sethsv_noeeprom(hue, sat, val);
-                } else {
-                        // all LEDs in same color
-                        if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
-                                // breathing mode, ignore the change of val, use in memory value instead
-                                val = rgblight_config.val;
-                        } else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 14) {
-                                // rainbow mood and rainbow swirl, ignore the change of hue
-                                hue = rgblight_config.hue;
-                        }
-                }
-                rgblight_config.hue = hue;
-                rgblight_config.sat = sat;
-                rgblight_config.val = val;
-                eeconfig_update_rgblight(rgblight_config.raw);
-                xprintf("rgblight set hsv [EEPROM]: %u,%u,%u\n", rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
-  }
-}
-
-void rgblight_setrgb(uint8_t r, uint8_t g, uint8_t b){
+    if (rgblight_config.mode == 1) {
+      // same static color
+      rgblight_sethsv_noeeprom(hue, sat, val);
+    } else {
+      // all LEDs in same color
+      if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
+        // breathing mode, ignore the change of val, use in memory value instead
+        val = rgblight_config.val;
+      } else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 14) {
+        // rainbow mood and rainbow swirl, ignore the change of hue
+        hue = rgblight_config.hue;
+      }
+    }
+    rgblight_config.hue = hue;
+    rgblight_config.sat = sat;
+    rgblight_config.val = val;
+    eeconfig_update_rgblight(rgblight_config.raw);
+    xprintf("rgblight set hsv [EEPROM]: %u,%u,%u\n", rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
+  }
+}
+
+void rgblight_setrgb(uint8_t r, uint8_t g, uint8_t b) {
   // dprintf("rgblight set rgb: %u,%u,%u\n", r,g,b);
-  for (uint8_t i=0;i<RGBLED_NUM;i++) {
+  for (uint8_t i = 0; i < RGBLED_NUM; i++) {
     led[i].r = r;
     led[i].g = g;
     led[i].b = b;
   }
   rgblight_set();
-
 }
 
 void rgblight_set(void) {
-        if (rgblight_config.enable) {
-                ws2812_setleds(led, RGBLED_NUM);
-        } else {
-                for (uint8_t i=0;i<RGBLED_NUM;i++) {
-            led[i].r = 0;
-            led[i].g = 0;
-            led[i].b = 0;
-          }
-                ws2812_setleds(led, RGBLED_NUM);
-        }
+  if (rgblight_config.enable) {
+    ws2812_setleds(led, RGBLED_NUM);
+  } else {
+    for (uint8_t i = 0; i < RGBLED_NUM; i++) {
+      led[i].r = 0;
+      led[i].g = 0;
+      led[i].b = 0;
+    }
+    ws2812_setleds(led, RGBLED_NUM);
+  }
 }
 
-
 #if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
 
 // Animation timer -- AVR Timer3
 void rgblight_timer_init(void) {
-        static uint8_t rgblight_timer_is_init = 0;
-        if (rgblight_timer_is_init) {
-                return;
-        }
-        rgblight_timer_is_init = 1;
-        /* Timer 3 setup */
-        TCCR3B = _BV(WGM32) //CTC mode OCR3A as TOP
-              | _BV(CS30); //Clock selelct: clk/1
-        /* Set TOP value */
-        uint8_t sreg = SREG;
-        cli();
-        OCR3AH = (RGBLED_TIMER_TOP>>8)&0xff;
-        OCR3AL = RGBLED_TIMER_TOP&0xff;
-        SREG = sreg;
+  static uint8_t rgblight_timer_is_init = 0;
+  if (rgblight_timer_is_init) {
+    return;
+  }
+  rgblight_timer_is_init = 1;
+  /* Timer 3 setup */
+  TCCR3B = _BV(WGM32) //CTC mode OCR3A as TOP
+        | _BV(CS30); //Clock selelct: clk/1
+  /* Set TOP value */
+  uint8_t sreg = SREG;
+  cli();
+  OCR3AH = (RGBLED_TIMER_TOP >> 8) & 0xff;
+  OCR3AL = RGBLED_TIMER_TOP & 0xff;
+  SREG = sreg;
 }
 void rgblight_timer_enable(void) {
-        TIMSK3 |= _BV(OCIE3A);
-        dprintf("TIMER3 enabled.\n");
+  TIMSK3 |= _BV(OCIE3A);
+  dprintf("TIMER3 enabled.\n");
 }
 void rgblight_timer_disable(void) {
-        TIMSK3 &= ~_BV(OCIE3A);
-        dprintf("TIMER3 disabled.\n");
+  TIMSK3 &= ~_BV(OCIE3A);
+  dprintf("TIMER3 disabled.\n");
 }
 void rgblight_timer_toggle(void) {
-        TIMSK3 ^= _BV(OCIE3A);
-        dprintf("TIMER3 toggled.\n");
+  TIMSK3 ^= _BV(OCIE3A);
+  dprintf("TIMER3 toggled.\n");
 }
 
 ISR(TIMER3_COMPA_vect) {
-        // Mode = 1, static light, do nothing here
-        if (rgblight_config.mode>=2 && rgblight_config.mode<=5) {
-                // mode = 2 to 5, breathing mode
-                rgblight_effect_breathing(rgblight_config.mode-2);
-
-        } else if (rgblight_config.mode>=6 && rgblight_config.mode<=8) {
-                rgblight_effect_rainbow_mood(rgblight_config.mode-6);
-        } else if (rgblight_config.mode>=9 && rgblight_config.mode<=14) {
-                rgblight_effect_rainbow_swirl(rgblight_config.mode-9);
-        } else if (rgblight_config.mode>=15 && rgblight_config.mode<=20) {
-                rgblight_effect_snake(rgblight_config.mode-15);
-        } else if (rgblight_config.mode>=21 && rgblight_config.mode<=23) {
-                rgblight_effect_knight(rgblight_config.mode-21);
-        }
-}
-
-// effects
+  // mode = 1, static light, do nothing here
+  if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
+    // mode = 2 to 5, breathing mode
+    rgblight_effect_breathing(rgblight_config.mode - 2);
+  } else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 8) {
+    // mode = 6 to 8, rainbow mood mod
+    rgblight_effect_rainbow_mood(rgblight_config.mode - 6);
+  } else if (rgblight_config.mode >= 9 && rgblight_config.mode <= 14) {
+    // mode = 9 to 14, rainbow swirl mode
+    rgblight_effect_rainbow_swirl(rgblight_config.mode - 9);
+  } else if (rgblight_config.mode >= 15 && rgblight_config.mode <= 20) {
+    // mode = 15 to 20, snake mode
+    rgblight_effect_snake(rgblight_config.mode - 15);
+  } else if (rgblight_config.mode >= 21 && rgblight_config.mode <= 23) {
+    // mode = 21 to 23, knight mode
+    rgblight_effect_knight(rgblight_config.mode - 21);
+  }
+}
+
+// Effects
 void rgblight_effect_breathing(uint8_t interval) {
-        static uint8_t pos = 0;
-        static uint16_t last_timer = 0;
+  static uint8_t pos = 0;
+  static uint16_t last_timer = 0;
 
-        if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_BREATHING_INTERVALS[interval])) return;
-        last_timer = timer_read();
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_BREATHING_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
 
-        rgblight_sethsv_noeeprom(rgblight_config.hue, rgblight_config.sat, pgm_read_byte(&RGBLED_BREATHING_TABLE[pos]));
-        pos = (pos+1) % 256;
+  rgblight_sethsv_noeeprom(rgblight_config.hue, rgblight_config.sat, pgm_read_byte(&RGBLED_BREATHING_TABLE[pos]));
+  pos = (pos + 1) % 256;
 }
-
 void rgblight_effect_rainbow_mood(uint8_t interval) {
-        static uint16_t current_hue=0;
-        static uint16_t last_timer = 0;
+  static uint16_t current_hue = 0;
+  static uint16_t last_timer = 0;
 
-        if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval])) return;
-        last_timer = timer_read();
-        rgblight_sethsv_noeeprom(current_hue, rgblight_config.sat, rgblight_config.val);
-        current_hue = (current_hue+1) % 360;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
+  rgblight_sethsv_noeeprom(current_hue, rgblight_config.sat, rgblight_config.val);
+  current_hue = (current_hue + 1) % 360;
 }
-
 void rgblight_effect_rainbow_swirl(uint8_t interval) {
-        static uint16_t current_hue=0;
-        static uint16_t last_timer = 0;
-        uint16_t hue;
-        uint8_t i;
-        if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval/2])) return;
-        last_timer = timer_read();
-        for (i=0; i<RGBLED_NUM; i++) {
-                hue = (360/RGBLED_NUM*i+current_hue)%360;
-                sethsv(hue, rgblight_config.sat, rgblight_config.val, &led[i]);
-        }
-        rgblight_set();
-
-        if (interval % 2) {
-                current_hue = (current_hue+1) % 360;
-        } else {
-                if (current_hue -1 < 0) {
-                        current_hue = 359;
-                } else {
-                        current_hue = current_hue - 1;
-                }
-
-        }
+  static uint16_t current_hue = 0;
+  static uint16_t last_timer = 0;
+  uint16_t hue;
+  uint8_t i;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval / 2])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    hue = (360 / RGBLED_NUM * i + current_hue) % 360;
+    sethsv(hue, rgblight_config.sat, rgblight_config.val, &led[i]);
+  }
+  rgblight_set();
+
+  if (interval % 2) {
+    current_hue = (current_hue + 1) % 360;
+  } else {
+    if (current_hue - 1 < 0) {
+      current_hue = 359;
+    } else {
+      current_hue = current_hue - 1;
+    }
+  }
 }
 void rgblight_effect_snake(uint8_t interval) {
-        static uint8_t pos=0;
-        static uint16_t last_timer = 0;
-        uint8_t i,j;
-        int8_t k;
-        int8_t increament = 1;
-        if (interval%2) increament = -1;
-        if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_SNAKE_INTERVALS[interval/2])) return;
-        last_timer = timer_read();
-        for (i=0;i<RGBLED_NUM;i++) {
-                led[i].r=0;
-                led[i].g=0;
-                led[i].b=0;
-                for (j=0;j<RGBLIGHT_EFFECT_SNAKE_LENGTH;j++) {
-                        k = pos+j*increament;
-                        if (k<0) k = k+RGBLED_NUM;
-                        if (i==k) {
-                                sethsv(rgblight_config.hue, rgblight_config.sat, (uint8_t)(rgblight_config.val*(RGBLIGHT_EFFECT_SNAKE_LENGTH-j)/RGBLIGHT_EFFECT_SNAKE_LENGTH), &led[i]);
-                        }
-                }
-        }
-        rgblight_set();
-        if (increament == 1) {
-                if (pos - 1 < 0) {
-                        pos = RGBLED_NUM-1;
-                } else {
-                        pos -= 1;
-                }
-        } else {
-                pos = (pos+1)%RGBLED_NUM;
-        }
-
+  static uint8_t pos = 0;
+  static uint16_t last_timer = 0;
+  uint8_t i, j;
+  int8_t k;
+  int8_t increment = 1;
+  if (interval % 2) {
+    increment = -1;
+  }
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_SNAKE_INTERVALS[interval / 2])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    led[i].r = 0;
+    led[i].g = 0;
+    led[i].b = 0;
+    for (j = 0; j < RGBLIGHT_EFFECT_SNAKE_LENGTH; j++) {
+      k = pos + j * increment;
+      if (k < 0) {
+        k = k + RGBLED_NUM;
+      }
+      if (i == k) {
+        sethsv(rgblight_config.hue, rgblight_config.sat, (uint8_t)(rgblight_config.val*(RGBLIGHT_EFFECT_SNAKE_LENGTH-j)/RGBLIGHT_EFFECT_SNAKE_LENGTH), &led[i]);
+      }
+    }
+  }
+  rgblight_set();
+  if (increment == 1) {
+    if (pos - 1 < 0) {
+      pos = RGBLED_NUM - 1;
+    } else {
+      pos -= 1;
+    }
+  } else {
+    pos = (pos + 1) % RGBLED_NUM;
+  }
 }
-
 void rgblight_effect_knight(uint8_t interval) {
-        static int8_t pos=0;
-        static uint16_t last_timer = 0;
-        uint8_t i,j,cur;
-        int8_t k;
-        struct cRGB preled[RGBLED_NUM];
-        static int8_t increament = -1;
-        if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_KNIGHT_INTERVALS[interval])) return;
-        last_timer = timer_read();
-        for (i=0;i<RGBLED_NUM;i++) {
-                preled[i].r=0;
-                preled[i].g=0;
-                preled[i].b=0;
-                for (j=0;j<RGBLIGHT_EFFECT_KNIGHT_LENGTH;j++) {
-                        k = pos+j*increament;
-                        if (k<0) k = 0;
-                        if (k>=RGBLED_NUM) k=RGBLED_NUM-1;
-                        if (i==k) {
-                                sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val, &preled[i]);
-                        }
-                }
-        }
-        if (RGBLIGHT_EFFECT_KNIGHT_OFFSET) {
-                for (i=0;i<RGBLED_NUM;i++) {
-                        cur = (i+RGBLIGHT_EFFECT_KNIGHT_OFFSET) % RGBLED_NUM;
-                        led[i].r = preled[cur].r;
-                        led[i].g = preled[cur].g;
-                        led[i].b = preled[cur].b;
-                }
-        }
-        rgblight_set();
-        if (increament == 1) {
-                if (pos - 1 < 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
-                        pos = 0- RGBLIGHT_EFFECT_KNIGHT_LENGTH;
-                        increament = -1;
-                } else {
-                        pos -= 1;
-                }
-        } else {
-                if (pos+1>RGBLED_NUM+RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
-                        pos = RGBLED_NUM+RGBLIGHT_EFFECT_KNIGHT_LENGTH-1;
-                        increament = 1;
-                } else {
-                        pos += 1;
-                }
-        }
-
-}
-
-#endif
\ No newline at end of file
+  static int8_t pos = 0;
+  static uint16_t last_timer = 0;
+  uint8_t i, j, cur;
+  int8_t k;
+  struct cRGB preled[RGBLED_NUM];
+  static int8_t increment = -1;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_KNIGHT_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    preled[i].r = 0;
+    preled[i].g = 0;
+    preled[i].b = 0;
+    for (j = 0; j < RGBLIGHT_EFFECT_KNIGHT_LENGTH; j++) {
+      k = pos + j * increment;
+      if (k < 0) {
+        k = 0;
+      }
+      if (k >= RGBLED_NUM) {
+        k = RGBLED_NUM - 1;
+      }
+      if (i == k) {
+        sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val, &preled[i]);
+      }
+    }
+  }
+  if (RGBLIGHT_EFFECT_KNIGHT_OFFSET) {
+    for (i = 0; i < RGBLED_NUM; i++) {
+      cur = (i + RGBLIGHT_EFFECT_KNIGHT_OFFSET) % RGBLED_NUM;
+      led[i].r = preled[cur].r;
+      led[i].g = preled[cur].g;
+      led[i].b = preled[cur].b;
+    }
+  }
+  rgblight_set();
+  if (increment == 1) {
+    if (pos - 1 < 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
+      pos = 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH;
+      increment = -1;
+    } else {
+      pos -= 1;
+    }
+  } else {
+    if (pos + 1 > RGBLED_NUM + RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
+      pos = RGBLED_NUM + RGBLIGHT_EFFECT_KNIGHT_LENGTH - 1;
+      increment = 1;
+    } else {
+      pos += 1;
+    }
+  }
+}
+
+#endif
diff --git a/quantum/serial_link/protocol/byte_stuffer.c b/quantum/serial_link/protocol/byte_stuffer.c
index fb4c45a8d..2c87d64c2 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 2cc88beb4..97e896856 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 f418d11ce..ff795fe20 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 9a052d880..2c5d890b2 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 64b170e8c..ff49d727b 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 6c806fa93..000000000
--- 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_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, master_sends_to_master_does_nothing) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(0);
-    router_send_frame(0, (uint8_t*)&data, 4);
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, link_sends_to_other_link_does_nothing) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(1);
-    router_send_frame(2, (uint8_t*)&data, 4);
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, master_receives_on_uplink_does_nothing) {
-    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));
-
-    never_expect(transport_recv_frame);
-    activate_router(0);
-    receive_data(UP_LINK,
-        router_buffers[1].send_buffers[UP_LINK].sent_data,
-        router_buffers[1].send_buffers[UP_LINK].sent_data_size);
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-}
diff --git a/quantum/serial_link/tests/frame_router_tests.cpp b/quantum/serial_link/tests/frame_router_tests.cpp
new file mode 100644
index 000000000..2bd5bf830
--- /dev/null
+++ b/quantum/serial_link/tests/frame_router_tests.cpp
@@ -0,0 +1,229 @@
+/*
+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 "gtest/gtest.h"
+#include "gmock/gmock.h"
+#include <array>
+extern "C" {
+    #include "serial_link/protocol/transport.h"
+    #include "serial_link/protocol/byte_stuffer.h"
+    #include "serial_link/protocol/frame_router.h"
+}
+
+using testing::_;
+using testing::ElementsAreArray;
+using testing::Args;
+
+class FrameRouter : public testing::Test {
+public:
+    FrameRouter() :
+        current_router_buffer(nullptr)
+    {
+        Instance = this;
+        init_byte_stuffer();
+    }
+
+    ~FrameRouter() {
+        Instance = nullptr;
+    }
+
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        auto& buffer = current_router_buffer->send_buffers[link];
+        std::copy(data, data + size, std::back_inserter(buffer));
+    }
+
+    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]);
+        }
+    }
+
+    void activate_router(uint8_t num) {
+        current_router_buffer = router_buffers + num;
+        router_set_master(num==0);
+    }
+
+    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].data(),
+                   router_buffers[from].send_buffers[UP_LINK].size());
+       }
+       else if(to > from) {
+           receive_data(UP_LINK,
+                   router_buffers[from].send_buffers[DOWN_LINK].data(),
+                   router_buffers[from].send_buffers[DOWN_LINK].size());
+       }
+    }
+
+    MOCK_METHOD3(transport_recv_frame, void (uint8_t from, uint8_t* data, uint16_t size));
+
+    std::vector<uint8_t> received_data;
+
+    struct router_buffer {
+        std::vector<uint8_t> send_buffers[2];
+    };
+
+    router_buffer router_buffers[8];
+    router_buffer* current_router_buffer;
+
+    static FrameRouter* Instance;
+};
+
+FrameRouter* FrameRouter::Instance = nullptr;
+
+
+typedef struct {
+    std::array<uint8_t, 4> data;
+    uint8_t extra[16];
+} frame_buffer_t;
+
+
+extern "C" {
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        FrameRouter::Instance->send_data(link, data, size);
+    }
+
+
+    void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) {
+        FrameRouter::Instance->transport_recv_frame(from, data, size);
+    }
+}
+
+TEST_F(FrameRouter, master_broadcast_is_received_by_everyone) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(0);
+    router_send_frame(0xFF, (uint8_t*)&data, 4);
+    EXPECT_GT(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+    EXPECT_CALL(*this, transport_recv_frame(0, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(0, 1);
+    EXPECT_GT(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(0, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(1, 2);
+    EXPECT_GT(router_buffers[2].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[2].send_buffers[UP_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, master_send_is_received_by_targets) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(0);
+    router_send_frame((1 << 1) | (1 << 2), (uint8_t*)&data, 4);
+    EXPECT_GT(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+
+    simulate_transport(0, 1);
+    EXPECT_GT(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(0, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(1, 2);
+    EXPECT_GT(router_buffers[2].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[2].send_buffers[UP_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(0, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(2, 3);
+    EXPECT_GT(router_buffers[3].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[3].send_buffers[UP_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, first_link_sends_to_master) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(1);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    EXPECT_GT(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(1, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(1, 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, second_link_sends_to_master) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(2);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    EXPECT_GT(router_buffers[2].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[2].send_buffers[DOWN_LINK].size(), 0);
+
+    simulate_transport(2, 1);
+    EXPECT_GT(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(2, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data.data)));
+    simulate_transport(1, 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, master_sends_to_master_does_nothing) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(0);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, link_sends_to_other_link_does_nothing) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(1);
+    router_send_frame(2, (uint8_t*)&data, 4);
+    EXPECT_EQ(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+}
+
+TEST_F(FrameRouter, master_receives_on_uplink_does_nothing) {
+    frame_buffer_t data;
+    data.data = {0xAB, 0x70, 0x55, 0xBB};
+    activate_router(1);
+    router_send_frame(0, (uint8_t*)&data, 4);
+    EXPECT_GT(router_buffers[1].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[1].send_buffers[DOWN_LINK].size(), 0);
+
+    EXPECT_CALL(*this, transport_recv_frame(_, _, _))
+        .Times(0);
+    activate_router(0);
+    receive_data(UP_LINK,
+        router_buffers[1].send_buffers[UP_LINK].data(),
+        router_buffers[1].send_buffers[UP_LINK].size());
+    EXPECT_EQ(router_buffers[0].send_buffers[UP_LINK].size(), 0);
+    EXPECT_EQ(router_buffers[0].send_buffers[DOWN_LINK].size(), 0);
+}
diff --git a/quantum/serial_link/tests/frame_validator_tests.c b/quantum/serial_link/tests/frame_validator_tests.cpp
index d20947e2c..9223af83b 100644
--- a/quantum/serial_link/tests/frame_validator_tests.c
+++ b/quantum/serial_link/tests/frame_validator_tests.cpp
@@ -22,24 +22,47 @@ 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/frame_validator.c"
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+extern "C" {
+#include "serial_link/protocol/frame_validator.h"
+}
+
+using testing::_;
+using testing::ElementsAreArray;
+using testing::Args;
+
+class FrameValidator : public testing::Test {
+public:
+    FrameValidator() {
+        Instance = this;
+    }
+
+    ~FrameValidator() {
+        Instance = nullptr;
+    }
+
+    MOCK_METHOD3(route_incoming_frame, void (uint8_t link, uint8_t* data, uint16_t size));
+    MOCK_METHOD3(byte_stuffer_send_frame, void (uint8_t link, uint8_t* data, uint16_t size));
 
+    static FrameValidator* Instance;
+};
+
+FrameValidator* FrameValidator::Instance = nullptr;
+
+extern "C" {
 void route_incoming_frame(uint8_t link, uint8_t* data, uint16_t size) {
-    mock(data, size);
+    FrameValidator::Instance->route_incoming_frame(link, data, size);
 }
 
 void byte_stuffer_send_frame(uint8_t link, uint8_t* data, uint16_t size) {
-    mock(data, size);
+    FrameValidator::Instance->byte_stuffer_send_frame(link, data, size);
+}
 }
 
-Describe(FrameValidator);
-BeforeEach(FrameValidator) {}
-AfterEach(FrameValidator) {}
-
-Ensure(FrameValidator, doesnt_validate_frames_under_5_bytes) {
-    never_expect(route_incoming_frame);
+TEST_F(FrameValidator, doesnt_validate_frames_under_5_bytes) {
+    EXPECT_CALL(*this, route_incoming_frame(_, _, _))
+        .Times(0);
     uint8_t data[] = {1, 2};
     validator_recv_frame(0, 0, 1);
     validator_recv_frame(0, data, 2);
@@ -47,55 +70,46 @@ Ensure(FrameValidator, doesnt_validate_frames_under_5_bytes) {
     validator_recv_frame(0, data, 4);
 }
 
-Ensure(FrameValidator, validates_one_byte_frame_with_correct_crc) {
+TEST_F(FrameValidator, validates_one_byte_frame_with_correct_crc) {
     uint8_t data[] = {0x44, 0x04, 0x6A, 0xB3, 0xA3};
-    expect(route_incoming_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(data, 1))
-    );
+    EXPECT_CALL(*this, route_incoming_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data, 1)));
     validator_recv_frame(0, data, 5);
 }
 
-Ensure(FrameValidator, does_not_validate_one_byte_frame_with_incorrect_crc) {
+TEST_F(FrameValidator, does_not_validate_one_byte_frame_with_incorrect_crc) {
     uint8_t data[] = {0x44, 0, 0, 0, 0};
-    never_expect(route_incoming_frame);
+    EXPECT_CALL(*this, route_incoming_frame(_, _, _))
+        .Times(0);
     validator_recv_frame(1, data, 5);
 }
 
-Ensure(FrameValidator, validates_four_byte_frame_with_correct_crc) {
+TEST_F(FrameValidator, validates_four_byte_frame_with_correct_crc) {
     uint8_t data[] = {0x44, 0x10, 0xFF, 0x00, 0x74, 0x4E, 0x30, 0xBA};
-    expect(route_incoming_frame,
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(data, 4))
-    );
+    EXPECT_CALL(*this, route_incoming_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data, 4)));
     validator_recv_frame(1, data, 8);
 }
 
-Ensure(FrameValidator, validates_five_byte_frame_with_correct_crc) {
+TEST_F(FrameValidator, validates_five_byte_frame_with_correct_crc) {
     uint8_t data[] = {1, 2, 3, 4, 5, 0xF4, 0x99, 0x0B, 0x47};
-    expect(route_incoming_frame,
-        when(size, is_equal_to(5)),
-        when(data, is_equal_to_contents_of(data, 5))
-    );
+    EXPECT_CALL(*this, route_incoming_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(data, 5)));
     validator_recv_frame(0, data, 9);
 }
 
-Ensure(FrameValidator, sends_one_byte_with_correct_crc) {
+TEST_F(FrameValidator, sends_one_byte_with_correct_crc) {
     uint8_t original[] = {0x44, 0, 0, 0, 0};
     uint8_t expected[] = {0x44, 0x04, 0x6A, 0xB3, 0xA3};
-    expect(byte_stuffer_send_frame,
-        when(size, is_equal_to(sizeof(expected))),
-        when(data, is_equal_to_contents_of(expected, sizeof(expected)))
-    );
+    EXPECT_CALL(*this, byte_stuffer_send_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     validator_send_frame(0, original, 1);
 }
 
-Ensure(FrameValidator, sends_five_bytes_with_correct_crc) {
+TEST_F(FrameValidator, sends_five_bytes_with_correct_crc) {
     uint8_t original[] = {1, 2, 3, 4, 5, 0, 0, 0, 0};
     uint8_t expected[] = {1, 2, 3, 4, 5, 0xF4, 0x99, 0x0B, 0x47};
-    expect(byte_stuffer_send_frame,
-        when(size, is_equal_to(sizeof(expected))),
-        when(data, is_equal_to_contents_of(expected, sizeof(expected)))
-    );
+    EXPECT_CALL(*this, byte_stuffer_send_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     validator_send_frame(0, original, 5);
 }
diff --git a/quantum/serial_link/tests/rules.mk b/quantum/serial_link/tests/rules.mk
new file mode 100644
index 000000000..b81515bc5
--- /dev/null
+++ b/quantum/serial_link/tests/rules.mk
@@ -0,0 +1,22 @@
+serial_link_byte_stuffer_SRC :=\
+        $(SERIAL_PATH)/tests/byte_stuffer_tests.cpp \
+        $(SERIAL_PATH)/protocol/byte_stuffer.c
+
+serial_link_frame_validator_SRC := \
+        $(SERIAL_PATH)/tests/frame_validator_tests.cpp \
+        $(SERIAL_PATH)/protocol/frame_validator.c 
+
+serial_link_frame_router_SRC := \
+        $(SERIAL_PATH)/tests/frame_router_tests.cpp \
+        $(SERIAL_PATH)/protocol/byte_stuffer.c \
+        $(SERIAL_PATH)/protocol/frame_validator.c \
+        $(SERIAL_PATH)/protocol/frame_router.c
+
+serial_link_triple_buffered_object_SRC := \
+        $(SERIAL_PATH)/tests/triple_buffered_object_tests.cpp \
+        $(SERIAL_PATH)/protocol/triple_buffered_object.c 
+
+serial_link_transport_SRC := \
+        $(SERIAL_PATH)/tests/transport_tests.cpp \
+        $(SERIAL_PATH)/protocol/transport.c \
+        $(SERIAL_PATH)/protocol/triple_buffered_object.c 
diff --git a/quantum/serial_link/tests/testlist.mk b/quantum/serial_link/tests/testlist.mk
new file mode 100644
index 000000000..a80e88884
--- /dev/null
+++ b/quantum/serial_link/tests/testlist.mk
@@ -0,0 +1,6 @@
+TEST_LIST +=\
+        serial_link_byte_stuffer\
+        serial_link_frame_validator\
+        serial_link_frame_router\
+        serial_link_triple_buffered_object\
+        serial_link_transport
\ No newline at end of file
diff --git a/quantum/serial_link/tests/transport_tests.c b/quantum/serial_link/tests/transport_tests.c
deleted file mode 100644
index 358e1b9fd..000000000
--- a/quantum/serial_link/tests/transport_tests.c
+++ /dev/null
@@ -1,168 +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/transport.c"
-#include "serial_link/protocol/triple_buffered_object.c"
-
-void signal_data_written(void) {
-    mock();
-}
-
-static uint8_t sent_data[2048];
-static uint16_t sent_data_size;
-
-void router_send_frame(uint8_t destination, uint8_t* data, uint16_t size) {
-    mock(destination);
-    memcpy(sent_data + sent_data_size, data, size);
-    sent_data_size += size;
-}
-
-typedef struct {
-    uint32_t test;
-} test_object1_t;
-
-typedef struct {
-    uint32_t test1;
-    uint32_t test2;
-} test_object2_t;
-
-MASTER_TO_ALL_SLAVES_OBJECT(master_to_slave, test_object1_t);
-MASTER_TO_SINGLE_SLAVE_OBJECT(master_to_single_slave, test_object1_t);
-SLAVE_TO_MASTER_OBJECT(slave_to_master, test_object1_t);
-
-static remote_object_t* test_remote_objects[] = {
-    REMOTE_OBJECT(master_to_slave),
-    REMOTE_OBJECT(master_to_single_slave),
-    REMOTE_OBJECT(slave_to_master),
-};
-
-Describe(Transport);
-BeforeEach(Transport) {
-    add_remote_objects(test_remote_objects, sizeof(test_remote_objects) / sizeof(remote_object_t*));
-    sent_data_size = 0;
-}
-AfterEach(Transport) {}
-
-Ensure(Transport, write_to_local_signals_an_event) {
-    begin_write_master_to_slave();
-    expect(signal_data_written);
-    end_write_master_to_slave();
-    begin_write_slave_to_master();
-    expect(signal_data_written);
-    end_write_slave_to_master();
-    begin_write_master_to_single_slave(1);
-    expect(signal_data_written);
-    end_write_master_to_single_slave(1);
-}
-
-Ensure(Transport, writes_from_master_to_all_slaves) {
-    update_transport();
-    test_object1_t* obj = begin_write_master_to_slave();
-    obj->test = 5;
-    expect(signal_data_written);
-    end_write_master_to_slave();
-    expect(router_send_frame,
-            when(destination, is_equal_to(0xFF)));
-    update_transport();
-    transport_recv_frame(0, sent_data, sent_data_size);
-    test_object1_t* obj2 = read_master_to_slave();
-    assert_that(obj2, is_not_equal_to(NULL));
-    assert_that(obj2->test, is_equal_to(5));
-}
-
-Ensure(Transport, writes_from_slave_to_master) {
-    update_transport();
-    test_object1_t* obj = begin_write_slave_to_master();
-    obj->test = 7;
-    expect(signal_data_written);
-    end_write_slave_to_master();
-    expect(router_send_frame,
-            when(destination, is_equal_to(0)));
-    update_transport();
-    transport_recv_frame(3, sent_data, sent_data_size);
-    test_object1_t* obj2 = read_slave_to_master(2);
-    assert_that(read_slave_to_master(0), is_equal_to(NULL));
-    assert_that(obj2, is_not_equal_to(NULL));
-    assert_that(obj2->test, is_equal_to(7));
-}
-
-Ensure(Transport, writes_from_master_to_single_slave) {
-    update_transport();
-    test_object1_t* obj = begin_write_master_to_single_slave(3);
-    obj->test = 7;
-    expect(signal_data_written);
-    end_write_master_to_single_slave(3);
-    expect(router_send_frame,
-            when(destination, is_equal_to(4)));
-    update_transport();
-    transport_recv_frame(0, sent_data, sent_data_size);
-    test_object1_t* obj2 = read_master_to_single_slave();
-    assert_that(obj2, is_not_equal_to(NULL));
-    assert_that(obj2->test, is_equal_to(7));
-}
-
-Ensure(Transport, ignores_object_with_invalid_id) {
-    update_transport();
-    test_object1_t* obj = begin_write_master_to_single_slave(3);
-    obj->test = 7;
-    expect(signal_data_written);
-    end_write_master_to_single_slave(3);
-    expect(router_send_frame,
-            when(destination, is_equal_to(4)));
-    update_transport();
-    sent_data[sent_data_size - 1] = 44;
-    transport_recv_frame(0, sent_data, sent_data_size);
-    test_object1_t* obj2 = read_master_to_single_slave();
-    assert_that(obj2, is_equal_to(NULL));
-}
-
-Ensure(Transport, ignores_object_with_size_too_small) {
-    update_transport();
-    test_object1_t* obj = begin_write_master_to_slave();
-    obj->test = 7;
-    expect(signal_data_written);
-    end_write_master_to_slave();
-    expect(router_send_frame);
-    update_transport();
-    sent_data[sent_data_size - 2] = 0;
-    transport_recv_frame(0, sent_data, sent_data_size - 1);
-    test_object1_t* obj2 = read_master_to_slave();
-    assert_that(obj2, is_equal_to(NULL));
-}
-
-Ensure(Transport, ignores_object_with_size_too_big) {
-    update_transport();
-    test_object1_t* obj = begin_write_master_to_slave();
-    obj->test = 7;
-    expect(signal_data_written);
-    end_write_master_to_slave();
-    expect(router_send_frame);
-    update_transport();
-    sent_data[sent_data_size + 21] = 0;
-    transport_recv_frame(0, sent_data, sent_data_size + 22);
-    test_object1_t* obj2 = read_master_to_slave();
-    assert_that(obj2, is_equal_to(NULL));
-}
diff --git a/quantum/serial_link/tests/transport_tests.cpp b/quantum/serial_link/tests/transport_tests.cpp
new file mode 100644
index 000000000..21b7b165f
--- /dev/null
+++ b/quantum/serial_link/tests/transport_tests.cpp
@@ -0,0 +1,188 @@
+/*
+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 "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+using testing::_;
+using testing::ElementsAreArray;
+using testing::Args;
+
+extern "C" {
+#include "serial_link/protocol/transport.h"
+}
+
+struct test_object1 {
+    uint32_t test;
+};
+
+struct test_object2 {
+    uint32_t test1;
+    uint32_t test2;
+};
+
+MASTER_TO_ALL_SLAVES_OBJECT(master_to_slave, test_object1);
+MASTER_TO_SINGLE_SLAVE_OBJECT(master_to_single_slave, test_object1);
+SLAVE_TO_MASTER_OBJECT(slave_to_master, test_object1);
+
+static remote_object_t* test_remote_objects[] = {
+    REMOTE_OBJECT(master_to_slave),
+    REMOTE_OBJECT(master_to_single_slave),
+    REMOTE_OBJECT(slave_to_master),
+};
+
+class Transport : public testing::Test {
+public:
+    Transport() {
+        Instance = this;
+        add_remote_objects(test_remote_objects, sizeof(test_remote_objects) / sizeof(remote_object_t*));
+    }
+
+    ~Transport() {
+        Instance = nullptr;
+        reinitialize_serial_link_transport();
+    }
+
+    MOCK_METHOD0(signal_data_written, void ());
+    MOCK_METHOD1(router_send_frame, void (uint8_t destination));
+
+    void router_send_frame(uint8_t destination, uint8_t* data, uint16_t size) {
+        router_send_frame(destination);
+        std::copy(data, data + size, std::back_inserter(sent_data));
+    }
+
+    static Transport* Instance;
+
+    std::vector<uint8_t> sent_data;
+};
+
+Transport* Transport::Instance = nullptr;
+
+extern "C" {
+void signal_data_written(void) {
+    Transport::Instance->signal_data_written();
+}
+
+void router_send_frame(uint8_t destination, uint8_t* data, uint16_t size) {
+    Transport::Instance->router_send_frame(destination, data, size);
+}
+}
+
+TEST_F(Transport, write_to_local_signals_an_event) {
+    begin_write_master_to_slave();
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_slave();
+    begin_write_slave_to_master();
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_slave_to_master();
+    begin_write_master_to_single_slave(1);
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_single_slave(1);
+}
+
+TEST_F(Transport, writes_from_master_to_all_slaves) {
+    update_transport();
+    test_object1* obj = begin_write_master_to_slave();
+    obj->test = 5;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_slave();
+    EXPECT_CALL(*this, router_send_frame(0xFF));
+    update_transport();
+    transport_recv_frame(0, sent_data.data(), sent_data.size());
+    test_object1* obj2 = read_master_to_slave();
+    EXPECT_NE(obj2, nullptr);
+    EXPECT_EQ(obj2->test, 5);
+}
+
+TEST_F(Transport, writes_from_slave_to_master) {
+    update_transport();
+    test_object1* obj = begin_write_slave_to_master();
+    obj->test = 7;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_slave_to_master();
+    EXPECT_CALL(*this, router_send_frame(0));
+    update_transport();
+    transport_recv_frame(3, sent_data.data(), sent_data.size());
+    test_object1* obj2 = read_slave_to_master(2);
+    EXPECT_EQ(read_slave_to_master(0), nullptr);
+    EXPECT_NE(obj2, nullptr);
+    EXPECT_EQ(obj2->test, 7);
+}
+
+TEST_F(Transport, writes_from_master_to_single_slave) {
+    update_transport();
+    test_object1* obj = begin_write_master_to_single_slave(3);
+    obj->test = 7;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_single_slave(3);
+    EXPECT_CALL(*this, router_send_frame(4));
+    update_transport();
+    transport_recv_frame(0, sent_data.data(), sent_data.size());
+    test_object1* obj2 = read_master_to_single_slave();
+    EXPECT_NE(obj2, nullptr);
+    EXPECT_EQ(obj2->test, 7);
+}
+
+TEST_F(Transport, ignores_object_with_invalid_id) {
+    update_transport();
+    test_object1* obj = begin_write_master_to_single_slave(3);
+    obj->test = 7;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_single_slave(3);
+    EXPECT_CALL(*this, router_send_frame(4));
+    update_transport();
+    sent_data[sent_data.size() - 1] = 44;
+    transport_recv_frame(0, sent_data.data(), sent_data.size());
+    test_object1* obj2 = read_master_to_single_slave();
+    EXPECT_EQ(obj2, nullptr);
+}
+
+TEST_F(Transport, ignores_object_with_size_too_small) {
+    update_transport();
+    test_object1* obj = begin_write_master_to_slave();
+    obj->test = 7;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_slave();
+    EXPECT_CALL(*this, router_send_frame(_));
+    update_transport();
+    sent_data[sent_data.size() - 2] = 0;
+    transport_recv_frame(0, sent_data.data(), sent_data.size() - 1);
+    test_object1* obj2 = read_master_to_slave();
+    EXPECT_EQ(obj2, nullptr);
+}
+
+TEST_F(Transport, ignores_object_with_size_too_big) {
+    update_transport();
+    test_object1* obj = begin_write_master_to_slave();
+    obj->test = 7;
+    EXPECT_CALL(*this, signal_data_written());
+    end_write_master_to_slave();
+    EXPECT_CALL(*this, router_send_frame(_));
+    update_transport();
+    sent_data.resize(sent_data.size() + 22);
+    sent_data[sent_data.size() - 1] = 0;
+    transport_recv_frame(0, sent_data.data(), sent_data.size());
+    test_object1* obj2 = read_master_to_slave();
+    EXPECT_EQ(obj2, nullptr);
+}
diff --git a/quantum/serial_link/tests/triple_buffered_object_tests.c b/quantum/serial_link/tests/triple_buffered_object_tests.cpp
index 6f7c82b46..7724bbee9 100644
--- a/quantum/serial_link/tests/triple_buffered_object_tests.c
+++ b/quantum/serial_link/tests/triple_buffered_object_tests.cpp
@@ -22,53 +22,55 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 
-#include <cgreen/cgreen.h>
-#include "serial_link/protocol/triple_buffered_object.c"
+#include "gtest/gtest.h"
+extern "C" {
+#include "serial_link/protocol/triple_buffered_object.h"
+}
 
-typedef struct {
+struct test_object{
     uint8_t state;
     uint32_t buffer[3];
-}test_object_t;
-
-test_object_t test_object;
+};
 
-Describe(TripleBufferedObject);
-BeforeEach(TripleBufferedObject) {
-    triple_buffer_init((triple_buffer_object_t*)&test_object);
-}
-AfterEach(TripleBufferedObject) {}
+test_object test_object;
 
+class TripleBufferedObject : public testing::Test {
+public:
+    TripleBufferedObject() {
+        triple_buffer_init((triple_buffer_object_t*)&test_object);
+    }
+};
 
-Ensure(TripleBufferedObject, writes_and_reads_object) {
+TEST_F(TripleBufferedObject, writes_and_reads_object) {
     *triple_buffer_begin_write(&test_object) = 0x3456ABCC;
     triple_buffer_end_write(&test_object);
-    assert_that(*triple_buffer_read(&test_object), is_equal_to(0x3456ABCC));
+    EXPECT_EQ(*triple_buffer_read(&test_object), 0x3456ABCC);
 }
 
-Ensure(TripleBufferedObject, does_not_read_empty) {
-    assert_that(triple_buffer_read(&test_object), is_equal_to(NULL));
+TEST_F(TripleBufferedObject, does_not_read_empty) {
+    EXPECT_EQ(triple_buffer_read(&test_object), nullptr);
 }
 
-Ensure(TripleBufferedObject, writes_twice_and_reads_object) {
+TEST_F(TripleBufferedObject, writes_twice_and_reads_object) {
     *triple_buffer_begin_write(&test_object) = 0x3456ABCC;
     triple_buffer_end_write(&test_object);
     *triple_buffer_begin_write(&test_object) = 0x44778899;
     triple_buffer_end_write(&test_object);
-    assert_that(*triple_buffer_read(&test_object), is_equal_to(0x44778899));
+    EXPECT_EQ(*triple_buffer_read(&test_object), 0x44778899);
 }
 
-Ensure(TripleBufferedObject, performs_another_write_in_the_middle_of_read) {
+TEST_F(TripleBufferedObject, performs_another_write_in_the_middle_of_read) {
     *triple_buffer_begin_write(&test_object) = 1;
     triple_buffer_end_write(&test_object);
     uint32_t* read = triple_buffer_read(&test_object);
     *triple_buffer_begin_write(&test_object) = 2;
     triple_buffer_end_write(&test_object);
-    assert_that(*read, is_equal_to(1));
-    assert_that(*triple_buffer_read(&test_object), is_equal_to(2));
-    assert_that(triple_buffer_read(&test_object), is_equal_to(NULL));
+    EXPECT_EQ(*read, 1);
+    EXPECT_EQ(*triple_buffer_read(&test_object), 2);
+    EXPECT_EQ(triple_buffer_read(&test_object), nullptr);
 }
 
-Ensure(TripleBufferedObject, performs_two_writes_in_the_middle_of_read) {
+TEST_F(TripleBufferedObject, performs_two_writes_in_the_middle_of_read) {
     *triple_buffer_begin_write(&test_object) = 1;
     triple_buffer_end_write(&test_object);
     uint32_t* read = triple_buffer_read(&test_object);
@@ -76,7 +78,7 @@ Ensure(TripleBufferedObject, performs_two_writes_in_the_middle_of_read) {
     triple_buffer_end_write(&test_object);
     *triple_buffer_begin_write(&test_object) = 3;
     triple_buffer_end_write(&test_object);
-    assert_that(*read, is_equal_to(1));
-    assert_that(*triple_buffer_read(&test_object), is_equal_to(3));
-    assert_that(triple_buffer_read(&test_object), is_equal_to(NULL));
+    EXPECT_EQ(*read, 1);
+    EXPECT_EQ(*triple_buffer_read(&test_object), 3);
+    EXPECT_EQ(triple_buffer_read(&test_object), nullptr);
 }
diff --git a/quantum/template/Makefile b/quantum/template/Makefile
index 3f6d133c9..4e2a6f00f 100644
--- a/quantum/template/Makefile
+++ b/quantum/template/Makefile
@@ -1,75 +1,3 @@
-
-
-# MCU name
-#MCU = at90usb1287
-MCU = atmega32u4
-
-# Processor frequency.
-#     This will define a symbol, F_CPU, in all source code files equal to the
-#     processor frequency in Hz. You can then use this symbol in your source code to
-#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done
-#     automatically to create a 32-bit value in your source code.
-#
-#     This will be an integer division of F_USB below, as it is sourced by
-#     F_USB after it has run through any CPU prescalers. Note that this value
-#     does not *change* the processor frequency - it should merely be updated to
-#     reflect the processor speed set externally so that the code can use accurate
-#     software delays.
-F_CPU = 16000000
-
-
-#
-# LUFA specific
-#
-# Target architecture (see library "Board Types" documentation).
-ARCH = AVR8
-
-# Input clock frequency.
-#     This will define a symbol, F_USB, in all source code files equal to the
-#     input clock frequency (before any prescaling is performed) in Hz. This value may
-#     differ from F_CPU if prescaling is used on the latter, and is required as the
-#     raw input clock is fed directly to the PLL sections of the AVR for high speed
-#     clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL'
-#     at the end, this will be done automatically to create a 32-bit value in your
-#     source code.
-#
-#     If no clock division is performed on the input clock inside the AVR (via the
-#     CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU.
-F_USB = $(F_CPU)
-
-# Interrupt driven control endpoint task(+60)
-OPT_DEFS += -DINTERRUPT_CONTROL_ENDPOINT
-
-
-# Boot Section Size in *bytes*
-#   Teensy halfKay   512
-#   Teensy++ halfKay 1024
-#   Atmel DFU loader 4096
-#   LUFA bootloader  4096
-#   USBaspLoader     2048
-OPT_DEFS += -DBOOTLOADER_SIZE=512
-
-
-# Build Options
-#   change yes to no to disable
-#
-BOOTMAGIC_ENABLE ?= no      # Virtual DIP switch configuration(+1000)
-MOUSEKEY_ENABLE ?= yes       # Mouse keys(+4700)
-EXTRAKEY_ENABLE ?= yes       # Audio control and System control(+450)
-CONSOLE_ENABLE ?= yes        # Console for debug(+400)
-COMMAND_ENABLE ?= yes        # Commands for debug and configuration
-# Do not enable SLEEP_LED_ENABLE. it uses the same timer as BACKLIGHT_ENABLE
-SLEEP_LED_ENABLE ?= no       # Breathing sleep LED during USB suspend
-# if this doesn't work, see here: https://github.com/tmk/tmk_keyboard/wiki/FAQ#nkro-doesnt-work
-NKRO_ENABLE ?= no            # USB Nkey Rollover
-BACKLIGHT_ENABLE ?= no       # Enable keyboard backlight functionality on B7 by default
-MIDI_ENABLE ?= no            # MIDI controls
-UNICODE_ENABLE ?= no         # Unicode
-BLUETOOTH_ENABLE ?= no       # Enable Bluetooth with the Adafruit EZ-Key HID
-AUDIO_ENABLE ?= no           # Audio output on port C6
-
-ifndef QUANTUM_DIR
+ifndef MAKEFILE_INCLUDED
         include ../../Makefile
-endif
-
-
+endif
\ No newline at end of file
diff --git a/quantum/template/readme.md b/quantum/template/readme.md
index b2fb4dd98..b16f4cd76 100644
--- a/quantum/template/readme.md
+++ b/quantum/template/readme.md
@@ -3,7 +3,7 @@
 
 ## Quantum MK Firmware
 
-For the full Quantum feature list, see [the parent readme.md](/doc/readme.md).
+For the full Quantum feature list, see [the parent readme](/).
 
 ## Building
 
@@ -13,16 +13,16 @@ Depending on which keymap you would like to use, you will have to compile slight
 
 ### Default
 
-To build with the default keymap, simply run `make`.
+To build with the default keymap, simply run `make default`.
 
 ### Other Keymaps
 
 Several version of keymap are available in advance but you are recommended to define your favorite layout yourself. To define your own keymap create a folder with the name of your keymap in the keymaps folder, and see keymap documentation (you can find in top readme.md) and existant keymap files.
 
-To build the firmware binary hex file with a keymap just do `make` with `keymap` option like:
+To build the firmware binary hex file with a keymap just do `make` with a keymap like this:
 
 ```
-$ make keymap=[default|jack|<name>]
+$ make [default|jack|<name>]
 ```
 
-Keymaps follow the format **__keymap.c__** and are stored in folders in the `keymaps` folder, eg `keymaps/my_keymap/`
\ No newline at end of file
+Keymaps follow the format **__\<name\>.c__** and are stored in the `keymaps` folder.
diff --git a/quantum/template/rules.mk b/quantum/template/rules.mk
new file mode 100644
index 000000000..55898147d
--- /dev/null
+++ b/quantum/template/rules.mk
@@ -0,0 +1,67 @@
+# MCU name
+#MCU = at90usb1287
+MCU = atmega32u4
+
+# Processor frequency.
+#     This will define a symbol, F_CPU, in all source code files equal to the
+#     processor frequency in Hz. You can then use this symbol in your source code to
+#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done
+#     automatically to create a 32-bit value in your source code.
+#
+#     This will be an integer division of F_USB below, as it is sourced by
+#     F_USB after it has run through any CPU prescalers. Note that this value
+#     does not *change* the processor frequency - it should merely be updated to
+#     reflect the processor speed set externally so that the code can use accurate
+#     software delays.
+F_CPU = 16000000
+
+
+#
+# LUFA specific
+#
+# Target architecture (see library "Board Types" documentation).
+ARCH = AVR8
+
+# Input clock frequency.
+#     This will define a symbol, F_USB, in all source code files equal to the
+#     input clock frequency (before any prescaling is performed) in Hz. This value may
+#     differ from F_CPU if prescaling is used on the latter, and is required as the
+#     raw input clock is fed directly to the PLL sections of the AVR for high speed
+#     clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL'
+#     at the end, this will be done automatically to create a 32-bit value in your
+#     source code.
+#
+#     If no clock division is performed on the input clock inside the AVR (via the
+#     CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU.
+F_USB = $(F_CPU)
+
+# Interrupt driven control endpoint task(+60)
+OPT_DEFS += -DINTERRUPT_CONTROL_ENDPOINT
+
+
+# Boot Section Size in *bytes*
+#   Teensy halfKay   512
+#   Teensy++ halfKay 1024
+#   Atmel DFU loader 4096
+#   LUFA bootloader  4096
+#   USBaspLoader     2048
+OPT_DEFS += -DBOOTLOADER_SIZE=512
+
+
+# Build Options
+#   change yes to no to disable
+#
+BOOTMAGIC_ENABLE ?= no      # Virtual DIP switch configuration(+1000)
+MOUSEKEY_ENABLE ?= yes       # Mouse keys(+4700)
+EXTRAKEY_ENABLE ?= yes       # Audio control and System control(+450)
+CONSOLE_ENABLE ?= yes        # Console for debug(+400)
+COMMAND_ENABLE ?= yes        # Commands for debug and configuration
+# Do not enable SLEEP_LED_ENABLE. it uses the same timer as BACKLIGHT_ENABLE
+SLEEP_LED_ENABLE ?= no       # Breathing sleep LED during USB suspend
+# if this doesn't work, see here: https://github.com/tmk/tmk_keyboard/wiki/FAQ#nkro-doesnt-work
+NKRO_ENABLE ?= no            # USB Nkey Rollover
+BACKLIGHT_ENABLE ?= no       # Enable keyboard backlight functionality on B7 by default
+MIDI_ENABLE ?= no            # MIDI controls
+UNICODE_ENABLE ?= no         # Unicode
+BLUETOOTH_ENABLE ?= no       # Enable Bluetooth with the Adafruit EZ-Key HID
+AUDIO_ENABLE ?= no           # Audio output on port C6