Implement busy wait loop for LCD status (busy flag)

lcd @ 1MHz

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

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

VAR_BF = $3000 ; RAM location used to save off results from LCD status register

  .org $8000

reset: 

  ; initialize stack pointer

  ldx #$ff
  txs 

  ; configure VIA ports

  lda #%11111111 ; set all Port B pins to output
  sta DDRB

  lda #%11100000 ; set top 3 Port A pins to output
  sta DDRA


  ; configure LCD display

  lda #%00111000 ; Set 8-bit mode; 2-line display, 5x8 font
  jsr lcd_instruction
  lda #%00001110 ; Display on, cursor on, blink off
  jsr lcd_instruction
  lda #%00000110 ; increment cursor position, no display shift
  jsr lcd_instruction
  lda #%00000001 ; clear display
  jsr lcd_instruction

  ; write data

  lda #"H"
  jsr print_char
  lda #"e"
  jsr print_char
  lda #"l"
  jsr print_char
  lda #"l"
  jsr print_char
  lda #"o"  
  jsr print_char
  lda #","
  jsr print_char
  lda #" "
  jsr print_char
  lda #"w"
  jsr print_char
  lda #"o"
  jsr print_char
  lda #"r"
  jsr print_char
  lda #"l"
  jsr print_char
  lda #"d"
  jsr print_char
  lda #"!"
  jsr print_char

 
loop:
  jmp loop


lcd_instruction:
  jsr wait_until_lcd_not_busy
  sta PORTB
  lda #0         ; Clear RS/RW/E bits
  sta PORTA 
  lda #E         ; Set E bit to send instruction
  sta PORTA      
  lda #0         ; Clear RS/RW/E bits
  sta PORTA
  rts 

print_char:
  jsr wait_until_lcd_not_busy
  sta PORTB
  lda #RS         ; Set RS; Clear RW/E bits
  sta PORTA 
  lda #(RS | E)   ; Set E bit to send instruction
  sta PORTA
  lda #RS         ; Clear E bit
  sta PORTA
  rts

wait_until_lcd_not_busy:
  ; read Busy flag (DB7) and address counter (DB0 - DB6)

  ; preserve accumulator
  pha 

  ; Set Port B to read
  lda #%00000000 ; set all Port B pins to input
  sta DDRB

__wulnb_read_loop:
  ; RS = 0 and RW = 1 for register read
  lda #RW         ; Set RW and clear RS/E bits
  sta PORTA 
  lda #(RW | E)  ; Set E bit to send instruction
  sta PORTA      
  ; LCD status register should now be on port B pins, read from VIA
  lda PORTB
  sta VAR_BF      ; Save off flags to RAM
  lda #RW         ; Clear E bit
  sta PORTA

  ; bit 7 is busy flag; if high, wait (repeat)
  lda VAR_BF
  rol
  bcs __wulnb_read_loop

  ; Set Port B back to write 
  lda #%11111111 ; set all Port B pins to output
  sta DDRB

  ; restore accumulator and return 
  pla
  rts


  .org RESETVEC
  .word reset
  .word $0000