1/*
2 * Copyright (c) 2020, Arm Limited. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <arch.h>
8#include <asm_macros.S>
9
10	.global	mtpmu_disable
11
12/* -------------------------------------------------------------
13 * The functions in this file are called at entrypoint, before
14 * the CPU has decided whether this is a cold or a warm boot.
15 * Therefore there are no stack yet to rely on for a C function
16 * call.
17 * -------------------------------------------------------------
18 */
19
20/*
21 * bool mtpmu_supported(void)
22 *
23 * Return a boolean indicating whether FEAT_MTPMU is supported or not.
24 *
25 * Trash registers: x0, x1
26 */
27func mtpmu_supported
28	mrs	x0, id_aa64dfr0_el1
29	mov_imm	x1, ID_AA64DFR0_MTPMU_MASK
30	and	x0, x1, x0, LSR #ID_AA64DFR0_MTPMU_SHIFT
31	cmp	x0, ID_AA64DFR0_MTPMU_SUPPORTED
32	cset	x0, eq
33	ret
34endfunc mtpmu_supported
35
36/*
37 * bool el_implemented(unsigned int el_shift)
38 *
39 * Return a boolean indicating if the specified EL is implemented.
40 * The EL is represented as the bitmask shift on id_aa64pfr0_el1 register.
41 *
42 * Trash registers: x0, x1
43 */
44func el_implemented
45	mrs	x1, id_aa64pfr0_el1
46	lsr	x1, x1, x0
47	cmp	x1, #ID_AA64PFR0_ELX_MASK
48	cset	x0, eq
49	ret
50endfunc el_implemented
51
52/*
53 * void mtpmu_disable(void)
54 *
55 * Disable mtpmu feature if supported.
56 *
57 * Trash register: x0, x1, x30
58 */
59func mtpmu_disable
60	mov	x10, x30
61	bl	mtpmu_supported
62	cbz	x0, exit_disable
63
64	/* FEAT_MTMPU Supported */
65	mov_imm	x0, ID_AA64PFR0_EL3_SHIFT
66	bl	el_implemented
67	cbz	x0, 1f
68
69	/* EL3 implemented */
70	mrs	x0, mdcr_el3
71	mov_imm x1, MDCR_MTPME_BIT
72	bic	x0, x0, x1
73	msr	mdcr_el3, x0
74
75	/*
76	 * If EL3 is implemented, MDCR_EL2.MTPME is implemented as Res0 and
77	 * FEAT_MTPMU is controlled only from EL3, so no need to perform
78	 * any operations for EL2.
79	 */
80	isb
81exit_disable:
82	ret	x10
831:
84	/* EL3 not implemented */
85	mov_imm	x0, ID_AA64PFR0_EL2_SHIFT
86	bl	el_implemented
87	cbz	x0, exit_disable
88
89	/* EL2 implemented */
90	mrs	x0, mdcr_el2
91	mov_imm x1, MDCR_EL2_MTPME
92	bic	x0, x0, x1
93	msr	mdcr_el2, x0
94	isb
95	ret	x10
96endfunc mtpmu_disable
97