Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Chen Fan <chen.fan.f...@cn.fujitsu.com>
---
UefiCpuPkg/CpuDxe/ApStartup.c | 28 +++++++++++++++++---
UefiCpuPkg/CpuDxe/CpuMp.c | 36 ++++++++++++++++++++++++--
UefiCpuPkg/CpuDxe/X64/MpAsm.S | 9 +++++--
UefiCpuPkg/Include/Library/LocalApicLib.h | 8 +++++-
UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c | 14 ++++++++++
5 files changed, 86 insertions(+), 9 deletions(-)
diff --git a/UefiCpuPkg/CpuDxe/ApStartup.c b/UefiCpuPkg/CpuDxe/ApStartup.c
index b911fca..27abe7a 100644
--- a/UefiCpuPkg/CpuDxe/ApStartup.c
+++ b/UefiCpuPkg/CpuDxe/ApStartup.c
@@ -16,6 +16,9 @@
#include "CpuGdt.h"
#include "CpuMp.h"
+UINT32 ProcessorIdx = 1;
+UINT32 ProcessorIds[255];
+
#pragma pack(1)
typedef struct {
@@ -153,6 +156,8 @@ StartApsStackless (
volatile STARTUP_CODE *StartupCode;
IA32_DESCRIPTOR Gdtr;
EFI_PHYSICAL_ADDRESS StartAddress;
+ UINTN CurrentCPUs;
+ UINTN TimeCount;
StartAddress = BASE_1MB;
Status = gBS->AllocatePages (
@@ -182,14 +187,29 @@ StartApsStackless (
StartupCode->LongJmpOffset += (UINT32) StartAddress;
#endif
+ CurrentCPUs = 1;
+
SendInitSipiSipiAllExcludingSelf ((UINT32)(UINTN)(VOID*) StartupCode);
- DisableInterrupts ();
- CpuSleep ();
- CpuDeadLoop ();
+ for (TimeCount = 0; TimeCount < 100; TimeCount++) {
+ CurrentCPUs = ProcessorIdx;
+ gBS->Stall(100000); // 100ms
+ }
+
+ Status = EFI_SUCCESS;
+
+ if (CurrentCPUs != (ProcessorIdx)) {
+ Status = EFI_TIMEOUT;
+ }
+
+ DEBUG ((EFI_D_INFO, "Found CPU Count: %d\n", CurrentCPUs));
gBS->FreePages (StartAddress, EFI_SIZE_TO_PAGES (sizeof (*StartupCode)));
- return EFI_SUCCESS;
+ if (CurrentCPUs == 1) {
+ return EFI_SUCCESS;
+ }
+
+ return Status;
}
diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index 01805c3..e46de49 100644
--- a/UefiCpuPkg/CpuDxe/CpuMp.c
+++ b/UefiCpuPkg/CpuDxe/CpuMp.c
@@ -17,15 +17,41 @@
VOID *mCommonStack = 0;
VOID *mTopOfApCommonStack = 0;
+VOID *OldTopOfApCommonStack = 0;
+IA32_DESCRIPTOR gIdtr;
+
+extern UINT32 ProcessorIdx;
+extern UINT32 ProcessorIds[];
VOID
EFIAPI
-ApEntryPointInC (
+ApEntryPointInit (
VOID
)
{
- DEBUG ((EFI_D_INFO, "Hello from AP! (Apic ID %d)\n", GetApicId ()));
+ UINT32 ProcessorId;
+ VOID *CommonStack;
+
+ // update ESP
+ CommonStack = AllocatePages (EFI_SIZE_TO_PAGES (SIZE_64KB));
+ if (CommonStack == NULL) {
+ mTopOfApCommonStack = 0;
+ return;
+ }
+
+ mTopOfApCommonStack = (VOID*) ((UINTN)CommonStack + SIZE_64KB);
+
+ // Update idtr
+ AsmWriteIdtr (&gIdtr);
+ // Enable Spurious intrrupt
+ EnableSpuriousInterrupt();
+
+ ProcessorId = GetApicId();
+
+ ProcessorIds[ProcessorIdx] = ProcessorId;
+
+ ProcessorIdx++;
}
@@ -39,8 +65,14 @@ InitializeMpSupport (
if (mCommonStack == NULL) {
return;
}
+ OldTopOfApCommonStack = mTopOfApCommonStack;
+
DEBUG ((EFI_D_INFO, "mTopOfApCommonStack: %p\n", mTopOfApCommonStack));
+ AsmReadIdtr (&gIdtr);
+
StartApsStackless (AsmApEntryPoint);
+
+ gBS->FreePages ((EFI_PHYSICAL_ADDRESS)mCommonStack,
EFI_SIZE_TO_PAGES(SIZE_64KB));
}
diff --git a/UefiCpuPkg/CpuDxe/X64/MpAsm.S b/UefiCpuPkg/CpuDxe/X64/MpAsm.S
index cfad551..c20d985 100644
--- a/UefiCpuPkg/CpuDxe/X64/MpAsm.S
+++ b/UefiCpuPkg/CpuDxe/X64/MpAsm.S
@@ -33,8 +33,10 @@ lock btsl $0, ApStackLock
pause
jc AsmApEntryPointAcquireLock
+ movq (ASM_PFX(OldTopOfApCommonStack)), %rsp
+ call ASM_PFX(ApEntryPointInit)
+
movq (ASM_PFX(mTopOfApCommonStack)), %rsp
- call ASM_PFX(ApEntryPointInC)
cli
@@ -46,7 +48,10 @@ AsmApEntryPointShareLock:
decl %eax
jnz AsmApEntryPointShareLock
- jmp ASM_PFX(AsmApEntryPoint)
+ sti
+loop:
+ hlt
+ jmp loop
#------------------------------------------------------------------------------
# VOID
diff --git a/UefiCpuPkg/Include/Library/LocalApicLib.h
b/UefiCpuPkg/Include/Library/LocalApicLib.h
index 8960204..82ced0e 100644
--- a/UefiCpuPkg/Include/Library/LocalApicLib.h
+++ b/UefiCpuPkg/Include/Library/LocalApicLib.h
@@ -393,6 +393,12 @@ GetApicMsiValue (
IN BOOLEAN LevelTriggered,
IN BOOLEAN AssertionLevel
);
-
+
+VOID
+EFIAPI
+EnableSpuriousInterrupt(
+ VOID
+ );
+
#endif
diff --git a/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c
b/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c
index bd97fae..2e3613f 100644
--- a/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c
+++ b/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c
@@ -820,3 +820,17 @@ GetApicMsiValue (
}
return MsiData.Uint64;
}
+
+VOID
+EFIAPI
+EnableSpuriousInterrupt(
+ VOID
+ )
+{
+ LOCAL_APIC_SVR Svr;
+
+ Svr.Uint32 = ReadLocalApicReg (XAPIC_SPURIOUS_VECTOR_OFFSET);
+ Svr.Bits.SpuriousVector = 0xf;
+ Svr.Bits.SoftwareEnable = 1;
+ WriteLocalApicReg (XAPIC_SPURIOUS_VECTOR_OFFSET, Svr.Uint32);
+}
--
1.9.3
------------------------------------------------------------------------------
Want fast and easy access to all the code in your enterprise? Index and
search up to 200,000 lines of code with a free copy of Black Duck
Code Sight - the same software that powers the world's largest code
search on Ohloh, the Black Duck Open Hub! Try it now.
http://p.sf.net/sfu/bds
_______________________________________________
edk2-devel mailing list
edk2-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/edk2-devel
