[PATCH v1 06/14] xen/riscv: riscv_of_processor_hartid() implementation

[Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>]

Implements riscv_of_processor_hartid() to get the hart ID of the given
device tree node and do some checks if CPU is available and given device
tree node has proper riscv,isa property.

As a helper function of_get_cpu_hwid() is introduced to deal specifically
with reg propery of a CPU device node.

Signed-off-by: Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>
---
 xen/arch/riscv/include/asm/smp.h |  3 ++
 xen/arch/riscv/smpboot.c         | 68 ++++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+)

diff --git a/xen/arch/riscv/include/asm/smp.h b/xen/arch/riscv/include/asm/smp.h
index 188c033718..9b68f1e27a 100644
--- a/xen/arch/riscv/include/asm/smp.h
+++ b/xen/arch/riscv/include/asm/smp.h
@@ -26,6 +26,9 @@ static inline void set_cpuid_to_hartid(unsigned long cpuid,
 
 void setup_tp(unsigned int cpuid);
 
+struct dt_device_node;
+int riscv_of_processor_hartid(struct dt_device_node *node, unsigned long 
*hart);
+
 void smp_clear_cpu_maps(void);
 
 #endif
diff --git a/xen/arch/riscv/smpboot.c b/xen/arch/riscv/smpboot.c
index 0f4dcc28e1..3193639f00 100644
--- a/xen/arch/riscv/smpboot.c
+++ b/xen/arch/riscv/smpboot.c
@@ -1,5 +1,8 @@
 #include <xen/cpumask.h>
+#include <xen/device_tree.h>
+#include <xen/errno.h>
 #include <xen/init.h>
+#include <xen/types.h>
 
 cpumask_t cpu_online_map;
 cpumask_t cpu_present_map;
@@ -13,3 +16,68 @@ void __init smp_clear_cpu_maps(void)
     cpumask_set_cpu(0, &cpu_online_map);
     cpumask_copy(&cpu_present_map, &cpu_possible_map);
 }
+
+/**
+ * of_get_cpu_hwid - Get the hardware ID from a CPU device node
+ *
+ * @cpun: CPU number(logical index) for which device node is required
+ * @thread: The local thread number to get the hardware ID for.
+ *
+ * Return: The hardware ID for the CPU node or ~0ULL if not found.
+ */
+static uint64_t of_get_cpu_hwid(struct dt_device_node *cpun, unsigned int 
thread)
+{
+    const __be32 *cell;
+    int ac;
+    uint32_t len;
+
+    ac = dt_n_addr_cells(cpun);
+    cell = dt_get_property(cpun, "reg", &len);
+    if ( !cell || !ac || ((sizeof(*cell) * ac * (thread + 1)) > len) )
+        return ~0ULL;
+
+    cell += ac * thread;
+    return dt_read_number(cell, ac);
+}
+
+/*
+ * Returns the hart ID of the given device tree node, or -ENODEV if the node
+ * isn't an enabled and valid RISC-V hart node.
+ */
+int riscv_of_processor_hartid(struct dt_device_node *node, unsigned long *hart)
+{
+    const char *isa;
+
+    if ( !dt_device_is_compatible(node, "riscv") )
+    {
+        printk("Found incompatible CPU\n");
+        return -ENODEV;
+    }
+
+    *hart = (unsigned long) of_get_cpu_hwid(node, 0);
+    if ( *hart == ~0UL )
+    {
+        printk("Found CPU without hart ID\n");
+        return -ENODEV;
+    }
+
+    if ( !dt_device_is_available(node))
+    {
+        printk("CPU with hartid=%lu is not available\n", *hart);
+        return -ENODEV;
+    }
+
+    if ( dt_property_read_string(node, "riscv,isa", &isa) )
+    {
+        printk("CPU with hartid=%lu has no \"riscv,isa\" property\n", *hart);
+        return -ENODEV;
+    }
+
+    if ( isa[0] != 'r' || isa[1] != 'v' )
+    {
+        printk("CPU with hartid=%lu has an invalid ISA of \"%s\"\n", *hart, 
isa);
+        return -ENODEV;
+    }
+
+    return 0;
+}
-- 
2.49.0