Merge https://github.com/jackhumbert/qmk_firmware

36 files changed, 1821 insertions, 949 deletions

diff --git a/quantum/config_common.h b/quantum/config_common.h
index 09a4fe7010..4d3939dae1 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
@@ -67,53 +76,7 @@
         } while(0)
 #   else
 #       error "USART configuration is needed."
+#   endif
 #endif
 
-// I'm fairly sure these aren't needed, but oh well - Jack
-
-/*
- * PS/2 Interrupt configuration
- */
-#ifdef PS2_USE_INT
-/* uses INT1 for clock line(ATMega32U4) */
-#define PS2_CLOCK_PORT  PORTD
-#define PS2_CLOCK_PIN   PIND
-#define PS2_CLOCK_DDR   DDRD
-#define PS2_CLOCK_BIT   1
-
-#define PS2_DATA_PORT   PORTD
-#define PS2_DATA_PIN    PIND
-#define PS2_DATA_DDR    DDRD
-#define PS2_DATA_BIT    0
-
-#define PS2_INT_INIT()  do {    \
-    EICRA |= ((1<<ISC11) |      \
-              (0<<ISC10));      \
-} while (0)
-#define PS2_INT_ON()  do {      \
-    EIMSK |= (1<<INT1);         \
-} while (0)
-#define PS2_INT_OFF() do {      \
-    EIMSK &= ~(1<<INT1);        \
-} while (0)
-#define PS2_INT_VECT    INT1_vect
-#endif
-
-/*
- * PS/2 Busywait configuration
- */
-#ifdef PS2_USE_BUSYWAIT
-#define PS2_CLOCK_PORT  PORTD
-#define PS2_CLOCK_PIN   PIND
-#define PS2_CLOCK_DDR   DDRD
-#define PS2_CLOCK_BIT   1
-
-#define PS2_DATA_PORT   PORTD
-#define PS2_DATA_PIN    PIND
-#define PS2_DATA_DDR    DDRD
-#define PS2_DATA_BIT    0
-#endif
-
-#endif
-
-#endif
+#endif
\ No newline at end of file
diff --git a/quantum/dynamic_macro.h b/quantum/dynamic_macro.h
index a3ad61bc7e..e6dbc5b9c1 100644
--- a/quantum/dynamic_macro.h
+++ b/quantum/dynamic_macro.h
@@ -8,8 +8,13 @@
 /* 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
+ * intended behavior.
+ *
+ * Usually it should be fine to set the macro size to at least 256 but
+ * there have been reports of it being too much in some users' cases,
+ * so 128 is considered a safe default.
+ */
+#define DYNAMIC_MACRO_SIZE 128
 #endif
 
 /* DYNAMIC_MACRO_RANGE must be set as the last element of user's
diff --git a/quantum/keycode_config.h b/quantum/keycode_config.h
index 6216eefc90..c15b0d32f8 100644
--- a/quantum/keycode_config.h
+++ b/quantum/keycode_config.h
@@ -1,6 +1,9 @@
 #include "eeconfig.h"
 #include "keycode.h"
 
+#ifndef KEYCODE_CONFIG_H
+#define KEYCODE_CONFIG_H
+
 uint16_t keycode_config(uint16_t keycode);
 
 /* NOTE: Not portable. Bit field order depends on implementation */
@@ -19,3 +22,5 @@ typedef union {
 } keymap_config_t;
 
 extern keymap_config_t keymap_config;
+
+#endif /* KEYCODE_CONFIG_H */
diff --git a/quantum/keymap.h b/quantum/keymap.h
index f2d94d75c3..a01bbfbd14 100644
--- a/quantum/keymap.h
+++ b/quantum/keymap.h
@@ -84,6 +84,10 @@ enum quantum_keycodes {
     QK_MOD_TAP_MAX        = 0x6FFF,
     QK_TAP_DANCE          = 0x7100,
     QK_TAP_DANCE_MAX      = 0x71FF,
+#ifdef UNICODEMAP_ENABLE
+    QK_UNICODE_MAP        = 0x7800,
+    QK_UNICODE_MAP_MAX    = 0x7FFF,
+#endif
 #ifdef UNICODE_ENABLE
     QK_UNICODE            = 0x8000,
     QK_UNICODE_MAX        = 0xFFFF,
@@ -111,6 +115,7 @@ enum quantum_keycodes {
     MAGIC_UNSWAP_BACKSLASH_BACKSPACE,
     MAGIC_UNHOST_NKRO,
     MAGIC_UNSWAP_ALT_GUI,
+    MAGIC_TOGGLE_NKRO,
 
     // Leader key
 #ifndef DISABLE_LEADER
@@ -190,6 +195,7 @@ enum quantum_keycodes {
 #define HYPR(kc) (kc | QK_LCTL | QK_LSFT | QK_LALT | QK_LGUI)
 #define MEH(kc)  (kc | QK_LCTL | QK_LSFT | QK_LALT)
 #define LCAG(kc) (kc | QK_LCTL | QK_LALT | QK_LGUI)
+#define ALTG(kc) (kc | QK_RCTL | QK_RALT)
 
 #define MOD_HYPR 0xf
 #define MOD_MEH 0x7
@@ -294,7 +300,10 @@ enum quantum_keycodes {
 // ON_PRESS    = 1
 // ON_RELEASE  = 2
 // Unless you have a good reason not to do so, prefer  ON_PRESS (1) as your default.
-#define TO(layer, when) (layer | QK_TO | (when << 0x4))
+// In fact, we changed it to assume ON_PRESS for sanity/simplicity. If needed, you can add your own
+// keycode modeled after the old version, kept below for this.
+/* #define TO(layer, when) (layer | QK_TO | (when << 0x4)) */
+#define TO(layer) (layer | QK_TO | (ON_PRESS << 0x4))
 
 // Momentary switch layer - 256 layer max
 #define MO(layer) (layer | QK_MOMENTARY)
@@ -334,5 +343,8 @@ enum quantum_keycodes {
     #define UC(n) UNICODE(n)
 #endif
 
+#ifdef UNICODEMAP_ENABLE
+    #define X(n) (n | QK_UNICODE_MAP)
+#endif
 
 #endif
diff --git a/quantum/keymap_extras/keymap_br_abnt2.h b/quantum/keymap_extras/keymap_br_abnt2.h
new file mode 100644
index 0000000000..0df177721d
--- /dev/null
+++ b/quantum/keymap_extras/keymap_br_abnt2.h
@@ -0,0 +1,58 @@
+#ifndef KEYMAP_BR_ABNT2_H
+#define KEYMAP_BR_ABNT2_H
+
+#include "keymap_common.h"
+
+/* Scan codes for the Brazilian ABNT2 keyboard layout */
+
+#define BR_CCDL KC_SCLN      //  Ç   same scancode as ;: on US layout
+#define BR_SCLN KC_SLSH      //  ;:  same scancode as /? on US layout
+#define BR_QUOT KC_GRV       //  '"  same scancode as `~ on US layout
+#define BR_TILD KC_QUOT      //  ~^  dead keys, same scancode as '" on US layout
+#define BR_ACUT KC_LBRC      //  ´`  dead keys, same scancode as [{ on US layout
+#define BR_LBRC KC_RBRC      //  [{  same scancode as ]} on US layout
+#define BR_RBRC KC_BSLS      //  ]}  same scancode as \| on US layout
+#define BR_BSLS KC_NUBS      //  \|  uses the non-US hash scancode (#~, sometimes §±)
+#define BR_SLSH KC_INT1      //  /?  uses the INTL1 scancode
+
+#define BR_COLN LSFT(BR_SCLN)   // shifted :
+#define BR_DQT  LSFT(BR_QUOT)   // shifted "
+#define BR_CIRC LSFT(BR_TILD)   // shifted ^ (dead key)
+#define BR_GRAV LSFT(BR_ACUT)   // shifted ` (dead key)
+#define BR_LCBR LSFT(BR_LBRC)   // shifted {
+#define BR_RCBR LSFT(BR_RBRC)   // shifted }
+#define BR_PIPE LSFT(BR_BSLS)   // shifted |
+#define BR_QUES LSFT(BR_SLSH)   // shifted ?
+#define BR_TRMA LSFT(KC_6)      // shifted ¨ (dead key - trema accent)
+
+// On the ABNT2 the keypad comma and the keypad dot scancodes are switched
+// (presumably because in Brazil comma is used as the decimal separator)
+#define BR_KPDT KC_KP_COMMA  //  keypad .
+#define BR_KPCM KC_KP_DOT    //  keypad ,
+
+#define BR_1UP    LALT(KC_1)      // 1 superscript                    ¹   alt+1
+#define BR_2UP    LALT(KC_2)      // 2 superscript                    ²   alt+2
+#define BR_3UP    LALT(KC_3)      // 3 superscript                    ³   alt+3
+#define BR_PND    LALT(KC_4)      // Pound sign                       £   alt+4
+#define BR_CENT   LALT(KC_5)      // Cent sign                        ¢   alt+5
+#define BR_NOT    LALT(KC_6)      // Not sign                         ¬   alt+6
+#define BR_SECT   LALT(KC_EQL)    // Section sign                     §   alt+=
+#define BR_FORD   LALT(BR_LBRC)   // Feminine Ordinal Sign            ª   alt+[
+#define BR_MORD   LALT(BR_RBRC)   // Masculine Ordinal Sign           º   alt+]
+#define BR_DGRE   LALT(BR_SLSH)   // Degree sign                      °   alt+/
+
+#define BR_EURO   LALT(KC_E)      // Euro sign                        €   alt+e
+#define BR_NDTD   LALT(BR_TILD)   // Non-dead key tilde               ~   alt+~
+#define BR_NDAC   LALT(BR_ACUT)   // Non-dead key acute accent        ´   alt+´
+#define BR_NDGV   LALT(BR_QUOT)   // Non-dead key grave accent        `   alt+'
+#define BR_NDCR   LALT(BR_CIRC)   // Non-dead key circumflex accent   ^   alt+^ (alt+shift+~)
+#define BR_NDTR   LALT(BR_TRMA)   // Non-dead key trema accent        ¨   alt+¨ (alt+shift+6)
+
+// For 101-key keyboard layouts, the ABNT2 layout allows
+// the slash and question mark to be typed using alt+q and alt+w.
+// The shortcuts are provided here for completeness' sake,
+// but it's recommended to use BR_SLSH and BR_QUES instead
+#define BR_ASLS   LALT(KC_Q)
+#define BR_AQST   LALT(KC_W)
+
+#endif
diff --git a/quantum/keymap_extras/keymap_canadian_multilingual.h b/quantum/keymap_extras/keymap_canadian_multilingual.h
new file mode 100644
index 0000000000..0bc20c7b9c
--- /dev/null
+++ b/quantum/keymap_extras/keymap_canadian_multilingual.h
@@ -0,0 +1,255 @@
+#ifndef KEYMAP_CANADIAN_MULTILINGUAG_H
+#define KEYMAP_CANADIAN_MULTILINGUAG_H
+
+#include "keymap.h"
+
+// Alt gr
+#ifndef ALTGR
+#define ALTGR(kc)   RALT(kc)
+#endif
+#ifndef ALGR
+#define ALGR(kc)    ALTGR(kc)
+#endif
+
+#define CSA_ALTGR   KC_RALT
+#define CSA_ALGR    CSA_ALTGR
+
+#ifndef GR2A
+#define GR2A(kc)    RCTL(kc)
+#endif
+
+// Normal characters
+// First row
+#define CSA_SLASH   KC_GRV      // /
+#define CSA_SLSH    CSA_SLASH
+
+// Second row
+#define CSA_DEAD_CIRCUMFLEX     KC_LBRACKET         // dead ^
+#define CSA_DCRC                CSA_DEAD_CIRCUMFLEX
+#define CSA_C_CEDILLA           KC_RBRACKET         // Ç
+#define CSA_CCED                CSA_C_CEDILLA
+
+// Third row
+#define CSA_E_GRAVE     KC_QUOT     // è
+#define CSA_EGRV        CSA_E_GRAVE
+#define CSA_A_GRAVE     KC_BSLASH   // à
+#define CSA_AGRV        CSA_A_GRAVE
+
+// Fourth row
+#define CSA_U_GRAVE     KC_NONUS_BSLASH     // ù
+#define CSA_UGRV        CSA_U_GRAVE
+#define CSA_E_ACUTE     KC_SLSH             // é
+#define CSA_ECUT        CSA_E_ACUTE
+
+// Shifted characters
+// First row
+#define CSA_BACKSLASH   LSFT(CSA_SLASH) /* \ */
+#define CSA_BSLS        CSA_BACKSLASH
+#define CSA_QUESTION    LSFT(KC_6)      // ?
+#define CSA_QEST        CSA_QUESTION
+
+// Second row
+#define CSA_DEAD_TREMA  LSFT(CSA_DEAD_CIRCUMFLEX)    // dead trema/umlaut/diaresis for ä ë ï ö ü
+#define CSA_DTRM        CSA_DEAD_TREMA
+
+// Third row
+// all same as US-QWERTY, or capitalised character of the non-shifted key
+
+// Fourth row
+#define CSA_APOSTROPHE      LSFT(KC_COMMA)  // '
+#define CSA_APOS            CSA_APOSTROPHE
+#define CSA_DOUBLE_QUOTE    LSFT(KC_DOT)    // "
+#define CSA_DQOT            CSA_DOUBLE_QUOTE
+
+// Alt Gr-ed characters
+// First row
+#define CSA_PIPE                ALTGR(CSA_SLASH)        // |
+#define CSA_CURRENCY            ALTGR(KC_4)             // ¤
+#define CSA_CURR                CSA_CURRENCY
+#define CSA_LEFT_CURLY_BRACE    ALTGR(KC_7)             // {
+#define CSA_LCBR                CSA_LEFT_CURLY_BRACE
+#define CSA_RIGHT_CURLY_BRACE   ALTGR(KC_8)             // }
+#define CSA_RCBR                CSA_RIGHT_CURLY_BRACE
+#define CSA_LBRACKET            ALTGR(KC_9)             // [
+#define CSA_LBRC                CSA_LBRACKET
+#define CSA_RBRACKET            ALTGR(KC_0)             // ]
+#define CSA_RBRC                CSA_RBRACKET
+#define CSA_NEGATION            ALTGR(KC_EQUAL)         // ¬
+#define CSA_NEGT                CSA_NEGATION
+
+// Second row
+// euro symbol not available on Linux? (X.org)
+#define CSA_EURO        ALTGR(KC_E)                 // €
+#define CSA_DEAD_GRAVE  ALTGR(CSA_DEAD_CIRCUMFLEX)
+#define CSA_DGRV        CSA_DEAD_GRAVE              // dead `
+#define CSA_DEAD_TILDE  ALTGR(CSA_C_CEDILLA)        // ~
+#define CSA_DTLD        CSA_DEAD_TILDE
+
+// Third row
+#define CSA_DEGREE  ALTGR(KC_SCOLON)    // °
+#define CSA_DEGR    CSA_DEGREE
+
+// Fourth row
+#define CSA_LEFT_GUILLEMET      ALTGR(KC_Z)         // «
+#define CSA_LGIL                CSA_LEFT_GUILLEMET
+#define CSA_RIGHT_GUILLEMET     ALTGR(KC_X)         // »
+#define CSA_RGIL                CSA_RIGHT_GUILLEMET
+#define CSA_LESS                ALTGR(KC_COMMA)     // <
+#define CSA_GREATER             ALTGR(KC_DOT)       // >
+#define CSA_GRTR                CSA_GREATER
+
+// Space bar
+#define CSA_NON_BREAKING_SPACE  ALTGR(KC_SPACE)
+#define CSA_NBSP                CSA_NON_BREAKING_SPACE
+
+// GR2A-ed characters
+// First row
+#define CSA_SUPERSCRIPT_ONE     GR2A(KC_1)  // ¹
+#define CSA_SUP1                CSA_SUPERSCRIPT_ONE
+#define CSA_SUPERSCRIPT_TWO     GR2A(KC_2)  // ²
+#define CSA_SUP2                CSA_SUPERSCRIPT_TWO
+#define CSA_SUPERSCRIPT_THREE   GR2A(KC_3)  // ³
+#define CSA_SUP3                CSA_SUPERSCRIPT_THREE
+#define CSA_ONE_QUARTER         GR2A(KC_4)  // ¼
+#define CSA_1QRT                CSA_ONE_QUARTER
+#define CSA_ONE_HALF            GR2A(KC_5)  // ½
+#define CSA_1HLF                CSA_ONE_HALF
+#define CSA_THREE_QUARTERS      GR2A(KC_6)  // ¾
+#define CSA_3QRT                CSA_THREE_QUARTERS
+// nothing on 7-0 and -
+#define CSA_DEAD_CEDILLA        GR2A(KC_EQUAL)  // dead ¸
+#define CSA_DCED                CSA_DEAD_CEDILLA
+
+// Second row
+#define CSA_OMEGA           GR2A(KC_Q)  // ω
+#define CSA_OMEG            CSA_OMEGA
+#define CSA_L_STROKE        GR2A(KC_W)  // ł
+#define CSA_LSTK            CSA_L_STROKE
+#define CSA_OE_LIGATURE     GR2A(KC_E)  // œ
+#define CSA_OE              CSA_OE_LIGATURE
+#define CSA_PARAGRAPH       GR2A(KC_R)  // ¶
+#define CSA_PARG            CSA_PARAGRAPH
+#define CSA_T_STROKE        GR2A(KC_T)  // ŧ
+#define CSA_LEFT_ARROW      GR2A(KC_Y)  // ←
+#define CSA_LARW            CSA_LEFT_ARROW
+#define CSA_DOWN_ARROW      GR2A(KC_U)  // ↓
+#define CSA_DARW            CSA_DOWN_ARROW
+#define CSA_RIGHT_ARROW     GR2A(KC_I)  // →
+#define CSA_RARW            CSA_RIGHT_ARROW
+#define CSA_O_STROKE        GR2A(KC_O)  // ø
+#define CSA_OSTK            CSA_O_STROKE
+#define CSA_THORN           GR2A(KC_P)  // þ
+#define CSA_THRN            CSA_THORN
+// nothing on ^
+#define CSA_TILDE           GR2A(CSA_C_CEDILLA)  // dead ~
+#define CSA_TILD            CSA_TILDE
+
+// Third row
+#define CSA_AE_LIGATURE     GR2A(KC_A)      // æ
+#define CSA_AE              CSA_AE_LIGATURE
+#define CSA_SHARP_S         GR2A(KC_S)      // ß
+#define CSA_SRPS            CSA_SHARP_S
+#define CSA_ETH             GR2A(KC_D)      // ð
+// nothing on F
+#define CSA_ENG             GR2A(KC_G)      // ŋ
+#define CSA_H_SRTOKE        GR2A(KC_H)      // ħ
+#define CSA_HSTK            CSA_H_SRTOKE
+#define CSA_IJ_LIGATURE     GR2A(KC_J)      // ij
+#define CSA_IJ              CSA_IJ_LIGATURE
+#define CSA_KRA             GR2A(KC_K)      // ĸ
+#define CSA_L_FLOWN_DOT     GR2A(KC_L)      // ŀ
+#define CSA_LFLD            CSA_L_FLOWN_DOT
+#define CSA_DEAD_ACUTE      GR2A(KC_SCLN)   // dead acute accent
+#define CSA_DACT            CSA_DEAD_ACUTE
+// nothing on È & À
+
+// Fourth row
+#define CSA_CENT                GR2A(KC_C)  // ¢
+#define CSA_LEFT_DOUBLE_QUOTE   GR2A(KC_V)  // “
+#define CSA_LDQT                CSA_LEFT_DOUBLE_QUOTE
+#define CSA_RIGHT_DOUBLE_QUOTE  GR2A(KC_B)  // ”
+#define CSA_RDQT                CSA_RIGHT_DOUBLE_QUOTE
+#define CSA_N_APOSTROPHE        GR2A(KC_N)  // ʼn (deprecated unicode codepoint)
+#define CSA_NAPO                CSA_N_APOSTROPHE
+#define CSA_MU                  GR2A(KC_M)  // μ
+#define CSA_HORIZONTAL_BAR      GR2A(KC_COMMA)  // ―
+#define CSA_HZBR                CSA_HORIZONTAL_BAR
+#define CSA_DEAD_DOT_ABOVE      GR2A(KC_DOT)    // dead ˙
+#define CSA_DDTA                CSA_DEAD_DOT_ABOVE
+
+// GR2A-shifted characters (different from capitalised GR2A-ed characters)
+// First row
+#define CSA_SOFT_HYPHEN         GR2A(LSFT(CSA_SLASH))   // soft-hyphen, appears as a hyphen in wrapped word
+#define CSA_SHYP                CSA_SOFT_HYPHEN
+#define CSA_INVERTED_EXCLAIM    GR2A(KC_EXCLAIM)    // ¡
+#define CSA_IXLM                CSA_INVERTED_EXCLAIM
+// nothing on 2
+#define CSA_POUND               GR2A(LSFT(KC_3))    // £
+#define CSA_GBP                 CSA_POUND_SIGN
+// already on ALTGR(KC_E)
+#define CSA_EURO_BIS            GR2A(LSFT(KC_4))    // €
+#define CSA_EURB                CSA_EURO_BIS
+#define CSA_THREE_EIGHTHS       GR2A(LSFT(KC_5))    // ⅜
+#define CSA_3ON8                CSA_THREE_EIGHTHS
+#define CSA_FIVE_EIGHTHS        GR2A(LSFT(KC_6))    // ⅝
+#define CSA_5ON8                CSA_FIVE_EIGHTHS
+#define CSA_SEVEN_EIGHTHS       GR2A(LSFT(KC_7))    // ⅞
+#define CSA_7ON8                CSA_SEVEN_EIGHTHS
+#define CSA_TRADEMARK           GR2A(LSFT(KC_8))    // ™
+#define CSA_TM                  CSA_TRADEMARK
+#define CSA_PLUS_MINUS          GR2A(LSFT(KC_9))    // ±
+#define CSA_PSMS                CSA_PLUS_MINUS
+// nothing on 0
+#define CSA_INVERTED_QUESTION   GR2A(LSFT(KC_MINUS))    // ¿
+#define CSA_IQST                CSA_INVERTED_QUESTION
+#define CSA_DEAD_OGONEK         GR2A(LSFT(KC_EQUAL))    // dead ˛
+#define CSA_DOGO                CSA_DEAD_OGONEK
+
+// Second row
+#define CSA_REGISTERED_TRADEMARK    GR2A(LSFT(KC_R))        // ®
+#define CSA_RTM                     CSA_REGISTERED_TRADEMARK
+#define CSA_YEN                     GR2A(LSFT(KC_Y))        // ¥
+#define CSA_YUAN                    CSA_YEN
+#define CSA_UP_ARROW                LSFT(CSA_DOWN_ARROW)    // ↑
+#define CSA_DOTLESS_I               GR2A(LSFT(KC_I))        // ı
+#define CSA_DLSI                    CSA_DOTLESS_I
+#define CSA_DEAD_RING               GR2A(LSFT(CSA_DCRC))    // dead °
+#define CSA_DRNG                    CSA_DEAD_RING
+#define CSA_DEAD_MACRON             GR2A(LSFT(CSA_C_CEDILLA))   // dead ¯
+#define CSA_DMCR                    CSA_DEAD_MACRON
+
+// Third row
+#define CSA_SECTION                 GR2A(LSFT(KC_S))        // §
+#define CSA_SECT                    CSA_SECTION
+#define CSA_ORDINAL_INDICATOR_A     GR2A(LSFT(KC_F))        // ª
+#define CSA_ORDA                    CSA_ORDINAL_INDICATOR_A
+#define CSA_DEAD_DOUBLE_ACUTE       LSFT(CSA_DEAD_ACUTE)    // ˝
+#define CSA_DDCT                    CSA_DEAD_DOUBLE_ACUTE
+#define CSA_DEAD_CARON              GR2A(LSFT(CSA_E_GRAVE)) // dead ˇ
+#define CSA_DCAR                    CSA_DEAD_CARON
+#define CSA_DEAD_BREVE              GR2A(LSFT(CSA_A_GRAVE)) // dead ˘
+#define CSA_DBRV                    CSA_DEAD_BREVE
+
+// Fourth row
+#define CSA_BROKEN_PIPE         GR2A(LSFT(CSA_U_GRAVE)) // ¦
+#define CSA_BPIP                CSA_BROKEN_PIPE
+#define CSA_COPYRIGHT           GR2A(LSFT(KC_C))        // ©
+#define CSA_CPRT                CSA_COPYRIGHT
+#define CSA_LEFT_QUOTE          GR2A(LSFT(KC_V))        // ‘
+#define CSA_LQOT                CSA_LEFT_QUOTE
+#define CSA_RIGHT_QUOTE         GR2A(LSFT(KC_B))        // ’
+#define CSA_RQOT                CSA_RIGHT_QUOTE
+#define CSA_EIGHTH_NOTE         GR2A(LSFT(KC_N))        // ♪
+#define CSA_8NOT                CSA_EIGHTH_NOTE
+#define CSA_ORDINAL_INDICATOR_O GR2A(LSFT(KC_M))        // º
+#define CSA_ORDO                CSA_ORDINAL_INDICATOR_O
+#define CSA_TIMES               GR2A(LSFT(KC_COMMA))    // ×
+#define CSA_TIMS                CSA_TIMES
+#define CSA_OBELUS              GR2A(LSFT(KC_DOT))      // ÷
+#define CSA_OBEL                CSA_OBELUS
+// more conventional name of the symbol
+#define CSA_DIVISION_SIGN       CSA_OBELUS
+#define CSA_DVSN                CSA_DIVISION_SIGN
+// TODO GR2A(LSFT(CSA_E_ACUTE))
+
+#endif
diff --git a/quantum/keymap_extras/keymap_dvorak.h b/quantum/keymap_extras/keymap_dvorak.h
index 17f2059774..a0feed850d 100644
--- a/quantum/keymap_extras/keymap_dvorak.h
+++ b/quantum/keymap_extras/keymap_dvorak.h
@@ -70,6 +70,10 @@
 #define DV_LCBR	LSFT(DV_LBRC)
 #define DV_RCBR	LSFT(DV_RBRC)
 
+#define DV_DQUO	LSFT(DV_QUOT)
+#define DV_LABK	LSFT(DV_COMM)
+#define DV_RABK	LSFT(DV_DOT)
+
 #define DV_QUES	LSFT(DV_SLSH)
 #define DV_PLUS	LSFT(DV_EQL)
 #define DV_PIPE	LSFT(DV_BSLS)
diff --git a/quantum/keymap_extras/keymap_dvp.h b/quantum/keymap_extras/keymap_dvp.h
new file mode 100644
index 0000000000..83f49a52b5
--- /dev/null
+++ b/quantum/keymap_extras/keymap_dvp.h
@@ -0,0 +1,82 @@
+#ifndef KEYMAP_DVP_H
+#define KEYMAP_DVP_H
+
+#include "keymap.h"
+
+// Normal characters
+#define DP_DLR	KC_GRV
+#define DP_AMPR	KC_1
+#define DP_LBRC	KC_2
+#define DP_LCBR	KC_3
+#define DP_RCBR	KC_4
+#define DP_LPRN	KC_5
+#define DP_EQL	KC_6
+#define DP_ASTR	KC_7
+#define DP_RPRN	KC_8
+#define DP_PLUS	KC_9
+#define DP_RBRC	KC_0
+#define DP_EXLM	KC_MINS
+#define DP_HASH	KC_EQL
+
+#define DP_SCLN	KC_Q
+#define DP_COMM	KC_W
+#define DP_DOT	KC_E
+#define DP_P	KC_R
+#define DP_Y	KC_T
+#define DP_F	KC_Y
+#define DP_G	KC_U
+#define DP_C	KC_I
+#define DP_R	KC_O
+#define DP_L	KC_P
+#define DP_SLSH	KC_LBRC
+#define DP_AT	KC_RBRC
+#define DP_BSLS	KC_BSLS
+
+#define DP_A	KC_A
+#define DP_O	KC_S
+#define DP_E	KC_D
+#define DP_U	KC_F
+#define DP_I	KC_G
+#define DP_D	KC_H
+#define DP_H	KC_J
+#define DP_T	KC_K
+#define DP_N	KC_L
+#define DP_S	KC_SCLN
+#define DP_MINS	KC_QUOT
+
+#define DP_QUOT	KC_Z
+#define DP_Q	KC_X
+#define DP_J	KC_C
+#define DP_K	KC_V
+#define DP_X	KC_B
+#define DP_B	KC_N
+#define DP_M	KC_M
+#define DP_W	KC_COMM
+#define DP_V	KC_DOT
+#define DP_Z	KC_SLSH
+
+// Shifted characters
+#define DP_TILD	LSFT(DP_DLR)
+#define DP_PERC	LSFT(DP_AMPR)
+#define DP_7	LSFT(DP_LBRC)
+#define DP_5	LSFT(DP_LCBR)
+#define DP_3	LSFT(DP_RCBR)
+#define DP_1	LSFT(DP_LPRN)
+#define DP_9	LSFT(DP_EQL)
+#define DP_0	LSFT(DP_ASTR)
+#define DP_2	LSFT(DP_RPRN)
+#define DP_4	LSFT(DP_PLUS)
+#define DP_6	LSFT(DP_RBRC)
+#define DP_8	LSFT(DP_EXLM)
+#define DP_GRV	LSFT(DP_HASH)
+
+#define DP_COLN	LSFT(DP_SCLN)
+#define DP_LABK	LSFT(DP_COMM)
+#define DP_RABK	LSFT(DP_DOT)
+#define DP_QUES	LSFT(DP_SLSH)
+#define DP_CIRC	LSFT(DP_AT)
+#define DP_PIPE	LSFT(DP_BSLS)
+#define DP_UNDS	LSFT(DP_MINS)
+#define DP_DQUO	LSFT(DP_QUOT)
+
+#endif
diff --git a/quantum/keymap_extras/keymap_jp.h b/quantum/keymap_extras/keymap_jp.h
new file mode 100644
index 0000000000..e81b5952e0
--- /dev/null
+++ b/quantum/keymap_extras/keymap_jp.h
@@ -0,0 +1,62 @@
+/* JP106-layout (Japanese Standard)
+ *
+ * For more information, see
+ * http://www2d.biglobe.ne.jp/~msyk/keyboard/layout/usbkeycode.html
+ * note: This website is written in Japanese.
+ */
+
+
+#ifndef KEYMAP_JP_H
+#define KEYMAP_JP_H
+
+
+#include "keymap.h"
+
+
+#define JP_ZHTG KC_GRV  // hankaku/zenkaku|kanzi
+#define JP_YEN  KC_INT3 // yen, |
+#define JP_CIRC KC_EQL  // ^, ~
+#define JP_AT   KC_LBRC // @, `
+#define JP_LBRC KC_RBRC // [, {
+#define JP_COLN KC_QUOT // :, *
+#define JP_RBRC KC_NUHS // ], }
+#define JP_BSLS KC_INT1 // \, _
+#define JP_MHEN KC_INT5 // muhenkan
+#define JP_HENK KC_INT4 // henkan
+#define JP_KANA KC_INT2 // katakana/hiragana|ro-mazi
+
+
+//Aliases for shifted symbols
+#define JP_DQT  LSFT(KC_2)    // "
+#define JP_AMPR LSFT(KC_6)    // &
+#define JP_QUOT LSFT(KC_7)    // '
+#define JP_LPRN LSFT(KC_8)    // (
+#define JP_RPRN LSFT(KC_9)    // )
+#define JP_EQL  LSFT(KC_MINS) // =
+#define JP_TILD LSFT(JP_CIRC) // ~
+#define JP_PIPE LSFT(JP_YEN)  // |
+#define JP_GRV  LSFT(JP_AT)   // `
+#define JP_LCBR LSFT(JP_LBRC) // {
+#define JP_PLUS LSFT(KC_SCLN) // +
+#define JP_ASTR LSFT(JP_COLN) // *
+#define JP_RCBR LSFT(JP_RBRC) // }
+#define JP_UNDS LSFT(JP_BSLS) // _
+
+
+// These symbols are correspond to US101-layout.
+#define JP_MINS KC_MINS // -
+#define JP_SCLN KC_SCLN // ;
+#define JP_COMM KC_COMM // ,
+#define JP_DOT  KC_DOT  // .
+#define JP_SLSH KC_SLSH // /
+// shifted
+#define JP_EXLM KC_EXLM // !
+#define JP_HASH KC_HASH // #
+#define JP_DLR  KC_DLR  // $
+#define JP_PERC KC_PERC // %
+#define JP_LT   KC_LT   // <
+#define JP_GT   KC_GT   // >
+#define JP_QUES KC_QUES // ?
+
+
+#endif
diff --git a/quantum/matrix.c b/quantum/matrix.c
index 3174e07390..07eb87bc36 100644
--- a/quantum/matrix.c
+++ b/quantum/matrix.c
@@ -25,37 +25,65 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "debug.h"
 #include "util.h"
 #include "matrix.h"
+#include "timer.h"
+
 
 /* Set 0 if debouncing isn't needed */
 
 #ifndef DEBOUNCING_DELAY
 #   define DEBOUNCING_DELAY 5
 #endif
-static uint8_t debouncing = DEBOUNCING_DELAY;
+
+#if (DEBOUNCING_DELAY > 0)
+    static uint16_t debouncing_time;
+    static bool debouncing = false;
+#endif
+
+#if (MATRIX_COLS <= 8)
+#    define print_matrix_header()  print("\nr/c 01234567\n")
+#    define print_matrix_row(row)  print_bin_reverse8(matrix_get_row(row))
+#    define matrix_bitpop(i)       bitpop(matrix[i])
+#    define ROW_SHIFTER ((uint8_t)1)
+#elif (MATRIX_COLS <= 16)
+#    define print_matrix_header()  print("\nr/c 0123456789ABCDEF\n")
+#    define print_matrix_row(row)  print_bin_reverse16(matrix_get_row(row))
+#    define matrix_bitpop(i)       bitpop16(matrix[i])
+#    define ROW_SHIFTER ((uint16_t)1)
+#elif (MATRIX_COLS <= 32)
+#    define print_matrix_header()  print("\nr/c 0123456789ABCDEF0123456789ABCDEF\n")
+#    define print_matrix_row(row)  print_bin_reverse32(matrix_get_row(row))
+#    define matrix_bitpop(i)       bitpop32(matrix[i])
+#    define ROW_SHIFTER  ((uint32_t)1)
+#endif
+
+#ifdef MATRIX_MASKED
+    extern const matrix_row_t matrix_mask[];
+#endif
 
 static const uint8_t row_pins[MATRIX_ROWS] = MATRIX_ROW_PINS;
 static const uint8_t col_pins[MATRIX_COLS] = MATRIX_COL_PINS;
 
 /* matrix state(1:on, 0:off) */
 static matrix_row_t matrix[MATRIX_ROWS];
+
+static matrix_row_t matrix_raw[MATRIX_ROWS];
 static matrix_row_t matrix_debouncing[MATRIX_ROWS];
 
-#if DIODE_DIRECTION == ROW2COL
-    static matrix_row_t matrix_reversed[MATRIX_COLS];
-    static matrix_row_t matrix_reversed_debouncing[MATRIX_COLS];
-#endif
 
-#if MATRIX_COLS > 16
-    #define SHIFTER 1UL
-#else
-    #define SHIFTER 1
+#if (DIODE_DIRECTION == COL2ROW)
+    static void init_cols(void);
+    static bool read_cols_on_row(matrix_row_t current_matrix[], uint8_t current_row);
+    static void unselect_rows(void);
+    static void select_row(uint8_t row);
+    static void unselect_row(uint8_t row);
+#else // ROW2COL
+    static void init_rows(void);
+    static bool read_rows_on_col(matrix_row_t current_matrix[], uint8_t current_col);
+    static void unselect_cols(void);
+    static void unselect_col(uint8_t col);
+    static void select_col(uint8_t col);
 #endif
 
-static matrix_row_t read_cols(void);
-static void init_cols(void);
-static void unselect_rows(void);
-static void select_row(uint8_t row);
-
 __attribute__ ((weak))
 void matrix_init_quantum(void) {
     matrix_init_kb();
@@ -95,7 +123,7 @@ uint8_t matrix_cols(void) {
 }
 
 // void matrix_power_up(void) {
-// #if DIODE_DIRECTION == COL2ROW
+// #if (DIODE_DIRECTION == COL2ROW)
 //     for (int8_t r = MATRIX_ROWS - 1; r >= 0; --r) {
 //         /* DDRxn */
 //         _SFR_IO8((row_pins[r] >> 4) + 1) |= _BV(row_pins[r] & 0xF);
@@ -119,19 +147,26 @@ uint8_t matrix_cols(void) {
 // }
 
 void matrix_init(void) {
+
     // To use PORTF disable JTAG with writing JTD bit twice within four cycles.
-    #ifdef __AVR_ATmega32U4__
+    #if  (defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1287__) || defined(__AVR_ATmega32U4__))
         MCUCR |= _BV(JTD);
         MCUCR |= _BV(JTD);
     #endif
 
     // initialize row and col
+#if (DIODE_DIRECTION == COL2ROW)
     unselect_rows();
     init_cols();
+#else // ROW2COL
+    unselect_cols();
+    init_rows();
+#endif
 
     // initialize matrix state: all keys off
     for (uint8_t i=0; i < MATRIX_ROWS; i++) {
         matrix[i] = 0;
+        matrix_raw[i] = 0;
         matrix_debouncing[i] = 0;
     }
 
@@ -141,71 +176,60 @@ void matrix_init(void) {
 uint8_t matrix_scan(void)
 {
 
-#if DIODE_DIRECTION == COL2ROW
-    for (uint8_t i = 0; i < MATRIX_ROWS; i++) {
-        select_row(i);
-        wait_us(30);  // without this wait read unstable value.
-        matrix_row_t cols = read_cols();
-        if (matrix_debouncing[i] != cols) {
-            matrix_debouncing[i] = cols;
-            if (debouncing) {
-                debug("bounce!: "); debug_hex(debouncing); debug("\n");
-            }
-            debouncing = DEBOUNCING_DELAY;
-        }
-        unselect_rows();
-    }
+#if (DIODE_DIRECTION == COL2ROW)
 
-    if (debouncing) {
-        if (--debouncing) {
-            wait_ms(1);
-        } else {
-            for (uint8_t i = 0; i < MATRIX_ROWS; i++) {
-                matrix[i] = matrix_debouncing[i];
+    // Set row, read cols
+    for (uint8_t current_row = 0; current_row < MATRIX_ROWS; current_row++) {
+#       if (DEBOUNCING_DELAY > 0)
+            bool matrix_changed = read_cols_on_row(matrix_debouncing, current_row);
+
+            if (matrix_changed) {
+                debouncing = true;
+                debouncing_time = timer_read();
             }
-        }
+
+#       else
+            read_cols_on_row(matrix, current_row);
+#       endif
+
     }
-#else
-    for (uint8_t i = 0; i < MATRIX_COLS; i++) {
-        select_row(i);
-        wait_us(30);  // without this wait read unstable value.
-        matrix_row_t rows = read_cols();
-        if (matrix_reversed_debouncing[i] != rows) {
-            matrix_reversed_debouncing[i] = rows;
-            if (debouncing) {
-                debug("bounce!: "); debug_hex(debouncing); debug("\n");
+
+#else // ROW2COL
+
+    // Set col, read rows
+    for (uint8_t current_col = 0; current_col < MATRIX_COLS; current_col++) {
+#       if (DEBOUNCING_DELAY > 0)
+            bool matrix_changed = read_rows_on_col(matrix_debouncing, current_col);
+            if (matrix_changed) {
+                debouncing = true;
+                debouncing_time = timer_read();
             }
-            debouncing = DEBOUNCING_DELAY;
-        }
-        unselect_rows();
+#       else
+             read_rows_on_col(matrix, current_col);
+#       endif
+
     }
 
-    if (debouncing) {
-        if (--debouncing) {
-            wait_ms(1);
-        } else {
-            for (uint8_t i = 0; i < MATRIX_COLS; i++) {
-                matrix_reversed[i] = matrix_reversed_debouncing[i];
+#endif
+
+#   if (DEBOUNCING_DELAY > 0)
+        if (debouncing && (timer_elapsed(debouncing_time) > DEBOUNCING_DELAY)) {
+            for (uint8_t i = 0; i < MATRIX_ROWS; i++) {
+                matrix[i] = matrix_debouncing[i];
             }
+            debouncing = false;
         }
-    }
-    for (uint8_t y = 0; y < MATRIX_ROWS; y++) {
-        matrix_row_t row = 0;
-        for (uint8_t x = 0; x < MATRIX_COLS; x++) {
-            row |= ((matrix_reversed[x] & (1<<y)) >> y) << x;
-        }
-        matrix[y] = row;
-    }
-#endif
+#   endif
 
     matrix_scan_quantum();
-
     return 1;
 }
 
 bool matrix_is_modified(void)
 {
+#if (DEBOUNCING_DELAY > 0)
     if (debouncing) return false;
+#endif
     return true;
 }
 
@@ -218,15 +242,22 @@ bool matrix_is_on(uint8_t row, uint8_t col)
 inline
 matrix_row_t matrix_get_row(uint8_t row)
 {
+    // Matrix mask lets you disable switches in the returned matrix data. For example, if you have a
+    // switch blocker installed and the switch is always pressed.
+#ifdef MATRIX_MASKED
+    return matrix[row] & matrix_mask[row];
+#else
     return matrix[row];
+#endif
 }
 
 void matrix_print(void)
 {
-    print("\nr/c 0123456789ABCDEF\n");
+    print_matrix_header();
+
     for (uint8_t row = 0; row < MATRIX_ROWS; row++) {
         phex(row); print(": ");
-        pbin_reverse16(matrix_get_row(row));
+        print_matrix_row(row);
         print("\n");
     }
 }
@@ -235,63 +266,148 @@ uint8_t matrix_key_count(void)
 {
     uint8_t count = 0;
     for (uint8_t i = 0; i < MATRIX_ROWS; i++) {
-        count += bitpop16(matrix[i]);
+        count += matrix_bitpop(i);
     }
     return count;
 }
 
+
+
+#if (DIODE_DIRECTION == COL2ROW)
+
 static void init_cols(void)
 {
-#if DIODE_DIRECTION == COL2ROW
-    for(int x = 0; x < MATRIX_COLS; x++) {
-        int pin = col_pins[x];
-#else
-    for(int x = 0; x < MATRIX_ROWS; x++) {
-        int pin = row_pins[x];
-#endif
-        _SFR_IO8((pin >> 4) + 1) &=  ~_BV(pin & 0xF);
-        _SFR_IO8((pin >> 4) + 2) |= _BV(pin & 0xF);
+    for(uint8_t x = 0; x < MATRIX_COLS; x++) {
+        uint8_t pin = col_pins[x];
+        _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+        _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
     }
 }
 
-static matrix_row_t read_cols(void)
+static bool read_cols_on_row(matrix_row_t current_matrix[], uint8_t current_row)
 {
-    matrix_row_t result = 0;
+    // Store last value of row prior to reading
+    matrix_row_t last_row_value = current_matrix[current_row];
 
-#if DIODE_DIRECTION == COL2ROW
-    for(int x = 0; x < MATRIX_COLS; x++) {     
-        int pin = col_pins[x];
-#else
-    for(int x = 0; x < MATRIX_ROWS; x++) {
-        int pin = row_pins[x];
-#endif
-        result |= (_SFR_IO8(pin >> 4) & _BV(pin & 0xF)) ? 0 : (SHIFTER << x);
+    // Clear data in matrix row
+    current_matrix[current_row] = 0;
+
+    // Select row and wait for row selecton to stabilize
+    select_row(current_row);
+    wait_us(30);
+
+    // For each col...
+    for(uint8_t col_index = 0; col_index < MATRIX_COLS; col_index++) {
+
+        // Select the col pin to read (active low)
+        uint8_t pin = col_pins[col_index];
+        uint8_t pin_state = (_SFR_IO8(pin >> 4) & _BV(pin & 0xF));
+
+        // Populate the matrix row with the state of the col pin
+        current_matrix[current_row] |=  pin_state ? 0 : (ROW_SHIFTER << col_index);
     }
-    return result;
+
+    // Unselect row
+    unselect_row(current_row);
+
+    return (last_row_value != current_matrix[current_row]);
+}
+
+static void select_row(uint8_t row)
+{
+    uint8_t pin = row_pins[row];
+    _SFR_IO8((pin >> 4) + 1) |=  _BV(pin & 0xF); // OUT
+    _SFR_IO8((pin >> 4) + 2) &= ~_BV(pin & 0xF); // LOW
+}
+
+static void unselect_row(uint8_t row)
+{
+    uint8_t pin = row_pins[row];
+    _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+    _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
 }
 
 static void unselect_rows(void)
 {
-#if DIODE_DIRECTION == COL2ROW
-    for(int x = 0; x < MATRIX_ROWS; x++) { 
-        int pin = row_pins[x];
-#else
-    for(int x = 0; x < MATRIX_COLS; x++) { 
-        int pin = col_pins[x];
-#endif
-        _SFR_IO8((pin >> 4) + 1) &=  ~_BV(pin & 0xF);
-        _SFR_IO8((pin >> 4) + 2) |= _BV(pin & 0xF);
+    for(uint8_t x = 0; x < MATRIX_ROWS; x++) {
+        uint8_t pin = row_pins[x];
+        _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+        _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
     }
 }
 
-static void select_row(uint8_t row)
+#else // ROW2COL
+
+static void init_rows(void)
 {
+    for(uint8_t x = 0; x < MATRIX_ROWS; x++) {
+        uint8_t pin = row_pins[x];
+        _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+        _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
+    }
+}
 
-#if DIODE_DIRECTION == COL2ROW
-    int pin = row_pins[row];
-#else
-    int pin = col_pins[row];
-#endif
-    _SFR_IO8((pin >> 4) + 1) |=  _BV(pin & 0xF);
-    _SFR_IO8((pin >> 4) + 2) &= ~_BV(pin & 0xF);
+static bool read_rows_on_col(matrix_row_t current_matrix[], uint8_t current_col)
+{
+    bool matrix_changed = false;
+
+    // Select col and wait for col selecton to stabilize
+    select_col(current_col);
+    wait_us(30);
+
+    // For each row...
+    for(uint8_t row_index = 0; row_index < MATRIX_ROWS; row_index++)
+    {
+
+        // Store last value of row prior to reading
+        matrix_row_t last_row_value = current_matrix[row_index];
+
+        // Check row pin state
+        if ((_SFR_IO8(row_pins[row_index] >> 4) & _BV(row_pins[row_index] & 0xF)) == 0)
+        {
+            // Pin LO, set col bit
+            current_matrix[row_index] |= (ROW_SHIFTER << current_col);
+        }
+        else
+        {
+            // Pin HI, clear col bit
+            current_matrix[row_index] &= ~(ROW_SHIFTER << current_col);
+        }
+
+        // Determine if the matrix changed state
+        if ((last_row_value != current_matrix[row_index]) && !(matrix_changed))
+        {
+            matrix_changed = true;
+        }
+    }
+
+    // Unselect col
+    unselect_col(current_col);
+
+    return matrix_changed;
 }
+
+static void select_col(uint8_t col)
+{
+    uint8_t pin = col_pins[col];
+    _SFR_IO8((pin >> 4) + 1) |=  _BV(pin & 0xF); // OUT
+    _SFR_IO8((pin >> 4) + 2) &= ~_BV(pin & 0xF); // LOW
+}
+
+static void unselect_col(uint8_t col)
+{
+    uint8_t pin = col_pins[col];
+    _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+    _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
+}
+
+static void unselect_cols(void)
+{
+    for(uint8_t x = 0; x < MATRIX_COLS; x++) {
+        uint8_t pin = col_pins[x];
+        _SFR_IO8((pin >> 4) + 1) &= ~_BV(pin & 0xF); // IN
+        _SFR_IO8((pin >> 4) + 2) |=  _BV(pin & 0xF); // HI
+    }
+}
+
+#endif
diff --git a/quantum/process_keycode/process_midi.c b/quantum/process_keycode/process_midi.c
index 8784e64f36..577dad43ac 100644
--- a/quantum/process_keycode/process_midi.c
+++ b/quantum/process_keycode/process_midi.c
@@ -7,7 +7,9 @@ int midi_offset = 7;
 bool process_midi(uint16_t keycode, keyrecord_t *record) {
     if (keycode == MI_ON && record->event.pressed) {
       midi_activated = true;
+#ifdef AUDIO_ENABLE
       music_scale_user();
+#endif
       return false;
     }
 
@@ -63,4 +65,4 @@ bool process_midi(uint16_t keycode, keyrecord_t *record) {
         return false;
     }
   return true;
-}
\ No newline at end of file
+}
diff --git a/quantum/process_keycode/process_tap_dance.c b/quantum/process_keycode/process_tap_dance.c
index 07de3ecb8f..6ae362c4c2 100644
--- a/quantum/process_keycode/process_tap_dance.c
+++ b/quantum/process_keycode/process_tap_dance.c
@@ -65,11 +65,12 @@ bool process_tap_dance(uint16_t keycode, keyrecord_t *record) {
       highest_td = idx;
     action = &tap_dance_actions[idx];
 
-    action->state.keycode = keycode;
     action->state.pressed = record->event.pressed;
     if (record->event.pressed) {
+      action->state.keycode = keycode;
       action->state.count++;
       action->state.timer = timer_read();
+      process_tap_dance_action_on_each_tap (action);
 
       if (last_td && last_td != keycode) {
         qk_tap_dance_action_t *paction = &tap_dance_actions[last_td - QK_TAP_DANCE];
@@ -77,8 +78,9 @@ bool process_tap_dance(uint16_t keycode, keyrecord_t *record) {
         process_tap_dance_action_on_dance_finished (paction);
         reset_tap_dance (&paction->state);
       }
+
+      last_td = keycode;
     }
-    last_td = keycode;
 
     break;
 
diff --git a/quantum/process_keycode/process_tap_dance.h b/quantum/process_keycode/process_tap_dance.h
index d7b857bdc6..f753cbba66 100644
--- a/quantum/process_keycode/process_tap_dance.h
+++ b/quantum/process_keycode/process_tap_dance.h
@@ -39,15 +39,17 @@ typedef struct
 
 #define ACTION_TAP_DANCE_DOUBLE(kc1, kc2) { \
     .fn = { NULL, qk_tap_dance_pair_finished, qk_tap_dance_pair_reset }, \
-    .user_data = (void *)&((qk_tap_dance_pair_t) { kc1, kc2 })  \
+    .user_data = (void *)&((qk_tap_dance_pair_t) { kc1, kc2 }),  \
   }
 
 #define ACTION_TAP_DANCE_FN(user_fn) {  \
-    .fn = { NULL, user_fn, NULL } \
+    .fn = { NULL, user_fn, NULL }, \
+    .user_data = NULL, \
   }
 
-#define ACTION_TAP_DANCE_FN_ADVANCED(user_fn_on_each_tap, user_fn_on_dance_finished, user_fn_on_reset) { \
-    .fn = { user_fn_on_each_tap, user_fn_on_dance_finished, user_fn_on_reset } \
+#define ACTION_TAP_DANCE_FN_ADVANCED(user_fn_on_each_tap, user_fn_on_dance_finished, user_fn_on_dance_reset) { \
+    .fn = { user_fn_on_each_tap, user_fn_on_dance_finished, user_fn_on_dance_reset }, \
+    .user_data = NULL, \
   }
 
 extern qk_tap_dance_action_t tap_dance_actions[];
diff --git a/quantum/process_keycode/process_unicode.c b/quantum/process_keycode/process_unicode.c
index 06c1694f2e..cd3a610b4d 100644
--- a/quantum/process_keycode/process_unicode.c
+++ b/quantum/process_keycode/process_unicode.c
@@ -2,6 +2,7 @@
 
 static uint8_t input_mode;
 
+__attribute__((weak))
 uint16_t hex_to_keycode(uint8_t hex)
 {
   if (hex == 0x0) {
@@ -18,6 +19,10 @@ void set_unicode_input_mode(uint8_t os_target)
   input_mode = os_target;
 }
 
+uint8_t get_unicode_input_mode(void) {
+  return input_mode;
+}
+
 __attribute__((weak))
 void unicode_input_start (void) {
   switch(input_mode) {
@@ -37,6 +42,11 @@ void unicode_input_start (void) {
     register_code(KC_PPLS);
     unregister_code(KC_PPLS);
     break;
+  case UC_WINC:
+    register_code(KC_RALT);
+    unregister_code(KC_RALT);
+    register_code(KC_U);
+    unregister_code(KC_U);
   }
   wait_ms(UNICODE_TYPE_DELAY);
 }
@@ -73,6 +83,52 @@ bool process_unicode(uint16_t keycode, keyrecord_t *record) {
   return true;
 }
 
+#ifdef UNICODEMAP_ENABLE
+__attribute__((weak))
+const uint32_t PROGMEM unicode_map[] = {
+};
+
+void register_hex32(uint32_t hex) {
+  uint8_t onzerostart = 1;
+  for(int i = 7; i >= 0; i--) {
+    if (i <= 3) {
+      onzerostart = 0;
+    }
+    uint8_t digit = ((hex >> (i*4)) & 0xF);
+    if (digit == 0) {
+      if (onzerostart == 0) {
+        register_code(hex_to_keycode(digit));
+        unregister_code(hex_to_keycode(digit));
+      }
+    } else {
+      register_code(hex_to_keycode(digit));
+      unregister_code(hex_to_keycode(digit));
+      onzerostart = 0;
+    }
+  }
+}
+
+__attribute__((weak))
+void unicode_map_input_error() {}
+
+bool process_unicode_map(uint16_t keycode, keyrecord_t *record) {
+  if ((keycode & QK_UNICODE_MAP) == QK_UNICODE_MAP && record->event.pressed) {
+    const uint32_t* map = unicode_map;
+    uint16_t index = keycode & 0x7FF;
+    uint32_t code = pgm_read_dword_far(&map[index]);
+    if ((code > 0xFFFF && input_mode == UC_OSX) || (code > 0xFFFFF && input_mode == UC_LNX)) {
+      // when character is out of range supported by the OS
+      unicode_map_input_error();
+    } else {
+      unicode_input_start();
+      register_hex32(code);
+      unicode_input_finish();
+    }
+  }
+  return true;
+}
+#endif
+
 #ifdef UCIS_ENABLE
 qk_ucis_state_t qk_ucis_state;
 
diff --git a/quantum/process_keycode/process_unicode.h b/quantum/process_keycode/process_unicode.h
index 02ce3dd7e0..f17cfa6cf2 100644
--- a/quantum/process_keycode/process_unicode.h
+++ b/quantum/process_keycode/process_unicode.h
@@ -3,22 +3,29 @@
 
 #include "quantum.h"
 
-#define UC_OSX 0
-#define UC_LNX 1
-#define UC_WIN 2
-#define UC_BSD 3
+#define UC_OSX 0  // Mac OS X
+#define UC_LNX 1  // Linux
+#define UC_WIN 2  // Windows 'HexNumpad'
+#define UC_BSD 3  // BSD (not implemented)
+#define UC_WINC 4 // WinCompose https://github.com/samhocevar/wincompose
 
 #ifndef UNICODE_TYPE_DELAY
 #define UNICODE_TYPE_DELAY 10
 #endif
 
 void set_unicode_input_mode(uint8_t os_target);
+uint8_t get_unicode_input_mode(void);
 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 UNICODEMAP_ENABLE
+void unicode_map_input_error(void);
+bool process_unicode_map(uint16_t keycode, keyrecord_t *record);
+#endif
+
 #ifdef UCIS_ENABLE
 #ifndef UCIS_MAX_SYMBOL_LENGTH
 #define UCIS_MAX_SYMBOL_LENGTH 32
diff --git a/quantum/quantum.c b/quantum/quantum.c
index e3a20f43e0..b5e2d60b9d 100644
--- a/quantum/quantum.c
+++ b/quantum/quantum.c
@@ -1,5 +1,9 @@
 #include "quantum.h"
 
+#ifndef TAPPING_TERM
+#define TAPPING_TERM 200
+#endif
+
 static void do_code16 (uint16_t code, void (*f) (uint8_t)) {
   switch (code) {
   case QK_MODS ... QK_MODS_MAX:
@@ -75,6 +79,7 @@ void reset_keyboard(void) {
 #endif
 
 static bool shift_interrupted[2] = {0, 0};
+static uint16_t scs_timer = 0;
 
 bool process_record_quantum(keyrecord_t *record) {
 
@@ -129,6 +134,9 @@ bool process_record_quantum(keyrecord_t *record) {
   #ifdef UCIS_ENABLE
     process_ucis(keycode, record) &&
   #endif
+  #ifdef UNICODEMAP_ENABLE
+    process_unicode_map(keycode, record) &&
+  #endif
       true)) {
     return false;
   }
@@ -199,7 +207,7 @@ bool process_record_quantum(keyrecord_t *record) {
 	  return false;
       break;
 	#endif
-    case MAGIC_SWAP_CONTROL_CAPSLOCK ... MAGIC_UNSWAP_ALT_GUI:
+    case MAGIC_SWAP_CONTROL_CAPSLOCK ... MAGIC_TOGGLE_NKRO:
       if (record->event.pressed) {
         // MAGIC actions (BOOTMAGIC without the boot)
         if (!eeconfig_is_enabled()) {
@@ -207,54 +215,80 @@ bool process_record_quantum(keyrecord_t *record) {
         }
         /* keymap config */
         keymap_config.raw = eeconfig_read_keymap();
-        if (keycode == MAGIC_SWAP_CONTROL_CAPSLOCK) {
-            keymap_config.swap_control_capslock = 1;
-        } else if (keycode == MAGIC_CAPSLOCK_TO_CONTROL) {
-            keymap_config.capslock_to_control = 1;
-        } else if (keycode == MAGIC_SWAP_LALT_LGUI) {
-            keymap_config.swap_lalt_lgui = 1;
-        } else if (keycode == MAGIC_SWAP_RALT_RGUI) {
-            keymap_config.swap_ralt_rgui = 1;
-        } else if (keycode == MAGIC_NO_GUI) {
-            keymap_config.no_gui = 1;
-        } else if (keycode == MAGIC_SWAP_GRAVE_ESC) {
-            keymap_config.swap_grave_esc = 1;
-        } else if (keycode == MAGIC_SWAP_BACKSLASH_BACKSPACE) {
-            keymap_config.swap_backslash_backspace = 1;
-        } else if (keycode == MAGIC_HOST_NKRO) {
-            keymap_config.nkro = 1;
-        } else if (keycode == MAGIC_SWAP_ALT_GUI) {
-            keymap_config.swap_lalt_lgui = 1;
-            keymap_config.swap_ralt_rgui = 1;
-        }
-        /* UNs */
-        else if (keycode == MAGIC_UNSWAP_CONTROL_CAPSLOCK) {
-            keymap_config.swap_control_capslock = 0;
-        } else if (keycode == MAGIC_UNCAPSLOCK_TO_CONTROL) {
-            keymap_config.capslock_to_control = 0;
-        } else if (keycode == MAGIC_UNSWAP_LALT_LGUI) {
-            keymap_config.swap_lalt_lgui = 0;
-        } else if (keycode == MAGIC_UNSWAP_RALT_RGUI) {
-            keymap_config.swap_ralt_rgui = 0;
-        } else if (keycode == MAGIC_UNNO_GUI) {
-            keymap_config.no_gui = 0;
-        } else if (keycode == MAGIC_UNSWAP_GRAVE_ESC) {
-            keymap_config.swap_grave_esc = 0;
-        } else if (keycode == MAGIC_UNSWAP_BACKSLASH_BACKSPACE) {
-            keymap_config.swap_backslash_backspace = 0;
-        } else if (keycode == MAGIC_UNHOST_NKRO) {
-            keymap_config.nkro = 0;
-        } else if (keycode == MAGIC_UNSWAP_ALT_GUI) {
-            keymap_config.swap_lalt_lgui = 0;
-            keymap_config.swap_ralt_rgui = 0;
+        switch (keycode)
+        {
+          case MAGIC_SWAP_CONTROL_CAPSLOCK:
+            keymap_config.swap_control_capslock = true;
+            break;
+          case MAGIC_CAPSLOCK_TO_CONTROL:
+            keymap_config.capslock_to_control = true;
+            break;
+          case MAGIC_SWAP_LALT_LGUI:
+            keymap_config.swap_lalt_lgui = true;
+            break;
+          case MAGIC_SWAP_RALT_RGUI:
+            keymap_config.swap_ralt_rgui = true;
+            break;
+          case MAGIC_NO_GUI:
+            keymap_config.no_gui = true;
+            break;
+          case MAGIC_SWAP_GRAVE_ESC:
+            keymap_config.swap_grave_esc = true;
+            break;
+          case MAGIC_SWAP_BACKSLASH_BACKSPACE:
+            keymap_config.swap_backslash_backspace = true;
+            break;
+          case MAGIC_HOST_NKRO:
+            keymap_config.nkro = true;
+            break;
+          case MAGIC_SWAP_ALT_GUI:
+            keymap_config.swap_lalt_lgui = true;
+            keymap_config.swap_ralt_rgui = true;
+            break;
+          case MAGIC_UNSWAP_CONTROL_CAPSLOCK:
+            keymap_config.swap_control_capslock = false;
+            break;
+          case MAGIC_UNCAPSLOCK_TO_CONTROL:
+            keymap_config.capslock_to_control = false;
+            break;
+          case MAGIC_UNSWAP_LALT_LGUI:
+            keymap_config.swap_lalt_lgui = false;
+            break;
+          case MAGIC_UNSWAP_RALT_RGUI:
+            keymap_config.swap_ralt_rgui = false;
+            break;
+          case MAGIC_UNNO_GUI:
+            keymap_config.no_gui = false;
+            break;
+          case MAGIC_UNSWAP_GRAVE_ESC:
+            keymap_config.swap_grave_esc = false;
+            break;
+          case MAGIC_UNSWAP_BACKSLASH_BACKSPACE:
+            keymap_config.swap_backslash_backspace = false;
+            break;
+          case MAGIC_UNHOST_NKRO:
+            keymap_config.nkro = false;
+            break;
+          case MAGIC_UNSWAP_ALT_GUI:
+            keymap_config.swap_lalt_lgui = false;
+            keymap_config.swap_ralt_rgui = false;
+            break;
+          case MAGIC_TOGGLE_NKRO:
+            keymap_config.nkro = !keymap_config.nkro;
+            break;
+          default:
+            break;
         }
         eeconfig_update_keymap(keymap_config.raw);
+        clear_keyboard(); // clear to prevent stuck keys
+
         return false;
       }
       break;
     case KC_LSPO: {
       if (record->event.pressed) {
         shift_interrupted[0] = false;
+        scs_timer = timer_read ();
         register_mods(MOD_BIT(KC_LSFT));
       }
       else {
@@ -264,19 +298,20 @@ bool process_record_quantum(keyrecord_t *record) {
             shift_interrupted[1] = true;
           }
         #endif
-        if (!shift_interrupted[0]) {
+        if (!shift_interrupted[0] && timer_elapsed(scs_timer) < TAPPING_TERM) {
           register_code(LSPO_KEY);
           unregister_code(LSPO_KEY);
         }
         unregister_mods(MOD_BIT(KC_LSFT));
       }
       return false;
-      break;
+      // break;
     }
 
     case KC_RSPC: {
       if (record->event.pressed) {
         shift_interrupted[1] = false;
+        scs_timer = timer_read ();
         register_mods(MOD_BIT(KC_RSFT));
       }
       else {
@@ -286,14 +321,14 @@ bool process_record_quantum(keyrecord_t *record) {
             shift_interrupted[1] = true;
           }
         #endif
-        if (!shift_interrupted[1]) {
+        if (!shift_interrupted[1] && timer_elapsed(scs_timer) < TAPPING_TERM) {
           register_code(RSPC_KEY);
           unregister_code(RSPC_KEY);
         }
         unregister_mods(MOD_BIT(KC_RSFT));
       }
       return false;
-      break;
+      // break;
     }
     default: {
       shift_interrupted[0] = true;
diff --git a/quantum/rgblight.c b/quantum/rgblight.c
index f82e3ec558..d550c58660 100644
--- a/quantum/rgblight.c
+++ b/quantum/rgblight.c
@@ -6,24 +6,37 @@
 #include "rgblight.h"
 #include "debug.h"
 
+// Lightness curve using the CIE 1931 lightness formula
+//Generated by the python script provided in http://jared.geek.nz/2013/feb/linear-led-pwm
 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, 0, 0, 0, 0, 1, 1, 1, 1, 1,
+    1, 1, 1, 1, 2, 2, 2, 2, 2, 2,
+    2, 2, 2, 3, 3, 3, 3, 3, 3, 3,
+    3, 4, 4, 4, 4, 4, 4, 5, 5, 5,
+    5, 5, 6, 6, 6, 6, 6, 7, 7, 7,
+    7, 8, 8, 8, 8, 9, 9, 9, 10, 10,
+    10, 10, 11, 11, 11, 12, 12, 12, 13, 13,
+    13, 14, 14, 15, 15, 15, 16, 16, 17, 17,
+    17, 18, 18, 19, 19, 20, 20, 21, 21, 22,
+    22, 23, 23, 24, 24, 25, 25, 26, 26, 27,
+    28, 28, 29, 29, 30, 31, 31, 32, 32, 33,
+    34, 34, 35, 36, 37, 37, 38, 39, 39, 40,
+    41, 42, 43, 43, 44, 45, 46, 47, 47, 48,
+    49, 50, 51, 52, 53, 54, 54, 55, 56, 57,
+    58, 59, 60, 61, 62, 63, 64, 65, 66, 67,
+    68, 70, 71, 72, 73, 74, 75, 76, 77, 79,
+    80, 81, 82, 83, 85, 86, 87, 88, 90, 91,
+    92, 94, 95, 96, 98, 99, 100, 102, 103, 105,
+    106, 108, 109, 110, 112, 113, 115, 116, 118, 120,
+    121, 123, 124, 126, 128, 129, 131, 132, 134, 136,
+    138, 139, 141, 143, 145, 146, 148, 150, 152, 154,
+    155, 157, 159, 161, 163, 165, 167, 169, 171, 173,
+    175, 177, 179, 181, 183, 185, 187, 189, 191, 193,
+    196, 198, 200, 202, 204, 207, 209, 211, 214, 216,
+    218, 220, 223, 225, 228, 230, 232, 235, 237, 240,
+    242, 245, 247, 250, 252, 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,
@@ -42,10 +55,16 @@ const uint8_t RGBLED_BREATHING_TABLE[] PROGMEM = {
   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
 };
+
+__attribute__ ((weak))
 const uint8_t RGBLED_BREATHING_INTERVALS[] PROGMEM = {30, 20, 10, 5};
+__attribute__ ((weak))
 const uint8_t RGBLED_RAINBOW_MOOD_INTERVALS[] PROGMEM = {120, 60, 30};
+__attribute__ ((weak))
 const uint8_t RGBLED_RAINBOW_SWIRL_INTERVALS[] PROGMEM = {100, 50, 20};
+__attribute__ ((weak))
 const uint8_t RGBLED_SNAKE_INTERVALS[] PROGMEM = {100, 50, 20};
+__attribute__ ((weak))
 const uint8_t RGBLED_KNIGHT_INTERVALS[] PROGMEM = {100, 50, 20};
 
 rgblight_config_t rgblight_config;
@@ -55,13 +74,8 @@ uint8_t rgblight_inited = 0;
 
 
 void sethsv(uint16_t hue, uint8_t sat, uint8_t val, struct cRGB *led1) {
-  // 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]);
-
   if (sat == 0) { // Acromatic color (gray). Hue doesn't mind.
     r = val;
     g = val;
@@ -103,6 +117,9 @@ void sethsv(uint16_t hue, uint8_t sat, uint8_t val, struct cRGB *led1) {
         break;
     }
   }
+  r = pgm_read_byte(&DIM_CURVE[r]);
+  g = pgm_read_byte(&DIM_CURVE[g]);
+  b = pgm_read_byte(&DIM_CURVE[b]);
 
   setrgb(r, g, b, led1);
 }
diff --git a/quantum/rgblight.h b/quantum/rgblight.h
index def26c428c..17f04ffcf2 100644
--- a/quantum/rgblight.h
+++ b/quantum/rgblight.h
@@ -40,6 +40,12 @@
 #include "eeconfig.h"
 #include "light_ws2812.h"
 
+extern const uint8_t RGBLED_BREATHING_INTERVALS[4] PROGMEM;
+extern const uint8_t RGBLED_RAINBOW_MOOD_INTERVALS[3] PROGMEM;
+extern const uint8_t RGBLED_RAINBOW_SWIRL_INTERVALS[3] PROGMEM;
+extern const uint8_t RGBLED_SNAKE_INTERVALS[3] PROGMEM;
+extern const uint8_t RGBLED_KNIGHT_INTERVALS[3] PROGMEM;
+
 typedef union {
   uint32_t raw;
   struct {
diff --git a/quantum/serial_link/protocol/byte_stuffer.c b/quantum/serial_link/protocol/byte_stuffer.c
index fb4c45a8dc..2c87d64c29 100644
--- a/quantum/serial_link/protocol/byte_stuffer.c
+++ b/quantum/serial_link/protocol/byte_stuffer.c
@@ -31,9 +31,6 @@ SOFTWARE.
 // https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
 // http://www.stuartcheshire.org/papers/COBSforToN.pdf
 
-#define MAX_FRAME_SIZE 1024
-#define NUM_LINKS 2
-
 typedef struct byte_stuffer_state {
     uint16_t next_zero;
     uint16_t data_pos;
diff --git a/quantum/serial_link/protocol/byte_stuffer.h b/quantum/serial_link/protocol/byte_stuffer.h
index 2cc88beb42..97e8968564 100644
--- a/quantum/serial_link/protocol/byte_stuffer.h
+++ b/quantum/serial_link/protocol/byte_stuffer.h
@@ -27,6 +27,9 @@ SOFTWARE.
 
 #include <stdint.h>
 
+#define MAX_FRAME_SIZE 1024
+#define NUM_LINKS 2
+
 void init_byte_stuffer(void);
 void byte_stuffer_recv_byte(uint8_t link, uint8_t data);
 void byte_stuffer_send_frame(uint8_t link, uint8_t* data, uint16_t size);
diff --git a/quantum/serial_link/protocol/transport.c b/quantum/serial_link/protocol/transport.c
index f418d11ceb..ff795fe201 100644
--- a/quantum/serial_link/protocol/transport.c
+++ b/quantum/serial_link/protocol/transport.c
@@ -31,6 +31,10 @@ SOFTWARE.
 static remote_object_t* remote_objects[MAX_REMOTE_OBJECTS];
 static uint32_t num_remote_objects = 0;
 
+void reinitialize_serial_link_transport(void) {
+    num_remote_objects = 0;
+}
+
 void add_remote_objects(remote_object_t** _remote_objects, uint32_t _num_remote_objects) {
     unsigned int i;
     for(i=0;i<_num_remote_objects;i++) {
diff --git a/quantum/serial_link/protocol/transport.h b/quantum/serial_link/protocol/transport.h
index 9a052d8809..2c5d890b21 100644
--- a/quantum/serial_link/protocol/transport.h
+++ b/quantum/serial_link/protocol/transport.h
@@ -82,7 +82,7 @@ typedef struct { \
         remote_object_t* obj = (remote_object_t*)&remote_object_##name; \
         uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size);\
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define MASTER_TO_SINGLE_SLAVE_OBJECT(name, type) \
@@ -112,7 +112,7 @@ typedef struct { \
         remote_object_t* obj = (remote_object_t*)&remote_object_##name; \
         uint8_t* start = obj->buffer + NUM_SLAVES * LOCAL_OBJECT_SIZE(obj->object_size);\
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define SLAVE_TO_MASTER_OBJECT(name, type) \
@@ -139,12 +139,13 @@ typedef struct { \
         uint8_t* start = obj->buffer + LOCAL_OBJECT_SIZE(obj->object_size);\
         start+=slave * REMOTE_OBJECT_SIZE(obj->object_size); \
         triple_buffer_object_t* tb = (triple_buffer_object_t*)start; \
-        return triple_buffer_read_internal(obj->object_size, tb); \
+        return (type*)triple_buffer_read_internal(obj->object_size, tb); \
     }
 
 #define REMOTE_OBJECT(name) (remote_object_t*)&remote_object_##name
 
 void add_remote_objects(remote_object_t** remote_objects, uint32_t num_remote_objects);
+void reinitialize_serial_link_transport(void);
 void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size);
 void update_transport(void);
 
diff --git a/quantum/serial_link/tests/byte_stuffer_tests.c b/quantum/serial_link/tests/byte_stuffer_tests.cpp
index 64b170e8c1..ff49d727bb 100644
--- a/quantum/serial_link/tests/byte_stuffer_tests.c
+++ b/quantum/serial_link/tests/byte_stuffer_tests.cpp
@@ -22,70 +22,90 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 
-#include <cgreen/cgreen.h>
-#include <cgreen/mocks.h>
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+#include <vector>
+#include <algorithm>
+extern "C" {
 #include "serial_link/protocol/byte_stuffer.h"
-#include "serial_link/protocol/byte_stuffer.c"
 #include "serial_link/protocol/frame_validator.h"
 #include "serial_link/protocol/physical.h"
+}
 
-static uint8_t sent_data[MAX_FRAME_SIZE*2];
-static uint16_t sent_data_size;
+using testing::_;
+using testing::ElementsAreArray;
+using testing::Args;
 
-Describe(ByteStuffer);
-BeforeEach(ByteStuffer) {
-    init_byte_stuffer();
-    sent_data_size = 0;
-}
-AfterEach(ByteStuffer) {}
+class ByteStuffer : public ::testing::Test{
+public:
+    ByteStuffer() {
+        Instance = this;
+        init_byte_stuffer();
+    }
 
-void validator_recv_frame(uint8_t link, uint8_t* data, uint16_t size) {
-    mock(data, size);
-}
+    ~ByteStuffer() {
+        Instance = nullptr;
+    }
+
+    MOCK_METHOD3(validator_recv_frame, void (uint8_t link, uint8_t* data, uint16_t size));
+
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        std::copy(data, data + size, std::back_inserter(sent_data));
+    }
+    std::vector<uint8_t> sent_data;
 
-void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
-    memcpy(sent_data + sent_data_size, data, size);
-    sent_data_size += size;
+    static ByteStuffer* Instance;
+};
+
+ByteStuffer* ByteStuffer::Instance = nullptr;
+
+extern "C" {
+    void validator_recv_frame(uint8_t link, uint8_t* data, uint16_t size) {
+        ByteStuffer::Instance->validator_recv_frame(link, data, size);
+    }
+
+    void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
+        ByteStuffer::Instance->send_data(link, data, size);
+    }
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_zero_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_zero_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_FF_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_FF_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0xFF);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_single_random_byte) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_single_random_byte) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 0x4A);
 }
 
-Ensure(ByteStuffer, receives_no_frame_for_a_zero_length_frame) {
-    never_expect(validator_recv_frame);
+TEST_F(ByteStuffer, receives_no_frame_for_a_zero_length_frame) {
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_single_byte_valid_frame) {
+TEST_F(ByteStuffer, receives_single_byte_valid_frame) {
     uint8_t expected[] = {0x37};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 0x37);
     byte_stuffer_recv_byte(0, 0);
 }
-
-Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
+TEST_F(ByteStuffer, receives_three_bytes_valid_frame) {
     uint8_t expected[] = {0x37, 0x99, 0xFF};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(3)),
-        when(data, is_equal_to_contents_of(expected, 3))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 4);
     byte_stuffer_recv_byte(0, 0x37);
     byte_stuffer_recv_byte(0, 0x99);
@@ -93,23 +113,19 @@ Ensure(ByteStuffer, receives_three_bytes_valid_frame) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_single_zero_valid_frame) {
+TEST_F(ByteStuffer, receives_single_zero_valid_frame) {
     uint8_t expected[] = {0};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 1);
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
+TEST_F(ByteStuffer, receives_valid_frame_with_zeroes) {
     uint8_t expected[] = {5, 0, 3, 0};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(expected, 4))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 5);
     byte_stuffer_recv_byte(0, 2);
@@ -118,17 +134,14 @@ Ensure(ByteStuffer, receives_valid_frame_with_zeroes) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_two_valid_frames) {
+
+TEST_F(ByteStuffer, receives_two_valid_frames) {
     uint8_t expected1[] = {5, 0};
     uint8_t expected2[] = {3};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected1, 2))
-    );
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected2, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected1)));
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected2)));
     byte_stuffer_recv_byte(1, 2);
     byte_stuffer_recv_byte(1, 5);
     byte_stuffer_recv_byte(1, 1);
@@ -138,12 +151,10 @@ Ensure(ByteStuffer, receives_two_valid_frames) {
     byte_stuffer_recv_byte(1, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
+TEST_F(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
     uint8_t expected[] = {5, 7};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected, 2))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(1, 3);
     byte_stuffer_recv_byte(1, 1);
     byte_stuffer_recv_byte(1, 0);
@@ -153,12 +164,10 @@ Ensure(ByteStuffer, receives_valid_frame_after_unexpected_zero) {
     byte_stuffer_recv_byte(1, 0);
 }
 
-Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
+TEST_F(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
     uint8_t expected[] = {5, 7};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(2)),
-        when(data, is_equal_to_contents_of(expected, 2))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 2);
     byte_stuffer_recv_byte(0, 9);
     byte_stuffer_recv_byte(0, 4); // This should have been zero
@@ -169,16 +178,14 @@ Ensure(ByteStuffer, receives_valid_frame_after_unexpected_non_zero) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_of_frame) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_of_frame) {
     uint8_t expected[254];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
-    expect(validator_recv_frame,
-        when(size, is_equal_to(254)),
-        when(data, is_equal_to_contents_of(expected, 254))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -186,17 +193,15 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_and_then_end_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_non_zero) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_non_zero) {
     uint8_t expected[255];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
     expected[254] = 7;
-    expect(validator_recv_frame,
-        when(size, is_equal_to(255)),
-        when(data, is_equal_to_contents_of(expected, 255))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -206,17 +211,15 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_zero) {
+TEST_F(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_zero) {
     uint8_t expected[255];
     int i;
     for (i=0;i<254;i++) {
         expected[i] = i + 1;
     }
     expected[254] = 0;
-    expect(validator_recv_frame,
-        when(size, is_equal_to(255)),
-        when(data, is_equal_to_contents_of(expected, 255))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -226,7 +229,7 @@ Ensure(ByteStuffer, receives_a_valid_frame_with_over254_non_zeroes_next_byte_is_
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
+TEST_F(ByteStuffer, receives_two_long_frames_and_some_more) {
     uint8_t expected[515];
     int i;
     int j;
@@ -238,10 +241,8 @@ Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
     for (i=0;i<7;i++) {
         expected[254*2+i] = i + 1;
     }
-    expect(validator_recv_frame,
-        when(size, is_equal_to(515)),
-        when(data, is_equal_to_contents_of(expected, 510))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     byte_stuffer_recv_byte(0, 0xFF);
     for (i=0;i<254;i++) {
         byte_stuffer_recv_byte(0, i+1);
@@ -261,12 +262,10 @@ Ensure(ByteStuffer, receives_two_long_frames_and_some_more) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
+TEST_F(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
     uint8_t expected[MAX_FRAME_SIZE] = {};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(MAX_FRAME_SIZE)),
-        when(data, is_equal_to_contents_of(expected, MAX_FRAME_SIZE))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -275,9 +274,10 @@ Ensure(ByteStuffer, receives_an_all_zeros_frame_that_is_maximum_size) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
+TEST_F(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
     uint8_t expected[1] = {0};
-    never_expect(validator_recv_frame);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .Times(0);
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -287,12 +287,10 @@ Ensure(ByteStuffer, doesnt_recv_a_frame_thats_too_long_all_zeroes) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
+TEST_F(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
     uint8_t expected[1] = {1};
-    expect(validator_recv_frame,
-        when(size, is_equal_to(1)),
-        when(data, is_equal_to_contents_of(expected, 1))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(expected)));
     int i;
     byte_stuffer_recv_byte(0, 1);
     for(i=0;i<MAX_FRAME_SIZE;i++) {
@@ -303,76 +301,68 @@ Ensure(ByteStuffer, received_frame_is_aborted_when_its_too_long) {
     byte_stuffer_recv_byte(0, 0);
 }
 
-Ensure(ByteStuffer, does_nothing_when_sending_zero_size_frame) {
-    assert_that(sent_data_size, is_equal_to(0));
+TEST_F(ByteStuffer, does_nothing_when_sending_zero_size_frame) {
+    EXPECT_EQ(sent_data.size(), 0);
     byte_stuffer_send_frame(0, NULL, 0);
 }
 
-Ensure(ByteStuffer, send_one_byte_frame) {
+TEST_F(ByteStuffer, send_one_byte_frame) {
     uint8_t data[] = {5};
     byte_stuffer_send_frame(1, data, 1);
     uint8_t expected[] = {2, 5, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame) {
+TEST_F(ByteStuffer, sends_two_byte_frame) {
     uint8_t data[] = {5, 0x77};
     byte_stuffer_send_frame(0, data, 2);
     uint8_t expected[] = {3, 5, 0x77, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_one_byte_frame_with_zero) {
+TEST_F(ByteStuffer, sends_one_byte_frame_with_zero) {
     uint8_t data[] = {0};
     byte_stuffer_send_frame(0, data, 1);
     uint8_t expected[] = {1, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame_starting_with_zero) {
+TEST_F(ByteStuffer, sends_two_byte_frame_starting_with_zero) {
     uint8_t data[] = {0, 9};
     byte_stuffer_send_frame(1, data, 2);
     uint8_t expected[] = {1, 2, 9, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_two_byte_frame_starting_with_non_zero) {
+TEST_F(ByteStuffer, sends_two_byte_frame_starting_with_non_zero) {
     uint8_t data[] = {9, 0};
     byte_stuffer_send_frame(1, data, 2);
     uint8_t expected[] = {2, 9, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_zero_in_the_middle) {
+TEST_F(ByteStuffer, sends_three_byte_frame_zero_in_the_middle) {
     uint8_t data[] = {9, 0, 0x68};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {2, 9, 2, 0x68, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_data_in_the_middle) {
+TEST_F(ByteStuffer, sends_three_byte_frame_data_in_the_middle) {
     uint8_t data[] = {0, 0x55, 0};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {1, 2, 0x55, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_three_byte_frame_with_all_zeroes) {
+TEST_F(ByteStuffer, sends_three_byte_frame_with_all_zeroes) {
     uint8_t data[] = {0, 0, 0};
     byte_stuffer_send_frame(0, data, 3);
     uint8_t expected[] = {1, 1, 1, 1, 0};
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_254_non_zeroes) {
+TEST_F(ByteStuffer, sends_frame_with_254_non_zeroes) {
     uint8_t data[254];
     int i;
     for(i=0;i<254;i++) {
@@ -385,11 +375,10 @@ Ensure(ByteStuffer, sends_frame_with_254_non_zeroes) {
         expected[i] = i;
     }
     expected[255] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_255_non_zeroes) {
+TEST_F(ByteStuffer, sends_frame_with_255_non_zeroes) {
     uint8_t data[255];
     int i;
     for(i=0;i<255;i++) {
@@ -404,17 +393,16 @@ Ensure(ByteStuffer, sends_frame_with_255_non_zeroes) {
     expected[255] = 2;
     expected[256] = 255;
     expected[257] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
+TEST_F(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
     uint8_t data[255];
     int i;
     for(i=0;i<254;i++) {
         data[i] = i + 1;
     }
-    data[255] = 0;
+    data[254] = 0;
     byte_stuffer_send_frame(0, data, 255);
     uint8_t expected[258];
     expected[0] = 0xFF;
@@ -424,53 +412,46 @@ Ensure(ByteStuffer, sends_frame_with_254_non_zeroes_followed_by_zero) {
     expected[255] = 1;
     expected[256] = 1;
     expected[257] = 0;
-    assert_that(sent_data_size, is_equal_to(sizeof(expected)));
-    assert_that(sent_data, is_equal_to_contents_of(expected, sizeof(expected)));
+    EXPECT_THAT(sent_data, ElementsAreArray(expected));
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_small_packet) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_small_packet) {
     uint8_t original_data[] = { 1, 2, 3};
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
     int i;
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_small_packet_with_zeros) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_small_packet_with_zeros) {
     uint8_t original_data[] = { 1, 0, 3, 0, 0, 9};
     byte_stuffer_send_frame(1, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
     int i;
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(0, sent_data[i]);
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes) {
     uint8_t original_data[254];
     int i;
     for(i=0;i<254;i++) {
         original_data[i] = i + 1;
     }
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
     uint8_t original_data[256];
     int i;
     for(i=0;i<254;i++) {
@@ -479,16 +460,14 @@ Ensure(ByteStuffer, sends_and_receives_full_roundtrip_256_bytes) {
     original_data[254] = 22;
     original_data[255] = 23;
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
 
-Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
+TEST_F(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
     uint8_t original_data[255];
     int i;
     for(i=0;i<254;i++) {
@@ -496,11 +475,9 @@ Ensure(ByteStuffer, sends_and_receives_full_roundtrip_254_bytes_and_then_zero) {
     }
     original_data[254] = 0;
     byte_stuffer_send_frame(0, original_data, sizeof(original_data));
-    expect(validator_recv_frame,
-        when(size, is_equal_to(sizeof(original_data))),
-        when(data, is_equal_to_contents_of(original_data, sizeof(original_data)))
-    );
-    for(i=0;i<sent_data_size;i++) {
-       byte_stuffer_recv_byte(1, sent_data[i]);
+    EXPECT_CALL(*this, validator_recv_frame(_, _, _))
+        .With(Args<1, 2>(ElementsAreArray(original_data)));
+    for(auto& d : sent_data) {
+       byte_stuffer_recv_byte(1, d);
     }
 }
diff --git a/quantum/serial_link/tests/frame_router_tests.c b/quantum/serial_link/tests/frame_router_tests.c
deleted file mode 100644
index 6c806fa939..0000000000
--- a/quantum/serial_link/tests/frame_router_tests.c
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
-The MIT License (MIT)
-
-Copyright (c) 2016 Fred Sundvik
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#include <cgreen/cgreen.h>
-#include <cgreen/mocks.h>
-#include "serial_link/protocol/byte_stuffer.c"
-#include "serial_link/protocol/frame_validator.c"
-#include "serial_link/protocol/frame_router.c"
-#include "serial_link/protocol/transport.h"
-
-static uint8_t received_data[256];
-static uint16_t received_data_size;
-
-typedef struct {
-    uint8_t sent_data[256];
-    uint16_t sent_data_size;
-} receive_buffer_t;
-
-typedef struct {
-    receive_buffer_t send_buffers[2];
-} router_buffer_t;
-
-router_buffer_t router_buffers[8];
-
-router_buffer_t* current_router_buffer;
-
-
-Describe(FrameRouter);
-BeforeEach(FrameRouter) {
-    init_byte_stuffer();
-    memset(router_buffers, 0, sizeof(router_buffers));
-    current_router_buffer = 0;
-}
-AfterEach(FrameRouter) {}
-
-typedef struct {
-    uint32_t data;
-    uint8_t extra[16];
-} frame_buffer_t;
-
-
-void send_data(uint8_t link, const uint8_t* data, uint16_t size) {
-    receive_buffer_t* buffer = &current_router_buffer->send_buffers[link];
-    memcpy(buffer->sent_data + buffer->sent_data_size, data, size);
-    buffer->sent_data_size += size;
-}
-
-static void receive_data(uint8_t link, uint8_t* data, uint16_t size) {
-    int i;
-    for(i=0;i<size;i++) {
-        byte_stuffer_recv_byte(link, data[i]);
-    }
-}
-
-static void activate_router(uint8_t num) {
-    current_router_buffer = router_buffers + num;
-    router_set_master(num==0);
-}
-
-static void simulate_transport(uint8_t from, uint8_t to) {
-   activate_router(to);
-   if (from > to) {
-       receive_data(DOWN_LINK,
-               router_buffers[from].send_buffers[UP_LINK].sent_data,
-               router_buffers[from].send_buffers[UP_LINK].sent_data_size);
-   }
-   else if(to > from) {
-       receive_data(UP_LINK,
-               router_buffers[from].send_buffers[DOWN_LINK].sent_data,
-               router_buffers[from].send_buffers[DOWN_LINK].sent_data_size);
-   }
-}
-
-void transport_recv_frame(uint8_t from, uint8_t* data, uint16_t size) {
-    mock(from, data, size);
-}
-
-
-Ensure(FrameRouter, master_broadcast_is_received_by_everyone) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(0);
-    router_send_frame(0xFF, (uint8_t*)&data, 4);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(0, 1);
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 2);
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, master_send_is_received_by_targets) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(0);
-    router_send_frame((1 << 1) | (1 << 2), (uint8_t*)&data, 4);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    simulate_transport(0, 1);
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 2);
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(0)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(2, 3);
-    assert_that(router_buffers[3].send_buffers[DOWN_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[3].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, first_link_sends_to_master) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(1);
-    router_send_frame(0, (uint8_t*)&data, 4);
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(1)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 0);
-    assert_that(router_buffers[0].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-    assert_that(router_buffers[0].send_buffers[UP_LINK].sent_data_size, is_equal_to(0));
-}
-
-Ensure(FrameRouter, second_link_sends_to_master) {
-    frame_buffer_t data;
-    data.data = 0xAB7055BB;
-    activate_router(2);
-    router_send_frame(0, (uint8_t*)&data, 4);
-    assert_that(router_buffers[2].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[2].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    simulate_transport(2, 1);
-    assert_that(router_buffers[1].send_buffers[UP_LINK].sent_data_size, is_greater_than(0));
-    assert_that(router_buffers[1].send_buffers[DOWN_LINK].sent_data_size, is_equal_to(0));
-
-    expect(transport_recv_frame,
-        when(from, is_equal_to(2)),
-        when(size, is_equal_to(4)),
-        when(data, is_equal_to_contents_of(&data.data, 4))
-    );
-    simulate_transport(1, 0);
-    assert_that(router_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 0000000000..2bd5bf830d
--- /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 d20947e2c9..9223af83b0 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 0000000000..b81515bc55
--- /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 0000000000..a80e888849
--- /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 358e1b9fd4..0000000000
--- 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 0000000000..21b7b165f6
--- /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 6f7c82b468..7724bbee9c 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 3f6d133c9b..4e2a6f00fd 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 b2fb4dd98d..b16f4cd768 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 0000000000..55898147dd
--- /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
diff --git a/quantum/variable_trace.c b/quantum/variable_trace.c
new file mode 100644
index 0000000000..de580244c3
--- /dev/null
+++ b/quantum/variable_trace.c
@@ -0,0 +1,110 @@
+#include "variable_trace.h"
+#include <stddef.h>
+#include <string.h>
+
+#ifdef NO_PRINT
+#error "You need undef NO_PRINT to use the variable trace feature"
+#endif
+
+#ifndef CONSOLE_ENABLE
+#error "The console needs to be enabled in the makefile to use the variable trace feature"
+#endif
+
+
+#define NUM_TRACED_VARIABLES 1
+#ifndef MAX_VARIABLE_TRACE_SIZE
+    #define MAX_VARIABLE_TRACE_SIZE 4
+#endif
+
+typedef struct {
+    const char* name;
+    void* addr;
+    unsigned size;
+    const char* func;
+    int line;
+    uint8_t last_value[MAX_VARIABLE_TRACE_SIZE];
+
+} traced_variable_t;
+
+static traced_variable_t traced_variables[NUM_TRACED_VARIABLES];
+
+void add_traced_variable(const char* name, void* addr, unsigned size, const char* func, int line) {
+    verify_traced_variables(func, line);
+    if (size > MAX_VARIABLE_TRACE_SIZE) {
+#if defined(__AVR__)
+       xprintf("Traced variable \"%S\" exceeds the maximum size %d\n", name, size);
+#else
+       xprintf("Traced variable \"%s\" exceeds the maximum size %d\n", name, size);
+#endif
+       size = MAX_VARIABLE_TRACE_SIZE;
+    }
+    int index = -1;
+    for (int i = 0; i < NUM_TRACED_VARIABLES; i++) {
+        if (index == -1 && traced_variables[i].addr == NULL){
+            index = i;
+        }
+        else if (strcmp_P(name, traced_variables[i].name)==0) {
+            index = i;
+            break;
+        }
+    }
+
+    if (index == -1) {
+        xprintf("You can only trace %d variables at the same time\n", NUM_TRACED_VARIABLES);
+        return;
+    }
+
+    traced_variable_t* t = &traced_variables[index];
+    t->name = name;
+    t->addr = addr;
+    t->size = size;
+    t->func = func;
+    t->line = line;
+    memcpy(&t->last_value[0], addr, size);
+
+}
+
+void remove_traced_variable(const char* name, const char* func, int line) {
+    verify_traced_variables(func, line);
+    for (int i = 0; i < NUM_TRACED_VARIABLES; i++) {
+        if (strcmp_P(name, traced_variables[i].name)==0) {
+            traced_variables[i].name = 0;
+            traced_variables[i].addr = NULL;
+            break;
+        }
+    }
+}
+
+void verify_traced_variables(const char* func, int line) {
+    for (int i = 0; i < NUM_TRACED_VARIABLES; i++) {
+        traced_variable_t* t = &traced_variables[i];
+        if (t->addr != NULL && t->name != NULL) {
+            if (memcmp(t->last_value, t->addr, t->size)!=0){
+#if defined(__AVR__)
+               xprintf("Traced variable \"%S\" has been modified\n", t->name);
+               xprintf("Between %S:%d\n", t->func, t->line);
+               xprintf("And %S:%d\n", func, line);
+
+#else
+               xprintf("Traced variable \"%s\" has been modified\n", t->name);
+               xprintf("Between %s:%d\n", t->func, t->line);
+               xprintf("And %s:%d\n", func, line);
+#endif
+               xprintf("Previous value ");
+               for (int j=0; j<t->size;j++) {
+                   print_hex8(t->last_value[j]);
+               }
+               xprintf("\nNew value ");
+               uint8_t* addr = (uint8_t*)(t->addr);
+               for (int j=0; j<t->size;j++) {
+                   print_hex8(addr[j]);
+               }
+               xprintf("\n");
+               memcpy(t->last_value, addr, t->size);
+           }
+        }
+
+        t->func = func;
+        t->line = line;
+    }
+}
diff --git a/quantum/variable_trace.h b/quantum/variable_trace.h
new file mode 100644
index 0000000000..46bd827861
--- /dev/null
+++ b/quantum/variable_trace.h
@@ -0,0 +1,34 @@
+#ifndef VARIABLE_TRACE_H
+#define VARIABLE_TRACE_H
+
+// For more information about the variable tracing see the readme.
+
+#include "print.h"
+
+#ifdef NUM_TRACED_VARIABLES
+
+// Start tracing a variable at the memory address addr
+// The name can be anything and is used only for reporting
+// The size should usually be the same size as the variable you are interested in
+#define ADD_TRACED_VARIABLE(name, addr, size) \
+    add_traced_variable(PSTR(name), (void*)addr, size, PSTR(__FILE__), __LINE__)
+
+// Stop tracing the variable with the given name
+#define REMOVE_TRACED_VARIABLE(name) remove_traced_variable(PSTR(name), PSTR(__FILE__), __LINE__)
+
+// Call to get messages when the variable has been changed
+#define VERIFY_TRACED_VARIABLES() verify_traced_variables(PSTR(__FILE__), __LINE__)
+
+#else
+
+#define ADD_TRACED_VARIABLE(name, addr, size)
+#define REMOVE_TRACED_VARIABLE(name)
+#define VERIFY_TRACED_VARIABLES()
+
+#endif
+
+// Don't call directly, use the macros instead
+void add_traced_variable(const char* name, void* addr, unsigned size, const char* func, int line);
+void remove_traced_variable(const char* name, const char* func, int line);
+void verify_traced_variables(const char* func, int line);
+#endif