1/* 2 * Copyright (c) 2019, 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#include <platform_def.h> 10 11 .globl plat_secondary_cold_boot_setup 12 .globl plat_get_my_entrypoint 13 .globl plat_is_my_cpu_primary 14 15 /* ----------------------------------------------------- 16 * void plat_secondary_cold_boot_setup (void); 17 * 18 * This function performs any platform specific actions 19 * needed for a secondary cpu after a cold reset e.g 20 * mark the cpu's presence, mechanism to place it in a 21 * holding pen etc. 22 * ----------------------------------------------------- 23 */ 24func plat_secondary_cold_boot_setup 25 /* Calculate address of our hold entry */ 26 bl plat_my_core_pos 27 lsl r0, r0, #A5DS_HOLD_ENTRY_SHIFT 28 mov_imm r2, A5DS_HOLD_BASE 29 /* Clear the value stored in the hold address for the specific core */ 30 mov_imm r3, A5DS_HOLD_STATE_WAIT 31 str r3, [r2, r0] 32 dmb ish 33 34 /* Wait until we have a go */ 35poll_mailbox: 36 ldr r1, [r2, r0] 37 cmp r1, #A5DS_HOLD_STATE_WAIT 38 beq 1f 39 mov_imm r0, A5DS_TRUSTED_MAILBOX_BASE 40 ldr r1, [r0] 41 bx r1 421: 43 wfe 44 b poll_mailbox 45endfunc plat_secondary_cold_boot_setup 46 47 /* --------------------------------------------------------------------- 48 * unsigned long plat_get_my_entrypoint (void); 49 * 50 * Main job of this routine is to distinguish between a cold and warm 51 * boot. 52 * --------------------------------------------------------------------- 53 */ 54func plat_get_my_entrypoint 55 /* TODO support warm boot */ 56 /* Cold reset */ 57 mov r0, #0 58 bx lr 59 60endfunc plat_get_my_entrypoint 61 62 /* ----------------------------------------------------- 63 * unsigned int plat_is_my_cpu_primary (void); 64 * 65 * Find out whether the current cpu is the primary 66 * cpu. 67 * ----------------------------------------------------- 68 */ 69func plat_is_my_cpu_primary 70 ldcopr r0, MPIDR 71 ldr r1, =MPIDR_AFFINITY_MASK 72 and r0, r1 73 cmp r0, #0 74 moveq r0, #1 75 movne r0, #0 76 bx lr 77endfunc plat_is_my_cpu_primary 78 79 /* --------------------------------------------------------------------- 80 * Loads MPIDR in r0 and calls plat_arm_calc_core_pos 81 * --------------------------------------------------------------------- 82 */ 83func plat_my_core_pos 84 ldcopr r0, MPIDR 85 b plat_arm_calc_core_pos 86 87endfunc plat_my_core_pos 88 89 /* --------------------------------------------------------------------- 90 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr) 91 * 92 * Function to calculate the core position on A5DS. 93 * 94 * (ClusterId * A5DS_MAX_CPUS_PER_CLUSTER * A5DS_MAX_PE_PER_CPU) + 95 * (CPUId * A5DS_MAX_PE_PER_CPU) + 96 * ThreadId 97 * 98 * which can be simplified as: 99 * 100 * ((ClusterId * A5DS_MAX_CPUS_PER_CLUSTER + CPUId) * A5DS_MAX_PE_PER_CPU) 101 * + ThreadId 102 * --------------------------------------------------------------------- 103 */ 104func plat_arm_calc_core_pos 105 mov r3, r0 106 107 /* 108 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it 109 * look as if in a multi-threaded implementation 110 */ 111 tst r0, #MPIDR_MT_MASK 112 lsleq r3, r0, #MPIDR_AFFINITY_BITS 113 114 /* Extract individual affinity fields from MPIDR */ 115 ubfx r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS 116 ubfx r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 117 ubfx r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 118 119 /* Compute linear position */ 120 mov r3, #A5DS_MAX_CPUS_PER_CLUSTER 121 mla r1, r2, r3, r1 122 mov r3, #A5DS_MAX_PE_PER_CPU 123 mla r0, r1, r3, r0 124 125 bx lr 126endfunc plat_arm_calc_core_pos 127