[PATCH 5/7] 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, implemnt 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 | 111 ++++++++++++++++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c        |  36 +++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf      |  36 +++++++
 ArmVirtPkg/Library/ArmPlatformLibQemu/IdMap.S                     |  57 ++++++++++
 4 files changed, 240 insertions(+)

diff --git a/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S b/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S
new file mode 100644
index 000000000000..7b78e2928710
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/AArch64/ArmPlatformHelper.S
@@ -0,0 +1,111 @@
+//
+//  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)
+  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..4a5ea82ef5e2
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.c
@@ -0,0 +1,36 @@
+/** @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>
+
+EFI_BOOT_MODE
+ArmPlatformGetBootMode (
+  VOID
+  )
+{
+  return BOOT_WITH_FULL_CONFIGURATION;
+}
+
+RETURN_STATUS
+ArmPlatformInitialize (
+  IN  UINTN  MpId
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+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..d2eaa6be6586
--- /dev/null
+++ b/ArmVirtPkg/Library/ArmPlatformLibQemu/ArmPlatformLibQemu.inf
@@ -0,0 +1,36 @@
+#/* @file
+#  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..9c174bb81dc2
--- /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
+
+                       /* level 0 */
+idmap: .quad           1f + TT_TYPE_TABLE
+       .fill           511, 8, 0x0
+
+                       /* level 1 */
+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
+
+                       /* level 2 */
+20:    .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
+
+                       /* level 2 */
+21:    .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
+
+                       /* level 3 */
+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