2 files changed, 28 insertions, 3 deletions
diff --git a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.S b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.S
index 847424fb4b..1c256af241 100644
--- a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.S
+++ b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.S
@@ -13,6 +13,7 @@
 
 #include <AsmMacroIoLib.h>
 #include <Base.h>
+#include <Library/ArmLib.h>
 #include <Library/PcdLib.h>
 #include <AutoGen.h>
 
@@ -24,6 +25,7 @@
 GCC_ASM_EXPORT(ArmGetCpuCountPerCluster)
 GCC_ASM_EXPORT(ArmPlatformIsPrimaryCore)
 GCC_ASM_EXPORT(ArmPlatformGetPrimaryCoreMpId)
+GCC_ASM_EXPORT(ArmPlatformGetCorePosition)
 
 GCC_ASM_IMPORT(_gPcd_FixedAtBuild_PcdArmPrimaryCore)
 GCC_ASM_IMPORT(_gPcd_FixedAtBuild_PcdArmPrimaryCoreMask)
@@ -96,4 +98,14 @@ ASM_PFX(ArmPlatformIsPrimaryCore):
   movne r0, #0
   bx 	lr
 
+//UINTN
+//ArmPlatformGetCorePosition (
+//  IN UINTN MpId
+//  );
+ASM_PFX(ArmPlatformGetCorePosition):
+  and   r1, r0, #ARM_CORE_MASK
+  and   r0, r0, #ARM_CLUSTER_MASK
+  add   r0, r1, r0, LSR #7
+  bx    lr
+
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED 
diff --git a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.asm b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.asm
index 167762c34e..02b9f0f4a7 100644
--- a/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.asm
+++ b/ArmPlatformPkg/ArmVExpressPkg/Library/ArmVExpressLibRTSM/Arm/RTSMHelper.asm
@@ -1,5 +1,5 @@
 //
-//  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
+//  Copyright (c) 2011-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         
@@ -13,6 +13,7 @@
 
 #include <AsmMacroIoLib.h>
 #include <Base.h>
+#include <Library/ArmLib.h>
 #include <Library/PcdLib.h>
 
 #include <Chipset/ArmCortexA9.h>
@@ -24,9 +25,10 @@
   EXPORT    ArmGetCpuCountPerCluster
   EXPORT    ArmPlatformIsPrimaryCore
   EXPORT    ArmPlatformGetPrimaryCoreMpId
+  EXPORT    ArmPlatformGetCorePosition
 
-  IMPORT  _gPcd_FixedAtBuild_PcdArmPrimaryCore
-  IMPORT  _gPcd_FixedAtBuild_PcdArmPrimaryCoreMask
+  IMPORT    _gPcd_FixedAtBuild_PcdArmPrimaryCore
+  IMPORT    _gPcd_FixedAtBuild_PcdArmPrimaryCoreMask
 
   AREA RTSMHelper, CODE, READONLY
 
@@ -103,4 +105,15 @@ ArmPlatformIsPrimaryCore FUNCTION
   bx 	lr
   ENDFUNC
 
+//UINTN
+//ArmPlatformGetCorePosition (
+//  IN UINTN MpId
+//  );
+ArmPlatformGetCorePosition FUNCTION
+  and   r1, r0, #ARM_CORE_MASK
+  and   r0, r0, #ARM_CLUSTER_MASK
+  add   r0, r1, r0, LSR #7
+  bx    lr
+  ENDFUNC
+
   END