[PATCH v3 08/16] x86/hyperlaunch: Add helpers to locate multiboot modules

[Alejandro Vallejo <agarciav@xxxxxxx>]

Hyperlaunch mandates either a reg or module-index DT prop on nodes that
contain `multiboot,module" under their "compatible" prop. This patch
introduces a helper to generically find such index, appending the module
to the list of modules if it wasn't already (i.e: because it's given via
the "reg" prop).

Signed-off-by: Jason Andryuk <jason.andryuk@xxxxxxx>
Signed-off-by: Alejandro Vallejo <agarciav@xxxxxxx>
---
v3:
    * New on v3.
    * Subsumes much of the dup code between kernel/initrd patches.
    * Changes previous behaviour in v2 to look into "reg" and
      "module-index" props, rather than just index.
    * Use addr_cells/size_cells rather than size_size
---
 xen/arch/x86/domain-builder/fdt.c   | 142 ++++++++++++++++++++++++++++
 xen/arch/x86/domain-builder/fdt.h   |   2 +
 xen/include/xen/libfdt/libfdt-xen.h |  57 +++++++++++
 3 files changed, 201 insertions(+)

diff --git a/xen/arch/x86/domain-builder/fdt.c 
b/xen/arch/x86/domain-builder/fdt.c
index 4c5b7747f5..9ebc8fd0e4 100644
--- a/xen/arch/x86/domain-builder/fdt.c
+++ b/xen/arch/x86/domain-builder/fdt.c
@@ -13,6 +13,148 @@
 
 #include "fdt.h"
 
+/*
+ * Unpacks a "reg" property into its address and size constituents.
+ *
+ * @param prop          Pointer to an FDT "reg" property.
+ * @param address_cells Number of 4-octet cells that make up an "address".
+ * @param size_cells    Number of 4-octet cells that make up a "size".
+ * @param p_addr[out]   Address encoded in the property.
+ * @param p_size[out]   Size encoded in the property.
+ * @returns             -EINVAL on malformed property, 0 otherwise.
+ */
+static int __init read_fdt_prop_as_reg(const struct fdt_property *prop,
+                                       int address_cells, int size_cells,
+                                       uint64_t *p_addr, uint64_t *p_size)
+{
+    const fdt32_t *cell = (const fdt32_t *)prop->data;
+    uint64_t addr, size;
+
+    if ( fdt32_to_cpu(prop->len) !=
+         (address_cells + size_cells) * sizeof(*cell) )
+    {
+        printk("  Cannot read reg %lu+%lu from prop len %u\n",
+            address_cells * sizeof(*cell), size_cells * sizeof(*cell),
+            fdt32_to_cpu(prop->len));
+        return -EINVAL;
+    }
+
+    switch ( address_cells ) {
+    case 1:
+        addr = fdt32_to_cpu(*cell);
+        break;
+    case 2:
+        addr = fdt64_to_cpu(*(const fdt64_t *)cell);
+        break;
+    default:
+        printk("  unsupported sized address_cells\n");
+        return -EINVAL;
+    }
+
+    cell += address_cells;
+    switch ( size_cells ) {
+    case 1:
+        size = fdt32_to_cpu(*cell);
+        break;
+    case 2:
+        size = fdt64_to_cpu(*(const fdt64_t *)cell);
+        break;
+    default:
+        printk("  unsupported sized size_cells\n");
+        return -EINVAL;
+    }
+
+    *p_addr = addr;
+    *p_size = size;
+
+    return 0;
+}
+
+/*
+ * Locate a multiboot module given its node offset in the FDT.
+ *
+ * The module location may be given via either FDT property:
+ *     * reg = <address, size>
+ *         * Mutates `bi` to append the module.
+ *     * module-index = <idx>
+ *         * Leaves `bi` unchanged.
+ *
+ * @param fdt           Pointer to the full FDT.
+ * @param node          Offset for the module node.
+ * @param address_cells Number of 4-octet cells that make up an "address".
+ * @param size_cells    Number of 4-octet cells that make up a "size".
+ * @param bi[inout]     Xen's representation of the boot parameters.
+ * @return              -EINVAL on malformed nodes, otherwise
+ *                      index inside `bi->mods`
+ */
+int __init fdt_read_multiboot_module(const void *fdt, int node,
+                                     int address_cells, int size_cells,
+                                     struct boot_info *bi)
+{
+    const struct fdt_property *prop;
+    uint64_t addr, size;
+    int ret;
+    int idx;
+
+    ASSERT(!fdt_node_check_compatible(fdt, node, "multiboot,module"));
+
+    /* Location given as a `module-index` property. */
+    prop = fdt_get_property(fdt, node, "module-index", NULL);
+
+    if ( prop )
+    {
+        if ( fdt_get_property(fdt, node, "reg", NULL) )
+        {
+            printk("  Location of multiboot,module defined multiple times\n");
+            return -EINVAL;
+        }
+        return fdt_cell_as_u32((const fdt32_t *)prop->data);
+    }
+
+    /* Otherwise location given as a `reg` property. */
+    prop = fdt_get_property(fdt, node, "reg", NULL);
+
+    if ( !prop )
+    {
+        printk("  No location for multiboot,module\n");
+        return -EINVAL;
+    }
+    if ( fdt_get_property(fdt, node, "module-index", NULL) )
+    {
+        printk("  Location of multiboot,module defined multiple times\n");
+        return -EINVAL;
+    }
+
+    ret = read_fdt_prop_as_reg(prop, address_cells, size_cells, &addr, &size);
+
+    if ( ret < 0 )
+    {
+        printk("  Failed reading reg for multiboot,module\n");
+        return -EINVAL;
+    }
+
+    idx = bi->nr_modules + 1;
+    if ( idx > MAX_NR_BOOTMODS )
+    {
+        /*
+         * MAX_NR_BOOTMODS cannot exceed the max for MB1, represented by 
32bits,
+         * thus the cast down to a u32 will be safe due to the prior check.
+         */
+        BUILD_BUG_ON(MAX_NR_BOOTMODS >= (uint64_t)UINT32_MAX);
+        printk("  idx %d exceeds maximum boot modules\n", idx);
+        return -EINVAL;
+    }
+
+    /* Append new module to the existing list */
+
+    bi->nr_modules = idx;
+    bi->mods[idx].start = addr;
+    bi->mods[idx].size = size;
+    printk("  module[%d]: addr %lx size %lx\n", idx, addr, size);
+
+    return idx;
+}
+
 static int __init find_hyperlaunch_node(const void *fdt)
 {
     int hv_node = fdt_path_offset(fdt, "/chosen/hypervisor");
diff --git a/xen/arch/x86/domain-builder/fdt.h 
b/xen/arch/x86/domain-builder/fdt.h
index 1849656571..e8769dc51c 100644
--- a/xen/arch/x86/domain-builder/fdt.h
+++ b/xen/arch/x86/domain-builder/fdt.h
@@ -3,6 +3,8 @@
 #define __XEN_X86_FDT_H__
 
 #include <xen/init.h>
+#include <xen/libfdt/libfdt.h>
+#include <xen/libfdt/libfdt-xen.h>
 
 struct boot_info;
 
diff --git a/xen/include/xen/libfdt/libfdt-xen.h 
b/xen/include/xen/libfdt/libfdt-xen.h
index a5340bc9f4..2259c09a6a 100644
--- a/xen/include/xen/libfdt/libfdt-xen.h
+++ b/xen/include/xen/libfdt/libfdt-xen.h
@@ -13,6 +13,63 @@
 
 #include <xen/libfdt/libfdt.h>
 
+static inline int __init fdt_cell_as_u32(const fdt32_t *cell)
+{
+    return fdt32_to_cpu(*cell);
+}
+
+static inline uint64_t  __init fdt_cell_as_u64(const fdt32_t *cell)
+{
+    return ((uint64_t)fdt32_to_cpu(cell[0]) << 32) | fdt32_to_cpu(cell[1]);
+}
+
+/*
+ * Property: reg
+ *
+ * Defined in Section 2.3.6 of the Device Tree Specification is the "reg"
+ * standard property. The property is a prop-encoded-array that is encoded as
+ * an arbitrary number of (address, size) pairs.  We only extract a single
+ * pair since that is what is used in practice.
+ */
+static inline int __init fdt_get_reg_prop(
+    const void *fdt, int node, unsigned int addr_cells, unsigned int 
size_cells,
+    uint64_t *addr, uint64_t *size)
+{
+    int ret;
+    const struct fdt_property *prop;
+    fdt32_t *cell;
+
+    /* FDT spec max size is 4 (128bit int), but largest arch int size is 64 */
+    if ( size_cells > 2 || addr_cells > 2 )
+        return -EINVAL;
+
+    prop = fdt_get_property(fdt, node, "reg", &ret);
+    if ( !prop || ret < sizeof(u32) )
+        return ret < 0 ? ret : -EINVAL;
+
+    if ( fdt32_to_cpu(prop->len) !=
+        ((size_cells + addr_cells) * sizeof(*cell)) )
+        return -EINVAL;
+
+    cell = (fdt32_t *)prop->data;
+
+    /* read address field */
+    if ( addr_cells == 1 )
+        *addr = fdt_cell_as_u32(cell);
+    else
+        *addr = fdt_cell_as_u64(cell);
+
+    cell += addr_cells;
+
+    /* read size field */
+    if ( size_cells == 1 )
+        *size = fdt_cell_as_u32(cell);
+    else
+        *size = fdt_cell_as_u64(cell);
+
+    return 0;
+}
+
 static inline int fdt_get_mem_rsv_paddr(const void *fdt, int n,
                                         paddr_t *address,
                                         paddr_t *size)
-- 
2.43.0