;
; Profiling code
;

		AREA	|ARM$$code|,CODE,READONLY

		GET	OSLib:OSLib.Hdr.OSModule

		GET	hdr.cp
		GET	hdr.defs
		GET	hdr.profile

		[ :DEF: PROFILING

		[ :LNOT: :DEF: NDEBUG
		IMPORT	assert_failed
		]

		EXPORT	profile_gettime
		EXPORT	profile_init
		EXPORT	profile_start_timing
		EXPORT	profile_stop_timing



; The cycle counter register (CCNT) is only accessible from
; privileged modes (though maybe this is a config option somewhere?)
; A small module intercepts the undefined instruction vector, quickly
; checking and performing the read for us if we try it from USR mode.

profile_init	STMFD	R13!,{R4,R5,R14}
		MOV	R0,#18
		ADR	R1,profile_mod
		SWI	XOS_Module
		MOVVC	R0,#0
		BVC	init_leave

		;try to load the module

		MOV	R0,#1
		ADR	R1,profile_cmd
		SWI	XOS_Module
		MOVVC	R0,#0
init_leave	LDMFD	R13!,{R4,R5,PC}

profile_cmd	=	"$.Debugging."
profile_mod	=	"ProfileMod",0
		ALIGN


; Profiling code hand-written because in frequently-called code,
; where it's often used, it can add quite an overhead
;
; a1 -> prof_time structure

profile_start_timing
		ROUT
		LDR	ip,=prof_current

		[ :LNOT: :DEF: NDEBUG
		;check that this counter isn't already running
		LDR	a3,[a1,#prof_time_active]
		TEQ	a3,#0
		BLNE	assert_failed
		]

		MOV	a2,a1
		LDR	a3,[ip]
		MRC	CP14,0,a1,C1,C0,0		;read CCNT
		TEQ	a3,#0
		STR	a3,[a2,#prof_time_prev]
		BEQ	%00

		LDR	a4,[a3,#prof_time_start_time]
		STR	lr,[sp,#-4]!
		STR	a2,[sp,#-4]!
		LDR	lr,[a3,#prof_time_total_time]
		LDR	a2,[a3,#prof_time_total_time + 4]
		SUB	a4,a1,a4
		ADDS	lr,lr,a4
		ADC	a2,a2,#0
		STR	lr,[a3,#prof_time_total_time]
		STR	a2,[a3,#prof_time_total_time + 4]
		LDR	a2,[sp],#4
		LDR	lr,[sp],#4

00		STR	pc,[a2,#prof_time_active]
		STR	a1,[a2,#prof_time_start_time]
		STR	a2,[ip]
		MOV	pc,lr


; a1 -> prof_time structure

profile_stop_timing
		[ :LNOT: :DEF: NDEBUG
		LDR	a3,[a1,#prof_time_active]
		TEQ	a3,#0
		BLEQ	assert_failed
		]

		LDR	a3,[a1,#prof_time_start_time]
		MOV	a2,a1
		MRC	CP14,0,a1,C1,C0,0		;read CCNT
		LDR	a4,[a2,#prof_time_total_time]
		LDR	ip,[a2,#prof_time_total_time+4]
		SUB	a3,a1,a3
		ADDS	a4,a4,a3
		ADC	ip,ip,#0
		MOV	a3,#0
		STR	a3,[a2,#prof_time_active]
		LDR	a3,[a2,#prof_time_prev]
		STR	a4,[a2,#prof_time_total_time]
		LDR	a4,=prof_current
		STR	ip,[a2,#prof_time_total_time+4]

		;restart previous counter, if any

		TEQ	a3,#0
		STRNE	a1,[a3,#prof_time_start_time]
		STR	a3,[a4]
		MOV	pc,lr


profile_gettime	MRC	CP14,0,a1,C1,C0,0
		MOV	pc,lr


		AREA	|ARM$$zidata|,DATA,NOINIT

prof_current	%	4

		]

		END
