[RFC v2 3/8] xen/arm: Export host device-tree to hypfs

[Oleksii Moisieiev <Oleksii_Moisieiev@epam.com>]

If enabled, host device-tree will be exported to hypfs and can be
accessed through /devicetree path.
Exported device-tree has the same format, as the device-tree
exported to the sysfs by the Linux kernel.
This is useful when XEN toolstack needs an access to the host device-tree.

Signed-off-by: Oleksii Moisieiev <oleksii_moisieiev@epam.com>
---
 xen/arch/arm/Kconfig           |   8 +
 xen/arch/arm/Makefile          |   1 +
 xen/arch/arm/host_dtb_export.c | 307 +++++++++++++++++++++++++++++++++
 3 files changed, 316 insertions(+)
 create mode 100644 xen/arch/arm/host_dtb_export.c

diff --git a/xen/arch/arm/Kconfig b/xen/arch/arm/Kconfig
index ecfa6822e4..895016b21e 100644
--- a/xen/arch/arm/Kconfig
+++ b/xen/arch/arm/Kconfig
@@ -33,6 +33,14 @@ config ACPI
 	  Advanced Configuration and Power Interface (ACPI) support for Xen is
 	  an alternative to device tree on ARM64.
 
+config HOST_DTB_EXPORT
+	bool "Export host device tree to hypfs if enabled"
+	depends on ARM && HYPFS && !ACPI
+	---help---
+
+	  Export host device-tree to hypfs so toolstack can have an access for the
+	  host device tree from Dom0. If you unsure say N.
+
 config GICV3
 	bool "GICv3 driver"
 	depends on ARM_64 && !NEW_VGIC
diff --git a/xen/arch/arm/Makefile b/xen/arch/arm/Makefile
index 07f634508e..0a41f68f8c 100644
--- a/xen/arch/arm/Makefile
+++ b/xen/arch/arm/Makefile
@@ -8,6 +8,7 @@ obj-y += platforms/
 endif
 obj-$(CONFIG_TEE) += tee/
 obj-$(CONFIG_HAS_VPCI) += vpci.o
+obj-$(CONFIG_HOST_DTB_EXPORT) += host_dtb_export.o
 
 obj-$(CONFIG_HAS_ALTERNATIVE) += alternative.o
 obj-y += bootfdt.init.o
diff --git a/xen/arch/arm/host_dtb_export.c b/xen/arch/arm/host_dtb_export.c
new file mode 100644
index 0000000000..794395683c
--- /dev/null
+++ b/xen/arch/arm/host_dtb_export.c
@@ -0,0 +1,307 @@
+/*
+ * xen/arch/arm/host_dtb_export.c
+ *
+ * Export host device-tree to the hypfs so toolstack can access
+ * host device-tree from Dom0
+ *
+ * Oleksii Moisieiev <oleksii_moisieiev@epam.com>
+ * Copyright (C) 2021, EPAM Systems.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <xen/device_tree.h>
+#include <xen/err.h>
+#include <xen/guest_access.h>
+#include <xen/hypfs.h>
+#include <xen/init.h>
+
+#define HOST_DT_DIR "devicetree"
+
+static int host_dt_dir_read(const struct hypfs_entry *entry,
+                            XEN_GUEST_HANDLE_PARAM(void) uaddr);
+static unsigned int host_dt_dir_getsize(const struct hypfs_entry *entry);
+
+static const struct hypfs_entry *host_dt_dir_enter(
+    const struct hypfs_entry *entry);
+static void host_dt_dir_exit(const struct hypfs_entry *entry);
+
+static struct hypfs_entry *host_dt_dir_findentry(
+    const struct hypfs_entry_dir *dir, const char *name, unsigned int name_len);
+
+static const struct hypfs_funcs host_dt_dir_funcs = {
+    .enter = host_dt_dir_enter,
+    .exit = host_dt_dir_exit,
+    .read = host_dt_dir_read,
+    .write = hypfs_write_deny,
+    .getsize = host_dt_dir_getsize,
+    .findentry = host_dt_dir_findentry,
+};
+
+static int host_dt_prop_read(const struct hypfs_entry *entry,
+                    XEN_GUEST_HANDLE_PARAM(void) uaddr);
+
+static unsigned int host_dt_prop_getsize(const struct hypfs_entry *entry);
+
+const struct hypfs_funcs host_dt_prop_ro_funcs = {
+    .enter = host_dt_dir_enter,
+    .exit = host_dt_dir_exit,
+    .read = host_dt_prop_read,
+    .write = hypfs_write_deny,
+    .getsize = host_dt_prop_getsize,
+    .findentry = hypfs_leaf_findentry,
+};
+
+static HYPFS_DIR_INIT_FUNC(dt_dir, "node_template", &host_dt_dir_funcs);
+
+#define HYPFS_PROPERTY_MAX_SIZE 256
+static HYPFS_VARSIZE_INIT(dt_prop, XEN_HYPFS_TYPE_BLOB, "prop_template",
+                            HYPFS_PROPERTY_MAX_SIZE, &host_dt_prop_ro_funcs);
+
+static const char *get_name_from_path(const char *path)
+{
+    const char *name = strrchr(path, '/');
+    if ( !name )
+        name = path;
+    else
+    {
+        name++;
+        if ( !*name )
+            name--;
+    }
+
+    return name;
+}
+
+static char *get_root_from_path(const char *path, char *name)
+{
+    const char *nm = strchr(path, '/');
+    if ( !nm )
+        nm = path + strlen(path);
+    else
+    {
+        if ( !*nm )
+            nm--;
+    }
+
+    return memcpy(name, path, nm - path);
+}
+
+static int host_dt_dir_read(const struct hypfs_entry *entry,
+                            XEN_GUEST_HANDLE_PARAM(void) uaddr)
+{
+    int ret = 0;
+    struct dt_device_node *node;
+    struct dt_device_node *child;
+    const struct dt_property *prop;
+    struct hypfs_dyndir_id *data;
+
+    data = hypfs_get_dyndata();
+    if ( !data )
+        return -EINVAL;
+
+    node = data->data;
+    if ( !node )
+        return -EINVAL;
+
+    dt_for_each_property_node( node, prop )
+    {
+        ret = hypfs_read_dyndir_entry(&dt_prop.e, prop->name,
+                                      strlen(prop->name),
+                                      !prop->next && !node->child,
+                                      &uaddr);
+
+        if ( ret )
+            break;
+    }
+
+    for ( child = node->child; child != NULL; child = child->sibling )
+    {
+        const char *parsed_name = get_name_from_path(child->full_name);
+        data->data = child;
+
+        ret = hypfs_read_dyndir_entry(&dt_dir.e, parsed_name,
+                                         strlen(parsed_name),
+                                         child->sibling == NULL,
+                                         &uaddr);
+
+        if ( ret )
+            break;
+    }
+
+    return ret;
+}
+
+static unsigned int host_dt_dir_getsize(const struct hypfs_entry *entry)
+{
+    struct dt_device_node *node;
+    struct dt_device_node *child;
+    struct hypfs_dyndir_id *data;
+    const struct dt_property *prop;
+    unsigned int size = 0;
+
+    data = hypfs_get_dyndata();
+    if ( !data )
+        return -EINVAL;
+
+    node = data->data;
+    if ( !node )
+        return -EINVAL;
+
+    dt_for_each_property_node( node, prop )
+    {
+        size += hypfs_dyndir_entry_size(entry, prop->name);
+    }
+
+    for ( child = node->child; child != NULL; child = child->sibling )
+    {
+        const char *parsed_name = get_name_from_path(child->full_name);
+        size += hypfs_dyndir_entry_size(entry, parsed_name);
+    }
+
+    return size;
+}
+
+static DEFINE_PER_CPU(bool, data_alloc);
+
+static inline bool data_is_alloc(void)
+{
+    unsigned int cpu = smp_processor_id();
+    return per_cpu(data_alloc, cpu);
+}
+
+static inline void set_data_alloc(void)
+{
+    unsigned int cpu = smp_processor_id();
+    ASSERT(!per_cpu(data_alloc, cpu));
+
+    this_cpu(data_alloc) = true;
+}
+
+static inline void unset_data_alloc(void)
+{
+    this_cpu(data_alloc) = false;
+}
+
+static const struct hypfs_entry *host_dt_dir_enter(
+    const struct hypfs_entry *entry)
+{
+    struct hypfs_dyndir_id *data;
+
+    if ( !data_is_alloc() )
+    {
+        data = hypfs_alloc_dyndata(struct hypfs_dyndir_id);
+        if ( !data )
+            return ERR_PTR(-ENOMEM);
+
+        set_data_alloc();
+    }
+
+    if ( strcmp(entry->name, HOST_DT_DIR) == 0 )
+    {
+        data = hypfs_get_dyndata();
+        data->data = dt_host;
+    }
+
+    return entry;
+}
+
+static void host_dt_dir_exit(const struct hypfs_entry *entry)
+{
+    if ( !data_is_alloc() )
+        return;
+
+    hypfs_free_dyndata();
+    unset_data_alloc();
+}
+
+static struct hypfs_entry *host_dt_dir_findentry(
+    const struct hypfs_entry_dir *dir, const char *name, unsigned int name_len)
+{
+    struct dt_device_node *node;
+    char root_name[HYPFS_DYNDIR_ID_NAMELEN];
+    struct dt_device_node *child;
+    struct hypfs_dyndir_id *data;
+    struct dt_property *prop;
+
+    data = hypfs_get_dyndata();
+    if ( !data )
+        return ERR_PTR(-EINVAL);
+
+    node = data->data;
+    if ( !node )
+        return ERR_PTR(-EINVAL);
+
+    memset(root_name, 0, sizeof(root_name));
+    get_root_from_path(name, root_name);
+
+    for ( child = node->child; child != NULL; child = child->sibling )
+    {
+        if ( strcmp(get_name_from_path(child->full_name), root_name) == 0 )
+            return hypfs_gen_dyndir_entry(&dt_dir.e,
+                                  get_name_from_path(child->full_name), child);
+    }
+
+    dt_for_each_property_node( node, prop )
+    {
+
+        if ( dt_property_name_is_equal(prop, root_name) )
+            return hypfs_gen_dyndir_entry(&dt_prop.e, prop->name, prop);
+    }
+
+    return ERR_PTR(-ENOENT);
+};
+
+static int host_dt_prop_read(const struct hypfs_entry *entry,
+                    XEN_GUEST_HANDLE_PARAM(void) uaddr)
+{
+    const struct dt_property *prop;
+    struct hypfs_dyndir_id *data;
+
+    data = hypfs_get_dyndata();
+    if ( !data )
+        return -EINVAL;
+
+    prop = data->data;
+    if ( !prop )
+        return -EINVAL;
+
+    return copy_to_guest(uaddr, prop->value, prop->length) ?  -EFAULT : 0;
+}
+
+static unsigned int host_dt_prop_getsize(const struct hypfs_entry *entry)
+{
+    const struct hypfs_dyndir_id *data;
+    const struct dt_property *prop;
+
+    data = hypfs_get_dyndata();
+    if ( !data )
+        return -EINVAL;
+
+    prop = data->data;
+    if ( !prop )
+        return -EINVAL;
+
+    return prop->length;
+}
+
+static HYPFS_DIR_INIT_FUNC(host_dt_dir, HOST_DT_DIR, &host_dt_dir_funcs);
+
+static int __init host_dtb_export_init(void)
+{
+    ASSERT(dt_host && (dt_host->sibling == NULL));
+    unset_data_alloc();
+
+    hypfs_add_dir(&hypfs_root, &host_dt_dir, true);
+    hypfs_add_dyndir(&hypfs_root, &dt_dir);
+    return 0;
+}
+__initcall(host_dtb_export_init);
-- 
2.27.0