;KBD routines
;KBDINIT - initializes 8042/8242 PS/2 keyboard controller
;Affects: A, X, Z
;Returns result code in  X:
;0x00 - OK
;0x01 - Controller self test failed
;0x02 - CLK stuck low
;0x03 - CLK stuck high
;0x04 - KBD DATA stuck low
;0x05 - KBD DATA stuck high
;0x06 - Interface didn't pass the test
;0x07 - Keyboard reset failure/no keyboard present
KBDINIT:
    ;1. Disable devices
    JSR KBDWAITINBUF             ;Send 0xAD command to the PS/2 controller
    LDA #$AD
    STA KBD_CMD
    ;2. Flush The Output Buffer
    LDA KBD_STATUS
    AND #$01                     ;Check if there is data to flush
    BEQ KBDCRTLSET               ;No? Next step then
    LDA KBD_DATA                 ;Yes? Get the data byte
KBDCRTLSET:
    ;3. Set the Controller Configuration Byte (temp)
    JSR KBDWAITINBUF             ;Send 0x60 command to the PS/2 controller
    LDA #$60
    STA KBD_CMD
    JSR KBDWAITINBUF             ;Send actual configuration byte
    LDA #$08                     ;Interrupts disabled, system flag reset, first port clock enabled
    STA KBD_DATA                 ;second port clock enabled, first port translation disabled
    ;4. Controller self test
    JSR KBDWAITINBUF
    LDA #$AA                     ;Send 0xAA command to the PS/2 controller
    STA KBD_CMD
    JSR KBDWAITOUTBUF            ;Wait for response
    LDA KBD_DATA                 ;Get byte
    CMP #$55                     ;Is it 0x55?
    BEQ KBDIFTEST
    LDX #$01                     ;Return result code if not
    RTS
KBDIFTEST:                       ;No? Return then
    ;5. Interface test
    JSR KBDWAITINBUF
    LDA #$AB                     ;Send 0xAB command
    STA KBD_CMD
    JSR KBDWAITOUTBUF            ;Wait for response
    LDA KBD_DATA                 ;Get byte
    CMP #$01                     ;Check if it is CLK stuck low error
    BNE KBDIFTEST_CLKSH
    LDX #$02                     ;Return result code if it is
    RTS
KBDIFTEST_CLKSH:
    CMP #$02                     ;Check if it is CLK stuck high error
    BNE KBDIFTEST_DATSL
    LDX #$03                     ;Return result code if it is
    RTS
KBDIFTEST_DATSL:
    CMP #$03                     ;Check if it is KBD DATA stuck low error
    BNE KBDIFTEST_DATSH
    LDX #$04                     ;Return result code if it is
    RTS
KBDIFTEST_DATSH:
    CMP #$04                     ;Check if it is KBD DATA stuck high error
    BNE KBDIFTEST_OK
    LDX #$05                     ;Return result code if it is
    RTS
KBDIFTEST_OK:
    CMP #$00                     ;Is it 0x00? Did it pass the test?
    BEQ KBDENDEVS
    LDX #$06                     ;Return result code if not
    RTS                          ;No? Return then
KBDENDEVS:
    ;6. Enable Devices
    JSR KBDWAITINBUF
    LDA #$AE                     ;Send 0xAE command
    STA KBD_CMD
    ;7. Reset Device
    JSR KBDWAITINBUF             ;Wait untill ready to send
    LDA #$FF                     ;Send 0xFF to device
    STA KBD_DATA                 ;Send it to device, not the controller
    LDY 130                      ;Setup DELAY routine
    JSR DELAY                    ;This is required to avoid freeze
    JSR KBDWAITOUTBUF            ;Wait for response
    LDA KBD_DATA                 ;Get byte
    CMP #$FA                     ;Is it 0xFA? 0xFC means failure. No response means no device present.
    BEQ KBD_OK
    LDX #$07                     ;Return result code if not
    RTS                          ;No? Return then
    ;3. Set the Controller Configuration Byte
    JSR KBDWAITINBUF             ;Send 0x60 command to the PS/2 controller
    LDA #$60
    STA KBD_CMD
    JSR KBDWAITINBUF             ;Send actual configuration byte
    LDA #$08                     ;Interrupts enabled, system flag reset, first port clock enabled
    STA KBD_DATA                 ;second port clock enabled, first port translation disabled
KBD_OK:
    ;LDA #$00                    ;Zero KBDDATA
    ;STA KBDDATA
    LDX #$00                     ;Return result code
    RTS