ARM platforms: Add support for SGI575
Add support for System Guidance for Infrastructure platform SGI575.
Change-Id: I0125c2ed4469fbc8367dafcc8adce770b6b3147d
Signed-off-by: Nariman Poushin <nariman.poushin@linaro.org>
diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
new file mode 100644
index 0000000..c435d8b
--- /dev/null
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <platform_def.h>
+
+ .globl plat_is_my_cpu_primary
+ .globl plat_arm_calc_core_pos
+
+ /* -----------------------------------------------------
+ * unsigned int plat_is_my_cpu_primary (void);
+ *
+ * Find out whether the current cpu is the primary
+ * cpu (applicable only after a cold boot)
+ * -----------------------------------------------------
+ */
+func plat_is_my_cpu_primary
+ mov x9, x30
+ bl plat_my_core_pos
+ ldr x1, =SGI_BOOT_CFG_ADDR
+ ldr x1, [x1]
+ ubfx x1, x1, #PLAT_CSS_PRIMARY_CPU_SHIFT, \
+ #PLAT_CSS_PRIMARY_CPU_BIT_WIDTH
+ cmp x0, x1
+ cset w0, eq
+ ret x9
+endfunc plat_is_my_cpu_primary
+
+ /* -----------------------------------------------------
+ * unsigned int plat_arm_calc_core_pos(uint64_t mpidr)
+ * Helper function to calculate the core position.
+ * -----------------------------------------------------
+ */
+func plat_arm_calc_core_pos
+ mrs x2, mpidr_el1
+ ands x2, x2, #MPIDR_MT_MASK
+ beq 1f
+ lsr x0, x0, #MPIDR_AFF1_SHIFT
+1:
+ and x1, x0, #MPIDR_CPU_MASK
+ and x0, x0, #MPIDR_CLUSTER_MASK
+ add x0, x1, x0, LSR #6
+ and x0, x0, #MPIDR_AFFLVL_MASK
+ ret
+endfunc plat_arm_calc_core_pos