ArmPlatformPkg/ArmPlatformLib: Added support for ArmPlatformIsPrimaryCore()

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

diff --git a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.asm b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.asm
index 73b3d97d6654ca90a909cdc86a8d0d7930c74f88..1b5474b5f243db20caf0e1310417cbb0d5345b19 100644 (file)
--- a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.asm
+++ b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibCTA15-A7/CTA15-A7Helper.asm
@@ -1,5 +1,5 @@
 //\r
-//  Copyright (c) 2012, ARM Limited. All rights reserved.\r
+//  Copyright (c) 2012-2013, ARM Limited. All rights reserved.\r
 //\r
 //  This program and the accompanying materials\r
 //  are licensed and made available under the terms and conditions of the BSD License\r
@@ -11,11 +11,15 @@
 //\r
 //\r
 \r
+#include <AsmMacroIoLib.h>\r
 #include <Library/ArmLib.h>\r
 \r
+#include <ArmPlatform.h>\r
+\r
   INCLUDE AsmMacroIoLib.inc\r
 \r
   EXPORT  ArmPlatformGetCorePosition\r
+  EXPORT  ArmPlatformIsPrimaryCore\r
 \r
   PRESERVE8\r
   AREA    CTA15A7Helper, CODE, READONLY\r
@@ -31,4 +35,35 @@ ArmPlatformGetCorePosition FUNCTION
   bx   lr\r
   ENDFUNC\r
 \r
+//UINTN\r
+//ArmPlatformIsPrimaryCore (\r
+//  IN UINTN MpId\r
+//  );\r
+ArmPlatformIsPrimaryCore FUNCTION\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, r1)\r
+  ldr   r1, [r1]\r
+  lsr   r1, #24\r
+\r
+  // Shift the SCC value to get the cluster ID at the offset #8\r
+  lsl   r2, r1, #4\r
+  and   r2, r2, #0xF00\r
+\r
+  // Keep only the cpu ID from the original SCC\r
+  and   r1, r1, #0x0F\r
+  // Add the Cluster ID to the Cpu ID\r
+  orr   r1, r1, r2\r
+\r
+  // Keep the Cluster ID and Core ID from the MPID\r
+  LoadConstantToReg (ARM_CLUSTER_MASK | ARM_CORE_MASK, r2)\r
+  and   r0, r0, r2\r
+\r
+  // Compare mpid and boot cpu from ARM_SCC_CFGREG48\r
+  cmp   r0, r1\r
+  moveq r0, #1\r
+  movne r0, #0\r
+  bx    lr\r
+  ENDFUNC\r
+\r
   END\r