diff --git a/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.S b/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.S
index 34fab31dd4..96e925981f 100644
--- a/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.S
+++ b/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.S
@@ -22,10 +22,11 @@ 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)
-GCC_ASM_IMPORT(_gPcd_FixedAtBuild_PcdArmPrimaryCore)
 
 //VOID
 //ArmPlatformStackSet (
@@ -35,14 +36,29 @@ GCC_ASM_IMPORT(_gPcd_FixedAtBuild_PcdArmPrimaryCore)
 //  IN UINTN SecondaryStackSize
 //  );
 ASM_PFX(ArmPlatformStackSet):
+  // Save parameters
+  mov   r6, r3
+  mov   r5, r2
+  mov   r4, r1
+  mov   r3, r0
+
+  // Save the Link register
+  mov   r7, lr
+
   // Identify Stack
-  // Mask for ClusterId|CoreId
-  LoadConstantToReg (0xFFFF, r4)
-  and   r1, r1, r4
-  // Is it the Primary Core ?
-  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r4)
-  ldr   r4, [r4]
-  cmp   r1, r4
+  mov   r0, r1
+  bl    ASM_PFX(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   ASM_PFX(ArmPlatformStackSetPrimary)
   bne   ASM_PFX(ArmPlatformStackSetSecondary)
 
@@ -87,8 +103,7 @@ ASM_PFX(ArmPlatformStackSetSecondary):
   mov   r5, r0
 
   // Get Primary Core Position
-  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r0)
-  ldr   r0, [r0]
+  bl    ASM_PFX(ArmPlatformGetPrimaryCoreMpId)
   bl    ASM_PFX(ArmPlatformGetCorePosition)
 
   // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
diff --git a/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm b/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm
index db92023922..e7fd4a06a8 100644
--- a/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm
+++ b/ArmPlatformPkg/Library/ArmPlatformStackLib/Arm/ArmPlatformStackLib.asm
@@ -1,5 +1,5 @@
 //
-//  Copyright (c) 2012, ARM Limited. All rights reserved.
+//  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
@@ -21,10 +21,11 @@
   EXPORT  ArmPlatformStackSetPrimary
   EXPORT  ArmPlatformStackSetSecondary
 
+  IMPORT  ArmPlatformIsPrimaryCore
   IMPORT  ArmPlatformGetCorePosition
+  IMPORT  ArmPlatformGetPrimaryCoreMpId
 
   IMPORT  _gPcd_FixedAtBuild_PcdCoreCount
-  IMPORT  _gPcd_FixedAtBuild_PcdArmPrimaryCore
 
   PRESERVE8
   AREA    ArmPlatformStackLib, CODE, READONLY
@@ -37,14 +38,29 @@
 //  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
-  // Mask for ClusterId|CoreId
-  LoadConstantToReg (0xFFFF, r4)
-  and	r1, r1, r4
-  // Is it the Primary Core ?
-  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r4)
-  ldr	r4, [r4]
-  cmp   r1, r4
+  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
@@ -91,8 +107,7 @@ ArmPlatformStackSetSecondary FUNCTION
   mov	r5, r0
 
   // Get Primary Core Position
-  LoadConstantToReg (_gPcd_FixedAtBuild_PcdArmPrimaryCore, r0)
-  ldr	r0, [r0]
+  bl ArmPlatformGetPrimaryCoreMpId
   bl ArmPlatformGetCorePosition
 
   // Get Secondary Core Position. We should get consecutive secondary stack number from 1...(CoreCount-1)
diff --git a/ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf b/ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
index 700bc89eac..affd321964 100644
--- a/ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
+++ b/ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
@@ -33,10 +33,8 @@
 [Sources.AARCH64]
   AArch64/ArmPlatformStackLib.S     | GCC
 
+[LibraryClasses]
+  ArmPlatformLib
+
 [FixedPcd]
   gArmPlatformTokenSpaceGuid.PcdCoreCount
-
-  gArmPlatformTokenSpaceGuid.PcdCoreCount
-
-  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
-  gArmTokenSpaceGuid.PcdArmPrimaryCore