[RFC v2 8/8] xen/arm: add SCI mediator support for DomUs

[Oleksii Moisieiev <Oleksii_Moisieiev@xxxxxxxx>]

Integration of the SCMI mediator with xen libs:
- add hypercalls to aquire SCI channel and set device permissions
for DomUs;
- add SCMI_SMC nodes to DomUs device-tree based on partial device-tree;
- SCI requests redirection from DomUs to Firmware.

Signed-off-by: Oleksii Moisieiev <oleksii_moisieiev@xxxxxxxx>
---
 tools/include/xenctrl.h               |   3 +
 tools/libs/light/libxl_arm.c          | 214 ++++++++++++++++++++++++--
 tools/libs/light/libxl_create.c       |  44 +++++-
 tools/libs/light/libxl_internal.h     |   3 +
 xen/arch/arm/domctl.c                 |   7 +
 xen/include/public/device_tree_defs.h |   1 +
 6 files changed, 260 insertions(+), 12 deletions(-)

diff --git a/tools/include/xenctrl.h b/tools/include/xenctrl.h
index 07b96e6671..cdd14f465f 100644
--- a/tools/include/xenctrl.h
+++ b/tools/include/xenctrl.h
@@ -1238,6 +1238,9 @@ int xc_domain_getvnuma(xc_interface *xch,
 int xc_domain_soft_reset(xc_interface *xch,
                          uint32_t domid);
 
+int xc_domain_add_sci_device(xc_interface *xch,
+                              uint32_t domid, char *path);
+
 #if defined(__i386__) || defined(__x86_64__)
 /*
  * PC BIOS standard E820 types and structure.
diff --git a/tools/libs/light/libxl_arm.c b/tools/libs/light/libxl_arm.c
index c5090e2b32..106ff33c84 100644
--- a/tools/libs/light/libxl_arm.c
+++ b/tools/libs/light/libxl_arm.c
@@ -7,6 +7,12 @@
 #include <libfdt.h>
 #include <assert.h>
 #include <xen/device_tree_defs.h>
+#include <xenhypfs.h>
+
+#define SCMI_NODE_PATH         "/firmware/scmi"
+#define SCMI_NODE_COMPATIBLE   "arm,scmi-smc"
+#define SCMI_SHMEM_COMPATIBLE  "arm,scmi-shmem"
+#define HYPFS_DEVICETREE_PATH  "/devicetree"
 
 static const char *gicv_to_string(libxl_gic_version gic_version)
 {
@@ -101,6 +107,19 @@ int libxl__arch_domain_prepare_config(libxl__gc *gc,
         return ERROR_FAIL;
     }
 
+    switch (d_config->b_info.arm_sci) {
+    case LIBXL_ARM_SCI_TYPE_NONE:
+        config->arch.arm_sci_type = XEN_DOMCTL_CONFIG_ARM_SCI_NONE;
+        break;
+    case LIBXL_ARM_SCI_TYPE_SCMI_SMC:
+        config->arch.arm_sci_type = XEN_DOMCTL_CONFIG_ARM_SCI_SCMI_SMC;
+        break;
+    default:
+        LOG(ERROR, "Unknown ARM_SCI type %d",
+            d_config->b_info.arm_sci);
+        return ERROR_FAIL;
+    }
+
     if (libxl_defbool_val(d_config->b_info.force_assign_without_iommu))
         config->iommu_opts |= XEN_DOMCTL_IOMMU_force_iommu;
 
@@ -125,6 +144,7 @@ int libxl__arch_domain_save_config(libxl__gc *gc,
     }
 
     state->clock_frequency = config->arch.clock_frequency;
+    state->arm_sci_agent_paddr = config->arch.arm_sci_agent_paddr;
 
     return 0;
 }
@@ -505,9 +525,6 @@ static int make_optee_node(libxl__gc *gc, void *fdt)
     int res;
     LOG(DEBUG, "Creating OP-TEE node in dtb");
 
-    res = fdt_begin_node(fdt, "firmware");
-    if (res) return res;
-
     res = fdt_begin_node(fdt, "optee");
     if (res) return res;
 
@@ -520,9 +537,6 @@ static int make_optee_node(libxl__gc *gc, void *fdt)
     res = fdt_end_node(fdt);
     if (res) return res;
 
-    res = fdt_end_node(fdt);
-    if (res) return res;
-
     return 0;
 }
 
@@ -905,10 +919,9 @@ static int copy_node(libxl__gc *gc, void *fdt, void *pfdt,
     return 0;
 }
 
-static int copy_node_by_path(libxl__gc *gc, const char *path,
-                             void *fdt, void *pfdt)
+static int get_path_nodeoff(const char *path, void *pfdt)
 {
-    int nodeoff, r;
+    int nodeoff;
     const char *name = strrchr(path, '/');
 
     if (!name)
@@ -928,12 +941,189 @@ static int copy_node_by_path(libxl__gc *gc, const char 
*path,
     if (strcmp(fdt_get_name(pfdt, nodeoff, NULL), name))
         return -FDT_ERR_NOTFOUND;
 
+    return nodeoff;
+}
+
+static int copy_node_by_path(libxl__gc *gc, const char *path,
+                             void *fdt, void *pfdt)
+{
+    int nodeoff, r;
+
+    nodeoff = get_path_nodeoff(path, pfdt);
+    if (nodeoff < 0)
+        return nodeoff;
+
     r = copy_node(gc, fdt, pfdt, nodeoff, 0);
     if (r) return r;
 
     return 0;
 }
 
+static int make_scmi_shmem_node(libxl__gc *gc, void *fdt, void *pfdt)
+{
+    int res;
+    char buf[64];
+
+#ifdef CONFIG_ARM_32
+    snprintf(buf, sizeof(buf), "scp-shmem@%lx",
+             GUEST_SCI_SHMEM_BASE);
+#else
+    snprintf(buf, sizeof(buf), "scp-shmem@%llx",
+             GUEST_SCI_SHMEM_BASE);
+#endif
+
+    res = fdt_begin_node(fdt, buf);
+    if (res) return res;
+
+    res = fdt_property_compat(gc, fdt, 1, SCMI_SHMEM_COMPATIBLE);
+    if (res) return res;
+
+    res = fdt_property_regs(gc, fdt, GUEST_ROOT_ADDRESS_CELLS,
+                    GUEST_ROOT_SIZE_CELLS, 1,
+                    GUEST_SCI_SHMEM_BASE, GUEST_SCI_SHMEM_SIZE);
+    if (res) return res;
+
+    res = fdt_property_cell(fdt, "phandle", GUEST_PHANDLE_SCMI);
+    if (res) return res;
+
+    res = fdt_end_node(fdt);
+    if (res) return res;
+
+    return 0;
+}
+
+static int create_hypfs_property(struct xenhypfs_handle *hdl, void *fdt,
+                                 char *path, char *name)
+{
+    char *p, *result;
+    int ret = 0;
+    struct xenhypfs_dirent *ent;
+
+    if (strcmp(name, "shmem") == 0)
+        return fdt_property_cell(fdt, name, GUEST_PHANDLE_SCMI);
+
+    ret = asprintf(&p, "%s%s", HYPFS_DEVICETREE_PATH, path);
+    result = xenhypfs_read_raw(hdl, p, &ent);
+    free(p);
+    if (!result)
+        return -EINVAL;
+
+    ret = fdt_property(fdt, name, result, ent->size);
+    free(result);
+    free(ent);
+
+    return ret;
+}
+static int create_hypfs_subnode(struct xenhypfs_handle *hdl, void *fdt,
+                                const char *path, const char *name)
+{
+    struct xenhypfs_dirent *ent;
+    unsigned int n, i;
+    char *p, *p_sub;
+    int res = 0;
+
+    res = asprintf(&p, "%s%s", HYPFS_DEVICETREE_PATH, path);
+    if (res < 0)
+        return -ENOMEM;
+
+    ent = xenhypfs_readdir(hdl, p, &n);
+    free(p);
+    if (!ent)
+        return -EINVAL;
+
+    res = fdt_begin_node(fdt, name);
+    if (res) return res;
+
+    for (i = 0; i < n; i++) {
+        res = asprintf(&p_sub,"%s/%s", path, ent[i].name);
+        if (res < 0)
+            break;
+
+        if (ent[i].type == xenhypfs_type_dir)
+             res = create_hypfs_subnode(hdl, fdt, p_sub, ent[i].name);
+        else
+             res = create_hypfs_property(hdl, fdt, p_sub, ent[i].name);
+
+        free(p_sub);
+        if (res)
+            break;
+    }
+
+    res = fdt_end_node(fdt);
+    free(ent);
+    return res;
+}
+
+static int create_scmi_from_hypfs(void *fdt, const char *path)
+{
+    struct xenhypfs_handle *hdl;
+    int res;
+    hdl = xenhypfs_open(NULL, 0);
+    if (!hdl)
+        return -EINVAL;
+
+    res = create_hypfs_subnode(hdl, fdt, path, "scmi");
+    xenhypfs_close(hdl);
+
+    return res;
+}
+
+static int set_shmem_phandle(void *fdt, const char *scmi_node_copmat)
+{
+    uint32_t val;
+    int nodeoff = fdt_node_offset_by_compatible(fdt, 0, scmi_node_copmat);
+    if (nodeoff < 0)
+        return -EINVAL;
+
+    val = cpu_to_fdt32(GUEST_PHANDLE_SCMI);
+    return fdt_setprop_inplace(fdt, nodeoff, "shmem", &val, sizeof(val));
+}
+
+static int make_scmi_node(libxl__gc *gc, void *fdt, void *pfdt)
+{
+    int res = 0;
+    int nodeoff =
+        fdt_node_offset_by_compatible(pfdt, 0, SCMI_NODE_COMPATIBLE);
+    if (nodeoff > 0) {
+        res = copy_node(gc, fdt, pfdt, nodeoff, 0);
+        if (res) return res;
+
+        res = set_shmem_phandle(fdt, SCMI_NODE_COMPATIBLE);
+        if (res) return res;
+    }
+    else
+        res = create_scmi_from_hypfs(fdt, SCMI_NODE_PATH);
+
+    return res;
+}
+
+static int make_firmware_node(libxl__gc *gc, void *fdt, void *pfdt, int tee,
+                              int sci)
+{
+    int res;
+
+    if ((tee == LIBXL_TEE_TYPE_NONE) && (sci == LIBXL_ARM_SCI_TYPE_NONE))
+        return 0;
+
+    res = fdt_begin_node(fdt, "firmware");
+    if (res) return res;
+
+    if (tee == LIBXL_TEE_TYPE_OPTEE) {
+       res = make_optee_node(gc, fdt);
+       if (res) return res;
+    }
+
+    if (sci == LIBXL_ARM_SCI_TYPE_SCMI_SMC) {
+        res = make_scmi_node(gc, fdt, pfdt);
+        if (res) return res;
+    }
+
+    res = fdt_end_node(fdt);
+    if (res) return res;
+
+    return 0;
+}
+
 /*
  * The partial device tree is not copied entirely. Only the relevant bits are
  * copied to the guest device tree:
@@ -1091,8 +1281,10 @@ next_resize:
         if (info->arch_arm.vuart == LIBXL_VUART_TYPE_SBSA_UART)
             FDT( make_vpl011_uart_node(gc, fdt, ainfo, dom) );
 
-        if (info->tee == LIBXL_TEE_TYPE_OPTEE)
-            FDT( make_optee_node(gc, fdt) );
+        if (info->arm_sci == LIBXL_ARM_SCI_TYPE_SCMI_SMC)
+            FDT( make_scmi_shmem_node(gc, fdt, pfdt) );
+
+        FDT( make_firmware_node(gc, fdt, pfdt, info->tee, info->arm_sci) );
 
         if (d_config->num_pcidevs)
             FDT( make_vpci_node(gc, fdt, ainfo, dom) );
diff --git a/tools/libs/light/libxl_create.c b/tools/libs/light/libxl_create.c
index dcd09d32ba..f1f1e66275 100644
--- a/tools/libs/light/libxl_create.c
+++ b/tools/libs/light/libxl_create.c
@@ -596,6 +596,38 @@ out:
     return ret;
 }
 
+static int map_sci_page(libxl__gc *gc, uint32_t domid, uint64_t paddr,
+                         uint64_t guest_addr)
+{
+    int ret;
+    uint64_t _paddr_pfn = paddr >> XC_PAGE_SHIFT;
+    uint64_t _guest_pfn = guest_addr >> XC_PAGE_SHIFT;
+
+    assert(paddr && guest_addr);
+    LOG(DEBUG, "iomem %"PRIx64, _paddr_pfn);
+
+    ret = xc_domain_iomem_permission(CTX->xch, domid, _paddr_pfn, 1, 1);
+    if (ret < 0) {
+        LOG(ERROR,
+              "failed give domain access to iomem page %"PRIx64,
+             _paddr_pfn);
+        return ret;
+    }
+
+    ret = xc_domain_memory_mapping(CTX->xch, domid,
+                                   _guest_pfn, _paddr_pfn,
+                                   1, 1);
+    if (ret < 0) {
+        LOG(ERROR,
+              "failed to map to domain iomem page %"PRIx64
+              " to guest address %"PRIx64,
+              _paddr_pfn, _guest_pfn);
+        return ret;
+    }
+
+    return 0;
+}
+
 int libxl__domain_make(libxl__gc *gc, libxl_domain_config *d_config,
                        libxl__domain_build_state *state,
                        uint32_t *domid, bool soft_reset)
@@ -762,6 +794,16 @@ int libxl__domain_make(libxl__gc *gc, libxl_domain_config 
*d_config,
         goto out;
     }
 
+    if (d_config->b_info.arm_sci == LIBXL_ARM_SCI_TYPE_SCMI_SMC) {
+        ret = map_sci_page(gc, *domid, state->arm_sci_agent_paddr,
+                            GUEST_SCI_SHMEM_BASE);
+        if (ret < 0) {
+            LOGED(ERROR, *domid, "map scmi fail");
+            rc = ERROR_FAIL;
+            goto out;
+        }
+    }
+
     dom_path = libxl__xs_get_dompath(gc, *domid);
     if (!dom_path) {
         rc = ERROR_FAIL;
@@ -1825,7 +1867,7 @@ static void libxl__add_dtdevs(libxl__egc *egc, libxl__ao 
*ao, uint32_t domid,
         LOGD(DEBUG, domid, "Assign device \"%s\" to domain", dtdev->path);
         rc = xc_assign_dt_device(CTX->xch, domid, dtdev->path);
         if (rc < 0) {
-            LOGD(ERROR, domid, "xc_assign_dtdevice failed: %d", rc);
+            LOGD(ERROR, domid, "xc_assign_dt_device failed: %d", rc);
             goto out;
         }
     }
diff --git a/tools/libs/light/libxl_internal.h 
b/tools/libs/light/libxl_internal.h
index 0b4671318c..79f38b60d4 100644
--- a/tools/libs/light/libxl_internal.h
+++ b/tools/libs/light/libxl_internal.h
@@ -1407,6 +1407,9 @@ typedef struct {
     /* Whether this domain is being migrated/restored, or booting fresh.  Only
      * applicable to the primary domain, not support domains (e.g. stub QEMU). 
*/
     bool restore;
+
+    /* arm_sci channel paddr to be set to device-tree node */
+    uint64_t arm_sci_agent_paddr;
 } libxl__domain_build_state;
 
 _hidden void libxl__domain_build_state_init(libxl__domain_build_state *s);
diff --git a/xen/arch/arm/domctl.c b/xen/arch/arm/domctl.c
index 6245af6d0b..23c44f3a13 100644
--- a/xen/arch/arm/domctl.c
+++ b/xen/arch/arm/domctl.c
@@ -4,6 +4,7 @@
  * Copyright (c) 2012, Citrix Systems
  */
 
+#include <asm/sci/sci.h>
 #include <xen/errno.h>
 #include <xen/guest_access.h>
 #include <xen/hypercall.h>
@@ -182,7 +183,13 @@ long arch_do_domctl(struct xen_domctl *domctl, struct 
domain *d,
         rc = subarch_do_domctl(domctl, d, u_domctl);
 
         if ( rc == -ENOSYS )
+        {
             rc = iommu_do_domctl(domctl, d, u_domctl);
+            if ( (rc) && (rc != -ENOSYS) )
+                return rc;
+
+            rc = sci_do_domctl(domctl, d, u_domctl);
+        }
 
         return rc;
     }
diff --git a/xen/include/public/device_tree_defs.h 
b/xen/include/public/device_tree_defs.h
index 209d43de3f..f57684547b 100644
--- a/xen/include/public/device_tree_defs.h
+++ b/xen/include/public/device_tree_defs.h
@@ -7,6 +7,7 @@
  * onwards. Reserve a high value for the GIC phandle.
  */
 #define GUEST_PHANDLE_GIC (65000)
+#define GUEST_PHANDLE_SCMI (67000)
 
 #define GUEST_ROOT_ADDRESS_CELLS 2
 #define GUEST_ROOT_SIZE_CELLS 2
-- 
2.27.0