Rewrite layer action with layer_switch

7 files changed, 148 insertions, 112 deletions

diff --git a/common/action.c b/common/action.c
index 4f0a5f9064..246fd99d8a 100644
--- a/common/action.c
+++ b/common/action.c
@@ -27,12 +27,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "layer_switch.h"
 
 
-/* default layer indicates base layer */
-uint8_t default_layer = 0;
-/* current layer indicates active layer at this time */
-uint8_t current_layer = 0;
-
-
 static void process_action(keyrecord_t *record);
 static bool process_tapping(keyrecord_t *record);
 static void waiting_buffer_scan_tap(void);
@@ -219,17 +213,8 @@ static action_t get_action(key_t key)
         return action;
     }
 
-    /* current layer: 0 means default layer */
-    if (current_layer) {
-        action = action_for_key(current_layer, key);
-        if (action.code != ACTION_TRANSPARENT) {
-            debug("current layer: used. "); debug_dec(current_layer); debug("\n");
-            return action;
-        }
-    }
-
     /* default layer */
-    debug("default layer: used. \n");
+    //debug("get_aciton: default layer: "); debug_dec(default_layer); debug("\n");
     action = action_for_key(default_layer, key);
     return action;
 }
@@ -242,7 +227,8 @@ static void process_action(keyrecord_t *record)
     if (IS_NOEVENT(event)) { return; }
 
     action_t action = get_action(event.key);
-    debug("ACTION: "); debug_action(action); debug("\n");
+    debug("ACTION: "); debug_action(action); debug(" ");
+    layer_switch_debug(); debug("["); debug_dec(default_layer); debug("]\n");
 
     switch (action.kind.id) {
         /* Key and Mods */
@@ -383,57 +369,57 @@ static void process_action(keyrecord_t *record)
             break;
 
         /* Layer key */
-        case ACT_LAYER:
+        case ACT_LAYER_SET:
             switch (action.layer.code) {
                 case LAYER_MOMENTARY:  /* momentary */
                     if (event.pressed) {
-                        layer_switch(action.layer.val);
+                        layer_switch_move(action.layer.val);
                     }
                     else {
                         // NOTE: This is needed by legacy keymap support
-                        layer_switch(0);
+                        layer_switch_move(0);
                     }
                     break;
                 case LAYER_ON_PRESS:
                     if (event.pressed) {
-                        layer_switch(action.layer.val);
+                        layer_switch_move(action.layer.val);
                     }
                     break;
                 case LAYER_ON_RELEASE:
                     if (!event.pressed) {
-                        layer_switch(action.layer.val);
+                        layer_switch_move(action.layer.val);
                     }
                     break;
                 case LAYER_ON_BOTH:
-                    layer_switch(action.layer.val);
+                    layer_switch_move(action.layer.val);
                     break;
                 case LAYER_TAP_TOGGLE:  /* switch on hold and toggle on several taps */
                     if (event.pressed) {
                         if (tap_count < TAPPING_TOGGLE) {
-                            layer_switch(action.layer.val);
+                            layer_switch_move(action.layer.val);
                         }
                     } else {
                         if (tap_count >= TAPPING_TOGGLE) {
                             debug("LAYER_PRESSED: tap toggle.\n");
-                            layer_switch(action.layer.val);
+                            layer_switch_move(action.layer.val);
                         }
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_PRESS:
                     if (event.pressed) {
                         default_layer = action.layer.val;
-                        layer_switch(0);
+                        layer_switch_move(0);
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_RELEASE:
                     if (!event.pressed) {
                         default_layer = action.layer.val;
-                        layer_switch(0);
+                        layer_switch_move(0);
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_BOTH:
                     default_layer = action.layer.val;
-                    layer_switch(0);
+                    layer_switch_move(0);
                     break;
                 default:
                     /* tap key */
@@ -443,7 +429,7 @@ static void process_action(keyrecord_t *record)
                              register_code(action.layer.code);
                         } else {
                              debug("LAYER_SET: No tap: layer_set(on press)\n");
-                             layer_switch(action.layer.val);
+                             layer_switch_move(action.layer.val);
                         }
                     } else {
                         if (IS_TAPPING_KEY(event.key) && tap_count > 0) {
@@ -452,7 +438,7 @@ static void process_action(keyrecord_t *record)
                         } else {
                             // NOTE: This is needed by legacy keymap support
                             debug("LAYER_SET: No tap: return to default layer(on release)\n");
-                            layer_switch(0);
+                            layer_switch_move(0);
                         }
                     }
                     break;
@@ -462,52 +448,52 @@ static void process_action(keyrecord_t *record)
             switch (action.layer.code) {
                 case LAYER_MOMENTARY:  /* momentary */
                     if (event.pressed) {
-                        layer_switch(current_layer | action.layer.val);
+                        layer_switch_move(layer_switch_get_layer() | action.layer.val);
                     } else {
-                        layer_switch(current_layer & ~action.layer.val);
+                        layer_switch_move(layer_switch_get_layer() & ~action.layer.val);
                     }
                     break;
                 case LAYER_ON_PRESS:
                     if (event.pressed) {
-                        layer_switch(current_layer ^ action.layer.val);
+                        layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                     }
                     break;
                 case LAYER_ON_RELEASE:
                     if (!event.pressed) {
-                        layer_switch(current_layer ^ action.layer.val);
+                        layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                     }
                     break;
                 case LAYER_ON_BOTH:
-                    layer_switch(current_layer ^ action.layer.val);
+                    layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                     break;
                 case LAYER_TAP_TOGGLE:  /* switch on hold and toggle on several taps */
                     if (event.pressed) {
                         if (tap_count < TAPPING_TOGGLE) {
                             debug("LAYER_BIT: tap toggle(press).\n");
-                            layer_switch(current_layer ^ action.layer.val);
+                            layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                         }
                     } else {
                         if (tap_count <= TAPPING_TOGGLE) {
                             debug("LAYER_BIT: tap toggle(release).\n");
-                            layer_switch(current_layer ^ action.layer.val);
+                            layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                         }
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_PRESS:
                     if (event.pressed) {
                         default_layer = default_layer ^ action.layer.val;
-                        layer_switch(0);
+                        layer_switch_move(default_layer);
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_RELEASE:
                     if (!event.pressed) {
                         default_layer = default_layer ^ action.layer.val;
-                        layer_switch(0);
+                        layer_switch_move(default_layer);
                     }
                     break;
                 case LAYER_SET_DEFAULT_ON_BOTH:
                     default_layer = default_layer ^ action.layer.val;
-                    layer_switch(0);
+                    layer_switch_move(default_layer);
                     break;
                 default:
                     // tap key
@@ -517,7 +503,7 @@ static void process_action(keyrecord_t *record)
                             register_code(action.layer.code);
                         } else {
                             debug("LAYER_BIT: No tap: layer_bit(on press)\n");
-                            layer_switch(current_layer ^ action.layer.val);
+                            layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                         }
                     } else {
                         if (IS_TAPPING_KEY(event.key) && tap_count > 0) {
@@ -525,7 +511,7 @@ static void process_action(keyrecord_t *record)
                             unregister_code(action.layer.code);
                         } else {
                             debug("LAYER_BIT: No tap: layer_bit(on release)\n");
-                            layer_switch(current_layer ^ action.layer.val);
+                            layer_switch_move(layer_switch_get_layer() ^ action.layer.val);
                         }
                     }
                     break;
@@ -542,27 +528,27 @@ static void process_action(keyrecord_t *record)
                     break;
                 case LAYER_ON_PRESS:
                     if (event.pressed) {
-                        layer_switch_inv(action.layer.val);
+                        layer_switch_invert(action.layer.val);
                     }
                     break;
                 case LAYER_ON_RELEASE:
                     if (!event.pressed) {
-                        layer_switch_inv(action.layer.val);
+                        layer_switch_invert(action.layer.val);
                     }
                     break;
                 case LAYER_ON_BOTH:
-                    layer_switch_inv(action.layer.val);
+                    layer_switch_invert(action.layer.val);
                     break;
                 case LAYER_TAP_TOGGLE:  /* switch on hold and toggle on several taps */
                     if (event.pressed) {
                         if (tap_count < TAPPING_TOGGLE) {
                             debug("LAYER_SWITCH: tap toggle(press).\n");
-                            layer_switch_inv(action.layer.val);
+                            layer_switch_invert(action.layer.val);
                         }
                     } else {
                         if (tap_count <= TAPPING_TOGGLE) {
                             debug("LAYER_SWITCH: tap toggle(release).\n");
-                            layer_switch_inv(action.layer.val);
+                            layer_switch_invert(action.layer.val);
                         }
                     }
                     break;
@@ -573,16 +559,16 @@ static void process_action(keyrecord_t *record)
                             debug("LAYER_SWITCH: Tap: register_code\n");
                             register_code(action.layer.code);
                         } else {
-                            debug("LAYER_SWITCH: No tap: layer_switch(on press)\n");
-                            layer_switch_inv(action.layer.val);
+                            debug("LAYER_SWITCH: No tap: layer_switch on press\n");
+                            layer_switch_invert(action.layer.val);
                         }
                     } else {
                         if (IS_TAPPING_KEY(event.key) && tap_count > 0) {
                             debug("LAYER_SWITCH: Tap: unregister_code\n");
                             unregister_code(action.layer.code);
                         } else {
-                            debug("LAYER_SWITCH: No tap: layer_switch(on release)\n");
-                            layer_switch_inv(action.layer.val);
+                            debug("LAYER_SWITCH: No tap: layer_switch on release\n");
+                            layer_switch_invert(action.layer.val);
                         }
                     }
                     break;
@@ -889,19 +875,6 @@ bool sending_anykey(void)
             host_last_sysytem_report() || host_last_consumer_report());
 }
 
-// TODO: rename or reinpl with new layer_switch.c
-void layer_switch(uint8_t new_layer)
-{
-    if (current_layer != new_layer) {
-        debug("Layer Switch: "); debug_hex(current_layer);
-        debug(" -> "); debug_hex(new_layer); debug("\n");
-
-        current_layer = new_layer;
-        clear_keyboard_but_mods(); // To avoid stuck keys
-        // NOTE: update mods with full scan of matrix? if modifier changes between layers
-    }
-}
-
 bool is_tap_key(key_t key)
 {
     action_t action = get_action(key);
@@ -910,7 +883,7 @@ bool is_tap_key(key_t key)
         case ACT_LMODS_TAP:
         case ACT_RMODS_TAP:
             return true;
-        case ACT_LAYER:
+        case ACT_LAYER_SET:
         case ACT_LAYER_BIT:
             switch (action.layer.code) {
                 case LAYER_MOMENTARY:
@@ -956,9 +929,9 @@ static void debug_action(action_t action)
         case ACT_RMODS_TAP:         debug("ACT_RMODS_TAP");         break;
         case ACT_USAGE:             debug("ACT_USAGE");             break;
         case ACT_MOUSEKEY:          debug("ACT_MOUSEKEY");          break;
-        case ACT_LAYER:             debug("ACT_LAYER");             break;
+        case ACT_LAYER_SET:         debug("ACT_LAYER_SET");         break;
         case ACT_LAYER_BIT:         debug("ACT_LAYER_BIT");         break;
-        case ACT_LAYER_SWITCH:      debug("ACT_LAYER_SWITCH");     break;
+        case ACT_LAYER_SWITCH:      debug("ACT_LAYER_SWITCH");      break;
         case ACT_MACRO:             debug("ACT_MACRO");             break;
         case ACT_COMMAND:           debug("ACT_COMMAND");           break;
         case ACT_FUNCTION:          debug("ACT_FUNCTION");          break;
diff --git a/common/action.h b/common/action.h
index 1d00e02d54..46ae809cb7 100644
--- a/common/action.h
+++ b/common/action.h
@@ -76,11 +76,6 @@ typedef union {
 
 
 
-/* layer used currently */
-extern uint8_t current_layer;
-/* layer to return or start with */
-extern uint8_t default_layer;
-
 /* Execute action per keyevent */
 void action_exec(keyevent_t event);
 
@@ -155,14 +150,14 @@ bool waiting_buffer_has_anykey_pressed(void);
  *
  * Mouse Keys
  * ----------
- * TODO: can be combined with 'Other HID Usage'? to save action kind id.
+ * NOTE: can be combined with 'Other HID Usage'? to save action kind id.
  * ACT_MOUSEKEY(0110):
  * 0101|XXXX| keycode     Mouse key
  *
  *
  * Layer Actions
  * -------------
- * ACT_LAYER(1000):             Set layer
+ * ACT_LAYER_SET(1000):         Set layer
  * 1000|LLLL|0000 0000   set current layer on press and return to default on release(momentary)
  * 1000|LLLL|0000 0001   set current layer on press
  * 1000|LLLL|0000 0010   set current layer on release
@@ -216,7 +211,7 @@ enum action_kind_id {
     ACT_USAGE           = 0b0100,
     ACT_MOUSEKEY        = 0b0101,
 
-    ACT_LAYER           = 0b1000,
+    ACT_LAYER_SET       = 0b1000,
     ACT_LAYER_BIT       = 0b1001,
     ACT_LAYER_SWITCH    = 0b1011,
 
@@ -277,14 +272,14 @@ enum layer_codes {
  */
 /* set default layer */
 #define ACTION_LAYER_SET_DEFAULT(layer)         ACTION_LAYER_SET_DEFAULT_R(layer)
-#define ACTION_LAYER_SET_DEFAULT_P(layer)       ACTION(ACT_LAYER, (layer)<<8 | LAYER_SET_DEFAULT_ON_PRESS)
-#define ACTION_LAYER_SET_DEFAULT_R(layer)       ACTION(ACT_LAYER, (layer)<<8 | LAYER_SET_DEFAULT_ON_RELEASE)
-#define ACTION_LAYER_SET_DEFAULT_B(layer)       ACTION(ACT_LAYER, (layer)<<8 | LAYER_SET_DEFAULT_ON_BOTH)
+#define ACTION_LAYER_SET_DEFAULT_P(layer)       ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_SET_DEFAULT_ON_PRESS)
+#define ACTION_LAYER_SET_DEFAULT_R(layer)       ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_SET_DEFAULT_ON_RELEASE)
+#define ACTION_LAYER_SET_DEFAULT_B(layer)       ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_SET_DEFAULT_ON_BOTH)
 /* bit-xor default layer */
 #define ACTION_LAYER_BIT_DEFAULT(bits)          ACTION_LAYER_BIT_DEFAULT_R(bits)
-#define ACTION_LAYER_BIT_DEFAULT_P(bits)        ACTION(ACT_LAYER, (bits)<<8 | LAYER_SET_DEFAULT_ON_PRESS)
-#define ACTION_LAYER_BIT_DEFAULT_R(bits)        ACTION(ACT_LAYER, (bits)<<8 | LAYER_SET_DEFAULT_ON_RELEASE)
-#define ACTION_LAYER_BIT_DEFAULT_B(bits)        ACTION(ACT_LAYER, (bits)<<8 | LAYER_SET_DEFAULT_ON_BOTH)
+#define ACTION_LAYER_BIT_DEFAULT_P(bits)        ACTION(ACT_LAYER_BIT, (bits)<<8 | LAYER_SET_DEFAULT_ON_PRESS)
+#define ACTION_LAYER_BIT_DEFAULT_R(bits)        ACTION(ACT_LAYER_BIT, (bits)<<8 | LAYER_SET_DEFAULT_ON_RELEASE)
+#define ACTION_LAYER_BIT_DEFAULT_B(bits)        ACTION(ACT_LAYER_BIT, (bits)<<8 | LAYER_SET_DEFAULT_ON_BOTH)
 /*
  * Current layer: Return to default layer
  */
@@ -296,13 +291,13 @@ enum layer_codes {
  * Current layer: Set
  */
 #define ACTION_LAYER_SET(layer)                 ACTION_LAYER_SET_P(layer)
-#define ACTION_LAYER_SET_MOMENTARY(layer)       ACTION(ACT_LAYER, (layer)<<8 | LAYER_MOMENTARY)
+#define ACTION_LAYER_SET_MOMENTARY(layer)       ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_MOMENTARY)
 #define ACTION_LAYER_SET_TOGGLE(layer)          ACTION_LAYER_SET_R(layer)
-#define ACTION_LAYER_SET_P(layer)               ACTION(ACT_LAYER, (layer)<<8 | LAYER_ON_PRESS)
-#define ACTION_LAYER_SET_R(layer)               ACTION(ACT_LAYER, (layer)<<8 | LAYER_ON_RELEASE)
-#define ACTION_LAYER_SET_B(layer)               ACTION(ACT_LAYER, (layer)<<8 | LAYER_ON_BOTH)
-#define ACTION_LAYER_SET_TAP_TOGGLE(layer)      ACTION(ACT_LAYER, (layer)<<8 | LAYER_TAP_TOGGLE)
-#define ACTION_LAYER_SET_TAP_KEY(layer, key)    ACTION(ACT_LAYER, (layer)<<8 | (key))
+#define ACTION_LAYER_SET_P(layer)               ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_ON_PRESS)
+#define ACTION_LAYER_SET_R(layer)               ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_ON_RELEASE)
+#define ACTION_LAYER_SET_B(layer)               ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_ON_BOTH)
+#define ACTION_LAYER_SET_TAP_TOGGLE(layer)      ACTION(ACT_LAYER_SET, (layer)<<8 | LAYER_TAP_TOGGLE)
+#define ACTION_LAYER_SET_TAP_KEY(layer, key)    ACTION(ACT_LAYER_SET, (layer)<<8 | (key))
 /*
  * Current layer: Bit-op
  */
diff --git a/common/command.c b/common/command.c
index 82f647c8f3..2d01c95e6a 100644
--- a/common/command.c
+++ b/common/command.c
@@ -543,12 +543,9 @@ static uint8_t numkey2num(uint8_t code)
 
 static void switch_default_layer(uint8_t layer)
 {
-    print_val_hex8(current_layer);
-    print_val_hex8(default_layer);
-    print("switch to "); print_val_hex8(layer);
-
+    // TODO check existence of layer or whether it can be used as default layer
+    print("switch_default_layer: "); print_dec(default_layer); print(" to "); print_dec(layer); 
     default_layer = layer;
-    current_layer = 0;  /* 0 means default_layer */
     layer_switch_clear();
     clear_keyboard();
 }
diff --git a/common/layer_switch.c b/common/layer_switch.c
index 9bc804e641..22bfb34f65 100644
--- a/common/layer_switch.c
+++ b/common/layer_switch.c
@@ -2,50 +2,88 @@
 #include "keyboard.h"
 #include "action.h"
 #include "debug.h"
+#include "util.h"
 #include "layer_switch.h"
 
 
+uint8_t default_layer = 0;
+
 uint16_t layer_switch_stat = 0;
 
 
-uint16_t layer_switch_stat_get(void)
+uint16_t layer_switch_get_stat(void)
 {
     return layer_switch_stat;
 }
 
-void layer_switch_stat_set(uint16_t stat)
+/* return highest layer whose state is on */
+uint8_t layer_switch_get_layer(void)
+{
+    return biton16(layer_switch_stat);
+}
+
+static inline void stat_set(uint16_t stat)
 {
+    debug("layer_switch: ");
+    layer_switch_debug(); debug(" to ");
+
     layer_switch_stat = stat;
-    layer_switch_debug();
+
+    layer_switch_debug(); debug("\n");
+
+    clear_keyboard_but_mods(); // To avoid stuck keys
 }
 
 void layer_switch_clear(void)
 {
-    layer_switch_stat = 0;
-    layer_switch_debug();
+    stat_set(0);
+}
+
+
+void layer_switch_set(uint16_t stat)
+{
+    stat_set(stat);
+}
+
+void layer_switch_move(uint8_t layer)
+{
+    if (layer)
+        stat_set(1<<layer);
+    else
+        stat_set(0);    // fall back to default layer
 }
 
 void layer_switch_on(uint8_t layer)
 {
-    layer_switch_stat |= (1<<layer);
-    layer_switch_debug();
+    stat_set(layer_switch_stat | (1<<layer));
 }
 
 void layer_switch_off(uint8_t layer)
 {
-    layer_switch_stat &= ~(1<<layer);
-    layer_switch_debug();
+    stat_set(layer_switch_stat & ~(1<<layer));
+}
+
+void layer_switch_invert(uint8_t layer)
+{
+    stat_set(layer_switch_stat ^ (1<<layer));
 }
 
-void layer_switch_inv(uint8_t layer)
+void layer_switch_or(uint16_t stat)
+{
+    stat_set(layer_switch_stat | stat);
+}
+void layer_switch_and(uint16_t stat)
+{
+    stat_set(layer_switch_stat & stat);
+}
+void layer_switch_xor(uint16_t stat)
 {
-    layer_switch_stat ^= (1<<layer);
-    layer_switch_debug();
+    stat_set(layer_switch_stat ^ stat);
 }
 
 void layer_switch_debug(void)
 {
-    debug("layer_switch_stat: "); debug_bin16(layer_switch_stat); debug("\n");
+    debug_hex16(layer_switch_stat); debug("("); debug_dec(layer_switch_get_layer()); debug(")");
 }
 
 action_t layer_switch_get_action(key_t key)
@@ -58,8 +96,6 @@ action_t layer_switch_get_action(key_t key)
         if (layer_switch_stat & (1<<i)) {
             action = action_for_key(i, key);
             if (action.code != ACTION_TRANSPARENT) {
-                layer_switch_debug();
-                debug("layer_switch: used. "); debug_dec(i); debug("\n");
                 return action;
             }
         }
diff --git a/common/layer_switch.h b/common/layer_switch.h
index 9f0695260c..25c81a5dc2 100644
--- a/common/layer_switch.h
+++ b/common/layer_switch.h
@@ -21,16 +21,39 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "keyboard.h"
 #include "action.h"
 
-uint16_t layer_switch_stat;
 
-uint16_t layer_switch_stat_get(void);
-void layer_switch_stat_set(uint16_t stat);
+/* base layer to fall back */
+extern uint8_t default_layer;
+
+/* layer status */
+extern uint16_t layer_switch_stat;
+
+/* return layer status */
+uint16_t layer_switch_get_stat(void);
+/* return current active layer */
+uint8_t layer_switch_get_layer(void);
+
+/* switch off all layers */
 void layer_switch_clear(void);
+/* set layer status */
+void layer_switch_set(uint16_t stat);
+/* move to layer */
+void layer_switch_move(uint8_t layer);
+/* switch on layer */
 void layer_switch_on(uint8_t layer);
+/* switch off layer */
 void layer_switch_off(uint8_t layer);
-/* invert state */
-void layer_switch_inv(uint8_t layer);
+/* switch state of layer */
+void layer_switch_invert(uint8_t layer);
+
+/* bitwise operation against layer status */
+void layer_switch_or(uint16_t stat);
+void layer_switch_and(uint16_t stat);
+void layer_switch_xor(uint16_t stat);
+
 void layer_switch_debug(void);
+
+/* return action depending on current layer status */
 action_t layer_switch_get_action(key_t key);
 
 #endif
diff --git a/common/util.c b/common/util.c
index 9d8fb93219..ff1926d7d1 100644
--- a/common/util.c
+++ b/common/util.c
@@ -39,6 +39,7 @@ uint8_t bitpop16(uint16_t bits)
 }
 
 // most significant on-bit - return highest location of on-bit
+// NOTE: return 0 when bit0 is on or all bits are off
 uint8_t biton(uint8_t bits)
 {
     uint8_t n = 0;
@@ -47,3 +48,13 @@ uint8_t biton(uint8_t bits)
     if (bits >> 1) { bits >>= 1; n += 1;}
     return n;
 }
+
+uint8_t biton16(uint16_t bits)
+{
+    uint8_t n = 0;
+    if (bits >> 8) { bits >>= 8; n += 8;}
+    if (bits >> 4) { bits >>= 4; n += 4;}
+    if (bits >> 2) { bits >>= 2; n += 2;}
+    if (bits >> 1) { bits >>= 1; n += 1;}
+    return n;
+}
diff --git a/common/util.h b/common/util.h
index c3734487f9..58b7fdf145 100644
--- a/common/util.h
+++ b/common/util.h
@@ -31,5 +31,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 uint8_t bitpop(uint8_t bits);
 uint8_t bitpop16(uint16_t bits);
 uint8_t biton(uint8_t bits);
+uint8_t biton16(uint16_t bits);
 
 #endif