1*54fd6939SJiyong Park/* 2*54fd6939SJiyong Park * Copyright (c) 2020, Arm Limited. All rights reserved. 3*54fd6939SJiyong Park * 4*54fd6939SJiyong Park * SPDX-License-Identifier: BSD-3-Clause 5*54fd6939SJiyong Park */ 6*54fd6939SJiyong Park 7*54fd6939SJiyong Park#include <arch.h> 8*54fd6939SJiyong Park#include <asm_macros.S> 9*54fd6939SJiyong Park 10*54fd6939SJiyong Park .global mtpmu_disable 11*54fd6939SJiyong Park 12*54fd6939SJiyong Park/* ------------------------------------------------------------- 13*54fd6939SJiyong Park * The functions in this file are called at entrypoint, before 14*54fd6939SJiyong Park * the CPU has decided whether this is a cold or a warm boot. 15*54fd6939SJiyong Park * Therefore there are no stack yet to rely on for a C function 16*54fd6939SJiyong Park * call. 17*54fd6939SJiyong Park * ------------------------------------------------------------- 18*54fd6939SJiyong Park */ 19*54fd6939SJiyong Park 20*54fd6939SJiyong Park/* 21*54fd6939SJiyong Park * bool mtpmu_supported(void) 22*54fd6939SJiyong Park * 23*54fd6939SJiyong Park * Return a boolean indicating whether FEAT_MTPMU is supported or not. 24*54fd6939SJiyong Park * 25*54fd6939SJiyong Park * Trash registers: r0. 26*54fd6939SJiyong Park */ 27*54fd6939SJiyong Parkfunc mtpmu_supported 28*54fd6939SJiyong Park ldcopr r0, ID_DFR1 29*54fd6939SJiyong Park and r0, r0, #(ID_DFR1_MTPMU_MASK >> ID_DFR1_MTPMU_SHIFT) 30*54fd6939SJiyong Park cmp r0, #ID_DFR1_MTPMU_SUPPORTED 31*54fd6939SJiyong Park mov r0, #0 32*54fd6939SJiyong Park addeq r0, r0, #1 33*54fd6939SJiyong Park bx lr 34*54fd6939SJiyong Parkendfunc mtpmu_supported 35*54fd6939SJiyong Park 36*54fd6939SJiyong Park/* 37*54fd6939SJiyong Park * bool el_implemented(unsigned int el) 38*54fd6939SJiyong Park * 39*54fd6939SJiyong Park * Return a boolean indicating if the specified EL (2 or 3) is implemented. 40*54fd6939SJiyong Park * 41*54fd6939SJiyong Park * Trash registers: r0 42*54fd6939SJiyong Park */ 43*54fd6939SJiyong Parkfunc el_implemented 44*54fd6939SJiyong Park cmp r0, #3 45*54fd6939SJiyong Park ldcopr r0, ID_PFR1 46*54fd6939SJiyong Park lsreq r0, r0, #ID_PFR1_SEC_SHIFT 47*54fd6939SJiyong Park lsrne r0, r0, #ID_PFR1_VIRTEXT_SHIFT 48*54fd6939SJiyong Park /* 49*54fd6939SJiyong Park * ID_PFR1_VIRTEXT_MASK is the same as ID_PFR1_SEC_MASK 50*54fd6939SJiyong Park * so use any one of them 51*54fd6939SJiyong Park */ 52*54fd6939SJiyong Park and r0, r0, #ID_PFR1_VIRTEXT_MASK 53*54fd6939SJiyong Park cmp r0, #ID_PFR1_ELx_ENABLED 54*54fd6939SJiyong Park mov r0, #0 55*54fd6939SJiyong Park addeq r0, r0, #1 56*54fd6939SJiyong Park bx lr 57*54fd6939SJiyong Parkendfunc el_implemented 58*54fd6939SJiyong Park 59*54fd6939SJiyong Park/* 60*54fd6939SJiyong Park * void mtpmu_disable(void) 61*54fd6939SJiyong Park * 62*54fd6939SJiyong Park * Disable mtpmu feature if supported. 63*54fd6939SJiyong Park * 64*54fd6939SJiyong Park * Trash register: r0, r1, r2 65*54fd6939SJiyong Park */ 66*54fd6939SJiyong Parkfunc mtpmu_disable 67*54fd6939SJiyong Park mov r2, lr 68*54fd6939SJiyong Park bl mtpmu_supported 69*54fd6939SJiyong Park cmp r0, #0 70*54fd6939SJiyong Park bxeq r2 /* FEAT_MTPMU not supported */ 71*54fd6939SJiyong Park 72*54fd6939SJiyong Park /* FEAT_MTMPU Supported */ 73*54fd6939SJiyong Park mov r0, #3 74*54fd6939SJiyong Park bl el_implemented 75*54fd6939SJiyong Park cmp r0, #0 76*54fd6939SJiyong Park beq 1f 77*54fd6939SJiyong Park 78*54fd6939SJiyong Park /* EL3 implemented */ 79*54fd6939SJiyong Park ldcopr r0, SDCR 80*54fd6939SJiyong Park ldr r1, =SDCR_MTPME_BIT 81*54fd6939SJiyong Park bic r0, r0, r1 82*54fd6939SJiyong Park stcopr r0, SDCR 83*54fd6939SJiyong Park 84*54fd6939SJiyong Park /* 85*54fd6939SJiyong Park * If EL3 is implemented, HDCR.MTPME is implemented as Res0 and 86*54fd6939SJiyong Park * FEAT_MTPMU is controlled only from EL3, so no need to perform 87*54fd6939SJiyong Park * any operations for EL2. 88*54fd6939SJiyong Park */ 89*54fd6939SJiyong Park isb 90*54fd6939SJiyong Park bx r2 91*54fd6939SJiyong Park1: 92*54fd6939SJiyong Park /* EL3 not implemented */ 93*54fd6939SJiyong Park mov r0, #2 94*54fd6939SJiyong Park bl el_implemented 95*54fd6939SJiyong Park cmp r0, #0 96*54fd6939SJiyong Park bxeq r2 /* No EL2 or EL3 implemented */ 97*54fd6939SJiyong Park 98*54fd6939SJiyong Park /* EL2 implemented */ 99*54fd6939SJiyong Park ldcopr r0, HDCR 100*54fd6939SJiyong Park ldr r1, =HDCR_MTPME_BIT 101*54fd6939SJiyong Park orr r0, r0, r1 102*54fd6939SJiyong Park stcopr r0, HDCR 103*54fd6939SJiyong Park isb 104*54fd6939SJiyong Park bx r2 105*54fd6939SJiyong Parkendfunc mtpmu_disable 106