PORTB = $6000
PORTA = $6001
DDRB  = $6002
DDRA  = $6003

E     = %10000000
RW    = %01000000
RS    = %00100000

 .org #$f800

reset:
 ldx #$FF			; initialize call stack to $1FF
 txs

 lda #%11111111			; set output on all pins of PORTB
 sta DDRB

 lda #%11100000			; set output on LCD control pins of PORTA
 sta DDRA

 lda #%00111000			; set 8-bit, 2-line display, 5x8 font
 jsr lcd_send_command

 lda #%00001110			; display on, cursor on, no blink
 jsr lcd_send_command

 lda #%00000001			; clear display
 jsr lcd_send_command

 lda #%00000010			; return home
 jsr lcd_send_command

 lda #%00000110			; set entry mode: increment, no shift
 jsr lcd_send_command

set_memory:
 ldy #$0
 ldx #$FF
set_memory_loop:
 sty $0, x
 iny
 dex
 bne set_memory_loop
 
test_memory:
 ldy #$0
 ldx #$FF
test_memory_loop:
 tya
 pha 
 lda $0, x
 cmp $1FF			; stack was empty so Y is now on top of stack
 bne test_memory_bad
 pla
 tay
 iny
 dex
 bne test_memory_loop
 jmp test_memory_ok

test_memory_bad:
 ldx #0
msg_loop_bad:
 lda message_memory_bad, x
 beq msg_end_bad
 jsr lcd_send_data
 inx
 jmp msg_loop_bad
msg_end_bad:
 jmp finish

test_memory_ok:
 ldx #0
msg_loop_ok:
 lda message_memory_ok, x
 beq msg_end_ok
 jsr lcd_send_data
 inx
 jmp msg_loop_ok
msg_end_ok

finish:
 jmp finish

term:
 jmp term

lcd_wait:
 pha
 lda #%00000000			; port B to all input
 sta DDRB

lcd_wait_loop:
 lda #RW
 sta PORTA

 lda #(RW | E)
 sta PORTA

 lda PORTB
 and #%10000000
 bne lcd_wait_loop

 lda #RW
 sta PORTA
 lda #%11111111                 ; port B back to output
 sta DDRB

 pla
 rts

lcd_send_command:

 jsr lcd_wait

 sta PORTB

 lda #$0			; clear RS/RW/E
 sta PORTA

 lda #E				; clear RS/RW, set E, send instruction
 sta PORTA

 lda #$0			; clear RS/RW/E
 sta PORTA

 rts

lcd_send_data:

 jsr lcd_wait

 sta PORTB			; data to send to port B

 lda #RS			; clear RW, E, set RS for data
 sta PORTA

 lda #( E | RS )		; clear RW, set E/RS, send data
 sta PORTA

 lda #RS			; clear RS, E, keep RS
 sta PORTA

 rts

nmi:
 rti

irq:
 rti

message_memory_bad:
 .asciiz "mem bad"
 
message_memory_ok:
 .asciiz "mem ok"

 .org #$fffa
 .word nmi
 .word reset
 .word irq