Merge pull request #1387 from vishwanathahg/sgi575/core_pos_calc

Sgi575/core pos calc

7 files changed, 44 insertions, 57 deletions

diff --git a/plat/arm/css/common/css_topology.c b/plat/arm/css/common/css_topology.c
index bccf2c820..42f94554d 100644
--- a/plat/arm/css/common/css_topology.c
+++ b/plat/arm/css/common/css_topology.c
@@ -6,6 +6,7 @@
 
 #include <plat_arm.h>
 #include <platform.h>
+#include <assert.h>
 
 #if ARM_PLAT_MT
 #pragma weak plat_arm_get_cpu_pe_count
@@ -19,9 +20,12 @@
  *****************************************************************************/
 int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
-	if (arm_check_mpidr(mpidr) == 0)
+	if (arm_check_mpidr(mpidr) == 0) {
+#if ARM_PLAT_MT
+		assert((read_mpidr_el1() & MPIDR_MT_MASK) != 0);
+#endif
 		return plat_arm_calc_core_pos(mpidr);
-
+	}
 	return -1;
 }
 
diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
index c435d8b41..aaa51560f 100644
--- a/plat/arm/css/sgi/aarch64/sgi_helper.S
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -31,19 +31,37 @@ func plat_is_my_cpu_primary
 endfunc plat_is_my_cpu_primary
 
 	/* -----------------------------------------------------
-	*  unsigned int plat_arm_calc_core_pos(uint64_t mpidr)
-	*  Helper function to calculate the core position.
-	* -----------------------------------------------------
-	*/
+	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *
+	 * Helper function to calculate the core position.
+	 * (ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER * CSS_SGI_MAX_PE_PER_CPU) +
+	 * (CPUId * CSS_SGI_MAX_PE_PER_CPU) +
+	 * ThreadId
+	 *
+	 * which can be simplified as:
+	 *
+	 * ((ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER + CPUId) *
+	 * CSS_SGI_MAX_PE_PER_CPU) + ThreadId
+	 * ------------------------------------------------------
+	 */
+
 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
+	mov	x3, x0
+
+	/*
+	 * The MT bit in MPIDR is always set for SGI platforms
+	 * and the affinity level 0 corresponds to thread affinity level.
+	 */
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx    x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx    x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx    x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov     x4, #CSS_SGI_MAX_CPUS_PER_CLUSTER
+	madd    x1, x2, x4, x1
+	mov     x5, #CSS_SGI_MAX_PE_PER_CPU
+	madd    x0, x1, x5, x0
 	ret
 endfunc plat_arm_calc_core_pos
diff --git a/plat/arm/css/sgi/include/platform_def.h b/plat/arm/css/sgi/include/platform_def.h
index 49a33ad6c..0f024076d 100644
--- a/plat/arm/css/sgi/include/platform_def.h
+++ b/plat/arm/css/sgi/include/platform_def.h
@@ -14,12 +14,14 @@
 #include <css_def.h>
 #include <soc_css_def.h>
 
-#define CSS_SGI_MAX_CORES_PER_CLUSTER	4
+#define CSS_SGI_MAX_CPUS_PER_CLUSTER	4
 
 /* CPU topology */
 #define PLAT_ARM_CLUSTER_COUNT		2
+#define CSS_SGI_MAX_PE_PER_CPU		1
 #define PLATFORM_CORE_COUNT		(PLAT_ARM_CLUSTER_COUNT *	\
-					CSS_SGI_MAX_CORES_PER_CLUSTER)
+					CSS_SGI_MAX_CPUS_PER_CLUSTER * \
+					CSS_SGI_MAX_PE_PER_CPU)
 
 #if ARM_BOARD_OPTIMISE_MEM
 
diff --git a/plat/arm/css/sgi/sgi-common.mk b/plat/arm/css/sgi/sgi-common.mk
index 659351a8b..f6ef95a03 100644
--- a/plat/arm/css/sgi/sgi-common.mk
+++ b/plat/arm/css/sgi/sgi-common.mk
@@ -19,7 +19,6 @@ ENT_GIC_SOURCES		:=	drivers/arm/gic/common/gic_common.c	\
 				drivers/arm/gic/v3/gicv3_helpers.c	\
 				plat/common/plat_gicv3.c		\
 				plat/arm/common/arm_gicv3.c		\
-				${CSS_ENT_BASE}/sgi_gic_config.c	\
 				drivers/arm/gic/v3/gic600.c
 
 
diff --git a/plat/arm/css/sgi/sgi_gic_config.c b/plat/arm/css/sgi/sgi_gic_config.c
deleted file mode 100644
index dfccc1b6d..000000000
--- a/plat/arm/css/sgi/sgi_gic_config.c
+++ /dev/null
@@ -1,18 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <sgi_plat_config.h>
-
-void plat_arm_gic_driver_init(void)
-{
-	/*
-	 * The GICv3 driver is initialized in EL3 and does not need
-	 * to be initialized again in S-EL1. This is because the S-EL1
-	 * can use GIC system registers to manage interrupts and does
-	 * not need GIC interface base addresses to be configured.
-	 */
-	gicv3_driver_init(get_plat_config()->gic_data);
-}
diff --git a/plat/arm/css/sgi/sgi_plat_config.c b/plat/arm/css/sgi/sgi_plat_config.c
index 96d31e21c..29b99a3c2 100644
--- a/plat/arm/css/sgi/sgi_plat_config.c
+++ b/plat/arm/css/sgi/sgi_plat_config.c
@@ -18,23 +18,6 @@ static css_plat_config_t *css_plat_info;
 /* The GICv3 driver only needs to be initialized in EL3 */
 uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
 
-const interrupt_prop_t sgi575_interrupt_props[] = {
-	CSS_G1S_IRQ_PROPS(INTR_GROUP1S),
-	ARM_G0_IRQ_PROPS(INTR_GROUP0),
-};
-
-/* Special definition for SGI575 */
-/* GIC configuration for SGI575 */
-const gicv3_driver_data_t sgi575_gic_data = {
-		.gicd_base = PLAT_ARM_GICD_BASE,
-		.gicr_base = PLAT_ARM_GICR_BASE,
-		.interrupt_props = sgi575_interrupt_props,
-		.interrupt_props_num = ARRAY_SIZE(sgi575_interrupt_props),
-		.rdistif_num = PLATFORM_CORE_COUNT,
-		.rdistif_base_addrs = rdistif_base_addrs,
-		.mpidr_to_core_pos = plat_arm_calc_core_pos
-		};
-
 /* Interconnect configuration for SGI575 */
 const css_inteconn_config_t sgi575_inteconn = {
 	.ip_type = ARM_CMN,
@@ -43,7 +26,6 @@ const css_inteconn_config_t sgi575_inteconn = {
 
 /* Configuration structure for SGI575 */
 css_plat_config_t sgi575_config = {
-	.gic_data = &sgi575_gic_data,
 	.inteconn = &sgi575_inteconn,
 };
 
diff --git a/plat/arm/css/sgi/sgi_topology.c b/plat/arm/css/sgi/sgi_topology.c
index 213659115..1d2e027df 100644
--- a/plat/arm/css/sgi/sgi_topology.c
+++ b/plat/arm/css/sgi/sgi_topology.c
@@ -16,14 +16,14 @@
  */
 const unsigned char sgi_pd_tree_desc[] = {
 	PLAT_ARM_CLUSTER_COUNT,
-	CSS_SGI_MAX_CORES_PER_CLUSTER,
-	CSS_SGI_MAX_CORES_PER_CLUSTER
+	CSS_SGI_MAX_CPUS_PER_CLUSTER,
+	CSS_SGI_MAX_CPUS_PER_CLUSTER
 };
 
 /* Topology configuration for sgi platform */
 const css_topology_t sgi_topology = {
 	.power_tree = sgi_pd_tree_desc,
-	.plat_cluster_core_count = CSS_SGI_MAX_CORES_PER_CLUSTER
+	.plat_cluster_core_count = CSS_SGI_MAX_CPUS_PER_CLUSTER
 };
 
 /*******************************************************************************