diff --git a/tests/layer_cache/config.h b/tests/layer_cache/config.h
new file mode 100644
index 0000000000..656cedb338
--- /dev/null
+++ b/tests/layer_cache/config.h
@@ -0,0 +1,21 @@
+/* Copyright 2018 Fred Sundvik
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define MATRIX_ROWS 2
+#define MATRIX_COLS 2
+#define PREVENT_STUCK_MODIFIERS
diff --git a/tests/layer_cache/keymap.c b/tests/layer_cache/keymap.c
new file mode 100644
index 0000000000..d310bff8b9
--- /dev/null
+++ b/tests/layer_cache/keymap.c
@@ -0,0 +1,24 @@
+/* Copyright 2018 Fred Sundvik
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "quantum.h"
+
+const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
+    [0] = {
+      {KC_A, KC_B},
+      {KC_C, KC_D},
+    }
+};
diff --git a/tests/layer_cache/rules.mk b/tests/layer_cache/rules.mk
new file mode 100644
index 0000000000..9b8615f954
--- /dev/null
+++ b/tests/layer_cache/rules.mk
@@ -0,0 +1,16 @@
+# Copyright 2018 Fred Sundvik
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+CUSTOM_MATRIX=yes
diff --git a/tests/layer_cache/test_layer_cache.cpp b/tests/layer_cache/test_layer_cache.cpp
new file mode 100644
index 0000000000..4aebe24b74
--- /dev/null
+++ b/tests/layer_cache/test_layer_cache.cpp
@@ -0,0 +1,140 @@
+/* Copyright 2018 Fred Sundvik
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "test_common.hpp"
+
+#if MAX_LAYER_BITS != 5
+#error "Tese tests assume that the MAX_LAYER_BITS is equal to 5"
+// If this is changed, change the constants below
+#endif
+
+#if MATRIX_COLS != 2 || MATRIX_ROWS !=2
+#error "These tests assume that the second row starts after the second column"
+#endif
+
+namespace
+{
+  constexpr uint8_t max_layer_value = 0b11111;
+  constexpr uint8_t min_layer_value = 0;
+  constexpr uint8_t alternating_starting_with_1 = 0b10101;
+  constexpr uint8_t alternating_starting_with_0 = 0b01010;
+
+
+  uint8_t read_cache(uint8_t col, uint8_t row) {
+    keypos_t key;
+    key.col = col;
+    key.row =  row;
+    return read_source_layers_cache(key);
+  }
+
+  void write_cache(uint8_t col, uint8_t row, uint8_t value) {
+    keypos_t key;
+    key.col = col;
+    key.row =  row;
+    return update_source_layers_cache(key, value);
+  }
+
+  void fill_cache() {
+    for (int i=0; i < MATRIX_ROWS; i++) {
+      for (int j=0; j < MATRIX_COLS; j++) {
+        write_cache(j, i, max_layer_value);
+      }
+    }
+  }
+
+  void clear_cache() {
+    for (int i=0; i < MATRIX_ROWS; i++) {
+      for (int j=0; j < MATRIX_COLS; j++) {
+        write_cache(j, i, min_layer_value);
+      }
+    }
+  }
+}
+
+class LayerCache : public testing::Test
+{
+public:
+  LayerCache()
+  {
+    clear_cache();
+  }
+};
+
+TEST_F(LayerCache, LayerCacheIsInitializedToZero) {
+  for (int i=0; i < MATRIX_ROWS; i++) {
+    for (int j=0; j < MATRIX_COLS; j++) {
+      EXPECT_EQ(read_cache(j, i), min_layer_value);
+    }
+  }
+}
+
+TEST_F(LayerCache, FillAndClearCache) {
+  fill_cache();
+  clear_cache();
+  for (int i=0; i < MATRIX_ROWS; i++) {
+    for (int j=0; j < MATRIX_COLS; j++) {
+      EXPECT_EQ(read_cache(j, i), min_layer_value);
+    }
+  }
+}
+
+TEST_F(LayerCache, WriteAndReadFirstPosMaximumValue) {
+  write_cache(0, 0, max_layer_value);
+  EXPECT_EQ(read_cache(0, 0), max_layer_value);
+  // The second position should not be updated
+  EXPECT_EQ(read_cache(1, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
+
+TEST_F(LayerCache, WriteAndReadSecondPosMaximumValue) {
+  write_cache(1, 0, max_layer_value);
+  EXPECT_EQ(read_cache(1, 0), max_layer_value);
+  // The surrounding positions should not be updated
+  EXPECT_EQ(read_cache(0, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
+
+TEST_F(LayerCache, WriteAndReadFirstPosAlternatingBitsStartingWith1) {
+  write_cache(0, 0, alternating_starting_with_1);
+  EXPECT_EQ(read_cache(0, 0), alternating_starting_with_1);
+  // The second position should not be updated
+  EXPECT_EQ(read_cache(1, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
+
+TEST_F(LayerCache, WriteAndReadSecondPosAlternatingBitsStartingWith1) {
+  write_cache(1, 0, alternating_starting_with_1);
+  EXPECT_EQ(read_cache(1, 0), alternating_starting_with_1);
+  // The surrounding positions should not be updated
+  EXPECT_EQ(read_cache(0, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
+
+TEST_F(LayerCache, WriteAndReadFirstPosAlternatingBitsStartingWith0) {
+  write_cache(0, 0, alternating_starting_with_0);
+  EXPECT_EQ(read_cache(0, 0), alternating_starting_with_0);
+  // The second position should not be updated
+  EXPECT_EQ(read_cache(1, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
+
+TEST_F(LayerCache, WriteAndReadSecondPosAlternatingBitsStartingWith0) {
+  write_cache(1, 0, alternating_starting_with_0);
+  EXPECT_EQ(read_cache(1, 0), alternating_starting_with_0);
+  // The surrounding positions should not be updated
+  EXPECT_EQ(read_cache(0, 0), min_layer_value);
+  EXPECT_EQ(read_cache(0, 1), min_layer_value);
+}
diff --git a/tmk_core/common/action_layer.c b/tmk_core/common/action_layer.c
index f3cd381ab0..a1ead402c8 100644
--- a/tmk_core/common/action_layer.c
+++ b/tmk_core/common/action_layer.c
@@ -220,37 +220,72 @@ void layer_debug(void)
 #endif
 
 #if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
-uint8_t source_layers_cache[(MATRIX_ROWS * MATRIX_COLS + 7) / 8][MAX_LAYER_BITS] = {{0}};
+uint8_t source_layers_cache[(MATRIX_ROWS * MATRIX_COLS * MAX_LAYER_BITS + 7) / 8] = {0};
+static const uint8_t layer_cache_mask = (1u << MAX_LAYER_BITS) - 1;
 
 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;
+  const uint16_t key_number = key.col + (key.row * MATRIX_COLS);
+  const uint32_t bit_number = key_number * MAX_LAYER_BITS;
+  const uint16_t byte_number = bit_number / 8;
+  if (byte_number >= sizeof(source_layers_cache)) {
+    return;
+  }
+  const uint8_t bit_position = bit_number % 8;
+  int8_t shift = 16 - MAX_LAYER_BITS - bit_position;
 
-    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);
+  if (shift > 8 ) {
+    // We need to write only one byte
+    shift -= 8;
+    const uint8_t mask = layer_cache_mask << shift;
+    const uint8_t shifted_layer = layer << shift;
+    source_layers_cache[byte_number] = (shifted_layer & mask) | (source_layers_cache[byte_number] & (~mask));
+  } else {
+    if (byte_number + 1 >= sizeof(source_layers_cache)) {
+      return;
     }
+    // We need to write two bytes
+    uint16_t value = layer;
+    uint16_t mask = layer_cache_mask;
+    value <<= shift;
+    mask <<= shift;
+
+    uint16_t masked_value = value & mask;
+    uint16_t inverse_mask = ~mask;
+
+    // This could potentially be done with a single write, but then we have to assume the endian
+    source_layers_cache[byte_number + 1] = masked_value | (source_layers_cache[byte_number + 1] & (inverse_mask));
+    masked_value >>= 8;
+    inverse_mask >>= 8;
+    source_layers_cache[byte_number] = masked_value | (source_layers_cache[byte_number] & (inverse_mask));
+  }
 }
 
 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;
+  const uint16_t key_number = key.col + (key.row * MATRIX_COLS);
+  const uint32_t bit_number = key_number * MAX_LAYER_BITS;
+  const uint16_t byte_number = bit_number / 8;
+  if (byte_number >= sizeof(source_layers_cache)) {
+    return 0;
+  }
+  const uint8_t bit_position = bit_number % 8;
 
-    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;
+  int8_t shift = 16 - MAX_LAYER_BITS - bit_position;
+
+  if (shift > 8 ) {
+    // We need to read only one byte
+    shift -= 8;
+    return (source_layers_cache[byte_number] >> shift) & layer_cache_mask;
+  } else {
+    if (byte_number + 1 >= sizeof(source_layers_cache)) {
+      return 0;
     }
-
-    return layer;
+    // Otherwise read two bytes
+    // This could potentially be done with a single read, but then we have to assume the endian
+    uint16_t value = source_layers_cache[byte_number] << 8 | source_layers_cache[byte_number + 1];
+    return (value >> shift) & layer_cache_mask;
+  }
 }
 #endif