;
; Harness for executing code on the ASIM model.
;

 GET VTest-Head

	EXPORT time
	EXPORT Niqgen
	EXPORT Fiqgen
	EXPORT printf
	EXPORT printval
	EXPORT printstr
	EXPORT div
	EXPORT quit
	IMPORT main

; start of code!
	ENTRY
        ADRL	R13,Sstktop		; Set up Supervisor stack

	MVP 	CPSR_ctl, #, &10     ; Set into USR_32 (only works in SVC mode)
	MOV	R0,R0		     ; NOP

        ADRL	R13,Ustktop		; Set up User stack

	BL 	main
quit
	MOV 	R0, #CTRLD
	SWI 	0          	; Halt simulation
	B	quit


; ********** Print current simulation time **********
time
	STMDB	sp!,{R0,R1,lr}
	MOV	R1, #TubeAd
	MOV 	R0, #TIMEC
	STRB	R0,[R1]		; Tube model prints time
	LDMIA	sp!,{R0,R1,pc}

; ********** Generate an Interrupt **********
Fiqgen
	STMDB	sp!,{R0-R2,lr}
	MOV	R1,#1
	B	genint
Niqgen
	STMDB	sp!,{R0-R2,lr}
	MOV	R1,#0
genint
	MOV	R2,#MAXINTR
	SUB	R2,R2,#1
	CMP	R0,#1
	MOVLT	R0,#1		; value 0 is used to clear interrupts
	CMP	R0,R2
	MOVGT	R0,R2
	CMP	R1,#0
	ADDNE	R0,R0,R2
	ADDNE	R0,R0,#1
	MOV	R1, #IntrAd
	STR	R0,[R1]
	LDMIA	sp!,{R0-R2,pc}

; ********** Fake up a 'C' printf() routines **********

printstr
	STMDB	sp!,{R0-R4,lr}
	MOV	R4, #TubeAd
	MOV	R1,R0

	ANDS	R2,R1,#3	; check ptr word alignment.
	BEQ	nextword
nextbyte
	LDRB	R0,[R1],#1	; get first byte with auto increment to next
	CMP	R0,#0		; test for a NULL (zero) terminating char
	STRNEB	R0,[R4]		; print if not
	BEQ	done		; stop printing if so
	ANDS	R2,R1,#3	; check ptr word alignment.
	BEQ	nextword
	BNE	nextbyte	; NOT word aligned, so ... next byte


printf
	STMDB	sp!,{R0-R4,lr}
	MOV	R4, #TubeAd
	MOV	R1,R0
nextword
	LDR	R2,[R1],#4	; get first word with auto increment to next
	MOV	R3,#4
nextchar
	MOV	R0,R2		; copy print word
	MOV	R2,R2,LSR #8	; First char is stripped off ...
	AND	R0,R0,#&FF	; ... and prepared for printing.
	CMP	R0,#0		; test for a NULL (zero) terminating char
	STRNEB	R0,[R4]		; print if not
	BEQ	done		; stop printing if so
	SUBS	R3,R3,#1
	BNE	nextchar	; next char in word
	B	nextword	; next word
done
	LDMIA	sp!,{R0-R4,pc}

; ********** Print a 32-bit hex value **********

printval
	STMDB	sp!,{R0-R3,lr}
	MOV	R3, #TubeAd
	MOV	R1,R0
	MOV	R2,#8
nextdigit
	MOV	R1,R1,LSL #4
	MOV	R0,R0,LSR #28
	CMP	R0,#9
	ADDHI	R0,R0,#55
	BHI	prdigit
	ADD	R0,R0,#48
prdigit
	STRB	R0,[R3]
	SUBS	R2,R2,#1
	MOVNE	R0,R1
	BNE	nextdigit
	LDMIA	sp!,{R0-R3,pc}

; ********** Fake up a divide routine **********

div
	STMDB	sp!,{R1-R3,lr}
	MOV	R2,#1
div1
	CMP	R1,R0
	MOVCC	R1,R1, LSL #1
	MOVCC	R2,R2, ASL #1
	BCC	div1
	MOV	R3,#0
div2
	CMP	R0,R1
	SUBCS	R0,R0,R1
	ADDCS	R3,R3,R2
	MOVS	R2,R2, LSR #1
	MOVNE	R1,R1, LSR #1
	BNE	div2
	MOV	R0,R3
	LDMIA	sp!,{R1-R3,pc}


Ustkend % 1024
Ustktop
Sstkend % 1024
Sstktop
	END
