[PATCH v3 07/22] xen/device-tree: Read NUMA node distance from Device Tree 'distance-map'

[Hirokazu Takahashi <taka@xxxxxxxxxxxxx>]

Read the distance between NUMA nodes from the Device Tree's
'distance-map' node.

Signed-off-by: Hirokazu Takahashi <taka@xxxxxxxxxxxxx>
---
 xen/arch/arm/setup.c           |  4 ++
 xen/common/device-tree/numa.c  | 76 ++++++++++++++++++++++++++++++++++
 xen/common/numa-distance-map.c | 49 ++++++++++++++++++++--
 xen/include/xen/dt-numa.h      |  1 +
 xen/include/xen/numa.h         |  5 +++
 5 files changed, 132 insertions(+), 3 deletions(-)

diff --git a/xen/arch/arm/setup.c b/xen/arch/arm/setup.c
index cdabf536b0..cf12c406fc 100644
--- a/xen/arch/arm/setup.c
+++ b/xen/arch/arm/setup.c
@@ -375,6 +375,10 @@ void asmlinkage __init noreturn start_xen(unsigned long 
fdt_paddr)
         device_tree_flattened = NULL;
     }
 
+#ifdef CONFIG_NUMA
+    numa_distance_table_init();
+#endif /* CONFIG_NUMA */
+
     init_IRQ();
 
     platform_init();
diff --git a/xen/common/device-tree/numa.c b/xen/common/device-tree/numa.c
index ff5a7dee9a..3dd608b599 100644
--- a/xen/common/device-tree/numa.c
+++ b/xen/common/device-tree/numa.c
@@ -1,4 +1,80 @@
 /* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Derived from Linux kernel 7.0's $drivers/of/of_numa.c
+ * Parse 'distance-map'
+ */
+
+#include <xen/bootinfo.h>
+#include <xen/device_tree.h>
+#include <xen/libfdt/libfdt.h>
+#include <xen/bootfdt.h>
+#include <xen/errno.h>
+#include <xen/init.h>
+#include <xen/nodemask.h>
+#include <xen/numa.h>
+
+#define LOCAL_DISTANCE      10
+#define REMOTE_DISTANCE     20
+
+/*
+ * Parse the '/distance-map' node from the flattened device tree
+ * and extract the 3-tuple triplets <from, to, distance>.
+ */
+static void __init dt_numa_parse_distance_map(void)
+{
+    const void *fdt = device_tree_flattened;
+    const struct fdt_property *prop;
+    const __be32 *matrix;
+    int entry_count;
+    int node;
+    int len;
+    int i;
+
+    node = fdt_path_offset(fdt, "/distance-map");
+    if ( node < 0 )
+        return;
+
+    if ( fdt_node_check_compatible(fdt, node, "numa-distance-map-v1") )
+        return;
+
+    prop = fdt_get_property(fdt, node, "distance-matrix", &len);
+    if ( !prop )
+        return;
+
+    matrix = (const __be32*)prop->data;
+    entry_count = len / sizeof(__be32);
+
+    if ( (entry_count <= 0) || (entry_count % 3) )
+        return;
+
+    for ( i = 0; i + 2 < entry_count; i += 3 )
+    {
+        uint32_t nodea, nodeb, distance;
+
+        nodea = dt_next_cell(1, &matrix);
+        nodeb = dt_next_cell(1, &matrix);
+        distance = dt_next_cell(1, &matrix);
+
+        if ( (nodea == nodeb && distance != LOCAL_DISTANCE) ||
+             (nodea != nodeb && distance <= LOCAL_DISTANCE) )
+        {
+            printk(XENLOG_WARNING "Invalid distance[node%d -> node%d] = %d\n",
+                   nodea, nodeb, distance);
+            continue;
+        }
+
+        numa_set_distance(nodea, nodeb, distance);
+
+        /* Set default distance of node B->A same as A->B */
+        if ( nodeb > nodea )
+            numa_set_distance(nodeb, nodea, distance);
+    }
+}
+
+void __init dt_numa_distance_table_init(void)
+{
+    dt_numa_parse_distance_map();
+}
 
 /*
  * Initialize memory affinity by registering bootinfo memory banks into Xen's
diff --git a/xen/common/numa-distance-map.c b/xen/common/numa-distance-map.c
index 73344f7f33..6e68912646 100644
--- a/xen/common/numa-distance-map.c
+++ b/xen/common/numa-distance-map.c
@@ -1,19 +1,62 @@
 /* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Derived from Linux kernel 7.0's $mm/numa_memblks.c
+ */
 
 #include <xen/errno.h>
 #include <xen/init.h>
 #include <xen/nodemask.h>
 #include <xen/numa.h>
+#include <xen/acpi.h>
+
 
 #define LOCAL_DISTANCE      10
 #define REMOTE_DISTANCE     20
 
+uint8_t * __ro_after_init numa_distance;
+
 /*
  * Get the distance between node 'from' and node 'to'.
  */
 uint8_t numa_node_distance(unsigned int from, unsigned int to)
 {
-    if ( from != to )
-        return REMOTE_DISTANCE;
-    return LOCAL_DISTANCE;
+    const unsigned int nr_nodes = last_node(node_online_map) + 1U;
+
+    if ( from >= nr_nodes || to >= nr_nodes )
+        return from == to ? LOCAL_DISTANCE : REMOTE_DISTANCE;
+
+    return numa_distance[from * nr_nodes + to];
+}
+
+void __init numa_set_distance(unsigned int from, unsigned int to,
+                                     unsigned int distance)
+{
+    const unsigned int nr_nodes = last_node(node_online_map) + 1U;
+
+    if ( (uint8_t)distance != distance || from >= nr_nodes || to >= nr_nodes )
+        printk(XENLOG_WARNING "Invalid distance[node%d -> node%d] = %d\n",
+               from, to, distance);
+    else
+        numa_distance[from * nr_nodes + to] = distance;
+}
+
+void __init numa_distance_table_init(void)
+{
+    const unsigned int nr_nodes = last_node(node_online_map) + 1U;
+    unsigned int i, j;
+
+    numa_distance = xzalloc_array(uint8_t, nr_nodes * nr_nodes);
+    if ( !numa_distance )
+        panic("Failed to allocate memory for numa distance-map array\n");
+
+    /* fill with the default distances */
+    for ( i = 0U; i < nr_nodes; i++ )
+        for ( j = 0U; j < nr_nodes; j++ )
+            numa_distance[i * nr_nodes + j] = i == j ?
+                LOCAL_DISTANCE : REMOTE_DISTANCE;
+
+    if ( acpi_disabled )
+        dt_numa_distance_table_init();
+    else
+        ; /* Initialize the distance-map array with ACPI SLIT */
 }
diff --git a/xen/include/xen/dt-numa.h b/xen/include/xen/dt-numa.h
index 943bc0e453..352853dd81 100644
--- a/xen/include/xen/dt-numa.h
+++ b/xen/include/xen/dt-numa.h
@@ -14,6 +14,7 @@ static inline unsigned int numa_node_to_dt_nid(unsigned int n)
 #ifdef CONFIG_DEVICE_TREE_NUMA
 
 void dt_numa_memory_affinity_init(void);
+void dt_numa_distance_table_init(void);
 
 #endif /* CONFIG_DEVICE_TREE_NUMA */
 
diff --git a/xen/include/xen/numa.h b/xen/include/xen/numa.h
index 18c22d3d30..e7201539d1 100644
--- a/xen/include/xen/numa.h
+++ b/xen/include/xen/numa.h
@@ -129,6 +129,11 @@ extern bool numa_update_node_memblks(nodeid_t node, 
unsigned int arch_nid,
 extern void numa_set_processor_nodes_parsed(nodeid_t node);
 
 extern uint8_t numa_node_distance(unsigned int from, unsigned int to);
+extern void numa_set_distance(unsigned int from, unsigned int to,
+                              unsigned int distance);
+extern void numa_distance_table_init(void);
+
+extern uint8_t *numa_distance;
 
 #else
 
-- 
2.43.0