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: r0.
26 */
27func mtpmu_supported
28	ldcopr	r0, ID_DFR1
29	and	r0, r0, #(ID_DFR1_MTPMU_MASK >> ID_DFR1_MTPMU_SHIFT)
30	cmp	r0, #ID_DFR1_MTPMU_SUPPORTED
31	mov	r0, #0
32	addeq	r0, r0, #1
33	bx	lr
34endfunc mtpmu_supported
35
36/*
37 * bool el_implemented(unsigned int el)
38 *
39 * Return a boolean indicating if the specified EL (2 or 3) is implemented.
40 *
41 * Trash registers: r0
42 */
43func el_implemented
44	cmp	r0, #3
45	ldcopr	r0, ID_PFR1
46	lsreq	r0, r0, #ID_PFR1_SEC_SHIFT
47	lsrne	r0, r0, #ID_PFR1_VIRTEXT_SHIFT
48	/*
49	 * ID_PFR1_VIRTEXT_MASK is the same as ID_PFR1_SEC_MASK
50	 * so use any one of them
51	 */
52	and	r0, r0, #ID_PFR1_VIRTEXT_MASK
53	cmp	r0, #ID_PFR1_ELx_ENABLED
54	mov	r0, #0
55	addeq	r0, r0, #1
56	bx	lr
57endfunc el_implemented
58
59/*
60 * void mtpmu_disable(void)
61 *
62 * Disable mtpmu feature if supported.
63 *
64 * Trash register: r0, r1, r2
65 */
66func mtpmu_disable
67	mov	r2, lr
68	bl	mtpmu_supported
69	cmp	r0, #0
70	bxeq	r2	/* FEAT_MTPMU not supported */
71
72	/* FEAT_MTMPU Supported */
73	mov	r0, #3
74	bl	el_implemented
75	cmp	r0, #0
76	beq	1f
77
78	/* EL3 implemented */
79	ldcopr	r0, SDCR
80	ldr	r1, =SDCR_MTPME_BIT
81	bic	r0, r0, r1
82	stcopr	r0, SDCR
83
84	/*
85	 * If EL3 is implemented, HDCR.MTPME is implemented as Res0 and
86	 * FEAT_MTPMU is controlled only from EL3, so no need to perform
87	 * any operations for EL2.
88	 */
89	isb
90	bx	r2
911:
92	/* EL3 not implemented */
93	mov	r0, #2
94	bl	el_implemented
95	cmp	r0, #0
96	bxeq	r2	/* No EL2 or EL3 implemented */
97
98	/* EL2 implemented */
99	ldcopr	r0, HDCR
100	ldr	r1, =HDCR_MTPME_BIT
101	orr	r0, r0, r1
102	stcopr	r0, HDCR
103	isb
104	bx	r2
105endfunc mtpmu_disable
106