From 4a15661ee0e25616f89cc85e837c7f8e49bda671 Mon Sep 17 00:00:00 2001
From: Peter Collingbourne <pcc@google.com>
Date: Wed, 30 Mar 2022 11:46:03 -0700
Subject: [PATCH] [arch][arm64] use SMC when booted at EL2 in QEMU

If we were booted at EL2 (e.g. when passing -machine
virt,virtualization=on), we need to use SMC instead of HVC for PSCI
calls. Change psci_call() to do this and add a flag to do-qemuarm to
allow testing this scenario.
---
 arch/arm64/start.S                      | 11 +++++++++++
 platform/qemu-virt-arm/secondary_boot.S | 10 +++++++++-
 scripts/do-qemuarm                      |  8 +++++++-
 3 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/arch/arm64/start.S b/arch/arm64/start.S
index f9abc460..9055d1b6 100644
--- a/arch/arm64/start.S
+++ b/arch/arm64/start.S
@@ -27,11 +27,15 @@ paddr                   .req x24
 mapping_size            .req x25
 size                    .req x26
 attr                    .req x27
+boot_el                 .req x28
 
 .section .text.boot
 FUNCTION(_start)
 .globl arm_reset
 arm_reset:
+    /* keep track of the boot EL */
+    mrs     boot_el, currentel
+
     /* if we came in at higher than EL1, drop down to EL1 */
     bl      arm64_elX_to_el1
 
@@ -76,6 +80,10 @@ arm_reset:
     stp     x0, x1, [tmp], #16
     stp     x2, x3, [tmp]
 
+    /* save the boot EL */
+    adrp    tmp, arm64_boot_el
+    str     boot_el, [tmp, #:lo12:arm64_boot_el]
+
 #if WITH_KERNEL_VM
     /* walk through all the entries in the translation table, setting them up */
     mov     tmp, #0
@@ -413,6 +421,9 @@ arm_reset:
 LOCAL_DATA(arm64_boot_args)
     .skip (4 * 8)
 END_DATA(arm64_boot_args)
+DATA(arm64_boot_el)
+    .skip 8
+END_DATA(arm64_boot_el)
 
 #if WITH_SMP
 .data
diff --git a/platform/qemu-virt-arm/secondary_boot.S b/platform/qemu-virt-arm/secondary_boot.S
index 59bec5ac..9ba1d05b 100644
--- a/platform/qemu-virt-arm/secondary_boot.S
+++ b/platform/qemu-virt-arm/secondary_boot.S
@@ -12,10 +12,18 @@
 
 /* used to call into PSCI firmware (Power State Coordination Firmware) */
 FUNCTION(psci_call)
-    hvc     #0
 #if ARCH_ARM
+    hvc     #0
     bx      lr
 #else
+    /* If booted at EL2 (bit 3 set of boot EL), we need to use SMC instead of HVC. */
+    adrp    x9, arm64_boot_el
+    ldr     x9, [x9, #:lo12:arm64_boot_el]
+    tbnz    x9, #3, .Lsmc
+    hvc     #0
+    ret
+.Lsmc:
+    smc     #0
     ret
 #endif
 
diff --git a/scripts/do-qemuarm b/scripts/do-qemuarm
index d1d2e1cf..71248921 100755
--- a/scripts/do-qemuarm
+++ b/scripts/do-qemuarm
@@ -12,6 +12,7 @@ function HELP {
     echo "-d a virtio display"
     echo "-3 cortex-m3 based platform"
     echo "-6 64bit arm"
+    echo "-v boot kernel at EL2"
     echo "-m <memory in MB>"
     echo "-s <number of cpus>"
     echo "-h for help"
@@ -23,6 +24,7 @@ DO_NET=0
 DO_NET_TAP=0
 DO_BLOCK=0
 DO_64BIT=0
+DO_VIRT=0
 DO_CORTEX_M3=0
 DO_DISPLAY=0
 DO_CMPCTMALLOC=0
@@ -32,7 +34,7 @@ MEMSIZE=512
 SUDO=""
 PROJECT=""
 
-while getopts bdhm:cMnt36p:s: FLAG; do
+while getopts bdhm:cMnt36vp:s: FLAG; do
     case $FLAG in
         b) DO_BLOCK=1;;
         c) DO_CMPCTMALLOC=1;;
@@ -42,6 +44,7 @@ while getopts bdhm:cMnt36p:s: FLAG; do
         t) DO_NET_TAP=1;;
         3) DO_CORTEX_M3=1;;
         6) DO_64BIT=1;;
+        v) DO_VIRT=1;;
         m) MEMSIZE=$OPTARG;;
         s) SMP=$OPTARG;;
         p) PROJECT=$OPTARG;;
@@ -58,6 +61,9 @@ if [ $DO_64BIT == 1 ]; then
     QEMU="qemu-system-aarch64"
     CPU="cortex-a53"
     MACHINE="virt"
+    if [ $DO_VIRT == 1 ]; then
+      MACHINE+=",virtualization=on"
+    fi
     _PROJECT="qemu-virt-arm64-test"
 elif [ $DO_CORTEX_M3 == 1 ]; then
     QEMU="qemu-system-arm"