From d2841e8d3c8fd81f74f96f9a35f5eef043a0044a Mon Sep 17 00:00:00 2001
From: mooinglemur <mooinglemur@users.noreply.github.com>
Date: Thu, 22 Aug 2024 08:33:56 -0700
Subject: [PATCH] [KERNAL] enable control and persisting of ps2kbd LED state,
 respond to numlock (#353)

* [KERNAL] enable control and persisting of ps2kbd LED state, respond to numlock

* extapi setter must use .X, and for consistency change getter
---
 kernal/drivers/x16/ps2data.s |  9 +++++-
 kernal/drivers/x16/ps2kbd.s  | 60 ++++++++++++++++++++++++++----------
 kernal/x16/extapi.s          |  2 ++
 3 files changed, 53 insertions(+), 18 deletions(-)

diff --git a/kernal/drivers/x16/ps2data.s b/kernal/drivers/x16/ps2data.s
index fbf69f09..3cc3532f 100644
--- a/kernal/drivers/x16/ps2data.s
+++ b/kernal/drivers/x16/ps2data.s
@@ -17,8 +17,11 @@ CMD_GET_PS2DATA_FAST = $43
 PS2DATA_OLD_STYLE = $01
 PS2DATA_NEW_STYLE = $02
 
+LED_NUM_LOCK	= 2
+
 .import i2c_read_byte, i2c_read_first_byte, i2c_direct_read, i2c_read_next_byte, i2c_read_stop, i2c_write_byte
-.import tpmflg
+.import tpmflg, ledstate
+
 .export ps2data_init, ps2data_fetch, ps2data_kbd, ps2data_kbd_count, ps2data_mouse, ps2data_mouse_count
 .export ps2data_keyboard_and_mouse, ps2data_keyboard_only, ps2data_raw
 
@@ -46,6 +49,10 @@ ps2data_init:
 	; Clear keyboard set typematic rate/delay flag
 	stz tpmflg
 
+	; Set internal numlock state to defaults
+	lda #LED_NUM_LOCK
+	sta ledstate
+
 	; Compare SMC firmare version major
 	ldx #I2C_ADDR
 	ldy #CMD_GET_VER1
diff --git a/kernal/drivers/x16/ps2kbd.s b/kernal/drivers/x16/ps2kbd.s
index fb5edfe8..95bda023 100644
--- a/kernal/drivers/x16/ps2kbd.s
+++ b/kernal/drivers/x16/ps2kbd.s
@@ -31,8 +31,8 @@
 .import memory_decompress_internal ; [lzsa]
 
 .export kbd_config, kbd_scan, receive_scancode_resume, keymap, ps2kbd_typematic
-.export MODIFIER_4080
-.export tpmflg
+.export kbd_leds
+.export tpmflg, ledstate
 
 .import extapi, fetch_typematic_from_nvram
 
@@ -69,13 +69,15 @@ kbtmp:  .res 1           ;    meant for exclusive use in kbd_scan
 .segment "KVARSB0"
 
 tpmflg:	.res 1           ;    Set typematic rate/delay flag
-brkflg:	.res 1           ;    PS/2: was key-up event
+ledstate:
+	.res 1
 curkbd:	.res 1           ;    current keyboard layout index
 dk_shift:
 	.res 1
 dk_scan:
 	.res 1
 
+
 .segment "KEYMAP"
 keymap_data:
 	.res TABLE_COUNT*128
@@ -278,7 +280,7 @@ _kbd_scan:
 @1:	ldy tpmflg
 	bne @3
 	inc tpmflg
-	
+
 	pha
 	phx
 	jsr fetch_typematic_from_nvram
@@ -303,13 +305,21 @@ _kbd_scan:
 
 	; Is it Caps Lock down?
 	cmp #KEYCODE_CAPSLOCK
-	bne is_reg_key
+	bne check_numlock
 	pla			; Restore key code from stack
 	bmi @5			; Ignore key up
 	lda shflag
 	eor #MODIFIER_CAPS
 	sta shflag
-	jmp set_caps_led
+	and #MODIFIER_CAPS
+	beq @caps_off
+	lda #LED_CAPS_LOCK
+	tsb ledstate
+	jmp _set_kbd_leds
+@caps_off:
+	lda #LED_CAPS_LOCK
+	trb ledstate
+	jmp _set_kbd_leds
 @5:	rts
 
 is_mod_key:
@@ -331,15 +341,23 @@ mod_key_down:
 	and #((~MODIFIER_TOGGLE_MASK) & $ff)
 	jmp check_charset_switch
 
-is_reg_key:
-	pla 
-
+check_numlock:
+	pla
 	; Ignore key up events
 	bpl :+
 	rts
 
+:	cmp #KEYCODE_NUMLOCK
+	bne is_reg_key
+
+	lda #LED_NUM_LOCK
+	eor ledstate
+	sta ledstate
+	jmp _set_kbd_leds
+
+is_reg_key:
 	; Transfer key code to Y
-:	tay
+	tay
 
 	; Pause/break key?
 	cmp #KEYCODE_PAUSEBRK
@@ -560,18 +578,26 @@ receive_scancode_resume:
 	rts
 
 ;*****************************************
-; SET CAPS LOCK LED
+; GET/SET CAPS/NUM/SCROLL LOCK LED
 ;*****************************************
-set_caps_led:
+kbd_leds:
+	KVARS_START
+	bcc @1
+	ldx ledstate
+	bra @2
+@1:	txa
+	and #(LED_NUM_LOCK + LED_CAPS_LOCK + LED_SCROLL_LOCK)
+	sta ledstate
+	jsr _set_kbd_leds
+@2:	KVARS_END
+	rts
+
+_set_kbd_leds:
 	ldx #I2C_ADDRESS
 	ldy #I2C_KBD_CMD2
 	lda #$ed
 	jsr i2c_write_first_byte
-	lda shflag
-	and #MODIFIER_CAPS			; Caps Lock is bit 2
-	lsr
-	lsr
-	ora #LED_NUM_LOCK			; Num Lock always on
+	lda ledstate
 	jsr i2c_write_next_byte
 	jmp i2c_write_stop
 
diff --git a/kernal/x16/extapi.s b/kernal/x16/extapi.s
index 9db1ccb9..6cdfdff4 100644
--- a/kernal/x16/extapi.s
+++ b/kernal/x16/extapi.s
@@ -11,6 +11,7 @@
 .import led_update
 .import mouse_set_position
 .import scnsiz
+.import kbd_leds
 
 .export extapi
 
@@ -55,3 +56,4 @@ apitbl:
 	.word led_update-1            ; API 11
 	.word mouse_set_position-1    ; API 12
 	.word scnsiz-1                ; API 13
+	.word kbd_leds-1              ; API 14