.macro debug value
        brk
        .byte   value
.endmacro

; MIT License
;
; Copyright (c) 2022 Willis Blackburn
;
; Permission is hereby granted, free of charge, to any person obtaining a copy
; of this software and associated documentation files (the "Software"), to deal
; in the Software without restriction, including without limitation the rights
; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
; copies of the Software, and to permit persons to whom the Software is
; furnished to do so, subject to the following conditions:
;
; The above copyright notice and this permission notice shall be included in all
; copies or substantial portions of the Software.
;
; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
; SOFTWARE

; cc65 runtime
.import pushax, pusha0

; C standard library
.import _fprintf, _stderr

; Point the BRK handler to the debug_handler function.

install_debug_handler:
        lda     #<debug_handler
        sta     $FFFE                   ; BRK vector low byte
        lda     #>debug_handler
        sta     $FFFF                   ; BRK vector high byte
        rts

.export install_debug_handler

.zeropage

save_pc: .res 2

.bss

save_a: .res 1
save_x: .res 1
save_y: .res 1
save_sp: .res 1
save_flags: .res 1

flag_indicators: .res 8

.code

format: .byte "$%02X: A=%02X X=%02X Y=%02X SP=%02X %.8s", $0A, $00
flag_names: .byte "NV-BDIZC"

; Prints the register values to stderr.
; Although calling into the C library from an interrupt handler is normally asking for trouble, since sim65
; doesn't generate interrupts, this will only be called by a BRK statement.

debug_handler:
        cld                             ; Clear decimal flag (just in case)
        sta     save_a                  ; Save 6502 registers
        stx     save_x     
        sty     save_y     
        tsx                             ; Get stack pointer into X
        stx     save_sp                 ; Save it so we can print it
        ldy     $102,x                  ; PC low byte
        sty     save_pc     
        ldy     $103,x                  ; PC high byte
        dey                             ; Subtract 256 from PC; we will index with Y = 255 to get PC-1
        sty     save_pc+1       
        lda     $101,x                  ; Flags
        sta     save_flags
        ldy     #0
@next_flag:
        lda     flag_names,y            ; Store name in indicator string if flag on
        rol     save_flags
        bcs     @on
        lda     #'-'                    ; Store '-' indicator string if flag off
@on:
        sta     flag_indicators,y
        iny
        cpy     #8
        bne     @next_flag
        lda     _stderr                 ; fprintf(stderr, ...
        ldx     _stderr+1
        jsr     pushax
        lda     #<format                ; format, ...
        ldx     #>format
        jsr     pushax
        ldy     #$FF
        lda     (save_pc),y             ; id, ...
        jsr     pusha0         
        lda     save_a                  ; A, ...
        jsr     pusha0
        lda     save_x                  ; X, ...
        jsr     pusha0
        lda     save_y                  ; Y, ...
        jsr     pusha0
        lda     save_sp                 ; SP, ...
        jsr     pusha0
        lda     #<flag_indicators       ; flag_indicators)
        ldx     #>flag_indicators
        jsr     pushax           
        ldy     #16                     ; 16 bytes on the C stack
        jsr     _fprintf
        lda     save_a                  ; Restore 6502 registers
        ldx     save_x
        ldy     save_y
        rti

;This front end for the IRQ vector saves the CPU registers and determines if a BRK
; instruction was the cause. There are 25 clock cycles to jump to the IRQ vector,
; and there are 26 clock cycles to jump to the BRK vector. Note that there is an
; additional 18 clock cycles for the IRQ return vector, which restores the registers.
; This creates an overhead of 43 (IRQ) or 44 (BRK) clock cycles, plus whatever the
; ISR or BRK service routines add.
;
IRQ_VECTOR                              ;This is the ROM start for the BRK/IRQ handler
                PHA                     ;Save A Reg (3)
                PHX                     ;Save X Reg (3)
                PHY                     ;Save Y Reg (3)
                TSX                     ;Get Stack pointer (2)
                LDA     $0100+4,X       ;Get Status Register (4)
                AND     #$10            ;Mask for BRK bit set (2)
                BNE     DO_BRK          ;If set, handle BRK (2/3)
                JMP     (IRQVEC0)       ;Jump to Soft vectored IRQ Handler (6)
DO_BRK          JMP     (BRKVEC0)       ;Jump to Soft vectored BRK Handler (6)
;
NMI_ROM         JMP     (NMIVEC0)       ;Jump to Soft vectored NMI handler (6)
;
;This is the standard return for the IRQ/BRK handler routines (18 clock cycles)
;
IRQ_EXIT0       PLY                     ;Restore Y Reg (4)
                PLX                     ;Restore X Reg (4)
                PLA                     ;Restore A Reg (4)
                RTI                     ;Return from IRQ/BRK routine (6)
;

;
; BRK Vector defaults to here
;
BRKINSTR0       PLY                     ;Restore Y Reg (4)
                PLX                     ;Restore X Reg (4)
                PLA                     ;Restore A Reg (4)
                STA     AREG            ;Save A Reg (3)
                STX     XREG            ;Save X Reg (3)
                STY     YREG            ;Save Y Reg (3)
                PLA                     ;Get Processor Status (4)
                STA     PREG            ;Save in PROCESSOR STATUS preset/result (3)
                TSX                     ;Xfer STACK pointer to X Reg (2)
                STX     SREG            ;Save STACK pointer (3)
;
                PLX                     ;Pull Low RETURN address from STACK then save it (4)
                STX     PCL             ;Store program counter Low byte (3)
                STX     INDEXL          ;Seed Indexl for DIS_LINE (3)
                PLY                     ;Pull High RETURN address from STACK then save it (4)
                STY     PCH             ;Store program counter High byte (3)
                STY     INDEXH          ;Seed Indexh for DIS_LINE (3)
                BBR4    PREG,DO_NULL    ;Check for BRK bit set (5)
;
; The following three subroutines are contained in the base C02 Monitor code. These calls
; do a register display and disassembles the line of code that caused the BRK to occur
;
                JSR     M_PRSTAT1       ;Display CPU status (6)
                JSR     M_DECINDEX      ;Decrement Index to BRK ID Byte (6)
                JSR     M_DECINDEX      ;Decrement Index to BRK instruction (6)
                JSR     M_DIS_LINE      ;Disassemble BRK instruction (6)
;
; Note: This routine only clears Port A, as it is used for the Console
;
DO_NULL         LDA     #$00            ;Clear all Processor Status Register bits (2)
                PHA                     ;Push it to Stack (3)
                PLP                     ;Pull it to Processor Status (4)
                STZ     ITAIL_A         ;Clear input buffer pointers (3)
                STZ     IHEAD_A         ; (3)
                STZ     ICNT_A          ; (3)
                JMP     (BRKRTVEC0)     ;Done BRK service process, re-enter monitor (6)
;

;[R] REGISTERS command: Display contents of all preset/result memory locations
PRSTAT          JSR     B_CHROUT        ;Send "R" to terminal
PRSTAT1         LDA     #$13            ;Get Header msg
                JSR     PROMPT          ;Send to terminal
                LDA     PCL             ;Get PC Low byte
                LDY     PCH             ;Get PC High byte
                JSR     PRWORD          ;Print 16-bit word
                JSR     SPC             ;Send 1 space
;
                LDX     #$04            ;Set for count of 4
REGPLOOP        LDA     PREG,X          ;Start with A Reg variable
                JSR     PRBYTE          ;Print it
                JSR     SPC             ;Send 1 space
                DEX                     ;Decrement count
                BNE     REGPLOOP        ;Loop back till all 4 are sent
;
                LDA     PREG            ;Get Status Register preset
                LDX     #$08            ;Get the index count for 8 bits
SREG_LP         ASL     A               ;Shift bit into Carry
                PHA                     ;Save current (shifted) SR value
                LDA     #$30            ;Load an Ascii zero
                ADC     #$00            ;Add zero (with Carry)
                JSR     B_CHROUT        ;Print bit value (0 or 1)
                PLA                     ;Get current (shifted) SR value
                DEX                     ;Decrement bit count
                BNE     SREG_LP         ;Loop back until all 8 printed
                JMP     CROUT           ;Send CR/LF and return to caller
;

;DECINDEX subroutine: decrement 16 bit variable INDEXL/INDEXH
DECINDEX        LDA     INDEXL          ;Get index low byte
                BNE     SKP_IDXH        ;Test for INDEXL = zero
                DEC     INDEXH          ;Decrement index high byte
SKP_IDXH        DEC     INDEXL          ;Decrement index low byte
                RTS                     ;Return to caller
;

.macro   debug   char
   PHP
   PHA
   LDA #char
   STA f:SERIAL
   PLA
   PLP
.endmacro