[PATCH v3 10/16] ArmVirtPkg/ArmVirtQemu: implement ArmPlatformLib with static ID map

["Ard Biesheuvel" <ardb@kernel.org>]

To substantially reduce the amount of processing that takes place with
the MMU and caches off, implement a version of ArmPlatformLib specific
for QEMU/mach-virt in AArch64 mode that carries a statically allocated
and populated ID map that covers the NOR flash and device region, and
128 MiB of DRAM at the base of memory (0x4000_0000).

Note that 128 MiB has always been the minimum amount of DRAM we support
for this configuration, and the existing code already ASSERT()s in DEBUG
mode when booting with less.

Signed-off-by: Ard Biesheuvel <ardb@kernel.org>
---
 ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S | 115 ++++++++++++++++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c        |  64 +++++++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf      |  40 +++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/IdMap.S                     |  57 ++++++++++
 4 files changed, 276 insertions(+)

diff --git a/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S b/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S
new file mode 100644
index 000000000000..05ccc7f9f043
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S
@@ -0,0 +1,115 @@
+//
+//  Copyright (c) 2022, Google LLC. All rights reserved.
+//
+//  SPDX-License-Identifier: BSD-2-Clause-Patent
+//
+//
+
+#include <AsmMacroIoLibV8.h>
+
+  .macro mov_i, reg:req, imm:req
+  movz   \reg, :abs_g3:\imm
+  movk   \reg, :abs_g2_nc:\imm
+  movk   \reg, :abs_g1_nc:\imm
+  movk   \reg, :abs_g0_nc:\imm
+  .endm
+
+ .set    MAIR_DEV_nGnRnE,     0x00
+ .set    MAIR_MEM_NC,         0x44
+ .set    MAIR_MEM_WT,         0xbb
+ .set    MAIR_MEM_WBWA,       0xff
+ .set    mairval, MAIR_DEV_nGnRnE | (MAIR_MEM_NC << 8) | (MAIR_MEM_WT << 16) | (MAIR_MEM_WBWA << 24)
+
+ .set    TCR_TG0_4KB,         0x0 << 14
+ .set    TCR_TG1_4KB,         0x2 << 30
+ .set    TCR_IPS_SHIFT,       32
+ .set    TCR_EPD1,            0x1 << 23
+ .set    TCR_SH_INNER,        0x3 << 12
+ .set    TCR_RGN_OWB,         0x1 << 10
+ .set    TCR_RGN_IWB,         0x1 << 8
+ .set    tcrval, TCR_TG0_4KB | TCR_TG1_4KB | TCR_EPD1 | TCR_RGN_OWB
+ .set    tcrval, tcrval | TCR_RGN_IWB | TCR_SH_INNER
+
+ .set    SCTLR_ELx_I,         0x1 << 12
+ .set    SCTLR_ELx_SA,        0x1 << 3
+ .set    SCTLR_ELx_C,         0x1 << 2
+ .set    SCTLR_ELx_M,         0x1 << 0
+ .set    SCTLR_EL1_SPAN,      0x1 << 23
+ .set    SCTLR_EL1_WXN,       0x1 << 19
+ .set    SCTLR_EL1_SED,       0x1 << 8
+ .set    SCTLR_EL1_ITD,       0x1 << 7
+ .set    SCTLR_EL1_RES1,      (0x1 << 11) | (0x1 << 20) | (0x1 << 22) | (0x1 << 28) | (0x1 << 29)
+ .set    sctlrval, SCTLR_ELx_M | SCTLR_ELx_C | SCTLR_ELx_SA | SCTLR_EL1_ITD | SCTLR_EL1_SED
+ .set    sctlrval, sctlrval | SCTLR_ELx_I | SCTLR_EL1_SPAN | SCTLR_EL1_RES1
+
+
+ASM_FUNC(ArmPlatformPeiBootAction)
+  mrs    x0, CurrentEL           // check current exception level
+  tbz    x0, #3, 0f              // bail if above EL1
+  ret
+
+0:mov_i  x0, mairval
+  mov_i  x1, tcrval
+  adrp   x2, idmap
+  orr    x2, x2, #0xff << 48     // set non-zero ASID
+  mov_i  x3, sctlrval
+
+  mrs    x6, id_aa64mmfr0_el1    // get the supported PA range
+  and    x6, x6, #0xf            // isolate PArange bits
+  cmp    x6, #6                  // 0b0110 == 52 bits
+  sub    x6, x6, #1              // subtract 1
+  cinc   x6, x6, ne              // add back 1 unless PArange == 52 bits
+  bfi    x1, x6, #32, #3         // copy updated PArange into TCR_EL1.IPS
+
+  cmp    x6, #3                  // 0b0011 == 42 bits
+  sub    x6, x6, #1              // subtract 1
+  cinc   x6, x6, lt              // add back 1 unless VA range >= 42
+
+  mov    x7, #32
+  sub    x6, x7, x6, lsl #2      // T0SZ for PArange != 42
+  mov    x7, #64 - 42            // T0SZ for PArange == 42
+  csel   x6, x6, x7, ne
+  orr    x1, x1, x6              // set T0SZ field in TCR
+
+  cmp    x6, #64 - 40            // VA size < 40 bits?
+  add    x4, x2, #0x1000         // advance to level 1 descriptor
+  csel   x2, x4, x2, gt
+
+  msr    mair_el1, x0            // set up the 1:1 mapping
+  msr    tcr_el1, x1
+  msr    ttbr0_el1, x2
+  isb
+
+  tlbi   vmalle1                 // invalidate any cached translations
+  ic     iallu                   // invalidate the I-cache
+  dsb    nsh
+  isb
+
+  msr    sctlr_el1, x3           // enable MMU and caches
+  isb
+  ret
+
+//UINTN
+//ArmPlatformGetCorePosition (
+//  IN UINTN MpId
+//  );
+// With this function: CorePos = (ClusterId * 4) + CoreId
+ASM_FUNC(ArmPlatformGetCorePosition)
+  mov   x0, xzr
+  ret
+
+//UINTN
+//ArmPlatformGetPrimaryCoreMpId (
+//  VOID
+//  );
+ASM_FUNC(ArmPlatformGetPrimaryCoreMpId)
+  MOV32  (w0, FixedPcdGet32 (PcdArmPrimaryCore))
+  ret
+
+//UINTN
+//ArmPlatformIsPrimaryCore (
+//  IN UINTN MpId
+//  );
+ASM_FUNC(ArmPlatformIsPrimaryCore)
+  mov   x0, #1
+  ret
diff --git a/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c
new file mode 100644
index 000000000000..1de80422ee4c
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c
@@ -0,0 +1,64 @@
+/** @file
+
+  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Library/ArmLib.h>
+#include <Library/ArmPlatformLib.h>
+
+/**
+  Return the current Boot Mode.
+
+  This function returns the boot reason on the platform
+
+  @return   Return the current Boot Mode of the platform
+
+**/
+EFI_BOOT_MODE
+ArmPlatformGetBootMode (
+  VOID
+  )
+{
+  return BOOT_WITH_FULL_CONFIGURATION;
+}
+
+/**
+  Initialize controllers that must setup in the normal world.
+
+  This function is called by the ArmPlatformPkg/PrePi or
+  ArmPlatformPkg/PlatformPei in the PEI phase.
+
+  @param[in]     MpId               ID of the calling CPU
+
+  @return        RETURN_SUCCESS unless the operation failed
+**/
+RETURN_STATUS
+ArmPlatformInitialize (
+  IN  UINTN  MpId
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  Return the Platform specific PPIs.
+
+  This function exposes the Platform Specific PPIs. They can be used by any
+  PrePi modules or passed to the PeiCore by PrePeiCore.
+
+  @param[out]   PpiListSize         Size in Bytes of the Platform PPI List
+  @param[out]   PpiList             Platform PPI List
+
+**/
+VOID
+ArmPlatformGetPlatformPpiList (
+  OUT UINTN                   *PpiListSize,
+  OUT EFI_PEI_PPI_DESCRIPTOR  **PpiList
+  )
+{
+  *PpiListSize = 0;
+  *PpiList     = NULL;
+}
diff --git a/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf
new file mode 100644
index 000000000000..b2ecdfa061cb
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf
@@ -0,0 +1,40 @@
+## @file
+#  ArmPlatformLib implementation for QEMU/mach-virt on AArch64 that contains a
+#  statically allocated 1:1 mapping of the first 128 MiB of DRAM, as well as
+#  the NOR flash and the device region
+#
+#  Copyright (c) 2011-2012, ARM Limited. All rights reserved.
+#  Copyright (c) 2022, Google LLC. All rights reserved.
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 1.27
+  BASE_NAME                      = ArmPlatformLibQemu
+  FILE_GUID                      = 40af3a25-f02c-4aef-94ef-7ac0282d21d4
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ArmPlatformLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+
+[LibraryClasses]
+  ArmLib
+  DebugLib
+
+[Sources.common]
+  ArmPlatformLibQemu.c
+  IdMap.S
+
+[Sources.AArch64]
+  AArch64/ArmPlatformHelper.S
+
+[FixedPcd]
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
diff --git a/ArmVirtPkg/Library/ArmPlatformLibQemu/IdMap.S b/ArmVirtPkg/Library/ArmPlatformLibQemu/IdMap.S
new file mode 100644
index 000000000000..4a4b7b77ed83
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/IdMap.S
@@ -0,0 +1,57 @@
+// SPDX-License-Identifier: BSD-2-Clause-Patent
+// Copyright 2022 Google LLC
+// Author: Ard Biesheuvel <ardb@google.com>
+
+  .set      TT_TYPE_BLOCK, 0x1
+  .set      TT_TYPE_PAGE,  0x3
+  .set      TT_TYPE_TABLE, 0x3
+
+  .set      TT_AF, 0x1 << 10
+  .set      TT_NG, 0x1 << 11
+  .set      TT_RO, 0x2 << 6
+  .set      TT_XN, 0x3 << 53
+
+  .set      TT_MT_DEV, 0x0 << 2                 // MAIR #0
+  .set      TT_MT_MEM, (0x3 << 2) | (0x3 << 8)  // MAIR #3
+
+  .set      PAGE_XIP,  TT_TYPE_PAGE  | TT_MT_MEM | TT_AF | TT_RO | TT_NG
+  .set      BLOCK_DEV, TT_TYPE_BLOCK | TT_MT_DEV | TT_AF | TT_XN | TT_NG
+  .set      BLOCK_MEM, TT_TYPE_BLOCK | TT_MT_MEM | TT_AF | TT_XN | TT_NG
+
+  .globl    idmap
+  .section  ".rodata.idmap", "a", %progbits
+  .align    12
+
+idmap:      /* level 0 */
+  .quad     1f + TT_TYPE_TABLE
+  .fill     511, 8, 0x0
+
+1:          /* level 1 */
+  .quad     20f + TT_TYPE_TABLE           // 1 GB of flash and device mappings
+  .quad     21f + TT_TYPE_TABLE           // up to 1 GB of DRAM
+  .fill     510, 8, 0x0                   // 510 GB of remaining VA space
+
+20:         /* level 2 */
+  .quad     3f + TT_TYPE_TABLE            // up to 2 MB of flash
+  .fill     63, 8, 0x0                    // 126 MB of unused flash
+  .set      idx, 64
+  .rept     448
+  .quad     BLOCK_DEV | (idx << 21)       // 896 MB of RW- device mappings
+  .set      idx, idx + 1
+  .endr
+
+21:         /* level 2 */
+  .set      idx, 0x40000000 >> 21
+  .rept     64
+  .quad     BLOCK_MEM | (idx << 21)       // 128 MB of RW- memory mappings
+  .set      idx, idx + 1
+  .endr
+  .fill     448, 8, 0x0
+
+3:          /* level 3 */
+  .quad     0x0                           // omit first 4k page
+  .set      idx, 1
+  .rept     511
+  .quad     PAGE_XIP | (idx << 12)        // 2044 KiB of R-X flash mappings
+  .set      idx, idx + 1
+  .endr
-- 
2.35.1