/*
 * 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