4 files changed, 115 insertions, 9 deletions

diff --git a/tmk_core/common/action.c b/tmk_core/common/action.c
index 2ccc0e0b9..f9e6c17dc 100644
--- a/tmk_core/common/action.c
+++ b/tmk_core/common/action.c
@@ -53,6 +53,22 @@ void action_exec(keyevent_t event)
 #endif
 }
 
+#if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
+bool disable_action_cache = false;
+
+void process_action_nocache(keyrecord_t *record)
+{
+    disable_action_cache = true;
+    process_action(record);
+    disable_action_cache = false;
+}
+#else
+void process_action_nocache(keyrecord_t *record)
+{
+    process_action(record);
+}
+#endif
+
 __attribute__ ((weak))
 void process_action_kb(keyrecord_t *record) {}
 
@@ -67,7 +83,7 @@ void process_action(keyrecord_t *record)
 
     process_action_kb(record);
 
-    action_t action = layer_switch_get_action(event.key);
+    action_t action = store_or_get_action(event.pressed, event.key);
     dprint("ACTION: "); debug_action(action);
 #ifndef NO_ACTION_LAYER
     dprint(" layer_state: "); layer_debug();
@@ -88,14 +104,24 @@ void process_action(keyrecord_t *record)
                                                                 action.key.mods<<4;
                 if (event.pressed) {
                     if (mods) {
-                        add_weak_mods(mods);
+                        if (IS_MOD(action.key.code)) {
+                            // e.g. LSFT(KC_LGUI): we don't want the LSFT to be weak as it would make it useless.
+                            // this also makes LSFT(KC_LGUI) behave exactly the same as LGUI(KC_LSFT)
+                            add_mods(mods);
+                        } else {
+                            add_weak_mods(mods);
+                        }
                         send_keyboard_report();
                     }
                     register_code(action.key.code);
                 } else {
                     unregister_code(action.key.code);
                     if (mods) {
-                        del_weak_mods(mods);
+                        if (IS_MOD(action.key.code)) {
+                            del_mods(mods);
+                        } else {
+                            del_weak_mods(mods);
+                        }
                         send_keyboard_report();
                     }
                 }
diff --git a/tmk_core/common/action.h b/tmk_core/common/action.h
index 9f528af4b..44ec3047b 100644
--- a/tmk_core/common/action.h
+++ b/tmk_core/common/action.h
@@ -62,6 +62,10 @@ void action_function(keyrecord_t *record, uint8_t id, uint8_t opt);
 void process_action_kb(keyrecord_t *record);
 
 /* Utilities for actions.  */
+#if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
+extern bool disable_action_cache;
+#endif
+void process_action_nocache(keyrecord_t *record);
 void process_action(keyrecord_t *record);
 void register_code(uint8_t code);
 void unregister_code(uint8_t code);
diff --git a/tmk_core/common/action_layer.c b/tmk_core/common/action_layer.c
index c535615f4..fc721a732 100644
--- a/tmk_core/common/action_layer.c
+++ b/tmk_core/common/action_layer.c
@@ -110,9 +110,71 @@ void layer_debug(void)
 }
 #endif
 
+#if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
+uint8_t source_layers_cache[MAX_LAYER_BITS][(MATRIX_ROWS * MATRIX_COLS + 7) / 8] = {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;
 
-action_t layer_switch_get_action(keypos_t key)
+    for (uint8_t bit_number = 0; bit_number < MAX_LAYER_BITS; bit_number++) {
+        source_layers_cache[bit_number][storage_row] ^=
+            (-((layer & (1U << bit_number)) != 0)
+             ^ source_layers_cache[bit_number][storage_row])
+            & (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[bit_number][storage_row]
+              & (1U << storage_bit)) != 0)
+            << bit_number;
+    }
+
+    return layer;
+}
+#endif
+
+/*
+ * Make sure the action triggered when the key is released is the same
+ * one as the one triggered on press. It's important for the mod keys
+ * 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)
+{
+#if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
+    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
+}
+
+
+int8_t layer_switch_get_layer(keypos_t key)
 {
     action_t action;
     action.code = ACTION_TRANSPARENT;
@@ -124,15 +186,18 @@ action_t layer_switch_get_action(keypos_t key)
         if (layers & (1UL<<i)) {
             action = action_for_key(i, key);
             if (action.code != ACTION_TRANSPARENT) {
-                return action;
+                return i;
             }
         }
     }
     /* fall back to layer 0 */
-    action = action_for_key(0, key);
-    return action;
+    return 0;
 #else
-    action = action_for_key(biton32(default_layer_state), key);
-    return action;
+    return biton32(default_layer_state);
 #endif
 }
+
+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_layer.h b/tmk_core/common/action_layer.h
index b6da353cf..3a4b1e334 100644
--- a/tmk_core/common/action_layer.h
+++ b/tmk_core/common/action_layer.h
@@ -70,6 +70,17 @@ void layer_xor(uint32_t state);
 #define layer_debug()
 #endif
 
+/* pressed actions cache */
+#if !defined(NO_ACTION_LAYER) && defined(PREVENT_STUCK_MODIFIERS)
+/* The number of bits needed to represent the layer number: log2(32). */
+#define MAX_LAYER_BITS 5
+void update_source_layers_cache(keypos_t key, uint8_t layer);
+uint8_t read_source_layers_cache(keypos_t key);
+#endif
+action_t store_or_get_action(bool pressed, keypos_t key);
+
+/* return the topmost non-transparent layer currently associated with key */
+int8_t layer_switch_get_layer(keypos_t key);
 
 /* return action depending on current layer status */
 action_t layer_switch_get_action(keypos_t key);