1 files changed, 129 insertions, 0 deletions
diff --git a/ArmPlatformPkg/Library/ArmPlatformStackLib/AArch64/ArmPlatformStackLib.S b/ArmPlatformPkg/Library/ArmPlatformStackLib/AArch64/ArmPlatformStackLib.S
new file mode 100644
index 0000000000..337af9c6a9
--- /dev/null
+++ b/ArmPlatformPkg/Library/ArmPlatformStackLib/AArch64/ArmPlatformStackLib.S
@@ -0,0 +1,129 @@
+//
+//  Copyright (c) 2012-2013, ARM Limited. All rights reserved.
+//
+//  This program and the accompanying materials
+//  are licensed and made available under the terms and conditions of the BSD License
+//  which accompanies this distribution.  The full text of the license may be found at
+//  http://opensource.org/licenses/bsd-license.php
+//
+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+//
+//
+
+#include <AsmMacroIoLib.h>
+#include <Base.h>
+#include <AutoGen.h>
+
+.text
+.align 3
+
+GCC_ASM_EXPORT(ArmPlatformStackSet)
+GCC_ASM_EXPORT(ArmPlatformStackSetPrimary)
+GCC_ASM_EXPORT(ArmPlatformStackSetSecondary)
+
+GCC_ASM_IMPORT(ArmPlatformIsPrimaryCore)
+GCC_ASM_IMPORT(ArmPlatformGetCorePosition)
+GCC_ASM_IMPORT(ArmPlatformGetPrimaryCoreMpId)
+
+GCC_ASM_IMPORT(gPcd_FixedAtBuild_PcdCoreCount)
+
+//VOID
+//ArmPlatformStackSet (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ASM_PFX(ArmPlatformStackSet):
+  // Save parameters
+  mov   x6, x3
+  mov   x5, x2
+  mov   x4, x1
+  mov   x3, x0
+
+  // Save the Link register
+  mov   x7, x30
+
+  // Identify Stack
+  mov   x0, x1
+  bl    ASM_PFX(ArmPlatformIsPrimaryCore)
+  cmp   x0, #1
+
+  // Restore parameters
+  mov   x0, x3
+  mov   x1, x4
+  mov   x2, x5
+  mov   x3, x6
+
+  // Restore the Link register
+  mov   x30, x7
+
+  // Should be ASM_PFX(ArmPlatformStackSetPrimary) but generate linker error 'unsupported ELF EM_AARCH64'
+  b.eq  ArmPlatformStackSetPrimaryL
+  // Should be ASM_PFX(ArmPlatformStackSetSecondary) but generate linker error 'unsupported ELF EM_AARCH64'
+  b.ne  ArmPlatformStackSetSecondaryL
+
+//VOID
+//ArmPlatformStackSetPrimary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetPrimaryL:
+ASM_PFX(ArmPlatformStackSetPrimary):
+  // Save the Link register
+  mov   x4, x30
+
+  // Add stack of primary stack to StackBase
+  add   x0, x0, x2
+
+  // Compute SecondaryCoresCount * SecondaryCoreStackSize
+  LoadConstantToReg (_gPcd_FixedAtBuild_PcdCoreCount, x1)
+  ldr   w1, [x1]
+  sub   x1, x1, #1
+  mul   x3, x3, x1
+
+  // Set Primary Stack ((StackBase + PrimaryStackSize) + (SecondaryCoresCount * SecondaryCoreStackSize))
+  add   sp, x0, x3
+
+  br    x4
+
+//VOID
+//ArmPlatformStackSetSecondary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetSecondaryL:
+ASM_PFX(ArmPlatformStackSetSecondary):
+  // Save the Link register
+  mov   x4, x30
+  mov   sp, x0
+
+  // Get Core Position
+  mov   x0, x1
+  bl    ASM_PFX(ArmPlatformGetCorePosition)
+  mov   x5, x0
+
+  // Get Primary Core Position
+  bl    ASM_PFX(ArmPlatformGetPrimaryCoreMpId)
+  bl    ASM_PFX(ArmPlatformGetCorePosition)
+
+  // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
+  cmp   x5, x0
+  b.ls  1f
+  // Decrement the position if after the primary core
+  sub   x5, x5, #1
+1:
+  add   x5, x5, #1
+
+  // Compute top of the secondary stack
+  mul   x3, x3, x5
+
+  // Set stack
+  add   sp, sp, x3
+
+  br    x4