a5ds: Hold the secondary cpus in pen rather than panic

For the secondary CPUs, hold the cpu in wfe rather then panic.
This will be needed when multicore support is added to a5ds as
the smc call will write to the hold base and signal an event to
power on the secondary CPUs.

Change-Id: I0ffc2059e9ef894c21375ca5c94def859bfa6599
Signed-off-by: Usama Arif <usama.arif@arm.com>

diff --git a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
index 23a22d9c5e8671a969059ea6c773e80556020458..ed7ad9c867651042b147ca2177df5882f53e3a7b 100644 (file)
--- a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
+++ b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
@@ -12,17 +12,36 @@
        .globl  plat_get_my_entrypoint
        .globl  plat_is_my_cpu_primary
 
-       /* --------------------------------------------------------------------
+       /* -----------------------------------------------------
         * void plat_secondary_cold_boot_setup (void);
         *
-        * For AArch32, cold-booting secondary CPUs is not yet
-        * implemented and they panic.
-        * --------------------------------------------------------------------
+        * This function performs any platform specific actions
+        * needed for a secondary cpu after a cold reset e.g
+        * mark the cpu's presence, mechanism to place it in a
+        * holding pen etc.
+        * -----------------------------------------------------
         */
 func plat_secondary_cold_boot_setup
-cb_panic:
-       wfi
-       b       cb_panic
+       /* Calculate address of our hold entry */
+       bl      plat_my_core_pos
+       lsl     r0, r0, #A5DS_HOLD_ENTRY_SHIFT
+       mov_imm r2, A5DS_HOLD_BASE
+       /* Clear the value stored in the hold address for the specific core */
+       mov_imm r3, A5DS_HOLD_STATE_WAIT
+       str     r3, [r2, r0]
+       dmb     ish
+
+       /* Wait until we have a go */
+poll_mailbox:
+       ldr     r1, [r2, r0]
+       cmp     r1, #A5DS_HOLD_STATE_WAIT
+       beq     1f
+       mov_imm r0, A5DS_TRUSTED_MAILBOX_BASE
+       ldr     r1, [r0]
+       bx      r1
+1:
+       wfe
+       b       poll_mailbox
 endfunc plat_secondary_cold_boot_setup
 
        /* ---------------------------------------------------------------------
@@ -56,3 +75,52 @@ func plat_is_my_cpu_primary
        movne   r0, #0
        bx      lr
 endfunc plat_is_my_cpu_primary
+
+       /* ---------------------------------------------------------------------
+        * Loads MPIDR in r0 and calls plat_arm_calc_core_pos
+        * ---------------------------------------------------------------------
+        */
+func plat_my_core_pos
+       ldcopr  r0, MPIDR
+       b       plat_arm_calc_core_pos
+
+endfunc plat_my_core_pos
+
+       /* ---------------------------------------------------------------------
+        * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+        *
+        * Function to calculate the core position on A5DS.
+        *
+        * (ClusterId * A5DS_MAX_CPUS_PER_CLUSTER * A5DS_MAX_PE_PER_CPU) +
+        * (CPUId * A5DS_MAX_PE_PER_CPU) +
+        * ThreadId
+        *
+        * which can be simplified as:
+        *
+        * ((ClusterId * A5DS_MAX_CPUS_PER_CLUSTER + CPUId) * A5DS_MAX_PE_PER_CPU)
+        * + ThreadId
+        * ---------------------------------------------------------------------
+        */
+func plat_arm_calc_core_pos
+       mov     r3, r0
+
+       /*
+        * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
+        * look as if in a multi-threaded implementation
+        */
+       tst     r0, #MPIDR_MT_MASK
+       lsleq   r3, r0, #MPIDR_AFFINITY_BITS
+
+       /* Extract individual affinity fields from MPIDR */
+       ubfx    r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+       ubfx    r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+       ubfx    r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+       /* Compute linear position */
+       mov     r3, #A5DS_MAX_CPUS_PER_CLUSTER
+       mla     r1, r2, r3, r1
+       mov     r3, #A5DS_MAX_PE_PER_CPU
+       mla     r0, r1, r3, r0
+
+       bx      lr
+endfunc plat_arm_calc_core_pos

diff --git a/plat/arm/board/a5ds/include/platform_def.h b/plat/arm/board/a5ds/include/platform_def.h
index db65c377825bb1569fccab6e2dfdddc825efbc42..13c19343f95ffc3933cd2caef423446d0188cb7c 100644 (file)
--- a/plat/arm/board/a5ds/include/platform_def.h
+++ b/plat/arm/board/a5ds/include/platform_def.h
@@ -325,6 +325,14 @@
 
 /* Mailbox base address */
 #define A5DS_TRUSTED_MAILBOX_BASE      A5DS_SHARED_RAM_BASE
+#define A5DS_TRUSTED_MAILBOX_SIZE      (8 + A5DS_HOLD_SIZE)
+#define A5DS_HOLD_BASE         (A5DS_TRUSTED_MAILBOX_BASE + 8)
+#define A5DS_HOLD_SIZE         (PLATFORM_CORE_COUNT * \
+                                        A5DS_HOLD_ENTRY_SIZE)
+#define A5DS_HOLD_ENTRY_SHIFT  3
+#define A5DS_HOLD_ENTRY_SIZE   (1 << A5DS_HOLD_ENTRY_SHIFT)
+#define A5DS_HOLD_STATE_WAIT   0
+#define A5DS_HOLD_STATE_GO     1
 
 /*
  * GIC related constants to cater for GICv2