From 5d9306045872724afcbb29e06653fb49f2cc34a8 Mon Sep 17 00:00:00 2001
From: Axel Heider <axel.heider@hensoldt.net>
Date: Fri, 31 Mar 2023 17:24:21 +0200
Subject: [PATCH] vm_arm: helper function vmm_init_iommu()

Signed-off-by: Axel Heider <axel.heider@hensoldt.net>
---
 components/VM_Arm/src/main.c | 80 ++++++++++++++++++++++++------------
 1 file changed, 53 insertions(+), 27 deletions(-)

diff --git a/components/VM_Arm/src/main.c b/components/VM_Arm/src/main.c
index ff65e211..8e0e8891 100644
--- a/components/VM_Arm/src/main.c
+++ b/components/VM_Arm/src/main.c
@@ -525,6 +525,55 @@ static int vmm_init(const vm_config_t *vm_config)
     return 0;
 }
 
+static int vm_init_iommu(vm_t *vm)
+{
+
+#if defined(CONFIG_ARM_SMMU)
+
+    int err;
+    /* configure the smmu */
+    ZF_LOGD("Getting sid and cb caps");
+    seL4_CPtr cb_cap = camkes_get_smmu_cb_cap();
+    seL4_CPtr sid_cap = camkes_get_smmu_sid_cap();
+
+    ZF_LOGD("Assigning vspace to context bank");
+    err = seL4_ARM_CB_AssignVspace(cb_cap, vspace_get_root(&vm.mem.vm_vspace));
+    if (err) {
+        ZF_LOGE("Failed to assign vspace to CB (%d)");
+        return -1;
+    }
+
+    ZF_LOGD("Binding stream id to context bank");
+    err = seL4_ARM_SID_BindCB(sid_cap, cb_cap);
+    if (err) {
+        ZF_LOGE("Failed to bind CB to SID (%d)");
+        return -1;
+    }
+
+#elif defined(CONFIG_TK1_SMMU)
+
+    int err;
+    /* install any iospaces */
+    int iospace_caps;
+    err = simple_get_iospace_cap_count(&_simple, &iospace_caps);
+    if (err) {
+        ZF_LOGE("Failed to get iospace count (%d)");
+        return -1;
+    }
+    for (int i = 0; i < iospace_caps; i++) {
+        seL4_CPtr iospace = simple_get_nth_iospace_cap(&_simple, i);
+        err = vm_guest_add_iospace(&vm, &_vspace, iospace);
+        if (err) {
+            ZF_LOGE("Failed to add iospace (%d)");
+            return -1;
+        }
+    }
+
+#endif /* platform specific IOMMU init */
+
+    return 0;
+}
+
 void restart_component(void)
 {
     longjmp(restart_jmp_buf, 1);
@@ -1133,35 +1182,12 @@ static int main_continued(void)
     assert(!err);
     err = vm_register_notification_callback(&vm, handle_async_event, NULL);
     assert(!err);
-#ifdef CONFIG_TK1_SMMU
-    /* install any iospaces */
-    int iospace_caps;
-    err = simple_get_iospace_cap_count(&_simple, &iospace_caps);
+
+    err = vm_init_iommu(&vm);
     if (err) {
-        ZF_LOGF("Failed to get iospace count");
-    }
-    for (int i = 0; i < iospace_caps; i++) {
-        seL4_CPtr iospace = simple_get_nth_iospace_cap(&_simple, i);
-        err = vm_guest_add_iospace(&vm, &_vspace, iospace);
-        if (err) {
-            ZF_LOGF("Failed to add iospace");
-        }
+        ZF_LOGE("Failed to initialise IO-MMU");
+        return err;
     }
-#endif /* CONFIG_TK1_SMMU */
-#ifdef CONFIG_ARM_SMMU
-    /* configure the smmu */
-    ZF_LOGD("Getting sid and cb caps");
-    seL4_CPtr cb_cap = camkes_get_smmu_cb_cap();
-    seL4_CPtr sid_cap = camkes_get_smmu_sid_cap();
-
-    ZF_LOGD("Assigning vspace to context bank");
-    err = seL4_ARM_CB_AssignVspace(cb_cap, vspace_get_root(&vm.mem.vm_vspace));
-    ZF_LOGF_IF(err, "Failed to assign vspace to CB");
-
-    ZF_LOGD("Binding stream id to context bank");
-    err = seL4_ARM_SID_BindCB(sid_cap, cb_cap);
-    ZF_LOGF_IF(err, "Failed to bind CB to SID");
-#endif /* CONFIG_ARM_SMMU */
 
     err = vm_create_default_irq_controller(&vm);
     assert(!err);