1/* 2 * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7#include <arch.h> 8#include <asm_macros.S> 9#include <platform_def.h> 10#include <cortex_a75.h> 11#include <cortex_a55.h> 12#include <cpu_macros.S> 13 14 .globl plat_arm_calc_core_pos 15 .globl plat_reset_handler 16 17 /* --------------------------------------------------------------------- 18 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr) 19 * 20 * Function to calculate the core position on FVP. 21 * 22 * (ClusterId * MAX_CPUS_PER_CLUSTER * MAX_PE_PER_CPU) + 23 * (CPUId * MAX_PE_PER_CPU) + 24 * ThreadId 25 * 26 * which can be simplified as: 27 * 28 * ((ClusterId * MAX_CPUS_PER_CLUSTER + CPUId) * MAX_PE_PER_CPU) 29 * + ThreadId 30 * --------------------------------------------------------------------- 31 */ 32func plat_arm_calc_core_pos 33 /* 34 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it 35 * look as if in a multi-threaded implementation. 36 */ 37 tst x0, #MPIDR_MT_MASK 38 lsr x3, x0, #MPIDR_AFFINITY_BITS 39 csel x3, x3, x0, eq 40 41 /* Extract individual affinity fields from MPIDR */ 42 ubfx x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS 43 ubfx x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 44 ubfx x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 45 46 /* Compute linear position */ 47 mov x4, #PLAT_MAX_CPUS_PER_CLUSTER 48 madd x1, x2, x4, x1 49 mov x5, #PLAT_MAX_PE_PER_CPU 50 madd x0, x1, x5, x0 51 ret 52endfunc plat_arm_calc_core_pos 53 54 /* ----------------------------------------------------- 55 * void plat_reset_handler(void); 56 * 57 * Determine the CPU MIDR and disable power down bit for 58 * that CPU. 59 * ----------------------------------------------------- 60 */ 61func plat_reset_handler 62 jump_if_cpu_midr CORTEX_A75_MIDR, A75 63 jump_if_cpu_midr CORTEX_A55_MIDR, A55 64 ret 65 66 /* ----------------------------------------------------- 67 * Disable CPU power down bit in power control register 68 * ----------------------------------------------------- 69 */ 70A75: 71 mrs x0, CORTEX_A75_CPUPWRCTLR_EL1 72 bic x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK 73 msr CORTEX_A75_CPUPWRCTLR_EL1, x0 74 isb 75 ret 76A55: 77 mrs x0, CORTEX_A55_CPUPWRCTLR_EL1 78 bic x0, x0, #CORTEX_A55_CORE_PWRDN_EN_MASK 79 msr CORTEX_A55_CPUPWRCTLR_EL1, x0 80 isb 81 ret 82endfunc plat_reset_handler 83