ArmPlatformPkg: Fix and Implement ArmPlatformGetPrimaryCoreMpId

[mirror_edk2.git] / ArmPlatformPkg / ArmVExpressPkg / Library / ArmVExpressLibCTA15-A7 / CTA15-A7Helper.S

diff --git a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.S b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.S
index b560f56c51352ba1f61f35c2ae83949ef17620c0..ac5378a622d0b16d539a79bd2a480c09bff728c3 100644 (file)
--- a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.S
+++ b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.S
@@ -22,6 +22,7 @@
 GCC_ASM_EXPORT(ArmPlatformPeiBootAction)\r
 GCC_ASM_EXPORT(ArmPlatformGetCorePosition)\r
 GCC_ASM_EXPORT(ArmPlatformIsPrimaryCore)\r
+GCC_ASM_EXPORT(ArmPlatformGetPrimaryCoreMpId)\r
 \r
 ASM_PFX(ArmPlatformPeiBootAction):\r
   bx    lr\r
@@ -66,3 +67,23 @@ ASM_PFX(ArmPlatformIsPrimaryCore):
   movne r0, #0\r
   bx    lr\r
 \r
+//UINTN\r
+//ArmPlatformGetPrimaryCoreMpId (\r
+//  VOID\r
+//  );\r
+ASM_PFX(ArmPlatformGetPrimaryCoreMpId):\r
+  // Extract cpu_id and cluster_id from ARM_SCC_CFGREG48\r
+  // with cpu_id[0:3] and cluster_id[4:7]\r
+  LoadConstantToReg (ARM_CTA15A7_SCC_CFGREG48, r0)\r
+  ldr   r0, [r0]\r
+  lsr   r0, #24\r
+\r
+  // Shift the SCC value to get the cluster ID at the offset #8\r
+  lsl   r1, r0, #4\r
+  and   r1, r1, #0xF00\r
+\r
+  // Keep only the cpu ID from the original SCC\r
+  and   r0, r0, #0x0F\r
+  // Add the Cluster ID to the Cpu ID\r
+  orr   r0, r0, r1\r
+  bx    lr\r