DO NOT USE Merge branch 'master' into debounce_refactor

Merged, however now there are two debounce.h and debounce.c to mess around with and coalesce.
# Conflicts:
#	quantum/matrix.c

30 files changed, 450 insertions, 952 deletions

diff --git a/tmk_core/common/action.c b/tmk_core/common/action.c
index b99c2acaa..ec8d6ed7b 100644
--- a/tmk_core/common/action.c
+++ b/tmk_core/common/action.c
@@ -653,7 +653,7 @@ void process_action(keyrecord_t *record, action_t action)
 
 #ifndef NO_ACTION_TAPPING
   #ifdef RETRO_TAPPING
-  if (!is_tap_key(record->event.key)) {
+  if (!is_tap_action(action)) {
     retro_tapping_counter = 0;
   } else {
     if (event.pressed) {
@@ -929,7 +929,15 @@ void clear_keyboard_but_mods_and_keys()
 bool is_tap_key(keypos_t key)
 {
     action_t action = layer_switch_get_action(key);
+    return is_tap_action(action);
+}
 
+/** \brief Utilities for actions. (FIXME: Needs better description)
+ *
+ * FIXME: Needs documentation.
+ */
+bool is_tap_action(action_t action)
+{
     switch (action.kind.id) {
         case ACT_LMODS_TAP:
         case ACT_RMODS_TAP:
diff --git a/tmk_core/common/action.h b/tmk_core/common/action.h
index 8e47e5339..799e3bb0e 100644
--- a/tmk_core/common/action.h
+++ b/tmk_core/common/action.h
@@ -97,6 +97,7 @@ void clear_keyboard_but_mods(void);
 void clear_keyboard_but_mods_and_keys(void);
 void layer_switch(uint8_t new_layer);
 bool is_tap_key(keypos_t key);
+bool is_tap_action(action_t action);
 
 #ifndef NO_ACTION_TAPPING
 void process_record_tap_hint(keyrecord_t *record);
diff --git a/tmk_core/common/action_layer.c b/tmk_core/common/action_layer.c
index 120ce3f51..6ff8c5549 100644
--- a/tmk_core/common/action_layer.c
+++ b/tmk_core/common/action_layer.c
@@ -17,82 +17,76 @@ uint32_t default_layer_state = 0;
 
 /** \brief Default Layer State Set At user Level
  *
- * FIXME: Needs docs
+ * Run user code on default layer state change
  */
 __attribute__((weak))
 uint32_t default_layer_state_set_user(uint32_t state) {
-    return state;
+  return state;
 }
 
 /** \brief Default Layer State Set At Keyboard Level
  *
- * FIXME: Needs docs
+ *  Run keyboard code on default layer state change
  */
 __attribute__((weak))
 uint32_t default_layer_state_set_kb(uint32_t state) {
-    return default_layer_state_set_user(state);
+  return default_layer_state_set_user(state);
 }
 
 /** \brief Default Layer State Set
  *
- * FIXME: Needs docs
+ * Static function to set the default layer state, prints debug info and clears keys
  */
-static void default_layer_state_set(uint32_t state)
-{
-    state = default_layer_state_set_kb(state);
-    debug("default_layer_state: ");
-    default_layer_debug(); debug(" to ");
-    default_layer_state = state;
-    default_layer_debug(); debug("\n");
+static void default_layer_state_set(uint32_t state) {
+  state = default_layer_state_set_kb(state);
+  debug("default_layer_state: ");
+  default_layer_debug(); debug(" to ");
+  default_layer_state = state;
+  default_layer_debug(); debug("\n");
 #ifdef STRICT_LAYER_RELEASE
-    clear_keyboard_but_mods(); // To avoid stuck keys
+  clear_keyboard_but_mods(); // To avoid stuck keys
 #else
-    clear_keyboard_but_mods_and_keys(); // Don't reset held keys
+  clear_keyboard_but_mods_and_keys(); // Don't reset held keys
 #endif
 }
 
 /** \brief Default Layer Print
  *
- * FIXME: Needs docs
+ * Print out the hex value of the 32-bit default layer state, as well as the value of the highest bit.
  */
-void default_layer_debug(void)
-{
-    dprintf("%08lX(%u)", default_layer_state, biton32(default_layer_state));
+void default_layer_debug(void) {
+  dprintf("%08lX(%u)", default_layer_state, biton32(default_layer_state));
 }
 
 /** \brief Default Layer Set
  *
- * FIXME: Needs docs
+ * Sets the default layer state.
  */
-void default_layer_set(uint32_t state)
-{
-    default_layer_state_set(state);
+void default_layer_set(uint32_t state) {
+  default_layer_state_set(state);
 }
 
 #ifndef NO_ACTION_LAYER
 /** \brief Default Layer Or
  *
- * FIXME: Needs docs
+ * Turns on the default layer based on matching bits between specifed layer and existing layer state
  */
-void default_layer_or(uint32_t state)
-{
-    default_layer_state_set(default_layer_state | state);
+void default_layer_or(uint32_t state) {
+  default_layer_state_set(default_layer_state | state);
 }
 /** \brief Default Layer And
  *
- * FIXME: Needs docs
+ * Turns on default layer based on matching enabled bits between specifed layer and existing layer state
  */
-void default_layer_and(uint32_t state)
-{
-    default_layer_state_set(default_layer_state & state);
+void default_layer_and(uint32_t state) {
+  default_layer_state_set(default_layer_state & state);
 }
 /** \brief Default Layer Xor
  *
- * FIXME: Needs docs
+ * Turns on default layer based on non-matching bits between specifed layer and existing layer state
  */
-void default_layer_xor(uint32_t state)
-{
-    default_layer_state_set(default_layer_state ^ state);
+void default_layer_xor(uint32_t state) {
+  default_layer_state_set(default_layer_state ^ state);
 }
 #endif
 
@@ -104,170 +98,168 @@ uint32_t layer_state = 0;
 
 /** \brief Layer state set user
  *
- * FIXME: Needs docs
+ * Runs user code on layer state change
  */
 __attribute__((weak))
 uint32_t layer_state_set_user(uint32_t state) {
-    return state;
+  return state;
 }
 
 /** \brief Layer state set keyboard
  *
- * FIXME: Needs docs
+ * Runs keyboard code on layer state change
  */
 __attribute__((weak))
 uint32_t layer_state_set_kb(uint32_t state) {
-    return layer_state_set_user(state);
+  return layer_state_set_user(state);
 }
 
 /** \brief Layer state set
  *
- * FIXME: Needs docs
+ * Sets the layer to match the specifed state (a bitmask)
  */
-void layer_state_set(uint32_t state)
-{
-    state = layer_state_set_kb(state);
-    dprint("layer_state: ");
-    layer_debug(); dprint(" to ");
-    layer_state = state;
-    layer_debug(); dprintln();
+void layer_state_set(uint32_t state) {
+  state = layer_state_set_kb(state);
+  dprint("layer_state: ");
+  layer_debug(); dprint(" to ");
+  layer_state = state;
+  layer_debug(); dprintln();
 #ifdef STRICT_LAYER_RELEASE
-    clear_keyboard_but_mods(); // To avoid stuck keys
+  clear_keyboard_but_mods(); // To avoid stuck keys
 #else
-    clear_keyboard_but_mods_and_keys(); // Don't reset held keys
+  clear_keyboard_but_mods_and_keys(); // Don't reset held keys
 #endif
 }
 
 /** \brief Layer clear
  *
- * FIXME: Needs docs
+ * Turn off all layers
  */
-void layer_clear(void)
-{
-    layer_state_set(0);
+void layer_clear(void) {
+  layer_state_set(0);
 }
 
 /** \brief Layer state is
  *
- * FIXME: Needs docs
+ * Return whether the given state is on (it might still be shadowed by a higher state, though)
  */
-bool layer_state_is(uint8_t layer)
-{
-    return layer_state_cmp(layer_state, layer);
+bool layer_state_is(uint8_t layer) {
+  return layer_state_cmp(layer_state, layer);
 }
 
 /** \brief Layer state compare
  *
- * FIXME: Needs docs
+ * Used for comparing layers {mostly used for unit testing}
  */
 bool layer_state_cmp(uint32_t cmp_layer_state, uint8_t layer) {
-    if (!cmp_layer_state) { return layer == 0; }
-    return (cmp_layer_state & (1UL<<layer)) != 0;
+  if (!cmp_layer_state) { return layer == 0; }
+  return (cmp_layer_state & (1UL<<layer)) != 0;
 }
 
 /** \brief Layer move
  *
- * FIXME: Needs docs
+ * Turns on the given layer and turn off all other layers
  */
-void layer_move(uint8_t layer)
-{
-    layer_state_set(1UL<<layer);
+void layer_move(uint8_t layer) {
+  layer_state_set(1UL<<layer);
 }
 
 /** \brief Layer on
  *
- * FIXME: Needs docs
+ * Turns on given layer
  */
-void layer_on(uint8_t layer)
-{
-    layer_state_set(layer_state | (1UL<<layer));
+void layer_on(uint8_t layer) {
+  layer_state_set(layer_state | (1UL<<layer));
 }
 
 /** \brief Layer off
  *
- * FIXME: Needs docs
+ * Turns off given layer
  */
-void layer_off(uint8_t layer)
-{
-    layer_state_set(layer_state & ~(1UL<<layer));
+void layer_off(uint8_t layer) {
+  layer_state_set(layer_state & ~(1UL<<layer));
 }
 
 /** \brief Layer invert
  *
- * FIXME: Needs docs
+ * Toggle the given layer (set it if it's unset, or unset it if it's set)
  */
-void layer_invert(uint8_t layer)
-{
-    layer_state_set(layer_state ^ (1UL<<layer));
+void layer_invert(uint8_t layer) {
+  layer_state_set(layer_state ^ (1UL<<layer));
 }
 
 /** \brief Layer or
  *
- * FIXME: Needs docs
+ * Turns on layers based on matching bits between specifed layer and existing layer state
  */
-void layer_or(uint32_t state)
-{
-    layer_state_set(layer_state | state);
+void layer_or(uint32_t state) {
+  layer_state_set(layer_state | state);
 }
 /** \brief Layer and
  *
- * FIXME: Needs docs
+ * Turns on layers based on matching enabled bits between specifed layer and existing layer state
  */
-void layer_and(uint32_t state)
-{
-    layer_state_set(layer_state & state);
+void layer_and(uint32_t state) {
+  layer_state_set(layer_state & state);
 }
 /** \brief Layer xor
  *
- * FIXME: Needs docs
+ * Turns on layers based on non-matching bits between specifed layer and existing layer state
  */
-void layer_xor(uint32_t state)
-{
-    layer_state_set(layer_state ^ state);
+void layer_xor(uint32_t state) {
+  layer_state_set(layer_state ^ state);
 }
 
 /** \brief Layer debug printing
  *
- * FIXME: Needs docs
+ * Print out the hex value of the 32-bit layer state, as well as the value of the highest bit.
  */
-void layer_debug(void)
-{
-    dprintf("%08lX(%u)", layer_state, biton32(layer_state));
+void layer_debug(void) {
+  dprintf("%08lX(%u)", layer_state, biton32(layer_state));
 }
 #endif
 
 #if !defined(NO_ACTION_LAYER) && !defined(STRICT_LAYER_RELEASE)
+/** \brief source layer cache
+ */
+
 uint8_t source_layers_cache[(MATRIX_ROWS * MATRIX_COLS + 7) / 8][MAX_LAYER_BITS] = {{0}};
 
-void update_source_layers_cache(keypos_t key, uint8_t layer)
-{
-    const uint8_t key_number = key.col + (key.row * MATRIX_COLS);
-    const uint8_t storage_row = key_number / 8;
-    const uint8_t storage_bit = key_number % 8;
-
-    for (uint8_t bit_number = 0; bit_number < MAX_LAYER_BITS; bit_number++) {
-        source_layers_cache[storage_row][bit_number] ^=
-            (-((layer & (1U << bit_number)) != 0)
-             ^ source_layers_cache[storage_row][bit_number])
-            & (1U << storage_bit);
-    }
+/** \brief update source layers cache
+ *
+ * Updates the cached keys when changing layers
+ */
+void update_source_layers_cache(keypos_t key, uint8_t layer) {
+  const uint8_t key_number = key.col + (key.row * MATRIX_COLS);
+  const uint8_t storage_row = key_number / 8;
+  const uint8_t storage_bit = key_number % 8;
+
+  for (uint8_t bit_number = 0; bit_number < MAX_LAYER_BITS; bit_number++) {
+    source_layers_cache[storage_row][bit_number] ^=
+      (-((layer & (1U << bit_number)) != 0)
+        ^ source_layers_cache[storage_row][bit_number])
+      & (1U << storage_bit);
+  }
 }
 
-uint8_t read_source_layers_cache(keypos_t key)
-{
-    const uint8_t key_number = key.col + (key.row * MATRIX_COLS);
-    const uint8_t storage_row = key_number / 8;
-    const uint8_t storage_bit = key_number % 8;
-    uint8_t layer = 0;
-
-    for (uint8_t bit_number = 0; bit_number < MAX_LAYER_BITS; bit_number++) {
-        layer |=
-            ((source_layers_cache[storage_row][bit_number]
-              & (1U << storage_bit)) != 0)
-            << bit_number;
-    }
-
-    return layer;
+/** \brief read source layers cache
+ *
+ * reads the cached keys stored when the layer was changed
+ */
+uint8_t read_source_layers_cache(keypos_t key) {
+  const uint8_t key_number = key.col + (key.row * MATRIX_COLS);
+  const uint8_t storage_row = key_number / 8;
+  const uint8_t storage_bit = key_number % 8;
+  uint8_t layer = 0;
+
+  for (uint8_t bit_number = 0; bit_number < MAX_LAYER_BITS; bit_number++) {
+    layer |=
+      ((source_layers_cache[storage_row][bit_number]
+        & (1U << storage_bit)) != 0)
+      << bit_number;
+  }
+
+  return layer;
 }
 #endif
 
@@ -278,61 +270,58 @@ uint8_t read_source_layers_cache(keypos_t key)
  * when the layer is switched after the down event but before the up
  * event as they may get stuck otherwise.
  */
-action_t store_or_get_action(bool pressed, keypos_t key)
-{
+action_t store_or_get_action(bool pressed, keypos_t key) {
 #if !defined(NO_ACTION_LAYER) && !defined(STRICT_LAYER_RELEASE)
-    if (disable_action_cache) {
-        return layer_switch_get_action(key);
-    }
-
-    uint8_t layer;
-
-    if (pressed) {
-        layer = layer_switch_get_layer(key);
-        update_source_layers_cache(key, layer);
-    }
-    else {
-        layer = read_source_layers_cache(key);
-    }
-    return action_for_key(layer, key);
-#else
+  if (disable_action_cache) {
     return layer_switch_get_action(key);
+  }
+
+  uint8_t layer;
+
+  if (pressed) {
+    layer = layer_switch_get_layer(key);
+    update_source_layers_cache(key, layer);
+  }
+  else {
+    layer = read_source_layers_cache(key);
+  }
+  return action_for_key(layer, key);
+#else
+  return layer_switch_get_action(key);
 #endif
 }
 
 
 /** \brief Layer switch get layer
  *
- * FIXME: Needs docs
+ * Gets the layer based on key info
  */
-int8_t layer_switch_get_layer(keypos_t key)
-{
+int8_t layer_switch_get_layer(keypos_t key) {
 #ifndef NO_ACTION_LAYER
-    action_t action;
-    action.code = ACTION_TRANSPARENT;
-
-    uint32_t layers = layer_state | default_layer_state;
-    /* check top layer first */
-    for (int8_t i = 31; i >= 0; i--) {
-        if (layers & (1UL<<i)) {
-            action = action_for_key(i, key);
-            if (action.code != ACTION_TRANSPARENT) {
-                return i;
-            }
-        }
+  action_t action;
+  action.code = ACTION_TRANSPARENT;
+
+  uint32_t layers = layer_state | default_layer_state;
+  /* check top layer first */
+  for (int8_t i = 31; i >= 0; i--) {
+    if (layers & (1UL<<i)) {
+      action = action_for_key(i, key);
+      if (action.code != ACTION_TRANSPARENT) {
+          return i;
+      }
     }
-    /* fall back to layer 0 */
-    return 0;
+  }
+  /* fall back to layer 0 */
+  return 0;
 #else
-    return biton32(default_layer_state);
+  return biton32(default_layer_state);
 #endif
 }
 
 /** \brief Layer switch get layer
  *
- * FIXME: Needs docs
+ * Gets action code based on key position
  */
-action_t layer_switch_get_action(keypos_t key)
-{
-    return action_for_key(layer_switch_get_layer(key), key);
+action_t layer_switch_get_action(keypos_t key) {
+  return action_for_key(layer_switch_get_layer(key), key);
 }
diff --git a/tmk_core/common/action_util.c b/tmk_core/common/action_util.c
index afd4ae8b2..58401ace5 100644
--- a/tmk_core/common/action_util.c
+++ b/tmk_core/common/action_util.c
@@ -54,7 +54,7 @@ int8_t get_oneshot_locked_mods(void) { return oneshot_locked_mods; }
 void set_oneshot_locked_mods(int8_t mods) { oneshot_locked_mods = mods; }
 void clear_oneshot_locked_mods(void) { oneshot_locked_mods = 0; }
 #if (defined(ONESHOT_TIMEOUT) && (ONESHOT_TIMEOUT > 0))
-static int16_t oneshot_time = 0;
+static uint16_t oneshot_time = 0;
 bool has_oneshot_mods_timed_out(void) {
   return TIMER_DIFF_16(timer_read(), oneshot_time) >= ONESHOT_TIMEOUT;
 }
@@ -79,7 +79,7 @@ inline uint8_t get_oneshot_layer(void) { return oneshot_layer_data >> 3; }
 inline uint8_t get_oneshot_layer_state(void) { return oneshot_layer_data & 0b111; }
 
 #if (defined(ONESHOT_TIMEOUT) && (ONESHOT_TIMEOUT > 0))
-static int16_t oneshot_layer_time = 0;
+static uint16_t oneshot_layer_time = 0;
 inline bool has_oneshot_layer_timed_out() {
     return TIMER_DIFF_16(timer_read(), oneshot_layer_time) >= ONESHOT_TIMEOUT &&
         !(get_oneshot_layer_state() & ONESHOT_TOGGLED);
diff --git a/tmk_core/common/arm_atsam/timer.c b/tmk_core/common/arm_atsam/timer.c
index bcfe5002c..6c3905e30 100644
--- a/tmk_core/common/arm_atsam/timer.c
+++ b/tmk_core/common/arm_atsam/timer.c
@@ -9,7 +9,7 @@ void set_time(uint64_t tset)
 
 void timer_init(void)
 {
-    ms_clk = 0;
+    timer_clear();
 }
 
 uint16_t timer_read(void)
@@ -37,23 +37,7 @@ uint32_t timer_elapsed32(uint32_t tlast)
     return TIMER_DIFF_32(timer_read32(), tlast);
 }
 
-uint32_t timer_elapsed64(uint32_t tlast)
-{
-    uint64_t tnow = timer_read64();
-    return (tnow >= tlast ? tnow - tlast : UINT64_MAX - tlast + tnow);
-}
-
 void timer_clear(void)
 {
-    ms_clk = 0;
-}
-
-void wait_ms(uint64_t msec)
-{
-    CLK_delay_ms(msec);
-}
-
-void wait_us(uint16_t usec)
-{
-    CLK_delay_us(usec);
+    set_time(0);
 }
diff --git a/tmk_core/common/chibios/eeprom_stm32.c b/tmk_core/common/chibios/eeprom_stm32.c
index a86998550..a15430d67 100755
--- a/tmk_core/common/chibios/eeprom_stm32.c
+++ b/tmk_core/common/chibios/eeprom_stm32.c
@@ -10,664 +10,206 @@
  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  * DEALINGS IN THE SOFTWARE.
  *
- * This files are free to use from https://github.com/rogerclarkmelbourne/Arduino_STM32 and
- * https://github.com/leaflabs/libmaple
+ * This files are free to use from http://engsta.com/stm32-flash-memory-eeprom-emulator/ by
+ * Artur F.
  *
  * Modifications for QMK and STM32F303 by Yiancar
  */
 
+#include <stdio.h>
+#include <string.h>
 #include "eeprom_stm32.h"
+/*****************************************************************************
+ * Allows to use the internal flash to store non volatile data. To initialize
+ * the functionality use the EEPROM_Init() function. Be sure that by reprogramming
+ * of the controller just affected pages will be deleted. In other case the non
+ * volatile data will be lost.
+******************************************************************************/
+
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+/* Functions -----------------------------------------------------------------*/
+
+uint8_t DataBuf[FEE_PAGE_SIZE];
+/*****************************************************************************
+*  Delete Flash Space used for user Data, deletes the whole space between
+*  RW_PAGE_BASE_ADDRESS and the last uC Flash Page
+******************************************************************************/
+uint16_t EEPROM_Init(void) {
+    // unlock flash
+    FLASH_Unlock();
 
-    FLASH_Status EE_ErasePage(uint32_t);
-
-    uint16_t EE_CheckPage(uint32_t, uint16_t);
-    uint16_t EE_CheckErasePage(uint32_t, uint16_t);
-    uint16_t EE_Format(void);
-    uint32_t EE_FindValidPage(void);
-    uint16_t EE_GetVariablesCount(uint32_t, uint16_t);
-    uint16_t EE_PageTransfer(uint32_t, uint32_t, uint16_t);
-    uint16_t EE_VerifyPageFullWriteVariable(uint16_t, uint16_t);
-
-    uint32_t PageBase0 = EEPROM_PAGE0_BASE;
-    uint32_t PageBase1 = EEPROM_PAGE1_BASE;
-    uint32_t PageSize = EEPROM_PAGE_SIZE;
-    uint16_t Status = EEPROM_NOT_INIT;
-
-// See http://www.st.com/web/en/resource/technical/document/application_note/CD00165693.pdf
-
-/**
-  * @brief  Check page for blank
-  * @param  page base address
-  * @retval Success or error
-  *     EEPROM_BAD_FLASH:   page not empty after erase
-  *     EEPROM_OK:          page blank
-  */
-uint16_t EE_CheckPage(uint32_t pageBase, uint16_t status)
-{
-    uint32_t pageEnd = pageBase + (uint32_t)PageSize;
-
-    // Page Status not EEPROM_ERASED and not a "state"
-    if ((*(__IO uint16_t*)pageBase) != EEPROM_ERASED && (*(__IO uint16_t*)pageBase) != status)
-        return EEPROM_BAD_FLASH;
-    for(pageBase += 4; pageBase < pageEnd; pageBase += 4)
-        if ((*(__IO uint32_t*)pageBase) != 0xFFFFFFFF)  // Verify if slot is empty
-            return EEPROM_BAD_FLASH;
-    return EEPROM_OK;
-}
-
-/**
-  * @brief  Erase page with increment erase counter (page + 2)
-  * @param  page base address
-  * @retval Success or error
-  *         FLASH_COMPLETE: success erase
-  *         - Flash error code: on write Flash error
-  */
-FLASH_Status EE_ErasePage(uint32_t pageBase)
-{
-    FLASH_Status FlashStatus;
-    uint16_t data = (*(__IO uint16_t*)(pageBase));
-    if ((data == EEPROM_ERASED) || (data == EEPROM_VALID_PAGE) || (data == EEPROM_RECEIVE_DATA))
-        data = (*(__IO uint16_t*)(pageBase + 2)) + 1;
-    else
-        data = 0;
-
-    FlashStatus = FLASH_ErasePage(pageBase);
-    if (FlashStatus == FLASH_COMPLETE)
-        FlashStatus = FLASH_ProgramHalfWord(pageBase + 2, data);
-
-    return FlashStatus;
-}
+    // Clear Flags
+    //FLASH_ClearFlag(FLASH_SR_EOP|FLASH_SR_PGERR|FLASH_SR_WRPERR);
 
-/**
-  * @brief  Check page for blank and erase it
-  * @param  page base address
-  * @retval Success or error
-  *         - Flash error code: on write Flash error
-  *         - EEPROM_BAD_FLASH: page not empty after erase
-  *         - EEPROM_OK:            page blank
-  */
-uint16_t EE_CheckErasePage(uint32_t pageBase, uint16_t status)
-{
-    uint16_t FlashStatus;
-    if (EE_CheckPage(pageBase, status) != EEPROM_OK)
-    {
-        FlashStatus = EE_ErasePage(pageBase);
-        if (FlashStatus != FLASH_COMPLETE)
-            return FlashStatus;
-        return EE_CheckPage(pageBase, status);
-    }
-    return EEPROM_OK;
+    return FEE_DENSITY_BYTES;
 }
+/*****************************************************************************
+*  Erase the whole reserved Flash Space used for user Data
+******************************************************************************/
+void EEPROM_Erase (void) {
 
-/**
-  * @brief  Find valid Page for write or read operation
-  * @param  Page0: Page0 base address
-  *         Page1: Page1 base address
-  * @retval Valid page address (PAGE0 or PAGE1) or NULL in case of no valid page was found
-  */
-uint32_t EE_FindValidPage(void)
-{
-    uint16_t status0 = (*(__IO uint16_t*)PageBase0);        // Get Page0 actual status
-    uint16_t status1 = (*(__IO uint16_t*)PageBase1);        // Get Page1 actual status
-
-    if (status0 == EEPROM_VALID_PAGE && status1 == EEPROM_ERASED)
-        return PageBase0;
-    if (status1 == EEPROM_VALID_PAGE && status0 == EEPROM_ERASED)
-        return PageBase1;
-
-    return 0;
-}
+    int page_num = 0;
 
-/**
-  * @brief  Calculate unique variables in EEPROM
-  * @param  start: address of first slot to check (page + 4)
-  * @param  end: page end address
-  * @param  address: 16 bit virtual address of the variable to excluse (or 0XFFFF)
-  * @retval count of variables
-  */
-uint16_t EE_GetVariablesCount(uint32_t pageBase, uint16_t skipAddress)
-{
-    uint16_t varAddress, nextAddress;
-    uint32_t idx;
-    uint32_t pageEnd = pageBase + (uint32_t)PageSize;
-    uint16_t count = 0;
-
-    for (pageBase += 6; pageBase < pageEnd; pageBase += 4)
-    {
-        varAddress = (*(__IO uint16_t*)pageBase);
-        if (varAddress == 0xFFFF || varAddress == skipAddress)
-            continue;
-
-        count++;
-        for(idx = pageBase + 4; idx < pageEnd; idx += 4)
-        {
-            nextAddress = (*(__IO uint16_t*)idx);
-            if (nextAddress == varAddress)
-            {
-                count--;
-                break;
-            }
-        }
-    }
-    return count;
+    // delete all pages from specified start page to the last page
+    do {
+        FLASH_ErasePage(FEE_PAGE_BASE_ADDRESS + (page_num * FEE_PAGE_SIZE));
+        page_num++;
+    } while (page_num < FEE_DENSITY_PAGES);
 }
+/*****************************************************************************
+*  Writes once data byte to flash on specified address. If a byte is already
+*  written, the whole page must be copied to a buffer, the byte changed and
+*  the manipulated buffer written after PageErase.
+*******************************************************************************/
+uint16_t EEPROM_WriteDataByte (uint16_t Address, uint8_t DataByte) {
 
-/**
-  * @brief  Transfers last updated variables data from the full Page to an empty one.
-  * @param  newPage: new page base address
-  * @param  oldPage: old page base address
-  * @param  SkipAddress: 16 bit virtual address of the variable (or 0xFFFF)
-  * @retval Success or error status:
-  *           - FLASH_COMPLETE: on success
-  *           - EEPROM_OUT_SIZE: if valid new page is full
-  *           - Flash error code: on write Flash error
-  */
-uint16_t EE_PageTransfer(uint32_t newPage, uint32_t oldPage, uint16_t SkipAddress)
-{
-    uint32_t oldEnd, newEnd;
-    uint32_t oldIdx, newIdx, idx;
-    uint16_t address, data, found;
-    FLASH_Status FlashStatus;
-
-    // Transfer process: transfer variables from old to the new active page
-    newEnd = newPage + ((uint32_t)PageSize);
-
-    // Find first free element in new page
-    for (newIdx = newPage + 4; newIdx < newEnd; newIdx += 4)
-        if ((*(__IO uint32_t*)newIdx) == 0xFFFFFFFF)    // Verify if element
-            break;                                  //  contents are 0xFFFFFFFF
-    if (newIdx >= newEnd)
-        return EEPROM_OUT_SIZE;
-
-    oldEnd = oldPage + 4;
-    oldIdx = oldPage + (uint32_t)(PageSize - 2);
-
-    for (; oldIdx > oldEnd; oldIdx -= 4)
-    {
-        address = *(__IO uint16_t*)oldIdx;
-        if (address == 0xFFFF || address == SkipAddress)
-            continue;                       // it's means that power off after write data
-
-        found = 0;
-        for (idx = newPage + 6; idx < newIdx; idx += 4)
-            if ((*(__IO uint16_t*)(idx)) == address)
-            {
-                found = 1;
-                break;
-            }
-
-        if (found)
-            continue;
-
-        if (newIdx < newEnd)
-        {
-            data = (*(__IO uint16_t*)(oldIdx - 2));
-
-            FlashStatus = FLASH_ProgramHalfWord(newIdx, data);
-            if (FlashStatus != FLASH_COMPLETE)
-                return FlashStatus;
+    FLASH_Status FlashStatus = FLASH_COMPLETE;
 
-            FlashStatus = FLASH_ProgramHalfWord(newIdx + 2, address);
-            if (FlashStatus != FLASH_COMPLETE)
-                return FlashStatus;
+    uint32_t page;
+    int i;
 
-            newIdx += 4;
-        }
-        else
-            return EEPROM_OUT_SIZE;
+    // exit if desired address is above the limit (e.G. under 2048 Bytes for 4 pages)
+    if (Address > FEE_DENSITY_BYTES) {
+        return 0;
     }
 
-    // Erase the old Page: Set old Page status to EEPROM_EEPROM_ERASED status
-    data = EE_CheckErasePage(oldPage, EEPROM_ERASED);
-    if (data != EEPROM_OK)
-        return data;
+    // calculate which page is affected (Pagenum1/Pagenum2...PagenumN)
+    page = (FEE_PAGE_BASE_ADDRESS + FEE_ADDR_OFFSET(Address)) & 0x00000FFF;
 
-    // Set new Page status
-    FlashStatus = FLASH_ProgramHalfWord(newPage, EEPROM_VALID_PAGE);
-    if (FlashStatus != FLASH_COMPLETE)
-        return FlashStatus;
+    if (page % FEE_PAGE_SIZE) page = page + FEE_PAGE_SIZE;
+    page = (page / FEE_PAGE_SIZE) - 1;
 
-    return EEPROM_OK;
-}
+    // if current data is 0xFF, the byte is empty, just overwrite with the new one
+    if ((*(__IO uint16_t*)(FEE_PAGE_BASE_ADDRESS + FEE_ADDR_OFFSET(Address))) == FEE_EMPTY_WORD) {
 
-/**
-  * @brief  Verify if active page is full and Writes variable in EEPROM.
-  * @param  Address: 16 bit virtual address of the variable
-  * @param  Data: 16 bit data to be written as variable value
-  * @retval Success or error status:
-  *           - FLASH_COMPLETE: on success
-  *           - EEPROM_PAGE_FULL: if valid page is full (need page transfer)
-  *           - EEPROM_NO_VALID_PAGE: if no valid page was found
-  *           - EEPROM_OUT_SIZE: if EEPROM size exceeded
-  *           - Flash error code: on write Flash error
-  */
-uint16_t EE_VerifyPageFullWriteVariable(uint16_t Address, uint16_t Data)
-{
-    FLASH_Status FlashStatus;
-    uint32_t idx, pageBase, pageEnd, newPage;
-    uint16_t count;
-
-    // Get valid Page for write operation
-    pageBase = EE_FindValidPage();
-    if (pageBase == 0)
-        return  EEPROM_NO_VALID_PAGE;
-
-    // Get the valid Page end Address
-    pageEnd = pageBase + PageSize;          // Set end of page
-
-    for (idx = pageEnd - 2; idx > pageBase; idx -= 4)
-    {
-        if ((*(__IO uint16_t*)idx) == Address)      // Find last value for address
-        {
-            count = (*(__IO uint16_t*)(idx - 2));   // Read last data
-            if (count == Data)
-                return EEPROM_OK;
-            if (count == 0xFFFF)
-            {
-                FlashStatus = FLASH_ProgramHalfWord(idx - 2, Data); // Set variable data
-                if (FlashStatus == FLASH_COMPLETE)
-                    return EEPROM_OK;
-            }
-            break;
-        }
+        FlashStatus = FLASH_ProgramHalfWord(FEE_PAGE_BASE_ADDRESS + FEE_ADDR_OFFSET(Address), (uint16_t)(0x00FF & DataByte));
     }
+    else {
 
-    // Check each active page address starting from begining
-    for (idx = pageBase + 4; idx < pageEnd; idx += 4)
-        if ((*(__IO uint32_t*)idx) == 0xFFFFFFFF)               // Verify if element
-        {                                                   //  contents are 0xFFFFFFFF
-            FlashStatus = FLASH_ProgramHalfWord(idx, Data); // Set variable data
-            if (FlashStatus != FLASH_COMPLETE)
-                return FlashStatus;
-            FlashStatus = FLASH_ProgramHalfWord(idx + 2, Address);  // Set variable virtual address
-            if (FlashStatus != FLASH_COMPLETE)
-                return FlashStatus;
-            return EEPROM_OK;
-        }
-
-    // Empty slot not found, need page transfer
-    // Calculate unique variables in page
-    count = EE_GetVariablesCount(pageBase, Address) + 1;
-    if (count >= (PageSize / 4 - 1))
-        return EEPROM_OUT_SIZE;
-
-    if (pageBase == PageBase1)
-        newPage = PageBase0;        // New page address where variable will be moved to
-    else
-        newPage = PageBase1;
-
-    // Set the new Page status to RECEIVE_DATA status
-    FlashStatus = FLASH_ProgramHalfWord(newPage, EEPROM_RECEIVE_DATA);
-    if (FlashStatus != FLASH_COMPLETE)
-        return FlashStatus;
-
-    // Write the variable passed as parameter in the new active page
-    FlashStatus = FLASH_ProgramHalfWord(newPage + 4, Data);
-    if (FlashStatus != FLASH_COMPLETE)
-        return FlashStatus;
-
-    FlashStatus = FLASH_ProgramHalfWord(newPage + 6, Address);
-    if (FlashStatus != FLASH_COMPLETE)
-        return FlashStatus;
-
-    return EE_PageTransfer(newPage, pageBase, Address);
-}
-
-/*EEPROMClass::EEPROMClass(void)
-{
-    PageBase0 = EEPROM_PAGE0_BASE;
-    PageBase1 = EEPROM_PAGE1_BASE;
-    PageSize = EEPROM_PAGE_SIZE;
-    Status = EEPROM_NOT_INIT;
-}*/
-/*
-uint16_t EEPROM_init(uint32_t pageBase0, uint32_t pageBase1, uint32_t pageSize)
-{
-    PageBase0 = pageBase0;
-    PageBase1 = pageBase1;
-    PageSize = pageSize;
-    return EEPROM_init();
-}*/
+        // Copy Page to a buffer
+        memcpy(DataBuf, (uint8_t*)FEE_PAGE_BASE_ADDRESS + (page * FEE_PAGE_SIZE), FEE_PAGE_SIZE); // !!! Calculate base address for the desired page
 
-uint16_t EEPROM_init(void)
-{
-    uint16_t status0 = 6, status1 = 6;
-    FLASH_Status FlashStatus;
+        // check if new data is differ to current data, return if not, proceed if yes
+        if (DataByte == *(__IO uint8_t*)(FEE_PAGE_BASE_ADDRESS + FEE_ADDR_OFFSET(Address))) {
+            return 0;
+        }
 
-    FLASH_Unlock();
-    Status = EEPROM_NO_VALID_PAGE;
+        // manipulate desired data byte in temp data array if new byte is differ to the current
+        DataBuf[FEE_ADDR_OFFSET(Address)] = DataByte;
 
-    status0 = (*(__IO uint16_t *)PageBase0);
-    status1 = (*(__IO uint16_t *)PageBase1);
+        //Erase Page
+        FlashStatus = FLASH_ErasePage(FEE_PAGE_BASE_ADDRESS + page);
 
-    switch (status0)
-    {
-/*
-        Page0               Page1
-        -----               -----
-    EEPROM_ERASED       EEPROM_VALID_PAGE           Page1 valid, Page0 erased
-                        EEPROM_RECEIVE_DATA         Page1 need set to valid, Page0 erased
-                        EEPROM_ERASED               make EE_Format
-                        any                         Error: EEPROM_NO_VALID_PAGE
-*/
-    case EEPROM_ERASED:
-        if (status1 == EEPROM_VALID_PAGE)           // Page0 erased, Page1 valid
-            Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
-        else if (status1 == EEPROM_RECEIVE_DATA)    // Page0 erased, Page1 receive
-        {
-            FlashStatus = FLASH_ProgramHalfWord(PageBase1, EEPROM_VALID_PAGE);
-            if (FlashStatus != FLASH_COMPLETE)
-                Status = FlashStatus;
-            else
-                Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
-        }
-        else if (status1 == EEPROM_ERASED)          // Both in erased state so format EEPROM
-            Status = EEPROM_format();
-        break;
-/*
-        Page0               Page1
-        -----               -----
-    EEPROM_RECEIVE_DATA EEPROM_VALID_PAGE           Transfer Page1 to Page0
-                        EEPROM_ERASED               Page0 need set to valid, Page1 erased
-                        any                         EEPROM_NO_VALID_PAGE
-*/
-    case EEPROM_RECEIVE_DATA:
-        if (status1 == EEPROM_VALID_PAGE)           // Page0 receive, Page1 valid
-            Status = EE_PageTransfer(PageBase0, PageBase1, 0xFFFF);
-        else if (status1 == EEPROM_ERASED)          // Page0 receive, Page1 erased
-        {
-            Status = EE_CheckErasePage(PageBase1, EEPROM_ERASED);
-            if (Status == EEPROM_OK)
-            {
-                FlashStatus = FLASH_ProgramHalfWord(PageBase0, EEPROM_VALID_PAGE);
-                if (FlashStatus != FLASH_COMPLETE)
-                    Status = FlashStatus;
-                else
-                    Status = EEPROM_OK;
+        // Write new data (whole page) to flash if data has beed changed
+        for(i = 0; i < (FEE_PAGE_SIZE / 2); i++) {
+            if ((__IO uint16_t)(0xFF00 | DataBuf[FEE_ADDR_OFFSET(i)]) != 0xFFFF) {
+                FlashStatus = FLASH_ProgramHalfWord((FEE_PAGE_BASE_ADDRESS + (page * FEE_PAGE_SIZE)) + (i * 2), (uint16_t)(0xFF00 | DataBuf[FEE_ADDR_OFFSET(i)]));
             }
         }
-        break;
-/*
-        Page0               Page1
-        -----               -----
-    EEPROM_VALID_PAGE   EEPROM_VALID_PAGE           Error: EEPROM_NO_VALID_PAGE
-                        EEPROM_RECEIVE_DATA         Transfer Page0 to Page1
-                        any                         Page0 valid, Page1 erased
-*/
-    case EEPROM_VALID_PAGE:
-        if (status1 == EEPROM_VALID_PAGE)           // Both pages valid
-            Status = EEPROM_NO_VALID_PAGE;
-        else if (status1 == EEPROM_RECEIVE_DATA)
-            Status = EE_PageTransfer(PageBase1, PageBase0, 0xFFFF);
-        else
-            Status = EE_CheckErasePage(PageBase1, EEPROM_ERASED);
-        break;
-/*
-        Page0               Page1
-        -----               -----
-        any             EEPROM_VALID_PAGE           Page1 valid, Page0 erased
-                        EEPROM_RECEIVE_DATA         Page1 valid, Page0 erased
-                        any                         EEPROM_NO_VALID_PAGE
-*/
-    default:
-        if (status1 == EEPROM_VALID_PAGE)
-            Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);   // Check/Erase Page0
-        else if (status1 == EEPROM_RECEIVE_DATA)
-        {
-            FlashStatus = FLASH_ProgramHalfWord(PageBase1, EEPROM_VALID_PAGE);
-            if (FlashStatus != FLASH_COMPLETE)
-                Status = FlashStatus;
-            else
-                Status = EE_CheckErasePage(PageBase0, EEPROM_ERASED);
-        }
-        break;
-    }
-    return Status;
-}
-
-/**
-  * @brief  Erases PAGE0 and PAGE1 and writes EEPROM_VALID_PAGE / 0 header to PAGE0
-  * @param  PAGE0 and PAGE1 base addresses
-  * @retval Status of the last operation (Flash write or erase) done during EEPROM formating
-  */
-uint16_t EEPROM_format(void)
-{
-    uint16_t status;
-    FLASH_Status FlashStatus;
 
-    FLASH_Unlock();
-
-    // Erase Page0
-    status = EE_CheckErasePage(PageBase0, EEPROM_VALID_PAGE);
-    if (status != EEPROM_OK)
-        return status;
-    if ((*(__IO uint16_t*)PageBase0) == EEPROM_ERASED)
-    {
-        // Set Page0 as valid page: Write VALID_PAGE at Page0 base address
-        FlashStatus = FLASH_ProgramHalfWord(PageBase0, EEPROM_VALID_PAGE);
-        if (FlashStatus != FLASH_COMPLETE)
-            return FlashStatus;
     }
-    // Erase Page1
-    return EE_CheckErasePage(PageBase1, EEPROM_ERASED);
-}
-
-/**
-  * @brief  Returns the erase counter for current page
-  * @param  Data: Global variable contains the read variable value
-  * @retval Success or error status:
-  *         - EEPROM_OK: if erases counter return.
-  *         - EEPROM_NO_VALID_PAGE: if no valid page was found.
-  */
-uint16_t EEPROM_erases(uint16_t *Erases)
-{
-    uint32_t pageBase;
-    if (Status != EEPROM_OK)
-        if (EEPROM_init() != EEPROM_OK)
-            return Status;
-
-    // Get active Page for read operation
-    pageBase = EE_FindValidPage();
-    if (pageBase == 0)
-        return  EEPROM_NO_VALID_PAGE;
-
-    *Erases = (*(__IO uint16_t*)pageBase+2);
-    return EEPROM_OK;
-}
-
-/**
-  * @brief  Returns the last stored variable data, if found,
-  *         which correspond to the passed virtual address
-  * @param  Address: Variable virtual address
-  * @retval Data for variable or EEPROM_DEFAULT_DATA, if any errors
-  */
-/*
-uint16_t EEPROM_read (uint16_t Address)
-{
-    uint16_t data;
-    EEPROM_read(Address, &data);
-    return data;
-}*/
-
-/**
-  * @brief  Returns the last stored variable data, if found,
-  *         which correspond to the passed virtual address
-  * @param  Address: Variable virtual address
-  * @param  Data: Pointer to data variable
-  * @retval Success or error status:
-  *           - EEPROM_OK: if variable was found
-  *           - EEPROM_BAD_ADDRESS: if the variable was not found
-  *           - EEPROM_NO_VALID_PAGE: if no valid page was found.
-  */
-uint16_t EEPROM_read(uint16_t Address, uint16_t *Data)
-{
-    uint32_t pageBase, pageEnd;
-
-    // Set default data (empty EEPROM)
-    *Data = EEPROM_DEFAULT_DATA;
-
-    if (Status == EEPROM_NOT_INIT)
-        if (EEPROM_init() != EEPROM_OK)
-            return Status;
-
-    // Get active Page for read operation
-    pageBase = EE_FindValidPage();
-    if (pageBase == 0)
-        return  EEPROM_NO_VALID_PAGE;
-
-    // Get the valid Page end Address
-    pageEnd = pageBase + ((uint32_t)(PageSize - 2));
-
-    // Check each active page address starting from end
-    for (pageBase += 6; pageEnd >= pageBase; pageEnd -= 4)
-        if ((*(__IO uint16_t*)pageEnd) == Address)      // Compare the read address with the virtual address
-        {
-            *Data = (*(__IO uint16_t*)(pageEnd - 2));       // Get content of Address-2 which is variable value
-            return EEPROM_OK;
-        }
-
-    // Return ReadStatus value: (0: variable exist, 1: variable doesn't exist)
-    return EEPROM_BAD_ADDRESS;
-}
-
-/**
-  * @brief  Writes/upadtes variable data in EEPROM.
-  * @param  VirtAddress: Variable virtual address
-  * @param  Data: 16 bit data to be written
-  * @retval Success or error status:
-  *         - FLASH_COMPLETE: on success
-  *         - EEPROM_BAD_ADDRESS: if address = 0xFFFF
-  *         - EEPROM_PAGE_FULL: if valid page is full
-  *         - EEPROM_NO_VALID_PAGE: if no valid page was found
-  *         - EEPROM_OUT_SIZE: if no empty EEPROM variables
-  *         - Flash error code: on write Flash error
-  */
-uint16_t EEPROM_write(uint16_t Address, uint16_t Data)
-{
-    if (Status == EEPROM_NOT_INIT)
-        if (EEPROM_init() != EEPROM_OK)
-            return Status;
-
-    if (Address == 0xFFFF)
-        return EEPROM_BAD_ADDRESS;
-
-    // Write the variable virtual address and value in the EEPROM
-    uint16_t status = EE_VerifyPageFullWriteVariable(Address, Data);
-    return status;
-}
-
-/**
-  * @brief  Writes/upadtes variable data in EEPROM.
-            The value is written only if differs from the one already saved at the same address.
-  * @param  VirtAddress: Variable virtual address
-  * @param  Data: 16 bit data to be written
-  * @retval Success or error status:
-  *         - EEPROM_SAME_VALUE: If new Data matches existing EEPROM Data
-  *         - FLASH_COMPLETE: on success
-  *         - EEPROM_BAD_ADDRESS: if address = 0xFFFF
-  *         - EEPROM_PAGE_FULL: if valid page is full
-  *         - EEPROM_NO_VALID_PAGE: if no valid page was found
-  *         - EEPROM_OUT_SIZE: if no empty EEPROM variables
-  *         - Flash error code: on write Flash error
-  */
-uint16_t EEPROM_update(uint16_t Address, uint16_t Data)
-{
-    uint16_t temp;
-    EEPROM_read(Address, &temp);
-    if (temp == Data)
-        return EEPROM_SAME_VALUE;
-    else
-        return EEPROM_write(Address, Data);
+    return FlashStatus;
 }
+/*****************************************************************************
+*  Read once data byte from a specified address.
+*******************************************************************************/
+uint8_t EEPROM_ReadDataByte (uint16_t Address) {
 
-/**
-  * @brief  Return number of variable
-  * @retval Number of variables
-  */
-uint16_t EEPROM_count(uint16_t *Count)
-{
-    if (Status == EEPROM_NOT_INIT)
-        if (EEPROM_init() != EEPROM_OK)
-            return Status;
-
-    // Get valid Page for write operation
-    uint32_t pageBase = EE_FindValidPage();
-    if (pageBase == 0)
-        return EEPROM_NO_VALID_PAGE;    // No valid page, return max. numbers
+    uint8_t DataByte = 0xFF;
 
-    *Count = EE_GetVariablesCount(pageBase, 0xFFFF);
-    return EEPROM_OK;
-}
+    // Get Byte from specified address
+    DataByte = (*(__IO uint8_t*)(FEE_PAGE_BASE_ADDRESS + FEE_ADDR_OFFSET(Address)));
 
-uint16_t EEPROM_maxcount(void)
-{
-    return ((PageSize / 4)-1);
+    return DataByte;
 }
 
-
+/*****************************************************************************
+*  Wrap library in AVR style functions.
+*******************************************************************************/
 uint8_t eeprom_read_byte (const uint8_t *Address)
 {
     const uint16_t p = (const uint32_t) Address;
-    uint16_t temp;
-    EEPROM_read(p, &temp);
-    return (uint8_t) temp;
+    return EEPROM_ReadDataByte(p);
 }
 
 void eeprom_write_byte (uint8_t *Address, uint8_t Value)
 {
     uint16_t p = (uint32_t) Address;
-    EEPROM_write(p, (uint16_t) Value);
+    EEPROM_WriteDataByte(p, Value);
 }
 
 void eeprom_update_byte (uint8_t *Address, uint8_t Value)
 {
     uint16_t p = (uint32_t) Address;
-    EEPROM_update(p, (uint16_t) Value);
+    EEPROM_WriteDataByte(p, Value);
 }
 
 uint16_t eeprom_read_word (const uint16_t *Address)
 {
     const uint16_t p = (const uint32_t) Address;
-    uint16_t temp;
-    EEPROM_read(p, &temp);
-    return temp;
+    return EEPROM_ReadDataByte(p) | (EEPROM_ReadDataByte(p+1) << 8);
 }
 
 void eeprom_write_word (uint16_t *Address, uint16_t Value)
 {
     uint16_t p = (uint32_t) Address;
-    EEPROM_write(p, Value);
+    EEPROM_WriteDataByte(p, (uint8_t) Value);
+    EEPROM_WriteDataByte(p + 1, (uint8_t) (Value >> 8));
 }
 
 void eeprom_update_word (uint16_t *Address, uint16_t Value)
 {
     uint16_t p = (uint32_t) Address;
-    EEPROM_update(p, Value);
+    EEPROM_WriteDataByte(p, (uint8_t) Value);
+    EEPROM_WriteDataByte(p + 1, (uint8_t) (Value >> 8));
 }
 
 uint32_t eeprom_read_dword (const uint32_t *Address)
 {
     const uint16_t p = (const uint32_t) Address;
-    uint16_t temp1, temp2;
-    EEPROM_read(p, &temp1);
-    EEPROM_read(p + 1, &temp2);
-    return temp1 | (temp2 << 16);
+    return EEPROM_ReadDataByte(p) | (EEPROM_ReadDataByte(p+1) << 8) 
+        | (EEPROM_ReadDataByte(p+2) << 16) | (EEPROM_ReadDataByte(p+3) << 24);
 }
 
 void eeprom_write_dword (uint32_t *Address, uint32_t Value)
 {
-    uint16_t temp = (uint16_t) Value;
-    uint16_t p = (uint32_t) Address;
-    EEPROM_write(p, temp);
-    temp = (uint16_t) (Value >> 16);
-    EEPROM_write(p + 1, temp);
+    uint16_t p = (const uint32_t) Address;
+    EEPROM_WriteDataByte(p, (uint8_t) Value);
+    EEPROM_WriteDataByte(p+1, (uint8_t) (Value >> 8));
+    EEPROM_WriteDataByte(p+2, (uint8_t) (Value >> 16));
+    EEPROM_WriteDataByte(p+3, (uint8_t) (Value >> 24));
 }
 
 void eeprom_update_dword (uint32_t *Address, uint32_t Value)
 {
-    uint16_t temp = (uint16_t) Value;
-    uint16_t p = (uint32_t) Address;
-    EEPROM_update(p, temp);
-    temp = (uint16_t) (Value >> 16);
-    EEPROM_update(p + 1, temp);
+    uint16_t p = (const uint32_t) Address;
+    EEPROM_WriteDataByte(p, (uint8_t) Value);
+    EEPROM_WriteDataByte(p+1, (uint8_t) (Value >> 8));
+    EEPROM_WriteDataByte(p+2, (uint8_t) (Value >> 16));
+    EEPROM_WriteDataByte(p+3, (uint8_t) (Value >> 24));
+}
+
+void eeprom_read_block(void *buf, const void *addr, uint32_t len) {
+    const uint8_t *p = (const uint8_t *)addr;
+    uint8_t *dest = (uint8_t *)buf;
+    while (len--) {
+        *dest++ = eeprom_read_byte(p++);
+    }
+}
+
+void eeprom_write_block(const void *buf, void *addr, uint32_t len) {
+    uint8_t *p = (uint8_t *)addr;
+    const uint8_t *src = (const uint8_t *)buf;
+    while (len--) {
+        eeprom_write_byte(p++, *src++);
+    }
+}
+
+void eeprom_update_block(const void *buf, void *addr, uint32_t len) {
+    uint8_t *p = (uint8_t *)addr;
+    const uint8_t *src = (const uint8_t *)buf;
+    while (len--) {
+        eeprom_write_byte(p++, *src++);
+    }
 }
diff --git a/tmk_core/common/chibios/eeprom_stm32.h b/tmk_core/common/chibios/eeprom_stm32.h
index 09229530c..892e417b7 100755
--- a/tmk_core/common/chibios/eeprom_stm32.h
+++ b/tmk_core/common/chibios/eeprom_stm32.h
@@ -10,15 +10,17 @@
  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  * DEALINGS IN THE SOFTWARE.
  *
- * This files are free to use from https://github.com/rogerclarkmelbourne/Arduino_STM32 and
- * https://github.com/leaflabs/libmaple
+ * This files are free to use from http://engsta.com/stm32-flash-memory-eeprom-emulator/ by
+ * Artur F.
  *
  * Modifications for QMK and STM32F303 by Yiancar
+ *
+ * This library assumes 8-bit data locations. To add a new MCU, please provide the flash
+ * page size and the total flash size in Kb. The number of available pages must be a multiple
+ * of 2. Only half of the pages account for the total EEPROM size.
+ * This library also assumes that the pages are not used by the firmware.
  */
 
-// This file must be modified if the MCU is not defined below.
-// This library also assumes that the pages are not used by the firmware.
-
 #ifndef __EEPROM_H
 #define __EEPROM_H
 
@@ -38,9 +40,11 @@
 
 #ifndef EEPROM_PAGE_SIZE
     #if defined (MCU_STM32F103RB)
-        #define EEPROM_PAGE_SIZE    (uint16_t)0x400  /* Page size = 1KByte */
+        #define FEE_PAGE_SIZE    (uint16_t)0x400 // Page size = 1KByte
+        #define FEE_DENSITY_PAGES          2     // How many pages are used
     #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE) || defined (MCU_STM32F103RD) || defined (MCU_STM32F303CC)
-        #define EEPROM_PAGE_SIZE    (uint16_t)0x800  /* Page size = 2KByte */
+        #define FEE_PAGE_SIZE    (uint16_t)0x800 // Page size = 2KByte
+        #define FEE_DENSITY_PAGES          4     // How many pages are used
     #else
         #error  "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)."
     #endif
@@ -48,48 +52,30 @@
 
 #ifndef EEPROM_START_ADDRESS
     #if defined (MCU_STM32F103RB)
-        #define EEPROM_START_ADDRESS    ((uint32_t)(0x8000000 + 128 * 1024 - 2 * EEPROM_PAGE_SIZE))
+        #define FEE_MCU_FLASH_SIZE  128     // Size in Kb
     #elif defined (MCU_STM32F103ZE) || defined (MCU_STM32F103RE)
-        #define EEPROM_START_ADDRESS    ((uint32_t)(0x8000000 + 512 * 1024 - 2 * EEPROM_PAGE_SIZE))
+        #define FEE_MCU_FLASH_SIZE  512     // Size in Kb
     #elif defined (MCU_STM32F103RD)
-        #define EEPROM_START_ADDRESS    ((uint32_t)(0x8000000 + 384 * 1024 - 2 * EEPROM_PAGE_SIZE))
+        #define FEE_MCU_FLASH_SIZE  384     // Size in Kb
     #elif defined (MCU_STM32F303CC)
-        #define EEPROM_START_ADDRESS    ((uint32_t)(0x8000000 + 256 * 1024 - 2 * EEPROM_PAGE_SIZE))
+        #define FEE_MCU_FLASH_SIZE  256     // Size in Kb
     #else
         #error  "No MCU type specified. Add something like -DMCU_STM32F103RB to your compiler arguments (probably in a Makefile)."
     #endif
 #endif
 
-/* Pages 0 and 1 base and end addresses */
-#define EEPROM_PAGE0_BASE       ((uint32_t)(EEPROM_START_ADDRESS + 0x000))
-#define EEPROM_PAGE1_BASE       ((uint32_t)(EEPROM_START_ADDRESS + EEPROM_PAGE_SIZE))
-
-/* Page status definitions */
-#define EEPROM_ERASED           ((uint16_t)0xFFFF)  /* PAGE is empty */
-#define EEPROM_RECEIVE_DATA     ((uint16_t)0xEEEE)  /* PAGE is marked to receive data */
-#define EEPROM_VALID_PAGE       ((uint16_t)0x0000)  /* PAGE containing valid data */
-
-/* Page full define */
-enum uint16_t
-    {
-    EEPROM_OK               = ((uint16_t)0x0000),
-    EEPROM_OUT_SIZE         = ((uint16_t)0x0081),
-    EEPROM_BAD_ADDRESS      = ((uint16_t)0x0082),
-    EEPROM_BAD_FLASH        = ((uint16_t)0x0083),
-    EEPROM_NOT_INIT         = ((uint16_t)0x0084),
-    EEPROM_SAME_VALUE       = ((uint16_t)0x0085),
-    EEPROM_NO_VALID_PAGE    = ((uint16_t)0x00AB)
-    };
-
-#define EEPROM_DEFAULT_DATA     0xFFFF
+// DONT CHANGE
+// Choose location for the first EEPROM Page address on the top of flash
+#define FEE_PAGE_BASE_ADDRESS ((uint32_t)(0x8000000 + FEE_MCU_FLASH_SIZE * 1024 - FEE_DENSITY_PAGES * FEE_PAGE_SIZE))
+#define FEE_DENSITY_BYTES       ((FEE_PAGE_SIZE / 2) * FEE_DENSITY_PAGES - 1)
+#define FEE_LAST_PAGE_ADDRESS   (FEE_PAGE_BASE_ADDRESS + (FEE_PAGE_SIZE * FEE_DENSITY_PAGES))
+#define FEE_EMPTY_WORD          ((uint16_t)0xFFFF)
+#define FEE_ADDR_OFFSET(Address)(Address * 2) // 1Byte per Word will be saved to preserve Flash
 
-    uint16_t EEPROM_init(void);
-    uint16_t EEPROM_format(void);
-    uint16_t EEPROM_erases(uint16_t *);
-    uint16_t EEPROM_read (uint16_t address, uint16_t *data);
-    uint16_t EEPROM_write(uint16_t address, uint16_t data);
-    uint16_t EEPROM_update(uint16_t address, uint16_t data);
-    uint16_t EEPROM_count(uint16_t *);
-    uint16_t EEPROM_maxcount(void);
+// Use this function to initialize the functionality
+uint16_t EEPROM_Init(void);
+void EEPROM_Erase (void);
+uint16_t EEPROM_WriteDataByte (uint16_t Address, uint8_t DataByte);
+uint8_t EEPROM_ReadDataByte (uint16_t Address);
 
 #endif  /* __EEPROM_H */
diff --git a/tmk_core/common/chibios/flash_stm32.c b/tmk_core/common/chibios/flash_stm32.c
index 273593484..164654a15 100755
--- a/tmk_core/common/chibios/flash_stm32.c
+++ b/tmk_core/common/chibios/flash_stm32.c
@@ -186,3 +186,18 @@ void FLASH_Lock(void)
     /* Set the Lock Bit to lock the FPEC and the FCR */
     FLASH->CR |= FLASH_CR_LOCK;
 }
+
+/**
+  * @brief  Clears the FLASH's pending flags.
+  * @param  FLASH_FLAG: specifies the FLASH flags to clear.
+  *   This parameter can be any combination of the following values:
+  *     @arg FLASH_FLAG_PGERR: FLASH Programming error flag flag
+  *     @arg FLASH_FLAG_WRPERR: FLASH Write protected error flag
+  *     @arg FLASH_FLAG_EOP: FLASH End of Programming flag                
+  * @retval None
+  */
+void FLASH_ClearFlag(uint32_t FLASH_FLAG)
+{
+    /* Clear the flags */
+    FLASH->SR = FLASH_FLAG;
+}
diff --git a/tmk_core/common/chibios/flash_stm32.h b/tmk_core/common/chibios/flash_stm32.h
index cc065cbca..3c99cc566 100755
--- a/tmk_core/common/chibios/flash_stm32.h
+++ b/tmk_core/common/chibios/flash_stm32.h
@@ -45,6 +45,7 @@ FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data);
 
 void FLASH_Unlock(void);
 void FLASH_Lock(void);
+void FLASH_ClearFlag(uint32_t FLASH_FLAG);
 
 #ifdef __cplusplus
 }
diff --git a/tmk_core/common/eeconfig.c b/tmk_core/common/eeconfig.c
index d8bab7d2e..59b2bffbc 100644
--- a/tmk_core/common/eeconfig.c
+++ b/tmk_core/common/eeconfig.c
@@ -33,7 +33,7 @@ void eeconfig_init_kb(void) {
  */
 void eeconfig_init_quantum(void) {
 #ifdef STM32_EEPROM_ENABLE
-    EEPROM_format();
+    EEPROM_Erase();
 #endif
   eeprom_update_word(EECONFIG_MAGIC,          EECONFIG_MAGIC_NUMBER);
   eeprom_update_byte(EECONFIG_DEBUG,          0);
@@ -74,7 +74,7 @@ void eeconfig_enable(void)
 void eeconfig_disable(void)
 {
 #ifdef STM32_EEPROM_ENABLE
-    EEPROM_format();
+    EEPROM_Erase();
 #endif
     eeprom_update_word(EECONFIG_MAGIC, EECONFIG_MAGIC_NUMBER_OFF);
 }
diff --git a/tmk_core/common/eeconfig.h b/tmk_core/common/eeconfig.h
index 8d4e1d4d0..eedd67602 100644
--- a/tmk_core/common/eeconfig.h
+++ b/tmk_core/common/eeconfig.h
@@ -25,8 +25,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define EECONFIG_MAGIC_NUMBER                       (uint16_t)0xFEED
 #define EECONFIG_MAGIC_NUMBER_OFF                   (uint16_t)0xFFFF
 
-/* eeprom parameteter address */
-#if !defined(STM32_EEPROM_ENABLE)
+/* EEPROM parameter address */
 #define EECONFIG_MAGIC                              (uint16_t *)0
 #define EECONFIG_DEBUG                               (uint8_t *)2
 #define EECONFIG_DEFAULT_LAYER                       (uint8_t *)3
@@ -42,24 +41,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define EECONFIG_KEYBOARD                          (uint32_t *)15
 #define EECONFIG_USER                              (uint32_t *)19
 
-#else
-/* STM32F3 uses 16byte block. Reconfigure memory map */
-#define EECONFIG_MAGIC                              (uint16_t *)0
-#define EECONFIG_DEBUG                               (uint8_t *)1
-#define EECONFIG_DEFAULT_LAYER                       (uint8_t *)2
-#define EECONFIG_KEYMAP                              (uint8_t *)3
-#define EECONFIG_MOUSEKEY_ACCEL                      (uint8_t *)4
-#define EECONFIG_BACKLIGHT                           (uint8_t *)5
-#define EECONFIG_AUDIO                               (uint8_t *)6
-#define EECONFIG_RGBLIGHT                           (uint32_t *)7
-#define EECONFIG_UNICODEMODE                         (uint8_t *)9
-#define EECONFIG_STENOMODE                          (uint8_t *)10
-// EEHANDS for two handed boards
-#define EECONFIG_HANDEDNESS                     		(uint8_t *)11
-#define EECONFIG_KEYBOARD                          (uint32_t *)12
-#define EECONFIG_USER                              (uint32_t *)14
-#endif
-
 /* debug bit */
 #define EECONFIG_DEBUG_ENABLE                       (1<<0)
 #define EECONFIG_DEBUG_MATRIX                       (1<<1)
diff --git a/tmk_core/common/eeprom.h b/tmk_core/common/eeprom.h
index 3696d0df3..5ae0f6eeb 100644
--- a/tmk_core/common/eeprom.h
+++ b/tmk_core/common/eeprom.h
@@ -20,5 +20,4 @@ void 	eeprom_update_dword (uint32_t *__p, uint32_t __value);
 void 	eeprom_update_block (const void *__src, void *__dst, uint32_t __n);
 #endif
 
-
 #endif /* TMK_CORE_COMMON_EEPROM_H_ */
diff --git a/tmk_core/common/host.h b/tmk_core/common/host.h
index aeabba710..3d172eed6 100644
--- a/tmk_core/common/host.h
+++ b/tmk_core/common/host.h
@@ -15,14 +15,18 @@ You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef HOST_H
-#define HOST_H
+#pragma once
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "report.h"
 #include "host_driver.h"
 
+#define IS_LED_ON(leds, led_name)   ( (leds) & (1 << (led_name)))
+#define IS_LED_OFF(leds, led_name)  (~(leds) & (1 << (led_name)))
+
+#define IS_HOST_LED_ON(led_name)    IS_LED_ON(host_keyboard_leds(), led_name)
+#define IS_HOST_LED_OFF(led_name)   IS_LED_OFF(host_keyboard_leds(), led_name)
 
 #ifdef __cplusplus
 extern "C" {
@@ -31,7 +35,6 @@ extern "C" {
 extern uint8_t keyboard_idle;
 extern uint8_t keyboard_protocol;
 
-
 /* host driver */
 void host_set_driver(host_driver_t *driver);
 host_driver_t *host_get_driver(void);
@@ -49,5 +52,3 @@ uint16_t host_last_consumer_report(void);
 #ifdef __cplusplus
 }
 #endif
-
-#endif
diff --git a/tmk_core/common/keyboard.h b/tmk_core/common/keyboard.h
index 71e594a89..ea2f336e9 100644
--- a/tmk_core/common/keyboard.h
+++ b/tmk_core/common/keyboard.h
@@ -67,6 +67,8 @@ void keyboard_init(void);
 void keyboard_task(void);
 /* it runs when host LED status is updated */
 void keyboard_set_leds(uint8_t leds);
+/* it runs whenever code has to behave differently on a slave */
+bool is_keyboard_master(void);
 
 #ifdef __cplusplus
 }
diff --git a/tmk_core/common/keycode.h b/tmk_core/common/keycode.h
index ac3edbd21..d5904276e 100644
--- a/tmk_core/common/keycode.h
+++ b/tmk_core/common/keycode.h
@@ -46,6 +46,22 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define MOD_BIT(code)            (1 << MOD_INDEX(code))
 #define MOD_INDEX(code)          ((code) & 0x07)
 
+#define MOD_MASK_CTRL            (MOD_BIT(KC_LCTRL)  | MOD_BIT(KC_RCTRL))
+#define MOD_MASK_SHIFT           (MOD_BIT(KC_LSHIFT) | MOD_BIT(KC_RSHIFT))
+#define MOD_MASK_ALT             (MOD_BIT(KC_LALT)   | MOD_BIT(KC_RALT))
+#define MOD_MASK_GUI             (MOD_BIT(KC_LGUI)   | MOD_BIT(KC_RGUI))
+#define MOD_MASK_CS              (MOD_MASK_CTRL  | MOD_MASK_SHIFT)
+#define MOD_MASK_CA              (MOD_MASK_CTRL  | MOD_MASK_ALT)
+#define MOD_MASK_CG              (MOD_MASK_CTRL  | MOD_MASK_GUI)
+#define MOD_MASK_SA              (MOD_MASK_SHIFT | MOD_MASK_ALT)
+#define MOD_MASK_SG              (MOD_MASK_SHIFT | MOD_MASK_GUI)
+#define MOD_MASK_AG              (MOD_MASK_ALT   | MOD_MASK_GUI)
+#define MOD_MASK_CSA             (MOD_MASK_CTRL  | MOD_MASK_SHIFT | MOD_MASK_ALT)
+#define MOD_MASK_CSG             (MOD_MASK_CTRL  | MOD_MASK_SHIFT | MOD_MASK_GUI)
+#define MOD_MASK_CAG             (MOD_MASK_CTRL  | MOD_MASK_ALT   | MOD_MASK_GUI)
+#define MOD_MASK_SAG             (MOD_MASK_SHIFT | MOD_MASK_ALT   | MOD_MASK_GUI)
+#define MOD_MASK_CSAG            (MOD_MASK_CTRL  | MOD_MASK_SHIFT | MOD_MASK_ALT | MOD_MASK_GUI)
+
 #define FN_BIT(code)             (1 << FN_INDEX(code))
 #define FN_INDEX(code)           ((code) - KC_FN0)
 #define FN_MIN                   KC_FN0
@@ -174,6 +190,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define KC_BRIU KC_BRIGHTNESS_UP
 #define KC_BRID KC_BRIGHTNESS_DOWN
 
+/* System Specific */
+#define KC_BRMU KC_PAUSE
+#define KC_BRMD KC_SCROLLLOCK
+
 /* Mouse Keys */
 #define KC_MS_U KC_MS_UP
 #define KC_MS_D KC_MS_DOWN
diff --git a/tmk_core/common/progmem.h b/tmk_core/common/progmem.h
index dcc9efb3c..de9313839 100644
--- a/tmk_core/common/progmem.h
+++ b/tmk_core/common/progmem.h
@@ -5,9 +5,9 @@
 #   include <avr/pgmspace.h>
 #else
 #   define PROGMEM
-#   define pgm_read_byte(p)     *((unsigned char*)p)
-#   define pgm_read_word(p)     *((uint16_t*)p)
-#   define pgm_read_dword(p)    *((uint32_t*)p)
+#   define pgm_read_byte(p)     *((unsigned char*)(p))
+#   define pgm_read_word(p)     *((uint16_t*)(p))
+#   define pgm_read_dword(p)    *((uint32_t*)(p))
 #endif
 
 #endif
diff --git a/tmk_core/common/report.h b/tmk_core/common/report.h
index eb9afb727..e7c31bd37 100644
--- a/tmk_core/common/report.h
+++ b/tmk_core/common/report.h
@@ -48,8 +48,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define TRANSPORT_STOP          0x00B7
 #define TRANSPORT_STOP_EJECT    0x00CC
 #define TRANSPORT_PLAY_PAUSE    0x00CD
-#define BRIGHTNESSUP            0x006F
-#define BRIGHTNESSDOWN          0x0070
+#define BRIGHTNESS_UP           0x006F
+#define BRIGHTNESS_DOWN         0x0070
 /* application launch */
 #define AL_CC_CONFIG            0x0183
 #define AL_EMAIL                0x018A
@@ -192,8 +192,8 @@ typedef struct {
     (key == KC_WWW_FORWARD      ?  AC_FORWARD : \
     (key == KC_WWW_STOP         ?  AC_STOP : \
     (key == KC_WWW_REFRESH      ?  AC_REFRESH : \
-    (key == KC_BRIGHTNESS_UP    ?  BRIGHTNESSUP : \
-    (key == KC_BRIGHTNESS_DOWN  ?  BRIGHTNESSDOWN : \
+    (key == KC_BRIGHTNESS_UP    ?  BRIGHTNESS_UP : \
+    (key == KC_BRIGHTNESS_DOWN  ?  BRIGHTNESS_DOWN : \
     (key == KC_WWW_FAVORITES    ?  AC_BOOKMARKS : 0)))))))))))))))))))))))
 
 uint8_t has_anykey(report_keyboard_t* keyboard_report);
diff --git a/tmk_core/common/wait.h b/tmk_core/common/wait.h
index a7cded942..a77840bce 100644
--- a/tmk_core/common/wait.h
+++ b/tmk_core/common/wait.h
@@ -15,6 +15,10 @@ extern "C" {
 #   include "ch.h"
 #   define wait_ms(ms) chThdSleepMilliseconds(ms)
 #   define wait_us(us) chThdSleepMicroseconds(us)
+#elif defined PROTOCOL_ARM_ATSAM
+#   include "clks.h"
+#   define wait_ms(ms) CLK_delay_ms(ms)
+#   define wait_us(us) CLK_delay_us(us)
 #elif defined(__arm__)
 #   include "wait_api.h"
 #else  // Unit tests
diff --git a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
index 2ba099174..928af8c7e 100644
--- a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
+++ b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
@@ -21,8 +21,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "samd51j18a.h"
 #include "md_bootloader.h"
 
+#include "timer.h"
 #include "d51_util.h"
 #include "clks.h"
+#include "wait.h"
 #include "adc.h"
 #include "i2c_master.h"
 #include "spi.h"
diff --git a/tmk_core/protocol/arm_atsam/clks.c b/tmk_core/protocol/arm_atsam/clks.c
index 8768d0a99..1ff318e59 100644
--- a/tmk_core/protocol/arm_atsam/clks.c
+++ b/tmk_core/protocol/arm_atsam/clks.c
@@ -21,8 +21,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 volatile clk_t system_clks;
 volatile uint64_t ms_clk;
-
-volatile uint8_t us_delay_done;
+uint32_t usec_delay_mult;
+#define USEC_DELAY_LOOP_CYCLES 3 //Sum of instruction cycles in us delay loop
 
 const uint32_t sercom_apbbase[] = {(uint32_t)SERCOM0,(uint32_t)SERCOM1,(uint32_t)SERCOM2,(uint32_t)SERCOM3,(uint32_t)SERCOM4,(uint32_t)SERCOM5};
 const uint8_t sercom_pchan[] = {7, 8, 23, 24, 34, 35};
@@ -73,6 +73,9 @@ void CLK_oscctrl_init(void)
 
     system_clks.freq_gclk[0] = system_clks.freq_dpll[0];
 
+    usec_delay_mult = system_clks.freq_gclk[0] / (USEC_DELAY_LOOP_CYCLES * 1000000);
+    if (usec_delay_mult < 1) usec_delay_mult = 1; //Never allow a multiplier of zero
+
     DBGC(DC_CLK_OSC_INIT_COMPLETE);
 }
 
@@ -158,23 +161,11 @@ void TC4_Handler()
     }
 }
 
-void TC5_Handler()
-{
-    if (TC5->COUNT16.INTFLAG.bit.MC0)
-    {
-        TC5->COUNT16.INTFLAG.reg = TC_INTENCLR_MC0;
-        us_delay_done = 1;
-        TC5->COUNT16.CTRLA.bit.ENABLE = 0;
-        while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-    }
-}
-
 uint32_t CLK_enable_timebase(void)
 {
     Gclk *pgclk = GCLK;
     Mclk *pmclk = MCLK;
     Tc *ptc4 = TC4;
-    Tc *ptc5 = TC5;
     Tc *ptc0 = TC0;
     Evsys *pevsys = EVSYS;
 
@@ -189,11 +180,6 @@ uint32_t CLK_enable_timebase(void)
     pgclk->PCHCTRL[TC4_GCLK_ID].bit.GEN = GEN_TC45;
     pgclk->PCHCTRL[TC4_GCLK_ID].bit.CHEN = 1;
 
-    //unmask TC5 sourcegclk2 to TC5
-    pmclk->APBCMASK.bit.TC5_ = 1;
-    pgclk->PCHCTRL[TC5_GCLK_ID].bit.GEN = GEN_TC45;
-    pgclk->PCHCTRL[TC5_GCLK_ID].bit.CHEN = 1;
-
     //configure TC4
     DBGC(DC_CLK_ENABLE_TIMEBASE_TC4_BEGIN);
     ptc4->COUNT16.CTRLA.bit.ENABLE = 0;
@@ -220,30 +206,6 @@ uint32_t CLK_enable_timebase(void)
 
     DBGC(DC_CLK_ENABLE_TIMEBASE_TC4_COMPLETE);
 
-    //configure TC5
-    DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_BEGIN);
-    ptc5->COUNT16.CTRLA.bit.ENABLE = 0;
-    while (ptc5->COUNT16.SYNCBUSY.bit.ENABLE) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_DISABLE); }
-    ptc5->COUNT16.CTRLA.bit.SWRST = 1;
-    while (ptc5->COUNT16.SYNCBUSY.bit.SWRST) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_SWRST_1); }
-    while (ptc5->COUNT16.CTRLA.bit.SWRST) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_SWRST_2); }
-
-    //CTRLA defaults
-    //CTRLB as default, counting up
-    ptc5->COUNT16.CTRLBCLR.reg = 5;
-    while (ptc5->COUNT16.SYNCBUSY.bit.CTRLB) { DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_SYNC_CLTRB); }
-    //ptc5->COUNT16.DBGCTRL.bit.DBGRUN = 1;
-
-    //wave mode
-    ptc5->COUNT16.WAVE.bit.WAVEGEN = 1; //MFRQ match frequency mode, toggle each CC match
-    //generate event for next stage
-    ptc5->COUNT16.EVCTRL.bit.MCEO0 = 1;
-
-    NVIC_EnableIRQ(TC5_IRQn);
-    ptc5->COUNT16.INTENSET.bit.MC0 = 1;
-
-    DBGC(DC_CLK_ENABLE_TIMEBASE_TC5_COMPLETE);
-
     //unmask TC0,1, sourcegclk2 to TC0,1
     pmclk->APBAMASK.bit.TC0_ = 1;
     pgclk->PCHCTRL[TC0_GCLK_ID].bit.GEN = GEN_TC45;
@@ -289,37 +251,27 @@ uint32_t CLK_enable_timebase(void)
     return 0;
 }
 
-uint32_t CLK_get_ms(void)
+void CLK_delay_us(uint32_t usec)
 {
-    return ms_clk;
-}
-
-void CLK_delay_us(uint16_t usec)
-{
-    us_delay_done = 0;
-
-    if (TC5->COUNT16.CTRLA.bit.ENABLE)
-    {
-        TC5->COUNT16.CTRLA.bit.ENABLE = 0;
-        while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-    }
-
-    if (usec < 10) usec = 0;
-    else usec -= 10;
-
-    TC5->COUNT16.CC[0].reg = usec;
-    while (TC5->COUNT16.SYNCBUSY.bit.CC0) {}
-
-    TC5->COUNT16.CTRLA.bit.ENABLE = 1;
-    while (TC5->COUNT16.SYNCBUSY.bit.ENABLE) {}
-
-    while (!us_delay_done) {}
+    asm (
+        "CBZ R0, return\n\t"        //If usec == 0, branch to return label
+    );
+    asm (
+        "MULS R0, %0\n\t"           //Multiply R0(usec) by usec_delay_mult and store in R0
+        ".balign 16\n\t"            //Ensure loop is aligned for fastest performance
+        "loop: SUBS R0, #1\n\t"     //Subtract 1 from R0 and update flags (1 cycle)
+        "BNE loop\n\t"              //Branch if non-zero to loop label (2 cycles)  NOTE: USEC_DELAY_LOOP_CYCLES is the sum of loop cycles
+        "return:\n\t"               //Return label
+        : //No output registers
+        : "r" (usec_delay_mult)     //For %0
+    );
+    //Note: BX LR generated
 }
 
 void CLK_delay_ms(uint64_t msec)
 {
-    msec += CLK_get_ms();
-    while (msec > CLK_get_ms()) {}
+    msec += timer_read64();
+    while (msec > timer_read64()) {}
 }
 
 void clk_enable_sercom_apbmask(int sercomn)
diff --git a/tmk_core/protocol/arm_atsam/clks.h b/tmk_core/protocol/arm_atsam/clks.h
index 96819bfdd..1b01a1764 100644
--- a/tmk_core/protocol/arm_atsam/clks.h
+++ b/tmk_core/protocol/arm_atsam/clks.h
@@ -77,9 +77,8 @@ void CLK_oscctrl_init(void);
 void CLK_reset_time(void);
 uint32_t CLK_set_gclk_freq(uint8_t gclkn, uint32_t freq);
 uint32_t CLK_enable_timebase(void);
-uint32_t CLK_get_ms(void);
-uint64_t CLK_get_us(void);
-void CLK_delay_us(uint16_t usec);
+uint64_t timer_read64(void);
+void CLK_delay_us(uint32_t usec);
 void CLK_delay_ms(uint64_t msec);
 
 uint32_t CLK_set_spi_freq(uint8_t sercomn, uint32_t freq);
diff --git a/tmk_core/protocol/arm_atsam/i2c_master.c b/tmk_core/protocol/arm_atsam/i2c_master.c
index f608a79cc..d91a851f3 100644
--- a/tmk_core/protocol/arm_atsam/i2c_master.c
+++ b/tmk_core/protocol/arm_atsam/i2c_master.c
@@ -265,12 +265,12 @@ uint8_t I2C3733_Init_Control(void)
     //USB state machine will enable driver when communication is ready
     I2C3733_Control_Set(0);
 
-    CLK_delay_ms(1);
+    wait_ms(1);
 
     sr_exp_data.bit.IRST = 0;
     SR_EXP_WriteData();
 
-    CLK_delay_ms(1);
+    wait_ms(1);
 
     DBGC(DC_I2C3733_INIT_CONTROL_COMPLETE);
 
diff --git a/tmk_core/protocol/arm_atsam/led_matrix.c b/tmk_core/protocol/arm_atsam/led_matrix.c
index e914fc80e..04d05af6d 100644
--- a/tmk_core/protocol/arm_atsam/led_matrix.c
+++ b/tmk_core/protocol/arm_atsam/led_matrix.c
@@ -18,6 +18,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "arm_atsam_protocol.h"
 #include "tmk_core/common/led.h"
 #include <string.h>
+#include <math.h>
 
 void SERCOM1_0_Handler( void )
 {
@@ -217,6 +218,7 @@ void disp_calc_extents(void)
 
     disp.width = disp.right - disp.left;
     disp.height = disp.top - disp.bottom;
+    disp.max_distance = sqrtf(powf(disp.width, 2) + powf(disp.height, 2));
 }
 
 void disp_pixel_setup(void)
@@ -249,6 +251,7 @@ uint8_t led_animation_breathing;
 uint8_t led_animation_breathe_cur;
 uint8_t breathe_step;
 uint8_t breathe_dir;
+uint8_t led_animation_circular;
 uint64_t led_next_run;
 
 uint8_t led_animation_id;
@@ -265,6 +268,7 @@ void led_matrix_run(void)
     float go;
     float bo;
     float po;
+
     uint8_t led_this_run = 0;
     led_setup_t *f = (led_setup_t*)led_setups[led_animation_id];
 
@@ -327,13 +331,18 @@ void led_matrix_run(void)
             for (fcur = 0; fcur < fmax; fcur++)
             {
 
-                if (led_animation_orientation)
-                {
-                  po = led_cur->py;
+                if (led_animation_circular) {
+                    po = sqrtf((powf(fabsf((disp.width / 2) - (led_cur->x - disp.left)), 2) + powf(fabsf((disp.height / 2) - (led_cur->y - disp.bottom)), 2))) / disp.max_distance * 100;
                 }
-                else
-                {
-                  po = led_cur->px;
+                else {
+                    if (led_animation_orientation)
+                    {
+                        po = led_cur->py;
+                    }
+                    else
+                    {
+                        po = led_cur->px;
+                    }
                 }
 
                 float pomod;
@@ -466,6 +475,7 @@ uint8_t led_matrix_init(void)
     led_animation_breathe_cur = BREATHE_MIN_STEP;
     breathe_step = 1;
     breathe_dir = 1;
+    led_animation_circular = 0;
 
     gcr_min_counter = 0;
     v_5v_cat_hit = 0;
@@ -494,11 +504,11 @@ void led_matrix_task(void)
     if (led_enabled)
     {
         //If an update may run and frame processing has completed
-        if (CLK_get_ms() >= led_next_run && led_cur == lede)
+        if (timer_read64() >= led_next_run && led_cur == lede)
         {
             uint8_t drvid;
 
-            led_next_run = CLK_get_ms() + LED_UPDATE_RATE;  //Set next frame update time
+            led_next_run = timer_read64() + LED_UPDATE_RATE;  //Set next frame update time
 
             //NOTE: GCR does not need to be timed with LED processing, but there is really no harm
             if (gcr_actual != gcr_actual_last)
diff --git a/tmk_core/protocol/arm_atsam/led_matrix.h b/tmk_core/protocol/arm_atsam/led_matrix.h
index cedea8a85..4513234e7 100644
--- a/tmk_core/protocol/arm_atsam/led_matrix.h
+++ b/tmk_core/protocol/arm_atsam/led_matrix.h
@@ -83,6 +83,7 @@ typedef struct led_disp_s {
     float bottom;
     float width;
     float height;
+    float max_distance;
 } led_disp_t;
 
 uint8_t led_matrix_init(void);
@@ -129,6 +130,7 @@ extern uint8_t led_animation_orientation;
 extern uint8_t led_animation_breathing;
 extern uint8_t led_animation_breathe_cur;
 extern uint8_t breathe_dir;
+extern uint8_t led_animation_circular;
 extern const uint8_t led_setups_count;
 
 extern void *led_setups[];
diff --git a/tmk_core/protocol/arm_atsam/main_arm_atsam.c b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
index 2bda7d7c7..eaad66e9f 100644
--- a/tmk_core/protocol/arm_atsam/main_arm_atsam.c
+++ b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
@@ -159,7 +159,7 @@ void send_consumer(uint16_t data)
 
 void main_subtask_usb_state(void)
 {
-    static uint32_t fsmstate_on_delay = 0;                          //Delay timer to be sure USB is actually operating before bringing up hardware
+    static uint64_t fsmstate_on_delay = 0;                          //Delay timer to be sure USB is actually operating before bringing up hardware
     uint8_t fsmstate_now = USB->DEVICE.FSMSTATUS.reg;               //Current state from hardware register
 
     if (fsmstate_now == USB_FSMSTATUS_FSMSTATE_SUSPEND_Val)         //If USB SUSPENDED
@@ -188,9 +188,9 @@ void main_subtask_usb_state(void)
         {
             if (fsmstate_on_delay == 0)                             //If ON delay timer is cleared
             {
-                fsmstate_on_delay = CLK_get_ms() + 250;             //Set ON delay timer
+                fsmstate_on_delay = timer_read64() + 250;             //Set ON delay timer
             }
-            else if (CLK_get_ms() > fsmstate_on_delay)              //Else if ON delay timer is active and timed out
+            else if (timer_read64() > fsmstate_on_delay)              //Else if ON delay timer is active and timed out
             {
                 suspend_wakeup_init();                              //Run wakeup routine
                 g_usb_state = fsmstate_now;                         //Save current USB state
@@ -214,9 +214,9 @@ void main_subtask_power_check(void)
 {
     static uint64_t next_5v_checkup = 0;
 
-    if (CLK_get_ms() > next_5v_checkup)
+    if (timer_read64() > next_5v_checkup)
     {
-        next_5v_checkup = CLK_get_ms() + 5;
+        next_5v_checkup = timer_read64() + 5;
 
         v_5v = adc_get(ADC_5V);
         v_5v_avg = 0.9 * v_5v_avg + 0.1 * v_5v;
@@ -229,9 +229,9 @@ void main_subtask_usb_extra_device(void)
 {
     static uint64_t next_usb_checkup = 0;
 
-    if (CLK_get_ms() > next_usb_checkup)
+    if (timer_read64() > next_usb_checkup)
     {
-        next_usb_checkup = CLK_get_ms() + 10;
+        next_usb_checkup = timer_read64() + 10;
 
         USB_HandleExtraDevice();
     }
@@ -325,9 +325,9 @@ int main(void)
         keyboard_task();
 
 #ifdef CONSOLE_ENABLE
-        if (CLK_get_ms() > next_print)
+        if (timer_read64() > next_print)
         {
-            next_print = CLK_get_ms() + 250;
+            next_print = timer_read64() + 250;
             //Add any debug information here that you want to see very often
             //dprintf("5v=%u 5vu=%u dlow=%u dhi=%u gca=%u gcd=%u\r\n", v_5v, v_5v_avg, v_5v_avg - V5_LOW, v_5v_avg - V5_HIGH, gcr_actual, gcr_desired);
         }
diff --git a/tmk_core/protocol/arm_atsam/usb/compiler.h b/tmk_core/protocol/arm_atsam/usb/compiler.h
index 7d8350896..b2ccfd73e 100644
--- a/tmk_core/protocol/arm_atsam/usb/compiler.h
+++ b/tmk_core/protocol/arm_atsam/usb/compiler.h
@@ -134,13 +134,15 @@
  * heuristics and inline the function no matter how big it thinks it
  * becomes.
  */
+#if !defined(__always_inline)
 #if defined(__CC_ARM)
 #  define __always_inline             __forceinline
-#elif (defined __GNUC__ && __GNUC__ <= 6)
+#elif (defined __GNUC__)
 #  define __always_inline             __attribute__((__always_inline__))
 #elif (defined __ICCARM__)
 #  define __always_inline             _Pragma("inline=forced")
 #endif
+#endif
 
 /**
  * \def __no_inline
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
index 5f3c289e8..ffe3526db 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
+++ b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
@@ -1227,9 +1227,9 @@ uint32_t cdc_tx_send_time_next;
 
 void CDC_send(void)
 {
-    while (CLK_get_ms() < cdc_tx_send_time_next);
+    while (timer_read64() < cdc_tx_send_time_next);
     udi_cdc_tx_send(0);
-    cdc_tx_send_time_next = CLK_get_ms() + CDC_SEND_INTERVAL;
+    cdc_tx_send_time_next = timer_read64() + CDC_SEND_INTERVAL;
 }
 
 uint32_t CDC_print(char *printbuf)
@@ -1238,7 +1238,7 @@ uint32_t CDC_print(char *printbuf)
     char *buf = printbuf;
     char c;
 
-    if (CLK_get_ms() < 5000) return 0;
+    if (timer_read64() < 5000) return 0;
 
     while ((c = *buf++) != 0 && !(count >= MAX_PRINT))
     {
@@ -1339,7 +1339,7 @@ void CDC_init(void)
     inbuf.count = 0;
     inbuf.lastcount = 0;
     printbuf[0] = 0;
-    cdc_tx_send_time_next = CLK_get_ms() + CDC_SEND_INTERVAL;
+    cdc_tx_send_time_next = timer_read64() + CDC_SEND_INTERVAL;
 }
 
 #else //CDC line 62
diff --git a/tmk_core/protocol/arm_atsam/usb/usb2422.c b/tmk_core/protocol/arm_atsam/usb/usb2422.c
index ac19bf4ea..d6e192242 100644
--- a/tmk_core/protocol/arm_atsam/usb/usb2422.c
+++ b/tmk_core/protocol/arm_atsam/usb/usb2422.c
@@ -64,7 +64,7 @@ void USB_write2422_block(void)
         i2c0_transmit(USB2422_ADDR, dest, 34, 50000);
         SERCOM0->I2CM.CTRLB.bit.CMD = 0x03;
         while (SERCOM0->I2CM.SYNCBUSY.bit.SYSOP) { DBGC(DC_USB_WRITE2422_BLOCK_SYNC_SYSOP); }
-        CLK_delay_us(100);
+        wait_us(100);
     }
 
     DBGC(DC_USB_WRITE2422_BLOCK_COMPLETE);
@@ -135,7 +135,7 @@ void USB2422_init(void)
     sr_exp_data.bit.HUB_RESET_N = 1; //reset high
     SR_EXP_WriteData();
 
-    CLK_delay_us(100);
+    wait_us(100);
 
 #ifndef MD_BOOTLOADER
 
@@ -154,10 +154,9 @@ void USB_reset(void)
     //pulse reset for at least 1 usec
     sr_exp_data.bit.HUB_RESET_N = 0; //reset low
     SR_EXP_WriteData();
-    CLK_delay_us(1);
+    wait_us(2);
     sr_exp_data.bit.HUB_RESET_N = 1; //reset high to run
     SR_EXP_WriteData();
-    CLK_delay_us(1);
 
     DBGC(DC_USB_RESET_COMPLETE);
 }
@@ -247,7 +246,7 @@ void USB_set_host_by_voltage(void)
 
     SR_EXP_WriteData();
 
-    CLK_delay_ms(250);
+    wait_ms(250);
 
     while ((v_5v = adc_get(ADC_5V)) < ADC_5V_START_LEVEL) { DBGC(DC_USB_SET_HOST_5V_LOW_WAITING); }
 
@@ -313,11 +312,11 @@ uint8_t USB2422_Port_Detect_Init(void)
 
     USB_set_host_by_voltage();
 
-    port_detect_retry_ms = CLK_get_ms() + PORT_DETECT_RETRY_INTERVAL;
+    port_detect_retry_ms = timer_read64() + PORT_DETECT_RETRY_INTERVAL;
 
     while (!USB_active())
     {
-        tmod = CLK_get_ms() % PORT_DETECT_RETRY_INTERVAL;
+        tmod = timer_read64() % PORT_DETECT_RETRY_INTERVAL;
 
         if (v_con_1 > v_con_2) //Values updated from USB_set_host_by_voltage();
         {
@@ -333,7 +332,7 @@ uint8_t USB2422_Port_Detect_Init(void)
             else { DBG_LED_OFF; }
         }
 
-        if (CLK_get_ms() > port_detect_retry_ms)
+        if (timer_read64() > port_detect_retry_ms)
         {
             DBGC(DC_PORT_DETECT_INIT_FAILED);
             return 0;
diff --git a/tmk_core/protocol/chibios/main.c b/tmk_core/protocol/chibios/main.c
index ee9571c95..5436d4909 100644
--- a/tmk_core/protocol/chibios/main.c
+++ b/tmk_core/protocol/chibios/main.c
@@ -113,7 +113,7 @@ int main(void) {
   chSysInit();
 
 #ifdef STM32_EEPROM_ENABLE
-  EEPROM_init();
+  EEPROM_Init();
 #endif
 
   // TESTING
diff --git a/tmk_core/protocol/lufa/lufa.c b/tmk_core/protocol/lufa/lufa.c
index 27cf51b16..f2ecf2465 100644
--- a/tmk_core/protocol/lufa/lufa.c
+++ b/tmk_core/protocol/lufa/lufa.c
@@ -517,17 +517,16 @@ void EVENT_USB_Device_ControlRequest(void)
                         if (USB_DeviceState == DEVICE_STATE_Unattached)
                           return;
                     }
-#ifdef KEYBOARD_SHARED_EP
-                    uint8_t report_id = REPORT_ID_KEYBOARD;
-                    if (keyboard_protocol) {
-                       report_id = Endpoint_Read_8();
-                    }
-                    if (report_id == REPORT_ID_KEYBOARD || report_id == REPORT_ID_NKRO) {
+
+                    if (Endpoint_BytesInEndpoint() == 2) {
+                      uint8_t report_id = Endpoint_Read_8();
+
+                      if (report_id == REPORT_ID_KEYBOARD || report_id == REPORT_ID_NKRO) {
                         keyboard_led_stats = Endpoint_Read_8();
+                      }
+                    } else {
+                      keyboard_led_stats = Endpoint_Read_8();
                     }
-#else
-                    keyboard_led_stats = Endpoint_Read_8();
-#endif
 
                     Endpoint_ClearOUT();
                     Endpoint_ClearStatusStage();