From 8aaa8634a30ccc896178e7a47b62019187df247b Mon Sep 17 00:00:00 2001
From: Vishwanatha HG <vishwanatha.hg@arm.com>
Date: Tue, 8 May 2018 17:15:37 +0530
Subject: [PATCH] css/sgi: rework the core position calculation function

The MT bit in MPIDR is always set for SGI platforms and so the
core position calculation code is updated to take into account
the thread affinity value as well.

Change-Id: I7b2a52707f607dc3859c6bbcd2b145b7987cb4ed
Signed-off-by: Chandni Cherukuri <chandni.cherukuri@arm.com>
Signed-off-by: Vishwanatha HG <vishwanatha.hg@arm.com>
---
 plat/arm/css/common/css_topology.c      |  8 +++--
 plat/arm/css/sgi/aarch64/sgi_helper.S   | 44 +++++++++++++++++--------
 plat/arm/css/sgi/include/platform_def.h |  6 ++--
 plat/arm/css/sgi/sgi_topology.c         |  6 ++--
 4 files changed, 44 insertions(+), 20 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 62f405999..94d4ff914 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_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
 };
 
 /*******************************************************************************