diff options
Diffstat (limited to 'lib/extensions/mtpmu/aarch32/mtpmu.S')
| -rw-r--r-- | lib/extensions/mtpmu/aarch32/mtpmu.S | 105 |
1 files changed, 105 insertions, 0 deletions
diff --git a/lib/extensions/mtpmu/aarch32/mtpmu.S b/lib/extensions/mtpmu/aarch32/mtpmu.S new file mode 100644 index 0000000..834cee3 --- /dev/null +++ b/lib/extensions/mtpmu/aarch32/mtpmu.S @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2020, Arm Limited. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include <arch.h> +#include <asm_macros.S> + + .global mtpmu_disable + +/* ------------------------------------------------------------- + * The functions in this file are called at entrypoint, before + * the CPU has decided whether this is a cold or a warm boot. + * Therefore there are no stack yet to rely on for a C function + * call. + * ------------------------------------------------------------- + */ + +/* + * bool mtpmu_supported(void) + * + * Return a boolean indicating whether FEAT_MTPMU is supported or not. + * + * Trash registers: r0. + */ +func mtpmu_supported + ldcopr r0, ID_DFR1 + and r0, r0, #(ID_DFR1_MTPMU_MASK >> ID_DFR1_MTPMU_SHIFT) + cmp r0, #ID_DFR1_MTPMU_SUPPORTED + mov r0, #0 + addeq r0, r0, #1 + bx lr +endfunc mtpmu_supported + +/* + * bool el_implemented(unsigned int el) + * + * Return a boolean indicating if the specified EL (2 or 3) is implemented. + * + * Trash registers: r0 + */ +func el_implemented + cmp r0, #3 + ldcopr r0, ID_PFR1 + lsreq r0, r0, #ID_PFR1_SEC_SHIFT + lsrne r0, r0, #ID_PFR1_VIRTEXT_SHIFT + /* + * ID_PFR1_VIRTEXT_MASK is the same as ID_PFR1_SEC_MASK + * so use any one of them + */ + and r0, r0, #ID_PFR1_VIRTEXT_MASK + cmp r0, #ID_PFR1_ELx_ENABLED + mov r0, #0 + addeq r0, r0, #1 + bx lr +endfunc el_implemented + +/* + * void mtpmu_disable(void) + * + * Disable mtpmu feature if supported. + * + * Trash register: r0, r1, r2 + */ +func mtpmu_disable + mov r2, lr + bl mtpmu_supported + cmp r0, #0 + bxeq r2 /* FEAT_MTPMU not supported */ + + /* FEAT_MTMPU Supported */ + mov r0, #3 + bl el_implemented + cmp r0, #0 + beq 1f + + /* EL3 implemented */ + ldcopr r0, SDCR + ldr r1, =SDCR_MTPME_BIT + bic r0, r0, r1 + stcopr r0, SDCR + + /* + * If EL3 is implemented, HDCR.MTPME is implemented as Res0 and + * FEAT_MTPMU is controlled only from EL3, so no need to perform + * any operations for EL2. + */ + isb + bx r2 +1: + /* EL3 not implemented */ + mov r0, #2 + bl el_implemented + cmp r0, #0 + bxeq r2 /* No EL2 or EL3 implemented */ + + /* EL2 implemented */ + ldcopr r0, HDCR + ldr r1, =HDCR_MTPME_BIT + orr r0, r0, r1 + stcopr r0, HDCR + isb + bx r2 +endfunc mtpmu_disable |