Merge remote-tracking branch 'refs/remotes/jackhumbert/master'

63 files changed, 4280 insertions, 1491 deletions

diff --git a/quantum/api.c b/quantum/api.c
new file mode 100644
index 0000000000..4ca3b96762
--- /dev/null
+++ b/quantum/api.c
@@ -0,0 +1,178 @@
+#include "api.h"
+#include "quantum.h"
+
+void dword_to_bytes(uint32_t dword, uint8_t * bytes) {
+    bytes[0] = (dword >> 24) & 0xFF;
+    bytes[1] = (dword >> 16) & 0xFF; 
+    bytes[2] = (dword >> 8) & 0xFF; 
+    bytes[3] = (dword >> 0) & 0xFF; 
+}
+
+uint32_t bytes_to_dword(uint8_t * bytes, uint8_t index) {
+    return ((uint32_t)bytes[index + 0] << 24) | ((uint32_t)bytes[index + 1] << 16) | ((uint32_t)bytes[index + 2] << 8) | (uint32_t)bytes[index + 3];
+}
+
+__attribute__ ((weak))
+bool process_api_quantum(uint8_t length, uint8_t * data) {
+    return process_api_keyboard(length, data);
+}
+
+__attribute__ ((weak))
+bool process_api_keyboard(uint8_t length, uint8_t * data) {
+    return process_api_user(length, data);
+}
+
+__attribute__ ((weak))
+bool process_api_user(uint8_t length, uint8_t * data) {
+    return true;
+}
+
+void process_api(uint16_t length, uint8_t * data) {
+    // SEND_STRING("\nRX: ");
+    // for (uint8_t i = 0; i < length; i++) {
+    //     send_byte(data[i]);
+    //     SEND_STRING(" ");
+    // }
+    if (!process_api_quantum(length, data))
+        return;
+
+    switch (data[0]) {
+        case MT_SET_DATA:
+            switch (data[1]) {
+                case DT_DEFAULT_LAYER: {
+                    eeconfig_update_default_layer(data[2]);
+                    default_layer_set((uint32_t)(data[2]));
+                    break;
+                }
+                case DT_KEYMAP_OPTIONS: {
+                    eeconfig_update_keymap(data[2]);
+                    break;
+                }
+                case DT_RGBLIGHT: {
+                    #ifdef RGBLIGHT_ENABLE
+                        uint32_t rgblight = bytes_to_dword(data, 2);
+                        rgblight_update_dword(rgblight);
+                    #endif
+                    break;
+                }
+            }
+        case MT_GET_DATA:
+            switch (data[1]) {
+                case DT_HANDSHAKE: {
+                    MT_GET_DATA_ACK(DT_HANDSHAKE, NULL, 0);
+                    break;
+                }
+                case DT_DEBUG: {
+                    uint8_t debug_bytes[1] = { eeprom_read_byte(EECONFIG_DEBUG) };
+                    MT_GET_DATA_ACK(DT_DEBUG, debug_bytes, 1);
+                    break;
+                }
+                case DT_DEFAULT_LAYER: {
+                    uint8_t default_bytes[1] = { eeprom_read_byte(EECONFIG_DEFAULT_LAYER) };
+                    MT_GET_DATA_ACK(DT_DEFAULT_LAYER, default_bytes, 1);
+                    break;
+                }
+                case DT_CURRENT_LAYER: {
+                    uint8_t layer_state_bytes[4];
+                    dword_to_bytes(layer_state, layer_state_bytes);
+                    MT_GET_DATA_ACK(DT_CURRENT_LAYER, layer_state_bytes, 4);
+                    break;
+                }
+                case DT_AUDIO: {
+                    #ifdef AUDIO_ENABLE
+                        uint8_t audio_bytes[1] = { eeprom_read_byte(EECONFIG_AUDIO) };
+                        MT_GET_DATA_ACK(DT_AUDIO, audio_bytes, 1);
+                    #else
+                        MT_GET_DATA_ACK(DT_AUDIO, NULL, 0);
+                    #endif
+                    break;
+                }
+                case DT_BACKLIGHT: {
+                    #ifdef BACKLIGHT_ENABLE
+                        uint8_t backlight_bytes[1] = { eeprom_read_byte(EECONFIG_BACKLIGHT) };
+                        MT_GET_DATA_ACK(DT_BACKLIGHT, backlight_bytes, 1);
+                    #else
+                        MT_GET_DATA_ACK(DT_BACKLIGHT, NULL, 0);
+                    #endif
+                    break;
+                }
+                case DT_RGBLIGHT: {
+                    #ifdef RGBLIGHT_ENABLE
+                        uint8_t rgblight_bytes[4];
+                        dword_to_bytes(eeconfig_read_rgblight(), rgblight_bytes);
+                        MT_GET_DATA_ACK(DT_RGBLIGHT, rgblight_bytes, 4);
+                    #else
+                        MT_GET_DATA_ACK(DT_RGBLIGHT, NULL, 0);
+                    #endif
+                    break;
+                }
+                case DT_KEYMAP_OPTIONS: {
+                    uint8_t keymap_bytes[1] = { eeconfig_read_keymap() };
+                    MT_GET_DATA_ACK(DT_KEYMAP_OPTIONS, keymap_bytes, 1);
+                    break;
+                }
+                case DT_KEYMAP_SIZE: {
+                    uint8_t keymap_size[2] = {MATRIX_ROWS, MATRIX_COLS};
+                    MT_GET_DATA_ACK(DT_KEYMAP_SIZE, keymap_size, 2);
+                    break;
+                }
+                case DT_KEYMAP: {
+                    uint8_t keymap_data[MATRIX_ROWS * MATRIX_COLS * 4 + 3];
+                    keymap_data[0] = data[2];
+                    keymap_data[1] = MATRIX_ROWS;
+                    keymap_data[2] = MATRIX_COLS;
+                    for (int i = 0; i < MATRIX_ROWS; i++) {
+                        for (int j = 0; j < MATRIX_COLS; j++) {
+                            keymap_data[3 + (i*MATRIX_COLS*2) + (j*2)] = pgm_read_word(&keymaps[data[2]][i][j]) >> 8;
+                            keymap_data[3 + (i*MATRIX_COLS*2) + (j*2) + 1] = pgm_read_word(&keymaps[data[2]][i][j]) & 0xFF;
+                        }
+                    }
+                    MT_GET_DATA_ACK(DT_KEYMAP, keymap_data, MATRIX_ROWS * MATRIX_COLS * 4 + 3);
+                    // uint8_t keymap_data[5];
+                    // keymap_data[0] = data[2];
+                    // keymap_data[1] = data[3];
+                    // keymap_data[2] = data[4];
+                    // keymap_data[3] = pgm_read_word(&keymaps[data[2]][data[3]][data[4]]) >> 8;
+                    // keymap_data[4] = pgm_read_word(&keymaps[data[2]][data[3]][data[4]]) & 0xFF;
+
+                    // MT_GET_DATA_ACK(DT_KEYMAP, keymap_data, 5);
+                    break;
+                }
+                default:
+                    break;
+            }
+            break;
+        case MT_SET_DATA_ACK:
+        case MT_GET_DATA_ACK:
+            break;
+        case MT_SEND_DATA:
+            break;
+        case MT_SEND_DATA_ACK:
+            break;
+        case MT_EXE_ACTION:
+            break;
+        case MT_EXE_ACTION_ACK:
+            break;
+        case MT_TYPE_ERROR:
+            break;
+        default: ; // command not recognised
+            SEND_BYTES(MT_TYPE_ERROR, DT_NONE, data, length);
+            break;
+
+        // #ifdef RGBLIGHT_ENABLE
+        // case 0x27: ; // RGB LED functions
+        //     switch (*data++) {
+        //         case 0x00: ; // Update HSV
+        //             rgblight_sethsv((data[0] << 8 | data[1]) % 360, data[2], data[3]);
+        //             break;
+        //         case 0x01: ; // Update RGB
+        //             break;
+        //         case 0x02: ; // Update mode
+        //             rgblight_mode(data[0]);
+        //             break;
+        //     }
+        //     break;
+        // #endif
+    }
+
+}
\ No newline at end of file
diff --git a/quantum/api.h b/quantum/api.h
new file mode 100644
index 0000000000..00dcdb8954
--- /dev/null
+++ b/quantum/api.h
@@ -0,0 +1,59 @@
+#ifndef _API_H_
+#define _API_H_
+
+#include "lufa.h"
+
+enum MESSAGE_TYPE {
+    MT_GET_DATA =      0x10, // Get data from keyboard
+    MT_GET_DATA_ACK =  0x11, // returned data to process (ACK)
+    MT_SET_DATA =      0x20, // Set data on keyboard
+    MT_SET_DATA_ACK =  0x21, // returned data to confirm (ACK)
+    MT_SEND_DATA =     0x30, // Sending data/action from keyboard
+    MT_SEND_DATA_ACK = 0x31, // returned data/action confirmation (ACK)
+    MT_EXE_ACTION =    0x40, // executing actions on keyboard
+    MT_EXE_ACTION_ACK =0x41, // return confirmation/value (ACK)
+    MT_TYPE_ERROR =    0x80 // type not recofgnised (ACK)
+};
+
+enum DATA_TYPE {
+    DT_NONE = 0x00,
+    DT_HANDSHAKE,
+    DT_DEFAULT_LAYER,
+    DT_CURRENT_LAYER,
+    DT_KEYMAP_OPTIONS,
+    DT_BACKLIGHT,
+    DT_RGBLIGHT,
+    DT_UNICODE,
+    DT_DEBUG,
+    DT_AUDIO,
+    DT_QUANTUM_ACTION,
+    DT_KEYBOARD_ACTION,
+    DT_USER_ACTION,
+    DT_KEYMAP_SIZE,
+    DT_KEYMAP
+};
+
+void dword_to_bytes(uint32_t dword, uint8_t * bytes);
+uint32_t bytes_to_dword(uint8_t * bytes, uint8_t index);
+
+#define MT_GET_DATA(data_type, data, length) SEND_BYTES(MT_GET_DATA, data_type, data, length)
+#define MT_GET_DATA_ACK(data_type, data, length) SEND_BYTES(MT_GET_DATA_ACK, data_type, data, length)
+#define MT_SET_DATA(data_type, data, length) SEND_BYTES(MT_SET_DATA, data_type, data, length)
+#define MT_SET_DATA_ACK(data_type, data, length) SEND_BYTES(MT_SET_DATA_ACK, data_type, data, length)
+#define MT_SEND_DATA(data_type, data, length) SEND_BYTES(MT_SEND_DATA, data_type, data, length)
+#define MT_SEND_DATA_ACK(data_type, data, length) SEND_BYTES(MT_SEND_DATA_ACK, data_type, data, length)
+#define MT_EXE_ACTION(data_type, data, length) SEND_BYTES(MT_EXE_ACTION, data_type, data, length)
+#define MT_EXE_ACTION_ACK(data_type, data, length) SEND_BYTES(MT_EXE_ACTION_ACK, data_type, data, length)
+
+void process_api(uint16_t length, uint8_t * data);
+
+__attribute__ ((weak))
+bool process_api_quantum(uint8_t length, uint8_t * data);
+
+__attribute__ ((weak))
+bool process_api_keyboard(uint8_t length, uint8_t * data);
+
+__attribute__ ((weak))
+bool process_api_user(uint8_t length, uint8_t * data);
+
+#endif
\ No newline at end of file
diff --git a/quantum/api/api_sysex.c b/quantum/api/api_sysex.c
new file mode 100644
index 0000000000..a4a554e764
--- /dev/null
+++ b/quantum/api/api_sysex.c
@@ -0,0 +1,29 @@
+#include "api_sysex.h"
+
+void send_bytes_sysex(uint8_t message_type, uint8_t data_type, uint8_t * bytes, uint16_t length) {
+    // SEND_STRING("\nTX: ");
+    // for (uint8_t i = 0; i < length; i++) {
+    //     send_byte(bytes[i]);
+    //     SEND_STRING(" ");
+    // }
+    uint8_t * precode = malloc(sizeof(uint8_t) * (length + 2));
+    precode[0] = message_type;
+    precode[1] = data_type;
+    memcpy(precode + 2, bytes, length);
+    uint8_t * encoded = malloc(sizeof(uint8_t) * (sysex_encoded_length(length + 2)));
+    uint16_t encoded_length = sysex_encode(encoded, precode, length + 2);
+    uint8_t * array = malloc(sizeof(uint8_t) * (encoded_length + 5));
+    array[0] = 0xF0;
+    array[1] = 0x00;
+    array[2] = 0x00;
+    array[3] = 0x00;
+    array[encoded_length + 4] = 0xF7;
+    memcpy(array + 4, encoded, encoded_length);
+    midi_send_array(&midi_device, encoded_length + 5, array);
+
+    // SEND_STRING("\nTD: ");
+    // for (uint8_t i = 0; i < encoded_length + 5; i++) {
+    //     send_byte(array[i]);
+    //     SEND_STRING(" ");
+    // }
+}
\ No newline at end of file
diff --git a/quantum/api/api_sysex.h b/quantum/api/api_sysex.h
new file mode 100644
index 0000000000..b947b60e54
--- /dev/null
+++ b/quantum/api/api_sysex.h
@@ -0,0 +1,10 @@
+#ifndef _API_SYSEX_H_
+#define _API_SYSEX_H_
+
+#include "api.h"
+
+void send_bytes_sysex(uint8_t message_type, uint8_t data_type, uint8_t * bytes, uint16_t length);
+
+#define SEND_BYTES(mt, dt, b, l) send_bytes_sysex(mt, dt, b, l)
+
+#endif
\ No newline at end of file
diff --git a/quantum/audio/song_list.h b/quantum/audio/song_list.h
index fc6fcdeef1..8022ca6729 100644
--- a/quantum/audio/song_list.h
+++ b/quantum/audio/song_list.h
@@ -28,6 +28,14 @@
 	Q__NOTE(_E4), Q__NOTE(_C4), \
 	Q__NOTE(_E4),
 
+/* Requires: PLAY_NOTE_ARRAY(..., ..., STACCATO); */
+#define IN_LIKE_FLINT \
+    E__NOTE(_AS4), E__NOTE(_AS4), QD_NOTE(_B4),  \
+    E__NOTE(_AS4), E__NOTE(_B4),  QD_NOTE(_CS4), \
+    E__NOTE(_B4),  E__NOTE(_CS4), QD_NOTE(_DS4), \
+    E__NOTE(_CS4), E__NOTE(_B4),  QD_NOTE(_AS4), \
+    E__NOTE(_AS4), E__NOTE(_AS4), QD_NOTE(_B4),
+
 #define GOODBYE_SOUND \
     E__NOTE(_E7),     \
     E__NOTE(_A6),     \
diff --git a/quantum/audio/voices.c b/quantum/audio/voices.c
index 6d4172a06c..19f7b646ef 100644
--- a/quantum/audio/voices.c
+++ b/quantum/audio/voices.c
@@ -18,7 +18,7 @@ void voice_iterate() {
 }
 
 void voice_deiterate() {
-    voice = (voice - 1) % number_of_voices;
+    voice = (voice - 1 + number_of_voices) % number_of_voices;
 }
 
 float voice_envelope(float frequency) {
@@ -31,6 +31,27 @@ float voice_envelope(float frequency) {
             polyphony_rate = 0;
 	        break;
 
+        case something:
+            polyphony_rate = 0;
+            switch (compensated_index) {
+                case 0 ... 9:
+                    note_timbre = TIMBRE_12;
+                    break;
+
+                case 10 ... 19:
+                    note_timbre = TIMBRE_25;
+                    break;
+
+                case 20 ... 200:
+                    note_timbre = .25 + .125 + pow(((float)compensated_index - 20) / (200 - 20), 2)*.125;
+                    break;
+
+                default:
+                    note_timbre = .25;
+                    break;
+            }
+            break;
+
         case butts_fader:
             polyphony_rate = 0;
             switch (compensated_index) {
diff --git a/quantum/audio/voices.h b/quantum/audio/voices.h
index b2495b23b5..b43def3d7d 100644
--- a/quantum/audio/voices.h
+++ b/quantum/audio/voices.h
@@ -11,6 +11,7 @@ float voice_envelope(float frequency);
 
 typedef enum {
     default_voice,
+    something,
     butts_fader,
     octave_crunch,
     duty_osc,
diff --git a/quantum/config_common.h b/quantum/config_common.h
index 09a4fe7010..17c11faeb6 100644
--- a/quantum/config_common.h
+++ b/quantum/config_common.h
@@ -5,46 +5,56 @@
 #define COL2ROW 0
 #define ROW2COL 1
 /* I/O pins */
-#define B0 0x30
-#define B1 0x31
-#define B2 0x32
-#define B3 0x33
-#define B4 0x34
-#define B5 0x35
-#define B6 0x36
-#define B7 0x37
-#define C0 0x60
-#define C1 0x61
-#define C2 0x62
-#define C3 0x63
-#define C4 0x64
-#define C5 0x65
-#define C6 0x66
-#define C7 0x67
-#define D0 0x90
-#define D1 0x91
-#define D2 0x92
-#define D3 0x93
-#define D4 0x94
-#define D5 0x95
-#define D6 0x96
-#define D7 0x97
-#define E0 0xC0
-#define E1 0xC1
-#define E2 0xC2
-#define E3 0xC3
-#define E4 0xC4
-#define E5 0xC5
-#define E6 0xC6
-#define E7 0xC7
-#define F0 0xF0
-#define F1 0xF1
-#define F2 0xF2
-#define F3 0xF3
-#define F4 0xF4
-#define F5 0xF5
-#define F6 0xF6
-#define F7 0xF7
+#ifndef F0
+    #define B0 0x30
+    #define B1 0x31
+    #define B2 0x32
+    #define B3 0x33
+    #define B4 0x34
+    #define B5 0x35
+    #define B6 0x36
+    #define B7 0x37
+    #define C0 0x60
+    #define C1 0x61
+    #define C2 0x62
+    #define C3 0x63
+    #define C4 0x64
+    #define C5 0x65
+    #define C6 0x66
+    #define C7 0x67
+    #define D0 0x90
+    #define D1 0x91
+    #define D2 0x92
+    #define D3 0x93
+    #define D4 0x94
+    #define D5 0x95
+    #define D6 0x96
+    #define D7 0x97
+    #define E0 0xC0
+    #define E1 0xC1
+    #define E2 0xC2
+    #define E3 0xC3
+    #define E4 0xC4
+    #define E5 0xC5
+    #define E6 0xC6
+    #define E7 0xC7
+    #define F0 0xF0
+    #define F1 0xF1
+    #define F2 0xF2
+    #define F3 0xF3
+    #define F4 0xF4
+    #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
+#endif
 
 /* USART configuration */
 #ifdef BLUETOOTH_ENABLE
@@ -67,53 +77,7 @@
         } while(0)
 #   else
 #       error "USART configuration is needed."
-#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
diff --git a/quantum/dynamic_macro.h b/quantum/dynamic_macro.h
new file mode 100644
index 0000000000..e6dbc5b9c1
--- /dev/null
+++ b/quantum/dynamic_macro.h
@@ -0,0 +1,231 @@
+/* Author: Wojciech Siewierski < wojciech dot siewierski at onet dot pl > */
+#ifndef DYNAMIC_MACROS_H
+#define DYNAMIC_MACROS_H
+
+#include "action_layer.h"
+
+#ifndef DYNAMIC_MACRO_SIZE
+/* May be overridden with a custom value. Be aware that the effective
+ * macro length is half of this value: each keypress is recorded twice
+ * because of the down-event and up-event. This is not a bug, it's the
+ * intended behavior.
+ *
+ * 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
+ * "planck_keycodes" enum prior to including this header. This allows
+ * us to 'extend' it.
+ */
+enum dynamic_macro_keycodes {
+    DYN_REC_START1 = DYNAMIC_MACRO_RANGE,
+    DYN_REC_START2,
+    DYN_MACRO_PLAY1,
+    DYN_MACRO_PLAY2,
+};
+
+/* Blink the LEDs to notify the user about some event. */
+void dynamic_macro_led_blink(void)
+{
+    backlight_toggle();
+    _delay_ms(100);
+    backlight_toggle();
+}
+
+/**
+ * Start recording of the dynamic macro.
+ *
+ * @param[out] macro_pointer The new macro buffer iterator.
+ * @param[in]  macro_buffer  The macro buffer used to initialize macro_pointer.
+ */
+void dynamic_macro_record_start(
+    keyrecord_t **macro_pointer, keyrecord_t *macro_buffer)
+{
+    dynamic_macro_led_blink();
+
+    clear_keyboard();
+    layer_clear();
+    *macro_pointer = macro_buffer;
+}
+
+/**
+ * Play the dynamic macro.
+ *
+ * @param macro_buffer[in] The beginning of the macro buffer being played.
+ * @param macro_end[in]    The element after the last macro buffer element.
+ * @param direction[in]    Either +1 or -1, which way to iterate the buffer.
+ */
+void dynamic_macro_play(
+    keyrecord_t *macro_buffer, keyrecord_t *macro_end, int8_t direction)
+{
+    uint32_t saved_layer_state = layer_state;
+
+    clear_keyboard();
+    layer_clear();
+
+    while (macro_buffer != macro_end) {
+        process_record(macro_buffer);
+        macro_buffer += direction;
+    }
+
+    clear_keyboard();
+
+    layer_state = saved_layer_state;
+}
+
+/**
+ * Record a single key in a dynamic macro.
+ *
+ * @param macro_pointer[in,out] The current buffer position.
+ * @param macro_end2[in] The end of the other macro which shouldn't be overwritten.
+ * @param direction[in]  Either +1 or -1, which way to iterate the buffer.
+ * @param record[in]     The current keypress.
+ */
+void dynamic_macro_record_key(
+    keyrecord_t **macro_pointer,
+    keyrecord_t *macro_end2,
+    int8_t direction,
+    keyrecord_t *record)
+{
+    if (*macro_pointer + direction != macro_end2) {
+        **macro_pointer = *record;
+        *macro_pointer += direction;
+    } else {
+        /* Notify about the end of buffer. The blinks are paired
+         * because they should happen on both down and up events. */
+        backlight_toggle();
+    }
+}
+
+/**
+ * End recording of the dynamic macro. Essentially just update the
+ * pointer to the end of the macro.
+ */
+void dynamic_macro_record_end(keyrecord_t *macro_pointer, keyrecord_t **macro_end)
+{
+    dynamic_macro_led_blink();
+
+    *macro_end = macro_pointer;
+}
+
+/* Handle the key events related to the dynamic macros. Should be
+ * called from process_record_user() like this:
+ *
+ *   bool process_record_user(uint16_t keycode, keyrecord_t *record) {
+ *       if (!process_record_dynamic_macro(keycode, record)) {
+ *           return false;
+ *       }
+ *       <...THE REST OF THE FUNCTION...>
+ *   }
+ */
+bool process_record_dynamic_macro(uint16_t keycode, keyrecord_t *record)
+{
+    /* Both macros use the same buffer but read/write on different
+     * ends of it.
+     *
+     * Macro1 is written left-to-right starting from the beginning of
+     * the buffer.
+     *
+     * Macro2 is written right-to-left starting from the end of the
+     * buffer.
+     *
+     * &macro_buffer   macro_end
+     *  v                   v
+     * +------------------------------------------------------------+
+     * |>>>>>> MACRO1 >>>>>>|    |<<<<<<<<<<<<< MACRO2 <<<<<<<<<<<<<|
+     * +------------------------------------------------------------+
+     *                           ^                                 ^
+     *                         r_macro_end                  r_macro_buffer
+     *
+     * During the recording when one macro encounters the end of the
+     * other macro, the recording is stopped. Apart from this, there
+     * are no arbitrary limits for the macros' length in relation to
+     * each other: for example one can either have two medium sized
+     * macros or one long macro and one short macro. Or even one empty
+     * and one using the whole buffer.
+     */
+    static keyrecord_t macro_buffer[DYNAMIC_MACRO_SIZE];
+
+    /* Pointer to the first buffer element after the first macro.
+     * Initially points to the very beginning of the buffer since the
+     * macro is empty. */
+    static keyrecord_t *macro_end = macro_buffer;
+
+    /* The other end of the macro buffer. Serves as the beginning of
+     * the second macro. */
+    static keyrecord_t *const r_macro_buffer = macro_buffer + DYNAMIC_MACRO_SIZE - 1;
+
+    /* Like macro_end but for the second macro. */
+    static keyrecord_t *r_macro_end = r_macro_buffer;
+
+    /* A persistent pointer to the current macro position (iterator)
+     * used during the recording. */
+    static keyrecord_t *macro_pointer = NULL;
+
+    /* 0   - no macro is being recorded right now
+     * 1,2 - either macro 1 or 2 is being recorded */
+    static uint8_t macro_id = 0;
+
+    if (macro_id == 0) {
+        /* No macro recording in progress. */
+        if (!record->event.pressed) {
+            switch (keycode) {
+            case DYN_REC_START1:
+                dynamic_macro_record_start(&macro_pointer, macro_buffer);
+                macro_id = 1;
+                return false;
+            case DYN_REC_START2:
+                dynamic_macro_record_start(&macro_pointer, r_macro_buffer);
+                macro_id = 2;
+                return false;
+            case DYN_MACRO_PLAY1:
+                dynamic_macro_play(macro_buffer, macro_end, +1);
+                return false;
+            case DYN_MACRO_PLAY2:
+                dynamic_macro_play(r_macro_buffer, r_macro_end, -1);
+                return false;
+            }
+        }
+    } else {
+        /* A macro is being recorded right now. */
+        switch (keycode) {
+        case MO(_DYN):
+            /* Use the layer key used to access the macro recording as
+             * a stop button. */
+            if (record->event.pressed) { /* Ignore the initial release
+                                          * just after the recoding
+                                          * starts. */
+                switch (macro_id) {
+                case 1:
+                    dynamic_macro_record_end(macro_pointer, &macro_end);
+                    break;
+                case 2:
+                    dynamic_macro_record_end(macro_pointer, &r_macro_end);
+                    break;
+                }
+                macro_id = 0;
+            }
+            return false;
+        default:
+            /* Store the key in the macro buffer and process it normally. */
+            switch (macro_id) {
+            case 1:
+                dynamic_macro_record_key(&macro_pointer, r_macro_end, +1, record);
+                break;
+            case 2:
+                dynamic_macro_record_key(&macro_pointer, macro_end, -1, record);
+                break;
+            }
+            return true;
+            break;
+        }
+    }
+
+    return true;
+}
+
+#endif
diff --git a/quantum/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 a158651839..ae56d16c75 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
@@ -156,16 +161,16 @@ enum quantum_keycodes {
     BL_INC,
     BL_TOGG,
     BL_STEP,
-	
-	// RGB functionality
-	RGB_TOG,
-	RGB_MOD,
-	RGB_HUI,
-	RGB_HUD,
-	RGB_SAI,
-	RGB_SAD,
-	RGB_VAI,
-	RGB_VAD,
+
+    // RGB functionality
+    RGB_TOG,
+    RGB_MOD,
+    RGB_HUI,
+    RGB_HUD,
+    RGB_SAI,
+    RGB_SAD,
+    RGB_VAI,
+    RGB_VAD,
 
     // Left shift, open paren
     KC_LSPO,
@@ -173,6 +178,10 @@ enum quantum_keycodes {
     // Right shift, close paren
     KC_RSPC,
 
+    // Printing
+    PRINT_ON,
+    PRINT_OFF,
+
     // always leave at the end
     SAFE_RANGE
 };
@@ -190,6 +199,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 +304,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)
@@ -309,7 +322,7 @@ enum quantum_keycodes {
 #define OSL(layer) (layer | QK_ONE_SHOT_LAYER)
 
 // One-shot mod
-#define OSM(layer) (layer | QK_ONE_SHOT_MOD)
+#define OSM(mod) (mod | QK_ONE_SHOT_MOD)
 
 // M-od, T-ap - 256 keycode max
 #define MT(mod, kc) (kc | QK_MOD_TAP | ((mod & 0xF) << 8))
@@ -334,5 +347,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_common.c b/quantum/keymap_common.c
index d0a8312c1e..833e5a8f8d 100644
--- a/quantum/keymap_common.c
+++ b/quantum/keymap_common.c
@@ -30,7 +30,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "quantum.h"
 
 #ifdef MIDI_ENABLE
-	#include "keymap_midi.h"
+	#include "process_midi.h"
 #endif
 
 extern keymap_config_t keymap_config;
diff --git a/quantum/keymap_extras/keymap_bepo.h b/quantum/keymap_extras/keymap_bepo.h
index 4c30960547..e5ef39552a 100644
--- a/quantum/keymap_extras/keymap_bepo.h
+++ b/quantum/keymap_extras/keymap_bepo.h
@@ -118,7 +118,7 @@
 // Fourth row
 #define BP_COLON    LSFT(BP_DOT)    // :
 #define BP_COLN     BP_COLON
-#define BP_QUESTION LSFT(BP_QUOTE)  // ?
+#define BP_QUESTION LSFT(BP_APOS)  // ?
 #define BP_QEST     BP_QUESTION
 
 // Space bar
@@ -183,7 +183,7 @@
 // Third row
 #define BP_AE_LIGATURE              ALTGR(BP_A)         // æ
 #define BP_AE                       BP_AE_LIGATURE
-#define BP_U_GRAVE                  AGR(BP_U)           // ù
+#define BP_U_GRAVE                  ALTGR(BP_U)           // ù
 #define BP_UGRV                     BP_U_GRAVE
 #define BP_DEAD_TREMA               ALTGR(BP_I)         // dead ¨ (trema/umlaut/diaresis)
 #define BP_DTRM                     BP_DEAD_TREMA
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 e855056e83..a0feed850d 100644
--- a/quantum/keymap_extras/keymap_dvorak.h
+++ b/quantum/keymap_extras/keymap_dvorak.h
@@ -18,18 +18,19 @@
 #define DV_LBRC	KC_MINS
 #define DV_RBRC	KC_EQL
 
-#define DV_QUOT KC_Q
+#define DV_QUOT	KC_Q
 #define DV_COMM	KC_W
 #define DV_DOT	KC_E
 #define DV_P	KC_R
 #define DV_Y	KC_T
 #define	DV_F	KC_Y
 #define DV_G	KC_U
-#define DV_C 	KC_I
+#define DV_C	KC_I
 #define	DV_R	KC_O
 #define DV_L	KC_P
 #define DV_SLSH	KC_LBRC
 #define DV_EQL	KC_RBRC
+#define DV_BSLS	KC_BSLS
 
 #define DV_A	KC_A
 #define DV_O	KC_S
@@ -68,7 +69,17 @@
 #define DV_RPRN	LSFT(DV_0)
 #define DV_LCBR	LSFT(DV_LBRC)
 #define DV_RCBR	LSFT(DV_RBRC)
-#define DV_UNDS LSFT(DV_MINS)
-#define DV_PLUS LSFT(DV_EQL)
+
+#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)
+
+#define DV_UNDS	LSFT(DV_MINS)
+
+#define DV_COLN	LSFT(DV_SCLN)
 
 #endif
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_fr_ch.h b/quantum/keymap_extras/keymap_fr_ch.h
index 3fd9713575..87d4bb24cf 100644
--- a/quantum/keymap_extras/keymap_fr_ch.h
+++ b/quantum/keymap_extras/keymap_fr_ch.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define FR_CH_ALGR KC_RALT
 
 // normal characters
diff --git a/quantum/keymap_extras/keymap_french.h b/quantum/keymap_extras/keymap_french.h
index 2a44c80b14..834c69650e 100644
--- a/quantum/keymap_extras/keymap_french.h
+++ b/quantum/keymap_extras/keymap_french.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define NO_ALGR KC_RALT
 
 // Normal characters
@@ -80,4 +80,4 @@
 #define FR_EURO	ALGR(KC_E)
 #define FR_BULT	ALGR(FR_DLR)
 
-#endif
\ No newline at end of file
+#endif
diff --git a/quantum/keymap_extras/keymap_german.h b/quantum/keymap_extras/keymap_german.h
index 3f9ae8bade..7e2e0ed44e 100644
--- a/quantum/keymap_extras/keymap_german.h
+++ b/quantum/keymap_extras/keymap_german.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define DE_ALGR KC_RALT
 
 // normal characters
diff --git a/quantum/keymap_extras/keymap_german_ch.h b/quantum/keymap_extras/keymap_german_ch.h
index 6a782bcd7b..b66d582a44 100644
--- a/quantum/keymap_extras/keymap_german_ch.h
+++ b/quantum/keymap_extras/keymap_german_ch.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define CH_ALGR KC_RALT
 
 // normal characters
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/keymap_extras/keymap_nordic.h b/quantum/keymap_extras/keymap_nordic.h
index 3acb8b6983..9b0ef35ca9 100644
--- a/quantum/keymap_extras/keymap_nordic.h
+++ b/quantum/keymap_extras/keymap_nordic.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define NO_ALGR KC_RALT
 
 // Normal characters
@@ -13,7 +13,7 @@
 #define NO_ACUT	KC_EQL
 
 #define NO_AM	KC_LBRC
-#define NO_QUOT	KC_RBRC
+#define NO_QUOT	KC_RBRC // this is the "umlaut" char on Nordic keyboards, Apple layout
 #define NO_AE	KC_SCLN
 #define NO_OSLH	KC_QUOT
 #define	NO_APOS	KC_NUHS
@@ -25,7 +25,7 @@
 #define NO_SECT LSFT(NO_HALF)
 #define NO_QUO2	LSFT(KC_2)
 #define NO_BULT LSFT(KC_4)
-#define NO_AMP	LSFT(KC_6)
+#define NO_AMPR	LSFT(KC_6)
 #define NO_SLSH LSFT(KC_7)
 #define NO_LPRN	LSFT(KC_8)
 #define NO_RPRN	LSFT(KC_9)
diff --git a/quantum/keymap_extras/keymap_norwegian.h b/quantum/keymap_extras/keymap_norwegian.h
index 018bfeae59..5c4e8c4955 100644
--- a/quantum/keymap_extras/keymap_norwegian.h
+++ b/quantum/keymap_extras/keymap_norwegian.h
@@ -13,7 +13,7 @@
 #undef NO_BSLS
 #define NO_BSLS KC_EQL  // '\'
 #undef NO_CIRC
-#define NO_CIRC LSFT(C_RBRC)  // ^
+#define NO_CIRC LSFT(KC_RBRC)  // ^
 #undef NO_GRV
 #define NO_GRV	LSFT(NO_BSLS)  //
 #undef NO_OSLH
diff --git a/quantum/keymap_extras/keymap_russian.h b/quantum/keymap_extras/keymap_russian.h
new file mode 100644
index 0000000000..237e9abde3
--- /dev/null
+++ b/quantum/keymap_extras/keymap_russian.h
@@ -0,0 +1,77 @@
+#ifndef KEYMAP_RUSSIAN_H
+#define KEYMAP_RUSSIAN_H
+
+#include "keymap.h"
+
+// Normal Chracters      // reg   SHIFT
+#define RU_A     KC_F    // а and А
+#define RU_BE    KC_COMM // б and Б
+#define RU_VE    KC_D    // в and В
+#define RU_GHE   KC_U    // г and Г
+#define RU_DE    KC_L    // д and Д
+#define RU_IE    KC_T    // е and Е
+#define RU_IO    KC_GRV  // ё and Ё
+#define RU_ZHE   KC_SCLN // ж and Ж
+#define RU_ZE    KC_P    // з and З
+#define RU_I     KC_B    // и and И
+#define RU_SRT_I KC_Q    // й and Й
+#define RU_KA    KC_R    // к and К
+#define RU_EL    KC_K    // л and Л
+#define RU_EM    KC_V    // м and М
+#define RU_EN    KC_Y    // н and Н
+#define RU_O     KC_J    // о and О
+#define RU_PE    KC_G    // п and П
+#define RU_ER    KC_H    // р and Р
+#define RU_ES    KC_C    // с and С
+#define RU_TE    KC_N    // т and Т
+#define RU_U     KC_E    // у and У
+#define RU_EF    KC_A    // ф and Ф
+#define RU_HA    KC_LBRC // х and Х
+#define RU_TSE   KC_W    // ц and Ц
+#define RU_CHE   KC_X    // ч and Ч
+#define RU_SHA   KC_I    // ш and Ш
+#define RU_SHCHA KC_O    // щ and Щ
+#define RU_HSIGN KC_RBRC // ъ and Ъ
+#define RU_YERU  KC_S    // ы and Ы
+#define RU_SSIGN KC_M    // ь and Ь
+#define RU_E     KC_QUOT // э and Э
+#define RU_YU    KC_DOT  // ю and Ю
+#define RU_YA    KC_Z    // я and Я
+
+#define RU_1     KC_1    // 1 and !
+#define RU_2     KC_2    // 2 and "
+#define RU_3     KC_3    // 3 and №
+#define RU_4     KC_4    // 4 and ;
+#define RU_5     KC_5    // 5 and %
+#define RU_6     KC_6    // 6 and :
+#define RU_7     KC_7    // 7 and ?
+#define RU_8     KC_8    // 8 and *
+#define RU_9     KC_9    // 9 and (
+#define RU_0     KC_0    // 0 and )
+
+#define RU_MINS  KC_MINS // - and _
+#define RU_EQL   KC_EQL  // = and +
+#define RU_BSLS  KC_BSLS // \ and /
+#define RU_DOT   KC_SLSH // . and ,
+
+// Shifted Chracters
+#define RU_EXLM LSFT(RU_1) // !
+#define RU_DQUT LSFT(RU_2) // "
+#define RU_NMRO LSFT(RU_3) // №
+#define RU_SCLN LSFT(RU_4) // ;
+#define RU_PERC LSFT(RU_5) // %
+#define RU_COLN LSFT(RU_6) // :
+#define RU_QUES LSFT(RU_7) // ?
+#define RU_ASTR LSFT(RU_8) // *
+#define RU_LPRN LSFT(RU_9) // (
+#define RU_RPRN LSFT(RU_0) // )
+
+#define RU_UNDR LSFT(RU_MINS) // _
+#define RU_PLUS LSFT(RU_EQL)  // +
+#define RU_SLSH LSFT(RU_BSLS) // /
+#define RU_COMM LSFT(RU_DOT)  // ,
+
+// Alt Gr-ed characters
+#define RU_RUBL RALT(RU_8) // ₽
+
+#endif
diff --git a/quantum/keymap_extras/keymap_spanish.h b/quantum/keymap_extras/keymap_spanish.h
index af76e39fcb..4ba568af23 100644
--- a/quantum/keymap_extras/keymap_spanish.h
+++ b/quantum/keymap_extras/keymap_spanish.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define NO_ALGR KC_RALT
 
 // Normal characters
diff --git a/quantum/keymap_extras/keymap_uk.h b/quantum/keymap_extras/keymap_uk.h
index 5c5d951791..00c87afc3e 100644
--- a/quantum/keymap_extras/keymap_uk.h
+++ b/quantum/keymap_extras/keymap_uk.h
@@ -4,7 +4,7 @@
 #include "keymap.h"
 
 // Alt gr
-#define ALGR(kc) kc | 0x1400
+#define ALGR(kc) RALT(kc)
 #define NO_ALGR KC_RALT
 
 // Normal characters
@@ -33,4 +33,4 @@
 
 #define UK_AACT ALGR(KC_A)
 
-#endif
\ No newline at end of file
+#endif
diff --git a/quantum/keymap_extras/keymap_unicode_cyrillic.h b/quantum/keymap_extras/keymap_unicode_cyrillic.h
new file mode 100644
index 0000000000..a40626d911
--- /dev/null
+++ b/quantum/keymap_extras/keymap_unicode_cyrillic.h
@@ -0,0 +1,163 @@
+#ifndef KEYMAP_CYRILLIC_H
+#define KEYMAP_CYRILLIC_H
+
+#include "keymap.h"
+
+/*
+ * This is based off of
+ * https://en.wikipedia.org/wiki/Cyrillic_script
+ *
+ * Unicode is iffy, a software implementation is preferred
+ */
+
+// Capital                   Char russian/ukrainian/bulgarian
+#define CY_A     UC(0x0410) // А  rus ukr bul
+#define CY_BE    UC(0x0411) // Б  rus ukr bul
+#define CY_VE    UC(0x0412) // В  rus ukr bul
+#define CY_GHE   UC(0x0413) // Г  rus ukr bul
+#define CY_GHEUP UC(0x0490) // Ґ      ukr
+#define CY_DE    UC(0x0414) // Д  rus ukr bul
+#define CY_DJE   UC(0x0402) // Ђ
+#define CY_GJE   UC(0x0403) // Ѓ
+#define CY_IE    UC(0x0415) // Е  rus ukr bul
+#define CY_IO    UC(0x0401) // Ё  rus
+#define CY_UIE   UC(0x0404) // Є      ukr
+#define CY_ZHE   UC(0x0416) // Ж  rus ukr bul
+#define CY_ZE    UC(0x0417) // З  rus ukr bul
+#define CY_DZE   UC(0x0405) // Ѕ
+#define CY_I     UC(0x0418) // И  rus ukr bul
+#define CY_B_U_I UC(0x0406) // І      ukr
+#define CY_YI    UC(0x0407) // Ї      ukr
+#define CY_SRT_I UC(0x0419) // Й  rus ukr bul
+#define CY_JE    UC(0x0408) // Ј
+#define CY_KA    UC(0x041a) // К  rus ukr bul
+#define CY_EL    UC(0x041b) // Л  rus ukr bul
+#define CY_LJE   UC(0x0409) // Љ
+#define CY_EM    UC(0x041c) // М  rus ukr bul
+#define CY_EN    UC(0x041d) // Н  rus ukr bul
+#define CY_NJE   UC(0x040a) // Њ
+#define CY_O     UC(0x041e) // О  rus ukr bul
+#define CY_PE    UC(0x041f) // П  rus ukr bul
+#define CY_ER    UC(0x0420) // Р  rus ukr bul
+#define CY_ES    UC(0x0421) // С  rus ukr bul
+#define CY_TE    UC(0x0422) // Т  rus ukr bul
+#define CY_TSHE  UC(0x040b) // Ћ
+#define CY_KJE   UC(0x040c) // Ќ
+#define CY_U     UC(0x0423) // У  rus ukr bul
+#define CY_SRT_U UC(0x040e) // Ў
+#define CY_EF    UC(0x0424) // Ф  rus ukr bul
+#define CY_HA    UC(0x0425) // Х  rus     bul
+#define CY_TSE   UC(0x0426) // Ц  rus ukr bul
+#define CY_CHE   UC(0x0427) // Ч  rus ukr bul
+#define CY_DZHE  UC(0x040f) // Џ
+#define CY_SHA   UC(0x0428) // Ш  rus ukr bul
+#define CY_SHCHA UC(0x0429) // Щ  rus ukr bul
+#define CY_HSIGN UC(0x042a) // Ъ  rus     bul
+#define CY_YERU  UC(0x042b) // Ы  rus
+#define CY_SSIGN UC(0x042c) // Ь  rus ukr bul
+#define CY_E     UC(0x042d) // Э  rus
+#define CY_YU    UC(0x042e) // Ю  rus ukr bul
+#define CY_YA    UC(0x042f) // Я  rus ukr bul
+// Important Cyrillic non-Slavic letters
+#define CY_PALOCHKA UC(0x04c0) // Ӏ
+#define CY_SCHWA UC(0x04d8) // Ә
+#define CY_GHE_S UC(0x0492) // Ғ
+#define CY_ZE_D  UC(0x0498) // Ҙ
+#define CY_ES_D  UC(0x04aa) // Ҫ
+#define CY_BR_KA UC(0x04a0) // Ҡ
+#define CY_ZHE_D UC(0x0496) // Җ
+#define CY_KA_D  UC(0x049a) // Қ
+#define CY_EN_D  UC(0x04a2) // Ң
+#define CY_ENGHE UC(0x04a4) // Ҥ
+#define CY_BRD_O UC(0x04e8) // Ө
+#define CY_STR_U UC(0x04ae) // Ү
+#define CY_S_U_S UC(0x04b0) // Ұ
+#define CY_SHHA  UC(0x04ba) // Һ
+#define CY_HA_D  UC(0x04b2) // Ҳ
+
+
+// Small
+#define CY_a     UC(0x0430) // a  rus ukr bul
+#define CY_be    UC(0x0431) // б  rus ukr bul
+#define CY_ve    UC(0x0432) // в  rus ukr bul
+#define CY_ghe   UC(0x0433) // г  rus ukr bul
+#define CY_gheup UC(0x0491) // ґ      ukr
+#define CY_de    UC(0x0434) // д  rus ukr bul
+#define CY_dje   UC(0x0452) // ђ
+#define CY_gje   UC(0x0453) // ѓ
+#define CY_ie    UC(0x0435) // е  rus ukr bul
+#define CY_io    UC(0x0451) // ё  rus
+#define CY_uie   UC(0x0454) // є      ukr
+#define CY_zhe   UC(0x0436) // ж  rus ukr bul
+#define CY_ze    UC(0x0437) // з  rus ukr bul
+#define CY_dze   UC(0x0455) // ѕ
+#define CY_i     UC(0x0438) // и  rus ukr bul
+#define CY_b_u_i UC(0x0456) // і      ukr
+#define CY_yi    UC(0x0457) // ї      ukr
+#define CY_srt_i UC(0x0439) // й  rus ukr bul
+#define CY_je    UC(0x0458) // ј
+#define CY_ka    UC(0x043a) // к  rus ukr bul
+#define CY_el    UC(0x043b) // л  rus ukr bul
+#define CY_lje   UC(0x0459) // љ
+#define CY_em    UC(0x043c) // м  rus ukr bul
+#define CY_en    UC(0x043d) // н  rus ukr bul
+#define CY_nje   UC(0x045a) // њ
+#define CY_o     UC(0x043e) // о  rus ukr bul
+#define CY_pe    UC(0x043f) // п  rus ukr bul
+#define CY_er    UC(0x0440) // р  rus ukr bul
+#define CY_es    UC(0x0441) // с  rus ukr bul
+#define CY_te    UC(0x0442) // т  rus ukr bul
+#define CY_tshe  UC(0x045b) // ћ
+#define CY_kje   UC(0x045c) // ќ
+#define CY_u     UC(0x0443) // у  rus ukr bul
+#define CY_srt_u UC(0x045e) // ў
+#define CY_ef    UC(0x0444) // ф  rus ukr bul
+#define CY_ha    UC(0x0445) // х  rus ukr bul
+#define CY_tse   UC(0x0446) // ц  rus ukr bul
+#define CY_che   UC(0x0447) // ч  rus ukr bul
+#define CY_dzhe  UC(0x045f) // џ
+#define CY_sha   UC(0x0448) // ш  rus ukr bul
+#define CY_shcha UC(0x0449) // щ  rus ukr bul
+#define CY_hsign UC(0x044a) // ъ  rus     bul
+#define CY_yeru  UC(0x044b) // ы  rus
+#define CY_ssign UC(0x044c) // ь  rus ukr bul
+#define CY_e     UC(0x044d) // э  rus
+#define CY_yu    UC(0x044e) // ю  rus ukr bul
+#define CY_ya    UC(0x044f) // я  rus ukr bul
+// Important Cyrillic non-Slavic letters
+#define CY_palochka UC(0x04cf) // ӏ
+#define CY_schwa UC(0x04d9) // ә
+#define CY_ghe_s UC(0x0493) // ғ
+#define CY_ze_d  UC(0x0499) // ҙ
+#define CY_es_d  UC(0x04ab) // ҫ
+#define CY_br_ka UC(0x04a1) // ҡ
+#define CY_zhe_d UC(0x0497) // җ
+#define CY_ka_d  UC(0x049b) // қ
+#define CY_en_d  UC(0x04a3) // ң
+#define CY_enghe UC(0x04a5) // ҥ
+#define CY_brd_o UC(0x04e9) // ө
+#define CY_str_u UC(0x04af) // ү
+#define CY_s_u_s UC(0x04b1) // ұ
+#define CY_shha  UC(0x04bb) // һ
+#define CY_ha_d  UC(0x04b3) // ҳ
+
+
+// Extra
+#define CY_slr_ve UC(0x1c80) // ᲀ CYRILLIC SMALL LETTER ROUNDED VE
+#define CY_ll_de  UC(0x1c81) // ᲁ CYRILLIC SMALL LETTER LONG-LEGGED DE
+#define CY_ZEMLYA UC(0xa640) // Ꙁ CYRILLIC CAPITAL LETTER ZEMLYA
+#define CY_zemlya UC(0xa641) // ꙁ CYRILLIC SMALL LETTER ZEMLYA
+#define CY_RV_DZE UC(0xa644) // Ꙅ CYRILLIC CAPITAL LETTER REVERSED DZE
+#define CY_rv_DZE UC(0xa645) // ꙅ CYRILLIC SMALL LETTER REVERSED DZE
+#define CY_slw_es UC(0x1c83) // ᲃ CYRILLIC SMALL LETTER WIDE ES
+#define CY_st_te  UC(0x1c84) // ᲄ CYRILLIC SMALL LETTER TALL TE
+#define CY_3l_te  UC(0x1c85) // ᲅ CYRILLIC SMALL LETTER THREE-LEGGED TE
+#define CY_thsign UC(0x1c86) // ᲆ CYRILLIC SMALL LETTER TALL HARD SIGN
+#define CY_YERUBY UC(0xa650) // Ꙑ CYRILLIC CAPITAL LETTER YERU WITH BACK YER
+#define CY_yeruby UC(0xa651) // ꙑ CYRILLIC SMALL LETTER YERU WITH BACK YER
+#define CY_RUBL   UC(0x20bd) // ₽
+#define CY_NMRO   UC(0x2116) // №
+
+// The letters Zje and Sje are made for other letters and accent marks
+
+#endif
diff --git a/quantum/light_ws2812.c b/quantum/light_ws2812.c
index 401845e855..a883b13884 100755
--- a/quantum/light_ws2812.c
+++ b/quantum/light_ws2812.c
@@ -16,14 +16,128 @@
 #include <util/delay.h>
 #include "debug.h"
 
+#ifdef RGBW_BB_TWI
+
+// Port for the I2C
+#define I2C_DDR DDRD
+#define I2C_PIN PIND
+#define I2C_PORT PORTD
+
+// Pins to be used in the bit banging
+#define I2C_CLK 0
+#define I2C_DAT 1
+
+#define I2C_DATA_HI()\
+I2C_DDR &= ~ (1 << I2C_DAT);\
+I2C_PORT |= (1 << I2C_DAT);
+#define I2C_DATA_LO()\
+I2C_DDR |= (1 << I2C_DAT);\
+I2C_PORT &= ~ (1 << I2C_DAT);
+
+#define I2C_CLOCK_HI()\
+I2C_DDR &= ~ (1 << I2C_CLK);\
+I2C_PORT |= (1 << I2C_CLK);
+#define I2C_CLOCK_LO()\
+I2C_DDR |= (1 << I2C_CLK);\
+I2C_PORT &= ~ (1 << I2C_CLK);
+
+#define I2C_DELAY 1
+
+void I2C_WriteBit(unsigned char c)
+{
+    if (c > 0)
+    {
+        I2C_DATA_HI();
+    }
+    else
+    {
+        I2C_DATA_LO();
+    }
+
+    I2C_CLOCK_HI();
+    _delay_us(I2C_DELAY);
+
+    I2C_CLOCK_LO();
+    _delay_us(I2C_DELAY);
+
+    if (c > 0)
+    {
+        I2C_DATA_LO();
+    }
+
+    _delay_us(I2C_DELAY);
+}
+
+// Inits bitbanging port, must be called before using the functions below
+//
+void I2C_Init()
+{
+    I2C_PORT &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
+
+    I2C_CLOCK_HI();
+    I2C_DATA_HI();
+
+    _delay_us(I2C_DELAY);
+}
+
+// Send a START Condition
+//
+void I2C_Start()
+{
+    // set both to high at the same time
+    I2C_DDR &= ~ ((1 << I2C_DAT) | (1 << I2C_CLK));
+    _delay_us(I2C_DELAY);
+
+    I2C_DATA_LO();
+    _delay_us(I2C_DELAY);
+
+    I2C_CLOCK_LO();
+    _delay_us(I2C_DELAY);
+}
+
+// Send a STOP Condition
+//
+void I2C_Stop()
+{
+    I2C_CLOCK_HI();
+    _delay_us(I2C_DELAY);
+
+    I2C_DATA_HI();
+    _delay_us(I2C_DELAY);
+}
+
+// write a byte to the I2C slave device
+//
+unsigned char I2C_Write(unsigned char c)
+{
+    for (char i = 0; i < 8; i++)
+    {
+        I2C_WriteBit(c & 128);
+
+        c <<= 1;
+    }
+
+    
+    I2C_WriteBit(0);
+    _delay_us(I2C_DELAY);
+    _delay_us(I2C_DELAY);
+  
+    // _delay_us(I2C_DELAY);
+    //return I2C_ReadBit();
+    return 0;
+}
+
+
+#endif
+
 // Setleds for standard RGB
-void inline ws2812_setleds(struct cRGB *ledarray, uint16_t leds)
+void inline ws2812_setleds(LED_TYPE *ledarray, uint16_t leds)
 {
    // ws2812_setleds_pin(ledarray,leds, _BV(ws2812_pin));
    ws2812_setleds_pin(ledarray,leds, _BV(RGB_DI_PIN & 0xF));
 }
 
-void inline ws2812_setleds_pin(struct cRGB *ledarray, uint16_t leds, uint8_t pinmask)
+void inline ws2812_setleds_pin(LED_TYPE *ledarray, uint16_t leds, uint8_t pinmask)
 {
   // ws2812_DDRREG |= pinmask; // Enable DDR
   // new universal format (DDR)
@@ -34,14 +148,41 @@ void inline ws2812_setleds_pin(struct cRGB *ledarray, uint16_t leds, uint8_t pin
 }
 
 // Setleds for SK6812RGBW
-void inline ws2812_setleds_rgbw(struct cRGBW *ledarray, uint16_t leds)
+void inline ws2812_setleds_rgbw(LED_TYPE *ledarray, uint16_t leds)
 {
+
+  #ifdef RGBW_BB_TWI
+    uint8_t sreg_prev, twcr_prev;
+    sreg_prev=SREG;
+    twcr_prev=TWCR;
+    cli();
+    TWCR &= ~(1<<TWEN);
+    I2C_Init();
+    I2C_Start();
+    I2C_Write(0x84);
+    uint16_t datlen = leds<<2;
+    uint8_t curbyte;
+    uint8_t * data = (uint8_t*)ledarray;
+    while (datlen--) {
+      curbyte=*data++;
+      I2C_Write(curbyte);
+    }
+    I2C_Stop();
+    SREG=sreg_prev;
+    TWCR=twcr_prev;
+  #endif
+
+
   // ws2812_DDRREG |= _BV(ws2812_pin); // Enable DDR
   // new universal format (DDR)
   _SFR_IO8((RGB_DI_PIN >> 4) + 1) |= _BV(RGB_DI_PIN & 0xF);
 
   ws2812_sendarray_mask((uint8_t*)ledarray,leds<<2,_BV(RGB_DI_PIN & 0xF));
-  _delay_us(80);
+
+
+  #ifndef RGBW_BB_TWI
+    _delay_us(80);
+  #endif
 }
 
 void ws2812_sendarray(uint8_t *data,uint16_t datlen)
@@ -123,7 +264,7 @@ void inline ws2812_sendarray_mask(uint8_t *data,uint16_t datlen,uint8_t maskhi)
   cli();
 
   while (datlen--) {
-    curbyte=*data++;
+    curbyte=(*data++);
 
     asm volatile(
     "       ldi   %0,8  \n\t"
diff --git a/quantum/light_ws2812.h b/quantum/light_ws2812.h
index 54eef22d9e..9498e550e9 100755
--- a/quantum/light_ws2812.h
+++ b/quantum/light_ws2812.h
@@ -16,6 +16,21 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 //#include "ws2812_config.h"
+//#include "i2cmaster.h"
+
+#define LIGHT_I2C 1
+#define LIGHT_I2C_ADDR        0x84
+#define LIGHT_I2C_ADDR_WRITE  ( (LIGHT_I2C_ADDR<<1) | I2C_WRITE )
+#define LIGHT_I2C_ADDR_READ   ( (LIGHT_I2C_ADDR<<1) | I2C_READ  )
+
+#define RGBW 1
+
+#ifdef RGBW
+  #define LED_TYPE struct cRGBW
+#else
+  #define LED_TYPE struct cRGB
+#endif
+
 
 /*
  *  Structure of the LED array
@@ -42,9 +57,9 @@ struct cRGBW { uint8_t g; uint8_t r; uint8_t b; uint8_t w;};
  *         - Wait 50�s to reset the LEDs
  */
 
-void ws2812_setleds     (struct cRGB  *ledarray, uint16_t number_of_leds);
-void ws2812_setleds_pin (struct cRGB  *ledarray, uint16_t number_of_leds,uint8_t pinmask);
-void ws2812_setleds_rgbw(struct cRGBW *ledarray, uint16_t number_of_leds);
+void ws2812_setleds     (LED_TYPE *ledarray, uint16_t number_of_leds);
+void ws2812_setleds_pin (LED_TYPE *ledarray, uint16_t number_of_leds,uint8_t pinmask);
+void ws2812_setleds_rgbw(LED_TYPE *ledarray, uint16_t number_of_leds);
 
 /*
  * Old interface / Internal functions
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/pincontrol.h b/quantum/pincontrol.h
new file mode 100644
index 0000000000..36ce29ef22
--- /dev/null
+++ b/quantum/pincontrol.h
@@ -0,0 +1,37 @@
+#pragma once
+// Some helpers for controlling gpio pins
+#include <avr/io.h>
+
+enum {
+  PinDirectionInput = 0,
+  PinDirectionOutput = 1,
+  PinLevelHigh = 1,
+  PinLevelLow = 0,
+};
+
+// ex: pinMode(B0, PinDirectionOutput);
+static inline void pinMode(uint8_t pin, int mode) {
+  uint8_t bv = _BV(pin & 0xf);
+  if (mode == PinDirectionOutput) {
+    _SFR_IO8((pin >> 4) + 1) |= bv;
+  } else {
+    _SFR_IO8((pin >> 4) + 1) &= ~bv;
+    _SFR_IO8((pin >> 4) + 2) &= ~bv;
+  }
+}
+
+// ex: digitalWrite(B0, PinLevelHigh);
+static inline void digitalWrite(uint8_t pin, int mode) {
+  uint8_t bv = _BV(pin & 0xf);
+  if (mode == PinLevelHigh) {
+    _SFR_IO8((pin >> 4) + 2) |= bv;
+  } else {
+    _SFR_IO8((pin >> 4) + 2) &= ~bv;
+  }
+}
+
+// Return true if the pin is HIGH
+// digitalRead(B0)
+static inline bool digitalRead(uint8_t pin) {
+  return _SFR_IO8(pin >> 4) & _BV(pin & 0xf);
+}
diff --git a/quantum/process_keycode/process_midi.c b/quantum/process_keycode/process_midi.c
index d6ab9c6264..577dad43ac 100644
--- a/quantum/process_keycode/process_midi.c
+++ b/quantum/process_keycode/process_midi.c
@@ -1,13 +1,15 @@
 #include "process_midi.h"
 
 bool midi_activated = false;
-uint8_t starting_note = 0x0C;
-int offset = 7;
+uint8_t midi_starting_note = 0x0C;
+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;
     }
 
@@ -20,42 +22,42 @@ bool process_midi(uint16_t keycode, keyrecord_t *record) {
     if (midi_activated) {
       if (record->event.key.col == (MATRIX_COLS - 1) && record->event.key.row == (MATRIX_ROWS - 1)) {
           if (record->event.pressed) {
-              starting_note++; // Change key
+              midi_starting_note++; // Change key
               midi_send_cc(&midi_device, 0, 0x7B, 0);
           }
           return false;
       }
       if (record->event.key.col == (MATRIX_COLS - 2) && record->event.key.row == (MATRIX_ROWS - 1)) {
           if (record->event.pressed) {
-              starting_note--; // Change key
+              midi_starting_note--; // Change key
               midi_send_cc(&midi_device, 0, 0x7B, 0);
           }
           return false;
       }
       if (record->event.key.col == (MATRIX_COLS - 3) && record->event.key.row == (MATRIX_ROWS - 1) && record->event.pressed) {
-          offset++; // Change scale
+          midi_offset++; // Change scale
           midi_send_cc(&midi_device, 0, 0x7B, 0);
           return false;
       }
       if (record->event.key.col == (MATRIX_COLS - 4) && record->event.key.row == (MATRIX_ROWS - 1) && record->event.pressed) {
-          offset--; // Change scale
+          midi_offset--; // Change scale
           midi_send_cc(&midi_device, 0, 0x7B, 0);
           return false;
       }
       // basic
-      // uint8_t note = (starting_note + SCALE[record->event.key.col + offset])+12*(MATRIX_ROWS - record->event.key.row);
+      // uint8_t note = (midi_starting_note + SCALE[record->event.key.col + midi_offset])+12*(MATRIX_ROWS - record->event.key.row);
       // advanced
-      // uint8_t note = (starting_note + record->event.key.col + offset)+12*(MATRIX_ROWS - record->event.key.row);
+      // uint8_t note = (midi_starting_note + record->event.key.col + midi_offset)+12*(MATRIX_ROWS - record->event.key.row);
       // guitar
-      uint8_t note = (starting_note + record->event.key.col + offset)+5*(MATRIX_ROWS - record->event.key.row);
+      uint8_t note = (midi_starting_note + record->event.key.col + midi_offset)+5*(MATRIX_ROWS - record->event.key.row);
       // violin
-      // uint8_t note = (starting_note + record->event.key.col + offset)+7*(MATRIX_ROWS - record->event.key.row);
+      // uint8_t note = (midi_starting_note + record->event.key.col + midi_offset)+7*(MATRIX_ROWS - record->event.key.row);
 
       if (record->event.pressed) {
-        // midi_send_noteon(&midi_device, record->event.key.row, starting_note + SCALE[record->event.key.col], 127);
+        // midi_send_noteon(&midi_device, record->event.key.row, midi_starting_note + SCALE[record->event.key.col], 127);
         midi_send_noteon(&midi_device, 0, note, 127);
       } else {
-        // midi_send_noteoff(&midi_device, record->event.key.row, starting_note + SCALE[record->event.key.col], 127);
+        // midi_send_noteoff(&midi_device, record->event.key.row, midi_starting_note + SCALE[record->event.key.col], 127);
         midi_send_noteoff(&midi_device, 0, note, 127);
       }
 
@@ -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_music.c b/quantum/process_keycode/process_music.c
index c8f3ddb900..bae43943e0 100644
--- a/quantum/process_keycode/process_music.c
+++ b/quantum/process_keycode/process_music.c
@@ -1,11 +1,12 @@
 #include "process_music.h"
 
 bool music_activated = false;
-uint8_t starting_note = 0x0C;
-int offset = 7;
+uint8_t music_starting_note = 0x0C;
+int music_offset = 7;
 
 // music sequencer
 static bool music_sequence_recording = false;
+static bool music_sequence_recorded = false;
 static bool music_sequence_playing = false;
 static float music_sequence[16] = {0};
 static uint8_t music_sequence_count = 0;
@@ -77,6 +78,7 @@ bool process_music(uint16_t keycode, keyrecord_t *record) {
       if (keycode == KC_LCTL && record->event.pressed) { // Start recording
         stop_all_notes();
         music_sequence_recording = true;
+        music_sequence_recorded = false;
         music_sequence_playing = false;
         music_sequence_count = 0;
         return false;
@@ -84,12 +86,15 @@ bool process_music(uint16_t keycode, keyrecord_t *record) {
 
       if (keycode == KC_LALT && record->event.pressed) { // Stop recording/playing
         stop_all_notes();
+        if (music_sequence_recording) { // was recording
+          music_sequence_recorded = true;
+        }
         music_sequence_recording = false;
         music_sequence_playing = false;
         return false;
       }
 
-      if (keycode == KC_LGUI && record->event.pressed) { // Start playing
+      if (keycode == KC_LGUI && record->event.pressed && music_sequence_recorded) { // Start playing
         stop_all_notes();
         music_sequence_recording = false;
         music_sequence_playing = true;
@@ -110,7 +115,7 @@ bool process_music(uint16_t keycode, keyrecord_t *record) {
         return false;
       }
 
-      float freq = ((float)220.0)*pow(2.0, -5.0)*pow(2.0,(starting_note + SCALE[record->event.key.col + offset])/12.0+(MATRIX_ROWS - record->event.key.row));
+      float freq = ((float)220.0)*pow(2.0, -5.0)*pow(2.0,(music_starting_note + SCALE[record->event.key.col + music_offset])/12.0+(MATRIX_ROWS - record->event.key.row));
       if (record->event.pressed) {
         play_note(freq, 0xF);
         if (music_sequence_recording) {
diff --git a/quantum/process_keycode/process_printer.c b/quantum/process_keycode/process_printer.c
new file mode 100644
index 0000000000..2e11dd366c
--- /dev/null
+++ b/quantum/process_keycode/process_printer.c
@@ -0,0 +1,254 @@
+#include "process_printer.h"
+#include "action_util.h"
+
+bool printing_enabled = false;
+uint8_t character_shift = 0;
+
+void enabled_printing() {
+	printing_enabled = true;
+	serial_init();
+}
+
+void disable_printing() {
+	printing_enabled = false;
+}
+
+uint8_t shifted_numbers[10] = {0x21, 0x40, 0x23, 0x24, 0x25, 0x5E, 0x26, 0x2A, 0x28, 0x29};
+
+// uint8_t keycode_to_ascii[0xFF][2];
+
+// keycode_to_ascii[KC_MINS] = {0x2D, 0x5F};
+
+void print_char(char c) {
+	USB_Disable();
+	serial_send(c);
+	USB_Init();
+}
+
+void print_box_string(uint8_t text[]) {
+	uint8_t len = strlen(text);
+	uint8_t out[len * 3 + 8];
+	out[0] = 0xDA;
+	for (uint8_t i = 0; i < len; i++) {
+		out[i+1] = 0xC4;
+	}
+	out[len + 1] = 0xBF;
+	out[len + 2] = '\n';
+
+	out[len + 3] = 0xB3;
+	for (uint8_t i = 0; i < len; i++) {
+		out[len + 4 + i] = text[i];
+	}
+	out[len * 2 + 4] = 0xB3;
+	out[len * 2 + 5] = '\n';
+
+
+	out[len * 2 + 6] = 0xC0;
+	for (uint8_t i = 0; i < len; i++) {
+		out[len * 2 + 7 + i] = 0xC4;
+	}
+	out[len * 3 + 7] = 0xD9;
+	out[len * 3 + 8] = '\n';
+
+	print_string(out); 
+}
+
+void print_string(char c[]) {
+	for(uint8_t i = 0; i < strlen(c); i++)
+		print_char(c[i]);
+}
+
+bool process_printer(uint16_t keycode, keyrecord_t *record) {
+	if (keycode == PRINT_ON) {
+		enabled_printing();
+		return false;
+	}
+	if (keycode == PRINT_OFF) {
+		disable_printing();
+		return false;
+	}
+
+	if (printing_enabled) {
+		switch(keycode) {
+			case KC_EXLM ... KC_RPRN:
+			case KC_UNDS:
+			case KC_PLUS:
+			case KC_LCBR:
+			case KC_RCBR:
+			case KC_PIPE:
+			case KC_TILD:
+				keycode &= 0xFF;
+			case KC_LSFT:
+			case KC_RSFT:
+				if (record->event.pressed) {
+					character_shift++;
+				} else {
+					character_shift--;
+				}
+				return false;
+			break;
+		}
+
+		switch(keycode) {
+			case KC_F1:
+				if (record->event.pressed) {
+					print_box_string("This is a line of text!");
+				}
+				return false;
+			case KC_ESC:
+				if (record->event.pressed) {
+					print_char(0x1B);
+				}
+				return false;
+			break;
+			case KC_SPC:
+				if (record->event.pressed) {
+					print_char(0x20);
+				}
+				return false;
+			break;
+			case KC_A ... KC_Z:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x41 + (keycode - KC_A));
+					} else {
+						print_char(0x61 + (keycode - KC_A));
+					}
+				}
+				return false;
+			break;
+			case KC_1 ... KC_0:
+				if (record->event.pressed) {
+					if (character_shift) {
+							print_char(shifted_numbers[keycode - KC_1]);
+					} else {
+							print_char(0x30 + ((keycode - KC_1 + 1) % 10));
+					}
+				}
+				return false;
+			break;
+			case KC_ENT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x0C);
+					} else {
+						print_char(0x0A);
+					}
+				}
+				return false;
+			break;
+			case KC_BSPC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x18);
+					} else {
+						print_char(0x1A);
+					}
+				}
+				return false;
+			break;
+			case KC_DOT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3E);
+					} else {
+						print_char(0x2E);
+					}
+				}
+				return false;
+			break;
+			case KC_COMM:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3C);
+					} else {
+						print_char(0x2C);
+					}
+				}
+				return false;
+			break;
+			case KC_SLSH:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3F);
+					} else {
+						print_char(0x2F);
+					}
+				}
+				return false;
+			break;
+			case KC_QUOT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x22);
+					} else {
+						print_char(0x27);
+					}
+				}
+				return false;
+			break;
+			case KC_GRV:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7E);
+					} else {
+						print_char(0x60);
+					}
+				}
+				return false;
+			break;
+			case KC_MINS:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x5F);
+					} else {
+						print_char(0x2D);
+					}
+				}
+				return false;
+			break;
+			case KC_EQL:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x2B);
+					} else {
+						print_char(0x3D);
+					}
+				}
+				return false;
+			break;
+			case KC_LBRC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7B);
+					} else {
+						print_char(0x5B);
+					}
+				}
+				return false;
+			break;
+			case KC_RBRC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7D);
+					} else {
+						print_char(0x5D);
+					}
+				}
+				return false;
+			break;
+			case KC_BSLS:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7C);
+					} else {
+						print_char(0x5C);
+					}
+				}
+				return false;
+			break;
+		}
+	}
+	return true;
+
+}
\ No newline at end of file
diff --git a/quantum/process_keycode/process_printer.h b/quantum/process_keycode/process_printer.h
new file mode 100644
index 0000000000..fdd36d75a8
--- /dev/null
+++ b/quantum/process_keycode/process_printer.h
@@ -0,0 +1,8 @@
+#ifndef PROCESS_PRINTER_H
+#define PROCESS_PRINTER_H
+
+#include "quantum.h"
+
+#include "protocol/serial.h"
+
+#endif
\ No newline at end of file
diff --git a/quantum/process_keycode/process_printer_bb.c b/quantum/process_keycode/process_printer_bb.c
new file mode 100644
index 0000000000..1924d03774
--- /dev/null
+++ b/quantum/process_keycode/process_printer_bb.c
@@ -0,0 +1,260 @@
+#include "process_printer.h"
+#include "action_util.h"
+
+bool printing_enabled = false;
+uint8_t character_shift = 0;
+
+#define SERIAL_PIN_DDR DDRD
+#define SERIAL_PIN_PORT PORTD
+#define SERIAL_PIN_MASK _BV(PD3)
+#define SERIAL_DELAY 52
+
+inline static
+void serial_delay(void) {
+  _delay_us(SERIAL_DELAY);
+}
+
+inline static
+void serial_high(void) {
+  SERIAL_PIN_PORT |= SERIAL_PIN_MASK;
+}
+
+inline static
+void serial_low(void) {
+  SERIAL_PIN_PORT &= ~SERIAL_PIN_MASK;
+}
+
+inline static
+void serial_output(void) {
+  SERIAL_PIN_DDR |= SERIAL_PIN_MASK;
+}
+
+
+void enabled_printing() {
+	printing_enabled = true;
+	serial_output();
+	serial_high();
+}
+
+void disable_printing() {
+	printing_enabled = false;
+}
+
+uint8_t shifted_numbers[10] = {0x21, 0x40, 0x23, 0x24, 0x25, 0x5E, 0x26, 0x2A, 0x28, 0x29};
+
+// uint8_t keycode_to_ascii[0xFF][2];
+
+// keycode_to_ascii[KC_MINS] = {0x2D, 0x5F};
+
+void print_char(char c) {
+  uint8_t b = 8;
+  serial_output();
+  while( b-- ) {
+    if(c & (1 << b)) {
+      serial_high();
+    } else {
+      serial_low();
+    }
+    serial_delay();
+  }
+}
+
+void print_string(char c[]) {
+	for(uint8_t i = 0; i < strlen(c); i++)
+		print_char(c[i]);
+}
+
+bool process_printer(uint16_t keycode, keyrecord_t *record) {
+	if (keycode == PRINT_ON) {
+		enabled_printing();
+		return false;
+	}
+	if (keycode == PRINT_OFF) {
+		disable_printing();
+		return false;
+	}
+
+	if (printing_enabled) {
+		switch(keycode) {
+			case KC_EXLM ... KC_RPRN:
+			case KC_UNDS:
+			case KC_PLUS:
+			case KC_LCBR:
+			case KC_RCBR:
+			case KC_PIPE:
+			case KC_TILD:
+				keycode &= 0xFF;
+			case KC_LSFT:
+			case KC_RSFT:
+				if (record->event.pressed) {
+					character_shift++;
+				} else {
+					character_shift--;
+				}
+				return false;
+			break;
+		}
+
+		switch(keycode) {
+			case KC_F1:
+				if (record->event.pressed) {
+					print_string("This is a line of text!\n\n\n");
+				}
+				return false;
+			case KC_ESC:
+				if (record->event.pressed) {
+					print_char(0x1B);
+				}
+				return false;
+			break;
+			case KC_SPC:
+				if (record->event.pressed) {
+					print_char(0x20);
+				}
+				return false;
+			break;
+			case KC_A ... KC_Z:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x41 + (keycode - KC_A));
+					} else {
+						print_char(0x61 + (keycode - KC_A));
+					}
+				}
+				return false;
+			break;
+			case KC_1 ... KC_0:
+				if (record->event.pressed) {
+					if (character_shift) {
+							print_char(shifted_numbers[keycode - KC_1]);
+					} else {
+							print_char(0x30 + ((keycode - KC_1 + 1) % 10));
+					}
+				}
+				return false;
+			break;
+			case KC_ENT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x0C);
+					} else {
+						print_char(0x0A);
+					}
+				}
+				return false;
+			break;
+			case KC_BSPC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x18);
+					} else {
+						print_char(0x1A);
+					}
+				}
+				return false;
+			break;
+			case KC_DOT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3E);
+					} else {
+						print_char(0x2E);
+					}
+				}
+				return false;
+			break;
+			case KC_COMM:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3C);
+					} else {
+						print_char(0x2C);
+					}
+				}
+				return false;
+			break;
+			case KC_SLSH:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x3F);
+					} else {
+						print_char(0x2F);
+					}
+				}
+				return false;
+			break;
+			case KC_QUOT:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x22);
+					} else {
+						print_char(0x27);
+					}
+				}
+				return false;
+			break;
+			case KC_GRV:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7E);
+					} else {
+						print_char(0x60);
+					}
+				}
+				return false;
+			break;
+			case KC_MINS:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x5F);
+					} else {
+						print_char(0x2D);
+					}
+				}
+				return false;
+			break;
+			case KC_EQL:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x2B);
+					} else {
+						print_char(0x3D);
+					}
+				}
+				return false;
+			break;
+			case KC_LBRC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7B);
+					} else {
+						print_char(0x5B);
+					}
+				}
+				return false;
+			break;
+			case KC_RBRC:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7D);
+					} else {
+						print_char(0x5D);
+					}
+				}
+				return false;
+			break;
+			case KC_BSLS:
+				if (record->event.pressed) {
+					if (character_shift) {
+						print_char(0x7C);
+					} else {
+						print_char(0x5C);
+					}
+				}
+				return false;
+			break;
+		}
+	}
+	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 9b172e1b6c..6ae362c4c2 100644
--- a/quantum/process_keycode/process_tap_dance.c
+++ b/quantum/process_keycode/process_tap_dance.c
@@ -1,90 +1,136 @@
 #include "quantum.h"
+#include "action_tapping.h"
 
-static qk_tap_dance_state_t qk_tap_dance_state;
+static uint16_t last_td;
+static int8_t highest_td = -1;
 
-static void _process_tap_dance_action_pair (qk_tap_dance_state_t *state,
-                                            uint16_t kc1, uint16_t kc2) {
-  uint16_t kc;
+void qk_tap_dance_pair_finished (qk_tap_dance_state_t *state, void *user_data) {
+  qk_tap_dance_pair_t *pair = (qk_tap_dance_pair_t *)user_data;
 
-  if (state->count == 0)
-    return;
-
-  kc = (state->count == 1) ? kc1 : kc2;
+  if (state->count == 1) {
+    register_code16 (pair->kc1);
+  } else if (state->count == 2) {
+    register_code16 (pair->kc2);
+  }
+}
 
-  register_code (kc);
-  unregister_code (kc);
+void qk_tap_dance_pair_reset (qk_tap_dance_state_t *state, void *user_data) {
+  qk_tap_dance_pair_t *pair = (qk_tap_dance_pair_t *)user_data;
 
-  if (state->count >= 2) {
-    reset_tap_dance (state);
+  if (state->count == 1) {
+    unregister_code16 (pair->kc1);
+  } else if (state->count == 2) {
+    unregister_code16 (pair->kc2);
   }
 }
 
-static void _process_tap_dance_action_fn (qk_tap_dance_state_t *state,
-                                          qk_tap_dance_user_fn_t fn)
+static inline void _process_tap_dance_action_fn (qk_tap_dance_state_t *state,
+                                                 void *user_data,
+                                                 qk_tap_dance_user_fn_t fn)
 {
-  fn(state);
+  if (fn) {
+    fn(state, user_data);
+  }
 }
 
-void process_tap_dance_action (uint16_t keycode)
+static inline void process_tap_dance_action_on_each_tap (qk_tap_dance_action_t *action)
 {
-  uint16_t idx = keycode - QK_TAP_DANCE;
-  qk_tap_dance_action_t action;
-
-  action = tap_dance_actions[idx];
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_each_tap);
+}
 
-  switch (action.type) {
-  case QK_TAP_DANCE_TYPE_PAIR:
-    _process_tap_dance_action_pair (&qk_tap_dance_state,
-                                    action.pair.kc1, action.pair.kc2);
-    break;
-  case QK_TAP_DANCE_TYPE_FN:
-    _process_tap_dance_action_fn (&qk_tap_dance_state, action.fn);
-    break;
+static inline void process_tap_dance_action_on_dance_finished (qk_tap_dance_action_t *action)
+{
+  if (action->state.finished)
+    return;
+  action->state.finished = true;
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_dance_finished);
+}
 
-  default:
-    break;
-  }
+static inline void process_tap_dance_action_on_reset (qk_tap_dance_action_t *action)
+{
+  _process_tap_dance_action_fn (&action->state, action->user_data, action->fn.on_reset);
 }
 
 bool process_tap_dance(uint16_t keycode, keyrecord_t *record) {
-  bool r = true;
+  uint16_t idx = keycode - QK_TAP_DANCE;
+  qk_tap_dance_action_t *action;
+
+  if (last_td && last_td != keycode) {
+    (&tap_dance_actions[last_td - QK_TAP_DANCE])->state.interrupted = true;
+  }
 
   switch(keycode) {
   case QK_TAP_DANCE ... QK_TAP_DANCE_MAX:
-    if (qk_tap_dance_state.keycode && qk_tap_dance_state.keycode != keycode) {
-      process_tap_dance_action (qk_tap_dance_state.keycode);
-    } else {
-      r = false;
-    }
+    if ((int16_t)idx > highest_td)
+      highest_td = idx;
+    action = &tap_dance_actions[idx];
 
+    action->state.pressed = record->event.pressed;
     if (record->event.pressed) {
-      qk_tap_dance_state.keycode = keycode;
-      qk_tap_dance_state.timer = timer_read ();
-      qk_tap_dance_state.count++;
+      action->state.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];
+        paction->state.interrupted = true;
+        process_tap_dance_action_on_dance_finished (paction);
+        reset_tap_dance (&paction->state);
+      }
+
+      last_td = keycode;
     }
+
     break;
 
   default:
-    if (qk_tap_dance_state.keycode) {
-      process_tap_dance_action (qk_tap_dance_state.keycode);
-
-      reset_tap_dance (&qk_tap_dance_state);
+    if (!record->event.pressed)
+      return true;
+
+    if (highest_td == -1)
+      return true;
+
+    for (int i = 0; i <= highest_td; i++) {
+      action = &tap_dance_actions[i];
+      if (action->state.count == 0)
+        continue;
+      action->state.interrupted = true;
+      process_tap_dance_action_on_dance_finished (action);
+      reset_tap_dance (&action->state);
     }
     break;
   }
 
-  return r;
+  return true;
 }
 
 void matrix_scan_tap_dance () {
-  if (qk_tap_dance_state.keycode && timer_elapsed (qk_tap_dance_state.timer) > TAPPING_TERM) {
-    process_tap_dance_action (qk_tap_dance_state.keycode);
+  if (highest_td == -1)
+    return;
 
-    reset_tap_dance (&qk_tap_dance_state);
+  for (int i = 0; i <= highest_td; i++) {
+    qk_tap_dance_action_t *action = &tap_dance_actions[i];
+
+    if (action->state.count && timer_elapsed (action->state.timer) > TAPPING_TERM) {
+      process_tap_dance_action_on_dance_finished (action);
+      reset_tap_dance (&action->state);
+    }
   }
 }
 
 void reset_tap_dance (qk_tap_dance_state_t *state) {
-  state->keycode = 0;
+  qk_tap_dance_action_t *action;
+
+  if (state->pressed)
+    return;
+
+  action = &tap_dance_actions[state->keycode - QK_TAP_DANCE];
+
+  process_tap_dance_action_on_reset (action);
+
   state->count = 0;
+  state->interrupted = false;
+  state->finished = false;
+  last_td = 0;
 }
diff --git a/quantum/process_keycode/process_tap_dance.h b/quantum/process_keycode/process_tap_dance.h
index b9d7c7fcf4..f753cbba66 100644
--- a/quantum/process_keycode/process_tap_dance.h
+++ b/quantum/process_keycode/process_tap_dance.h
@@ -11,41 +11,48 @@ typedef struct
   uint8_t count;
   uint16_t keycode;
   uint16_t timer;
+  bool interrupted;
+  bool pressed;
+  bool finished;
 } qk_tap_dance_state_t;
 
 #define TD(n) (QK_TAP_DANCE + n)
 
-typedef enum
-{
-  QK_TAP_DANCE_TYPE_PAIR,
-  QK_TAP_DANCE_TYPE_FN,
-} qk_tap_dance_type_t;
-
-typedef void (*qk_tap_dance_user_fn_t) (qk_tap_dance_state_t *state);
+typedef void (*qk_tap_dance_user_fn_t) (qk_tap_dance_state_t *state, void *user_data);
 
 typedef struct
 {
-  qk_tap_dance_type_t type;
-  union {
-    struct {
-      uint16_t kc1;
-      uint16_t kc2;
-    } pair;
-    qk_tap_dance_user_fn_t fn;
-  };
+  struct {
+    qk_tap_dance_user_fn_t on_each_tap;
+    qk_tap_dance_user_fn_t on_dance_finished;
+    qk_tap_dance_user_fn_t on_reset;
+  } fn;
+  qk_tap_dance_state_t state;
+  void *user_data;
 } qk_tap_dance_action_t;
 
+typedef struct
+{
+  uint16_t kc1;
+  uint16_t kc2;
+} qk_tap_dance_pair_t;
+
 #define ACTION_TAP_DANCE_DOUBLE(kc1, kc2) { \
-    .type = QK_TAP_DANCE_TYPE_PAIR,         \
-    .pair = { kc1, kc2 }                    \
+    .fn = { NULL, qk_tap_dance_pair_finished, qk_tap_dance_pair_reset }, \
+    .user_data = (void *)&((qk_tap_dance_pair_t) { kc1, kc2 }),  \
+  }
+
+#define ACTION_TAP_DANCE_FN(user_fn) {  \
+    .fn = { NULL, user_fn, NULL }, \
+    .user_data = NULL, \
   }
 
-#define ACTION_TAP_DANCE_FN(user_fn) { \
-    .type = QK_TAP_DANCE_TYPE_FN, \
-    .fn = user_fn                 \
+#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 const qk_tap_dance_action_t tap_dance_actions[];
+extern qk_tap_dance_action_t tap_dance_actions[];
 
 /* To be used internally */
 
@@ -53,6 +60,9 @@ bool process_tap_dance(uint16_t keycode, keyrecord_t *record);
 void matrix_scan_tap_dance (void);
 void reset_tap_dance (qk_tap_dance_state_t *state);
 
+void qk_tap_dance_pair_finished (qk_tap_dance_state_t *state, void *user_data);
+void qk_tap_dance_pair_reset (qk_tap_dance_state_t *state, void *user_data);
+
 #else
 
 #define TD(n) KC_NO
diff --git a/quantum/process_keycode/process_unicode.c b/quantum/process_keycode/process_unicode.c
index ad5d7f86b7..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) {
@@ -13,45 +14,251 @@ uint16_t hex_to_keycode(uint8_t hex)
   }
 }
 
-void set_unicode_mode(uint8_t os_target)
+void set_unicode_input_mode(uint8_t os_target)
 {
   input_mode = os_target;
 }
 
+uint8_t get_unicode_input_mode(void) {
+  return input_mode;
+}
+
+__attribute__((weak))
+void unicode_input_start (void) {
+  switch(input_mode) {
+  case UC_OSX:
+    register_code(KC_LALT);
+    break;
+  case UC_LNX:
+    register_code(KC_LCTL);
+    register_code(KC_LSFT);
+    register_code(KC_U);
+    unregister_code(KC_U);
+    unregister_code(KC_LSFT);
+    unregister_code(KC_LCTL);
+    break;
+  case UC_WIN:
+    register_code(KC_LALT);
+    register_code(KC_PPLS);
+    unregister_code(KC_PPLS);
+    break;
+  case UC_WINC:
+    register_code(KC_RALT);
+    unregister_code(KC_RALT);
+    register_code(KC_U);
+    unregister_code(KC_U);
+  }
+  wait_ms(UNICODE_TYPE_DELAY);
+}
+
+__attribute__((weak))
+void unicode_input_finish (void) {
+  switch(input_mode) {
+  case UC_OSX:
+  case UC_WIN:
+    unregister_code(KC_LALT);
+    break;
+  case UC_LNX:
+    register_code(KC_SPC);
+    unregister_code(KC_SPC);
+    break;
+  }
+}
+
+void register_hex(uint16_t hex) {
+  for(int i = 3; i >= 0; i--) {
+    uint8_t digit = ((hex >> (i*4)) & 0xF);
+    register_code(hex_to_keycode(digit));
+    unregister_code(hex_to_keycode(digit));
+  }
+}
+
 bool process_unicode(uint16_t keycode, keyrecord_t *record) {
   if (keycode > QK_UNICODE && record->event.pressed) {
     uint16_t unicode = keycode & 0x7FFF;
-    switch(input_mode) {
-      case UC_OSX:
-        register_code(KC_LALT);
-        break;
-      case UC_LNX:
-        register_code(KC_LCTL);
-        register_code(KC_LSFT);
-        register_code(KC_U);
-        unregister_code(KC_U);
-        break;
-      case UC_WIN:
-        register_code(KC_LALT);
-        register_code(KC_PPLS);
-        unregister_code(KC_PPLS);
-        break;
+    unicode_input_start();
+    register_hex(unicode);
+    unicode_input_finish();
+  }
+  return true;
+}
+
+#ifdef 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;
     }
-    for(int i = 3; i >= 0; i--) {
-        uint8_t digit = ((unicode >> (i*4)) & 0xF);
+    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;
     }
-    switch(input_mode) {
-      case UC_OSX:
-      case UC_WIN:
-        unregister_code(KC_LALT);
-        break;
-      case UC_LNX:
-        unregister_code(KC_LCTL);
-        unregister_code(KC_LSFT);
+  }
+}
+
+__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;
+
+void qk_ucis_start(void) {
+  qk_ucis_state.count = 0;
+  qk_ucis_state.in_progress = true;
+
+  qk_ucis_start_user();
+}
+
+__attribute__((weak))
+void qk_ucis_start_user(void) {
+  unicode_input_start();
+  register_hex(0x2328);
+  unicode_input_finish();
+}
+
+static bool is_uni_seq(char *seq) {
+  uint8_t i;
+
+  for (i = 0; seq[i]; i++) {
+    uint16_t code;
+    if (('1' <= seq[i]) && (seq[i] <= '0'))
+      code = seq[i] - '1' + KC_1;
+    else
+      code = seq[i] - 'a' + KC_A;
+
+    if (i > qk_ucis_state.count || qk_ucis_state.codes[i] != code)
+      return false;
+  }
+
+  return (qk_ucis_state.codes[i] == KC_ENT ||
+          qk_ucis_state.codes[i] == KC_SPC);
+}
+
+__attribute__((weak))
+void qk_ucis_symbol_fallback (void) {
+  for (uint8_t i = 0; i < qk_ucis_state.count - 1; i++) {
+    uint8_t code = qk_ucis_state.codes[i];
+    register_code(code);
+    unregister_code(code);
+    wait_ms(UNICODE_TYPE_DELAY);
+  }
+}
+
+void register_ucis(const char *hex) {
+  for(int i = 0; hex[i]; i++) {
+    uint8_t kc = 0;
+    char c = hex[i];
+
+    switch (c) {
+    case '0':
+      kc = KC_0;
+      break;
+    case '1' ... '9':
+      kc = c - '1' + KC_1;
+      break;
+    case 'a' ... 'f':
+      kc = c - 'a' + KC_A;
+      break;
+    case 'A' ... 'F':
+      kc = c - 'A' + KC_A;
+      break;
+    }
+
+    if (kc) {
+      register_code (kc);
+      unregister_code (kc);
+      wait_ms (UNICODE_TYPE_DELAY);
+    }
+  }
+}
+
+bool process_ucis (uint16_t keycode, keyrecord_t *record) {
+  uint8_t i;
+
+  if (!qk_ucis_state.in_progress)
+    return true;
+
+  if (qk_ucis_state.count >= UCIS_MAX_SYMBOL_LENGTH &&
+      !(keycode == KC_BSPC || keycode == KC_ESC || keycode == KC_SPC || keycode == KC_ENT)) {
+    return false;
+  }
+
+  if (!record->event.pressed)
+    return true;
+
+  qk_ucis_state.codes[qk_ucis_state.count] = keycode;
+  qk_ucis_state.count++;
+
+  if (keycode == KC_BSPC) {
+    if (qk_ucis_state.count >= 2) {
+      qk_ucis_state.count -= 2;
+      return true;
+    } else {
+      qk_ucis_state.count--;
+      return false;
+    }
+  }
+
+  if (keycode == KC_ENT || keycode == KC_SPC || keycode == KC_ESC) {
+    bool symbol_found = false;
+
+    for (i = qk_ucis_state.count; i > 0; i--) {
+      register_code (KC_BSPC);
+      unregister_code (KC_BSPC);
+      wait_ms(UNICODE_TYPE_DELAY);
+    }
+
+    if (keycode == KC_ESC) {
+      qk_ucis_state.in_progress = false;
+      return false;
+    }
+
+    unicode_input_start();
+    for (i = 0; ucis_symbol_table[i].symbol; i++) {
+      if (is_uni_seq (ucis_symbol_table[i].symbol)) {
+        symbol_found = true;
+        register_ucis(ucis_symbol_table[i].code + 2);
         break;
+      }
+    }
+    if (!symbol_found) {
+      qk_ucis_symbol_fallback();
     }
+    unicode_input_finish();
+
+    qk_ucis_state.in_progress = false;
+    return false;
   }
   return true;
-}
\ No newline at end of file
+}
+#endif
diff --git a/quantum/process_keycode/process_unicode.h b/quantum/process_keycode/process_unicode.h
index ca17f8f669..f17cfa6cf2 100644
--- a/quantum/process_keycode/process_unicode.h
+++ b/quantum/process_keycode/process_unicode.h
@@ -3,15 +3,60 @@
 
 #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
+#endif
+
+typedef struct {
+  char *symbol;
+  char *code;
+} qk_ucis_symbol_t;
+
+typedef struct {
+  uint8_t count;
+  uint16_t codes[UCIS_MAX_SYMBOL_LENGTH];
+  bool in_progress:1;
+} qk_ucis_state_t;
+
+extern qk_ucis_state_t qk_ucis_state;
+
+#define UCIS_TABLE(...) {__VA_ARGS__, {NULL, NULL}}
+#define UCIS_SYM(name, code) {name, #code}
+
+extern const qk_ucis_symbol_t ucis_symbol_table[];
+
+void qk_ucis_start(void);
+void qk_ucis_start_user(void);
+void qk_ucis_symbol_fallback (void);
+void register_ucis(const char *hex);
+bool process_ucis (uint16_t keycode, keyrecord_t *record);
+
+#endif
+
 #define UC_BSPC	UC(0x0008)
 
 #define UC_SPC	UC(0x0020)
@@ -119,4 +164,4 @@ bool process_unicode(uint16_t keycode, keyrecord_t *record);
 #define UC_TILD	UC(0x007E)
 #define UC_DEL	UC(0x007F)
 
-#endif
\ No newline at end of file
+#endif
diff --git a/quantum/quantum.c b/quantum/quantum.c
index bc2da510f2..f653564a67 100644
--- a/quantum/quantum.c
+++ b/quantum/quantum.c
@@ -1,5 +1,46 @@
 #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:
+    break;
+  default:
+    return;
+  }
+
+  if (code & QK_LCTL)
+    f(KC_LCTL);
+  if (code & QK_LSFT)
+    f(KC_LSFT);
+  if (code & QK_LALT)
+    f(KC_LALT);
+  if (code & QK_LGUI)
+    f(KC_LGUI);
+
+  if (code & QK_RCTL)
+    f(KC_RCTL);
+  if (code & QK_RSFT)
+    f(KC_RSFT);
+  if (code & QK_RALT)
+    f(KC_RALT);
+  if (code & QK_RGUI)
+    f(KC_RGUI);
+}
+
+void register_code16 (uint16_t code) {
+  do_code16 (code, register_code);
+  register_code (code);
+}
+
+void unregister_code16 (uint16_t code) {
+  unregister_code (code);
+  do_code16 (code, unregister_code);
+}
+
 __attribute__ ((weak))
 bool process_action_kb(keyrecord_t *record) {
   return true;
@@ -38,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) {
 
@@ -46,18 +88,20 @@ bool process_record_quantum(keyrecord_t *record) {
   uint16_t keycode;
 
   #if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
-    uint8_t layer;
+    /* TODO: Use store_or_get_action() or a similar function. */
+    if (!disable_action_cache) {
+      uint8_t layer;
 
-    if (record->event.pressed) {
-      layer = layer_switch_get_layer(key);
-      update_source_layers_cache(key, layer);
-    } else {
-      layer = read_source_layers_cache(key);
-    }
-    keycode = keymap_key_to_keycode(layer, key);
-  #else
-    keycode = keymap_key_to_keycode(layer_switch_get_layer(key), key);
+      if (record->event.pressed) {
+        layer = layer_switch_get_layer(key);
+        update_source_layers_cache(key, layer);
+      } else {
+        layer = read_source_layers_cache(key);
+      }
+      keycode = keymap_key_to_keycode(layer, key);
+    } else
   #endif
+    keycode = keymap_key_to_keycode(layer_switch_get_layer(key), key);
 
     // This is how you use actions here
     // if (keycode == KC_LEAD) {
@@ -87,6 +131,15 @@ bool process_record_quantum(keyrecord_t *record) {
   #ifdef UNICODE_ENABLE
     process_unicode(keycode, record) &&
   #endif
+  #ifdef UCIS_ENABLE
+    process_ucis(keycode, record) &&
+  #endif
+  #ifdef PRINTING_ENABLE
+    process_printer(keycode, record) &&
+  #endif
+  #ifdef UNICODEMAP_ENABLE
+    process_unicode_map(keycode, record) &&
+  #endif
       true)) {
     return false;
   }
@@ -157,7 +210,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()) {
@@ -165,54 +218,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 {
@@ -222,19 +301,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 {
@@ -244,14 +324,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;
@@ -729,6 +809,51 @@ void backlight_set(uint8_t level)
 #endif // backlight
 
 
+// Functions for spitting out values
+//
+
+void send_dword(uint32_t number) { // this might not actually work
+    uint16_t word = (number >> 16);
+    send_word(word);
+    send_word(number & 0xFFFFUL);
+}
+
+void send_word(uint16_t number) {
+    uint8_t byte = number >> 8;
+    send_byte(byte);
+    send_byte(number & 0xFF);
+}
+
+void send_byte(uint8_t number) {
+    uint8_t nibble = number >> 4;
+    send_nibble(nibble);
+    send_nibble(number & 0xF);
+}
+
+void send_nibble(uint8_t number) {
+    switch (number) {
+        case 0:
+            register_code(KC_0);
+            unregister_code(KC_0);
+            break;
+        case 1 ... 9:
+            register_code(KC_1 + (number - 1));
+            unregister_code(KC_1 + (number - 1));
+            break;
+        case 0xA ... 0xF:
+            register_code(KC_A + (number - 0xA));
+            unregister_code(KC_A + (number - 0xA));
+            break;
+    }
+}
+
+void api_send_unicode(uint32_t unicode) {
+#ifdef API_ENABLE
+    uint8_t chunk[4];
+    dword_to_bytes(unicode, chunk);
+    MT_SEND_DATA(DT_UNICODE, chunk, 5);
+#endif
+}
 
 __attribute__ ((weak))
 void led_set_user(uint8_t usb_led) {
diff --git a/quantum/quantum.h b/quantum/quantum.h
index 7ebfb24e30..e6adf974ab 100644
--- a/quantum/quantum.h
+++ b/quantum/quantum.h
@@ -25,6 +25,7 @@
 #include "led.h"
 #include "action_util.h"
 #include <stdlib.h>
+#include "print.h"
 
 
 extern uint32_t default_layer_state;
@@ -58,6 +59,10 @@ extern uint32_t default_layer_state;
 
 #include "process_tap_dance.h"
 
+#ifdef PRINTING_ENABLE
+	#include "process_printer.h"
+#endif
+
 #define SEND_STRING(str) send_string(PSTR(str))
 void send_string(const char *str);
 
@@ -82,6 +87,9 @@ void reset_keyboard(void);
 void startup_user(void);
 void shutdown_user(void);
 
+void register_code16 (uint16_t code);
+void unregister_code16 (uint16_t code);
+
 #ifdef BACKLIGHT_ENABLE
 void backlight_init_ports(void);
 
@@ -102,8 +110,15 @@ void breathing_speed_dec(uint8_t value);
 #endif
 
 #endif
+void send_dword(uint32_t number);
+void send_word(uint16_t number);
+void send_byte(uint8_t number);
+void send_nibble(uint8_t number);
+
 
 void led_set_user(uint8_t usb_led);
 void led_set_kb(uint8_t usb_led);
 
+void api_send_unicode(uint32_t unicode);
+
 #endif
diff --git a/quantum/rgblight.c b/quantum/rgblight.c
index b1b0f035d5..625971e0fe 100644
--- a/quantum/rgblight.c
+++ b/quantum/rgblight.c
@@ -6,98 +6,126 @@
 #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,
+  37, 40, 42, 44, 47, 49, 52, 54, 57, 59, 62, 65, 67, 70, 73, 76,
+  79, 82, 85, 88, 90, 93, 97, 100, 103, 106, 109, 112, 115, 118, 121, 124,
+  127, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 162, 165, 167, 170, 173,
+  176, 179, 182, 185, 188, 190, 193, 196, 198, 201, 203, 206, 208, 211, 213, 215,
+  218, 220, 222, 224, 226, 228, 230, 232, 234, 235, 237, 238, 240, 241, 243, 244,
+  245, 246, 248, 249, 250, 250, 251, 252, 253, 253, 254, 254, 254, 255, 255, 255,
+  255, 255, 255, 255, 254, 254, 254, 253, 253, 252, 251, 250, 250, 249, 248, 246,
+  245, 244, 243, 241, 240, 238, 237, 235, 234, 232, 230, 228, 226, 224, 222, 220,
+  218, 215, 213, 211, 208, 206, 203, 201, 198, 196, 193, 190, 188, 185, 182, 179,
+  176, 173, 170, 167, 165, 162, 158, 155, 152, 149, 146, 143, 140, 137, 134, 131,
+  128, 124, 121, 118, 115, 112, 109, 106, 103, 100, 97, 93, 90, 88, 85, 82,
+  79, 76, 73, 70, 67, 65, 62, 59, 57, 54, 52, 49, 47, 44, 42, 40,
+  37, 35, 33, 31, 29, 27, 25, 23, 21, 20, 18, 17, 15, 14, 12, 11,
+  10, 9, 7, 6, 5, 5, 4, 3, 2, 2, 1, 1, 1, 0, 0, 0
 };
-const uint8_t RGBLED_BREATHING_TABLE[] PROGMEM = {0,0,0,0,1,1,1,2,2,3,4,5,5,6,7,9,10,11,12,14,15,17,18,20,21,23,25,27,29,31,33,35,37,40,42,44,47,49,52,54,57,59,62,65,67,70,73,76,79,82,85,88,90,93,97,100,103,106,109,112,115,118,121,124,127,131,134,137,140,143,146,149,152,155,158,162,165,167,170,173,176,179,182,185,188,190,193,196,198,201,203,206,208,211,213,215,218,220,222,224,226,228,230,232,234,235,237,238,240,241,243,244,245,246,248,249,250,250,251,252,253,253,254,254,254,255,255,255,255,255,255,255,254,254,254,253,253,252,251,250,250,249,248,246,245,244,243,241,240,238,237,235,234,232,230,228,226,224,222,220,218,215,213,211,208,206,203,201,198,196,193,190,188,185,182,179,176,173,170,167,165,162,158,155,152,149,146,143,140,137,134,131,128,124,121,118,115,112,109,106,103,100,97,93,90,88,85,82,79,76,73,70,67,65,62,59,57,54,52,49,47,44,42,40,37,35,33,31,29,27,25,23,21,20,18,17,15,14,12,11,10,9,7,6,5,5,4,3,2,2,1,1,1,0,0,0};
+
+__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;
 rgblight_config_t inmem_config;
-struct cRGB led[RGBLED_NUM];
+
+LED_TYPE led[RGBLED_NUM];
 uint8_t rgblight_inited = 0;
+bool rgblight_timer_enabled = false;
+
+void sethsv(uint16_t hue, uint8_t sat, uint8_t val, LED_TYPE *led1) {
+  uint8_t r = 0, g = 0, b = 0, base, color;
 
+  if (sat == 0) { // Acromatic color (gray). Hue doesn't mind.
+    r = val;
+    g = val;
+    b = val;
+  } else {
+    base = ((255 - sat) * val) >> 8;
+    color = (val - base) * (hue % 60) / 60;
+
+    switch (hue / 60) {
+      case 0:
+        r = val;
+        g = base + color;
+        b = base;
+        break;
+      case 1:
+        r = val - color;
+        g = val;
+        b = base;
+        break;
+      case 2:
+        r = base;
+        g = val;
+        b = base + color;
+        break;
+      case 3:
+        r = base;
+        g = val - color;
+        b = val;
+        break;
+      case 4:
+        r = base + color;
+        g = base;
+        b = val;
+        break;
+      case 5:
+        r = val;
+        g = base;
+        b = val - color;
+        break;
+    }
+  }
+  r = pgm_read_byte(&DIM_CURVE[r]);
+  g = pgm_read_byte(&DIM_CURVE[g]);
+  b = pgm_read_byte(&DIM_CURVE[b]);
 
-void sethsv(uint16_t hue, uint8_t sat, uint8_t val, struct cRGB *led1) {
-	/* convert hue, saturation and brightness ( HSB/HSV ) to RGB
-	The DIM_CURVE is used only on brightness/value and on saturation (inverted).
-	This looks the most natural.
-	*/
-  uint8_t r = 0, g = 0, b = 0;
-
-  val = pgm_read_byte(&DIM_CURVE[val]);
-	sat = 255 - pgm_read_byte(&DIM_CURVE[255 - sat]);
-
-	uint8_t base;
-
-	if (sat == 0) { // Acromatic color (gray). Hue doesn't mind.
-		r = val;
-		g = val;
-		b = val;
-	} else  {
-		base = ((255 - sat) * val) >> 8;
-
-		switch (hue / 60) {
-		case 0:
-			r = val;
-			g = (((val - base)*hue) / 60) + base;
-			b = base;
-			break;
-
-		case 1:
-			r = (((val - base)*(60 - (hue % 60))) / 60) + base;
-			g = val;
-			b = base;
-			break;
-
-		case 2:
-			r = base;
-			g = val;
-			b = (((val - base)*(hue % 60)) / 60) + base;
-			break;
-
-		case 3:
-			r = base;
-			g = (((val - base)*(60 - (hue % 60))) / 60) + base;
-			b = val;
-			break;
-
-		case 4:
-			r = (((val - base)*(hue % 60)) / 60) + base;
-			g = base;
-			b = val;
-			break;
-
-		case 5:
-			r = val;
-			g = base;
-			b = (((val - base)*(60 - (hue % 60))) / 60) + base;
-			break;
-		}
-	}
-  setrgb(r,g,b, led1);
-}
-
-void setrgb(uint8_t r, uint8_t g, uint8_t b, struct cRGB *led1) {
+  setrgb(r, g, b, led1);
+}
+
+void setrgb(uint8_t r, uint8_t g, uint8_t b, LED_TYPE *led1) {
   (*led1).r = r;
   (*led1).g = g;
   (*led1).b = b;
@@ -111,103 +139,115 @@ void eeconfig_update_rgblight(uint32_t val) {
   eeprom_update_dword(EECONFIG_RGBLIGHT, val);
 }
 void eeconfig_update_rgblight_default(void) {
-	dprintf("eeconfig_update_rgblight_default\n");
-	rgblight_config.enable = 1;
-	rgblight_config.mode = 1;
-	rgblight_config.hue = 200;
-	rgblight_config.sat = 204;
-	rgblight_config.val = 204;
-	eeconfig_update_rgblight(rgblight_config.raw);
+  dprintf("eeconfig_update_rgblight_default\n");
+  rgblight_config.enable = 1;
+  rgblight_config.mode = 1;
+  rgblight_config.hue = 0;
+  rgblight_config.sat = 255;
+  rgblight_config.val = 255;
+  eeconfig_update_rgblight(rgblight_config.raw);
 }
 void eeconfig_debug_rgblight(void) {
-	dprintf("rgblight_config eprom\n");
-	dprintf("rgblight_config.enable = %d\n", rgblight_config.enable);
-	dprintf("rghlight_config.mode = %d\n", rgblight_config.mode);
-	dprintf("rgblight_config.hue = %d\n", rgblight_config.hue);
-	dprintf("rgblight_config.sat = %d\n", rgblight_config.sat);
-	dprintf("rgblight_config.val = %d\n", rgblight_config.val);
+  dprintf("rgblight_config eprom\n");
+  dprintf("rgblight_config.enable = %d\n", rgblight_config.enable);
+  dprintf("rghlight_config.mode = %d\n", rgblight_config.mode);
+  dprintf("rgblight_config.hue = %d\n", rgblight_config.hue);
+  dprintf("rgblight_config.sat = %d\n", rgblight_config.sat);
+  dprintf("rgblight_config.val = %d\n", rgblight_config.val);
 }
 
 void rgblight_init(void) {
   debug_enable = 1; // Debug ON!
-	dprintf("rgblight_init called.\n");
+  dprintf("rgblight_init called.\n");
   rgblight_inited = 1;
-	dprintf("rgblight_init start!\n");
+  dprintf("rgblight_init start!\n");
   if (!eeconfig_is_enabled()) {
-		dprintf("rgblight_init eeconfig is not enabled.\n");
+    dprintf("rgblight_init eeconfig is not enabled.\n");
     eeconfig_init();
-		eeconfig_update_rgblight_default();
+    eeconfig_update_rgblight_default();
   }
   rgblight_config.raw = eeconfig_read_rgblight();
-	if (!rgblight_config.mode) {
-		dprintf("rgblight_init rgblight_config.mode = 0. Write default values to EEPROM.\n");
-		eeconfig_update_rgblight_default();
-		rgblight_config.raw = eeconfig_read_rgblight();
-	}
-	eeconfig_debug_rgblight(); // display current eeprom values
+  if (!rgblight_config.mode) {
+    dprintf("rgblight_init rgblight_config.mode = 0. Write default values to EEPROM.\n");
+    eeconfig_update_rgblight_default();
+    rgblight_config.raw = eeconfig_read_rgblight();
+  }
+  eeconfig_debug_rgblight(); // display current eeprom values
 
-	#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-		rgblight_timer_init(); // setup the timer
-	#endif
+  #ifdef RGBLIGHT_ANIMATIONS
+    rgblight_timer_init(); // setup the timer
+  #endif
 
   if (rgblight_config.enable) {
     rgblight_mode(rgblight_config.mode);
   }
 }
 
+void rgblight_update_dword(uint32_t dword) {
+  rgblight_config.raw = dword;
+  eeconfig_update_rgblight(rgblight_config.raw);
+  if (rgblight_config.enable)
+    rgblight_mode(rgblight_config.mode);
+  else {
+    #ifdef RGBLIGHT_ANIMATIONS
+      rgblight_timer_disable();
+    #endif
+      rgblight_set();
+  }
+}
+
 void rgblight_increase(void) {
-	uint8_t mode = 0;
+  uint8_t mode = 0;
   if (rgblight_config.mode < RGBLIGHT_MODES) {
     mode = rgblight_config.mode + 1;
   }
-	rgblight_mode(mode);
+  rgblight_mode(mode);
 }
-
 void rgblight_decrease(void) {
-	uint8_t mode = 0;
-  if (rgblight_config.mode > 1) { //mode will never < 1, if mode is less than 1, eeprom need to be initialized.
-    mode = rgblight_config.mode-1;
+  uint8_t mode = 0;
+  // Mode will never be < 1. If it ever is, eeprom needs to be initialized.
+  if (rgblight_config.mode > 1) {
+    mode = rgblight_config.mode - 1;
   }
-	rgblight_mode(mode);
+  rgblight_mode(mode);
 }
-
 void rgblight_step(void) {
-	uint8_t mode = 0;
+  uint8_t mode = 0;
   mode = rgblight_config.mode + 1;
   if (mode > RGBLIGHT_MODES) {
     mode = 1;
   }
-	rgblight_mode(mode);
+  rgblight_mode(mode);
 }
 
 void rgblight_mode(uint8_t mode) {
-	if (!rgblight_config.enable) {
-		return;
-	}
-  if (mode<1) {
-		rgblight_config.mode = 1;
-	} else if (mode > RGBLIGHT_MODES) {
-		rgblight_config.mode = RGBLIGHT_MODES;
-	} else {
-		rgblight_config.mode = mode;
-	}
+  if (!rgblight_config.enable) {
+    return;
+  }
+  if (mode < 1) {
+    rgblight_config.mode = 1;
+  } else if (mode > RGBLIGHT_MODES) {
+    rgblight_config.mode = RGBLIGHT_MODES;
+  } else {
+    rgblight_config.mode = mode;
+  }
   eeconfig_update_rgblight(rgblight_config.raw);
   xprintf("rgblight mode: %u\n", rgblight_config.mode);
-	if (rgblight_config.mode == 1) {
-		#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-			rgblight_timer_disable();
-		#endif
-	} else if (rgblight_config.mode >=2 && rgblight_config.mode <=23) {
-		// MODE 2-5, breathing
-		// MODE 6-8, rainbow mood
-		// MODE 9-14, rainbow swirl
-		// MODE 15-20, snake
-		// MODE 21-23, knight
-
-		#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-			rgblight_timer_enable();
-		#endif
-	}
+  if (rgblight_config.mode == 1) {
+    #ifdef RGBLIGHT_ANIMATIONS
+      rgblight_timer_disable();
+    #endif
+  } else if (rgblight_config.mode >= 2 && rgblight_config.mode <= 23) {
+    // MODE 2-5, breathing
+    // MODE 6-8, rainbow mood
+    // MODE 9-14, rainbow swirl
+    // MODE 15-20, snake
+    // MODE 21-23, knight
+
+    #ifdef RGBLIGHT_ANIMATIONS
+      rgblight_timer_enable();
+    #endif
+  }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
 }
 
@@ -215,306 +255,343 @@ void rgblight_toggle(void) {
   rgblight_config.enable ^= 1;
   eeconfig_update_rgblight(rgblight_config.raw);
   xprintf("rgblight toggle: rgblight_config.enable = %u\n", rgblight_config.enable);
-	if (rgblight_config.enable) {
-		rgblight_mode(rgblight_config.mode);
-	} else {
+  if (rgblight_config.enable) {
+    rgblight_mode(rgblight_config.mode);
+  } else {
+    #ifdef RGBLIGHT_ANIMATIONS
+      rgblight_timer_disable();
+    #endif
+    _delay_ms(50);
+    rgblight_set();
+  }
+}
 
-		#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
-			rgblight_timer_disable();
-		#endif
-		_delay_ms(50);
-		rgblight_set();
-	}
+void rgblight_enable(void) {
+  rgblight_config.enable = 1;
+  eeconfig_update_rgblight(rgblight_config.raw);
+  xprintf("rgblight enable: rgblight_config.enable = %u\n", rgblight_config.enable);
+  rgblight_mode(rgblight_config.mode);
 }
 
 
-void rgblight_increase_hue(void){
-	uint16_t hue;
+void rgblight_increase_hue(void) {
+  uint16_t hue;
   hue = (rgblight_config.hue+RGBLIGHT_HUE_STEP) % 360;
   rgblight_sethsv(hue, rgblight_config.sat, rgblight_config.val);
 }
-void rgblight_decrease_hue(void){
-	uint16_t hue;
-	if (rgblight_config.hue-RGBLIGHT_HUE_STEP <0 ) {
-		hue = (rgblight_config.hue+360-RGBLIGHT_HUE_STEP) % 360;
-	} else {
-		hue = (rgblight_config.hue-RGBLIGHT_HUE_STEP) % 360;
-	}
+void rgblight_decrease_hue(void) {
+  uint16_t hue;
+  if (rgblight_config.hue-RGBLIGHT_HUE_STEP < 0) {
+    hue = (rgblight_config.hue + 360 - RGBLIGHT_HUE_STEP) % 360;
+  } else {
+    hue = (rgblight_config.hue - RGBLIGHT_HUE_STEP) % 360;
+  }
   rgblight_sethsv(hue, rgblight_config.sat, rgblight_config.val);
 }
 void rgblight_increase_sat(void) {
-	uint8_t sat;
+  uint8_t sat;
   if (rgblight_config.sat + RGBLIGHT_SAT_STEP > 255) {
     sat = 255;
   } else {
-    sat = rgblight_config.sat+RGBLIGHT_SAT_STEP;
+    sat = rgblight_config.sat + RGBLIGHT_SAT_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, sat, rgblight_config.val);
 }
-void rgblight_decrease_sat(void){
-	uint8_t sat;
+void rgblight_decrease_sat(void) {
+  uint8_t sat;
   if (rgblight_config.sat - RGBLIGHT_SAT_STEP < 0) {
     sat = 0;
   } else {
-    sat = rgblight_config.sat-RGBLIGHT_SAT_STEP;
+    sat = rgblight_config.sat - RGBLIGHT_SAT_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, sat, rgblight_config.val);
 }
-void rgblight_increase_val(void){
-	uint8_t val;
+void rgblight_increase_val(void) {
+  uint8_t val;
   if (rgblight_config.val + RGBLIGHT_VAL_STEP > 255) {
     val = 255;
   } else {
-    val = rgblight_config.val+RGBLIGHT_VAL_STEP;
+    val = rgblight_config.val + RGBLIGHT_VAL_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, val);
 }
 void rgblight_decrease_val(void) {
-	uint8_t val;
+  uint8_t val;
   if (rgblight_config.val - RGBLIGHT_VAL_STEP < 0) {
     val = 0;
   } else {
-    val = rgblight_config.val-RGBLIGHT_VAL_STEP;
+    val = rgblight_config.val - RGBLIGHT_VAL_STEP;
   }
   rgblight_sethsv(rgblight_config.hue, rgblight_config.sat, val);
 }
 
-void rgblight_sethsv_noeeprom(uint16_t hue, uint8_t sat, uint8_t val){
-	inmem_config.raw = rgblight_config.raw;
+void rgblight_sethsv_noeeprom(uint16_t hue, uint8_t sat, uint8_t val) {
+  inmem_config.raw = rgblight_config.raw;
   if (rgblight_config.enable) {
-    struct cRGB tmp_led;
+    LED_TYPE tmp_led;
     sethsv(hue, sat, val, &tmp_led);
-		inmem_config.hue = hue;
-		inmem_config.sat = sat;
-		inmem_config.val = val;
+    inmem_config.hue = hue;
+    inmem_config.sat = sat;
+    inmem_config.val = val;
     // dprintf("rgblight set hue [MEMORY]: %u,%u,%u\n", inmem_config.hue, inmem_config.sat, inmem_config.val);
     rgblight_setrgb(tmp_led.r, tmp_led.g, tmp_led.b);
   }
 }
-void rgblight_sethsv(uint16_t hue, uint8_t sat, uint8_t val){
+void rgblight_sethsv(uint16_t hue, uint8_t sat, uint8_t val) {
   if (rgblight_config.enable) {
-		if (rgblight_config.mode == 1) {
-			// same static color
-			rgblight_sethsv_noeeprom(hue, sat, val);
-		} else {
-			// all LEDs in same color
-			if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
-				// breathing mode, ignore the change of val, use in memory value instead
-				val = rgblight_config.val;
-			} else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 14) {
-				// rainbow mood and rainbow swirl, ignore the change of hue
-				hue = rgblight_config.hue;
-			}
-		}
-		rgblight_config.hue = hue;
-		rgblight_config.sat = sat;
-		rgblight_config.val = val;
-		eeconfig_update_rgblight(rgblight_config.raw);
-		xprintf("rgblight set hsv [EEPROM]: %u,%u,%u\n", rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
-  }
-}
-
-void rgblight_setrgb(uint8_t r, uint8_t g, uint8_t b){
+    if (rgblight_config.mode == 1) {
+      // same static color
+      rgblight_sethsv_noeeprom(hue, sat, val);
+    } else {
+      // all LEDs in same color
+      if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
+        // breathing mode, ignore the change of val, use in memory value instead
+        val = rgblight_config.val;
+      } else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 14) {
+        // rainbow mood and rainbow swirl, ignore the change of hue
+        hue = rgblight_config.hue;
+      }
+    }
+    rgblight_config.hue = hue;
+    rgblight_config.sat = sat;
+    rgblight_config.val = val;
+    eeconfig_update_rgblight(rgblight_config.raw);
+    xprintf("rgblight set hsv [EEPROM]: %u,%u,%u\n", rgblight_config.hue, rgblight_config.sat, rgblight_config.val);
+  }
+}
+
+void rgblight_setrgb(uint8_t r, uint8_t g, uint8_t b) {
   // dprintf("rgblight set rgb: %u,%u,%u\n", r,g,b);
-  for (uint8_t i=0;i<RGBLED_NUM;i++) {
+  for (uint8_t i = 0; i < RGBLED_NUM; i++) {
     led[i].r = r;
     led[i].g = g;
     led[i].b = b;
   }
   rgblight_set();
-
 }
 
 void rgblight_set(void) {
-	if (rgblight_config.enable) {
-		ws2812_setleds(led, RGBLED_NUM);
-	} else {
-		for (uint8_t i=0;i<RGBLED_NUM;i++) {
-	    led[i].r = 0;
-	    led[i].g = 0;
-	    led[i].b = 0;
-	  }
-		ws2812_setleds(led, RGBLED_NUM);
-	}
+  if (rgblight_config.enable) {
+    #ifdef RGBW
+      ws2812_setleds_rgbw(led, RGBLED_NUM);
+    #else
+      ws2812_setleds(led, RGBLED_NUM);
+    #endif
+  } else {
+    for (uint8_t i = 0; i < RGBLED_NUM; i++) {
+      led[i].r = 0;
+      led[i].g = 0;
+      led[i].b = 0;
+    }
+    #ifdef RGBW
+      ws2812_setleds_rgbw(led, RGBLED_NUM);
+    #else
+      ws2812_setleds(led, RGBLED_NUM);
+    #endif
+  }
 }
 
-
-#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+#ifdef RGBLIGHT_ANIMATIONS
 
 // Animation timer -- AVR Timer3
 void rgblight_timer_init(void) {
-	static uint8_t rgblight_timer_is_init = 0;
-	if (rgblight_timer_is_init) {
-		return;
-	}
-	rgblight_timer_is_init = 1;
-	/* Timer 3 setup */
-	TCCR3B = _BV(WGM32) //CTC mode OCR3A as TOP
-	      | _BV(CS30); //Clock selelct: clk/1
-	/* Set TOP value */
-	uint8_t sreg = SREG;
-	cli();
-	OCR3AH = (RGBLED_TIMER_TOP>>8)&0xff;
-	OCR3AL = RGBLED_TIMER_TOP&0xff;
-	SREG = sreg;
+  // static uint8_t rgblight_timer_is_init = 0;
+  // if (rgblight_timer_is_init) {
+  //   return;
+  // }
+  // rgblight_timer_is_init = 1;
+  // /* Timer 3 setup */
+  // TCCR3B = _BV(WGM32) // CTC mode OCR3A as TOP
+  //       | _BV(CS30); // Clock selelct: clk/1
+  // /* Set TOP value */
+  // uint8_t sreg = SREG;
+  // cli();
+  // OCR3AH = (RGBLED_TIMER_TOP >> 8) & 0xff;
+  // OCR3AL = RGBLED_TIMER_TOP & 0xff;
+  // SREG = sreg;
+
+  rgblight_timer_enabled = true;
 }
 void rgblight_timer_enable(void) {
-	TIMSK3 |= _BV(OCIE3A);
-	dprintf("TIMER3 enabled.\n");
+  rgblight_timer_enabled = true;
+  dprintf("TIMER3 enabled.\n");
 }
 void rgblight_timer_disable(void) {
-	TIMSK3 &= ~_BV(OCIE3A);
-	dprintf("TIMER3 disabled.\n");
+  rgblight_timer_enabled = false;
+  dprintf("TIMER3 disabled.\n");
 }
 void rgblight_timer_toggle(void) {
-	TIMSK3 ^= _BV(OCIE3A);
-	dprintf("TIMER3 toggled.\n");
-}
-
-ISR(TIMER3_COMPA_vect) {
-	// Mode = 1, static light, do nothing here
-	if (rgblight_config.mode>=2 && rgblight_config.mode<=5) {
-		// mode = 2 to 5, breathing mode
-		rgblight_effect_breathing(rgblight_config.mode-2);
-
-	} else if (rgblight_config.mode>=6 && rgblight_config.mode<=8) {
-		rgblight_effect_rainbow_mood(rgblight_config.mode-6);
-	} else if (rgblight_config.mode>=9 && rgblight_config.mode<=14) {
-		rgblight_effect_rainbow_swirl(rgblight_config.mode-9);
-	} else if (rgblight_config.mode>=15 && rgblight_config.mode<=20) {
-		rgblight_effect_snake(rgblight_config.mode-15);
-	} else if (rgblight_config.mode>=21 && rgblight_config.mode<=23) {
-		rgblight_effect_knight(rgblight_config.mode-21);
-	}
+  rgblight_timer_enabled ^= rgblight_timer_enabled;
+  dprintf("TIMER3 toggled.\n");
+}
+
+void rgblight_show_solid_color(uint8_t r, uint8_t g, uint8_t b) {
+  rgblight_enable();
+  rgblight_mode(1);
+  rgblight_setrgb(r, g, b);
+}
+
+void rgblight_task(void) {
+  if (rgblight_timer_enabled) {
+    // mode = 1, static light, do nothing here
+    if (rgblight_config.mode >= 2 && rgblight_config.mode <= 5) {
+      // mode = 2 to 5, breathing mode
+      rgblight_effect_breathing(rgblight_config.mode - 2);
+    } else if (rgblight_config.mode >= 6 && rgblight_config.mode <= 8) {
+      // mode = 6 to 8, rainbow mood mod
+      rgblight_effect_rainbow_mood(rgblight_config.mode - 6);
+    } else if (rgblight_config.mode >= 9 && rgblight_config.mode <= 14) {
+      // mode = 9 to 14, rainbow swirl mode
+      rgblight_effect_rainbow_swirl(rgblight_config.mode - 9);
+    } else if (rgblight_config.mode >= 15 && rgblight_config.mode <= 20) {
+      // mode = 15 to 20, snake mode
+      rgblight_effect_snake(rgblight_config.mode - 15);
+    } else if (rgblight_config.mode >= 21 && rgblight_config.mode <= 23) {
+      // mode = 21 to 23, knight mode
+      rgblight_effect_knight(rgblight_config.mode - 21);
+    }
+  }
 }
 
-// effects
+// Effects
 void rgblight_effect_breathing(uint8_t interval) {
-	static uint8_t pos = 0;
-	static uint16_t last_timer = 0;
+  static uint8_t pos = 0;
+  static uint16_t last_timer = 0;
 
-	if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_BREATHING_INTERVALS[interval])) return;
-	last_timer = timer_read();
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_BREATHING_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
 
-	rgblight_sethsv_noeeprom(rgblight_config.hue, rgblight_config.sat, pgm_read_byte(&RGBLED_BREATHING_TABLE[pos]));
-	pos = (pos+1) % 256;
+  rgblight_sethsv_noeeprom(rgblight_config.hue, rgblight_config.sat, pgm_read_byte(&RGBLED_BREATHING_TABLE[pos]));
+  pos = (pos + 1) % 256;
 }
-
 void rgblight_effect_rainbow_mood(uint8_t interval) {
-	static uint16_t current_hue=0;
-	static uint16_t last_timer = 0;
+  static uint16_t current_hue = 0;
+  static uint16_t last_timer = 0;
 
-	if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval])) return;
-	last_timer = timer_read();
-	rgblight_sethsv_noeeprom(current_hue, rgblight_config.sat, rgblight_config.val);
-	current_hue = (current_hue+1) % 360;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
+  rgblight_sethsv_noeeprom(current_hue, rgblight_config.sat, rgblight_config.val);
+  current_hue = (current_hue + 1) % 360;
 }
-
 void rgblight_effect_rainbow_swirl(uint8_t interval) {
-	static uint16_t current_hue=0;
-	static uint16_t last_timer = 0;
-	uint16_t hue;
-	uint8_t i;
-	if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval/2])) return;
-	last_timer = timer_read();
-	for (i=0; i<RGBLED_NUM; i++) {
-		hue = (360/RGBLED_NUM*i+current_hue)%360;
-		sethsv(hue, rgblight_config.sat, rgblight_config.val, &led[i]);
-	}
-	rgblight_set();
-
-	if (interval % 2) {
-		current_hue = (current_hue+1) % 360;
-	} else {
-		if (current_hue -1 < 0) {
-			current_hue = 359;
-		} else {
-			current_hue = current_hue - 1;
-		}
-
-	}
+  static uint16_t current_hue = 0;
+  static uint16_t last_timer = 0;
+  uint16_t hue;
+  uint8_t i;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_RAINBOW_MOOD_INTERVALS[interval / 2])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    hue = (360 / RGBLED_NUM * i + current_hue) % 360;
+    sethsv(hue, rgblight_config.sat, rgblight_config.val, (LED_TYPE *)&led[i]);
+  }
+  rgblight_set();
+
+  if (interval % 2) {
+    current_hue = (current_hue + 1) % 360;
+  } else {
+    if (current_hue - 1 < 0) {
+      current_hue = 359;
+    } else {
+      current_hue = current_hue - 1;
+    }
+  }
 }
 void rgblight_effect_snake(uint8_t interval) {
-	static uint8_t pos=0;
-	static uint16_t last_timer = 0;
-	uint8_t i,j;
-	int8_t k;
-	int8_t increament = 1;
-	if (interval%2) increament = -1;
-	if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_SNAKE_INTERVALS[interval/2])) return;
-	last_timer = timer_read();
-	for (i=0;i<RGBLED_NUM;i++) {
-		led[i].r=0;
-		led[i].g=0;
-		led[i].b=0;
-		for (j=0;j<RGBLIGHT_EFFECT_SNAKE_LENGTH;j++) {
-			k = pos+j*increament;
-			if (k<0) k = k+RGBLED_NUM;
-			if (i==k) {
-				sethsv(rgblight_config.hue, rgblight_config.sat, (uint8_t)(rgblight_config.val*(RGBLIGHT_EFFECT_SNAKE_LENGTH-j)/RGBLIGHT_EFFECT_SNAKE_LENGTH), &led[i]);
-			}
-		}
-	}
-	rgblight_set();
-	if (increament == 1) {
-		if (pos - 1 < 0) {
-			pos = RGBLED_NUM-1;
-		} else {
-			pos -= 1;
-		}
-	} else {
-		pos = (pos+1)%RGBLED_NUM;
-	}
-
+  static uint8_t pos = 0;
+  static uint16_t last_timer = 0;
+  uint8_t i, j;
+  int8_t k;
+  int8_t increment = 1;
+  if (interval % 2) {
+    increment = -1;
+  }
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_SNAKE_INTERVALS[interval / 2])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    led[i].r = 0;
+    led[i].g = 0;
+    led[i].b = 0;
+    for (j = 0; j < RGBLIGHT_EFFECT_SNAKE_LENGTH; j++) {
+      k = pos + j * increment;
+      if (k < 0) {
+        k = k + RGBLED_NUM;
+      }
+      if (i == k) {
+        sethsv(rgblight_config.hue, rgblight_config.sat, (uint8_t)(rgblight_config.val*(RGBLIGHT_EFFECT_SNAKE_LENGTH-j)/RGBLIGHT_EFFECT_SNAKE_LENGTH), (LED_TYPE *)&led[i]);
+      }
+    }
+  }
+  rgblight_set();
+  if (increment == 1) {
+    if (pos - 1 < 0) {
+      pos = RGBLED_NUM - 1;
+    } else {
+      pos -= 1;
+    }
+  } else {
+    pos = (pos + 1) % RGBLED_NUM;
+  }
 }
-
 void rgblight_effect_knight(uint8_t interval) {
-	static int8_t pos=0;
-	static uint16_t last_timer = 0;
-	uint8_t i,j,cur;
-	int8_t k;
-	struct cRGB preled[RGBLED_NUM];
-	static int8_t increament = -1;
-	if (timer_elapsed(last_timer)<pgm_read_byte(&RGBLED_KNIGHT_INTERVALS[interval])) return;
-	last_timer = timer_read();
-	for (i=0;i<RGBLED_NUM;i++) {
-		preled[i].r=0;
-		preled[i].g=0;
-		preled[i].b=0;
-		for (j=0;j<RGBLIGHT_EFFECT_KNIGHT_LENGTH;j++) {
-			k = pos+j*increament;
-			if (k<0) k = 0;
-			if (k>=RGBLED_NUM) k=RGBLED_NUM-1;
-			if (i==k) {
-				sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val, &preled[i]);
-			}
-		}
-	}
-	if (RGBLIGHT_EFFECT_KNIGHT_OFFSET) {
-		for (i=0;i<RGBLED_NUM;i++) {
-			cur = (i+RGBLIGHT_EFFECT_KNIGHT_OFFSET) % RGBLED_NUM;
-			led[i].r = preled[cur].r;
-			led[i].g = preled[cur].g;
-			led[i].b = preled[cur].b;
-		}
-	}
-	rgblight_set();
-	if (increament == 1) {
-		if (pos - 1 < 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
-			pos = 0- RGBLIGHT_EFFECT_KNIGHT_LENGTH;
-			increament = -1;
-		} else {
-			pos -= 1;
-		}
-	} else {
-		if (pos+1>RGBLED_NUM+RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
-			pos = RGBLED_NUM+RGBLIGHT_EFFECT_KNIGHT_LENGTH-1;
-			increament = 1;
-		} else {
-			pos += 1;
-		}
-	}
-
-}
-
-#endif
\ No newline at end of file
+  static int8_t pos = 0;
+  static uint16_t last_timer = 0;
+  uint8_t i, j, cur;
+  int8_t k;
+  LED_TYPE preled[RGBLED_NUM];
+  static int8_t increment = -1;
+  if (timer_elapsed(last_timer) < pgm_read_byte(&RGBLED_KNIGHT_INTERVALS[interval])) {
+    return;
+  }
+  last_timer = timer_read();
+  for (i = 0; i < RGBLED_NUM; i++) {
+    preled[i].r = 0;
+    preled[i].g = 0;
+    preled[i].b = 0;
+    for (j = 0; j < RGBLIGHT_EFFECT_KNIGHT_LENGTH; j++) {
+      k = pos + j * increment;
+      if (k < 0) {
+        k = 0;
+      }
+      if (k >= RGBLED_NUM) {
+        k = RGBLED_NUM - 1;
+      }
+      if (i == k) {
+        sethsv(rgblight_config.hue, rgblight_config.sat, rgblight_config.val, (LED_TYPE *)&preled[i]);
+      }
+    }
+  }
+  if (RGBLIGHT_EFFECT_KNIGHT_OFFSET) {
+    for (i = 0; i < RGBLED_NUM; i++) {
+      cur = (i + RGBLIGHT_EFFECT_KNIGHT_OFFSET) % RGBLED_NUM;
+      led[i].r = preled[cur].r;
+      led[i].g = preled[cur].g;
+      led[i].b = preled[cur].b;
+    }
+  }
+  rgblight_set();
+  if (increment == 1) {
+    if (pos - 1 < 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
+      pos = 0 - RGBLIGHT_EFFECT_KNIGHT_LENGTH;
+      increment = -1;
+    } else {
+      pos -= 1;
+    }
+  } else {
+    if (pos + 1 > RGBLED_NUM + RGBLIGHT_EFFECT_KNIGHT_LENGTH) {
+      pos = RGBLED_NUM + RGBLIGHT_EFFECT_KNIGHT_LENGTH - 1;
+      increment = 1;
+    } else {
+      pos += 1;
+    }
+  }
+}
+
+#endif
diff --git a/quantum/rgblight.h b/quantum/rgblight.h
index def26c428c..aa1d026e0e 100644
--- a/quantum/rgblight.h
+++ b/quantum/rgblight.h
@@ -1,8 +1,7 @@
 #ifndef RGBLIGHT_H
 #define RGBLIGHT_H
 
-
-#if !defined(AUDIO_ENABLE) && defined(RGBLIGHT_TIMER)
+#ifdef RGBLIGHT_ANIMATIONS
 	#define RGBLIGHT_MODES 23
 #else
 	#define RGBLIGHT_MODES 1
@@ -34,12 +33,19 @@
 #endif
 
 #define RGBLED_TIMER_TOP F_CPU/(256*64)
+// #define RGBLED_TIMER_TOP 0xFF10
 
 #include <stdint.h>
 #include <stdbool.h>
 #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 {
@@ -55,9 +61,11 @@ void rgblight_init(void);
 void rgblight_increase(void);
 void rgblight_decrease(void);
 void rgblight_toggle(void);
+void rgblight_enable(void);
 void rgblight_step(void);
 void rgblight_mode(uint8_t mode);
 void rgblight_set(void);
+void rgblight_update_dword(uint32_t dword);
 void rgblight_increase_hue(void);
 void rgblight_decrease_hue(void);
 void rgblight_increase_sat(void);
@@ -72,10 +80,15 @@ void eeconfig_update_rgblight(uint32_t val);
 void eeconfig_update_rgblight_default(void);
 void eeconfig_debug_rgblight(void);
 
-void sethsv(uint16_t hue, uint8_t sat, uint8_t val, struct cRGB *led1);
-void setrgb(uint8_t r, uint8_t g, uint8_t b, struct cRGB *led1);
+void sethsv(uint16_t hue, uint8_t sat, uint8_t val, LED_TYPE *led1);
+void setrgb(uint8_t r, uint8_t g, uint8_t b, LED_TYPE *led1);
 void rgblight_sethsv_noeeprom(uint16_t hue, uint8_t sat, uint8_t val);
 
+#define EZ_RGB(val) rgblight_show_solid_color((val >> 16) & 0xFF, (val >> 8) & 0xFF, val & 0xFF)
+void rgblight_show_solid_color(uint8_t r, uint8_t g, uint8_t b);
+
+void rgblight_task(void);
+
 void rgblight_timer_init(void);
 void rgblight_timer_enable(void);
 void rgblight_timer_disable(void);
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