SECTION "a",HOME
main:
	ld c, $51
	ld a, $FF
	ld [c],a
	ld sp,$DFFF

	ld hl,text
	call puts
	
	call memtest
	call waitsw
	jp main

text:
	db $0D,$0A,$1B,"[1mFPGABoy Diagnostic ROM",$1B,"[0m",$0D,$0A,0

memtest:
	ld hl,memteststr
	call puts
	
	ld hl, $C000
.wr:
	ld a,h
	xor l
	ld [hli],a
	ld a, $DF
	cp h
	jp nz, .wr
	ld a, $80
	cp l
	jp nz, .wr

	ld hl, $C000
.rd:
	ld a,h
	xor l
	ld b,a
	ld a, [hli]
	cp b
	jp nz, .memfail
	
	ld a, $DF
	cp h
	jp nz, .rd
	ld a, $80
	cp l
	jp nz, .rd
	
	ld hl, testokstr
	call puts
	ret
.memfail:
	@ decrement hl the easy way
	ld a,[hld]
	push hl
	ld hl, failatstr
	call puts
	pop hl
	ld a, h
	call puthex
	ld a, l
	call puthex
	ld a, $0A
	call putc
	ld a, $0D
	call putc
	ret

memteststr:
	db "Testing memory from $C000 to $DF80...",0
testokstr:
	db " OK!",$0D,$0A,0
failatstr:
	db " Test failed at $",0
puthex:
	push af
	rra
	rra
	rra
	rra
	ld b,$0F
	and b
	ld b,$30
	add b
	call putc
	pop af
	ld b,$0F
	and b
	ld b,$30
	add b
	call putc
	ret

waitsw:
	ld hl,waitswstr
	call puts
	
	ld c, $51
	ld a, $00
	ld [c],a
	
	ld c, $51
	ld b, $0
.loop1:
	ld a,[c]
	cp b
	jp z,.loop1
.loop2:
	ld a,[c]
	cp b
	jp nz,.loop2
	ret

waitswstr:
	db "Diagnostic ROM complete; flip switches to nonzero and then to zero to reset.",$0D,$0A,0

putc:
	push af
	ld b, 0
	ld c, $50
.waitport:
	ld a,[c]
	cp b
	jp nz,.waitport
	pop af
	ld [c],a
	ret

puts:
	ld a, [hli]
	ld b, $00
	cp b
	jp z, .done
	call putc
	jp puts
.done:
	ret