ArmPlatformPkg/ArmPlatformLib: Added support for ArmPlatformIsPrimaryCore()

[mirror_edk2.git] / ArmPkg / Include / AsmMacroIoLib.inc

diff --git a/ArmPkg/Include/AsmMacroIoLib.inc b/ArmPkg/Include/AsmMacroIoLib.inc
index a847579a37b5dd0681ae8aba4f72f59109b83b86..5796adcb12970fad235326ad42a0d86c0a224b90 100644 (file)
--- a/ArmPkg/Include/AsmMacroIoLib.inc
+++ b/ArmPkg/Include/AsmMacroIoLib.inc
@@ -81,7 +81,11 @@
   MEND \r
   \r
   MACRO\r
-  GetCorePositionInStack $Pos, $MpId, $Tmp\r
+  GetCorePositionFromMpId $Pos, $MpId, $Tmp\r
+  ;Note: The ARM macro does not support the pre-processing. 0xFF and (0xFF << 8) are the values of\r
+  ;      ARM_CORE_MASK and ARM_CLUSTER_MASK \r
+  mov   $Tmp, #(0xFF :OR: (0xFF << 8))\r
+  and   $MpId, $Tmp\r
   lsr   $Pos, $MpId, #6\r
   and   $Tmp, $MpId, #3\r
   add   $Pos, $Pos, $Tmp\r
@@ -104,8 +108,25 @@ _SetPrimaryStackInitGlobals
   beq     _SetPrimaryStackEnd\r
   str     $GlobalSize, [$Tmp], #4\r
   b       _SetPrimaryStackInitGlobals\r
-\r
 _SetPrimaryStackEnd\r
   MEND\r
 \r
+  MACRO\r
+  InitializePrimaryStack $GlobalSize, $Tmp1\r
+  and     $Tmp1, $GlobalSize, #7\r
+  rsbne   $Tmp1, $Tmp1, #8\r
+  add     $GlobalSize, $GlobalSize, $Tmp1\r
+\r
+  mov     $Tmp1, sp\r
+  sub     sp, $GlobalSize\r
+  ; Set all the global variables to 0\r
+  mov     $GlobalSize, #0x0\r
+_InitializePrimaryStackLoop\r
+  cmp     $Tmp1, sp\r
+  bls     _InitializePrimaryStackEnd\r
+  str     $GlobalSize, [$Tmp1], #-4\r
+  b       _InitializePrimaryStackLoop\r
+_InitializePrimaryStackEnd\r
+  MEND\r
+\r
   END\r