From 4692c722560ba9094c3e26e13ab57ea10bf7cb74 Mon Sep 17 00:00:00 2001
From: Alex Pavey <Alex.Pavey@dornerworks.com>
Date: Tue, 9 Nov 2021 15:27:28 -0500
Subject: [PATCH] smc_cap: Allow SMC forwarding, if configured

Implemented a custom SMC call handler. It will check for a capability
associated with the SMC function ID called from the VM. If it exists,
then the SMC is forwarded to the microkernel so it can make the actual
SMC call. If it does not exist then the default SMC handler is called.
This allows PSCI emulation to continue working.

Register this new function as the VM's smc_handler_callback.

Signed-off-by: Robbie VanVossen <robert.vanvossen@dornerworks.com>
---
 components/VM_Arm/src/main.c | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/components/VM_Arm/src/main.c b/components/VM_Arm/src/main.c
index 4f98a6e8..cf591e4d 100644
--- a/components/VM_Arm/src/main.c
+++ b/components/VM_Arm/src/main.c
@@ -48,6 +48,7 @@
 #include <sel4vmmplatsupport/arch/guest_reboot.h>
 #include <sel4vmmplatsupport/arch/guest_vcpu_fault.h>
 #include <sel4vmmplatsupport/guest_vcpu_util.h>
+#include <sel4vmmplatsupport/arch/smc.h>
 
 #include <sel4utils/process.h>
 #include <sel4utils/irq_server.h>
@@ -127,6 +128,10 @@ seL4_CPtr camkes_get_smmu_cb_cap();
 seL4_CPtr camkes_get_smmu_sid_cap();
 #endif
 
+#ifdef CONFIG_ALLOW_SMC_CALLS
+seL4_CPtr camkes_get_smc_cap(seL4_Word smc_call);
+#endif
+
 int get_crossvm_irq_num(void)
 {
     return free_plat_interrupts[0];
@@ -1141,6 +1146,20 @@ memory_fault_result_t unhandled_mem_fault_callback(vm_t *vm, vm_vcpu_t *vcpu,
     return FAULT_ERROR;
 }
 
+#ifdef CONFIG_ALLOW_SMC_CALLS
+int vm_smc_handler(vm_vcpu_t *vcpu, seL4_UserContext *regs)
+{
+    int err;
+    seL4_Word id = smc_get_function_id(regs);
+    seL4_ARM_SMC smc_cap = camkes_get_smc_cap(id);
+    if (smc_cap) {
+        return smc_forward(vcpu, regs, smc_cap);
+    } else {
+        return vm_smc_handle_default(vcpu, regs);
+    }
+}
+#endif
+
 static int main_continued(void)
 {
     vm_t vm;
@@ -1233,6 +1252,11 @@ static int main_continued(void)
     err = vm_create_default_irq_controller(&vm);
     assert(!err);
 
+#ifdef CONFIG_ALLOW_SMC_CALLS
+    err = vm_register_smc_handler_callback(&vm, vm_smc_handler);
+    assert(!err);
+#endif
+
     /* Create CPUs and DTB node */
     for (int i = 0; i < NUM_VCPUS; i++) {
         vm_vcpu_t *new_vcpu = create_vmm_plat_vcpu(&vm, VM_PRIO - 1);