diff options
Diffstat (limited to 'acsr/helpers.s')
| -rw-r--r-- | acsr/helpers.s | 1065 |
1 files changed, 1065 insertions, 0 deletions
diff --git a/acsr/helpers.s b/acsr/helpers.s new file mode 100644 index 0000000..99bb2a6 --- /dev/null +++ b/acsr/helpers.s @@ -0,0 +1,1065 @@ + ; + ; 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 |