1 files changed, 118 insertions, 0 deletions
diff --git a/roms/edk2/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm b/roms/edk2/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm
new file mode 100644
index 000000000..2c346146d
--- /dev/null
+++ b/roms/edk2/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm
@@ -0,0 +1,118 @@
+//
+//  Copyright (c) 2012-2013, ARM Limited. All rights reserved.
+//
+//  SPDX-License-Identifier: BSD-2-Clause-Patent
+//
+//
+
+#include <AutoGen.h>
+
+  INCLUDE AsmMacroIoLib.inc
+
+  EXPORT  ArmPlatformStackSet
+  EXPORT  ArmPlatformStackSetPrimary
+  EXPORT  ArmPlatformStackSetSecondary
+
+  IMPORT  ArmPlatformIsPrimaryCore
+  IMPORT  ArmPlatformGetCorePosition
+  IMPORT  ArmPlatformGetPrimaryCoreMpId
+
+  IMPORT  _gPcd_FixedAtBuild_PcdCoreCount
+
+  PRESERVE8
+  AREA    ArmPlatformStackLib, CODE, READONLY
+
+//VOID
+//ArmPlatformStackSet (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSet FUNCTION
+  // Save parameters
+  mov   r6, r3
+  mov   r5, r2
+  mov   r4, r1
+  mov   r3, r0
+
+  // Save the Link register
+  mov   r7, lr
+
+  // Identify Stack
+  mov   r0, r1
+  bl    ArmPlatformIsPrimaryCore
+  cmp   r0, #1
+
+  // Restore parameters
+  mov   r0, r3
+  mov   r1, r4
+  mov   r2, r5
+  mov   r3, r6
+
+  // Restore the Link register
+  mov   lr, r7
+
+  beq   ArmPlatformStackSetPrimary
+  bne   ArmPlatformStackSetSecondary
+  ENDFUNC
+
+//VOID
+//ArmPlatformStackSetPrimary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetPrimary FUNCTION
+  mov   r4, lr
+
+  // Add stack of primary stack to StackBase
+  add   r0, r0, r2
+
+  // Compute SecondaryCoresCount * SecondaryCoreStackSize
+  mov32 r1, FixedPcdGet32 (PcdCoreCount)
+  sub   r1, #1
+  mul   r3, r3, r1
+
+  // Set Primary Stack ((StackBase + PrimaryStackSize) + (SecondaryCoresCount * SecondaryCoreStackSize))
+  add   sp, r0, r3
+
+  bx    r4
+  ENDFUNC
+
+//VOID
+//ArmPlatformStackSetSecondary (
+//  IN UINTN StackBase,
+//  IN UINTN MpId,
+//  IN UINTN PrimaryStackSize,
+//  IN UINTN SecondaryStackSize
+//  );
+ArmPlatformStackSetSecondary FUNCTION
+  mov   r4, lr
+  mov   sp, r0
+
+  // Get Core Position
+  mov   r0, r1
+  bl ArmPlatformGetCorePosition
+  mov   r5, r0
+
+  // Get Primary Core Position
+  bl ArmPlatformGetPrimaryCoreMpId
+  bl ArmPlatformGetCorePosition
+
+  // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
+  cmp   r5, r0
+  subhi r5, r5, #1
+  add   r5, r5, #1
+
+  // Compute top of the secondary stack
+  mul   r3, r3, r5
+
+  // Set stack
+  add   sp, sp, r3
+
+  bx r4
+  ENDFUNC
+
+  END