Added mode reverse step function

[qmk_firmware.git] / quantum / quantum.c

diff --git a/quantum/quantum.c b/quantum/quantum.c
index f653564a67dea8396b04db82500c4b9dc531a480..0aecd238e45f9813e5a0a2da9a21cad8b972c558 100644 (file)
--- a/quantum/quantum.c
+++ b/quantum/quantum.c
@@ -21,6 +21,8 @@ static void do_code16 (uint16_t code, void (*f) (uint8_t)) {
   if (code & QK_LGUI)
     f(KC_LGUI);
 
+  if (code < QK_RMODS_MIN) return;
+
   if (code & QK_RCTL)
     f(KC_RCTL);
   if (code & QK_RSFT)
@@ -31,14 +33,42 @@ static void do_code16 (uint16_t code, void (*f) (uint8_t)) {
     f(KC_RGUI);
 }
 
+static inline void qk_register_weak_mods(uint8_t kc) {
+    add_weak_mods(MOD_BIT(kc));
+    send_keyboard_report();
+}
+
+static inline void qk_unregister_weak_mods(uint8_t kc) {
+    del_weak_mods(MOD_BIT(kc));
+    send_keyboard_report();
+}
+
+static inline void qk_register_mods(uint8_t kc) {
+    add_weak_mods(MOD_BIT(kc));
+    send_keyboard_report();
+}
+
+static inline void qk_unregister_mods(uint8_t kc) {
+    del_weak_mods(MOD_BIT(kc));
+    send_keyboard_report();
+}
+
 void register_code16 (uint16_t code) {
-  do_code16 (code, register_code);
+  if (IS_MOD(code) || code == KC_NO) {
+      do_code16 (code, qk_register_mods);
+  } else {
+      do_code16 (code, qk_register_weak_mods);
+  }
   register_code (code);
 }
 
 void unregister_code16 (uint16_t code) {
   unregister_code (code);
-  do_code16 (code, unregister_code);
+  if (IS_MOD(code) || code == KC_NO) {
+      do_code16 (code, qk_unregister_mods);
+  } else {
+      do_code16 (code, qk_unregister_weak_mods);
+  }
 }
 
 __attribute__ ((weak))