.include "sentinel65x.i"

* = $C8000

.null "WDC"
    jml w65c265s_init

w65c265s_init .proc
    ; Disable standard interrupts
    sei

    .short_a
    .long_i

    ; Set data bank to 0
    lda #0
    pha
    plb
    .databank 0

    ; Set direct page to $0000
    pea $0000
    pld
    .dpage $0000

    ; Set stack to $001FF
    ldx #$01FF
    txs

    ; Start FCLK
    lda #%00000001
    tsb SSCR

    ; Disable the on-CPU ROM
    lda #%10001001
    tsb BCR

    ; Disable secondary interrupt sources
    stz UIER
    stz TIER
    stz EIER

    ; Enable all used chip select lines.
    lda #%11110011
    sta PCS7

    ; Disable all timers
    stz TER

    ; Set P5.4 and P5.5 as output
    lda #%00110000
    tsb PDD5

    ; Set P6.1 as output
    lda #%00000010
    tsb PDD6

    ; Disable on-chip SRAM
    lda #%00000100
    tsb SSCR

    ; Select FCLK as the clock source
    lda #%11111010
    tsb SSCR

    ; Initialize the VERA
    jsl vera_init

    ; Jump to entry point.
    jml start
.endproc

blink_red .proc
    pha
    phx

    ; Turn on the red LED
    lda #%00010000
    tsb PD5

    ldx #02
    jsl delay

    ; Turn off the red LED
    lda #%00010000
    trb PD5

    plx
    pla
    rtl
.endproc

blink_green .proc
    pha
    phx

    ; Turn on the green LED
    lda #%00100000
    tsb PD5

    ldx #02
    jsl delay

    ; Turn off the green LED
    lda #%00100000
    trb PD5

    plx
    pla
    rtl
.endproc

blink_blue .proc
    pha
    phx

    ; Turn on the blue LED
    lda #%00000010
    tsb PD6

    ldx #02
    jsl delay

    ; Turn off the blue LED
    lda #%00000010
    trb PD6

    plx
    pla
    rtl
.endproc

; Delay X times.
delay .proc
    phy

loop_x
    ldy #$FFFF

loop_y
    dey
    bne loop_y

    dex
    bne loop_x

    ply
    rtl
.endproc

start .proc
    jsl blink_blue
main_loop
    nop
    bra main_loop
.endproc

empty_irq_handler .proc
    phb 
    phd
    .long_a
    .long_i
    pha
    phx
    phy

    jmp irq_exit
.endproc

nmi_handler .proc

    jsl blink_red
    jsl blink_green
    jsl blink_blue

    rti
.endproc

irq_exit .proc
    .long_a
    .long_i
    ply
    plx
    pla
    pld
    plb
    rti
.endproc

vera_init .proc
    pha

    jsl blink_red



    jsl blink_green

    pla
    rtl
.endproc

.fill $FF80 - *, 0
* = $FF80
.fill $FFBA - *, empty_irq_handler
* = $FFBA
.word nmi_handler
.fill $FFFF - *, empty_irq_handler
.fill 524288 - *, 0