From mboxrd@z Thu Jan 1 00:00:00 1970 Received: from EUR03-AM5-obe.outbound.protection.outlook.com (EUR03-AM5-obe.outbound.protection.outlook.com [40.107.3.82]) by mx.groups.io with SMTP id smtpd.web11.941.1634925130971695532 for ; Fri, 22 Oct 2021 10:52:12 -0700 Authentication-Results: mx.groups.io; dkim=pass header.i=@armh.onmicrosoft.com header.s=selector2-armh-onmicrosoft-com header.b=zxJsv7Vh; spf=pass (domain: arm.com, ip: 40.107.3.82, mailfrom: sami.mujawar@arm.com) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=armh.onmicrosoft.com; s=selector2-armh-onmicrosoft-com; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-SenderADCheck; bh=gPMjlULoJC+lskAQSBnqtA3QJW/wjBmb6KZ6I5Jejiw=; b=zxJsv7VhGXUt/Jt4TajJoCOzdq4v9GMUEkL71hnWVmNtPrTvAI4Auz6u1bPgJSeyzeFPBCX7enUYf2V06qplOtc8xfS54gaQ+n1LyPdKOWHyHrK5mQTb5iglnGzkeSsclNIPY73nP/rqBTUoX58m+S9SejwN1wnWllD4ga1Jo3o= Received: from AM6P191CA0107.EURP191.PROD.OUTLOOK.COM (2603:10a6:209:8a::48) by AS8PR08MB6679.eurprd08.prod.outlook.com (2603:10a6:20b:393::10) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4628.18; Fri, 22 Oct 2021 17:52:03 +0000 Received: from VE1EUR03FT024.eop-EUR03.prod.protection.outlook.com (2603:10a6:209:8a:cafe::20) by AM6P191CA0107.outlook.office365.com (2603:10a6:209:8a::48) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4628.16 via Frontend Transport; Fri, 22 Oct 2021 17:52:03 +0000 X-MS-Exchange-Authentication-Results: spf=pass (sender IP is 63.35.35.123) smtp.mailfrom=arm.com; dkim=pass (signature was verified) header.d=armh.onmicrosoft.com;dmarc=pass action=none header.from=arm.com; Received-SPF: Pass (protection.outlook.com: domain of arm.com designates 63.35.35.123 as permitted sender) receiver=protection.outlook.com; client-ip=63.35.35.123; helo=64aa7808-outbound-1.mta.getcheckrecipient.com; Received: from 64aa7808-outbound-1.mta.getcheckrecipient.com (63.35.35.123) by VE1EUR03FT024.mail.protection.outlook.com (10.152.18.87) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4628.18 via Frontend Transport; Fri, 22 Oct 2021 17:52:03 +0000 Received: ("Tessian outbound a8bfe25d7364:v103"); Fri, 22 Oct 2021 17:52:02 +0000 X-CheckRecipientChecked: true X-CR-MTA-CID: be484caf1e3c6323 X-CR-MTA-TID: 64aa7808 Received: from 3377527bec77.1 by 64aa7808-outbound-1.mta.getcheckrecipient.com id 1A7F9E73-228D-46EF-B6F8-5CC6CFD7EE4B.1; Fri, 22 Oct 2021 17:51:55 +0000 Received: from EUR04-HE1-obe.outbound.protection.outlook.com by 64aa7808-outbound-1.mta.getcheckrecipient.com with ESMTPS id 3377527bec77.1 (version=TLSv1.2 cipher=ECDHE-RSA-AES256-GCM-SHA384); Fri, 22 Oct 2021 17:51:55 +0000 ARC-Seal: i=1; a=rsa-sha256; s=arcselector9901; d=microsoft.com; cv=none; b=O8zpd2pVohrbISVR8oKJ4Ew/cau93/NFE6vY/V017nA6IDJZ/Ni/YCp7KRh509/00wJfkWNjfDR+1AK9XywzciFkl9psPi9zlD4vZV04CJLh4z8daEQ3MEsQc+H2tOrDFV7qj8k4n3I5lIVI2B2ZSKsJTdIK5owLizt1B9018wCp0zhI9hHepiEwG00Gxs1aIyDj7nCHLU32H047nGYmIxkWrFl0eu18VVyL+L+gl3oGQYIrGyHGa5ByydXnrusZPrhCeimawNF9h5w6Sr3yXvN7KI93xt97L1EjBAMe6IknDlldc5r7mOxHibGvDSWdFuZzfDg8YjIlO2U2ZHrcyg== ARC-Message-Signature: i=1; a=rsa-sha256; c=relaxed/relaxed; d=microsoft.com; s=arcselector9901; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-AntiSpam-MessageData-ChunkCount:X-MS-Exchange-AntiSpam-MessageData-0:X-MS-Exchange-AntiSpam-MessageData-1; bh=gPMjlULoJC+lskAQSBnqtA3QJW/wjBmb6KZ6I5Jejiw=; b=TJra4TtawSOKNYG3B3S15wdLKCBCGpB7NN2DDu/BUAfygn7QUikrCpyh/CS6mreVKwC1ZPntoyJIwbNUHhqPFpsywsPCkGmxfxddW4xzDvFBGSh5VtmnEe7ogiBF2d3QkUtN5wZR30rYHA/osf53Z6lprh4UbKa+b4qphjOxkESavs4hwOP2iQyPmxNJniW8VdjtK3DYTdLzk36Y4QWTK5hZOkv/r8KGaXQEC3sZx5Tg6DvDEH2FuW3oS0fDxVFOOCJH6cIyBYy4ZM56qn9eKi1MS/xBz/zeGQTs9h9PDV29J8UTeczwYBzaXTaA05A9/w0+h0JhSXExo2HH8jE12w== ARC-Authentication-Results: i=1; mx.microsoft.com 1; spf=pass smtp.mailfrom=arm.com; dmarc=pass action=none header.from=arm.com; dkim=pass header.d=arm.com; arc=none DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=armh.onmicrosoft.com; s=selector2-armh-onmicrosoft-com; h=From:Date:Subject:Message-ID:Content-Type:MIME-Version:X-MS-Exchange-SenderADCheck; bh=gPMjlULoJC+lskAQSBnqtA3QJW/wjBmb6KZ6I5Jejiw=; b=zxJsv7VhGXUt/Jt4TajJoCOzdq4v9GMUEkL71hnWVmNtPrTvAI4Auz6u1bPgJSeyzeFPBCX7enUYf2V06qplOtc8xfS54gaQ+n1LyPdKOWHyHrK5mQTb5iglnGzkeSsclNIPY73nP/rqBTUoX58m+S9SejwN1wnWllD4ga1Jo3o= Authentication-Results-Original: dkim=none (message not signed) header.d=none;dmarc=none action=none header.from=arm.com; Received: from AS8PR08MB6806.eurprd08.prod.outlook.com (2603:10a6:20b:39b::12) by AM6PR08MB4408.eurprd08.prod.outlook.com (2603:10a6:20b:be::21) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4628.18; Fri, 22 Oct 2021 17:51:48 +0000 Received: from AS8PR08MB6806.eurprd08.prod.outlook.com ([fe80::54b5:239d:9896:ee65]) by AS8PR08MB6806.eurprd08.prod.outlook.com ([fe80::54b5:239d:9896:ee65%4]) with mapi id 15.20.4628.018; Fri, 22 Oct 2021 17:51:48 +0000 From: "Sami Mujawar" Subject: Re: [PATCH 2/2] ArmPkg: Add Library/MpInitLib to support EFI_MP_SERVICES_PROTOCOL To: Rebecca Cran , Ard Biesheuvel , Gerd Hoffmann , Samer El-Haj-Mahmoud , Leif Lindholm , devel@edk2.groups.io, nd References: <20211018153945.1690-1-rebecca@nuviainc.com> <20211018153945.1690-3-rebecca@nuviainc.com> Message-ID: Date: Fri, 22 Oct 2021 18:51:48 +0100 User-Agent: Mozilla/5.0 (Windows NT 10.0; WOW64; rv:52.0) Gecko/20100101 Thunderbird/52.0.1 In-Reply-To: <20211018153945.1690-3-rebecca@nuviainc.com> X-ClientProxiedBy: LO2P265CA0302.GBRP265.PROD.OUTLOOK.COM (2603:10a6:600:a5::26) To AS8PR08MB6806.eurprd08.prod.outlook.com (2603:10a6:20b:39b::12) MIME-Version: 1.0 Received: from [10.1.196.43] (217.140.106.52) by LO2P265CA0302.GBRP265.PROD.OUTLOOK.COM (2603:10a6:600:a5::26) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384) id 15.20.4628.18 via Frontend Transport; Fri, 22 Oct 2021 17:51:47 +0000 X-MS-PublicTrafficType: Email X-MS-Office365-Filtering-Correlation-Id: 4162eb2a-18e3-4eb1-8f9e-08d99584a7df X-MS-TrafficTypeDiagnostic: AM6PR08MB4408:|AS8PR08MB6679: X-Microsoft-Antispam-PRVS: x-checkrecipientrouted: true NoDisclaimer: true X-MS-Oob-TLC-OOBClassifiers: OLM:8273;OLM:8273; X-MS-Exchange-SenderADCheck: 1 X-MS-Exchange-AntiSpam-Relay: 0 X-Microsoft-Antispam-Untrusted: BCL:0; X-Microsoft-Antispam-Message-Info-Original: X+MDXLJ8T0v/oq25tNQ8wne92cE8J6JE79ff76ZOaLIBGMO7ngkZsm1iqBAD1TVkGrsHix9wAb3gTe6j8FyNJX5LOZG3c8m9/xyFc72MSSEENpJKv+G/zxWbGWzNmleS53ZCyuglOYeHyJa8FphaLS798c3YL+tB5KA+0pYH86Mu9M8o1XgPbp+czsJGst0bkUMle3kcobeINNUBwSqeevz4AukVgpg+zBpYnp+gN6HgGAiACQXwNGGoF+VUOThFp2x1HnyphlrtLl6cPzLwpoxh1CLEAJNg2AzzW7KLGn9jHaVNMTDmALBc7x1hIasrF+VlxMr0AvccyaBOBosfZ4kjoWoBcBEl9Tdbe9Hil3brCJdY9DI88nk7dIe9w21f/rrmUUdUjeVQIfFDriEkPdYp4mSiB7vpmz7MmqMwf3GksmuPN40kMUVQAMA0LfhBRAdgoMCw6XpJeFVMz7scDyGSLcbkskymOJmryG34DADNrwpQ+AXzOuX7TPlduPLxlRuyl7Q+8uqbo+H7T3WyZQSGIrKanVJilMTpoJGXmbHsQOtOXL/vR/WmCCrwls96zXO9vdGImY1D1i4eZdk+uX43yoVapYwFtrQ6XAtlG9dZdmAMv8O/Zjh+NlW7gDhpC30GKokaSCSpMfXaQLt8I8VW2B5pu/joX3oqgXDJe9cBWp1gGYMo7FGNukyaIy8yqb3GGsD9fR7xj5OqcPJ3ZZTHKd3zjiPJRcvNFU8y/LRVhoI7xqP8dr8Ml5mJymS8iGwdYj0K31Mx97koNGdyvV+hvbhr7u+YsDXhxpCcw7B/0LGYOfc7bY4hR0UiqiJ9O2EsmBSfNVogeQjbwcg+/9JzmjCRpqkQxvEM6XMc7s+saF4M2D/0FNaw7SO2W7tX X-Forefront-Antispam-Report-Untrusted: CIP:255.255.255.255;CTRY:;LANG:en;SCL:1;SRV:;IPV:NLI;SFV:NSPM;H:AS8PR08MB6806.eurprd08.prod.outlook.com;PTR:;CAT:NONE;SFS:(6029001)(4636009)(366004)(84040400005)(30864003)(2616005)(31696002)(186003)(6486002)(956004)(26005)(83380400001)(5660300002)(33964004)(52116002)(2906002)(53546011)(36756003)(6636002)(166002)(44832011)(86362001)(66946007)(316002)(66556008)(16576012)(38100700002)(8676002)(8936002)(110136005)(966005)(66476007)(31686004)(38350700002)(19627235002)(508600001)(43740500002)(45980500001)(579004)(569008)(559001)(44824005);DIR:OUT;SFP:1101; X-MS-Exchange-Transport-CrossTenantHeadersStamped: AM6PR08MB4408 Original-Authentication-Results: dkim=none (message not signed) header.d=none;dmarc=none action=none header.from=arm.com; Return-Path: Sami.Mujawar@arm.com X-EOPAttributedMessage: 0 X-MS-Exchange-Transport-CrossTenantHeadersStripped: VE1EUR03FT024.eop-EUR03.prod.protection.outlook.com X-MS-Office365-Filtering-Correlation-Id-Prvs: 58a0a6ef-de74-43d1-22be-08d995849ea6 X-Microsoft-Antispam: BCL:0; X-Microsoft-Antispam-Message-Info: EI+9f1kptK40ZKRX3PR4zs50SUdFSuLF5dLWuXW26VQbJU+rdGdBEifkLOh5i4dXuNCKzcrPs26BGDrmt8818GRN+CUc5JzevpJmLmtqZqsHe+yJ0FlUanXJL6cX4SShTWMtKKtZ5LhxWrD1RuPfjMn4sH4VH31+tNVZRH+MMva385VaI6jztmQ444E0ts6jCTRxCe03rUmjxmBon2+XX2OUs8I/S1sfX6UpQs+cjXaIyOqKrS0WyxUJIXIpkMINze0LecgIjGDSxuOco6eNUQHDty1g/6Zq7MnM8BVjHc8scylPp94WuyuTG6OpS99VzhMAkcMRCGpl4S/Zu2GHxpB8Hr1Iw2ZOm2T3UsgZB7T4NOPFdlJAml0aHwsNI/BnXM9Pcny+xN0p0C+ILoNPLYaHHUGN7XIvDe2xEBcDGKNgSfeimjyLGjj2V3QWELRcdQkf0R+/oQmE5thj0mzrBpxb8KnKJc1O5HbyfHkJKVbAOAIaKo7OZHBAXVvClKcJsKwVGg2tUlsdj0O6dVBKtKRSrNIYff2poCOfoBLXvpzfjypvsXaGKCnCKRrfCGsgkoeEw1ZCValBUVhnVMGH3u2mkm+1EBdkColoo34njlKxFRa8gst5KCJlTYjUF+HSGFLGzSwRtVJOKvilesH0oyRmfL7LBUDdQ+OK6HWq79X2VFxxhtCbbHghdywLqHAegZbgntISpdxOiiKsEoXnEnEXKkkwyzAfEiLMCG1C8AacVCci1GCbfmTc5Aae/VqAcqRJ/Ym1MmYfQGY3hg9qiukbNz/RGSKN32Fr1MLrt0td8KOBCkc5baqzyvTKHi44SbAVwwfIPPKy/GeODbe4+2jRTtH9+hhdkOHVe9f7avs= X-Forefront-Antispam-Report: CIP:63.35.35.123;CTRY:IE;LANG:en;SCL:1;SRV:;IPV:CAL;SFV:NSPM;H:64aa7808-outbound-1.mta.getcheckrecipient.com;PTR:ec2-63-35-35-123.eu-west-1.compute.amazonaws.com;CAT:NONE;SFS:(6029001)(4636009)(84040400005)(36840700001)(46966006)(70586007)(31686004)(8936002)(5660300002)(16576012)(86362001)(36756003)(33964004)(6636002)(966005)(26005)(2906002)(19627235002)(508600001)(110136005)(81166007)(316002)(6486002)(44832011)(336012)(82310400003)(31696002)(186003)(30864003)(36860700001)(166002)(956004)(2616005)(47076005)(356005)(83380400001)(8676002)(53546011)(70206006)(43740500002)(579004)(559001)(569008)(44824005);DIR:OUT;SFP:1101; X-OriginatorOrg: arm.com X-MS-Exchange-CrossTenant-OriginalArrivalTime: 22 Oct 2021 17:52:03.2884 (UTC) X-MS-Exchange-CrossTenant-Network-Message-Id: 4162eb2a-18e3-4eb1-8f9e-08d99584a7df X-MS-Exchange-CrossTenant-Id: f34e5979-57d9-4aaa-ad4d-b122a662184d X-MS-Exchange-CrossTenant-OriginalAttributedTenantConnectingIp: TenantId=f34e5979-57d9-4aaa-ad4d-b122a662184d;Ip=[63.35.35.123];Helo=[64aa7808-outbound-1.mta.getcheckrecipient.com] X-MS-Exchange-CrossTenant-AuthSource: VE1EUR03FT024.eop-EUR03.prod.protection.outlook.com X-MS-Exchange-CrossTenant-AuthAs: Anonymous X-MS-Exchange-CrossTenant-FromEntityHeader: HybridOnPrem X-MS-Exchange-Transport-CrossTenantHeadersStamped: AS8PR08MB6679 Content-Type: multipart/alternative; boundary="------------F6A6CAB149BAC3BDAA6CE911" Content-Language: en-GB --------------F6A6CAB149BAC3BDAA6CE911 Content-Type: text/plain; charset="utf-8"; format=flowed Content-Transfer-Encoding: quoted-printable Hi Rebecca, Thanks you for this patch. I think this patch could be split in 2 - 3 patches (CpuDxe & MpInitLib and= the ArmVirtPkg change should be a separate patch). Please find my feedback inline marked [SAMI]. I think the usage of MPIDR and CpuInfo->ProcessorId needs to be revisited i= n this patch series. I have kept my feedback on usage of MPIDR and the MT b= it [24] to give the context. But, it will be good to only retain the affini= ty bits and mask all other bits of the MPIDR. Please let me know if you have any queries. Regards, Sami Mujawar On 18/10/2021 04:39 PM, Rebecca Cran wrote: Add support for EFI_MP_SERVICES_PROTOCOL during the DXE phase under AArch64. PSCI_CPU_ON is called to power on the core, the supplied procedure is executed and PSCI_CPU_OFF is called to power off the core. Minimal setup is done before calling the supplied procedure: for example the MMU and caches are not enabled. Signed-off-by: Rebecca Cran --- ArmPkg/ArmPkg.dec | 4 + ArmPkg/ArmPkg.dsc | 4 + ArmPkg/Drivers/CpuDxe/AArch64/Arch.c | 22 + ArmPkg/Drivers/CpuDxe/Arm/Arch.c | 22 + ArmPkg/Drivers/CpuDxe/CpuDxe.c | 2 + ArmPkg/Drivers/CpuDxe/CpuDxe.h | 10 + ArmPkg/Drivers/CpuDxe/CpuDxe.inf | 6 + ArmPkg/Drivers/CpuDxe/CpuMpInit.c | 604 ++++++++ ArmPkg/Include/Library/MpInitLib.h | 366 +++++ ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S | 61 + ArmPkg/Library/MpInitLib/DxeMpInitLib.inf | 53 + ArmPkg/Library/MpInitLib/DxeMpLib.c | 1498 ++++++++++++++++++++ ArmPkg/Library/MpInitLib/InternalMpInitLib.h | 358 +++++ ArmVirtPkg/ArmVirt.dsc.inc | 3 + 14 files changed, 3013 insertions(+) diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec index 5935663fa9a3..f8bb4af21749 100644 --- a/ArmPkg/ArmPkg.dec +++ b/ArmPkg/ArmPkg.dec @@ -74,6 +74,10 @@ # DefaultExceptionHandlerLib|Include/Library/DefaultExceptionHandlerLib.h + ## @libraryclass Provides a MP Services interface. + # + MpInitLib|Include/Library/MpInitLib.h + ## @libraryclass Provides an interface to query miscellaneous OEM # information. # diff --git a/ArmPkg/ArmPkg.dsc b/ArmPkg/ArmPkg.dsc index 8abe3713c829..26fb8bb94c07 100644 --- a/ArmPkg/ArmPkg.dsc +++ b/ArmPkg/ArmPkg.dsc @@ -99,6 +99,9 @@ PeiServicesLib|MdePkg/Library/PeiServicesLib/PeiServicesLib.inf PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLib/Pei= ServicesTablePointerLib.inf +[LibraryClasses.AARCH64] + MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf + [LibraryClasses.ARM, LibraryClasses.AARCH64] NULL|ArmPkg/Library/CompilerIntrinsicsLib/CompilerIntrinsicsLib.inf @@ -163,4 +166,5 @@ ArmPkg/Library/ArmMmuLib/ArmMmuPeiLib.inf [Components.AARCH64, Components.ARM] + ArmPkg/Library/MpInitLib/DxeMpInitLib.inf [SAMI] Should this be in the [Components.AARCH64] section, as DxeMpInitLib.= inf currently supports AARCH64 only? ArmPkg/Library/StandaloneMmMmuLib/ArmMmuStandaloneMmLib.inf diff --git a/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c b/ArmPkg/Drivers/CpuDxe/A= Arch64/Arch.c new file mode 100644 index 000000000000..a4b9e9037100 --- /dev/null +++ b/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c @@ -0,0 +1,22 @@ +/** @file + Architecture specific functions. + + Copyright (c) 2021, NUVIA Inc. All rights reserved.
+ + SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + + +#include + +/** Initializes multi-processor support. + * +**/ +VOID +ArchInitializeMpSupport ( + VOID + ) +{ + InitializeMpSupport (); +} diff --git a/ArmPkg/Drivers/CpuDxe/Arm/Arch.c b/ArmPkg/Drivers/CpuDxe/Arm/A= rch.c new file mode 100644 index 000000000000..0ded264cd06a --- /dev/null +++ b/ArmPkg/Drivers/CpuDxe/Arm/Arch.c @@ -0,0 +1,22 @@ +/** @file + Architecture specific functions. + + Copyright (c) 2021, NUVIA Inc. All rights reserved.
+ + SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + + +#include + +/** Initializes multi-processor support. + * +**/ +VOID +ArchInitializeMpSupport ( + VOID + ) +{ + /* Nothing to do - ARM doesn't support EFI_MP_SERVICES_PROTOCOL */ +} diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.c b/ArmPkg/Drivers/CpuDxe/CpuDxe.= c index 082ef30fb6c4..db8752f5b54f 100644 --- a/ArmPkg/Drivers/CpuDxe/CpuDxe.c +++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.c @@ -279,5 +279,7 @@ CpuDxeInitialize ( ); ASSERT_EFI_ERROR (Status); + ArchInitializeMpSupport (); + return Status; } diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.h b/ArmPkg/Drivers/CpuDxe/CpuDxe.= h index 4cf3ab258c24..6e7ceafa4436 100644 --- a/ArmPkg/Drivers/CpuDxe/CpuDxe.h +++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.h @@ -143,4 +143,14 @@ SetGcdMemorySpaceAttributes ( IN UINT64 Attributes ); +VOID +InitializeMpSupport ( + VOID + ); + +VOID +ArchInitializeMpSupport ( + VOID + ); + #endif // CPU_DXE_H_ diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf b/ArmPkg/Drivers/CpuDxe/CpuDx= e.inf index e5549fc71df7..f4cdb8ab5613 100644 --- a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf +++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf @@ -26,10 +26,13 @@ Exception.c [Sources.ARM] + Arm/Arch.c Arm/Mmu.c [Sources.AARCH64] + AArch64/Arch.c AArch64/Mmu.c + CpuMpInit.c [Packages] ArmPkg/ArmPkg.dec @@ -37,6 +40,9 @@ MdePkg/MdePkg.dec MdeModulePkg/MdeModulePkg.dec +[LibraryClasses.AARCH64] + MpInitLib + [LibraryClasses] ArmLib ArmMmuLib diff --git a/ArmPkg/Drivers/CpuDxe/CpuMpInit.c b/ArmPkg/Drivers/CpuDxe/CpuM= pInit.c new file mode 100644 index 000000000000..5bf4ed12b711 --- /dev/null +++ b/ArmPkg/Drivers/CpuDxe/CpuMpInit.c @@ -0,0 +1,604 @@ +/** @file + Construct MP Services Protocol. + + The MP Services Protocol provides a generalized way of performing follow= ing tasks: + - Retrieving information of multi-processor environment and MP-related= status of + specific processors. + - Dispatching user-provided function to APs. + - Maintain MP-related processor status. + + The MP Services Protocol must be produced on any system with more than o= ne logical + processor. + + The Protocol is available only during boot time. + + MP Services Protocol is hardware-independent. Most of the logic of this = protocol + is architecturally neutral. It abstracts the multi-processor environment= and + status of processors, and provides interfaces to retrieve information, m= aintain, + and dispatch. + + MP Services Protocol may be consumed by ACPI module. The ACPI module may= use this + protocol to retrieve data that are needed for an MP platform and report = them to OS. + MP Services Protocol may also be used to program and configure processor= s, such + as MTRR synchronization for memory space attributes setting in DXE Servi= ces. + MP Services Protocol may be used by non-CPU DXE drivers to speed up plat= form boot + by taking advantage of the processing capabilities of the APs, for examp= le, using + APs to help test system memory in parallel with other device initializat= ion. + Diagnostics applications may also use this protocol for multi-processor. + + Copyright (c) 2021, NUVIA Inc. All rights reserved.
+ SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + +#include +#include +#include +#include + +/** + This service retrieves the number of logical processor in the platform + and the number of those logical processors that are enabled on this boot= . + This service may only be called from the BSP. + + This function is used to retrieve the following information: + - The number of logical processors that are present in the system. + - The number of enabled logical processors in the system at the instan= t + this call is made. + + Because MP Service Protocol provides services to enable and disable proc= essors + dynamically, the number of enabled logical processors may vary during th= e + course of a boot session. + + If this service is called from an AP, then EFI_DEVICE_ERROR is returned. + If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then + EFI_INVALID_PARAMETER is returned. Otherwise, the total number of proces= sors + is returned in NumberOfProcessors, the number of currently enabled proce= ssor + is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned. + + @param[in] This A pointer to the + EFI_MP_SERVICES_PROTOCOL instanc= e. + @param[out] NumberOfProcessors Pointer to the total number of l= ogical + processors in the system, includ= ing + the BSP and disabled APs. + @param[out] NumberOfEnabledProcessors Pointer to the number of enabled + logical processors that exist in= the + system, including the BSP. + + @retval EFI_SUCCESS The number of logical processors and ena= bled + logical processors was retrieved. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER NumberOfProcessors is NULL. + @retval EFI_INVALID_PARAMETER NumberOfEnabledProcessors is NULL. + +**/ +STATIC +EFI_STATUS +EFIAPI +GetNumberOfProcessors ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *NumberOfProcessors, + OUT UINTN *NumberOfEnabledProcessors + ) +{ + return MpInitLibGetNumberOfProcessors ( + This, + NumberOfProcessors, + NumberOfEnabledProcessors + ); [SAMI] Please adjust the code alignment. +} + +/** + Gets detailed MP-related information on the requested processor at the + instant this call is made. This service may only be called from the BSP. + + This service retrieves detailed MP-related information about any process= or + on the platform. Note the following: + - The processor information may change during the course of a boot ses= sion. + - The information presented here is entirely MP related. + + Information regarding the number of caches and their sizes, frequency of + operation, slot numbers is all considered platform-related information a= nd is + not provided by this service. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTO= COL + instance. + @param[in] ProcessorNumber The index of the processor. + @param[out] ProcessorInfoBuffer A pointer to the buffer where informat= ion + for the requested processor is deposit= ed. + + @retval EFI_SUCCESS Processor information was returned. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER ProcessorInfoBuffer is NULL. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist in the pl= atform. + +**/ +STATIC +EFI_STATUS +EFIAPI +GetProcessorInfo ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer + ) +{ + return MpInitLibGetProcessorInfo ( + This, + ProcessorNumber, + ProcessorInfoBuffer + ); +} + +/** + This service executes a caller provided function on all enabled APs. APs= can + run either simultaneously or one at a time in sequence. This service sup= ports + both blocking and non-blocking requests. The non-blocking requests use E= FI + events so the BSP can detect when the APs have finished. This service ma= y only + be called from the BSP. + + This function is used to dispatch all the enabled APs to the function + specified by Procedure. If any enabled AP is busy, then EFI_NOT_READY i= s + returned immediately and Procedure is not started on any AP. + + If SingleThread is TRUE, all the enabled APs execute the function specif= ied by + Procedure one by one, in ascending order of processor handle number. + Otherwise, all the enabled APs execute the function specified by Procedu= re + simultaneously. + + If WaitEvent is NULL, execution is in blocking mode. The BSP waits until= all + APs finish or TimeoutInMicroseconds expires. Otherwise, execution is in + non-blocking mode, and the BSP returns from this service without waiting= for + APs. If a non-blocking mode is requested after the UEFI Event + EFI_EVENT_GROUP_READY_TO_BOOT is signaled, then EFI_UNSUPPORTED must be + returned. + + If the timeout specified by TimeoutInMicroseconds expires before all APs + return from Procedure, then Procedure on the failed APs is terminated. + All enabled APs are always available for further calls to + EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and + EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). If FailedCpuList is not NULL, = its + content points to the list of processor handle numbers in which Procedur= e was + terminated. + + Note: It is the responsibility of the consumer of the + EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() to make sure that the nature of= the + code that is executed on the BSP and the dispatched APs is well controll= ed. + The MP Services Protocol does not guarantee that the Procedure function = is + MP-safe. Hence, the tasks that can be run in parallel are limited to cer= tain + independent tasks and well-controlled exclusive code. EFI services and + protocols may not be called by APs unless otherwise specified. + + In blocking execution mode, BSP waits until all APs finish or + TimeoutInMicroseconds expires. + + In non-blocking execution mode, BSP is freed to return to the caller and= then + proceed to the next task without having to wait for APs. The following + sequence needs to occur in a non-blocking execution mode: + + -# The caller that intends to use this MP Services Protocol in non-blo= cking + mode creates WaitEvent by calling the EFI CreateEvent() service. T= he + caller invokes EFI_MP_SERVICES_PROTOCOL.StartupAllAPs(). If the par= ameter + WaitEvent is not NULL, then StartupAllAPs() executes in non-blockin= g + mode. It requests the function specified by Procedure to be started= on + all the enabled APs, and releases the BSP to continue with other ta= sks. + -# The caller can use the CheckEvent() and WaitForEvent() services to = check + the state of the WaitEvent created in step 1. + -# When the APs complete their task or TimeoutInMicroSecondss expires,= the + MP Service signals WaitEvent by calling the EFI SignalEvent() funct= ion. + If FailedCpuList is not NULL, its content is available when WaitEve= nt is + signaled. If all APs returned from Procedure prior to the timeout, = then + FailedCpuList is set to NULL. If not all APs return from Procedure = before + the timeout, then FailedCpuList is filled in with the list of the f= ailed + APs. The buffer is allocated by MP Service Protocol using AllocateP= ool(). + It is the caller's responsibility to free the buffer with FreePool(= ) + service. + -# This invocation of SignalEvent() function informs the caller that i= nvoked + EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() that either all the APs + completed the specified task or a timeout occurred. The contents of + FailedCpuList can be examined to determine which APs did not comple= te the + specified task prior to the timeout. + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] SingleThread If TRUE, then all the enabled APs ex= ecute + the function specified by Procedure = one by + one, in ascending order of processor + handle number. If FALSE, then all t= he + enabled APs execute the function spe= cified + by Procedure simultaneously. + @param[in] WaitEvent The event created by the caller with + CreateEvent() service. If it is NUL= L, + then execute in blocking mode. BSP w= aits + until all APs finish or + TimeoutInMicroseconds expires. If i= t's + not NULL, then execute in non-blocki= ng + mode. BSP requests the function spec= ified + by Procedure to be started on all th= e + enabled APs, and go on executing + immediately. If all return from Proc= edure, + or TimeoutInMicroseconds expires, th= is + event is signaled. The BSP can use t= he + CheckEvent() or WaitForEvent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds + for APs to return from Procedure, ei= ther + for blocking or non-blocking mode. Z= ero + means infinity. If the timeout expi= res + before all APs return from Procedure= , then + Procedure on the failed APs is termi= nated. + All enabled APs are available for ne= xt + function assigned by + EFI_MP_SERVICES_PROTOCOL.StartupAllA= Ps() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] FailedCpuList If NULL, this parameter is ignored. + Otherwise, if all APs finish success= fully, + then its content is set to NULL. If = not + all APs finish before timeout expire= s, + then its content is set to address o= f the + buffer holding handle numbers of the + failed APs. + The buffer is allocated by MP Servic= e + Protocol, and it's the caller's + responsibility to free the buffer wi= th + FreePool() service. + In blocking mode, it is ready for + consumption when the call returns. I= n + non-blocking mode, it is ready when + WaitEvent is signaled. The list of f= ailed + CPU is terminated by END_OF_CPU_LIS= T. + + @retval EFI_SUCCESS In blocking mode, all APs have finished = before + the timeout expired. + @retval EFI_SUCCESS In non-blocking mode, function has been + dispatched to all enabled APs. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR Caller processor is AP. + @retval EFI_NOT_STARTED No enabled APs exist in the system. + @retval EFI_NOT_READY Any enabled APs are busy. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + all enabled APs have finished. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +STATIC +EFI_STATUS +EFIAPI +StartupAllAPs ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN BOOLEAN SingleThread, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT UINTN **FailedCpuList OPTIONAL + ) +{ + return MpInitLibStartupAllAPs ( + This, + Procedure, + SingleThread, + WaitEvent, + TimeoutInMicroseconds, + ProcedureArgument, + FailedCpuList + ); +} + +/** + This service lets the caller get one enabled AP to execute a caller-prov= ided + function. The caller can request the BSP to either wait for the completi= on + of the AP or just proceed with the next task by using the EFI event mech= anism. + See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo= cking + execution support. This service may only be called from the BSP. + + This function is used to dispatch one enabled AP to the function specifi= ed by + Procedure passing in the argument specified by ProcedureArgument. If Wa= itEvent + is NULL, execution is in blocking mode. The BSP waits until the AP finis= hes or + TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking = mode. + BSP proceeds to the next task without waiting for the AP. If a non-block= ing mode + is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signa= led, + then EFI_UNSUPPORTED must be returned. + + If the timeout specified by TimeoutInMicroseconds expires before the AP = returns + from Procedure, then execution of Procedure by the AP is terminated. The= AP is + available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs= () and + EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] ProcessorNumber The handle number of the AP. The ran= ge is + from 0 to the total number of logica= l + processors minus 1. The total number= of + logical processors can be retrieved = by + EFI_MP_SERVICES_PROTOCOL.GetNumberOf= Processors(). + @param[in] WaitEvent The event created by the caller with= CreateEvent() + service. If it is NULL, then execut= e in + blocking mode. BSP waits until all A= Ps finish + or TimeoutInMicroseconds expires. I= f it's + not NULL, then execute in non-blocki= ng mode. + BSP requests the function specified = by + Procedure to be started on all the e= nabled + APs, and go on executing immediately= . If + all return from Procedure or Timeout= InMicroseconds + expires, this event is signaled. The= BSP + can use the CheckEvent() or WaitForE= vent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds for + APs to return from Procedure, either= for + blocking or non-blocking mode. Zero = means + infinity. If the timeout expires be= fore + all APs return from Procedure, then = Procedure + on the failed APs is terminated. All= enabled + APs are available for next function = assigned + by EFI_MP_SERVICES_PROTOCOL.StartupA= llAPs() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] Finished If NULL, this parameter is ignored. = In + blocking mode, this parameter is ign= ored. + In non-blocking mode, if AP returns = from + Procedure before the timeout expires= , its + content is set to TRUE. Otherwise, t= he + value is set to FALSE. The caller ca= n + determine if the AP returned from Pr= ocedure + by evaluating this value. + + @retval EFI_SUCCESS In blocking mode, specified AP finished = before + the timeout expires. + @retval EFI_SUCCESS In non-blocking mode, the function has b= een + dispatched to specified AP. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + the specified AP has finished. + @retval EFI_NOT_READY The specified AP is busy. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP or dis= abled AP. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +STATIC +EFI_STATUS +EFIAPI +StartupThisAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN UINTN ProcessorNumber, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT BOOLEAN *Finished OPTIONAL + ) +{ + return MpInitLibStartupThisAP ( + This, + Procedure, + ProcessorNumber, + WaitEvent, + TimeoutInMicroseconds, + ProcedureArgument, + Finished + ); +} + +/** + This service switches the requested AP to be the BSP from that point onw= ard. + This service changes the BSP for all purposes. This call can only be + performed by the current BSP. + + This service switches the requested AP to be the BSP from that point onw= ard. + This service changes the BSP for all purposes. The new BSP can take over= the + execution of the old BSP and continue seamlessly from where the old one = left + off. This service may not be supported after the UEFI Event EFI_EVENT_GR= OUP_READY_TO_BOOT + is signaled. + + If the BSP cannot be switched prior to the return from this service, the= n + EFI_UNSUPPORTED must be returned. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableOldBSP If TRUE, then the old BSP will be listed as= an + enabled AP. Otherwise, it will be disabled. + + @retval EFI_SUCCESS BSP successfully switched. + @retval EFI_UNSUPPORTED Switching the BSP cannot be completed pr= ior to + this service returning. + @retval EFI_UNSUPPORTED Switching the BSP is not supported. + @retval EFI_SUCCESS The calling processor is an AP. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the current BS= P or + a disabled AP. + @retval EFI_NOT_READY The specified AP is busy. + +**/ +STATIC +EFI_STATUS +EFIAPI +SwitchBSP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableOldBSP + ) +{ + return MpInitLibSwitchBSP (This, ProcessorNumber, EnableOldBSP); +} + +/** + This service lets the caller enable or disable an AP from this point onw= ard. + This service may only be called from the BSP. + + This service allows the caller enable or disable an AP from this point o= nward. + The caller can optionally specify the health status of the AP by Health.= If + an AP is being disabled, then the state of the disabled AP is implementa= tion + dependent. If an AP is enabled, then the implementation must guarantee t= hat a + complete initialization sequence is performed on the AP, so the AP is in= a state + that is compatible with an MP operating system. This service may not be = supported + after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled. + + If the enable or disable AP operation cannot be completed prior to the r= eturn + from this service, then EFI_UNSUPPORTED must be returned. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableAP Specifies the new state for the processor f= or + enabled, FALSE for disabled. + @param[in] HealthFlag If not NULL, a pointer to a value that spec= ifies + the new health status of the AP. This flag + corresponds to StatusFlag defined in + EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()= . Only + the PROCESSOR_HEALTH_STATUS_BIT is used. Al= l other + bits are ignored. If it is NULL, this para= meter + is ignored. + + @retval EFI_SUCCESS The specified AP was enabled or disabled= successfully. + @retval EFI_UNSUPPORTED Enabling or disabling an AP cannot be co= mpleted + prior to this service returning. + @retval EFI_UNSUPPORTED Enabling or disabling an AP is not suppo= rted. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_NOT_FOUND Processor with the handle specified by P= rocessorNumber + does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP. + +**/ +STATIC +EFI_STATUS +EFIAPI +EnableDisableAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableAP, + IN UINT32 *HealthFlag OPTIONAL + ) +{ + return MpInitLibEnableDisableAP (This, ProcessorNumber, EnableAP, Health= Flag); +} + +/** + This return the handle number for the calling processor. This service m= ay be + called from the BSP and APs. + + This service returns the processor handle number for the calling process= or. + The returned value is in the range from 0 to the total number of logical + processors minus 1. The total number of logical processors can be retrie= ved + with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may = be + called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALI= D_PARAMETER + is returned. Otherwise, the current processors handle number is returned= in + ProcessorNumber, and EFI_SUCCESS is returned. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[out] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + + @retval EFI_SUCCESS The current processor handle number was = returned + in ProcessorNumber. + @retval EFI_INVALID_PARAMETER ProcessorNumber is NULL. + +**/ +STATIC +EFI_STATUS +EFIAPI +WhoAmI ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *ProcessorNumber + ) +{ + return MpInitLibWhoAmI (This, ProcessorNumber); +} + +EFI_MP_SERVICES_PROTOCOL mMpServicesTemplate =3D { + GetNumberOfProcessors, + GetProcessorInfo, + StartupAllAPs, + StartupThisAP, + SwitchBSP, + EnableDisableAP, + WhoAmI +}; + +/** Initialize multi-processor support. + +**/ +VOID +InitializeMpSupport ( + VOID + ) +{ + EFI_STATUS Status; + EFI_HANDLE Handle; + UINTN MaxCpus; + EFI_HOB_GENERIC_HEADER *Hob; + VOID *HobData; + UINTN HobDataSize; + ARM_PROCESSOR_TABLE CpuInfo; + + DEBUG ((DEBUG_INFO, "Starting MP services")); + + Hob =3D GetFirstGuidHob (&gArmMpCoreInfoGuid); + if (Hob !=3D NULL) { + HobData =3D GET_GUID_HOB_DATA (Hob); + HobDataSize =3D GET_GUID_HOB_DATA_SIZE (Hob); + CpuInfo.ArmCpus =3D (ARM_CORE_INFO *)HobData; + CpuInfo.NumberOfEntries =3D HobDataSize / sizeof (ARM_CORE_INFO); + MaxCpus =3D CpuInfo.NumberOfEntries; + } + + if (MaxCpus =3D=3D 1) { + DEBUG ((DEBUG_WARN, "Trying to use EFI_MP_SERVICES_PROTOCOL on a UP sy= stem")); + // We are not MP so nothing to do + return; + } + + MpInitLibInitialize (MaxCpus, &CpuInfo); [SAMI] There is a chance that MaxCpus will be uninitialised if the Hob is N= ULL. I think MaxCpus must be initialised to zero at the start of this funct= ion. I also think CpuInfo.Header should be initialised (or at least zeroed) befo= re use, to prevent CpuInf.Header from containing random data. [/SAMI] + + // + // Now install the MP services protocol. + // + Handle =3D NULL; + Status =3D gBS->InstallMultipleProtocolInterfaces ( + &Handle, + &gEfiMpServiceProtocolGuid, + &mMpServicesTemplate, + NULL + ); + ASSERT_EFI_ERROR (Status); +} diff --git a/ArmPkg/Include/Library/MpInitLib.h b/ArmPkg/Include/Library/Mp= InitLib.h new file mode 100644 index 000000000000..a4b80c18a9e8 --- /dev/null +++ b/ArmPkg/Include/Library/MpInitLib.h @@ -0,0 +1,366 @@ +/** @file + +Copyright (c) 2021, NUVIA Inc. All rights reserved.
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.
+Portions copyright (c) 2011, Apple Inc. All rights reserved. + +SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + +#ifndef MP_INITLIB_H_ +#define MP_INITLIB_H_ + +#include +#include +#include +#include +#include + + +/** + This service retrieves the number of logical processor in the platform + and the number of those logical processors that are enabled on this boot= . + This service may only be called from the BSP. + + @param[in] This A pointer to the + EFI_MP_SERVICES_PROTOCOL instanc= e. + @param[out] NumberOfProcessors Pointer to the total number of l= ogical + processors in the system, includ= ing + the BSP and disabled APs. + @param[out] NumberOfEnabledProcessors Pointer to the number of enabled + logical processors that exist in= the + system, including the BSP. + + @retval EFI_SUCCESS The number of logical processors and ena= bled + logical processors was retrieved. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER NumberOfProcessors is NULL. + @retval EFI_INVALID_PARAMETER NumberOfEnabledProcessors is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibGetNumberOfProcessors ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *NumberOfProcessors, + OUT UINTN *NumberOfEnabledProcessors + ); + +/** + Gets detailed MP-related information on the requested processor at the + instant this call is made. This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTO= COL + instance. + @param[in] ProcessorIndex The index of the processor. + @param[out] ProcessorInfoBuffer A pointer to the buffer where informat= ion + for the requested processor is deposit= ed. + + @retval EFI_SUCCESS Processor information was returned. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER ProcessorInfoBuffer is NULL. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist in the pl= atform. + +**/ +EFI_STATUS +EFIAPI +MpInitLibGetProcessorInfo ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorIndex, + OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer + ); + + +/** + This service executes a caller provided function on all enabled APs. APs= can + run either simultaneously or one at a time in sequence. This service sup= ports + both blocking and non-blocking requests. The non-blocking requests use E= FI + events so the BSP can detect when the APs have finished. This service ma= y only + be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] SingleThread If TRUE, then all the enabled APs ex= ecute + the function specified by Procedure = one by + one, in ascending order of processor + handle number. If FALSE, then all t= he + enabled APs execute the function spe= cified + by Procedure simultaneously. + @param[in] WaitEvent The event created by the caller with + CreateEvent() service. If it is NUL= L, + then execute in blocking mode. BSP w= aits + until all APs finish or + TimeoutInMicroseconds expires. If i= t's + not NULL, then execute in non-blocki= ng + mode. BSP requests the function spec= ified + by Procedure to be started on all th= e + enabled APs, and go on executing + immediately. If all return from Proc= edure, + or TimeoutInMicroseconds expires, th= is + event is signaled. The BSP can use t= he + CheckEvent() or WaitForEvent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds + for APs to return from Procedure, ei= ther + for blocking or non-blocking mode. Z= ero + means infinity. If the timeout expi= res + before all APs return from Procedure= , then + Procedure on the failed APs is termi= nated. + All enabled APs are available for ne= xt + function assigned by + EFI_MP_SERVICES_PROTOCOL.StartupAllA= Ps() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] FailedCpuList If NULL, this parameter is ignored. + Otherwise, if all APs finish success= fully, + then its content is set to NULL. If = not + all APs finish before timeout expire= s, + then its content is set to address o= f the + buffer holding handle numbers of the + failed APs. + The buffer is allocated by MP Servic= e + Protocol, and it's the caller's + responsibility to free the buffer wi= th + FreePool() service. + In blocking mode, it is ready for + consumption when the call returns. I= n + non-blocking mode, it is ready when + WaitEvent is signaled. The list of f= ailed + CPU is terminated by END_OF_CPU_LIS= T. + + @retval EFI_SUCCESS In blocking mode, all APs have finished = before + the timeout expired. + @retval EFI_SUCCESS In non-blocking mode, function has been + dispatched to all enabled APs. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR Caller processor is AP. + @retval EFI_NOT_STARTED No enabled APs exist in the system. + @retval EFI_NOT_READY Any enabled APs are busy. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + all enabled APs have finished. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibStartupAllAPs ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN BOOLEAN SingleThread, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT UINTN **FailedCpuList OPTIONAL + ); + +/** + This service lets the caller get one enabled AP to execute a caller-prov= ided + function. The caller can request the BSP to either wait for the completi= on + of the AP or just proceed with the next task by using the EFI event mech= anism. + See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo= cking + execution support. This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] ProcessorNumber The handle number of the AP. The ran= ge is + from 0 to the total number of logica= l + processors minus 1. The total number= of + logical processors can be retrieved = by + EFI_MP_SERVICES_PROTOCOL.GetNumberOf= Processors(). + @param[in] WaitEvent The event created by the caller with= CreateEvent() + service. If it is NULL, then execut= e in + blocking mode. BSP waits until all A= Ps finish + or TimeoutInMicroseconds expires. I= f it's + not NULL, then execute in non-blocki= ng mode. + BSP requests the function specified = by + Procedure to be started on all the e= nabled + APs, and go on executing immediately= . If + all return from Procedure or Timeout= InMicroseconds + expires, this event is signaled. The= BSP + can use the CheckEvent() or WaitForE= vent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds for + APs to return from Procedure, either= for + blocking or non-blocking mode. Zero = means + infinity. If the timeout expires be= fore + all APs return from Procedure, then = Procedure + on the failed APs is terminated. All= enabled + APs are available for next function = assigned + by EFI_MP_SERVICES_PROTOCOL.StartupA= llAPs() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] Finished If NULL, this parameter is ignored. = In + blocking mode, this parameter is ign= ored. + In non-blocking mode, if AP returns = from + Procedure before the timeout expires= , its + content is set to TRUE. Otherwise, t= he + value is set to FALSE. The caller ca= n + determine if the AP returned from Pr= ocedure + by evaluating this value. + + @retval EFI_SUCCESS In blocking mode, specified AP finished = before + the timeout expires. + @retval EFI_SUCCESS In non-blocking mode, the function has b= een + dispatched to specified AP. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + the specified AP has finished. + @retval EFI_NOT_READY The specified AP is busy. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP or dis= abled AP. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibStartupThisAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN UINTN ProcessorNumber, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT BOOLEAN *Finished OPTIONAL + ); + +/** + This service switches the requested AP to be the BSP from that point onw= ard. + This service changes the BSP for all purposes. This call can only be + performed by the current BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableOldBSP If TRUE, then the old BSP will be listed as= an + enabled AP. Otherwise, it will be disabled. + + @retval EFI_SUCCESS BSP successfully switched. + @retval EFI_UNSUPPORTED Switching the BSP cannot be completed pr= ior to + this service returning. + @retval EFI_UNSUPPORTED Switching the BSP is not supported. + @retval EFI_SUCCESS The calling processor is an AP. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the current BS= P or + a disabled AP. + @retval EFI_NOT_READY The specified AP is busy. + +**/ +EFI_STATUS +EFIAPI +MpInitLibSwitchBSP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableOldBSP + ); + +/** + This service lets the caller enable or disable an AP from this point onw= ard. + This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableAP Specifies the new state for the processor f= or + enabled, FALSE for disabled. + @param[in] HealthFlag If not NULL, a pointer to a value that spec= ifies + the new health status of the AP. This flag + corresponds to StatusFlag defined in + EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()= . Only + the PROCESSOR_HEALTH_STATUS_BIT is used. Al= l other + bits are ignored. If it is NULL, this para= meter + is ignored. + + @retval EFI_SUCCESS The specified AP was enabled or disabled= successfully. + @retval EFI_UNSUPPORTED Enabling or disabling an AP cannot be co= mpleted + prior to this service returning. + @retval EFI_UNSUPPORTED Enabling or disabling an AP is not suppo= rted. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_NOT_FOUND Processor with the handle specified by P= rocessorNumber + does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP. + +**/ +EFI_STATUS +EFIAPI +MpInitLibEnableDisableAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableAP, + IN UINT32 *HealthFlag OPTIONAL + ); + +/** + This return the handle number for the calling processor. This service m= ay be + called from the BSP and APs. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[out] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + + @retval EFI_SUCCESS The current processor handle number was = returned + in ProcessorNumber. + @retval EFI_INVALID_PARAMETER ProcessorNumber is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibWhoAmI ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *ProcessorNumber + ); + +/** Initializes the MP Services system data + + @param NumberOfProcessors The number of processors, both BSP and AP. + @param CpuInfo CPU information gathered earlier during boot. + +**/ +VOID +MpInitLibInitialize ( + IN UINTN NumberOfProcessors, + IN ARM_PROCESSOR_TABLE *CpuInfo + ); + + + +#endif /* MP_INITLIB_H_ */ diff --git a/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S b/ArmPkg/Library/Mp= InitLib/AArch64/MpFuncs.S new file mode 100644 index 000000000000..287db060e594 --- /dev/null +++ b/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S @@ -0,0 +1,61 @@ +#=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D +# Copyright (c) 2021 NUVIA Inc. All rights reserved. +# +# SPDX-License-Identifier: BSD-2-Clause-Patent +#=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D= =3D=3D=3D=3D=3D + +.text +.align 3 + +#include +#include + +#include "InternalMpInitLib.h" + +GCC_ASM_IMPORT (gApStacksBase) +GCC_ASM_IMPORT (gProcessorIDs) +GCC_ASM_IMPORT (ApProcedure) +GCC_ASM_IMPORT (gApStackSize) + +GCC_ASM_EXPORT (ApEntryPoint) + +StartupAddr: .8byte ASM_PFX(ApProcedure) + +// Entry-point for the AP +// VOID +// ApEntryPoint ( +// VOID +// ); +ASM_PFX(ApEntryPoint): + mrs x0, mpidr_el1 + ldr x1, gProcessorIDs + mov x2, 0 // x2 =3D processor index + mov x3, 0 // x3 =3D address offset + +// Find index in gProcessorIDs for current processor +1: + ldr x4, [x1, x3] // x4 =3D gProcessorIDs + x3 + cmp x4, 0 // check if we've reached the end of gProces= sorIDs + beq ProcessorNotFound + add x3, x3, 8 // x3 +=3D sizeof (*gProcessorIDs) + add x2, x2, 1 // x2++ + cmp x0, x4 // if mpidr_el1 !=3D *(gProcessorIDs + x3) t= hen loop [SAMI] I think this may not work for PEs with the MT bit[24] set. Please se= e my comment further ahead in FillInProcessorInformation(). + bne 1b + sub x2, x2, 1 + +// Calculate stack address + // x2 contains the index for the current processor + ldr x0, gApStacksBase + ldr x1, gApStackSize + mul x3, x2, x1 // x3 =3D ProcessorIndex * gApStackSize + add x4, x0, x3 // x4 =3D gApStacksBase + x3 + add sp, x4, x1 // sp =3D x4 + gApStackSize + + ldr x0, StartupAddr // ASM_PFX(ApProcedure) + blr x0 // doesn't return + +ProcessorNotFound: +// Turn off the processor + MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF) + smc #0 + b . diff --git a/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf b/ArmPkg/Library/MpI= nitLib/DxeMpInitLib.inf new file mode 100644 index 000000000000..2275b6cca33a --- /dev/null +++ b/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf @@ -0,0 +1,53 @@ +#/** @file +# +# Component description file for the DxeMpInitLib module. +# +# Copyright (c) 2021, NUVIA Inc. All rights reserved.
+# +# SPDX-License-Identifier: BSD-2-Clause-Patent +# +#**/ + +[Defines] + INF_VERSION =3D 1.29 + BASE_NAME =3D DxeMpInitLib + FILE_GUID =3D c9ca773c-8ae4-4b74-82fd-f7345503294e + MODULE_TYPE =3D DXE_DRIVER + VERSION_STRING =3D 1.0 + LIBRARY_CLASS =3D MpInitLib|DXE_DRIVER + +# +# The following information is for reference only and not required by the = build tools. +# +# VALID_ARCHITECTURES =3D AARCH64 +# + +[Sources.AARCH64] + AArch64/MpFuncs.S + +[Sources] + DxeMpLib.c + InternalMpInitLib.h + +[Packages] + ArmPkg/ArmPkg.dec + MdeModulePkg/MdeModulePkg.dec + MdePkg/MdePkg.dec + +[LibraryClasses] + ArmLib + ArmSmcLib + BaseLib + BaseMemoryLib + DebugLib + HobLib + MemoryAllocationLib + UefiBootServicesTableLib + UefiDriverEntryPoint + UefiLib + +[Protocols] + gEfiMpServiceProtocolGuid + +[Guids] + gArmMpCoreInfoGuid diff --git a/ArmPkg/Library/MpInitLib/DxeMpLib.c b/ArmPkg/Library/MpInitLib= /DxeMpLib.c new file mode 100644 index 000000000000..6053db740624 --- /dev/null +++ b/ArmPkg/Library/MpInitLib/DxeMpLib.c @@ -0,0 +1,1498 @@ +/** @file + Construct MP Services Protocol. + + Copyright (c) 2021, NUVIA Inc. All rights reserved.
+ Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.
+ Portions Copyright (c) 2011, Apple Inc. All rights reserved. + + SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "InternalMpInitLib.h" + +#define POLL_INTERVAL_US 50000 + +STATIC CPU_MP_DATA mCpuMpData; +STATIC BOOLEAN mNonBlockingModeAllowed; +UINT64 *gApStacksBase; +UINT64 *gProcessorIDs; +CONST UINT64 gApStackSize =3D AP_STACK_SIZE; + +/** C entry-point for the AP. + This function gets called from the assembly function ApEntryPoint. + +**/ +VOID +ApProcedure ( + VOID + ) +{ + ARM_SMC_ARGS Args; + EFI_AP_PROCEDURE ApProcedure; + VOID *ApParameter; + UINTN ProcessorIndex; + + MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorIndex); + + /* Fetch the user-supplied procedure and parameter to execute */ + ApProcedure =3D mCpuMpData.CpuData[ProcessorIndex].Procedure; + ApParameter =3D mCpuMpData.CpuData[ProcessorIndex].Parameter; + [SAMI] Is it possible to change the name of the local function pointer vari= able ApProcedure, please? + ApProcedure (ApParameter); + + mCpuMpData.CpuData[ProcessorIndex].State =3D CpuStateFinished; + + /* Since we're finished with this AP, turn it off */ + Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_OFF; + ArmCallSmc (&Args); + + /* Should never be reached */ + ASSERT (FALSE); + CpuDeadLoop (); +} + +/** Turns on the specified core using PSCI and executes the user-supplied + function that's been configured via a previous call to SetApProcedure. + + @param ProcessorIndex The index of the core to turn on. + + @retval EFI_SUCCESS Success. + @retval EFI_DEVICE_ERROR The processor could not be turned on. + +**/ +STATIC +EFI_STATUS +EFIAPI +DispatchCpu ( + IN UINTN ProcessorIndex + ) +{ + ARM_SMC_ARGS Args; + EFI_STATUS Status; + + Status =3D EFI_SUCCESS; + + mCpuMpData.CpuData[ProcessorIndex].State =3D CpuStateBusy; + + /* Turn the AP on */ + if (sizeof (Args.Arg0) =3D=3D sizeof (UINT32)) { + Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_ON_AARCH32; + } else { + Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_ON_AARCH64; + } + Args.Arg1 =3D gProcessorIDs[ProcessorIndex]; + Args.Arg2 =3D (UINTN)ApEntryPoint; + + ArmCallSmc (&Args); + + if (Args.Arg0 !=3D ARM_SMC_PSCI_RET_SUCCESS) { + DEBUG ((DEBUG_ERROR, "PSCI_CPU_ON call failed: %d", Args.Arg0)); + Status =3D EFI_DEVICE_ERROR; + } + + return Status; +} + +/** Returns whether the specified processor is the BSP. + + @param[in] ProcessorIndex The index the processor to check. + + @return TRUE if the processor is the BSP, FALSE otherwise. +**/ +STATIC +BOOLEAN +IsProcessorBSP ( + UINTN ProcessorIndex + ) +{ + EFI_PROCESSOR_INFORMATION *CpuInfo; + + CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info; + + return (CpuInfo->StatusFlag & PROCESSOR_AS_BSP_BIT) !=3D 0; +} + +/** Returns whether the processor executing this function is the BSP. + + @return Whether the current processor is the BSP. +**/ +STATIC +BOOLEAN +IsCurrentProcessorBSP ( + VOID + ) +{ + EFI_STATUS Status; + UINTN ProcessorIndex; + + Status =3D MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorIndex); + ASSERT_EFI_ERROR (Status); [SAMI] I think this assert can be changed to ASSERT (0) and moved inside th= e if condition below. + if (EFI_ERROR (Status)) { + return FALSE; + } + + return IsProcessorBSP (ProcessorIndex); +} + +/** Get the Application Processors state. + + @param[in] CpuData The pointer to CPU_AP_DATA of specified AP. + + @return The AP status. +**/ +CPU_STATE +GetApState ( + IN CPU_AP_DATA *CpuData + ) +{ + return CpuData->State; +} + +/** Configures the processor context with the user-supplied procedure and + argument. + + @param CpuData The processor context. + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + +**/ +STATIC +VOID +SetApProcedure ( + IN CPU_AP_DATA *CpuData, + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument + ) +{ + ASSERT (CpuData !=3D NULL); + ASSERT (Procedure !=3D NULL); + + CpuData->Parameter =3D ProcedureArgument; + CpuData->Procedure =3D Procedure; +} + +/** Returns the index of the next processor that is blocked. + + @param[out] NextNumber The index of the next blocked processor. + + @retval EFI_SUCCESS Successfully found the next blocked processor. + @retval EFI_NOT_FOUND There are no blocked processors. + +**/ +STATIC +EFI_STATUS +GetNextBlockedNumber ( + OUT UINTN *NextNumber + ) +{ + UINTN Index; + CPU_STATE State; + CPU_AP_DATA *CpuData; + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + State =3D CpuData->State; + + if (State =3D=3D CpuStateBlocked) { + *NextNumber =3D Index; + return EFI_SUCCESS; + } + } + + return EFI_NOT_FOUND; +} + +/** Stalls the BSP for the minimum of POLL_INTERVAL_US and Timeout. + + @param[in] Timeout The time limit in microseconds remaining for + APs to return from Procedure. + + @retval StallTime Time of execution stall. +**/ +STATIC +UINTN +CalculateAndStallInterval ( + IN UINTN Timeout + ) +{ + UINTN StallTime; + + if (Timeout < POLL_INTERVAL_US && Timeout !=3D 0) { [SAMI] Please use extra parentheses. see https://edk2-docs.gitbook.io/edk-i= i-c-coding-standards-specification/5_source_files/52_spacing#5.2.2.10-use-e= xtra-parentheses-rather-than-depending-on-in-depth-knowledge-of-the-order-o= f-precedenc Same at other places in this series. [/SAMI] + StallTime =3D Timeout; + } else { + StallTime =3D POLL_INTERVAL_US; + } + + gBS->Stall (StallTime); + + return StallTime; +} + +/** + This service retrieves the number of logical processor in the platform + and the number of those logical processors that are enabled on this boot= . + This service may only be called from the BSP. + + @param[in] This A pointer to the + EFI_MP_SERVICES_PROTOCOL instanc= e. + @param[out] NumberOfProcessors Pointer to the total number of l= ogical + processors in the system, includ= ing + the BSP and disabled APs. + @param[out] NumberOfEnabledProcessors Pointer to the number of enabled + logical processors that exist in= the + system, including the BSP. + + @retval EFI_SUCCESS The number of logical processors and ena= bled + logical processors was retrieved. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER NumberOfProcessors is NULL. + @retval EFI_INVALID_PARAMETER NumberOfEnabledProcessors is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibGetNumberOfProcessors ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *NumberOfProcessors, + OUT UINTN *NumberOfEnabledProcessors + ) +{ + if ((NumberOfProcessors =3D=3D NULL) || (NumberOfEnabledProcessors =3D= =3D NULL)) { + return EFI_INVALID_PARAMETER; + } + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + *NumberOfProcessors =3D mCpuMpData.NumberOfProcessors; + *NumberOfEnabledProcessors =3D mCpuMpData.NumberOfEnabledProcessors; + return EFI_SUCCESS; +} + +/** + Gets detailed MP-related information on the requested processor at the + instant this call is made. This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTO= COL + instance. + @param[in] ProcessorIndex The index of the processor. + @param[out] ProcessorInfoBuffer A pointer to the buffer where informat= ion + for the requested processor is deposit= ed. + + @retval EFI_SUCCESS Processor information was returned. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_INVALID_PARAMETER ProcessorInfoBuffer is NULL. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist in the pl= atform. + +**/ +EFI_STATUS +EFIAPI +MpInitLibGetProcessorInfo ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorIndex, + OUT EFI_PROCESSOR_INFORMATION *ProcessorInfoBuffer + ) +{ + if (ProcessorInfoBuffer =3D=3D NULL) { + return EFI_INVALID_PARAMETER; + } + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + ProcessorIndex &=3D ~CPU_V2_EXTENDED_TOPOLOGY; [SAMI] I don't see this set anywhere in the code. Also the other functions = do not clear this bit before processing. Can you help me to understand this= , please? + + if (ProcessorIndex >=3D mCpuMpData.NumberOfProcessors) { + return EFI_NOT_FOUND; + } + + CopyMem ( + ProcessorInfoBuffer, + &mCpuMpData.CpuData[ProcessorIndex], + sizeof (EFI_PROCESSOR_INFORMATION) + ); + return EFI_SUCCESS; +} + +/** Returns whether the specified processor is enabled. + + @param[in] ProcessorIndex The index of the processor to check. + + @return TRUE if the processor is enabled, FALSE otherwise. +**/ +STATIC +BOOLEAN +IsProcessorEnabled ( + UINTN ProcessorIndex + ) +{ + EFI_PROCESSOR_INFORMATION *CpuInfo; + + CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info; + + return (CpuInfo->StatusFlag & PROCESSOR_ENABLED_BIT) !=3D 0; +} + +/** Returns whether all processors are in the idle state. + + @return Whether all the processors are idle. + +**/ +STATIC +BOOLEAN +CheckAllCpusReady ( + VOID + ) +{ + UINTN Index; + CPU_AP_DATA *CpuData; + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + if (!IsProcessorEnabled (Index)) { + // Skip Disabled processors + continue; + } + + if (GetApState (CpuData) !=3D CpuStateIdle) { + return FALSE; + } + } + + return TRUE; +} + +/** Sets up the state for the StartupAllAPs function. + + @param SingleThread Whether the APs will execute sequentially. + +**/ +STATIC +VOID +StartupAllAPsPrepareState ( + IN BOOLEAN SingleThread + ) +{ + UINTN Index; + CPU_STATE APInitialState; + CPU_AP_DATA *CpuData; + + mCpuMpData.FinishCount =3D 0; + mCpuMpData.StartCount =3D 0; + mCpuMpData.SingleThread =3D SingleThread; + + APInitialState =3D CpuStateReady; + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + + // + // Get APs prepared, and put failing APs into FailedCpuList. + // If "SingleThread", only 1 AP will put into ready state, other AP wi= ll be + // put into ready state 1 by 1, until the previous 1 finished its task= . + // If not "SingleThread", all APs are put into ready state from the + // beginning + // + + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + if (!IsProcessorEnabled (Index)) { + // Skip Disabled processors + if (mCpuMpData.FailedList !=3D NULL) { + mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] =3D Index; + } + + continue; + } + + ASSERT (GetApState (CpuData) =3D=3D CpuStateIdle); + CpuData->State =3D APInitialState; + + mCpuMpData.StartCount++; + if (SingleThread) { + APInitialState =3D CpuStateBlocked; + } + } +} + +/** Handles execution of StartupAllAPs when a WaitEvent has been specified= . + + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + @param WaitEvent The wait event to be signaled when the work is + complete or a timeout has occurred. + @param TimeoutInMicroseconds The timeout for the work to be completed. = Zero + indicates an infinite timeout. + + @return EFI_SUCCESS on success. +**/ +STATIC +EFI_STATUS +StartupAllAPsWithWaitEvent ( + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument, + IN EFI_EVENT WaitEvent, + IN UINTN TimeoutInMicroseconds + ) +{ + EFI_STATUS Status; + UINTN Index; + CPU_AP_DATA *CpuData; + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + if (!IsProcessorEnabled (Index)) { + // Skip Disabled processors + continue; + } + + if (GetApState (CpuData) =3D=3D CpuStateReady) { + SetApProcedure (CpuData, Procedure, ProcedureArgument); + } + } + + // + // Save data into private data structure, and create timer to poll AP st= ate + // before exiting + // + mCpuMpData.Procedure =3D Procedure; + mCpuMpData.ProcedureArgument =3D ProcedureArgument; + mCpuMpData.WaitEvent =3D WaitEvent; + mCpuMpData.Timeout =3D TimeoutInMicroseconds; + mCpuMpData.TimeoutActive =3D (BOOLEAN)(TimeoutInMicroseconds !=3D 0); + Status =3D gBS->SetTimer ( + mCpuMpData.CheckAllAPsEvent, + TimerPeriodic, + POLL_INTERVAL_US + ); + return Status; +} + +/** Handles execution of StartupAllAPs when no wait event has been specifi= ed. + + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + @param TimeoutInMicroseconds The timeout for the work to be completed. = Zero + indicates an infinite timeout. + @param SingleThread Whether the APs will execute sequentially. + @param FailedCpuList User-supplied pointer for list of failed C= PUs. + + @return EFI_SUCCESS on success. +**/ +STATIC +EFI_STATUS +StartupAllAPsNoWaitEvent ( + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument, + IN UINTN TimeoutInMicroseconds, + IN BOOLEAN SingleThread, + IN UINTN **FailedCpuList + ) +{ + EFI_STATUS Status; + UINTN Index; + UINTN NextIndex; + UINTN Timeout; + CPU_AP_DATA *CpuData; + + Timeout =3D TimeoutInMicroseconds; + + while (TRUE) { + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + if (!IsProcessorEnabled (Index)) { + // Skip Disabled processors + continue; + } + + switch (GetApState (CpuData)) { + case CpuStateReady: + SetApProcedure (CpuData, Procedure, ProcedureArgument); + Status =3D DispatchCpu (Index); + if (EFI_ERROR (Status)) { + CpuData->State =3D CpuStateIdle; + Status =3D EFI_NOT_READY; + goto Done; + } + + break; + + case CpuStateFinished: + mCpuMpData.FinishCount++; + if (SingleThread) { + Status =3D GetNextBlockedNumber (&NextIndex); + if (!EFI_ERROR (Status)) { + mCpuMpData.CpuData[NextIndex].State =3D CpuStateReady; + } + } + + CpuData->State =3D CpuStateIdle; + break; + + default: + break; + } + } + + if (mCpuMpData.FinishCount =3D=3D mCpuMpData.StartCount) { + Status =3D EFI_SUCCESS; + goto Done; + } + + if ((TimeoutInMicroseconds !=3D 0) && (Timeout =3D=3D 0)) { + Status =3D EFI_TIMEOUT; + goto Done; + } + + Timeout -=3D CalculateAndStallInterval (Timeout); + } + +Done: + if (FailedCpuList !=3D NULL) { + if (mCpuMpData.FailedListIndex =3D=3D 0) { + FreePool (*FailedCpuList); + *FailedCpuList =3D NULL; + } + } + + return Status; +} + +/** + This service executes a caller provided function on all enabled APs. APs= can + run either simultaneously or one at a time in sequence. This service sup= ports + both blocking and non-blocking requests. The non-blocking requests use E= FI + events so the BSP can detect when the APs have finished. This service ma= y only + be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] SingleThread If TRUE, then all the enabled APs ex= ecute + the function specified by Procedure = one by + one, in ascending order of processor + handle number. If FALSE, then all t= he + enabled APs execute the function spe= cified + by Procedure simultaneously. + @param[in] WaitEvent The event created by the caller with + CreateEvent() service. If it is NUL= L, + then execute in blocking mode. BSP w= aits + until all APs finish or + TimeoutInMicroseconds expires. If i= t's + not NULL, then execute in non-blocki= ng + mode. BSP requests the function spec= ified + by Procedure to be started on all th= e + enabled APs, and go on executing + immediately. If all return from Proc= edure, + or TimeoutInMicroseconds expires, th= is + event is signaled. The BSP can use t= he + CheckEvent() or WaitForEvent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds + for APs to return from Procedure, ei= ther + for blocking or non-blocking mode. Z= ero + means infinity. If the timeout expi= res + before all APs return from Procedure= , then + Procedure on the failed APs is termi= nated. + All enabled APs are available for ne= xt + function assigned by + EFI_MP_SERVICES_PROTOCOL.StartupAllA= Ps() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] FailedCpuList If NULL, this parameter is ignored. + Otherwise, if all APs finish success= fully, + then its content is set to NULL. If = not + all APs finish before timeout expire= s, + then its content is set to address o= f the + buffer holding handle numbers of the + failed APs. + The buffer is allocated by MP Servic= e + Protocol, and it's the caller's + responsibility to free the buffer wi= th + FreePool() service. + In blocking mode, it is ready for + consumption when the call returns. I= n + non-blocking mode, it is ready when + WaitEvent is signaled. The list of f= ailed + CPU is terminated by END_OF_CPU_LIS= T. + + @retval EFI_SUCCESS In blocking mode, all APs have finished = before + the timeout expired. + @retval EFI_SUCCESS In non-blocking mode, function has been + dispatched to all enabled APs. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR Caller processor is AP. + @retval EFI_NOT_STARTED No enabled APs exist in the system. + @retval EFI_NOT_READY Any enabled APs are busy. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + all enabled APs have finished. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibStartupAllAPs ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN BOOLEAN SingleThread, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT UINTN **FailedCpuList OPTIONAL + ) +{ + EFI_STATUS Status; + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + if (mCpuMpData.NumberOfProcessors =3D=3D 1) { + return EFI_NOT_STARTED; + } + + if (Procedure =3D=3D NULL) { + return EFI_INVALID_PARAMETER; + } + + if ((WaitEvent !=3D NULL) && !mNonBlockingModeAllowed) { + return EFI_UNSUPPORTED; + } + + if (!CheckAllCpusReady ()) { + return EFI_NOT_READY; + } + + if (FailedCpuList !=3D NULL) { + mCpuMpData.FailedList =3D AllocatePool ( + (mCpuMpData.NumberOfProcessors + 1) * + sizeof (UINTN) + ); + if (mCpuMpData.FailedList =3D=3D NULL) { + return EFI_OUT_OF_RESOURCES; + } + + SetMemN ( + mCpuMpData.FailedList, + (mCpuMpData.NumberOfProcessors + 1) * + sizeof (UINTN), + END_OF_CPU_LIST + ); + mCpuMpData.FailedListIndex =3D 0; + *FailedCpuList =3D mCpuMpData.FailedList; + } + + StartupAllAPsPrepareState (SingleThread); + + if (WaitEvent !=3D NULL) { + Status =3D StartupAllAPsWithWaitEvent ( + Procedure, + ProcedureArgument, + WaitEvent, + TimeoutInMicroseconds + ); + } else { + Status =3D StartupAllAPsNoWaitEvent ( + Procedure, + ProcedureArgument, + TimeoutInMicroseconds, + SingleThread, + FailedCpuList + ); + } + + return Status; +} + +/** + This service lets the caller get one enabled AP to execute a caller-prov= ided + function. The caller can request the BSP to either wait for the completi= on + of the AP or just proceed with the next task by using the EFI event mech= anism. + See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo= cking + execution support. This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PRO= TOCOL + instance. + @param[in] Procedure A pointer to the function to be run = on + enabled APs of the system. See type + EFI_AP_PROCEDURE. + @param[in] ProcessorNumber The handle number of the AP. The ran= ge is + from 0 to the total number of logica= l + processors minus 1. The total number= of + logical processors can be retrieved = by + EFI_MP_SERVICES_PROTOCOL.GetNumberOf= Processors(). + @param[in] WaitEvent The event created by the caller with= CreateEvent() + service. If it is NULL, then execut= e in + blocking mode. BSP waits until all A= Ps finish + or TimeoutInMicroseconds expires. I= f it's + not NULL, then execute in non-blocki= ng mode. + BSP requests the function specified = by + Procedure to be started on all the e= nabled + APs, and go on executing immediately= . If + all return from Procedure or Timeout= InMicroseconds + expires, this event is signaled. The= BSP + can use the CheckEvent() or WaitForE= vent() + services to check the state of event= . Type + EFI_EVENT is defined in CreateEvent(= ) in + the Unified Extensible Firmware Inte= rface + Specification. + @param[in] TimeoutInMicroseconds Indicates the time limit in microsec= onds for + APs to return from Procedure, either= for + blocking or non-blocking mode. Zero = means + infinity. If the timeout expires be= fore + all APs return from Procedure, then = Procedure + on the failed APs is terminated. All= enabled + APs are available for next function = assigned + by EFI_MP_SERVICES_PROTOCOL.StartupA= llAPs() + or EFI_MP_SERVICES_PROTOCOL.StartupT= hisAP(). + If the timeout expires in blocking m= ode, + BSP returns EFI_TIMEOUT. If the tim= eout + expires in non-blocking mode, WaitEv= ent + is signaled with SignalEvent(). + @param[in] ProcedureArgument The parameter passed into Procedure = for + all APs. + @param[out] Finished If NULL, this parameter is ignored. = In + blocking mode, this parameter is ign= ored. + In non-blocking mode, if AP returns = from + Procedure before the timeout expires= , its + content is set to TRUE. Otherwise, t= he + value is set to FALSE. The caller ca= n + determine if the AP returned from Pr= ocedure + by evaluating this value. + + @retval EFI_SUCCESS In blocking mode, specified AP finished = before + the timeout expires. + @retval EFI_SUCCESS In non-blocking mode, the function has b= een + dispatched to specified AP. + @retval EFI_UNSUPPORTED A non-blocking mode request was made aft= er the + UEFI event EFI_EVENT_GROUP_READY_TO_BOOT= was + signaled. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_TIMEOUT In blocking mode, the timeout expired be= fore + the specified AP has finished. + @retval EFI_NOT_READY The specified AP is busy. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP or dis= abled AP. + @retval EFI_INVALID_PARAMETER Procedure is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibStartupThisAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN EFI_AP_PROCEDURE Procedure, + IN UINTN ProcessorNumber, + IN EFI_EVENT WaitEvent OPTIONAL, + IN UINTN TimeoutInMicroseconds, + IN VOID *ProcedureArgument OPTIONAL, + OUT BOOLEAN *Finished OPTIONAL + ) +{ + EFI_STATUS Status; + UINTN Timeout; + CPU_AP_DATA *CpuData; + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + if (Procedure =3D=3D NULL) { + return EFI_INVALID_PARAMETER; + } + + if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) { + return EFI_NOT_FOUND; + } + + CpuData =3D &mCpuMpData.CpuData[ProcessorNumber]; + + if (IsProcessorBSP (ProcessorNumber)) { + return EFI_INVALID_PARAMETER; + } + + if (!IsProcessorEnabled (ProcessorNumber)) { + return EFI_INVALID_PARAMETER; + } + + if (GetApState (CpuData) !=3D CpuStateIdle) { + return EFI_NOT_READY; + } + + if ((WaitEvent !=3D NULL) && !mNonBlockingModeAllowed) { + return EFI_UNSUPPORTED; + } + + Timeout =3D TimeoutInMicroseconds; + + mCpuMpData.StartCount =3D 1; + mCpuMpData.FinishCount =3D 0; + + SetApProcedure ( + CpuData, + Procedure, + ProcedureArgument + ); + + Status =3D DispatchCpu (ProcessorNumber); + if (EFI_ERROR (Status)) { + CpuData->State =3D CpuStateIdle; + return EFI_NOT_READY; + } + + if (WaitEvent !=3D NULL) { + // Non Blocking + mCpuMpData.WaitEvent =3D WaitEvent; + gBS->SetTimer ( + CpuData->CheckThisAPEvent, + TimerPeriodic, + POLL_INTERVAL_US + ); + return EFI_SUCCESS; + } + + // Blocking + while (TRUE) { + if (GetApState (CpuData) =3D=3D CpuStateFinished) { + CpuData->State =3D CpuStateIdle; + break; + } + + if ((TimeoutInMicroseconds !=3D 0) && (Timeout =3D=3D 0)) { + return EFI_TIMEOUT; + } + + Timeout -=3D CalculateAndStallInterval (Timeout); + } + + return EFI_SUCCESS; +} + +/** + This service switches the requested AP to be the BSP from that point onw= ard. + This service changes the BSP for all purposes. This call can only be + performed by the current BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableOldBSP If TRUE, then the old BSP will be listed as= an + enabled AP. Otherwise, it will be disabled. + + @retval EFI_SUCCESS BSP successfully switched. + @retval EFI_UNSUPPORTED Switching the BSP cannot be completed pr= ior to + this service returning. + @retval EFI_UNSUPPORTED Switching the BSP is not supported. + @retval EFI_SUCCESS The calling processor is an AP. + @retval EFI_NOT_FOUND The processor with the handle specified = by + ProcessorNumber does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the current BS= P or + a disabled AP. + @retval EFI_NOT_READY The specified AP is busy. + +**/ +EFI_STATUS +EFIAPI +MpInitLibSwitchBSP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableOldBSP + ) +{ + UINTN Index; + CPU_AP_DATA *CpuData; + + CpuData =3D &mCpuMpData.CpuData[ProcessorNumber]; + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) { + return EFI_NOT_FOUND; + } + + if (!IsProcessorEnabled (ProcessorNumber)) { + return EFI_INVALID_PARAMETER; + } + + if (IsProcessorBSP (ProcessorNumber)) { + return EFI_INVALID_PARAMETER; + } + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + if (IsProcessorBSP (Index)) { + break; + } + } + + ASSERT (Index !=3D mCpuMpData.NumberOfProcessors); + + if (GetApState (CpuData) !=3D CpuStateIdle) { + return EFI_NOT_READY; + } + + // Skip for now as we need switch a bunch of stack stuff around and it's + // complex. May not be worth it? + return EFI_NOT_READY; [SAMI] Should this function just return EFI_UNSUPPORTED? I don't think any = other processing is needed in this function. +} + +/** + This service lets the caller enable or disable an AP from this point onw= ard. + This service may only be called from the BSP. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[in] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + @param[in] EnableAP Specifies the new state for the processor f= or + enabled, FALSE for disabled. + @param[in] HealthFlag If not NULL, a pointer to a value that spec= ifies + the new health status of the AP. This flag + corresponds to StatusFlag defined in + EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()= . Only + the PROCESSOR_HEALTH_STATUS_BIT is used. Al= l other + bits are ignored. If it is NULL, this para= meter + is ignored. + + @retval EFI_SUCCESS The specified AP was enabled or disabled= successfully. + @retval EFI_UNSUPPORTED Enabling or disabling an AP cannot be co= mpleted + prior to this service returning. + @retval EFI_UNSUPPORTED Enabling or disabling an AP is not suppo= rted. + @retval EFI_DEVICE_ERROR The calling processor is an AP. + @retval EFI_NOT_FOUND Processor with the handle specified by P= rocessorNumber + does not exist. + @retval EFI_INVALID_PARAMETER ProcessorNumber specifies the BSP. + +**/ +EFI_STATUS +EFIAPI +MpInitLibEnableDisableAP ( + IN EFI_MP_SERVICES_PROTOCOL *This, + IN UINTN ProcessorNumber, + IN BOOLEAN EnableAP, + IN UINT32 *HealthFlag OPTIONAL + ) +{ + UINTN StatusFlag; + CPU_AP_DATA *CpuData; + + StatusFlag =3D mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag; + CpuData =3D &mCpuMpData.CpuData[ProcessorNumber]; + + if (!IsCurrentProcessorBSP ()) { + return EFI_DEVICE_ERROR; + } + + if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) { + return EFI_NOT_FOUND; + } + + if (IsProcessorBSP (ProcessorNumber)) { + return EFI_INVALID_PARAMETER; + } + + if (GetApState (CpuData) !=3D CpuStateIdle) { + return EFI_UNSUPPORTED; + } + + if (EnableAP) { + if (!IsProcessorEnabled (ProcessorNumber)) { + mCpuMpData.NumberOfEnabledProcessors++; + } + + StatusFlag |=3D PROCESSOR_ENABLED_BIT; + } else { + if (IsProcessorEnabled (ProcessorNumber)) { + mCpuMpData.NumberOfEnabledProcessors--; + } + + StatusFlag &=3D ~PROCESSOR_ENABLED_BIT; + } + + if (HealthFlag !=3D NULL) { + StatusFlag &=3D ~PROCESSOR_HEALTH_STATUS_BIT; + StatusFlag |=3D (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT); + } + + return EFI_SUCCESS; +} + +/** + This return the handle number for the calling processor. This service m= ay be + called from the BSP and APs. + + @param[in] This A pointer to the EFI_MP_SERVICES_PROTOCOL i= nstance. + @param[out] ProcessorNumber The handle number of AP that is to become t= he new + BSP. The range is from 0 to the total numbe= r of + logical processors minus 1. The total numbe= r of + logical processors can be retrieved by + EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess= ors(). + + @retval EFI_SUCCESS The current processor handle number was = returned + in ProcessorNumber. + @retval EFI_INVALID_PARAMETER ProcessorNumber is NULL. + +**/ +EFI_STATUS +EFIAPI +MpInitLibWhoAmI ( + IN EFI_MP_SERVICES_PROTOCOL *This, + OUT UINTN *ProcessorNumber + ) +{ + UINTN Index; + UINT64 ProcessorId; + CPU_AP_DATA *CpuData; + + if (ProcessorNumber =3D=3D NULL) { + return EFI_INVALID_PARAMETER; + } + + ProcessorId =3D ArmReadMpidr (); [SAMI] Here again, I think it will not work on PEs with the MT bit[24] set.= I think it would be better to mask of all other bits of MPIDR and only kee= p use the affinity bits. CpuData->Info.ProcessorId should only store the a= ffinity bits. This would also match the requirement by the PSCI specification version D.b= (https://developer.arm.com/documentation/den0022/latest/). See section 5.1= .4 CPU_ON If the calling Exception level is using AArch64, the format is: =E2=80=A2 Bits[40:63]: Must be zero =E2=80=A2 Bits[32:39] Aff3: Match Aff3 of target core MPIDR =E2=80=A2 Bits[24:31] Must be zero =E2=80=A2 Bits[16:23] Aff2: Match Aff2 of target core MPIDR =E2=80=A2 Bits[8:15] Aff1: Match Aff1 of target core MPIDR =E2=80=A2 Bits[0:7] Aff0: Match Aff0 of target core MPIDR [/SAMI] + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; + if (CpuData->Info.ProcessorId =3D=3D ProcessorId) { + break; + } + } + + *ProcessorNumber =3D Index; + return EFI_SUCCESS; +} + +/** Adds the specified processor the list of failed processors. + + @param ProcessorIndex The processor index to add. + @param ApState Processor state. + +**/ +STATIC +VOID +AddProcessorToFailedList ( + UINTN ProcessorIndex, + CPU_STATE ApState + ) +{ + UINTN Index; + BOOLEAN Found; + + Found =3D FALSE; + + if (ApState =3D=3D CpuStateIdle) { + return; + } + + // If we are retrying make sure we don't double count + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { [SAMI] Can the for loop run from 0 to mCpuMpData.FailedListIndex? In that c= ase we would not need to check against END_OF_CPU_LIST right? + if (mCpuMpData.FailedList[Index] =3D=3D END_OF_CPU_LIST) { + break; + } + + if (mCpuMpData.FailedList[ProcessorIndex] =3D=3D Index) { [SAMI] Should this check be (mCpuMpData.FailedList[Index] =3D=3D ProcessorI= ndex) ? + Found =3D TRUE; + break; + } + } + + /* If the CPU isn't already in the FailedList, add it */ + if (!Found) { + mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] =3D Index; + } +} + +/** Handles the StartupAllAPs case where the timeout has occurred. + +**/ +STATIC +VOID +ProcessStartupAllAPsTimeout ( + VOID + ) +{ + CPU_AP_DATA *CpuData; + UINTN Index; + + if (mCpuMpData.FailedList =3D=3D NULL) { + return; + } + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + CpuData =3D &mCpuMpData.CpuData[Index]; [SAMI] Minor optimisation could be achieved if CpuData initialisation is do= ne just before calling AddProcessorToFailedList(). + if (IsProcessorBSP (Index)) { + // Skip BSP + continue; + } + + if (!IsProcessorEnabled (Index)) { + // Skip Disabled processors + continue; + } + + AddProcessorToFailedList (Index, GetApState (CpuData)); + } +} + +/** Updates the status of the APs. + + @param[in] ProcessorIndex The index of the AP to update. +**/ +STATIC +VOID +UpdateApStatus ( + IN UINTN ProcessorIndex + ) +{ + EFI_STATUS Status; + CPU_AP_DATA *CpuData; + CPU_AP_DATA *NextCpuData; + CPU_STATE State; + UINTN NextNumber; + + CpuData =3D &mCpuMpData.CpuData[ProcessorIndex]; + + if (IsProcessorBSP (ProcessorIndex)) { + // Skip BSP + return; + } + + if (!IsProcessorEnabled (ProcessorIndex)) { + // Skip Disabled processors + return; + } + + State =3D GetApState (CpuData); + + switch (State) { + case CpuStateFinished: + if (mCpuMpData.SingleThread) { + Status =3D GetNextBlockedNumber (&NextNumber); + if (!EFI_ERROR (Status)) { + NextCpuData =3D &mCpuMpData.CpuData[NextNumber]; + + NextCpuData->State =3D CpuStateReady; + + SetApProcedure ( + NextCpuData, + mCpuMpData.Procedure, + mCpuMpData.ProcedureArgument + ); + } + } + + CpuData->State =3D CpuStateIdle; + mCpuMpData.FinishCount++; + break; + + default: + break; + } +} + +/** + If a timeout is specified in StartupAllAps(), a timer is set, which invo= kes + this procedure periodically to check whether all APs have finished. + + @param[in] Event The WaitEvent the user supplied. + @param[in] Context The event context. +**/ +STATIC +VOID +EFIAPI +CheckAllAPsStatus ( + IN EFI_EVENT Event, + IN VOID *Context + ) +{ + UINTN Index; + + if (mCpuMpData.TimeoutActive) { + mCpuMpData.Timeout -=3D CalculateAndStallInterval (mCpuMpData.Timeout)= ; + } + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + UpdateApStatus (Index); + } + + if (mCpuMpData.TimeoutActive && mCpuMpData.Timeout =3D=3D 0) { + ProcessStartupAllAPsTimeout (); + + // Force terminal exit + mCpuMpData.FinishCount =3D mCpuMpData.StartCount; + } + + if (mCpuMpData.FinishCount !=3D mCpuMpData.StartCount) { + return; + } + + gBS->SetTimer ( + mCpuMpData.CheckAllAPsEvent, + TimerCancel, + 0 + ); + + if (mCpuMpData.FailedListIndex =3D=3D 0) { + if (mCpuMpData.FailedList !=3D NULL) { + FreePool (mCpuMpData.FailedList); + mCpuMpData.FailedList =3D NULL; + } + } + + gBS->SignalEvent (mCpuMpData.WaitEvent); +} + +/** Invoked periodically via a timer to check the state of the processor. + + @param Event The event supplied by the timer expiration. + @param Context The processor context. + +**/ +STATIC +VOID +EFIAPI +CheckThisAPStatus ( + IN EFI_EVENT Event, + IN VOID *Context + ) +{ + EFI_STATUS Status; + CPU_AP_DATA *CpuData; + CPU_STATE State; + + CpuData =3D Context; + CpuData->TimeTaken +=3D POLL_INTERVAL_US; + + State =3D GetApState (CpuData); + + if (State =3D=3D CpuStateFinished) { + Status =3D gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel, 0); + ASSERT_EFI_ERROR (Status); + + if (mCpuMpData.WaitEvent !=3D NULL) { + Status =3D gBS->SignalEvent (mCpuMpData.WaitEvent); + ASSERT_EFI_ERROR (Status); + } + + CpuData->State =3D CpuStateIdle; + } + + if (CpuData->TimeTaken > CpuData->Timeout) { + if (mCpuMpData.WaitEvent !=3D NULL) { + Status =3D gBS->SignalEvent (mCpuMpData.WaitEvent); + ASSERT_EFI_ERROR (Status); + } + } +} + +/** + This function is called by all processors (both BSP and AP) once and col= lects + MP related data. + + @param BSP TRUE if the processor is the BSP. + @param Mpidr The MPIDR for the specified processor. + @param ProcessorIndex The index of the processor. + + @return EFI_SUCCESS if the data for the processor collected and filled i= n. + +**/ +STATIC +EFI_STATUS +FillInProcessorInformation ( + IN BOOLEAN BSP, + IN UINTN Mpidr, + IN UINTN ProcessorIndex + ) +{ + EFI_PROCESSOR_INFORMATION *CpuInfo; + + CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info; + + /* The MPIDR passed in may be a pseudo-MPIDR that's missing the bit 31 R= ES1. + * Fix it so it's a proper MPIDR. + */ + CpuInfo->ProcessorId =3D BIT31 | Mpidr; [SAMI] I believe the MT, bit [24] may need to be set as well. Otherwise, th= e checks in MpInitLibWhoAm() would fail as ArmReadMpidr() would return this= bit as set if the PE is implemented using a multithreading type approach. + CpuInfo->StatusFlag =3D PROCESSOR_ENABLED_BIT | PROCESSOR_HEALTH_STATUS= _BIT; + + if (BSP) { + CpuInfo->StatusFlag |=3D PROCESSOR_AS_BSP_BIT; + } + + CpuInfo->Location.Package =3D GET_CLUSTER_ID (Mpidr); + CpuInfo->Location.Core =3D GET_CORE_ID (Mpidr); + CpuInfo->Location.Thread =3D 0; [SAMI] I think the MPIDR_EL1.MT bit should be used to decide if Multi Threa= ding is supported and value for CpuInfo->Location.Thread should be set appr= opriately. + + CpuInfo->ExtendedInformation.Location2.Package =3D 0; + CpuInfo->ExtendedInformation.Location2.Module =3D 0; + CpuInfo->ExtendedInformation.Location2.Tile =3D 0; + CpuInfo->ExtendedInformation.Location2.Die =3D GET_CLUSTER_ID (Mpidr= ); + CpuInfo->ExtendedInformation.Location2.Core =3D GET_CORE_ID (Mpidr); + CpuInfo->ExtendedInformation.Location2.Thread =3D 0; + + mCpuMpData.CpuData[ProcessorIndex].State =3D BSP ? CpuStateBusy : CpuSta= teIdle; + + mCpuMpData.CpuData[ProcessorIndex].Procedure =3D NULL; + mCpuMpData.CpuData[ProcessorIndex].Parameter =3D NULL; + + return EFI_SUCCESS; +} + +/** Initializes the MP Services system data + + @param NumberOfProcessors The number of processors, both BSP and AP. + @param CpuInfo CPU information gathered earlier during boot. + +**/ +VOID +MpInitLibInitialize ( + IN UINTN NumberOfProcessors, + IN ARM_PROCESSOR_TABLE *CpuInfo + ) +{ + EFI_STATUS Status; + UINTN Index; + EFI_EVENT ReadyToBootEvent; + + // + // Clear the data structure area first. + // + ZeroMem (&mCpuMpData, sizeof (CPU_MP_DATA)); + // + // First BSP fills and inits all known values, including its own records= . + // + mCpuMpData.NumberOfProcessors =3D NumberOfProcessors; + mCpuMpData.NumberOfEnabledProcessors =3D NumberOfProcessors; + + mCpuMpData.CpuData =3D AllocateZeroPool ( + mCpuMpData.NumberOfProcessors * + sizeof (CPU_AP_DATA) + ); + ASSERT (mCpuMpData.CpuData !=3D NULL); + + /* Allocate one extra for the NULL entry at the end */ + gProcessorIDs =3D AllocatePool ((mCpuMpData.NumberOfProcessors + 1) * si= zeof (UINT64)); + ASSERT (gProcessorIDs !=3D NULL); + + FillInProcessorInformation (TRUE, CpuInfo->ArmCpus[0].Mpidr, 0); + gProcessorIDs[0] =3D mCpuMpData.CpuData[0].Info.ProcessorId; + + Status =3D gBS->CreateEvent ( + EVT_TIMER | EVT_NOTIFY_SIGNAL, + TPL_CALLBACK, + CheckAllAPsStatus, + NULL, + &mCpuMpData.CheckAllAPsEvent + ); + ASSERT_EFI_ERROR (Status); + + gApStacksBase =3D AllocatePool ( + mCpuMpData.NumberOfProcessors * + gApStackSize + ); + ASSERT (gApStacksBase !=3D NULL); + + for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) { + if (IsProcessorBSP (Index)) { + /* Skip BSP */ + continue; + } + + FillInProcessorInformation (FALSE, CpuInfo->ArmCpus[Index].Mpidr, Inde= x); + + gProcessorIDs[Index] =3D mCpuMpData.CpuData[Index].Info.ProcessorId; + + Status =3D gBS->CreateEvent ( + EVT_TIMER | EVT_NOTIFY_SIGNAL, + TPL_CALLBACK, + CheckThisAPStatus, + (VOID *)&mCpuMpData.CpuData[Index], + &mCpuMpData.CpuData[Index].CheckThisAPEvent + ); + ASSERT (Status =3D=3D EFI_SUCCESS); + } + + Status =3D EfiCreateEventReadyToBootEx ( + TPL_CALLBACK, + ReadyToBootSignaled, + NULL, + &ReadyToBootEvent + ); + ASSERT_EFI_ERROR (Status); + + gProcessorIDs[Index] =3D 0; +} + +/** + Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOO= T is + signaled. After this point, non-blocking mode is no longer allowed. + + @param Event Event whose notification function is being invoked. + @param Context The pointer to the notification function's context, + which is implementation-dependent. + +**/ +STATIC +VOID +EFIAPI +ReadyToBootSignaled ( + IN EFI_EVENT Event, + IN VOID *Context + ) +{ + mNonBlockingModeAllowed =3D FALSE; +} + diff --git a/ArmPkg/Library/MpInitLib/InternalMpInitLib.h b/ArmPkg/Library/= MpInitLib/InternalMpInitLib.h new file mode 100644 index 000000000000..d08bb76246d7 --- /dev/null +++ b/ArmPkg/Library/MpInitLib/InternalMpInitLib.h @@ -0,0 +1,358 @@ +/** @file + +Copyright (c) 2021, NUVIA Inc. All rights reserved.
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.
+Portions copyright (c) 2011, Apple Inc. All rights reserved. + +SPDX-License-Identifier: BSD-2-Clause-Patent + +**/ + +#ifndef MP_INTERNAL_INIT_LIB_H_ +#define MP_INTERNAL_INIT_LIB_H_ + +#include +#include + +#include +#include + + +#define AP_STACK_SIZE 0x1000 + +// +// Internal Data Structures +// + +// +// AP state +// +// The state transitions for an AP when it process a procedure are: +// Idle ----> Ready ----> Busy ----> Idle +// [BSP] [AP] [AP] +// +typedef enum { + CpuStateIdle, + CpuStateReady, + CpuStateBlocked, + CpuStateBusy, + CpuStateFinished, + CpuStateDisabled +} CPU_STATE; + +// +// Define Individual Processor Data block. +// +typedef struct { + EFI_PROCESSOR_INFORMATION Info; + EFI_AP_PROCEDURE Procedure; + VOID *Parameter; + CPU_STATE State; + EFI_EVENT CheckThisAPEvent; + UINTN Timeout; + UINTN TimeTaken; +} CPU_AP_DATA; + +// +// Define MP data block which consumes individual processor block. +// +typedef struct { + UINTN NumberOfProcessors; + UINTN NumberOfEnabledProcessors; + EFI_EVENT CheckAllAPsEvent; + EFI_EVENT WaitEvent; + UINTN FinishCount; + UINTN StartCount; + EFI_AP_PROCEDURE Procedure; + VOID *ProcedureArgument; + BOOLEAN SingleThread; + UINTN StartedNumber; + CPU_AP_DATA *CpuData; + UINTN Timeout; + UINTN *FailedList; + UINTN FailedListIndex; + BOOLEAN TimeoutActive; +} CPU_MP_DATA; + +EFI_STATUS +EFIAPI +CpuMpServicesInit ( + IN EFI_HANDLE ImageHandle, + IN EFI_SYSTEM_TABLE *SystemTable + ); + +extern EFI_MP_SERVICES_PROTOCOL mMpServicesTemplate; + +/** Secondary core entry point. + +**/ +VOID ApEntryPoint ( + VOID + ); + +/** C entry-point for the AP. + This function gets called from the assembly function ApEntryPoint. +**/ +VOID +ApProcedure ( + VOID + ); + +/** Turns on the specified core using PSCI and executes the user-supplied + function that's been configured via a previous call to SetApProcedure. + + @param ProcessorIndex The index of the core to turn on. + + @retval EFI_SUCCESS The processor was successfully turned on. + @retval EFI_DEVICE_ERROR An error occurred turning the processor on. + +**/ +STATIC +EFI_STATUS +EFIAPI +DispatchCpu ( + IN UINTN ProcessorIndex + ); + +/** Returns whether the specified processor is the BSP. + + @param[in] ProcessorIndex The index the processor to check. + + @return TRUE if the processor is the BSP, FALSE otherwise. +**/ +STATIC +BOOLEAN +IsProcessorBSP ( + UINTN ProcessorIndex + ); + +/** Returns whether the processor executing this function is the BSP. + + @return Whether the current processor is the BSP. +**/ +STATIC +BOOLEAN +IsCurrentProcessorBSP ( + VOID + ); + +/** Returns whether the specified processor is enabled. + + @param[in] ProcessorIndex The index of the processor to check. + + @return TRUE if the processor is enabled, FALSE otherwise. +**/ +STATIC +BOOLEAN +IsProcessorEnabled ( + UINTN ProcessorIndex + ); + +/** Configures the processor context with the user-supplied procedure and + argument. + + @param CpuData The processor context. + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + +**/ +STATIC +VOID +SetApProcedure ( + IN CPU_AP_DATA *CpuData, + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument + ); + +/** + Get the Application Processors state. + + @param[in] CpuData The pointer to CPU_AP_DATA of specified AP + + @return The AP status +**/ +CPU_STATE +GetApState ( + IN CPU_AP_DATA *CpuData + ); + +/** Returns the index of the next processor that is blocked. + + @param[out] NextNumber The index of the next blocked processor. + + @retval EFI_SUCCESS Successfully found the next blocked processor. + @retval EFI_NOT_FOUND There are no blocked processors. + +**/ +STATIC +EFI_STATUS +GetNextBlockedNumber ( + OUT UINTN *NextNumber + ); + +/** Stalls the BSP for the minimum of gPollInterval and Timeout. + + @param[in] Timeout The time limit in microseconds remaining for + APs to return from Procedure. + + @retval StallTime Time of execution stall. +**/ +STATIC +UINTN +CalculateAndStallInterval ( + IN UINTN Timeout + ); + +/** Returns whether all processors are in the idle state. + + @return Whether all the processors are idle. + +**/ +STATIC +BOOLEAN +CheckAllCpusReady ( + VOID + ); + +/** Sets up the state for the StartupAllAPs function. + + @param SingleThread Whether the APs will execute sequentially. + +**/ +STATIC +VOID +StartupAllAPsPrepareState ( + IN BOOLEAN SingleThread + ); + +/** Handles execution of StartupAllAPs when a WaitEvent has been specified= . + + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + @param WaitEvent The wait event to be signaled when the work is + complete or a timeout has occurred. + @param TimeoutInMicroseconds The timeout for the work to be completed. = Zero + indicates an infinite timeout. + + @return EFI_SUCCESS on success. +**/ +STATIC +EFI_STATUS +StartupAllAPsWithWaitEvent ( + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument, + IN EFI_EVENT WaitEvent, + IN UINTN TimeoutInMicroseconds + ); + +/** Handles execution of StartupAllAPs when no wait event has been specifi= ed. + + @param Procedure The user-supplied procedure. + @param ProcedureArgument The user-supplied procedure argument. + @param TimeoutInMicroseconds The timeout for the work to be completed. = Zero + indicates an infinite timeout. + @param SingleThread Whether the APs will execute sequentially. + @param FailedCpuList User-supplied pointer for list of failed C= PUs. + + @return EFI_SUCCESS on success. +**/ +STATIC +EFI_STATUS +StartupAllAPsNoWaitEvent ( + IN EFI_AP_PROCEDURE Procedure, + IN VOID *ProcedureArgument, + IN UINTN TimeoutInMicroseconds, + IN BOOLEAN SingleThread, + IN UINTN **FailedCpuList + ); + + +/** Adds the specified processor the list of failed processors. + + @param ProcessorIndex The processor index to add. + @param ApState Processor state. + +**/ +STATIC +VOID +AddProcessorToFailedList ( + UINTN ProcessorIndex, + CPU_STATE ApState + ); + +/** Handles the StartupAllAPs case where the timeout has occurred. + +**/ +STATIC +VOID +ProcessStartupAllAPsTimeout ( + VOID + ); + +/** + If a timeout is specified in StartupAllAps(), a timer is set, which invo= kes + this procedure periodically to check whether all APs have finished. + + @param[in] Event The WaitEvent the user supplied. + @param[in] Context The event context. +**/ +STATIC +VOID +EFIAPI +CheckAllAPsStatus ( + IN EFI_EVENT Event, + IN VOID *Context + ); + +/** Invoked periodically via a timer to check the state of the processor. + + @param Event The event supplied by the timer expiration. + @param Context The processor context. + +**/ +STATIC +VOID +EFIAPI +CheckThisAPStatus ( + IN EFI_EVENT Event, + IN VOID *Context + ); + +/** + This function is called by all processors (both BSP and AP) once and col= lects + MP related data. + + @param BSP TRUE if the processor is the BSP. + @param Mpidr The MPIDR for the specified processor. + @param ProcessorIndex The index of the processor. + + @return EFI_SUCCESS if the data for the processor collected and filled i= n. + +**/ +STATIC +EFI_STATUS +FillInProcessorInformation ( + IN BOOLEAN BSP, + IN UINTN Mpidr, + IN UINTN ProcessorIndex + ); + +/** + Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOO= T is + signaled. After this point, non-blocking mode is no longer allowed. + + @param Event Event whose notification function is being invoked. + @param Context The pointer to the notification function's context, + which is implementation-dependent. + +**/ +STATIC +VOID +EFIAPI +ReadyToBootSignaled ( + IN EFI_EVENT Event, + IN VOID *Context + ); + + +#endif /* MP_INTERNAL_INIT_LIB_H_ */ diff --git a/ArmVirtPkg/ArmVirt.dsc.inc b/ArmVirtPkg/ArmVirt.dsc.inc index 5a1598d90ca7..af6e48fd6cfc 100644 --- a/ArmVirtPkg/ArmVirt.dsc.inc +++ b/ArmVirtPkg/ArmVirt.dsc.inc @@ -261,6 +261,9 @@ [LibraryClasses.ARM] ArmSoftFloatLib|ArmPkg/Library/ArmSoftFloatLib/ArmSoftFloatLib.inf +[LibraryClasses.AARCH64] + MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf + [BuildOptions] RVCT:RELEASE_*_*_CC_FLAGS =3D -DMDEPKG_NDEBUG IMPORTANT NOTICE: The contents of this email and any attachments are confid= ential and may also be privileged. If you are not the intended recipient, p= lease notify the sender immediately and do not disclose the contents to any= other person, use it for any purpose, or store or copy the information in = any medium. Thank you. --------------F6A6CAB149BAC3BDAA6CE911 Content-Type: text/html; charset=utf-8 Content-Transfer-Encoding: quoted-printable

Hi Rebecca,

Thanks you for this patch.

I think this patch could be split in 2 - 3 patches (CpuDxe  & M= pInitLib and the ArmVirtPkg change should be a separate patch).

Please find my feedback inline marked [SAMI].

I think the usage of MPIDR and CpuInfo->ProcessorId needs to be revis= ited in this patch series. I have kept my feedback on usage of MPIDR and th= e MT bit [24] to give the context. But, it will be good to only retain the = affinity bits and mask all other bits of the MPIDR.

Please let me know if you have any queries.

Regards,

Sami Mujawar


On 18/10/2021 04:39 PM, Rebecca Cran wrote:<= br> 
Add support for EFI_MP_SERVICES_PROTOCOL during the DXE phas=
e under
AArch64.

PSCI_CPU_ON is called to power on the core, the supplied procedure is
executed and PSCI_CPU_OFF is called to power off the core.

Minimal setup is done before calling the supplied procedure: for example
the MMU and caches are not enabled.

Signed-off-by: Rebecca Cran <rebecca@nuviainc.com>
---
 ArmPkg/ArmPkg.dec                            |    4 +
 ArmPkg/ArmPkg.dsc                            |    4 +
 ArmPkg/Drivers/CpuDxe/AArch64/Arch.c         |   22 +
 ArmPkg/Drivers/CpuDxe/Arm/Arch.c             |   22 +
 ArmPkg/Drivers/CpuDxe/CpuDxe.c               |    2 +
 ArmPkg/Drivers/CpuDxe/CpuDxe.h               |   10 +
 ArmPkg/Drivers/CpuDxe/CpuDxe.inf             |    6 +
 ArmPkg/Drivers/CpuDxe/CpuMpInit.c            |  604 ++++++++
 ArmPkg/Include/Library/MpInitLib.h           |  366 +++++
 ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S   |   61 +
 ArmPkg/Library/MpInitLib/DxeMpInitLib.inf    |   53 +
 ArmPkg/Library/MpInitLib/DxeMpLib.c          | 1498 ++++++++++++++++++++
 ArmPkg/Library/MpInitLib/InternalMpInitLib.h |  358 +++++
 ArmVirtPkg/ArmVirt.dsc.inc                   |    3 +
 14 files changed, 3013 insertions(+)

diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec
index 5935663fa9a3..f8bb4af21749 100644
--- a/ArmPkg/ArmPkg.dec
+++ b/ArmPkg/ArmPkg.dec
@@ -74,6 +74,10 @@
   #
   DefaultExceptionHandlerLib|Include/Library/DefaultExceptionHandlerLib.h
=20
+  ##  @libraryclass  Provides a MP Services interface.
+  #
+  MpInitLib|Include/Library/MpInitLib.h
+
   ##  @libraryclass  Provides an interface to query miscellaneous OEM
   #   information.
   #
diff --git a/ArmPkg/ArmPkg.dsc b/ArmPkg/ArmPkg.dsc
index 8abe3713c829..26fb8bb94c07 100644
--- a/ArmPkg/ArmPkg.dsc
+++ b/ArmPkg/ArmPkg.dsc
@@ -99,6 +99,9 @@
   PeiServicesLib|MdePkg/Library/PeiServicesLib/PeiServicesLib.inf
   PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLib/Pei=
ServicesTablePointerLib.inf
=20
+[LibraryClasses.AARCH64]
+  MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
+
 [LibraryClasses.ARM, LibraryClasses.AARCH64]
   NULL|ArmPkg/Library/CompilerIntrinsicsLib/CompilerIntrinsicsLib.inf
=20
@@ -163,4 +166,5 @@
   ArmPkg/Library/ArmMmuLib/ArmMmuPeiLib.inf
=20
 [Components.AARCH64, Components.ARM]
+  ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
[SAMI] Should this be in the [Components.AARCH64] section, as DxeMpInitLib.= inf currently supports AARCH64 only?
   ArmPkg/Library/StandaloneMmMmuLib/ArmMmuStandaloneMmLib.i=
nf
diff --git a/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c b/ArmPkg/Drivers/CpuDxe/A=
Arch64/Arch.c
new file mode 100644
index 000000000000..a4b9e9037100
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/AArch64/Arch.c
@@ -0,0 +1,22 @@
+/** @file
+  Architecture specific functions.
+
+  Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+
+#include <CpuDxe.h>
+
+/** Initializes multi-processor support.
+ *
+**/
+VOID
+ArchInitializeMpSupport (
+  VOID
+  )
+{
+  InitializeMpSupport ();
+}
diff --git a/ArmPkg/Drivers/CpuDxe/Arm/Arch.c b/ArmPkg/Drivers/CpuDxe/Arm/A=
rch.c
new file mode 100644
index 000000000000..0ded264cd06a
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/Arm/Arch.c
@@ -0,0 +1,22 @@
+/** @file
+  Architecture specific functions.
+
+  Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+
+#include <CpuDxe.h>
+
+/** Initializes multi-processor support.
+ *
+**/
+VOID
+ArchInitializeMpSupport (
+  VOID
+  )
+{
+  /* Nothing to do - ARM doesn't support EFI_MP_SERVICES_PROTOCOL */
+}
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.c b/ArmPkg/Drivers/CpuDxe/CpuDxe.=
c
index 082ef30fb6c4..db8752f5b54f 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.c
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.c
@@ -279,5 +279,7 @@ CpuDxeInitialize (
                   );
   ASSERT_EFI_ERROR (Status);
=20
+  ArchInitializeMpSupport ();
+
   return Status;
 }
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.h b/ArmPkg/Drivers/CpuDxe/CpuDxe.=
h
index 4cf3ab258c24..6e7ceafa4436 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.h
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.h
@@ -143,4 +143,14 @@ SetGcdMemorySpaceAttributes (
   IN UINT64                              Attributes
   );
=20
+VOID
+InitializeMpSupport (
+  VOID
+  );
+
+VOID
+ArchInitializeMpSupport (
+  VOID
+  );
+
 #endif // CPU_DXE_H_
diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf b/ArmPkg/Drivers/CpuDxe/CpuDx=
e.inf
index e5549fc71df7..f4cdb8ab5613 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
@@ -26,10 +26,13 @@
   Exception.c
=20
 [Sources.ARM]
+  Arm/Arch.c
   Arm/Mmu.c
=20
 [Sources.AARCH64]
+  AArch64/Arch.c
   AArch64/Mmu.c
+  CpuMpInit.c
=20
 [Packages]
   ArmPkg/ArmPkg.dec
@@ -37,6 +40,9 @@
   MdePkg/MdePkg.dec
   MdeModulePkg/MdeModulePkg.dec
=20
+[LibraryClasses.AARCH64]
+  MpInitLib
+
 [LibraryClasses]
   ArmLib
   ArmMmuLib
diff --git a/ArmPkg/Drivers/CpuDxe/CpuMpInit.c b/ArmPkg/Drivers/CpuDxe/CpuM=
pInit.c
new file mode 100644
index 000000000000..5bf4ed12b711
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/CpuMpInit.c
@@ -0,0 +1,604 @@
+/** @file
+  Construct MP Services Protocol.
+
+  The MP Services Protocol provides a generalized way of performing follow=
ing tasks:
+    - Retrieving information of multi-processor environment and MP-related=
 status of
+      specific processors.
+    - Dispatching user-provided function to APs.
+    - Maintain MP-related processor status.
+
+  The MP Services Protocol must be produced on any system with more than o=
ne logical
+  processor.
+
+  The Protocol is available only during boot time.
+
+  MP Services Protocol is hardware-independent. Most of the logic of this =
protocol
+  is architecturally neutral. It abstracts the multi-processor environment=
 and
+  status of processors, and provides interfaces to retrieve information, m=
aintain,
+  and dispatch.
+
+  MP Services Protocol may be consumed by ACPI module. The ACPI module may=
 use this
+  protocol to retrieve data that are needed for an MP platform and report =
them to OS.
+  MP Services Protocol may also be used to program and configure processor=
s, such
+  as MTRR synchronization for memory space attributes setting in DXE Servi=
ces.
+  MP Services Protocol may be used by non-CPU DXE drivers to speed up plat=
form boot
+  by taking advantage of the processing capabilities of the APs, for examp=
le, using
+  APs to help test system memory in parallel with other device initializat=
ion.
+  Diagnostics applications may also use this protocol for multi-processor.
+
+  Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/MpInitLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+/**
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot=
.
+  This service may only be called from the BSP.
+
+  This function is used to retrieve the following information:
+    - The number of logical processors that are present in the system.
+    - The number of enabled logical processors in the system at the instan=
t
+      this call is made.
+
+  Because MP Service Protocol provides services to enable and disable proc=
essors
+  dynamically, the number of enabled logical processors may vary during th=
e
+  course of a boot session.
+
+  If this service is called from an AP, then EFI_DEVICE_ERROR is returned.
+  If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then
+  EFI_INVALID_PARAMETER is returned. Otherwise, the total number of proces=
sors
+  is returned in NumberOfProcessors, the number of currently enabled proce=
ssor
+  is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned.
+
+  @param[in]  This                        A pointer to the
+                                          EFI_MP_SERVICES_PROTOCOL instanc=
e.
+  @param[out] NumberOfProcessors          Pointer to the total number of l=
ogical
+                                          processors in the system, includ=
ing
+                                          the BSP and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled
+                                          logical processors that exist in=
 the
+                                          system, including the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and ena=
bled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+GetNumberOfProcessors (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                     *NumberOfProcessors,
+  OUT UINTN                     *NumberOfEnabledProcessors
+  )
+{
+  return MpInitLibGetNumberOfProcessors (
+    This,
+    NumberOfProcessors,
+    NumberOfEnabledProcessors
+    );
[SAMI] Please adjust the code alignment.
+}
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  This service retrieves detailed MP-related information about any process=
or
+  on the platform. Note the following:
+    - The processor information may change during the course of a boot ses=
sion.
+    - The information presented here is entirely MP related.
+
+  Information regarding the number of caches and their sizes, frequency of
+  operation, slot numbers is all considered platform-related information a=
nd is
+  not provided by this service.
+
+  @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTO=
COL
+                                    instance.
+  @param[in]  ProcessorNumber       The index of the processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where informat=
ion
+                                    for the requested processor is deposit=
ed.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist in the pl=
atform.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+GetProcessorInfo (
+  IN  EFI_MP_SERVICES_PROTOCOL   *This,
+  IN  UINTN                      ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  )
+{
+  return MpInitLibGetProcessorInfo (
+           This,
+           ProcessorNumber,
+           ProcessorInfoBuffer
+           );
+}
+
+/**
+  This service executes a caller provided function on all enabled APs. APs=
 can
+  run either simultaneously or one at a time in sequence. This service sup=
ports
+  both blocking and non-blocking requests. The non-blocking requests use E=
FI
+  events so the BSP can detect when the APs have finished. This service ma=
y only
+  be called from the BSP.
+
+  This function is used to dispatch all the enabled APs to the function
+  specified by Procedure.  If any enabled AP is busy, then EFI_NOT_READY i=
s
+  returned immediately and Procedure is not started on any AP.
+
+  If SingleThread is TRUE, all the enabled APs execute the function specif=
ied by
+  Procedure one by one, in ascending order of processor handle number.
+  Otherwise, all the enabled APs execute the function specified by Procedu=
re
+  simultaneously.
+
+  If WaitEvent is NULL, execution is in blocking mode. The BSP waits until=
 all
+  APs finish or TimeoutInMicroseconds expires. Otherwise, execution is in
+  non-blocking mode, and the BSP returns from this service without waiting=
 for
+  APs. If a non-blocking mode is requested after the UEFI Event
+  EFI_EVENT_GROUP_READY_TO_BOOT is signaled, then EFI_UNSUPPORTED must be
+  returned.
+
+  If the timeout specified by TimeoutInMicroseconds expires before all APs
+  return from Procedure, then Procedure on the failed APs is terminated.
+  All enabled APs are always available for further calls to
+  EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() and
+  EFI_MP_SERVICES_PROTOCOL.StartupThisAP(). If FailedCpuList is not NULL, =
its
+  content points to the list of processor handle numbers in which Procedur=
e was
+  terminated.
+
+  Note: It is the responsibility of the consumer of the
+  EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() to make sure that the nature of=
 the
+  code that is executed on the BSP and the dispatched APs is well controll=
ed.
+  The MP Services Protocol does not guarantee that the Procedure function =
is
+  MP-safe. Hence, the tasks that can be run in parallel are limited to cer=
tain
+  independent tasks and well-controlled exclusive code. EFI services and
+  protocols may not be called by APs unless otherwise specified.
+
+  In blocking execution mode, BSP waits until all APs finish or
+  TimeoutInMicroseconds expires.
+
+  In non-blocking execution mode, BSP is freed to return to the caller and=
 then
+  proceed to the next task without having to wait for APs. The following
+  sequence needs to occur in a non-blocking execution mode:
+
+    -# The caller that intends to use this MP Services Protocol in non-blo=
cking
+       mode creates WaitEvent by calling the EFI CreateEvent() service.  T=
he
+       caller invokes EFI_MP_SERVICES_PROTOCOL.StartupAllAPs(). If the par=
ameter
+       WaitEvent is not NULL, then StartupAllAPs() executes in non-blockin=
g
+       mode. It requests the function specified by Procedure to be started=
 on
+       all the enabled APs, and releases the BSP to continue with other ta=
sks.
+    -# The caller can use the CheckEvent() and WaitForEvent() services to =
check
+       the state of the WaitEvent created in step 1.
+    -# When the APs complete their task or TimeoutInMicroSecondss expires,=
 the
+       MP Service signals WaitEvent by calling the EFI SignalEvent() funct=
ion.
+       If FailedCpuList is not NULL, its content is available when WaitEve=
nt is
+       signaled. If all APs returned from Procedure prior to the timeout, =
then
+       FailedCpuList is set to NULL. If not all APs return from Procedure =
before
+       the timeout, then FailedCpuList is filled in with the list of the f=
ailed
+       APs. The buffer is allocated by MP Service Protocol using AllocateP=
ool().
+       It is the caller's responsibility to free the buffer with FreePool(=
)
+       service.
+    -# This invocation of SignalEvent() function informs the caller that i=
nvoked
+       EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() that either all the APs
+       completed the specified task or a timeout occurred. The contents of
+       FailedCpuList can be examined to determine which APs did not comple=
te the
+       specified task prior to the timeout.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs ex=
ecute
+                                      the function specified by Procedure =
one by
+                                      one, in ascending order of processor
+                                      handle number.  If FALSE, then all t=
he
+                                      enabled APs execute the function spe=
cified
+                                      by Procedure simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with
+                                      CreateEvent() service.  If it is NUL=
L,
+                                      then execute in blocking mode. BSP w=
aits
+                                      until all APs finish or
+                                      TimeoutInMicroseconds expires.  If i=
t's
+                                      not NULL, then execute in non-blocki=
ng
+                                      mode. BSP requests the function spec=
ified
+                                      by Procedure to be started on all th=
e
+                                      enabled APs, and go on executing
+                                      immediately. If all return from Proc=
edure,
+                                      or TimeoutInMicroseconds expires, th=
is
+                                      event is signaled. The BSP can use t=
he
+                                      CheckEvent() or WaitForEvent()
+                                      services to check the state of event=
. Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds
+                                      for APs to return from Procedure, ei=
ther
+                                      for blocking or non-blocking mode. Z=
ero
+                                      means infinity.  If the timeout expi=
res
+                                      before all APs return from Procedure=
, then
+                                      Procedure on the failed APs is termi=
nated.
+                                      All enabled APs are available for ne=
xt
+                                      function assigned by
+                                      EFI_MP_SERVICES_PROTOCOL.StartupAllA=
Ps()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] FailedCpuList           If NULL, this parameter is ignored.
+                                      Otherwise, if all APs finish success=
fully,
+                                      then its content is set to NULL. If =
not
+                                      all APs finish before timeout expire=
s,
+                                      then its content is set to address o=
f the
+                                      buffer holding handle numbers of the
+                                      failed APs.
+                                      The buffer is allocated by MP Servic=
e
+                                      Protocol, and it's the caller's
+                                      responsibility to free the buffer wi=
th
+                                      FreePool() service.
+                                      In blocking mode, it is ready for
+                                      consumption when the call returns. I=
n
+                                      non-blocking mode, it is ready when
+                                      WaitEvent is signaled. The list of f=
ailed
+                                      CPU is terminated by  END_OF_CPU_LIS=
T.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished =
before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been
+                                  dispatched to all enabled APs.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.
+  @retval EFI_NOT_STARTED         No enabled APs exist in the system.
+  @retval EFI_NOT_READY           Any enabled APs are busy.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+StartupAllAPs (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  )
+{
+  return MpInitLibStartupAllAPs (
+           This,
+           Procedure,
+           SingleThread,
+           WaitEvent,
+           TimeoutInMicroseconds,
+           ProcedureArgument,
+           FailedCpuList
+           );
+}
+
+/**
+  This service lets the caller get one enabled AP to execute a caller-prov=
ided
+  function. The caller can request the BSP to either wait for the completi=
on
+  of the AP or just proceed with the next task by using the EFI event mech=
anism.
+  See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo=
cking
+  execution support.  This service may only be called from the BSP.
+
+  This function is used to dispatch one enabled AP to the function specifi=
ed by
+  Procedure passing in the argument specified by ProcedureArgument.  If Wa=
itEvent
+  is NULL, execution is in blocking mode. The BSP waits until the AP finis=
hes or
+  TimeoutInMicroSecondss expires. Otherwise, execution is in non-blocking =
mode.
+  BSP proceeds to the next task without waiting for the AP. If a non-block=
ing mode
+  is requested after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signa=
led,
+  then EFI_UNSUPPORTED must be returned.
+
+  If the timeout specified by TimeoutInMicroseconds expires before the AP =
returns
+  from Procedure, then execution of Procedure by the AP is terminated. The=
 AP is
+  available for subsequent calls to EFI_MP_SERVICES_PROTOCOL.StartupAllAPs=
() and
+  EFI_MP_SERVICES_PROTOCOL.StartupThisAP().
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  ProcessorNumber         The handle number of the AP. The ran=
ge is
+                                      from 0 to the total number of logica=
l
+                                      processors minus 1. The total number=
 of
+                                      logical processors can be retrieved =
by
+                                      EFI_MP_SERVICES_PROTOCOL.GetNumberOf=
Processors().
+  @param[in]  WaitEvent               The event created by the caller with=
 CreateEvent()
+                                      service.  If it is NULL, then execut=
e in
+                                      blocking mode. BSP waits until all A=
Ps finish
+                                      or TimeoutInMicroseconds expires.  I=
f it's
+                                      not NULL, then execute in non-blocki=
ng mode.
+                                      BSP requests the function specified =
by
+                                      Procedure to be started on all the e=
nabled
+                                      APs, and go on executing immediately=
. If
+                                      all return from Procedure or Timeout=
InMicroseconds
+                                      expires, this event is signaled. The=
 BSP
+                                      can use the CheckEvent() or WaitForE=
vent()
+                                      services to check the state of event=
.  Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds for
+                                      APs to return from Procedure, either=
 for
+                                      blocking or non-blocking mode. Zero =
means
+                                      infinity.  If the timeout expires be=
fore
+                                      all APs return from Procedure, then =
Procedure
+                                      on the failed APs is terminated. All=
 enabled
+                                      APs are available for next function =
assigned
+                                      by EFI_MP_SERVICES_PROTOCOL.StartupA=
llAPs()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] Finished                If NULL, this parameter is ignored. =
 In
+                                      blocking mode, this parameter is ign=
ored.
+                                      In non-blocking mode, if AP returns =
from
+                                      Procedure before the timeout expires=
, its
+                                      content is set to TRUE. Otherwise, t=
he
+                                      value is set to FALSE. The caller ca=
n
+                                      determine if the AP returned from Pr=
ocedure
+                                      by evaluating this value.
+
+  @retval EFI_SUCCESS             In blocking mode, specified AP finished =
before
+                                  the timeout expires.
+  @retval EFI_SUCCESS             In non-blocking mode, the function has b=
een
+                                  dispatched to specified AP.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  the specified AP has finished.
+  @retval EFI_NOT_READY           The specified AP is busy.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or dis=
abled AP.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+StartupThisAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  UINTN                     ProcessorNumber,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT BOOLEAN                   *Finished               OPTIONAL
+  )
+{
+  return MpInitLibStartupThisAP (
+           This,
+           Procedure,
+           ProcessorNumber,
+           WaitEvent,
+           TimeoutInMicroseconds,
+           ProcedureArgument,
+           Finished
+           );
+}
+
+/**
+  This service switches the requested AP to be the BSP from that point onw=
ard.
+  This service changes the BSP for all purposes.   This call can only be
+  performed by the current BSP.
+
+  This service switches the requested AP to be the BSP from that point onw=
ard.
+  This service changes the BSP for all purposes. The new BSP can take over=
 the
+  execution of the old BSP and continue seamlessly from where the old one =
left
+  off. This service may not be supported after the UEFI Event EFI_EVENT_GR=
OUP_READY_TO_BOOT
+  is signaled.
+
+  If the BSP cannot be switched prior to the return from this service, the=
n
+  EFI_UNSUPPORTED must be returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as=
 an
+                               enabled AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed pr=
ior to
+                                  this service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BS=
P or
+                                  a disabled AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+SwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  )
+{
+  return MpInitLibSwitchBSP (This, ProcessorNumber, EnableOldBSP);
+}
+
+/**
+  This service lets the caller enable or disable an AP from this point onw=
ard.
+  This service may only be called from the BSP.
+
+  This service allows the caller enable or disable an AP from this point o=
nward.
+  The caller can optionally specify the health status of the AP by Health.=
 If
+  an AP is being disabled, then the state of the disabled AP is implementa=
tion
+  dependent. If an AP is enabled, then the implementation must guarantee t=
hat a
+  complete initialization sequence is performed on the AP, so the AP is in=
 a state
+  that is compatible with an MP operating system. This service may not be =
supported
+  after the UEFI Event EFI_EVENT_GROUP_READY_TO_BOOT is signaled.
+
+  If the enable or disable AP operation cannot be completed prior to the r=
eturn
+  from this service, then EFI_UNSUPPORTED must be returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableAP          Specifies the new state for the processor f=
or
+                               enabled, FALSE for disabled.
+  @param[in] HealthFlag        If not NULL, a pointer to a value that spec=
ifies
+                               the new health status of the AP. This flag
+                               corresponds to StatusFlag defined in
+                               EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()=
. Only
+                               the PROCESSOR_HEALTH_STATUS_BIT is used. Al=
l other
+                               bits are ignored.  If it is NULL, this para=
meter
+                               is ignored.
+
+  @retval EFI_SUCCESS             The specified AP was enabled or disabled=
 successfully.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be co=
mpleted
+                                  prior to this service returning.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not suppo=
rted.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_NOT_FOUND           Processor with the handle specified by P=
rocessorNumber
+                                  does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+EnableDisableAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  )
+{
+  return MpInitLibEnableDisableAP (This, ProcessorNumber, EnableAP, Health=
Flag);
+}
+
+/**
+  This return the handle number for the calling processor.  This service m=
ay be
+  called from the BSP and APs.
+
+  This service returns the processor handle number for the calling process=
or.
+  The returned value is in the range from 0 to the total number of logical
+  processors minus 1. The total number of logical processors can be retrie=
ved
+  with EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcessors(). This service may =
be
+  called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALI=
D_PARAMETER
+  is returned. Otherwise, the current processors handle number is returned=
 in
+  ProcessorNumber, and EFI_SUCCESS is returned.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[out] ProcessorNumber  The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+
+  @retval EFI_SUCCESS             The current processor handle number was =
returned
+                                  in ProcessorNumber.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+WhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                    *ProcessorNumber
+  )
+{
+  return MpInitLibWhoAmI (This, ProcessorNumber);
+}
+
+EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate =3D {
+  GetNumberOfProcessors,
+  GetProcessorInfo,
+  StartupAllAPs,
+  StartupThisAP,
+  SwitchBSP,
+  EnableDisableAP,
+  WhoAmI
+};
+
+/** Initialize multi-processor support.
+
+**/
+VOID
+InitializeMpSupport (
+  VOID
+  )
+{
+  EFI_STATUS              Status;
+  EFI_HANDLE              Handle;
+  UINTN                   MaxCpus;
+  EFI_HOB_GENERIC_HEADER  *Hob;
+  VOID                    *HobData;
+  UINTN                   HobDataSize;
+  ARM_PROCESSOR_TABLE     CpuInfo;
+
+  DEBUG ((DEBUG_INFO, "Starting MP services"));
+
+  Hob =3D GetFirstGuidHob (&gArmMpCoreInfoGuid);
+  if (Hob !=3D NULL) {
+    HobData         =3D GET_GUID_HOB_DATA (Hob);
+    HobDataSize     =3D GET_GUID_HOB_DATA_SIZE (Hob);
+    CpuInfo.ArmCpus =3D (ARM_CORE_INFO *)HobData;
+    CpuInfo.NumberOfEntries =3D HobDataSize / sizeof (ARM_CORE_INFO);
+    MaxCpus =3D CpuInfo.NumberOfEntries;
+  }
+
+  if (MaxCpus =3D=3D 1) {
+    DEBUG ((DEBUG_WARN, "Trying to use EFI_MP_SERVICES_PROTOCOL on a =
UP system"));
+    // We are not MP so nothing to do
+    return;
+  }
+
+  MpInitLibInitialize (MaxCpus, &CpuInfo);
[SAMI] There is a chance that MaxCpus will be uninitialised if the Hob is N= ULL. I think MaxCpus must be initialised to zero at the start of this funct= ion.
I also think CpuInfo.Header should be initialised (or at least zeroed) befo= re use, to prevent CpuInf.Header from containing random data.
[/SAMI]
+
+  //
+  // Now install the MP services protocol.
+  //
+  Handle =3D NULL;
+  Status =3D gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiMpServiceProtocolGuid,
+                  &mMpServicesTemplate,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+}
diff --git a/ArmPkg/Include/Library/MpInitLib.h b/ArmPkg/Include/Library/Mp=
InitLib.h
new file mode 100644
index 000000000000..a4b80c18a9e8
--- /dev/null
+++ b/ArmPkg/Include/Library/MpInitLib.h
@@ -0,0 +1,366 @@
+/** @file
+
+Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR&g=
t;
+Portions copyright (c) 2011, Apple Inc. All rights reserved.
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MP_INITLIB_H_
+#define MP_INITLIB_H_
+
+#include <Protocol/Cpu.h>
+#include <Protocol/MpService.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Guid/ArmMpCoreInfo.h>
+
+
+/**
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot=
.
+  This service may only be called from the BSP.
+
+  @param[in]  This                        A pointer to the
+                                          EFI_MP_SERVICES_PROTOCOL instanc=
e.
+  @param[out] NumberOfProcessors          Pointer to the total number of l=
ogical
+                                          processors in the system, includ=
ing
+                                          the BSP and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled
+                                          logical processors that exist in=
 the
+                                          system, including the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and ena=
bled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetNumberOfProcessors (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                     *NumberOfProcessors,
+  OUT UINTN                     *NumberOfEnabledProcessors
+  );
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTO=
COL
+                                    instance.
+  @param[in]  ProcessorIndex        The index of the processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where informat=
ion
+                                    for the requested processor is deposit=
ed.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist in the pl=
atform.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetProcessorInfo (
+  IN  EFI_MP_SERVICES_PROTOCOL   *This,
+  IN  UINTN                      ProcessorIndex,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  );
+
+
+/**
+  This service executes a caller provided function on all enabled APs. APs=
 can
+  run either simultaneously or one at a time in sequence. This service sup=
ports
+  both blocking and non-blocking requests. The non-blocking requests use E=
FI
+  events so the BSP can detect when the APs have finished. This service ma=
y only
+  be called from the BSP.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs ex=
ecute
+                                      the function specified by Procedure =
one by
+                                      one, in ascending order of processor
+                                      handle number.  If FALSE, then all t=
he
+                                      enabled APs execute the function spe=
cified
+                                      by Procedure simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with
+                                      CreateEvent() service.  If it is NUL=
L,
+                                      then execute in blocking mode. BSP w=
aits
+                                      until all APs finish or
+                                      TimeoutInMicroseconds expires.  If i=
t's
+                                      not NULL, then execute in non-blocki=
ng
+                                      mode. BSP requests the function spec=
ified
+                                      by Procedure to be started on all th=
e
+                                      enabled APs, and go on executing
+                                      immediately. If all return from Proc=
edure,
+                                      or TimeoutInMicroseconds expires, th=
is
+                                      event is signaled. The BSP can use t=
he
+                                      CheckEvent() or WaitForEvent()
+                                      services to check the state of event=
. Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds
+                                      for APs to return from Procedure, ei=
ther
+                                      for blocking or non-blocking mode. Z=
ero
+                                      means infinity.  If the timeout expi=
res
+                                      before all APs return from Procedure=
, then
+                                      Procedure on the failed APs is termi=
nated.
+                                      All enabled APs are available for ne=
xt
+                                      function assigned by
+                                      EFI_MP_SERVICES_PROTOCOL.StartupAllA=
Ps()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] FailedCpuList           If NULL, this parameter is ignored.
+                                      Otherwise, if all APs finish success=
fully,
+                                      then its content is set to NULL. If =
not
+                                      all APs finish before timeout expire=
s,
+                                      then its content is set to address o=
f the
+                                      buffer holding handle numbers of the
+                                      failed APs.
+                                      The buffer is allocated by MP Servic=
e
+                                      Protocol, and it's the caller's
+                                      responsibility to free the buffer wi=
th
+                                      FreePool() service.
+                                      In blocking mode, it is ready for
+                                      consumption when the call returns. I=
n
+                                      non-blocking mode, it is ready when
+                                      WaitEvent is signaled. The list of f=
ailed
+                                      CPU is terminated by  END_OF_CPU_LIS=
T.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished =
before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been
+                                  dispatched to all enabled APs.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.
+  @retval EFI_NOT_STARTED         No enabled APs exist in the system.
+  @retval EFI_NOT_READY           Any enabled APs are busy.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupAllAPs (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  );
+
+/**
+  This service lets the caller get one enabled AP to execute a caller-prov=
ided
+  function. The caller can request the BSP to either wait for the completi=
on
+  of the AP or just proceed with the next task by using the EFI event mech=
anism.
+  See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo=
cking
+  execution support.  This service may only be called from the BSP.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  ProcessorNumber         The handle number of the AP. The ran=
ge is
+                                      from 0 to the total number of logica=
l
+                                      processors minus 1. The total number=
 of
+                                      logical processors can be retrieved =
by
+                                      EFI_MP_SERVICES_PROTOCOL.GetNumberOf=
Processors().
+  @param[in]  WaitEvent               The event created by the caller with=
 CreateEvent()
+                                      service.  If it is NULL, then execut=
e in
+                                      blocking mode. BSP waits until all A=
Ps finish
+                                      or TimeoutInMicroseconds expires.  I=
f it's
+                                      not NULL, then execute in non-blocki=
ng mode.
+                                      BSP requests the function specified =
by
+                                      Procedure to be started on all the e=
nabled
+                                      APs, and go on executing immediately=
. If
+                                      all return from Procedure or Timeout=
InMicroseconds
+                                      expires, this event is signaled. The=
 BSP
+                                      can use the CheckEvent() or WaitForE=
vent()
+                                      services to check the state of event=
.  Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds for
+                                      APs to return from Procedure, either=
 for
+                                      blocking or non-blocking mode. Zero =
means
+                                      infinity.  If the timeout expires be=
fore
+                                      all APs return from Procedure, then =
Procedure
+                                      on the failed APs is terminated. All=
 enabled
+                                      APs are available for next function =
assigned
+                                      by EFI_MP_SERVICES_PROTOCOL.StartupA=
llAPs()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] Finished                If NULL, this parameter is ignored. =
 In
+                                      blocking mode, this parameter is ign=
ored.
+                                      In non-blocking mode, if AP returns =
from
+                                      Procedure before the timeout expires=
, its
+                                      content is set to TRUE. Otherwise, t=
he
+                                      value is set to FALSE. The caller ca=
n
+                                      determine if the AP returned from Pr=
ocedure
+                                      by evaluating this value.
+
+  @retval EFI_SUCCESS             In blocking mode, specified AP finished =
before
+                                  the timeout expires.
+  @retval EFI_SUCCESS             In non-blocking mode, the function has b=
een
+                                  dispatched to specified AP.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  the specified AP has finished.
+  @retval EFI_NOT_READY           The specified AP is busy.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or dis=
abled AP.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupThisAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  UINTN                     ProcessorNumber,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT BOOLEAN                   *Finished               OPTIONAL
+  );
+
+/**
+  This service switches the requested AP to be the BSP from that point onw=
ard.
+  This service changes the BSP for all purposes.   This call can only be
+  performed by the current BSP.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as=
 an
+                               enabled AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed pr=
ior to
+                                  this service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BS=
P or
+                                  a disabled AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibSwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  );
+
+/**
+  This service lets the caller enable or disable an AP from this point onw=
ard.
+  This service may only be called from the BSP.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableAP          Specifies the new state for the processor f=
or
+                               enabled, FALSE for disabled.
+  @param[in] HealthFlag        If not NULL, a pointer to a value that spec=
ifies
+                               the new health status of the AP. This flag
+                               corresponds to StatusFlag defined in
+                               EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()=
. Only
+                               the PROCESSOR_HEALTH_STATUS_BIT is used. Al=
l other
+                               bits are ignored.  If it is NULL, this para=
meter
+                               is ignored.
+
+  @retval EFI_SUCCESS             The specified AP was enabled or disabled=
 successfully.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be co=
mpleted
+                                  prior to this service returning.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not suppo=
rted.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_NOT_FOUND           Processor with the handle specified by P=
rocessorNumber
+                                  does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibEnableDisableAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  );
+
+/**
+  This return the handle number for the calling processor.  This service m=
ay be
+  called from the BSP and APs.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[out] ProcessorNumber  The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+
+  @retval EFI_SUCCESS             The current processor handle number was =
returned
+                                  in ProcessorNumber.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibWhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                    *ProcessorNumber
+  );
+
+/** Initializes the MP Services system data
+
+   @param NumberOfProcessors The number of processors, both BSP and AP.
+   @param CpuInfo            CPU information gathered earlier during boot.
+
+**/
+VOID
+MpInitLibInitialize (
+  IN   UINTN     NumberOfProcessors,
+  IN   ARM_PROCESSOR_TABLE *CpuInfo
+  );
+
+
+
+#endif /* MP_INITLIB_H_ */
diff --git a/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S b/ArmPkg/Library/Mp=
InitLib/AArch64/MpFuncs.S
new file mode 100644
index 000000000000..287db060e594
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/AArch64/MpFuncs.S
@@ -0,0 +1,61 @@
+#=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D
+#  Copyright (c) 2021 NUVIA Inc. All rights reserved.
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D
+
+.text
+.align 3
+
+#include <AsmMacroIoLibV8.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "InternalMpInitLib.h"
+
+GCC_ASM_IMPORT (gApStacksBase)
+GCC_ASM_IMPORT (gProcessorIDs)
+GCC_ASM_IMPORT (ApProcedure)
+GCC_ASM_IMPORT (gApStackSize)
+
+GCC_ASM_EXPORT (ApEntryPoint)
+
+StartupAddr:        .8byte ASM_PFX(ApProcedure)
+
+// Entry-point for the AP
+// VOID
+// ApEntryPoint (
+//   VOID
+//   );
+ASM_PFX(ApEntryPoint):
+  mrs x0, mpidr_el1
+  ldr x1, gProcessorIDs
+  mov x2, 0                   // x2 =3D processor index
+  mov x3, 0                   // x3 =3D address offset
+
+// Find index in gProcessorIDs for current processor
+1:
+  ldr x4, [x1, x3]            // x4 =3D gProcessorIDs + x3
+  cmp x4, 0                   // check if we've reached the end of gProces=
sorIDs
+  beq ProcessorNotFound
+  add x3, x3, 8               // x3 +=3D sizeof (*gProcessorIDs)
+  add x2, x2, 1               // x2++
+  cmp x0, x4                  // if mpidr_el1 !=3D *(gProcessorIDs + x3) t=
hen loop
[SAMI] I think this may not work for PEs with the MT bit[24] set. Please se= e my comment further ahead in FillInProcessorInformation(). 
+  bne 1b
+  sub x2, x2, 1
+
+// Calculate stack address
+  // x2 contains the index for the current processor
+  ldr x0, gApStacksBase
+  ldr x1, gApStackSize
+  mul x3, x2, x1              // x3 =3D ProcessorIndex * gApStackSize
+  add x4, x0, x3              // x4 =3D gApStacksBase + x3
+  add sp, x4, x1              // sp =3D x4 + gApStackSize
+
+  ldr x0, StartupAddr         // ASM_PFX(ApProcedure)
+  blr x0                      // doesn't return
+
+ProcessorNotFound:
+// Turn off the processor
+  MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
+  smc #0
+  b .
diff --git a/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf b/ArmPkg/Library/MpI=
nitLib/DxeMpInitLib.inf
new file mode 100644
index 000000000000..2275b6cca33a
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
@@ -0,0 +1,53 @@
+#/** @file
+#
+# Component description file for the DxeMpInitLib module.
+#
+# Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+#
+# SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+#**/
+
+[Defines]
+  INF_VERSION                    =3D 1.29
+  BASE_NAME                      =3D DxeMpInitLib
+  FILE_GUID                      =3D c9ca773c-8ae4-4b74-82fd-f7345503294e
+  MODULE_TYPE                    =3D DXE_DRIVER
+  VERSION_STRING                 =3D 1.0
+  LIBRARY_CLASS                  =3D MpInitLib|DXE_DRIVER
+
+#
+# The following information is for reference only and not required by the =
build tools.
+#
+#  VALID_ARCHITECTURES           =3D AARCH64
+#
+
+[Sources.AARCH64]
+  AArch64/MpFuncs.S
+
+[Sources]
+  DxeMpLib.c
+  InternalMpInitLib.h
+
+[Packages]
+  ArmPkg/ArmPkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  ArmLib
+  ArmSmcLib
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  HobLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Protocols]
+  gEfiMpServiceProtocolGuid
+
+[Guids]
+  gArmMpCoreInfoGuid
diff --git a/ArmPkg/Library/MpInitLib/DxeMpLib.c b/ArmPkg/Library/MpInitLib=
/DxeMpLib.c
new file mode 100644
index 000000000000..6053db740624
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/DxeMpLib.c
@@ -0,0 +1,1498 @@
+/** @file
+  Construct MP Services Protocol.
+
+  Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+  Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR=
>
+  Portions Copyright (c) 2011, Apple Inc. All rights reserved.
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Ppi/ArmMpCoreInfo.h>
+#include <Library/ArmLib.h>
+#include <Library/ArmSmcLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/MpInitLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "InternalMpInitLib.h"
+
+#define POLL_INTERVAL_US  50000
+
+STATIC CPU_MP_DATA  mCpuMpData;
+STATIC BOOLEAN      mNonBlockingModeAllowed;
+UINT64              *gApStacksBase;
+UINT64              *gProcessorIDs;
+CONST UINT64        gApStackSize =3D AP_STACK_SIZE;
+
+/** C entry-point for the AP.
+    This function gets called from the assembly function ApEntryPoint.
+
+**/
+VOID
+ApProcedure (
+  VOID
+  )
+{
+  ARM_SMC_ARGS      Args;
+  EFI_AP_PROCEDURE  ApProcedure;
+  VOID              *ApParameter;
+  UINTN             ProcessorIndex;
+
+  MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorIndex);
+
+  /* Fetch the user-supplied procedure and parameter to execute */
+  ApProcedure =3D mCpuMpData.CpuData[ProcessorIndex].Procedure;
+  ApParameter =3D mCpuMpData.CpuData[ProcessorIndex].Parameter;
+
[SAMI] Is it possible to change the name of the local function pointer variable ApProcedure, p= lease?
 
+  ApProcedure (ApParameter);
+
+  mCpuMpData.CpuData[ProcessorIndex].State =3D CpuStateFinished;
+
+  /* Since we're finished with this AP, turn it off */
+  Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_OFF;
+  ArmCallSmc (&Args);
+
+  /* Should never be reached */
+  ASSERT (FALSE);
+  CpuDeadLoop ();
+}
+
+/** Turns on the specified core using PSCI and executes the user-supplied
+    function that's been configured via a previous call to SetApProcedure.
+
+    @param ProcessorIndex The index of the core to turn on.
+
+    @retval EFI_SUCCESS      Success.
+    @retval EFI_DEVICE_ERROR The processor could not be turned on.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+DispatchCpu (
+  IN UINTN ProcessorIndex
+  )
+{
+  ARM_SMC_ARGS  Args;
+  EFI_STATUS    Status;
+
+  Status =3D EFI_SUCCESS;
+
+  mCpuMpData.CpuData[ProcessorIndex].State =3D CpuStateBusy;
+
+  /* Turn the AP on */
+  if (sizeof (Args.Arg0) =3D=3D sizeof (UINT32)) {
+    Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_ON_AARCH32;
+  } else {
+    Args.Arg0 =3D ARM_SMC_ID_PSCI_CPU_ON_AARCH64;
+  }
+  Args.Arg1 =3D gProcessorIDs[ProcessorIndex];
+  Args.Arg2 =3D (UINTN)ApEntryPoint;
+
+  ArmCallSmc (&Args);
+
+  if (Args.Arg0 !=3D ARM_SMC_PSCI_RET_SUCCESS) {
+    DEBUG ((DEBUG_ERROR, "PSCI_CPU_ON call failed: %d", Args.Arg=
0));
+    Status =3D EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/** Returns whether the specified processor is the BSP.
+
+  @param[in] ProcessorIndex The index the processor to check.
+
+  @return TRUE if the processor is the BSP, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorBSP (
+  UINTN ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  return (CpuInfo->StatusFlag & PROCESSOR_AS_BSP_BIT) !=3D 0;
+}
+
+/** Returns whether the processor executing this function is the BSP.
+
+    @return Whether the current processor is the BSP.
+**/
+STATIC
+BOOLEAN
+IsCurrentProcessorBSP (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       ProcessorIndex;
+
+  Status =3D MpInitLibWhoAmI (&mMpServicesTemplate, &ProcessorInde=
x);
+  ASSERT_EFI_ERROR (Status);
[SAMI] I think this assert can be changed to ASSERT (0) and moved inside th= e if condition below. 
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  return IsProcessorBSP (ProcessorIndex);
+}
+
+/** Get the Application Processors state.
+
+  @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP.
+
+  @return The AP status.
+**/
+CPU_STATE
+GetApState (
+  IN  CPU_AP_DATA     *CpuData
+  )
+{
+  return CpuData->State;
+}
+
+/** Configures the processor context with the user-supplied procedure and
+    argument.
+
+   @param CpuData           The processor context.
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+
+**/
+STATIC
+VOID
+SetApProcedure (
+  IN   CPU_AP_DATA        *CpuData,
+  IN   EFI_AP_PROCEDURE   Procedure,
+  IN   VOID               *ProcedureArgument
+  )
+{
+  ASSERT (CpuData !=3D NULL);
+  ASSERT (Procedure !=3D NULL);
+
+  CpuData->Parameter =3D ProcedureArgument;
+  CpuData->Procedure =3D Procedure;
+}
+
+/** Returns the index of the next processor that is blocked.
+
+   @param[out] NextNumber The index of the next blocked processor.
+
+   @retval EFI_SUCCESS   Successfully found the next blocked processor.
+   @retval EFI_NOT_FOUND There are no blocked processors.
+
+**/
+STATIC
+EFI_STATUS
+GetNextBlockedNumber (
+  OUT UINTN *NextNumber
+  )
+{
+  UINTN        Index;
+  CPU_STATE    State;
+  CPU_AP_DATA  *CpuData;
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    State =3D CpuData->State;
+
+    if (State =3D=3D CpuStateBlocked) {
+      *NextNumber =3D Index;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/** Stalls the BSP for the minimum of POLL_INTERVAL_US and Timeout.
+
+   @param[in]  Timeout    The time limit in microseconds remaining for
+                          APs to return from Procedure.
+
+   @retval     StallTime  Time of execution stall.
+**/
+STATIC
+UINTN
+CalculateAndStallInterval (
+  IN UINTN Timeout
+  )
+{
+  UINTN  StallTime;
+
+  if (Timeout < POLL_INTERVAL_US && Timeout !=3D 0) {
[SAMI] Please use extra parentheses. see https://edk2-docs.gitbook.io/edk-ii-c-coding-standards-specification/5_sour= ce_files/52_spacing#5.2.2.10-use-extra-parentheses-rather-than-depending-on= -in-depth-knowledge-of-the-order-of-precedenc
Same at other places in this series.
[/SAMI]
+    StallTime =3D Timeout;
+  } else {
+    StallTime =3D POLL_INTERVAL_US;
+  }
+
+  gBS->Stall (StallTime);
+
+  return StallTime;
+}
+
+/**
+  This service retrieves the number of logical processor in the platform
+  and the number of those logical processors that are enabled on this boot=
.
+  This service may only be called from the BSP.
+
+  @param[in]  This                        A pointer to the
+                                          EFI_MP_SERVICES_PROTOCOL instanc=
e.
+  @param[out] NumberOfProcessors          Pointer to the total number of l=
ogical
+                                          processors in the system, includ=
ing
+                                          the BSP and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled
+                                          logical processors that exist in=
 the
+                                          system, including the BSP.
+
+  @retval EFI_SUCCESS             The number of logical processors and ena=
bled
+                                  logical processors was retrieved.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.
+  @retval EFI_INVALID_PARAMETER   NumberOfEnabledProcessors is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetNumberOfProcessors (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                     *NumberOfProcessors,
+  OUT UINTN                     *NumberOfEnabledProcessors
+  )
+{
+  if ((NumberOfProcessors =3D=3D NULL) || (NumberOfEnabledProcessors =3D=
=3D NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  *NumberOfProcessors =3D mCpuMpData.NumberOfProcessors;
+  *NumberOfEnabledProcessors =3D mCpuMpData.NumberOfEnabledProcessors;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made. This service may only be called from the BSP.
+
+  @param[in]  This                  A pointer to the EFI_MP_SERVICES_PROTO=
COL
+                                    instance.
+  @param[in]  ProcessorIndex        The index of the processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where informat=
ion
+                                    for the requested processor is deposit=
ed.
+
+  @retval EFI_SUCCESS             Processor information was returned.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist in the pl=
atform.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibGetProcessorInfo (
+  IN  EFI_MP_SERVICES_PROTOCOL   *This,
+  IN  UINTN                      ProcessorIndex,
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
+  )
+{
+  if (ProcessorInfoBuffer =3D=3D NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ProcessorIndex &=3D ~CPU_V2_EXTENDED_TOPOLOGY;
[SAMI] I don't see this set anywhere in the code. Also the other functions = do not clear this bit before processing. Can you help me to understand this= , please?
+
+  if (ProcessorIndex >=3D mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  CopyMem (
+    ProcessorInfoBuffer,
+    &mCpuMpData.CpuData[ProcessorIndex],
+    sizeof (EFI_PROCESSOR_INFORMATION)
+    );
+  return EFI_SUCCESS;
+}
+
+/** Returns whether the specified processor is enabled.
+
+   @param[in] ProcessorIndex The index of the processor to check.
+
+   @return TRUE if the processor is enabled, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorEnabled (
+  UINTN ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  return (CpuInfo->StatusFlag & PROCESSOR_ENABLED_BIT) !=3D 0;
+}
+
+/** Returns whether all processors are in the idle state.
+
+   @return Whether all the processors are idle.
+
+**/
+STATIC
+BOOLEAN
+CheckAllCpusReady (
+  VOID
+  )
+{
+  UINTN        Index;
+  CPU_AP_DATA  *CpuData;
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      continue;
+    }
+
+    if (GetApState (CpuData) !=3D CpuStateIdle) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/** Sets up the state for the StartupAllAPs function.
+
+   @param SingleThread Whether the APs will execute sequentially.
+
+**/
+STATIC
+VOID
+StartupAllAPsPrepareState (
+  IN BOOLEAN SingleThread
+  )
+{
+  UINTN        Index;
+  CPU_STATE    APInitialState;
+  CPU_AP_DATA  *CpuData;
+
+  mCpuMpData.FinishCount  =3D 0;
+  mCpuMpData.StartCount   =3D 0;
+  mCpuMpData.SingleThread =3D SingleThread;
+
+  APInitialState =3D CpuStateReady;
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
+
+    //
+    // Get APs prepared, and put failing APs into FailedCpuList.
+    // If "SingleThread", only 1 AP will put into ready state, o=
ther AP will be
+    // put into ready state 1 by 1, until the previous 1 finished its task=
.
+    // If not "SingleThread", all APs are put into ready state f=
rom the
+    // beginning
+    //
+
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      if (mCpuMpData.FailedList !=3D NULL) {
+        mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] =3D Index;
+      }
+
+      continue;
+    }
+
+    ASSERT (GetApState (CpuData) =3D=3D CpuStateIdle);
+    CpuData->State =3D APInitialState;
+
+    mCpuMpData.StartCount++;
+    if (SingleThread) {
+      APInitialState =3D CpuStateBlocked;
+    }
+  }
+}
+
+/** Handles execution of StartupAllAPs when a WaitEvent has been specified=
.
+
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+   @param WaitEvent         The wait event to be signaled when the work is
+                            complete or a timeout has occurred.
+   @param TimeoutInMicroseconds The timeout for the work to be completed. =
Zero
+                                indicates an infinite timeout.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *ProcedureArgument,
+  IN EFI_EVENT        WaitEvent,
+  IN UINTN            TimeoutInMicroseconds
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Index;
+  CPU_AP_DATA  *CpuData;
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      continue;
+    }
+
+    if (GetApState (CpuData) =3D=3D CpuStateReady) {
+      SetApProcedure (CpuData, Procedure, ProcedureArgument);
+    }
+  }
+
+  //
+  // Save data into private data structure, and create timer to poll AP st=
ate
+  // before exiting
+  //
+  mCpuMpData.Procedure =3D Procedure;
+  mCpuMpData.ProcedureArgument =3D ProcedureArgument;
+  mCpuMpData.WaitEvent     =3D WaitEvent;
+  mCpuMpData.Timeout       =3D TimeoutInMicroseconds;
+  mCpuMpData.TimeoutActive =3D (BOOLEAN)(TimeoutInMicroseconds !=3D 0);
+  Status =3D gBS->SetTimer (
+                  mCpuMpData.CheckAllAPsEvent,
+                  TimerPeriodic,
+                  POLL_INTERVAL_US
+                  );
+  return Status;
+}
+
+/** Handles execution of StartupAllAPs when no wait event has been specifi=
ed.
+
+   @param Procedure             The user-supplied procedure.
+   @param ProcedureArgument     The user-supplied procedure argument.
+   @param TimeoutInMicroseconds The timeout for the work to be completed. =
Zero
+                                indicates an infinite timeout.
+   @param SingleThread          Whether the APs will execute sequentially.
+   @param FailedCpuList         User-supplied pointer for list of failed C=
PUs.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsNoWaitEvent (
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *ProcedureArgument,
+  IN UINTN            TimeoutInMicroseconds,
+  IN BOOLEAN          SingleThread,
+  IN UINTN            **FailedCpuList
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Index;
+  UINTN        NextIndex;
+  UINTN        Timeout;
+  CPU_AP_DATA  *CpuData;
+
+  Timeout =3D TimeoutInMicroseconds;
+
+  while (TRUE) {
+    for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+      CpuData =3D &mCpuMpData.CpuData[Index];
+      if (IsProcessorBSP (Index)) {
+        // Skip BSP
+        continue;
+      }
+
+      if (!IsProcessorEnabled (Index)) {
+        // Skip Disabled processors
+        continue;
+      }
+
+      switch (GetApState (CpuData)) {
+        case CpuStateReady:
+          SetApProcedure (CpuData, Procedure, ProcedureArgument);
+          Status =3D DispatchCpu (Index);
+          if (EFI_ERROR (Status)) {
+            CpuData->State =3D CpuStateIdle;
+            Status =3D EFI_NOT_READY;
+            goto Done;
+          }
+
+          break;
+
+        case CpuStateFinished:
+          mCpuMpData.FinishCount++;
+          if (SingleThread) {
+            Status =3D GetNextBlockedNumber (&NextIndex);
+            if (!EFI_ERROR (Status)) {
+              mCpuMpData.CpuData[NextIndex].State =3D CpuStateReady;
+            }
+          }
+
+          CpuData->State =3D CpuStateIdle;
+          break;
+
+        default:
+          break;
+      }
+    }
+
+    if (mCpuMpData.FinishCount =3D=3D mCpuMpData.StartCount) {
+      Status =3D EFI_SUCCESS;
+      goto Done;
+    }
+
+    if ((TimeoutInMicroseconds !=3D 0) && (Timeout =3D=3D 0)) {
+      Status =3D EFI_TIMEOUT;
+      goto Done;
+    }
+
+    Timeout -=3D CalculateAndStallInterval (Timeout);
+  }
+
+Done:
+  if (FailedCpuList !=3D NULL) {
+    if (mCpuMpData.FailedListIndex =3D=3D 0) {
+      FreePool (*FailedCpuList);
+      *FailedCpuList =3D NULL;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  This service executes a caller provided function on all enabled APs. APs=
 can
+  run either simultaneously or one at a time in sequence. This service sup=
ports
+  both blocking and non-blocking requests. The non-blocking requests use E=
FI
+  events so the BSP can detect when the APs have finished. This service ma=
y only
+  be called from the BSP.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs ex=
ecute
+                                      the function specified by Procedure =
one by
+                                      one, in ascending order of processor
+                                      handle number.  If FALSE, then all t=
he
+                                      enabled APs execute the function spe=
cified
+                                      by Procedure simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with
+                                      CreateEvent() service.  If it is NUL=
L,
+                                      then execute in blocking mode. BSP w=
aits
+                                      until all APs finish or
+                                      TimeoutInMicroseconds expires.  If i=
t's
+                                      not NULL, then execute in non-blocki=
ng
+                                      mode. BSP requests the function spec=
ified
+                                      by Procedure to be started on all th=
e
+                                      enabled APs, and go on executing
+                                      immediately. If all return from Proc=
edure,
+                                      or TimeoutInMicroseconds expires, th=
is
+                                      event is signaled. The BSP can use t=
he
+                                      CheckEvent() or WaitForEvent()
+                                      services to check the state of event=
. Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds
+                                      for APs to return from Procedure, ei=
ther
+                                      for blocking or non-blocking mode. Z=
ero
+                                      means infinity.  If the timeout expi=
res
+                                      before all APs return from Procedure=
, then
+                                      Procedure on the failed APs is termi=
nated.
+                                      All enabled APs are available for ne=
xt
+                                      function assigned by
+                                      EFI_MP_SERVICES_PROTOCOL.StartupAllA=
Ps()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] FailedCpuList           If NULL, this parameter is ignored.
+                                      Otherwise, if all APs finish success=
fully,
+                                      then its content is set to NULL. If =
not
+                                      all APs finish before timeout expire=
s,
+                                      then its content is set to address o=
f the
+                                      buffer holding handle numbers of the
+                                      failed APs.
+                                      The buffer is allocated by MP Servic=
e
+                                      Protocol, and it's the caller's
+                                      responsibility to free the buffer wi=
th
+                                      FreePool() service.
+                                      In blocking mode, it is ready for
+                                      consumption when the call returns. I=
n
+                                      non-blocking mode, it is ready when
+                                      WaitEvent is signaled. The list of f=
ailed
+                                      CPU is terminated by  END_OF_CPU_LIS=
T.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished =
before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been
+                                  dispatched to all enabled APs.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.
+  @retval EFI_NOT_STARTED         No enabled APs exist in the system.
+  @retval EFI_NOT_READY           Any enabled APs are busy.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  all enabled APs have finished.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupAllAPs (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (mCpuMpData.NumberOfProcessors =3D=3D 1) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (Procedure =3D=3D NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((WaitEvent !=3D NULL) && !mNonBlockingModeAllowed) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (!CheckAllCpusReady ()) {
+    return EFI_NOT_READY;
+  }
+
+  if (FailedCpuList !=3D NULL) {
+    mCpuMpData.FailedList =3D AllocatePool (
+                              (mCpuMpData.NumberOfProcessors + 1) *
+                              sizeof (UINTN)
+                              );
+    if (mCpuMpData.FailedList =3D=3D NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    SetMemN (
+      mCpuMpData.FailedList,
+      (mCpuMpData.NumberOfProcessors + 1) *
+      sizeof (UINTN),
+      END_OF_CPU_LIST
+      );
+    mCpuMpData.FailedListIndex =3D 0;
+    *FailedCpuList =3D mCpuMpData.FailedList;
+  }
+
+  StartupAllAPsPrepareState (SingleThread);
+
+  if (WaitEvent !=3D NULL) {
+    Status =3D StartupAllAPsWithWaitEvent (
+               Procedure,
+               ProcedureArgument,
+               WaitEvent,
+               TimeoutInMicroseconds
+               );
+  } else {
+    Status =3D StartupAllAPsNoWaitEvent (
+               Procedure,
+               ProcedureArgument,
+               TimeoutInMicroseconds,
+               SingleThread,
+               FailedCpuList
+               );
+  }
+
+  return Status;
+}
+
+/**
+  This service lets the caller get one enabled AP to execute a caller-prov=
ided
+  function. The caller can request the BSP to either wait for the completi=
on
+  of the AP or just proceed with the next task by using the EFI event mech=
anism.
+  See EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() for more details on non-blo=
cking
+  execution support.  This service may only be called from the BSP.
+
+  @param[in]  This                    A pointer to the EFI_MP_SERVICES_PRO=
TOCOL
+                                      instance.
+  @param[in]  Procedure               A pointer to the function to be run =
on
+                                      enabled APs of the system. See type
+                                      EFI_AP_PROCEDURE.
+  @param[in]  ProcessorNumber         The handle number of the AP. The ran=
ge is
+                                      from 0 to the total number of logica=
l
+                                      processors minus 1. The total number=
 of
+                                      logical processors can be retrieved =
by
+                                      EFI_MP_SERVICES_PROTOCOL.GetNumberOf=
Processors().
+  @param[in]  WaitEvent               The event created by the caller with=
 CreateEvent()
+                                      service.  If it is NULL, then execut=
e in
+                                      blocking mode. BSP waits until all A=
Ps finish
+                                      or TimeoutInMicroseconds expires.  I=
f it's
+                                      not NULL, then execute in non-blocki=
ng mode.
+                                      BSP requests the function specified =
by
+                                      Procedure to be started on all the e=
nabled
+                                      APs, and go on executing immediately=
. If
+                                      all return from Procedure or Timeout=
InMicroseconds
+                                      expires, this event is signaled. The=
 BSP
+                                      can use the CheckEvent() or WaitForE=
vent()
+                                      services to check the state of event=
.  Type
+                                      EFI_EVENT is defined in CreateEvent(=
) in
+                                      the Unified Extensible Firmware Inte=
rface
+                                      Specification.
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microsec=
onds for
+                                      APs to return from Procedure, either=
 for
+                                      blocking or non-blocking mode. Zero =
means
+                                      infinity.  If the timeout expires be=
fore
+                                      all APs return from Procedure, then =
Procedure
+                                      on the failed APs is terminated. All=
 enabled
+                                      APs are available for next function =
assigned
+                                      by EFI_MP_SERVICES_PROTOCOL.StartupA=
llAPs()
+                                      or EFI_MP_SERVICES_PROTOCOL.StartupT=
hisAP().
+                                      If the timeout expires in blocking m=
ode,
+                                      BSP returns EFI_TIMEOUT.  If the tim=
eout
+                                      expires in non-blocking mode, WaitEv=
ent
+                                      is signaled with SignalEvent().
+  @param[in]  ProcedureArgument       The parameter passed into Procedure =
for
+                                      all APs.
+  @param[out] Finished                If NULL, this parameter is ignored. =
 In
+                                      blocking mode, this parameter is ign=
ored.
+                                      In non-blocking mode, if AP returns =
from
+                                      Procedure before the timeout expires=
, its
+                                      content is set to TRUE. Otherwise, t=
he
+                                      value is set to FALSE. The caller ca=
n
+                                      determine if the AP returned from Pr=
ocedure
+                                      by evaluating this value.
+
+  @retval EFI_SUCCESS             In blocking mode, specified AP finished =
before
+                                  the timeout expires.
+  @retval EFI_SUCCESS             In non-blocking mode, the function has b=
een
+                                  dispatched to specified AP.
+  @retval EFI_UNSUPPORTED         A non-blocking mode request was made aft=
er the
+                                  UEFI event EFI_EVENT_GROUP_READY_TO_BOOT=
 was
+                                  signaled.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired be=
fore
+                                  the specified AP has finished.
+  @retval EFI_NOT_READY           The specified AP is busy.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or dis=
abled AP.
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibStartupThisAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  UINTN                     ProcessorNumber,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT BOOLEAN                   *Finished               OPTIONAL
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Timeout;
+  CPU_AP_DATA  *CpuData;
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Procedure =3D=3D NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  CpuData =3D &mCpuMpData.CpuData[ProcessorNumber];
+
+  if (IsProcessorBSP (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsProcessorEnabled (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GetApState (CpuData) !=3D CpuStateIdle) {
+    return EFI_NOT_READY;
+  }
+
+  if ((WaitEvent !=3D NULL) && !mNonBlockingModeAllowed) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Timeout =3D TimeoutInMicroseconds;
+
+  mCpuMpData.StartCount  =3D 1;
+  mCpuMpData.FinishCount =3D 0;
+
+  SetApProcedure (
+    CpuData,
+    Procedure,
+    ProcedureArgument
+    );
+
+  Status =3D DispatchCpu (ProcessorNumber);
+  if (EFI_ERROR (Status)) {
+    CpuData->State =3D CpuStateIdle;
+    return EFI_NOT_READY;
+  }
+
+  if (WaitEvent !=3D NULL) {
+    // Non Blocking
+    mCpuMpData.WaitEvent =3D WaitEvent;
+    gBS->SetTimer (
+           CpuData->CheckThisAPEvent,
+           TimerPeriodic,
+           POLL_INTERVAL_US
+           );
+    return EFI_SUCCESS;
+  }
+
+  // Blocking
+  while (TRUE) {
+    if (GetApState (CpuData) =3D=3D CpuStateFinished) {
+      CpuData->State =3D CpuStateIdle;
+      break;
+    }
+
+    if ((TimeoutInMicroseconds !=3D 0) && (Timeout =3D=3D 0)) {
+      return EFI_TIMEOUT;
+    }
+
+    Timeout -=3D CalculateAndStallInterval (Timeout);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This service switches the requested AP to be the BSP from that point onw=
ard.
+  This service changes the BSP for all purposes.   This call can only be
+  performed by the current BSP.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableOldBSP      If TRUE, then the old BSP will be listed as=
 an
+                               enabled AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed pr=
ior to
+                                  this service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified =
by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BS=
P or
+                                  a disabled AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibSwitchBSP (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  )
+{
+  UINTN        Index;
+  CPU_AP_DATA  *CpuData;
+
+  CpuData =3D &mCpuMpData.CpuData[ProcessorNumber];
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (!IsProcessorEnabled (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsProcessorBSP (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    if (IsProcessorBSP (Index)) {
+      break;
+    }
+  }
+
+  ASSERT (Index !=3D mCpuMpData.NumberOfProcessors);
+
+  if (GetApState (CpuData) !=3D CpuStateIdle) {
+    return EFI_NOT_READY;
+  }
+
+  // Skip for now as we need switch a bunch of stack stuff around and it's
+  // complex. May not be worth it?
+  return EFI_NOT_READY;
[SAMI] Should this function just return EFI_UNSUPPORTED? I don't think any = other processing is needed in this function.
+}
+
+/**
+  This service lets the caller enable or disable an AP from this point onw=
ard.
+  This service may only be called from the BSP.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[in] ProcessorNumber   The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+  @param[in] EnableAP          Specifies the new state for the processor f=
or
+                               enabled, FALSE for disabled.
+  @param[in] HealthFlag        If not NULL, a pointer to a value that spec=
ifies
+                               the new health status of the AP. This flag
+                               corresponds to StatusFlag defined in
+                               EFI_MP_SERVICES_PROTOCOL.GetProcessorInfo()=
. Only
+                               the PROCESSOR_HEALTH_STATUS_BIT is used. Al=
l other
+                               bits are ignored.  If it is NULL, this para=
meter
+                               is ignored.
+
+  @retval EFI_SUCCESS             The specified AP was enabled or disabled=
 successfully.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be co=
mpleted
+                                  prior to this service returning.
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not suppo=
rted.
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.
+  @retval EFI_NOT_FOUND           Processor with the handle specified by P=
rocessorNumber
+                                  does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibEnableDisableAP (
+  IN  EFI_MP_SERVICES_PROTOCOL  *This,
+  IN  UINTN                     ProcessorNumber,
+  IN  BOOLEAN                   EnableAP,
+  IN  UINT32                    *HealthFlag OPTIONAL
+  )
+{
+  UINTN        StatusFlag;
+  CPU_AP_DATA  *CpuData;
+
+  StatusFlag =3D mCpuMpData.CpuData[ProcessorNumber].Info.StatusFlag;
+  CpuData    =3D &mCpuMpData.CpuData[ProcessorNumber];
+
+  if (!IsCurrentProcessorBSP ()) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ProcessorNumber >=3D mCpuMpData.NumberOfProcessors) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (IsProcessorBSP (ProcessorNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GetApState (CpuData) !=3D CpuStateIdle) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (EnableAP) {
+    if (!IsProcessorEnabled (ProcessorNumber)) {
+      mCpuMpData.NumberOfEnabledProcessors++;
+    }
+
+    StatusFlag |=3D PROCESSOR_ENABLED_BIT;
+  } else {
+    if (IsProcessorEnabled (ProcessorNumber)) {
+      mCpuMpData.NumberOfEnabledProcessors--;
+    }
+
+    StatusFlag &=3D ~PROCESSOR_ENABLED_BIT;
+  }
+
+  if (HealthFlag !=3D NULL) {
+    StatusFlag &=3D ~PROCESSOR_HEALTH_STATUS_BIT;
+    StatusFlag |=3D (*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This return the handle number for the calling processor.  This service m=
ay be
+  called from the BSP and APs.
+
+  @param[in] This              A pointer to the EFI_MP_SERVICES_PROTOCOL i=
nstance.
+  @param[out] ProcessorNumber  The handle number of AP that is to become t=
he new
+                               BSP. The range is from 0 to the total numbe=
r of
+                               logical processors minus 1. The total numbe=
r of
+                               logical processors can be retrieved by
+                               EFI_MP_SERVICES_PROTOCOL.GetNumberOfProcess=
ors().
+
+  @retval EFI_SUCCESS             The current processor handle number was =
returned
+                                  in ProcessorNumber.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+MpInitLibWhoAmI (
+  IN EFI_MP_SERVICES_PROTOCOL  *This,
+  OUT UINTN                    *ProcessorNumber
+  )
+{
+  UINTN        Index;
+  UINT64       ProcessorId;
+  CPU_AP_DATA  *CpuData;
+
+  if (ProcessorNumber =3D=3D NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ProcessorId =3D ArmReadMpidr ();
[SAMI] Here again, I think it will not work on PEs with the MT bit[24] set.= I think it would be better to mask of all other bits of MPIDR and only kee= p use the affinity bits.  CpuData->Info.ProcessorId should only sto= re the affinity bits.
This would also match the requirement by the PSCI specification version D.b= (https://developer.arm.com/documentation/den0022/= latest/). See section 5.1.4 CPU_ON
    If the calling Exception level is using AArch64, the for= mat is:
        =E2=80=A2 Bits[40:63]: Must be z= ero
        =E2=80=A2 Bits[32:39] Aff3: Matc= h Aff3 of target core MPIDR
        =E2=80=A2 Bits[24:31] Must be ze= ro
        =E2=80=A2 Bits[16:23] Aff2: Matc= h Aff2 of target core MPIDR
        =E2=80=A2 Bits[8:15] Aff1: Match= Aff1 of target core MPIDR
        =E2=80=A2 Bits[0:7] Aff0: Match = Aff0 of target core MPIDR
[/SAMI]
 
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessor=
s; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
+    if (CpuData->Info.ProcessorId =3D=3D ProcessorId) {
+      break;
+    }
+  }
+
+  *ProcessorNumber =3D Index;
+  return EFI_SUCCESS;
+}
+
+/** Adds the specified processor the list of failed processors.
+
+   @param ProcessorIndex The processor index to add.
+   @param ApState        Processor state.
+
+**/
+STATIC
+VOID
+AddProcessorToFailedList (
+  UINTN           ProcessorIndex,
+  CPU_STATE       ApState
+  )
+{
+  UINTN    Index;
+  BOOLEAN  Found;
+
+  Found =3D FALSE;
+
+  if (ApState =3D=3D CpuStateIdle) {
+    return;
+  }
+
+  // If we are retrying make sure we don't double count
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
[SAMI] Can the for loop run from 0 to mCpuMpData.FailedListIndex? In that c= ase we would not need to check against END_OF_CPU_LIST right?
+    if (mCpuMpData.FailedList[Index] =3D=3D END_OF_CPU_LIST) {
+      break;
+    }
+
+    if (mCpuMpData.FailedList[ProcessorIndex] =3D=3D Index) {
[SAMI] Should this check be (mCpuMpData.FailedList[Index] =3D=3D ProcessorI= ndex) ?
+      Found =3D TRUE;
+      break;
+    }
+  }
+
+  /* If the CPU isn't already in the FailedList, add it */
+  if (!Found) {
+    mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] =3D Index;
+  }
+}
+
+/** Handles the StartupAllAPs case where the timeout has occurred.
+
+**/
+STATIC
+VOID
+ProcessStartupAllAPsTimeout (
+  VOID
+  )
+{
+  CPU_AP_DATA  *CpuData;
+  UINTN        Index;
+
+  if (mCpuMpData.FailedList =3D=3D NULL) {
+    return;
+  }
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    CpuData =3D &mCpuMpData.CpuData[Index];
[SAMI] Minor optimisation could be achieved if CpuData initialisation is do= ne just before calling AddProcessorToFailedList().
+    if (IsProcessorBSP (Index)) {
+      // Skip BSP
+      continue;
+    }
+
+    if (!IsProcessorEnabled (Index)) {
+      // Skip Disabled processors
+      continue;
+    }
+
+    AddProcessorToFailedList (Index, GetApState (CpuData));
+  }
+}
+
+/** Updates the status of the APs.
+
+   @param[in] ProcessorIndex The index of the AP to update.
+**/
+STATIC
+VOID
+UpdateApStatus (
+  IN UINTN ProcessorIndex
+  )
+{
+  EFI_STATUS   Status;
+  CPU_AP_DATA  *CpuData;
+  CPU_AP_DATA  *NextCpuData;
+  CPU_STATE    State;
+  UINTN        NextNumber;
+
+  CpuData =3D &mCpuMpData.CpuData[ProcessorIndex];
+
+  if (IsProcessorBSP (ProcessorIndex)) {
+    // Skip BSP
+    return;
+  }
+
+  if (!IsProcessorEnabled (ProcessorIndex)) {
+    // Skip Disabled processors
+    return;
+  }
+
+  State =3D GetApState (CpuData);
+
+  switch (State) {
+    case CpuStateFinished:
+      if (mCpuMpData.SingleThread) {
+        Status =3D GetNextBlockedNumber (&NextNumber);
+        if (!EFI_ERROR (Status)) {
+          NextCpuData =3D &mCpuMpData.CpuData[NextNumber];
+
+          NextCpuData->State =3D CpuStateReady;
+
+          SetApProcedure (
+            NextCpuData,
+            mCpuMpData.Procedure,
+            mCpuMpData.ProcedureArgument
+            );
+        }
+      }
+
+      CpuData->State =3D CpuStateIdle;
+      mCpuMpData.FinishCount++;
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  If a timeout is specified in StartupAllAps(), a timer is set, which invo=
kes
+  this procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   The WaitEvent the user supplied.
+  @param[in] Context The event context.
+**/
+STATIC
+VOID
+EFIAPI
+CheckAllAPsStatus (
+  IN  EFI_EVENT   Event,
+  IN  VOID        *Context
+  )
+{
+  UINTN  Index;
+
+  if (mCpuMpData.TimeoutActive) {
+    mCpuMpData.Timeout -=3D CalculateAndStallInterval (mCpuMpData.Timeout)=
;
+  }
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    UpdateApStatus (Index);
+  }
+
+  if (mCpuMpData.TimeoutActive && mCpuMpData.Timeout =3D=3D 0) {
+    ProcessStartupAllAPsTimeout ();
+
+    // Force terminal exit
+    mCpuMpData.FinishCount =3D mCpuMpData.StartCount;
+  }
+
+  if (mCpuMpData.FinishCount !=3D mCpuMpData.StartCount) {
+    return;
+  }
+
+  gBS->SetTimer (
+         mCpuMpData.CheckAllAPsEvent,
+         TimerCancel,
+         0
+         );
+
+  if (mCpuMpData.FailedListIndex =3D=3D 0) {
+    if (mCpuMpData.FailedList !=3D NULL) {
+      FreePool (mCpuMpData.FailedList);
+      mCpuMpData.FailedList =3D NULL;
+    }
+  }
+
+  gBS->SignalEvent (mCpuMpData.WaitEvent);
+}
+
+/** Invoked periodically via a timer to check the state of the processor.
+
+   @param Event   The event supplied by the timer expiration.
+   @param Context The processor context.
+
+**/
+STATIC
+VOID
+EFIAPI
+CheckThisAPStatus (
+  IN  EFI_EVENT   Event,
+  IN  VOID        *Context
+  )
+{
+  EFI_STATUS   Status;
+  CPU_AP_DATA  *CpuData;
+  CPU_STATE    State;
+
+  CpuData =3D Context;
+  CpuData->TimeTaken +=3D POLL_INTERVAL_US;
+
+  State =3D GetApState (CpuData);
+
+  if (State =3D=3D CpuStateFinished) {
+    Status =3D gBS->SetTimer (CpuData->CheckThisAPEvent, TimerCancel=
, 0);
+    ASSERT_EFI_ERROR (Status);
+
+    if (mCpuMpData.WaitEvent !=3D NULL) {
+      Status =3D gBS->SignalEvent (mCpuMpData.WaitEvent);
+      ASSERT_EFI_ERROR (Status);
+    }
+
+    CpuData->State =3D CpuStateIdle;
+  }
+
+  if (CpuData->TimeTaken > CpuData->Timeout) {
+    if (mCpuMpData.WaitEvent !=3D NULL) {
+      Status =3D gBS->SignalEvent (mCpuMpData.WaitEvent);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+}
+
+/**
+  This function is called by all processors (both BSP and AP) once and col=
lects
+  MP related data.
+
+  @param BSP            TRUE if the processor is the BSP.
+  @param Mpidr          The MPIDR for the specified processor.
+  @param ProcessorIndex The index of the processor.
+
+  @return EFI_SUCCESS if the data for the processor collected and filled i=
n.
+
+**/
+STATIC
+EFI_STATUS
+FillInProcessorInformation (
+  IN BOOLEAN      BSP,
+  IN UINTN        Mpidr,
+  IN UINTN        ProcessorIndex
+  )
+{
+  EFI_PROCESSOR_INFORMATION  *CpuInfo;
+
+  CpuInfo =3D &mCpuMpData.CpuData[ProcessorIndex].Info;
+
+  /* The MPIDR passed in may be a pseudo-MPIDR that's missing the bit 31 R=
ES1.
+   * Fix it so it's a proper MPIDR.
+   */
+  CpuInfo->ProcessorId =3D BIT31 | Mpidr;
[SAMI] I believe the MT, bit [24] may need to be set as well. Otherwise, th= e checks in MpInitLibWhoAm() would fail as ArmReadMpidr() would return this= bit as set if the PE is implemented using a multithreading type approach. 
+  CpuInfo->StatusFlag  =3D PROCESSOR_ENABLED_BIT | PROCE=
SSOR_HEALTH_STATUS_BIT;
+
+  if (BSP) {
+    CpuInfo->StatusFlag |=3D PROCESSOR_AS_BSP_BIT;
+  }
+
+  CpuInfo->Location.Package =3D GET_CLUSTER_ID (Mpidr);
+  CpuInfo->Location.Core    =3D GET_CORE_ID (Mpidr);
+  CpuInfo->Location.Thread  =3D 0;
[SAMI] I think the MPIDR_EL1.MT bit should be used to decide if Multi Threa= ding is supported and value for CpuInfo->Location.Thread should be set a= ppropriately. 
+
+  CpuInfo->ExtendedInformation.Location2.Package =3D 0;
+  CpuInfo->ExtendedInformation.Location2.Module  =3D 0;
+  CpuInfo->ExtendedInformation.Location2.Tile    =3D 0;
+  CpuInfo->ExtendedInformation.Location2.Die     =3D GET_CLUSTER_ID (Mp=
idr);
+  CpuInfo->ExtendedInformation.Location2.Core    =3D GET_CORE_ID (Mpidr=
);
+  CpuInfo->ExtendedInformation.Location2.Thread  =3D 0;
+
+  mCpuMpData.CpuData[ProcessorIndex].State =3D BSP ? CpuStateBusy : CpuSta=
teIdle;
+
+  mCpuMpData.CpuData[ProcessorIndex].Procedure =3D NULL;
+  mCpuMpData.CpuData[ProcessorIndex].Parameter =3D NULL;
+
+  return EFI_SUCCESS;
+}
+
+/** Initializes the MP Services system data
+
+   @param NumberOfProcessors The number of processors, both BSP and AP.
+   @param CpuInfo            CPU information gathered earlier during boot.
+
+**/
+VOID
+MpInitLibInitialize (
+  IN   UINTN     NumberOfProcessors,
+  IN   ARM_PROCESSOR_TABLE *CpuInfo
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  EFI_EVENT   ReadyToBootEvent;
+
+  //
+  // Clear the data structure area first.
+  //
+  ZeroMem (&mCpuMpData, sizeof (CPU_MP_DATA));
+  //
+  // First BSP fills and inits all known values, including its own records=
.
+  //
+  mCpuMpData.NumberOfProcessors =3D NumberOfProcessors;
+  mCpuMpData.NumberOfEnabledProcessors =3D NumberOfProcessors;
+
+  mCpuMpData.CpuData =3D AllocateZeroPool (
+                         mCpuMpData.NumberOfProcessors *
+                         sizeof (CPU_AP_DATA)
+                         );
+  ASSERT (mCpuMpData.CpuData !=3D NULL);
+
+  /* Allocate one extra for the NULL entry at the end */
+  gProcessorIDs =3D AllocatePool ((mCpuMpData.NumberOfProcessors + 1) * si=
zeof (UINT64));
+  ASSERT (gProcessorIDs !=3D NULL);
+
+  FillInProcessorInformation (TRUE, CpuInfo->ArmCpus[0].Mpidr, 0);
+  gProcessorIDs[0] =3D mCpuMpData.CpuData[0].Info.ProcessorId;
+
+  Status =3D gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  CheckAllAPsStatus,
+                  NULL,
+                  &mCpuMpData.CheckAllAPsEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  gApStacksBase =3D AllocatePool (
+                    mCpuMpData.NumberOfProcessors *
+                    gApStackSize
+                    );
+  ASSERT (gApStacksBase !=3D NULL);
+
+  for (Index =3D 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
+    if (IsProcessorBSP (Index)) {
+      /* Skip BSP */
+      continue;
+    }
+
+    FillInProcessorInformation (FALSE, CpuInfo->ArmCpus[Index].Mpidr, I=
ndex);
+
+    gProcessorIDs[Index] =3D mCpuMpData.CpuData[Index].Info.ProcessorId;
+
+    Status =3D gBS->CreateEvent (
+                    EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    CheckThisAPStatus,
+                    (VOID *)&mCpuMpData.CpuData[Index],
+                    &mCpuMpData.CpuData[Index].CheckThisAPEvent
+                    );
+    ASSERT (Status =3D=3D EFI_SUCCESS);
+  }
+
+  Status =3D EfiCreateEventReadyToBootEx (
+             TPL_CALLBACK,
+             ReadyToBootSignaled,
+             NULL,
+             &ReadyToBootEvent
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  gProcessorIDs[Index] =3D 0;
+}
+
+/**
+  Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOO=
T is
+  signaled. After this point, non-blocking mode is no longer allowed.
+
+  @param  Event     Event whose notification function is being invoked.
+  @param  Context   The pointer to the notification function's context,
+                    which is implementation-dependent.
+
+**/
+STATIC
+VOID
+EFIAPI
+ReadyToBootSignaled (
+  IN  EFI_EVENT                Event,
+  IN  VOID                     *Context
+  )
+{
+  mNonBlockingModeAllowed =3D FALSE;
+}
+
diff --git a/ArmPkg/Library/MpInitLib/InternalMpInitLib.h b/ArmPkg/Library/=
MpInitLib/InternalMpInitLib.h
new file mode 100644
index 000000000000..d08bb76246d7
--- /dev/null
+++ b/ArmPkg/Library/MpInitLib/InternalMpInitLib.h
@@ -0,0 +1,358 @@
+/** @file
+
+Copyright (c) 2021, NUVIA Inc. All rights reserved.<BR>
+Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR&g=
t;
+Portions copyright (c) 2011, Apple Inc. All rights reserved.
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MP_INTERNAL_INIT_LIB_H_
+#define MP_INTERNAL_INIT_LIB_H_
+
+#include <Protocol/Cpu.h>
+#include <Protocol/MpService.h>
+
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+
+
+#define AP_STACK_SIZE 0x1000
+
+//
+// Internal Data Structures
+//
+
+//
+// AP state
+//
+// The state transitions for an AP when it process a procedure are:
+//  Idle ----> Ready ----> Busy ----> Idle
+//       [BSP]       [AP]       [AP]
+//
+typedef enum {
+  CpuStateIdle,
+  CpuStateReady,
+  CpuStateBlocked,
+  CpuStateBusy,
+  CpuStateFinished,
+  CpuStateDisabled
+} CPU_STATE;
+
+//
+// Define Individual Processor Data block.
+//
+typedef struct {
+  EFI_PROCESSOR_INFORMATION    Info;
+  EFI_AP_PROCEDURE             Procedure;
+  VOID                         *Parameter;
+  CPU_STATE                    State;
+  EFI_EVENT                    CheckThisAPEvent;
+  UINTN                        Timeout;
+  UINTN                        TimeTaken;
+} CPU_AP_DATA;
+
+//
+// Define MP data block which consumes individual processor block.
+//
+typedef struct {
+  UINTN                   NumberOfProcessors;
+  UINTN                   NumberOfEnabledProcessors;
+  EFI_EVENT               CheckAllAPsEvent;
+  EFI_EVENT               WaitEvent;
+  UINTN                   FinishCount;
+  UINTN                   StartCount;
+  EFI_AP_PROCEDURE        Procedure;
+  VOID                    *ProcedureArgument;
+  BOOLEAN                 SingleThread;
+  UINTN                   StartedNumber;
+  CPU_AP_DATA             *CpuData;
+  UINTN                   Timeout;
+  UINTN                   *FailedList;
+  UINTN                   FailedListIndex;
+  BOOLEAN                 TimeoutActive;
+} CPU_MP_DATA;
+
+EFI_STATUS
+EFIAPI
+CpuMpServicesInit (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  );
+
+extern EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate;
+
+/** Secondary core entry point.
+
+**/
+VOID  ApEntryPoint (
+  VOID
+  );
+
+/** C entry-point for the AP.
+    This function gets called from the assembly function ApEntryPoint.
+**/
+VOID
+ApProcedure (
+  VOID
+  );
+
+/** Turns on the specified core using PSCI and executes the user-supplied
+    function that's been configured via a previous call to SetApProcedure.
+
+   @param ProcessorIndex The index of the core to turn on.
+
+   @retval EFI_SUCCESS       The processor was successfully turned on.
+   @retval EFI_DEVICE_ERROR  An error occurred turning the processor on.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+DispatchCpu (
+  IN UINTN ProcessorIndex
+  );
+
+/** Returns whether the specified processor is the BSP.
+
+   @param[in] ProcessorIndex The index the processor to check.
+
+   @return TRUE if the processor is the BSP, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorBSP (
+  UINTN ProcessorIndex
+  );
+
+/** Returns whether the processor executing this function is the BSP.
+
+   @return Whether the current processor is the BSP.
+**/
+STATIC
+BOOLEAN
+IsCurrentProcessorBSP (
+  VOID
+  );
+
+/** Returns whether the specified processor is enabled.
+
+   @param[in] ProcessorIndex The index of the processor to check.
+
+   @return TRUE if the processor is enabled, FALSE otherwise.
+**/
+STATIC
+BOOLEAN
+IsProcessorEnabled (
+  UINTN ProcessorIndex
+  );
+
+/** Configures the processor context with the user-supplied procedure and
+    argument.
+
+   @param CpuData           The processor context.
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+
+**/
+STATIC
+VOID
+SetApProcedure (
+  IN   CPU_AP_DATA         *CpuData,
+  IN   EFI_AP_PROCEDURE    Procedure,
+  IN   VOID                *ProcedureArgument
+  );
+
+/**
+  Get the Application Processors state.
+
+  @param[in]  CpuData    The pointer to CPU_AP_DATA of specified AP
+
+  @return  The AP status
+**/
+CPU_STATE
+GetApState (
+  IN  CPU_AP_DATA     *CpuData
+  );
+
+/** Returns the index of the next processor that is blocked.
+
+   @param[out] NextNumber The index of the next blocked processor.
+
+   @retval EFI_SUCCESS   Successfully found the next blocked processor.
+   @retval EFI_NOT_FOUND There are no blocked processors.
+
+**/
+STATIC
+EFI_STATUS
+GetNextBlockedNumber (
+  OUT UINTN *NextNumber
+  );
+
+/** Stalls the BSP for the minimum of gPollInterval and Timeout.
+
+   @param[in]  Timeout    The time limit in microseconds remaining for
+                          APs to return from Procedure.
+
+   @retval     StallTime  Time of execution stall.
+**/
+STATIC
+UINTN
+CalculateAndStallInterval (
+  IN UINTN Timeout
+  );
+
+/** Returns whether all processors are in the idle state.
+
+   @return Whether all the processors are idle.
+
+**/
+STATIC
+BOOLEAN
+CheckAllCpusReady (
+  VOID
+  );
+
+/** Sets up the state for the StartupAllAPs function.
+
+   @param SingleThread Whether the APs will execute sequentially.
+
+**/
+STATIC
+VOID
+StartupAllAPsPrepareState (
+  IN BOOLEAN SingleThread
+  );
+
+/** Handles execution of StartupAllAPs when a WaitEvent has been specified=
.
+
+   @param Procedure         The user-supplied procedure.
+   @param ProcedureArgument The user-supplied procedure argument.
+   @param WaitEvent         The wait event to be signaled when the work is
+                            complete or a timeout has occurred.
+   @param TimeoutInMicroseconds The timeout for the work to be completed. =
Zero
+                                indicates an infinite timeout.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsWithWaitEvent (
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *ProcedureArgument,
+  IN EFI_EVENT        WaitEvent,
+  IN UINTN            TimeoutInMicroseconds
+  );
+
+/** Handles execution of StartupAllAPs when no wait event has been specifi=
ed.
+
+   @param Procedure             The user-supplied procedure.
+   @param ProcedureArgument     The user-supplied procedure argument.
+   @param TimeoutInMicroseconds The timeout for the work to be completed. =
Zero
+                                indicates an infinite timeout.
+   @param SingleThread          Whether the APs will execute sequentially.
+   @param FailedCpuList         User-supplied pointer for list of failed C=
PUs.
+
+   @return EFI_SUCCESS on success.
+**/
+STATIC
+EFI_STATUS
+StartupAllAPsNoWaitEvent (
+  IN EFI_AP_PROCEDURE Procedure,
+  IN VOID             *ProcedureArgument,
+  IN UINTN            TimeoutInMicroseconds,
+  IN BOOLEAN          SingleThread,
+  IN UINTN            **FailedCpuList
+  );
+
+
+/** Adds the specified processor the list of failed processors.
+
+   @param ProcessorIndex The processor index to add.
+   @param ApState         Processor state.
+
+**/
+STATIC
+VOID
+AddProcessorToFailedList (
+  UINTN           ProcessorIndex,
+  CPU_STATE       ApState
+  );
+
+/** Handles the StartupAllAPs case where the timeout has occurred.
+
+**/
+STATIC
+VOID
+ProcessStartupAllAPsTimeout (
+  VOID
+  );
+
+/**
+  If a timeout is specified in StartupAllAps(), a timer is set, which invo=
kes
+  this procedure periodically to check whether all APs have finished.
+
+  @param[in] Event   The WaitEvent the user supplied.
+  @param[in] Context The event context.
+**/
+STATIC
+VOID
+EFIAPI
+CheckAllAPsStatus (
+  IN  EFI_EVENT   Event,
+  IN  VOID        *Context
+  );
+
+/** Invoked periodically via a timer to check the state of the processor.
+
+   @param Event   The event supplied by the timer expiration.
+   @param Context The processor context.
+
+**/
+STATIC
+VOID
+EFIAPI
+CheckThisAPStatus (
+  IN  EFI_EVENT   Event,
+  IN  VOID        *Context
+  );
+
+/**
+  This function is called by all processors (both BSP and AP) once and col=
lects
+  MP related data.
+
+  @param BSP            TRUE if the processor is the BSP.
+  @param Mpidr          The MPIDR for the specified processor.
+  @param ProcessorIndex The index of the processor.
+
+  @return EFI_SUCCESS if the data for the processor collected and filled i=
n.
+
+**/
+STATIC
+EFI_STATUS
+FillInProcessorInformation (
+  IN BOOLEAN      BSP,
+  IN UINTN        Mpidr,
+  IN UINTN        ProcessorIndex
+  );
+
+/**
+  Event notification function called when the EFI_EVENT_GROUP_READY_TO_BOO=
T is
+  signaled. After this point, non-blocking mode is no longer allowed.
+
+  @param  Event     Event whose notification function is being invoked.
+  @param  Context   The pointer to the notification function's context,
+                    which is implementation-dependent.
+
+**/
+STATIC
+VOID
+EFIAPI
+ReadyToBootSignaled (
+  IN  EFI_EVENT                Event,
+  IN  VOID                     *Context
+  );
+
+
+#endif /* MP_INTERNAL_INIT_LIB_H_ */
diff --git a/ArmVirtPkg/ArmVirt.dsc.inc b/ArmVirtPkg/ArmVirt.dsc.inc
index 5a1598d90ca7..af6e48fd6cfc 100644
--- a/ArmVirtPkg/ArmVirt.dsc.inc
+++ b/ArmVirtPkg/ArmVirt.dsc.inc
@@ -261,6 +261,9 @@
 [LibraryClasses.ARM]
   ArmSoftFloatLib|ArmPkg/Library/ArmSoftFloatLib/ArmSoftFloatLib.inf
=20
+[LibraryClasses.AARCH64]
+  MpInitLib|ArmPkg/Library/MpInitLib/DxeMpInitLib.inf
+
 [BuildOptions]
   RVCT:RELEASE_*_*_CC_FLAGS  =3D -DMDEPKG_NDEBUG
=20

IMPORTANT NOTICE: The contents of this email and any attachments are confid= ential and may also be privileged. If you are not the intended recipient, p= lease notify the sender immediately and do not disclose the contents to any= other person, use it for any purpose, or store or copy the information in any medium. Thank you. --------------F6A6CAB149BAC3BDAA6CE911--