.org 0xF800
mov sp,#STACK
mov bx,#initdram_txt
call print

WAIT_PLL:
inw #0x20
test al,#0x04
jz WAIT_PLL
;;; DO ALL INIT STUFF ;;;

;////sdram_software_control_on///
MOV BX,#0x0000
MOV AX,#0x000E
call sdram_dfii_control_write
;/////////////////////////////////

MOV BX,#0x0000
MOV AX,#0x0000
CALL ddrctrl_init_done_write

mov bx,#0x0000
mov ax,#0x0000
call ddrctrl_init_error_write

mov bx,#0x0000
mov ax,#0x000C
call sdram_dfii_control_write

MOV DL,#0xF0
DELAY11:
MOV AX,#0xF000
DELAY1:
INC AX
JNZ DELAY1
INC DL
JNZ DELAY11

MOV BX,#0x0000
MOV AX,#0x0000
call sdram_dfii_pi0_address_write

mov bx,#0x0000
mov ax,#0x0000
call sdram_dfii_pi0_baddress_write

mov bx,#0x0000
mov ax,#0x000E
call sdram_dfii_control_write

MOV DL,#0xF0
DELAY21:
MOV AX,#0xF000
DELAY2:
INC AX
JNZ DELAY2
INC DL
JNZ DELAY21

MOV BX,#0x0000
MOV AX,#0x0200
call sdram_dfii_pi0_address_write
mov bx,#0x0000
mov ax,#0x0002
call sdram_dfii_pi0_baddress_write
mov bx,#0x0000
mov ax,#0x000F
call command_p0


MOV BX,#0x0000
MOV AX,#0x0000
call sdram_dfii_pi0_address_write
mov bx,#0x0000
mov ax,#0x0003
call sdram_dfii_pi0_baddress_write
mov bx,#0x0000
mov ax,#0x000F
call command_p0


MOV BX,#0x0000
MOV AX,#0x0006
call sdram_dfii_pi0_address_write
mov bx,#0x0000
mov ax,#0x0001
call sdram_dfii_pi0_baddress_write
mov bx,#0x0000
mov ax,#0x000F
call command_p0


MOV BX,#0x0000
MOV AX,#0x0320
call sdram_dfii_pi0_address_write
mov bx,#0x0000
mov ax,#0x0000
call sdram_dfii_pi0_baddress_write
mov bx,#0x0000
mov ax,#0x000F
call command_p0

MOV DL,#0xF0
DELAY31:
MOV AX,#0xF000
DELAY3:
INC AX
JNZ DELAY3
INC DL
JNZ DELAY31

MOV BX,#0x0000
MOV AX,#0x0400
call sdram_dfii_pi0_address_write
mov bx,#0x0000
mov ax,#0x0000
call sdram_dfii_pi0_baddress_write
mov bx,#0x0000
mov ax,#0x0003
call command_p0




;////sdram_software_control_off///
MOV BX,#0x0000
MOV AX,#0x0001
call sdram_dfii_control_write
;////////////////////////////////

;Signify end of init
MOV BX,#0x0000
MOV AX,#0x0001
CALL ddrctrl_init_done_write

;;;;;;;;;; PROBABLY NOT NECESSARY
MOV DL,#0xF0
DELAY41:
MOV AX,#0xF000
DELAY4:
INC AX
JNZ DELAY4
INC DL
JNZ DELAY41


;;; CHECK ;;;
inw #0x20
test al,#0x01
jz failram
mov bx,#OK_txt
call print
JMP FINISHED_DDR
failram:
mov bx,#FAIL_txt
call print
mov bx,#NOT_RDY_txt
call print

inw #0x20
test al,#0x02
jz skip_err
mov bx,#ERR_ASRT_txt
call print
skip_err:

inw #0x20
test al,#0x04
jnz skip_pll
mov bx,#PLL_ERR_txt
call print
skip_pll:
hlt

FINISHED_DDR:

MOV DI,#0x2000
MOV AX,#0xAAAA
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV AX,#0x6666
MOV AX,[DI]
CMP AX,#0xAAAA
JNZ FAILED
MOV BX,#MEMTEST_PASS
CALL print

MOV AL,#0x48
MOV [DI],AX
MOV AL,#0x00
MOV AX,[DI]
out byte #0xA5

MOV DI,#0x3000
MOV AL,#0x65
MOV [DI],AX
MOV AL,#0x00
MOV AX,[DI]
out byte #0xA5

MOV DI,#0x4000
MOV AL,#0x6c
MOV [DI],AX
MOV AL,#0x00
MOV AX,[DI]
out byte #0xA5

MOV DI,#0x5000
MOV AL,#0x6c
MOV [DI],AX
MOV AL,#0x00
MOV AX,[DI]
out byte #0xA5

MOV DI,#0x6000
MOV AL,#0x6f
MOV [DI],AX
MOV AL,#0x00
MOV AX,[DI]
out byte #0xA5

HLT
FAILED:
MOV BX,#MEMTEST_FAIL
CALL print

MOV DL,#0xF0
DELAY51:
MOV AX,#0x5000
DELAY5:
INC AX
JNZ DELAY5
INC DL
JNZ DELAY51

MOV DI,#0x2000
MOV AX,#0xAAAA
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV [DI],AX
MOV AX,#0x0000
MOV AX,[DI]
CMP AL,#0xAA
JNZ FAILED2
MOV BX,#MEMTEST_PASS
CALL print
HLT
FAILED2:
MOV BX,#MEMTEST_FAIL
CALL print
MOV AX,[DI]
CMP AL,#0xAA
JNZ FAILED3
MOV BX,#MEMTEST_PASS
CALL print
HLT
FAILED3:
MOV BX,#MEMTEST_FAIL
CALL print
HLT


;;; HELPER FUNCTION
command_p0:
call WISHBONE_SET_DATA
MOV AX,#0x0401 ;ORIG=0x1804L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
MOV AX,#0x0001
MOV BX,#0x0000
call WISHBONE_SET_DATA
MOV AX,#0x0402 ;ORIG=0x1808L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret

#AX: Lower 16 bits data
#BX: Upper 16 bits data
sdram_dfii_pi0_address_write:
call WISHBONE_SET_DATA
MOV AX,#0x0403 ;ORIG=0x180cL
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret

sdram_dfii_pi0_baddress_write:
call WISHBONE_SET_DATA
MOV AX,#0x0404 ;ORIG=0x1810L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret

sdram_dfii_control_write:
call WISHBONE_SET_DATA
MOV AX,#0x0400 ;ORIG=0x1800L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret

ddrctrl_init_done_write:
call WISHBONE_SET_DATA
MOV AX,#0x0000 ;ORIG=0x0L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret

ddrctrl_init_error_write:
call WISHBONE_SET_DATA
MOV AX,#0x0001 ;ORIG=0x4L
CALL  WISHBONE_SET_ADDRESS
CALL DO_WRITE_TRANSACTION
ret
;
DO_WRITE_TRANSACTION:
mov ax,#0x01
outw #0x46
mov bl,#0x01
WB_WRITE_LOOP:
inc bl
jz WB_WRITE_TIMEOUT
inw #0x44
test al,#0x01
jz WB_WRITE_LOOP
test al,#0x04
jnz WB_WRITE_ERR
ret
WB_WRITE_TIMEOUT:
mov bx,#WISHBONE_TIMEOUT_txt
call print
hlt
WB_WRITE_ERR:
mov bx,#WISHBONE_ERROR_txt
call print
hlt

#AX: address
WISHBONE_SET_ADDRESS:
outw #0x44
ret

#AX: Lower 16bits
#BX: Upper 16bits
WISHBONE_SET_DATA:
outw #0x40
MOV AX,BX
outw #0x42
ret

print:
mov al,[bx]
cmp al,#0
je print_exit
out byte #0xA5
inc bx
jmp print
print_exit:
ret

initdram_txt: .ASCII 'Init LiteDram: \0'
OK_txt: .ASCII 'OK\n\0'
PLL_ERR_txt: .ASCII 'PLL_ERR \0' ; PLL_ERR: pll_lock is not asserted
ERR_ASRT_txt: .ASCII 'ERR_ASRT \0' ; ERR_ASRT: init_error is asserted
NOT_RDY_txt: .ASCII 'NOT_RDY \0'  ; NOT READY: init_done is not asserted
FAIL_txt: .ASCII 'FAIL\n\0'
WISHBONE_TIMEOUT_txt: .ASCII 'FAIL\nWISHBONE TIMEOUT\0'
WISHBONE_ERROR_txt: .ASCII 'FAIL\nWISHBONE ERROR\0'
MEMTEST_PASS: .ASCII 'MEMTEST PASSED\n\0'
MEMTEST_FAIL: .ASCII 'MEMTEST FAILED\n\0'
 .BLKB 18 ; Using the text as stack space for the compiled program
STACK: ; brainfuck_mandelbrot depends on stack being at the end

.ORG 0xFFF0
MOV AX,#0xF800
JMP AX


.ORG 0xFFFF
DB 0x00 ;Make sure a full 64KiB image