From 59d0a1e8b2809040b449f56a013dbc254ce902df Mon Sep 17 00:00:00 2001
From: XiaoDong Huang <derrick.huang@rock-chips.com>
Date: Mon, 22 May 2023 16:36:12 +0800
Subject: [PATCH] firmware: rockchip_sip: fix error in cpu_logical_map_mpidr

If PE support multithread, the mpidr of cpu0~cpu3 in cluster0
and cpu4~cpu7 in cluster1 is as follow:
cpu0~3: 0x81000000, 0x81000100, 0x81000200, 0x81000300.
cpu4~7: 0x81010000, 0x81010100, 0x81010200, 0x81010300.

If PE doesn't support multithread, the mpidr of cpu0~cpu3 in cluster0
and cpu4~cpu7 in cluster1 is as follow:
cpu0~3: 0x80000000, 0x80000001, 0x80000002, 0x80000003.
cpu4~7: 0x80000100, 0x80000101, 0x80000102, 0x80000103.

Signed-off-by: XiaoDong Huang <derrick.huang@rock-chips.com>
Change-Id: Ib2fb18ad280c6c275850910f18cb4f653c4a108f
---
 drivers/firmware/rockchip_sip.c | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/drivers/firmware/rockchip_sip.c b/drivers/firmware/rockchip_sip.c
index 1072dbd753d2..186011fe6463 100644
--- a/drivers/firmware/rockchip_sip.c
+++ b/drivers/firmware/rockchip_sip.c
@@ -471,16 +471,21 @@ static ulong cpu_logical_map_mpidr(u32 cpu)
 {
 #ifdef MODULE
 	/* Empirically, local "cpu_logical_map()" for rockchip platforms */
-	ulong mpidr = 0x00;
+	ulong mpidr = read_cpuid_mpidr();
 
-	if (cpu < 4)
-		/* 0x00, 0x01, 0x02, 0x03 */
-		mpidr = cpu;
-	else if (cpu < 8)
-		/* 0x100, 0x101, 0x102, 0x103 */
-		mpidr = 0x100 | (cpu - 4);
-	else
-		pr_err("Unsupported map cpu: %d\n", cpu);
+	if (mpidr & MPIDR_MT_BITMASK) {
+		/* 0x100, 0x200, 0x300, 0x400 ... */
+		mpidr = (cpu & 0xff) << 8;
+	} else {
+		if (cpu < 4)
+			/* 0x00, 0x01, 0x02, 0x03 */
+			mpidr = cpu;
+		else if (cpu < 8)
+			/* 0x100, 0x101, 0x102, 0x103 */
+			mpidr = 0x100 | (cpu - 4);
+		else
+			pr_err("Unsupported map cpu: %d\n", cpu);
+	}
 
 	return mpidr;
 #else