CHIP SN8F2288

//{{SONIX_CODE_OPTION
	; Options for Lenovo Compact Keyboard
	.Code_Option	Fcpu		"Fosc/4"
	.Code_Option	Fslow		"Flosc/2"
	.Code_Option	High_CLK	"12M_X'tal"
	.Code_Option	LVD		"LVD_M"
	.Code_Option	Reset_Pin	"P07"
	.Code_Option	Rst_Length	"No"
	.Code_Option	Security	"Enable"
	.Code_Option	Watch_Dog	"Enable"
//}}SONIX_CODE_OPTION

.DATA
_canary_check EQU 0x2880
_flasher      EQU 0x2890
UTX           EQU P0.6    ; S15
URX           EQU P0.5    ; S10
S0            EQU P2.2
S0M           EQU P2M.2
S1            EQU P2.0
S1M           EQU P2M.0
S2            EQU P4.0
S2M           EQU P4M.0
S3            EQU P4.5
S3M           EQU P4M.5
S4            EQU P4.1
S4M           EQU P4M.1
S5            EQU P2.1
S5M           EQU P2M.1
S6            EQU P4.4
S6M           EQU P4M.4
S7            EQU P4.2
S7M           EQU P4M.2
S8            EQU P4.3
S8M           EQU P4M.3
S9            EQU P2.3
S9M           EQU P2M.3
S10           EQU P0.5
S10M          EQU P0M.5
S11           EQU P0.4
S11M          EQU P0M.4
S12           EQU P4.6
S12M          EQU P4M.6
S13           EQU P4.7
S13M          EQU P4M.7
S14           EQU P0.3
S14M          EQU P0M.3
S15           EQU P0.6
S15M          EQU P0M.6
R0            EQU P1.0
R0M           EQU P1M.0
R1            EQU P1.7
R1M           EQU P1M.7
R2            EQU P1.2
R2M           EQU P1M.2
R3            EQU P1.1
R3M           EQU P1M.1
R4            EQU P1.4
R4M           EQU P1M.4
R5            EQU P1.3
R5M           EQU P1M.3
R6            EQU P1.5
R6M           EQU P1M.5
R7            EQU P1.6
R7M           EQU P1M.6

EP0_BYTES     EQU 8
EP1_BYTES     EQU 8
EP2_BYTES     EQU 8

bmRequestType  DS 1
bRequest       DS 1
wValueLo       DS 1
wValueHi       DS 1
wIndexLo       DS 1
wIndexHi       DS 1
wLengthLo      DS 1
wLengthHi      DS 1

txPtrLo        DS 1
txPtrHi        DS 1
txSizeLo       DS 1
txSizeHi       DS 1
txPktLen       DS 1
txTmpCnt       DS 1

usbState       DS 1
usbStateSetupInvalid EQU usbState.0
usbStateSetAddress   EQU usbState.1
usbStateIn0Done      EQU usbState.3
usbStateZeroPad      EQU usbState.4
usbStateStringCnt    EQU usbState.5
usbStateCopyRAM      EQU usbState.6
usbStateHTDData      EQU usbState.7

usbState2      DS 1
usbStateAddressValid EQU usbState2.0
usbStateEnterFlasher EQU usbState2.1

xlatVal1       DS 1
xlatVal2       DS 1

kbdRow         DS 1
kbdRowsLow     DS 1
kbdSense1      DS 1
kbdSense2      DS 1


.CODE
ORG 0x0 ; Reset vector
	; Jump to bootloader, checks canary and continues execution at 0x10 if found
	JMP _canary_check

ORG 0x8 ; Interrupt vector
	JMP _flasher

ORG 0x10 ; Bootloader jumps here on successful canary check, start of payload execution
_start:
	; Set stack pointer and disable interrupts
	MOV A, #7
	B0MOV STKP, A

	MOV A, #0
	B0MOV RBANK, A

	; Jump into bootloader if watchdog triggered or undefined reset source
	B0BTS1 FNT0
	JMP _flasher    ; NT0 == 0 => watchdog reset or undefined reason

	; Light up the power LED (P5.3/PWM0)
	B0BCLR P5.3   ; Set to low level to light up
	B0BSET P5M.3  ; Set to output

	; DS indicates watchdog may start running before CPU
	; Tickle it once so we have a well-defined time left
	MOV A, #0x5a
	B0MOV WDTR, A

	; Set up P0.6/UTX (UART TX)
	; FIXME: PnUR is write-only, B0BSET/B0BCLR are likely broken
	B0BSET P0UR.6 ; Enable pull-up (UART idle is high)
	B0BCLR P0M.6  ; Set to input (TXEN will override to output)
	MOV A, #0x60  ; Baud 115200
	B0MOV URBRC, A
	MOV A, #0x90  ; 24MHz clock, TX enabled, 8n1, 1-byte mode
	B0MOV URTX, A

	; Send a message that we are alive
	MOV A, #'H'
	CALL _uart_tx
	MOV A, #'i'
	CALL _uart_tx
	MOV A, #'!'
	CALL _uart_tx
	MOV A, #13
	CALL _uart_tx
	MOV A, #10
	CALL _uart_tx

	; Jump into bootloader if "Return" key (S10/R6) is held
	B0BSET P0M.5 ; Set S10 to output
	B0BCLR P1M.5 ; Set R6 to input
	B0BSET P1UR.5 ; Enable pull-up on R6

	B0BSET S10   ; Set S10 high
	CALL _delayshort
	B0BTS1 R6    ; Jump if R6 is low
	JMP @F
	B0BCLR S10   ; Set S10 low
	CALL _delayshort
	B0BTS1 R6    ; Jump if R6 is low
	JMP _return_held
@@:

	; Jump into bootloader if P0.5/P0.6 are shorted (URX/UTX)
	B0BCLR 0xa9.4 ; Switch UTX back to GPIO
	B0BSET P0M.6 ; Set UTX to output
	B0BCLR P0M.5 ; Set URX to input
	B0BSET P0UR.5 ; Enable pull-up on URX

	B0BSET UTX   ; Set P0.6/UTX high
	CALL _delayshort
	B0BTS1 URX   ; Jump if P0.5/URX is low
	JMP @F
	B0BCLR UTX   ; Set P0.6/UTX low
	CALL _delayshort
	B0BTS1 URX   ; Jump if P0.5/URX is low
	JMP _uart_shorted
@@:
	B0BSET 0xa9.4 ; Switch UTX back to UART


	; Reset either from undervoltage (power-on) or external reset
	; Cold reset state per datasheet:
	; - Clock is 12MHz PLL synced to external oscillator
	; - IOs set to input

	; Setup GPIO
	CALL _gpio_init
	; Setup USB registers
	CALL _usb_init

_mainloop:
	; Tickle watchdog
	MOV A, #0x5a
	B0MOV WDTR, A
@@:
	B0BTS0 FEP0SETUP ; Jump if SETUP packet rx'd
	JMP _usb_setup
	B0BTS0 FEP0OUT
	JMP _usb_ep0_out
	B0BTS0 FEP0IN
	JMP _usb_ep0_in
	B0BTS0 FBUS_RST
	JMP _usb_reset
	B0BTS0 FSOF
	JMP _usb_sof
	JMP @B

_usb_sof:
	B0BCLR FSOF

	; Check for cross-talk from too many depressed keys
	CALL _kbd_count_rows_low
	; Read the columns
	CALL _kbd_sense
	; Release the scanned row
	CALL _kbd_set_row_input
	; Increment row
	INCMS kbdRow
	MOV A, #0x07
	AND kbdRow, A
	; Lower the next row
	CALL _kbd_set_row_output

	B0MOV A, kbdSense1
	OR    A, kbdSense2
	B0BTS0 FZ  ; Jump if kbdSense is zero
	JMP _mainloop

	MOV A, #'K'
	CALL _uart_tx
	B0MOV A, kbdRow
	CALL _uart_hex
	B0MOV A, kbdSense1
	CALL _uart_hex
	B0MOV A, kbdSense2
	CALL _uart_hex

	JMP _mainloop

_usb_reset:
	MOV A, #13
	CALL _uart_tx
	MOV A, #10
	CALL _uart_tx
	MOV A, #'R'
	CALL _uart_tx
	CALL _usb_init
	; Wait until reset is de-asserted
_usb_reset_wait:
	B0BTS0 FBUS_RST
	JMP _usb_reset_wait
	JMP _mainloop

_gpio_init:
	; Enable all pull-ups
	MOV A, #0xff
	B0MOV P0UR, A
	B0MOV P1UR, A
	B0MOV P2UR, A
	B0MOV P4UR, A
	B0MOV P5UR, A
	; Switch all to input
	MOV A, #0x00
	B0MOV P0M, A
	B0MOV P1M, A
	B0MOV P2M, A
	B0MOV P4M, A
	B0MOV P5M, A
	; Set port output data to 0
	B0MOV P0, A
	B0MOV P1, A
	B0MOV P2, A
	B0MOV P4, A
	B0MOV P5, A
	; Reset kbd scan state
	B0MOV kbdRow, A
	; Re-light LED
	B0BCLR P5.3   ; Set to low level to light up
	B0BSET P5M.3  ; Set to output
	;; Set UTX/URX to open-drain
	;MOV A, #0x0c
	;B0MOV P1OC, A
	RET

_usb_init:
	MOV A, #0
	B0MOV usbState, A
	B0MOV usbState2, A
	B0MOV USTATUS, A
	MOV A, #0x80
	B0MOV UDA, A  ; Set address to 0 and enable
	B0MOV UE1R, A ; Enable EP1, set to NAK
	B0MOV UE2R, A ; Enable EP2, set to NAK
	B0BSET FDP_PU_EN ; Enable D+ pull-up
	B0BSET FSOF_INT_EN ; Enable SOF interrupt request
	RET

_kbd_sense:
	MOV A, #0
	B0MOV kbdSense1, A
	B0MOV kbdSense2, A
	B0BTS1 S0
	B0BSET kbdSense1.0
	B0BTS1 S1
	B0BSET kbdSense1.1
	B0BTS1 S2
	B0BSET kbdSense1.2
	B0BTS1 S3
	B0BSET kbdSense1.3
	B0BTS1 S4
	B0BSET kbdSense1.4
	B0BTS1 S5
	B0BSET kbdSense1.5
	B0BTS1 S6
	B0BSET kbdSense1.6
	B0BTS1 S7
	B0BSET kbdSense1.7
	B0BTS1 S8
	B0BSET kbdSense2.0
	B0BTS1 S9
	B0BSET kbdSense2.1
	B0BTS1 S10
	B0BSET kbdSense2.2
	B0BTS1 S11
	B0BSET kbdSense2.3
	B0BTS1 S12
	B0BSET kbdSense2.4
	B0BTS1 S13
	B0BSET kbdSense2.5
	B0BTS1 S14
	B0BSET kbdSense2.6
	B0BTS1 S15
	B0BSET kbdSense2.7
	RET

_kbd_set_row_output:
	B0MOV A, kbdRow
	AND A, #0x7
	MOV R, A
	ADD A, R
	B0ADD PCL, A
	B0BSET R0M
	RET
	B0BSET R1M
	RET
	B0BSET R2M
	RET
	B0BSET R3M
	RET
	B0BSET R4M
	RET
	B0BSET R5M
	RET
	B0BSET R6M
	RET
	B0BSET R7M
	RET

_kbd_set_row_input:
	B0MOV A, kbdRow
	AND A, #0x7
	MOV R, A
	ADD A, R
	B0ADD PCL, A
	B0BCLR R0M
	RET
	B0BCLR R1M
	RET
	B0BCLR R2M
	RET
	B0BCLR R3M
	RET
	B0BCLR R4M
	RET
	B0BCLR R5M
	RET
	B0BCLR R6M
	RET
	B0BCLR R7M
	RET

_kbd_count_rows_low:
	MOV A, #0
	B0MOV kbdRowsLow, A
	B0BTS1 R0
	INCMS kbdRowsLow
	B0BTS1 R1
	INCMS kbdRowsLow
	B0BTS1 R2
	INCMS kbdRowsLow
	B0BTS1 R3
	INCMS kbdRowsLow
	B0BTS1 R4
	INCMS kbdRowsLow
	B0BTS1 R5
	INCMS kbdRowsLow
	B0BTS1 R6
	INCMS kbdRowsLow
	B0BTS1 R7
	INCMS kbdRowsLow
	NOP
	RET

_usb_ep0_in:
	B0BCLR FEP0IN ; Early ack of IN irq, we can't get a new one until EP0 NAK state is cleared
	MOV A, #'i'
	CALL _uart_tx
	CALL _usb_write_ep0
	B0BTS0 usbStateSetAddress
	JMP _usb_ep0_set_addr
	JMP _mainloop

_usb_ep0_set_addr:
	B0BSET wValueLo.7
	B0MOV A, wValueLo
	B0MOV UDA, A ; Update address register
	MOV A, #'A'
	CALL _uart_tx
	B0MOV A, wValueLo
	CALL _uart_hex
	JMP _mainloop

_usb_ep0_out:
	B0BCLR FEP0OUT ; Ack OUT irq

	MOV A, #'o'
	CALL _uart_tx

	B0MOV A, EP0OUT_CNT
	B0BTS1 FZ ; Call if non-zero
	CALL _uart_hex

	B0BTS0 usbStateHTDData
	JMP _usb_setup_htd_got_data
	JMP _mainloop

_setup_dispatch_table:
	DW 0x0005  ; SET_ADDRESS
	JMP _usb_htd_set_address
	DW 0x0009  ; SET_CONFIGURATION
	JMP _usb_htd_set_configuration
	DW 0x2109  ; HID SET_REPORT
	JMP _usb_htd_hid_set_report
	DW 0x210a  ; HID SET_IDLE
	JMP _usb_htd_hid_set_idle
	DW 0x210b  ; HID SET_PROTOCOL
	JMP _usb_setup_default
	DW 0x8000  ; GET_STATUS
	JMP _usb_dth_get_status
	DW 0x8006  ; GET_DESCRIPTOR (device)
	JMP _usb_dth_get_device_descriptor
	DW 0x8106  ; GET_DESCRIPTOR (interface)
	JMP _usb_dth_get_interface_descriptor
	DW 0xa101  ; HID GET_REPORT
	JMP _usb_dth_hid_get_report
	DW 0xa102  ; HID GET_IDLE
	JMP _usb_setup_default
	DW 0xa103  ; HID GET_PROTOCOL
	JMP _usb_setup_default
	DW 0xFFFF
	JMP _usb_setup_default

_usb_setup_default:
	B0BSET usbStateSetupInvalid
	MOV A, #'?'
	CALL _uart_tx
	B0MOV A, bmRequestType
	CALL _uart_hex
	B0MOV A, bRequest
	CALL _uart_hex
	RET

_usb_dth_get_device_descriptor:
	B0MOV A, wValueHi
	ADD A, #0xfc
	B0BTS0 FC
	JMP _usb_get_desc_badidx
	B0MOV A, wValueHi
	B0ADD PCL, A
	JMP _usb_get_desc_badidx
	JMP _usb_get_desc_device
	JMP _usb_get_desc_config
	JMP _usb_get_desc_string

_usb_dth_get_interface_descriptor:
	B0MOV A, wValueHi
	SUB A, #0x20
	ADD A, #0xfc
	B0BTS0 FC
	JMP _usb_get_desc_badidx
	B0MOV A, wValueHi
	SUB A, #0x20
	B0ADD PCL, A
	JMP _usb_get_desc_badidx
	JMP _usb_get_desc_hid
	JMP _usb_get_desc_report
	JMP _usb_get_desc_physical

_usb_get_desc_badidx:
	MOV A, #'d'
	CALL _uart_tx
	B0MOV A, wValueHi
	CALL _uart_hex
	B0MOV A, wValueLo
	CALL _uart_hex
	B0BSET usbStateSetupInvalid
	RET

_device_descriptor:
	DB  0x12       ; bLength
	DB  1          ; bDescriptorType (DEVICE)
	DB  0x00, 0x02 ; bcdUSB (2.00)
	DB  0, 0, 0    ; Class/Subclass/Protocol
	DB  EP0_BYTES  ; EP0 max packet size
	DB  0xef, 0x17 ; Vendor 0x17ef (Lenovo)
	DB  0x47, 0x60 ; Device 0x6047 (Lenovo ThinkPad Compact Keyboard with TrackPoint)
	DB  0x00, 0x01 ; bcdDevice (1.0)
	DB  1          ; iManufacturer (String 1)
	DB  2          ; iProduct (String 2)
	DB  0          ; iSerial (n/a)
	DB  1          ; bNumConfigurations (1)
_device_descriptor_end:

_configuration_descriptor:
	DB  9          ; bLength
	DB  2          ; bDescriptorType (CONFIGURATION)
	DB  0x32, 0x00 ; wTotalLength
	DB  2          ; bNumInterfaces
	DB  1          ; bConfigurationValue
	DB  0          ; iConfiguration (n/a)
	DB  0xa0       ; bmAttributes (BUS_POWERED, REMOTE_WAKEUP)
	DB  50         ; bMaxPower (100mA)

	; Keyboard interface
	DB  9          ; bLength
	DB  4          ; bDescriptorType (INTERFACE)
	DB  0          ; bInterfaceNumber
	DB  0          ; bAlternateSetting
	DB  1          ; bNumEndpoints
	DB  3          ; bInterfaceClass (HID)
	DB  1          ; bInterfaceSubClass (boot interface)
	DB  1          ; bInterfaceProtocol (keyboard)
	DB  0          ; iInterface (n/a)

	DB  9          ; bLength
	DB  0x21       ; bDescriptorType (HID)
	DB  0x00, 0x01 ; bcdHID (1.00)
	DB  0          ; bCountryCode (Not Supported)
	DB  1          ; bNumDescriptors
	DB  0x22       ; bDescriptorType (Report)
	DB  63, 0      ; wDescriptorLength (63)

	DB  7          ; bLength
	DB  5          ; bDescriptorType (ENDPOINT)
	DB  0x81       ; bEndpointAddress (EP1 IN)
	DB  0x03       ; bmAttributes (Interrupt, Data)
	DB  0x3f, 0x00 ; wMaxPacketSize (63 bytes)
	DB  10         ; bInterval (10ms)

	; Mouse interface
	DB  9          ; bLength
	DB  4          ; bDescriptorType (INTERFACE)
	DB  1          ; bInterfaceNumber
	DB  0          ; bAlternateSetting
	DB  1          ; bNumEndpoints
	DB  3          ; bInterfaceClass (HID)
	DB  1          ; bInterfaceSubClass (boot interface)
	DB  2          ; bInterfaceProtocol (mouse)
	DB  0          ; iInterface (n/a)

	; FIXME: Add Mouse HID descriptor once Keyboard is working.

	DB  7          ; bLength
	DB  5          ; bDescriptorType (ENDPOINT)
	DB  0x82       ; bEndpointAddress (EP2 IN)
	DB  0x03       ; bmAttributes (Interrupt, Data)
	DB  0x3f, 0x00 ; wMaxPacketSize (63 bytes)
	DB  10         ; bInterval (10ms)
_configuration_descriptor_end:

_string_langids:
	DB 4, 3, 9, 4
_string_mfg:
	DB 14, 3
	DW "Lenovo"
_string_product:
	DB 92, 3
	DW "ThinkPad Compact USB Keyboard with TrackPoint"

_usb_status_ok:
	DW 0

_kbd_hid_descriptor:
	DB 9           ; bLength
	DB 0x21        ; bDescriptorType (HID)
	DB 0x00, 0x01  ; bcdHID (1.00)
	DB 0           ; bCountryCode (Not Supported)
	DB 1           ; bNumDescriptors
	DB 0x22        ; bDescriptorType (Report)
	DB 63, 0       ; wDescriptorLength (63)
_kbd_hid_descriptor_end:

_kbd_report_descriptor:
	; Boot-compatible descriptor
	DB 0x05, 1     ; Usage Page (Generic Desktop)
	DB 0x09, 6     ; Usage (Keyboard)
	DB 0xa1, 1     ; Collection (Application)

	; Modifier keys
	DB 0x05, 7     ;   Usage Page (Key Codes)
	DB 0x19, 224   ;   Usage Minimum (224)
	DB 0x29, 231   ;   Usage Maximum (231)
	DB 0x15, 0     ;   Logical Minimum (0)
	DB 0x25, 1     ;   Logical Minimum (1)
	DB 0x75, 1     ;   Report Size (1)
	DB 0x95, 8     ;   Report Count (8)
	DB 0x81, 2     ;   Input (Data, Variable, Absolute)

	; Reserved byte
	DB 0x95, 1     ;   Report Count (1)
	DB 0x75, 8     ;   Report Size (8)
	DB 0x81, 1     ;   Input (Constant, Absolute)

	; LED byte (5 bits + 3 bits padding)
	DB 0x95, 5     ;   Report Count (5)
	DB 0x75, 1     ;   Report Size (1)
	DB 0x05, 8     ;   Usage Page (LEDs)
	DB 0x19, 1     ;   Usage Minimum (1)
	DB 0x29, 5     ;   Usage Maximum (5)
	DB 0x91, 2     ;   Output (Data, Variable, Absolute)
	DB 0x95, 1     ;   Report Count (1)
	DB 0x75, 3     ;   Report Size (3)
	DB 0x91, 1     ;   Output (Data, Variable, Absolute)

	; 6 key code bytes
	DB 0x95, 6     ;   Report Count (6)
	DB 0x75, 8     ;   Report Size (8)
	DB 0x15, 0     ;   Logical Minimum (0)
	DB 0x25, 175   ;   Logical Maximum (175)
	DB 0x05, 7     ;   Usage Page (Key Codes)
	DB 0x19, 0     ;   Usage Minimum (0)
	DB 0x29, 175   ;   Usage Maximum (175)
	DB 0x81, 0     ;   Input (Data, Array)

	DB 0xc0        ; End Collection (Application)
_kbd_report_descriptor_end:

_clamp_size:
	B0MOV A, wLengthLo
	SUB   A, txSizeLo
	B0MOV A, wLengthHi
	SBC   A, txSizeHi
	B0BTS0 FC ; Return if carry set
	RET
	B0MOV A, wLengthLo
	B0MOV txSizeLo, A
	B0MOV A, wLengthHi
	B0MOV txSizeHi, A
	RET

_usb_get_desc_device:
	MOV A, #'D'
	CALL _uart_tx
	MOV A, #_device_descriptor$M
	B0MOV txPtrHi, A
	MOV A, #_device_descriptor$L
	B0MOV txPtrLo, A
	MOV   A, #0x12
	B0MOV txSizeLo, A
_usb_get_desc:
	CALL _clamp_size
	CALL _usb_write_ep0
	RET

_usb_get_desc_config:
	MOV A, #'C'
	CALL _uart_tx
	MOV A, #_configuration_descriptor$M
	B0MOV txPtrHi, A
	MOV A, #_configuration_descriptor$L
	B0MOV txPtrLo, A
	MOV   A, #0x32
	B0MOV txSizeLo, A
	JMP _usb_get_desc

_usb_get_desc_string:
	B0MOV A, wValueLo
	ADD A, #0xfd
	B0BTS0 FC
	JMP _usb_get_desc_badidx
	B0MOV A, wValueLo
	B0ADD PCL, A
	JMP _usb_get_string_langids
	JMP _usb_get_string_mfg
	JMP _usb_get_string_product

_usb_get_string_langids:
	MOV A, #_string_langids$M
	B0MOV txPtrHi, A
	MOV A, #_string_langids$L
	B0MOV txPtrLo, A
	MOV   A, #4
	B0MOV txSizeLo, A
	JMP _usb_get_desc

_usb_get_string_mfg:
	MOV A, #_string_mfg$M
	B0MOV txPtrHi, A
	MOV A, #_string_mfg$L
	B0MOV txPtrLo, A
	MOV   A, #14
	B0MOV txSizeLo, A
	JMP _usb_get_desc

_usb_get_string_product:
	MOV A, #_string_product$M
	B0MOV txPtrHi, A
	MOV A, #_string_product$L
	B0MOV txPtrLo, A
	MOV   A, #92
	B0MOV txSizeLo, A
	JMP _usb_get_desc

_usb_get_desc_hid:
_usb_get_desc_report:
_usb_get_desc_physical:
	MOV A, #_kbd_report_descriptor$M
	B0MOV txPtrHi, A
	MOV A, #_kbd_report_descriptor$L
	B0MOV txPtrLo, A
	MOV   A, #63
	B0MOV txSizeLo, A
	JMP _usb_get_desc

_usb_write_ep0:
	MOV A, #'W'
	CALL _uart_tx

	; Check if this is a 0byte-write and bail out early
	B0MOV A, txSizeLo
	OR    A, txSizeHi
	B0BTS0 FZ ; Jump if zero
	JMP _write_empty_to_ep0

	; Clamp packet byte count to EP0 size
	B0MOV A, txSizeHi
	B0BTS1 FZ ; Jump if not zero
	JMP _write_to_ep0_big
	MOV A, txSizeLo
	B0MOV R, A
	MOV A, #EP0_BYTES
	SUB A, txSizeLo
	B0BTS1 FC ; Skip if carry
_write_to_ep0_big:
	B0MOV R, #EP0_BYTES

_write_to_ep0_copy:
	; Save packet size fo updating UE0R later
	MOV A, R
	B0MOV txPktLen, A
	B0MOV txTmpCnt, A

	; Copy data into EP0 xmit buffer
	MOV A, #0
	B0MOV UDP0, A
_write_to_ep0_copy_loop:
	B0MOV A, txPtrHi
	B0MOV Y, A
	B0MOV A, txPtrLo
	B0MOV Z, A
	MOVC
	B0MOV UDR0_W, A
	INCMS UDP0
	B0MOV A, R
	B0MOV UDR0_W, A
	INCMS UDP0
	INCMS txPtrLo
	JMP @F
	INCMS txPtrHi
@@:
	DECMS txTmpCnt
	JMP @F
	JMP _write_to_ep0_update_len
@@:
	DECMS txTmpCnt
	JMP _write_to_ep0_copy_loop

_write_to_ep0_update_len:
	B0MOV A, txPktLen
	XCH A, txSizeLo
	SUB txSizeLo, A  ; M = A - M
	MOV A, #0
	XCH A, txSizeHi
	SBC txSizeHi, A  ; M = A - M - /C

	B0MOV A, txPktLen
	OR A, #0x20  ; Set to ACK for
	B0MOV UE0R, A

	B0MOV A, txPktLen
	CALL _uart_hex

	B0MOV A, txPktLen
	B0BTS0 FZ ; Skip if not zero
_write_done:
	B0BSET usbStateIn0Done
_write_not_yet_done:
	RET

_write_empty_to_ep0:
	B0MOV txPktLen, A
	B0BTS1 usbStateIn0Done ; Jump if unset
	JMP _write_to_ep0_update_len
	MOV A, #'s'
	CALL _uart_tx
	JMP _write_to_ep0_update_len
	;B0BSET FUE0M1 ; Stall EP0 (FUE0M0 doesn't matter)
	;RET


_usb_dth_get_status:
	MOV A, #_usb_status_ok$M
	B0MOV txPtrHi, A
	MOV A, #_usb_status_ok$L
	B0MOV txPtrLo, A
	MOV   A, #2
	B0MOV txSizeLo, A
	JMP _usb_write_ep0

_usb_htd_set_configuration:
	; Only one configuration, nothing to do.
	MOV A, #0x20  ; ACK with no TX
	B0MOV UE0R, A
	RET

_usb_htd_set_address:
	MOV A, #'a'
	CALL _uart_tx
	B0BSET usbStateSetAddress
	MOV A, #0x20  ; ACK with no TX
	B0MOV UE0R, A
	RET

_usb_htd_hid_set_idle:
	MOV A, #'I'
	CALL _uart_tx
	MOV A, #0x20  ; ACK with no TX
	B0MOV UE0R, A
	RET

_usb_htd_hid_set_report:
	MOV A, #'r'
	CALL _uart_tx
	B0MOV A, EP0OUT_CNT
	CALL _uart_hex

	; First, set the "enter flasher bit"
	B0BSET usbStateEnterFlasher

	; Check each bit against the expected pattern and clear the flag if mismatch
	B0MOV A, EP0OUT_CNT
	CMPRS A, #0x08
	B0BCLR usbStateEnterFlasher

	MOV A, #0
	B0MOV UDP0, A
	B0MOV A, UDR0_R
	CMPRS A, #0xaa
	B0BCLR usbStateEnterFlasher
	CALL _uart_hex

	INCMS UDP0
	B0MOV A, UDR0_R
	CMPRS A, #0x55
	B0BCLR usbStateEnterFlasher
	CALL _uart_hex

	INCMS UDP0
	B0MOV A, UDR0_R
	CMPRS A, #0xa5
	B0BCLR usbStateEnterFlasher
	CALL _uart_hex

	INCMS UDP0
	B0MOV A, UDR0_R
	CMPRS A, #0x5a
	B0BCLR usbStateEnterFlasher
	CALL _uart_hex

	MOV A, #0x20  ; ACK with no TX
	B0MOV UE0R, A

	; Enter flasher if flag is still set
	B0BTS0 usbStateEnterFlasher
	JMP _flasher
	RET

_usb_dth_hid_get_report:
	MOV A, #'g'
	CALL _uart_tx
	; Send last 8 bytes in buffer
	; FIXME: Handle this properly
	MOV A, #0x28
	B0MOV UE0R, A
	RET

_usb_setup:
	MOV A, #0      ; Unstall & NAK EP0 IN/OUT
	B0MOV UE0R, A
	B0MOV txSizeLo, A
	B0MOV txSizeHi, A
	B0MOV usbState, A

	MOV A, #' '
	CALL _uart_tx
	MOV A, #'S'
	CALL _uart_tx

	; Copy setup packet
	MOV A, #8
	B0MOV R, A
	B0MOV Y, #bmRequestType$M
	B0MOV Z, #bmRequestType$L
	MOV A, #0
	B0MOV UDP0, A
@@:
	B0MOV A, UDR0_R
	B0MOV @YZ, A
	INCMS UDP0
	INCMS Z
	DECMS R
	JMP @B
	NOP

	; Log bRequest
	MOV A, #1
	B0MOV UDP0, A
	B0MOV A, UDR0_R
	CALL _uart_hex

	B0BTS0 bmRequestType.7  ; Jump if bit7 set
	JMP _usb_setup_dth

	; host is writing to device, check if there will be OUT with data.
	MOV A, wLengthLo
	OR  A, wLengthHi
	B0BTS0 FZ  ; Jump if length is 0
	JMP _usb_setup_htd_no_data

	B0BSET FUE0M0   ; Change EP0 to ACK, wait for data
	B0BSET usbStateHTDData
	JMP _usb_setup_exit

_usb_setup_htd_got_data:
_usb_setup_htd_no_data:
_usb_setup_dth:
	B0MOV  Y, #_setup_dispatch_table$M
	B0MOV  Z, #_setup_dispatch_table$L

_usb_setup_do_dispatch:
	B0MOV  A, bmRequestType
	B0MOV  R, A
	B0MOV  A, bRequest
	CALL _dispatch

	B0BTS1 usbStateSetupInvalid
	JMP _setup_done_valid

	B0BSET FUE0M1   ; Stall EP0

_setup_done_valid:
	B0BTS1 bmRequestType.7  ; Skip if bit7 set
	B0BSET FUE0M0   ; Unnak EP0 IN for final ACK

_usb_setup_exit:
	B0BCLR FEP0SETUP ; Late ack of SETUP irq
	JMP _mainloop


_dispatch:
	B0MOV  xlatVal1, A
	MOV A, R
	B0MOV  xlatVal2, A
	JMP _xlat_next
_xlat_loop:
	AND    A, R
	CMPRS  A, #0xff   ; Jump if last entry
	JMP @F
	JMP _xlat_do_indirect_jump
@@:
	CALL _inc_yz      ; skip jump target
	CALL _inc_yz
_xlat_next:
	MOVC   ; Read ROM word into R (hi) and A (lo)
	CMPRS  A, xlatVal1 ; Jump if A != xlatVal1
	JMP _xlat_loop
	XCH    A, R
	CMPRS  A, xlatVal2 ; Jump if R != xlatVal2
	JMP _xlat_loop
_xlat_do_indirect_jump:
	CALL _inc_yz
	CALL _jmp_yz
	RET ; never reached, kept for disassembler

_jmp_yz:  ; FIXME: Interrupts must be disabled
	; DS is underspecified, but experimentally it
	; looks like if CALL goes from level 0 to level 1,
	; then the return PC is stored in STK0H/STK0L
	;
	; Level STKPB2 STKPB1 STKPB0 HighByte LowByte
	;     0      1      1      1      n/a     n/a
	;     1      1      1      0    STK0H   STK0L
	;     2      1      0      1    STK1H   STK1L
	;     [...]
	;     6      0      0      1    STK5H   STK5L
	;     7      0      0      0    STK6H   STK6L
	;     8      1      1      1    STK7H   STK7L
	B0MOV A, STKP
	AND   A, #7
	B0ADD PCL, A
	JMP _set_stack_6 ; STKP 0 / Level 7
	JMP _set_stack_5 ; STKP 1 / Level 6
	JMP _set_stack_4 ; STKP 2 / Level 5
	JMP _set_stack_3 ; STKP 3 / Level 4
	JMP _set_stack_2 ; STKP 4 / Level 3
	JMP _set_stack_1 ; STKP 5 / Level 2
	JMP _set_stack_0 ; STKP 6 / Level 1
	JMP _set_stack_7 ; STKP 7 / Level 8 [or 0]
_set_stack_0:
	B0MOV A, Y
	B0MOV STK0H, A
	B0MOV A, Z
	B0MOV STK0L, A
	RET
_set_stack_1:
	B0MOV A, Y
	B0MOV STK1H, A
	B0MOV A, Z
	B0MOV STK1L, A
	RET
_set_stack_2:
	B0MOV A, Y
	B0MOV STK2H, A
	B0MOV A, Z
	B0MOV STK2L, A
	RET
_set_stack_3:
	B0MOV A, Y
	B0MOV STK3H, A
	B0MOV A, Z
	B0MOV STK3L, A
	RET
_set_stack_4:
	B0MOV A, Y
	B0MOV STK4H, A
	B0MOV A, Z
	B0MOV STK4L, A
	RET
_set_stack_5:
	B0MOV A, Y
	B0MOV STK5H, A
	B0MOV A, Z
	B0MOV STK5L, A
	RET
_set_stack_6:
	B0MOV A, Y
	B0MOV STK6H, A
	B0MOV A, Z
	B0MOV STK6L, A
	RET
_set_stack_7:
	B0MOV A, Y
	B0MOV STK7H, A
	B0MOV A, Z
	B0MOV STK7L, A
	RET

_return_held:
	MOV A, #'E'
	CALL _uart_tx
	MOV A, #'r'
	CALL _uart_tx
	JMP _flasher

_uart_shorted:
	B0BSET 0xa9.4 ; Switch UTX back to UART
	MOV A, #'E'
	CALL _uart_tx
	MOV A, #'s'
	CALL _uart_tx
	JMP _flasher

_inc_yz:
	INCMS Z
	JMP @F
	INCMS Y
	RET
@@:
	RET

_uart_hex:
	MOV R, A
	SWAP R
	CALL _uart_nibble
	MOV A, R
	; fall-through

_uart_nibble:
	AND A, #0xf
	ADD A, #0xf6 ; -0xa
	B0BTS0 FC    ; Skip next insn if carry unset
	ADD A, #0x27 ; 'a' - '0' - 0xa
	ADD A, #0x3a ; '0' + 0xa
	; fall-through

_uart_tx:
	B0MOV URTXD1, A
@@:
	B0BTS1 FUTTXIRQ  ; Check if TX is done
	JMP @B
	B0BCLR FUTTXIRQ
	RET

_delayshort:
	MOV A, #0
	B0MOV R, A
@@:
	DECMS R
	JMP @B
	RET

ORG 0x27ff
	DW  0xaaaa          ; canary

ORG _canary_check
	JMP _start

ORG _flasher
	JMP $