path: root/acsr/helpers.s

	;
 	; Copyright (c) 2011, ARM Limited. All rights reserved.
 	;       
 	; Redistribution and use in source and binary forms, with
 	; or without modification, are permitted provided that the
 	; following conditions are met:
 	;     
 	; Redistributions of source code must retain the above
 	; copyright notice, this list of conditions and the 
 	; following disclaimer.
 	;
 	; Redistributions in binary form must reproduce the
 	; above copyright notice, this list of conditions and 
 	; the following disclaimer in the documentation 
 	; and/or other materials provided with the distribution.
 	;      
 	; Neither the name of ARM nor the names of its
 	; contributors may be used to endorse or promote products
 	; derived from this software without specific prior written
 	; permission.                        
 	; 

	EXPORT isb	
	EXPORT dsb
	EXPORT dmb
	EXPORT wfi
	EXPORT endless_wfi
	EXPORT wfe
	EXPORT sev
	EXPORT copy_words
	EXPORT appf_memcpy
	EXPORT appf_memset
	EXPORT get_cpu_type
	EXPORT va_to_pa

	EXPORT read_drar
	EXPORT read_dsar	
	EXPORT read_cbar	
	EXPORT read_sctlr
	EXPORT read_actlr
	EXPORT read_prrr
	EXPORT read_nmrr
	EXPORT read_l2ctlr
	EXPORT read_dacr
	EXPORT read_ttbr0		
	EXPORT read_cpacr	
	EXPORT read_scr
	EXPORT read_cpsr
	EXPORT read_midr
	EXPORT read_mpidr		
	EXPORT read_cntpct		
	EXPORT read_cntfrq	
	EXPORT read_vmpidr
	EXPORT read_vmidr	
	EXPORT read_vttbr
	EXPORT read_httbr	
	EXPORT read_id_pfr0
	EXPORT read_id_pfr1
	EXPORT read_id_dfr0
	EXPORT read_id_afr0
	EXPORT read_id_mmfr0
	EXPORT read_id_mmfr1
	EXPORT read_id_mmfr2
	EXPORT read_id_mmfr3		
	EXPORT read_id_isar0
	EXPORT read_id_isar1
	EXPORT read_id_isar2
	EXPORT read_id_isar3
	EXPORT read_id_isar4
	EXPORT read_id_isar5					
	EXPORT read_cpuid
	EXPORT read_aidr	
	EXPORT read_ctr
	EXPORT read_tcmtr
	EXPORT read_tlbtr
	EXPORT read_clusterid
	EXPORT read_l2ctlr	
	EXPORT read_hsctlr
	EXPORT read_hdfar
	EXPORT read_hpfar
	EXPORT read_vtcr
	EXPORT read_hcr
	EXPORT read_hdcr
	EXPORT read_hcptr
	EXPORT read_hstr	
	EXPORT read_cnthctl
	EXPORT read_cntkctl	
	EXPORT read_cntp_ctl
	EXPORT read_cntp_tval	
	EXPORT read_cnthp_ctl
	EXPORT read_cnthp_tval
	EXPORT read_cnthp_cval		
	EXPORT read_ttbcr
	EXPORT read_nsacr
	EXPORT read_clidr	
	EXPORT read_csselr
	EXPORT read_ccsidr
	EXPORT read_nmrr
	EXPORT read_prrr
	EXPORT read_mvbar	
	EXPORT read_vbar
	EXPORT read_hsr
	EXPORT read_dfar
	EXPORT read_ifar
	EXPORT read_dfsr
	EXPORT read_ifsr
	EXPORT read_adfsr
	EXPORT read_aifsr				
	EXPORT read_l2ectlr 
	EXPORT read_pmuserenr 
	EXPORT read_pmintenset 
	EXPORT read_pmintenclr 
	EXPORT read_pmovsset 
	EXPORT read_pmccntr 
	EXPORT read_pmxevtyper 
	EXPORT read_pmxevcntr 
	EXPORT read_pmcr 
	EXPORT read_pmcntenset 
	EXPORT read_pmcntenclr 
	EXPORT read_pmovsr 
	EXPORT read_pmswinc 
	EXPORT read_pmselr 
	EXPORT read_pmceid0 
	EXPORT read_pmceid1 

	EXPORT write_l2ectlr 
	EXPORT write_pmuserenr 
	EXPORT write_pmintenset 
	EXPORT write_pmintenclr 
	EXPORT write_pmovsset 
	EXPORT write_pmccntr 
	EXPORT write_pmxevtyper 
	EXPORT write_pmxevcntr 
	EXPORT write_pmcr 
	EXPORT write_pmcntenset 
	EXPORT write_pmcntenclr 
	EXPORT write_pmovsr 
	EXPORT write_pmswinc 
	EXPORT write_pmselr 
	EXPORT write_pmceid0 
	EXPORT write_pmceid1 	
	EXPORT write_dacr
	EXPORT write_prrr
	EXPORT write_nmrr		
	EXPORT write_ttbr0
	EXPORT write_cpacr
	EXPORT write_nsacr
	EXPORT write_scr
	EXPORT write_mvbar
	EXPORT write_vbar
	EXPORT write_hvbar	
	EXPORT write_vmpidr
	EXPORT write_vmidr	
	EXPORT write_csselr	
	EXPORT write_hcr
	EXPORT write_hdcr
	EXPORT write_hcptr
	EXPORT write_hstr
	EXPORT write_sctlr
	EXPORT write_actlr
	EXPORT write_osdlr
	EXPORT write_ttbcr
	EXPORT write_cntfrq	
	EXPORT write_cnthctl
	EXPORT write_cntkctl	
	EXPORT write_cntp_ctl
	EXPORT write_cntp_tval	
	EXPORT write_cnthp_ctl
	EXPORT write_cnthp_tval
	EXPORT write_cnthp_cval			
	EXPORT write_hsctlr
	EXPORT write_httbr
	EXPORT write_vttbr	
	EXPORT write_htcr
	EXPORT write_vtcr	
	EXPORT write_hmair0	
	EXPORT write_hmair1
	EXPORT write_dfar
	EXPORT write_ifar
	EXPORT write_dfsr
	EXPORT write_ifsr
	EXPORT write_adfsr
	EXPORT write_aifsr					
	
	
MIDR_CPU_MASK	EQU 0xff00fff0

	AREA ACSR, CODE, ALIGN=5


dmb	FUNCTION
	dmb
	bx	lr
	ENDFUNC

wfi	FUNCTION
	wfi
	bx	lr
	ENDFUNC
        	
	; WFI forever, and attempt to prevent speculative accesses starting
	; FIQ and IRQ are assumed to be disabled
endless_wfi	FUNCTION
	b	wfistart

	ALIGN 32
wfistart
	b	bloop
loop
	dsb
	wfi
bloop
	b	loop
	ENDFUNC

wfe	FUNCTION
	wfe
	bx	lr
	ENDFUNC

sev	FUNCTION
	sev
	bx	lr
	ENDFUNC

	; This function takes three arguments
	; r0: Destination start address (must be word aligned)
	; r1: Source start address (must be word aligned)
	; r2: Number of words to copy
	; Return value is updated destination pointer (first unwritten word)
copy_words	FUNCTION
	cmp	r2, #0
	beq	%f1
0	ldr	r3, [r1], #4
	str	r3, [r0], #4
	subs	r2, r2, #1
	bne	%b0
1	bx	lr
	ENDFUNC
	
	
appf_memcpy	FUNCTION
	cmp	r2, #0
	bxeq	lr
0	ldrb	r3, [r1], #1
	strb	r3, [r0], #1
	subs	r2, #1
	bne	%b0
	bx	lr
	ENDFUNC

appf_memset	FUNCTION
	cmp	r2, #0
	bxeq	lr
0	strb	r1, [r0], #1
	subs	r2, #1
	bne	%b0
	bx	lr
	ENDFUNC

read_cntfrq FUNCTION
	mrc     p15, 0, r0, c14, c0, 0
	bx      lr
	ENDFUNC

write_cntfrq FUNCTION
	mcr     p15, 0, r0, c14, c0, 0
	bx      lr
	ENDFUNC
				
read_cntpct FUNCTION
	mrrc    p15, 0, r0, r1, c14
	bx      lr
	ENDFUNC
	
isb	FUNCTION
	isb
	bx	lr
	ENDFUNC	

read_vmpidr FUNCTION
	mrc	p15, 4, r0, c0, c0, 5
	bx	lr
	ENDFUNC
	
read_vmidr FUNCTION
	mrc	p15, 4, r0, c0, c0, 0
	bx	lr
	ENDFUNC			

read_id_pfr0 FUNCTION
	mrc	p15, 0, r0, c0, c1, 0
	bx	lr
	ENDFUNC

read_id_pfr1 FUNCTION
	mrc	p15, 0, r0, c0, c1, 1
	bx	lr
	ENDFUNC

read_id_dfr0 FUNCTION
	mrc	p15, 0, r0, c0, c1, 2
	bx	lr
	ENDFUNC
	
read_id_afr0 FUNCTION
	mrc	p15, 0, r0, c0, c1, 3
	bx	lr
	ENDFUNC
	
read_id_mmfr0 FUNCTION
	mrc	p15, 0, r0, c0, c1, 4
	bx	lr
	ENDFUNC

read_id_mmfr1 FUNCTION
	mrc	p15, 0, r0, c0, c1, 5
	bx	lr
	ENDFUNC
	
read_id_mmfr2 FUNCTION
	mrc	p15, 0, r0, c0, c1, 6
	bx	lr
	ENDFUNC		

read_id_mmfr3 FUNCTION
	mrc	p15, 0, r0, c0, c1, 7
	bx	lr
	ENDFUNC
	
read_id_isar0 FUNCTION
	mrc	p15, 0, r0, c0, c2, 0
	bx	lr
	ENDFUNC

read_id_isar1 FUNCTION
	mrc	p15, 0, r0, c0, c2, 1
	bx	lr
	ENDFUNC
	
read_id_isar2 FUNCTION
	mrc	p15, 0, r0, c0, c2, 2
	bx	lr
	ENDFUNC
	
read_id_isar3 FUNCTION
	mrc	p15, 0, r0, c0, c2, 3
	bx	lr
	ENDFUNC
	
read_id_isar4 FUNCTION
	mrc	p15, 0, r0, c0, c2, 4
	bx	lr
	ENDFUNC

read_id_isar5 FUNCTION
	mrc	p15, 0, r0, c0, c2, 5
	bx	lr
	ENDFUNC					
	
read_ctr FUNCTION
	mrc	p15, 0, r0, c0, c0, 1
	bx	lr
	ENDFUNC

read_tcmtr FUNCTION
	mrc	p15, 0, r0, c0, c0, 2
	bx	lr
	ENDFUNC

read_tlbtr FUNCTION
	mrc	p15, 0, r0, c0, c0, 3
	bx	lr
	ENDFUNC		
			
read_aidr FUNCTION
	mrc	p15, 1, r0, c0, c0, 7
	bx	lr
	ENDFUNC	

read_dacr FUNCTION
	mrc     p15, 0, r0, c3, c0, 0
	bx      lr
	ENDFUNC

read_ttbr0 FUNCTION
	mrc     p15, 0, r0, c2, c0, 0
	bx      lr
	ENDFUNC

write_dacr FUNCTION
	mcr     p15, 0, r0, c3, c0, 0
	isb
	bx      lr
	ENDFUNC

read_cpacr FUNCTION
	mrc     p15, 0, r0, c1, c0, 2
	bx      lr
	ENDFUNC
		
write_cpacr FUNCTION
	mcr     p15, 0, r0, c1, c0, 2
	bx      lr
	ENDFUNC

read_midr FUNCTION
	mrc     p15, 0, r0, c0, c0, 0;
	bx      lr
	ENDFUNC

read_mpidr FUNCTION
	mrc     p15, 0, r0, c0, c0, 5
	bx      lr
	ENDFUNC

read_scr FUNCTION
	mrc     p15, 0, r0, c1, c1, 0
	bx      lr
	ENDFUNC

write_scr FUNCTION
	mcr     p15, 0, r0, c1, c1, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

write_nsacr FUNCTION
	mcr     p15, 0, r0, c1, c1, 2
	dsb
	isb
	bx      lr
	ENDFUNC

read_cpsr FUNCTION
	mrs     r0, CPSR
	bx      lr
	ENDFUNC

write_mvbar FUNCTION
	mcr     p15, 0, r0, c12, c0, 1
	bx      lr
	ENDFUNC

write_vbar FUNCTION
	mcr     p15, 0, r0, c12, c0, 0 
	bx      lr
	ENDFUNC

write_hvbar FUNCTION
	mcr     p15, 4, r0, c12, c0, 0 
	bx      lr
	ENDFUNC	

read_mvbar FUNCTION
	mrc     p15, 0, r0, c12, c0, 1
	bx      lr
	ENDFUNC
		
read_vbar FUNCTION
	mrc     p15, 0, r0, c12, c0, 0 
	bx      lr
	ENDFUNC
	
read_cpuid FUNCTION
	mrc   p15, 0, r0, c0, c0, 5
	ands  r0, r0, #0xf
	bx    lr
	ENDFUNC

read_clusterid FUNCTION	
	mrc   p15, 0, r0, c0, c0, 5
	lsr   r0, r0, #0x8
	ands  r0, r0, #0xf
	bx    lr
	ENDFUNC

write_ttbr0 FUNCTION
	mcr     p15, 0, r0, c2, c0, 0
	mcr     p15, 0, r0, c7, c5, 6
	mcr     p15, 0, r0, c8, c7, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

read_ttbcr FUNCTION
	mrc     p15, 0, r0, c2, c0, 2
	bx      lr
	ENDFUNC	
		
write_ttbcr FUNCTION
	mcr     p15, 0, r0, c2, c0, 2
	bx      lr
	ENDFUNC	

write_vmpidr FUNCTION
	mcr	p15, 4, r0, c0, c0, 5
	isb
	bx	lr
	ENDFUNC
	
write_vmidr FUNCTION
	mcr	p15, 4, r0, c0, c0, 0
	isb
	bx	lr
	ENDFUNC			
		
read_vtcr FUNCTION
	mrc     p15, 4, r0, c2, c1, 2
	bx      lr
	ENDFUNC

read_hcr FUNCTION
	mrc     p15, 4, r0, c1, c1, 0
	bx      lr
	ENDFUNC

read_hdcr FUNCTION
	mrc     p15, 4, r0, c1, c1, 1
	bx      lr
	ENDFUNC
	
read_hcptr FUNCTION
	mrc     p15, 4, r0, c1, c1, 2
	bx      lr
	ENDFUNC
	
read_hstr FUNCTION
	mrc     p15, 4, r0, c1, c1, 3
	bx      lr
	ENDFUNC			

read_httbr FUNCTION
	mrrc    p15, 4, r0, r1, c2
	bx      lr
	ENDFUNC

read_vttbr FUNCTION
	mrrc    p15, 6, r0, r1, c2
	bx      lr
	ENDFUNC			
		
write_hcr FUNCTION
	mcr     p15, 4, r0, c1, c1, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

write_hdcr FUNCTION
	mcr     p15, 4, r0, c1, c1, 1
	bx      lr
	ENDFUNC		
	
write_hcptr FUNCTION
	mcr     p15, 4, r0, c1, c1, 2
	bx      lr
	ENDFUNC		
	
write_hstr FUNCTION
	mcr     p15, 4, r0, c1, c1, 3
	bx      lr
	ENDFUNC

write_httbr FUNCTION
	mcrr    p15, 4, r0, r1, c2
	mcr     p15, 0, r0, c7, c5, 6
	mcr     p15, 0, r0, c8, c7, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

write_vttbr FUNCTION
	mcrr    p15, 6, r0, r1, c2
	mcr     p15, 0, r0, c7, c5, 6
	mcr     p15, 0, r0, c8, c7, 0
	dsb
	isb	
	bx      lr
	ENDFUNC			
	
write_htcr FUNCTION
	mcr     p15, 4, r0, c2, c0, 2
	bx      lr
	ENDFUNC

write_vtcr FUNCTION
	mcr     p15, 4, r0, c2, c1, 2
	bx      lr
	ENDFUNC	

write_hmair0 FUNCTION
	mcr     p15, 4, r0, c10, c2, 0
	bx      lr
	ENDFUNC

write_hmair1 FUNCTION
	mcr     p15, 4, r0, c10, c2, 1
	bx      lr
	ENDFUNC									
						
read_nsacr FUNCTION
	mrc     p15, 0, r0, c1, c1, 2
	bx      lr
	ENDFUNC

read_l2ctlr FUNCTION
	mrc	p15, 1, r0, c9, c0, 2	
	bx      lr
	ENDFUNC

read_l2ectlr FUNCTION
	mrc	p15, 1, r0, c9, c0, 3	
	bx      lr
	ENDFUNC

read_pmuserenr FUNCTION
	mrc	p15, 0, r0, c9, c14, 0
	bx      lr
	ENDFUNC
	
read_pmintenset FUNCTION
	mrc	p15, 0, r0, c9, c14, 1
	bx      lr
	ENDFUNC				

read_pmintenclr FUNCTION
	mrc	p15, 0, r0, c9, c14, 2
	bx      lr
	ENDFUNC
	
read_pmovsset FUNCTION
	mrc	p15, 0, r0, c9, c14, 3
	bx      lr
	ENDFUNC
	
read_pmccntr FUNCTION
	mrc	p15, 0, r0, c9, c13, 0
	bx      lr
	ENDFUNC
	
read_pmxevtyper FUNCTION
	mrc	p15, 0, r0, c9, c13, 1
	bx      lr
	ENDFUNC
	
read_pmxevcntr FUNCTION
	mrc	p15, 0, r0, c9, c13, 2
	bx      lr
	ENDFUNC
	
read_pmcr FUNCTION
	mrc	p15, 0, r0, c9, c12, 0
	bx      lr
	ENDFUNC
	
read_pmcntenset FUNCTION
	mrc	p15, 0, r0, c9, c12, 1
	bx      lr
	ENDFUNC
	
read_pmcntenclr FUNCTION
	mrc	p15, 0, r0, c9, c12, 2
	bx      lr
	ENDFUNC
	
read_pmovsr FUNCTION
	mrc	p15, 0, r0, c9, c12, 3
	bx      lr
	ENDFUNC
	
read_pmswinc FUNCTION
	mrc	p15, 0, r0, c9, c12, 4
	bx      lr
	ENDFUNC
	
read_pmselr FUNCTION
	mrc	p15, 0, r0, c9, c12, 5
	bx      lr
	ENDFUNC
	
read_pmceid0 FUNCTION
	mrc	p15, 0, r0, c9, c12, 6
	bx      lr
	ENDFUNC				

read_pmceid1 FUNCTION
	mrc	p15, 0, r0, c9, c12, 7
	bx      lr
	ENDFUNC

write_l2ectlr FUNCTION
	mcr	p15, 1, r0, c9, c0, 3	
	bx      lr
	ENDFUNC

write_pmuserenr FUNCTION
	mcr	p15, 0, r0, c9, c14, 0
	bx      lr
	ENDFUNC
	
write_pmintenset FUNCTION
	mcr	p15, 0, r0, c9, c14, 1
	bx      lr
	ENDFUNC				

write_pmintenclr FUNCTION
	mcr	p15, 0, r0, c9, c14, 2
	bx      lr
	ENDFUNC
	
write_pmovsset FUNCTION
	mcr	p15, 0, r0, c9, c14, 3
	bx      lr
	ENDFUNC
	
write_pmccntr FUNCTION
	mcr	p15, 0, r0, c9, c13, 0
	bx      lr
	ENDFUNC
	
write_pmxevtyper FUNCTION
	mcr	p15, 0, r0, c9, c13, 1
	bx      lr
	ENDFUNC
	
write_pmxevcntr FUNCTION
	mcr	p15, 0, r0, c9, c13, 2
	bx      lr
	ENDFUNC
	
write_pmcr FUNCTION
	mcr	p15, 0, r0, c9, c12, 0
	bx      lr
	ENDFUNC
	
write_pmcntenset FUNCTION
	mcr	p15, 0, r0, c9, c12, 1
	bx      lr
	ENDFUNC
	
write_pmcntenclr FUNCTION
	mcr	p15, 0, r0, c9, c12, 2
	bx      lr
	ENDFUNC
	
write_pmovsr FUNCTION
	mcr	p15, 0, r0, c9, c12, 3
	bx      lr
	ENDFUNC
	
write_pmswinc FUNCTION
	mcr	p15, 0, r0, c9, c12, 4
	bx      lr
	ENDFUNC
	
write_pmselr FUNCTION
	mcr	p15, 0, r0, c9, c12, 5
	bx      lr
	ENDFUNC
	
write_pmceid0 FUNCTION
	mcr	p15, 0, r0, c9, c12, 6
	bx      lr
	ENDFUNC				

write_pmceid1 FUNCTION
	mcr	p15, 0, r0, c9, c12, 7
	bx      lr
	ENDFUNC					
		
read_sctlr FUNCTION        
	mrc     p15, 0, r0, c1, c0, 0
	bx      lr
	ENDFUNC

write_sctlr FUNCTION
	mcr     p15, 0, r0, c1, c0, 0
	dsb
	isb
	bx      lr
	ENDFUNC

read_hsctlr FUNCTION        
	mrc     p15, 4, r0, c1, c0, 0
	bx      lr
	ENDFUNC

read_hdfar FUNCTION
	mrc     p15, 4, r0, c6, c0, 0	
	bx      lr
	ENDFUNC
		
read_hpfar FUNCTION
	mrc     p15, 4, r0, c6, c0, 4		
	bx      lr
	ENDFUNC

read_hsr FUNCTION
	mrc     p15, 4, r0, c5, c2, 0
	bx      lr
	ENDFUNC

write_hsctlr FUNCTION
	mcr     p15, 4, r0, c1, c0, 0
	dsb
	isb	
	bx      lr
	ENDFUNC
	
read_cnthctl FUNCTION
	mrc	p15, 4, r0, c14, c1, 0
	bx      lr
	ENDFUNC

read_cntkctl FUNCTION
	mrc	p15, 0, r0, c14, c1, 0
	bx      lr
	ENDFUNC	

read_cnthp_cval FUNCTION
	mrrc	p15, 6, r0, r1, c14
	bx      lr
	ENDFUNC

read_cnthp_tval FUNCTION
	mrc	p15, 4, r0, c14, c2, 0
	bx      lr
	ENDFUNC

read_cntp_tval FUNCTION
	mrc	p15, 0, r0, c14, c2, 0
	bx      lr
	ENDFUNC
	
read_cntp_ctl FUNCTION
	mrc	p15, 0, r0, c14, c2, 1
	bx      lr
	ENDFUNC		
			
read_cnthp_ctl FUNCTION
	mrc	p15, 4, r0, c14, c2, 1
	bx      lr
	ENDFUNC		

write_cnthctl FUNCTION
	mcr	p15, 4, r0, c14, c1, 0
	bx      lr
	ENDFUNC

write_cntkctl FUNCTION
	mcr	p15, 0, r0, c14, c1, 0
	bx      lr
	ENDFUNC		

write_cntp_tval FUNCTION
	mcr	p15, 0, r0, c14, c2, 0
	isb
	bx      lr
	ENDFUNC		
		
write_cntp_ctl FUNCTION
	mcr	p15, 0, r0, c14, c2, 1
	dsb
	isb	
	bx      lr
	ENDFUNC		
		
write_cnthp_cval FUNCTION
	mcrr	p15, 6, r0, r1, c14
	dsb
	isb	
	bx      lr
	ENDFUNC

write_cnthp_tval FUNCTION
	mcr	p15, 4, r0, c14, c2, 0
	dsb
	isb	
	bx      lr
	ENDFUNC
	
write_cnthp_ctl FUNCTION
	mcr	p15, 4, r0, c14, c2, 1
	dsb
	isb	
	bx      lr
	ENDFUNC		
		
read_clidr FUNCTION        
	mrc     p15, 1, r0, c0, c0, 1           ; read clidr
	bx      lr
	ENDFUNC

read_ccsidr FUNCTION        
	mrc     p15, 1, r0, c0, c0, 0           ; read ccsidr
	bx      lr
	ENDFUNC

read_csselr FUNCTION        
	mrc     p15, 2, r0, c0, c0, 0           ; read csselr
	bx      lr
	ENDFUNC	
	
write_csselr FUNCTION        
	mcr     p15, 2, r0, c0, c0, 0           ; read csselr
	dsb
	isb	
	bx      lr
	ENDFUNC			
		
read_actlr FUNCTION        
	mrc     p15, 0, r0, c1, c0, 1
	bx      lr
	ENDFUNC

write_actlr FUNCTION
	mcr     p15, 0, r0, c1, c0, 1
	dsb
	isb		
	bx      lr
	ENDFUNC

read_prrr FUNCTION        
	mrc     p15, 0, r0, c10, c2, 0
	bx      lr
	ENDFUNC

read_nmrr FUNCTION        
	mrc     p15, 0, r0, c10, c2, 1
	bx      lr
	ENDFUNC

write_prrr FUNCTION        
	mcr     p15, 0, r0, c10, c2, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

write_nmrr FUNCTION        
	mcr     p15, 0, r0, c10, c2, 1
	dsb
	isb	
	bx      lr
	ENDFUNC	

read_dfar FUNCTION
	mrc     p15, 0, r0, c6, c0, 0
	bx      lr
	ENDFUNC
	
read_ifar FUNCTION
	mrc     p15, 0, r0, c6, c0, 2	
	bx      lr
	ENDFUNC
	
read_dfsr FUNCTION
	mrc     p15, 0, r0, c5, c0, 0	
	bx      lr
	ENDFUNC
	
read_ifsr FUNCTION
	mrc     p15, 0, r0, c5, c0, 1	
	bx      lr
	ENDFUNC
		
read_adfsr FUNCTION
	mrc     p15, 0, r0, c5, c1, 0
	bx      lr
	ENDFUNC
	
read_aifsr FUNCTION
	mrc     p15, 0, r0, c5, c1, 1	
	bx      lr
	ENDFUNC
		
write_dfar FUNCTION
	mcr     p15, 0, r0, c6, c0, 0
	dsb
	isb
	bx      lr
	ENDFUNC

write_ifar FUNCTION
	mcr     p15, 0, r0, c6, c0, 2
	dsb
	isb	
	bx      lr
	ENDFUNC

write_dfsr FUNCTION
	mcr     p15, 0, r0, c5, c0, 0
	dsb
	isb	
	bx      lr
	ENDFUNC

write_ifsr FUNCTION
	mcr     p15, 0, r0, c5, c0, 1
	dsb
	isb		
	bx      lr
	ENDFUNC
	
write_adfsr FUNCTION
	mcr     p15, 0, r0, c5, c1, 0
	dsb
	isb		
	bx      lr
	ENDFUNC
	
write_aifsr FUNCTION
	mcr     p15, 0, r0, c5, c1, 1
	dsb
	isb		
	bx      lr
	ENDFUNC
	
read_cbar	FUNCTION
	mrc	p15, 4, r0, c15, c0, 0	; Read Configuration Base Address Register
	bx	lr
	ENDFUNC

read_drar	FUNCTION
	mrc	p14, 0, r0, c1, c0, 0	; Read Debug ROM Address Register
	bx	lr
	ENDFUNC

read_dsar	FUNCTION
	mrc	p14, 0, r0, c2, c0, 0	; Read Debug Self Address Offset Register
	bx	lr
	ENDFUNC

write_osdlr	FUNCTION
	mcr	p14, 0, r0, c1, c3, 4	; Write OS Double Lock Register
	bx	lr
	ENDFUNC

get_cpu_type	FUNCTION
	mrc	p15, 0, r0, c0, c0, 0; read MIDR
	ldr	r1, =MIDR_CPU_MASK
	ands	r0, r1
	bx	lr
	ENDFUNC

dsb	FUNCTION
	dsb
	bx	lr
        ENDFUNC
        
va_to_pa FUNCTION	; Note: assumes conversion will be successful!
	mov	r1, r0
	mcr	p15, 0, r0, c7, c8, 1	; Priv Write Current World VA-PA
	mrc	p15, 0, r0, c7, c4, 0	; Get PA
	bfc	r0, #0, #12		; We want top bits of translated addr
	bfc	r1, #12, #20		; plus bottom bits of input addr
	orr	r0, r0, r1
	bx	lr
	ENDFUNC

	END