Enable temporary (weak) mods for specific keys and layer switching with mods

7 files changed, 235 insertions, 110 deletions

diff --git a/src/hid.c b/src/hid.c
index e322292..d4fdb7c 100644
--- a/src/hid.c
+++ b/src/hid.c
@@ -16,14 +16,22 @@ struct layerkey {
 	uint key;
 };
 
+struct hid_report {
+	uint8_t mods;
+	uint8_t codes[6];
+	uint8_t codecnt;
+};
+
 static uint32_t keysyms[KEY_ROWS][KEY_COLS] = { 0 };
 
 static struct layerkey active_stack[16] = { 0 };
 static uint active_top = 0;
 
-static uint8_t hid_report_prev[6] = { 0 };
-static uint8_t hid_report[6] = { 0 };
-static uint hid_report_len = 0;
+static struct hid_report hid_report_prev;
+static struct hid_report hid_report;
+
+static uint8_t active_weak_mods;
+static uint8_t active_mods;
 
 static uint64_t bounce_mat[KEY_ROWS][KEY_COLS] = { 0 };
 
@@ -31,6 +39,7 @@ static bool seen_mat[KEY_ROWS][KEY_COLS];
 
 static void active_pop(uint layer);
 static void active_push(uint layer, uint key);
+static void add_keycode(uint8_t keycode);
 
 void
 active_pop(uint layer)
@@ -46,124 +55,149 @@ active_pop(uint layer)
 void
 active_push(uint layer, uint key)
 {
+	uint i;
+
 	if (active_top == ARRLEN(active_stack) - 1) {
 		WARN("Active stack overflow");
 		return;
 	}
+
 	active_top += 1;
 	active_stack[active_top].layer = layer;
 	active_stack[active_top].key = key;
+
+	for (i = 0; i <= active_top; i++) {
+		INFO("%i. ACTIVE %u %u", i,
+			active_stack[i].layer, active_stack[i].key);
+	}
 }
 
 void
 add_keycode(uint8_t keycode)
 {
-	if (hid_report_len >= 6) {
+	if (hid_report.codecnt >= 6) {
 		WARN("HID report overflow");
 		return;
 	}
 
-	hid_report[hid_report_len] = keycode;
-	hid_report_len++;
+	hid_report.codes[hid_report.codecnt] = keycode;
+	hid_report.codecnt++;
 }
 
-void
-handle_keypress(uint x, uint y)
+uint8_t
+parse_modifiers(uint32_t keysym)
 {
-	uint32_t ksym;
-	int i;
+	uint8_t mods;
 
-	if (!keymat_prev[y][x]) {
-		for (i = (int) active_top; i >= 0; i--) {
-			ksym = keymap_layers[active_stack[i].layer][y][x];
-			if (ksym == KC_NO) return;
-			if (ksym != KC_TRNS)
-				break;
-		}
-		if (i < 0) return;
-		keysyms[y][x] = ksym;
-
-		if (IS_SWITCH(keysyms[y][x])) {
-			INFO("LAYER %u", TO_LAYER(keysyms[y][x]));
-			active_push(TO_LAYER(ksym), y * KEY_COLS + x);
-			for (i = 0; i <= (int) active_top; i++) {
-				INFO("%i. ACTIVE %u %u", i,
-					active_stack[i].layer, active_stack[i].key);
-			}
-		}
-	}
+	mods = 0;
 
-	if (!seen_mat[y][x]) {
-		if (IS_CTRL(keysyms[y][x])) {
-			if (IS_RIGHT(keysyms[y][x])) {
-				add_keycode(KC_RIGHT_CTRL);
-			} else {
-				add_keycode(KC_LEFT_CTRL);
-			}
-		}
+	if (IS_LEFT_CTRL(keysym))
+		mods |= MOD_BIT(KC_LEFT_CTRL);
 
-		if (IS_SHIFT(keysyms[y][x])) {
-			if (IS_RIGHT(keysyms[y][x])) {
-				add_keycode(KC_RIGHT_SHIFT);
-			} else {
-				add_keycode(KC_LEFT_SHIFT);
-			}
-		}
+	if (IS_RIGHT_CTRL(keysym))
+		mods |= MOD_BIT(KC_RIGHT_CTRL);
 
-		if (IS_ALT(keysyms[y][x])) {
-			if (IS_RIGHT(keysyms[y][x])) {
-				add_keycode(KC_RIGHT_ALT);
-			} else {
-				add_keycode(KC_LEFT_ALT);
-			}
-		}
+	if (IS_LEFT_SHIFT(keysym))
+		mods |= MOD_BIT(KC_LEFT_SHIFT);
 
-		if (IS_GUI(keysyms[y][x])) {
-			if (IS_RIGHT(keysyms[y][x])) {
-				add_keycode(KC_RIGHT_GUI);
-			} else {
-				add_keycode(KC_LEFT_GUI);
-			}
-		}
+	if (IS_RIGHT_SHIFT(keysym))
+		mods |= MOD_BIT(KC_RIGHT_SHIFT);
 
-		if (IS_CODE(keysyms[y][x])) {
-			add_keycode(TO_CODE(keysyms[y][x]));
-			INFO("CODE %u %u", active_top, keysyms[y][x]);
-		}
+	if (IS_LEFT_GUI(keysym))
+		mods |= MOD_BIT(KC_LEFT_GUI);
+
+	if (IS_RIGHT_GUI(keysym))
+		mods |= MOD_BIT(KC_RIGHT_GUI);
+
+	if (IS_LEFT_ALT(keysym))
+		mods |= MOD_BIT(KC_LEFT_ALT);
+
+	if (IS_RIGHT_ALT(keysym))
+		mods |= MOD_BIT(KC_RIGHT_ALT);
+
+	return mods;
+}
+
+void
+handle_keypress_new(uint x, uint y)
+{
+	uint32_t ksym;
+	int i;
 
-		seen_mat[y][x] = true;
+	for (i = (int) active_top; i >= 0; i--) {
+		ksym = keymap_layers[active_stack[i].layer][y][x];
+		if (ksym == KC_NO) return;
+		if (ksym != KC_TRNS)
+			break;
+	}
+	if (i < 0) return;
+	keysyms[y][x] = ksym;
+
+	if (IS_SWITCH(keysyms[y][x])) {
+		active_push(TO_LAYER(keysyms[y][x]), y * KEY_COLS + x);
+	} else if (IS_USER(keysyms[y][x])) {
+		process_user_keypress_new(TO_SYM(keysyms[y][x]), x, y);
+	} else if (IS_KC(keysyms[y][x]) && IS_KEY_KC(TO_KC(keysyms[y][x]))) {
+		/* FIXME: two keys pressed at the exact same time with
+		 * different weak modifiers will not be reported correctly */
+		active_weak_mods = parse_modifiers(keysyms[y][x]);
 	}
 }
 
 void
-handle_keyrelease(uint x, uint y)
+handle_keypress(uint x, uint y)
+{
+	if (!keymat_prev[y][x])
+		handle_keypress_new(x, y);
+
+	if (seen_mat[y][x]) return;
+	seen_mat[y][x] = true;
+
+	if (IS_KC(keysyms[y][x]) && IS_KEY_KC(TO_KC(keysyms[y][x]))) {
+		add_keycode(TO_KC(keysyms[y][x]));
+		INFO("CODE %u %u", active_top, keysyms[y][x]);
+	} else if (IS_KC(keysyms[y][x]) && IS_MOD_KC(TO_KC(keysyms[y][x]))) {
+		active_mods |= MOD_BIT(TO_KC(keysyms[y][x]));
+	} else if (IS_MOD(keysyms[y][x])) {
+		active_mods |= parse_modifiers(keysyms[y][x]);
+	}
+}
+
+void
+handle_keyrelease_new(uint x, uint y)
 {
 	uint i;
 
-	if (keymat_prev[y][x]) {
-		for (i = 1; i <= active_top; i++) {
-			if (active_stack[i].key == y * KEY_COLS + x) {
-				active_pop(i);
-				break;
-			}
+	if (IS_USER(keysyms[y][x]))
+		process_user_keyrelease_new(TO_SYM(keysyms[y][x]), x, y);
+
+	for (i = 1; i <= active_top; i++) {
+		if (active_stack[i].key == y * KEY_COLS + x) {
+			active_pop(i);
+			break;
 		}
 	}
 }
 
-bool
+void
+handle_keyrelease(uint x, uint y)
+{
+	if (keymat_prev[y][x])
+		handle_keyrelease_new(x, y);
+}
+
+void
 update_report(void)
 {
 	uint64_t now_us;
-	uint keycnt;
 	uint x, y;
 
-	keycnt = 0;
 	now_us = time_us_64();
 	for (y = 0; y < KEY_ROWS; y++) {
 		for (x = 0; x < KEY_COLS; x++) {
 			if (keymat[y][x] != keymat_prev[y][x]) {
 				if (bounce_mat[y][x] > now_us - 50000) {
-					WARN("Bouncing prevented %i vs %i",
+					DEBUG("Bouncing prevented %i vs %i",
 						keymat[y][x], keymat_prev[y][x]);
 					keymat[y][x] = keymat_prev[y][x];
 				} else {
@@ -178,24 +212,16 @@ update_report(void)
 			}
 		}
 	}
-
-	return keycnt > 0;
-}
-
-void
-hid_init(void)
-{
 }
 
 bool
 send_keyboard_report(void)
 {
-	uint i;
-	if (memcmp(hid_report, hid_report_prev, sizeof(hid_report))) {
-		for (i = 0; i < 6; i++)
-			INFO("REPORT %u: %u", i, hid_report[i]);
-		tud_hid_keyboard_report(REPORT_ID_KEYBOARD, 0, hid_report);
-		memcpy(hid_report_prev, hid_report, sizeof(hid_report));
+	hid_report.mods = active_weak_mods | active_mods;
+	if (memcmp(&hid_report, &hid_report_prev, sizeof(hid_report))) {
+		tud_hid_keyboard_report(REPORT_ID_KEYBOARD,
+			hid_report.mods, hid_report.codes);
+		memcpy(&hid_report_prev, &hid_report, sizeof(hid_report));
 		return true;
 	}
 
@@ -262,13 +288,33 @@ tud_hid_report_complete_cb(uint8_t instance,
 }
 
 void
+hid_init(void)
+{
+}
+
+void
+hid_force_release(uint x, uint y)
+{
+	handle_keyrelease_new(x, y);
+	keysyms[y][x] = KC_NO;
+}
+
+void
+hid_switch_layer_with_key(uint8_t layer, uint x, uint y)
+{
+	active_push(layer, y * KEY_COLS + x);
+	keysyms[y][x] = SW(layer);
+	seen_mat[y][x] = true;
+}
+
+void
 hid_task(void)
 {
 	update_report();
 	if (tud_hid_ready()) {
 		send_hid_report(REPORT_ID_MIN);
-		memset(hid_report, 0, sizeof(hid_report));
+		memset(&hid_report, 0, sizeof(hid_report));
 		memset(seen_mat, 0, sizeof(seen_mat));
-		hid_report_len = 0;
+		active_mods = 0;
 	}
 }
diff --git a/src/hid.h b/src/hid.h
index 79e0348..e455c80 100644
--- a/src/hid.h
+++ b/src/hid.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "keymat.h"
+
 #include <stdbool.h>
 #include <stdint.h>
 
@@ -12,4 +14,6 @@ enum {
 };
 
 void hid_init(void);
+void hid_force_release(uint x, uint y);
+void hid_switch_layer_with_key(uint8_t layer, uint x, uint y);
 void hid_task(void);
diff --git a/src/keycode.h b/src/keycode.h
index 98d5831..afd4378 100644
--- a/src/keycode.h
+++ b/src/keycode.h
@@ -8,14 +8,14 @@
  * or http://www.usb.org/developers/hidpage/Hut1_12v2.pdf (older)
  */
 
-#define IS_ERROR(code) (KC_ROLL_OVER <= (code) && (code) <= KC_UNDEFINED)
-#define IS_ANY(code) (KC_A <= (code) && (code) <= 0xFF)
-#define IS_KEY(code) (KC_A <= (code) && (code) <= KC_EXSEL)
-#define IS_MOD(code) (KC_LEFT_CTRL <= (code) && (code) <= KC_RIGHT_GUI)
+#define IS_ANY_KC(code) (KC_A <= (code) && (code) <= 0xFF)
+#define IS_ERROR_KC(code) (KC_ROLL_OVER <= (code) && (code) <= KC_UNDEFINED)
+#define IS_KEY_KC(code) (KC_A <= (code) && (code) <= KC_EXSEL)
+#define IS_MOD_KC(code) (KC_LEFT_CTRL <= (code) && (code) <= KC_RIGHT_GUI)
 
-// #define IS_SPECIAL(code) ((0xA5 <= (code) && (code) <= 0xDF) || (0xE8 <= (code) && (code) <= 0xFF))
-// #define IS_SYSTEM(code) (KC_PWR <= (code) && (code) <= KC_WAKE)
-// #define IS_CONSUMER(code) (KC_MUTE <= (code) && (code) <= KC_BRID)
+#define IS_SPECIAL_KC(code) ((0xA5 <= (code) && (code) <= 0xDF) || (0xE8 <= (code) && (code) <= 0xFF))
+#define IS_SYSTEM_KC(code) (KC_PWR <= (code) && (code) <= KC_WAKE)
+#define IS_CONSUMER_KC(code) (KC_MUTE <= (code) && (code) <= KC_BRID)
 
 #define IS_MOUSEKEY(code) (KC_MS_UP <= (code) && (code) <= KC_MS_ACCEL2)
 #define IS_MOUSEKEY_MOVE(code) (KC_MS_UP <= (code) && (code) <= KC_MS_RIGHT)
@@ -23,6 +23,25 @@
 #define IS_MOUSEKEY_WHEEL(code) (KC_MS_WH_UP <= (code) && (code) <= KC_MS_WH_RIGHT)
 #define IS_MOUSEKEY_ACCEL(code) (KC_MS_ACCEL0 <= (code) && (code) <= KC_MS_ACCEL2)
 
+#define MOD_BIT(code) (1 << MOD_INDEX(code))
+#define MOD_INDEX(code) ((code) & 0b111)
+
+#define MOD_MASK_CTRL (MOD_BIT(KC_LEFT_CTRL) | MOD_BIT(KC_RIGHT_CTRL))
+#define MOD_MASK_SHIFT (MOD_BIT(KC_LEFT_SHIFT) | MOD_BIT(KC_RIGHT_SHIFT))
+#define MOD_MASK_ALT (MOD_BIT(KC_LEFT_ALT) | MOD_BIT(KC_RIGHT_ALT))
+#define MOD_MASK_GUI (MOD_BIT(KC_LEFT_GUI) | MOD_BIT(KC_RIGHT_GUI))
+#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)
+
 /* Short names for ease of definition of keymap */
 
 /* Transparent */
diff --git a/src/keymap.c b/src/keymap.c
index 8e933c3..68c20a4 100644
--- a/src/keymap.c
+++ b/src/keymap.c
@@ -2,6 +2,7 @@
 #include "keycode.h"
 #include "keysym.h"
 #include "keysym_de.h"
+#include "hid.h"
 #include "board.h"
 #include "util.h"
 
@@ -25,9 +26,22 @@
 		{ 0x0, 0x0, 0x0, K83, K82, K81 }  \
 	}
 
+/* KEY INDEX LOOKUP:
+ * x0y0    , x1y0    , x2y0    , x3y0    , x4y0    , x5y0    ,
+ * x0y1    , x1y1    , x2y1    , x3y1    , x4y1    , x5y1    ,
+ * x0y2    , x1y2    , x2y2    , x3y2    , x4y2    , x5y2    ,
+ *                               x3y3    , x4y3    , x5y3    ,
+ *
+ * x5y4    , x4y4    , x3y4    , x2y4    , x1y4    , x0y4    ,
+ * x5y5    , x4y5    , x3y5    , x2y5    , x1y5    , x0y5    ,
+ * x5y6    , x4y6    , x3y6    , x2y6    , x1y6    , x0y6    ,
+ * x5y7    , x4y7    , x3y7
+ */
+
+
 #define LAYER_BASE_DE KEYMAP(                                       \
 	KC_ESC  , DE_Q    , DE_W    , DE_F    , DE_P    , DE_B    , \
-	SW(QUIK), DE_A    , DE_R    , DE_S    , DE_T    , DE_G    , \
+	SX(QUSW), DE_A    , DE_R    , DE_S    , DE_T    , DE_G    , \
 	KC_LSFT , DE_Z    , DE_X    , DE_C    , DE_D    , DE_V    , \
 	                              KC_LGUI , KC_LALT , SW(SHRT), \
 	                                                            \
@@ -63,7 +77,7 @@
 
 #define LAYER_SHRT_DE KEYMAP(                                       \
 	_______ , G(KC_1) , G(KC_2) , G(KC_3) , G(KC_4) , G(KC_5) , \
-	_______ , A(KC_1) , A(KC_2) , A(KC_3) , A(KC_4) , A(KC_5) , \
+	CS(BASE), A(KC_1) , A(KC_2) , A(KC_3) , A(KC_4) , A(KC_5) , \
 	_______ ,G(KC_TAB),G(DE_DOT), A(DE_B) , A(DE_F) ,A(KC_SPC), \
 	                              _______ , _______ , _______ , \
 	                                                            \
@@ -122,7 +136,8 @@ enum {
 
 enum {
 	KVM1,
-	KVM2
+	KVM2,
+	QUSW,
 };
 
 const uint32_t keymap_layers_de[][KEY_ROWS][KEY_COLS] = {
@@ -139,3 +154,30 @@ const uint32_t keymap_layers_de_count = ARRLEN(keymap_layers_de);
 
 const uint32_t (*keymap_layers)[KEY_ROWS][KEY_COLS] = keymap_layers_de;
 uint32_t keymap_layers_count = keymap_layers_de_count;
+
+void
+process_user_keypress_new(uint8_t sym, uint x, uint y)
+{
+	switch (sym) {
+	case KVM1:
+		break;
+	case KVM2:
+		break;
+	case QUSW:
+		INFO("Handling quick switch %i", keymat[7][3]);
+		if (keymat[7][3]) {
+			hid_force_release(3, 7);
+			hid_switch_layer_with_key(QUIX, x, y);
+		} else {
+			hid_switch_layer_with_key(QUIK, x, y);
+		}
+		break;
+	}
+
+}
+
+void
+process_user_keyrelease_new(uint8_t sym, uint x, uint y)
+{
+
+}
diff --git a/src/keymap.h b/src/keymap.h
index d8a5755..7f78e8b 100644
--- a/src/keymap.h
+++ b/src/keymap.h
@@ -2,8 +2,13 @@
 
 #include "keymat.h"
 
+#include "pico/types.h"
+
 #include <stdint.h>
 
+void process_user_keypress_new(uint8_t sym, uint x, uint y);
+void process_user_keyrelease_new(uint8_t sym, uint x, uint y);
+
 extern const uint32_t keymap_layers_de[][KEY_ROWS][KEY_COLS];
 extern const uint32_t keymap_layers_de_count;
 
diff --git a/src/keysym.h b/src/keysym.h
index 17e5e87..7737284 100644
--- a/src/keysym.h
+++ b/src/keysym.h
@@ -7,7 +7,7 @@
 
 #define MASK(hi, lo) ((1U << (hi)) - (1U << (lo)))
 
-#define IS_CODE(x)    (!((x) & ~MASK(B_TOGGLE, 0)))
+#define IS_KC(x)    (!((x) & ~MASK(B_TOGGLE, 0)))
 #define IS_CTRL(x)    ((x) & (1 << B_CTRL))
 #define IS_SHIFT(x)   ((x) & (1 << B_SHIFT))
 #define IS_ALT(x)     ((x) & (1 << B_ALT))
@@ -15,10 +15,21 @@
 #define IS_RIGHT(x)   ((x) & (1 << B_RIGHT))
 #define IS_TOGGLE(x)  ((x) & (1 << B_TOGGLE))
 #define IS_SWITCH(x)  ((x) & (1 << B_SWITCH))
-#define IS_MODSWT(x)  ((x) & (1 << B_MODSWT))
-#define IS_SPECIAL(x) ((x) & (1 << B_SPECIAL))
+#define IS_USER(x)    ((x) & (1 << B_USER))
 
-#define TO_CODE(x)  ((x) & 0xFF)
+#define IS_LEFT_CTRL(x)   (IS_CTRL(x) && !IS_RIGHT(x))
+#define IS_RIGHT_CTRL(x)  (IS_CTRL(x) && IS_RIGHT(x))
+#define IS_LEFT_SHIFT(x)  (IS_SHIFT(x) && !IS_RIGHT(x))
+#define IS_RIGHT_SHIFT(x) (IS_SHIFT(x) && IS_RIGHT(x))
+#define IS_LEFT_ALT(x)    (IS_ALT(x) && !IS_RIGHT(x))
+#define IS_RIGHT_ALT(x)   (IS_ALT(x) && IS_RIGHT(x))
+#define IS_LEFT_GUI(x)    (IS_GUI(x) && !IS_RIGHT(x))
+#define IS_RIGHT_GUI(x)   (IS_GUI(x) && IS_RIGHT(x))
+
+#define IS_MOD(x) ((x) & MASK(B_TOGGLE, 8))
+
+#define TO_KC(x)    ((x) & 0xFF)
+#define TO_SYM(x)   ((x) & 0xFF)
 #define TO_LAYER(x) ((x) & 0xFF)
 
 #define LCTL(x) ((x) | (1 << B_CTRL))
@@ -32,8 +43,7 @@
 
 #define SW(x) ((x) | (1 << B_SWITCH))
 #define TO(x) ((x) | (1 << B_TOGGLE))
-#define MO(x) ((x) | (1 << B_MODSWT))
-#define SX(x) ((x) | (1 << B_SPECIAL))
+#define SX(x) ((x) | (1 << B_USER))
 
 #define LOPT(x) LALT(x)
 #define LCMD(x) LGUI(x)
@@ -47,8 +57,8 @@
 #define A(x) LALT(x)
 #define G(x) LGUI(x)
 
-#define CS(x) C(MO(x))
-#define GS(x) G(MO(x))
+#define CS(x) C(SW(x))
+#define GS(x) G(SW(x))
 
 enum {
 	B_CTRL = 8,
@@ -58,7 +68,6 @@ enum {
 	B_RIGHT,
 	B_TOGGLE,
 	B_SWITCH,
-	B_MODSWT,
-	B_SPECIAL
+	B_USER
 };
 
diff --git a/src/usb_descriptors.c b/src/usb_descriptors.c
index f55a0c5..97a525b 100644
--- a/src/usb_descriptors.c
+++ b/src/usb_descriptors.c
@@ -73,7 +73,7 @@ uint8_t const desc_fs_configuration[] = {
 	/* Interface number, string index, protocol, report descriptor len,
 	 * EP In address, size & polling interval */
 	TUD_HID_DESCRIPTOR(ITF_NUM_HID, 5, HID_ITF_PROTOCOL_NONE,
-		sizeof(desc_hid_report), EPNUM_HID, CFG_TUD_HID_EP_BUFSIZE, 5)
+		sizeof(desc_hid_report), EPNUM_HID, CFG_TUD_HID_EP_BUFSIZE, 1)
 };
 
 #if TUD_OPT_HIGH_SPEED