[U-Boot] [PATCH v3 5/8] x86: qemu: add qemu_fwcfg_fdt_fixup()

[Miao Yan]

Add a function to fixup 'cpus' node in dts files for qemu target.

Signed-off-by: Miao Yan <yanmiaobest at gmail.com>
---
Changes in v2:
  - various cleanups

 arch/x86/cpu/qemu/fw_cfg.c | 65 ++++++++++++++++++++++++++++++++++++++++++++++
 arch/x86/cpu/qemu/fw_cfg.h | 11 ++++++++
 2 files changed, 76 insertions(+)

diff --git a/arch/x86/cpu/qemu/fw_cfg.c b/arch/x86/cpu/qemu/fw_cfg.c
index bb6a11e..3832320 100644
--- a/arch/x86/cpu/qemu/fw_cfg.c
+++ b/arch/x86/cpu/qemu/fw_cfg.c
@@ -9,6 +9,7 @@
 #include <errno.h>
 #include <malloc.h>
 #include <asm/io.h>
+#include <libfdt.h>
 #include "fw_cfg.h"
 
 static bool fwcfg_present;
@@ -88,6 +89,70 @@ int qemu_fwcfg_online_cpus(void)
 	return le16_to_cpu(nb_cpus);
 }
 
+void qemu_fwcfg_fdt_fixup(void *fdt_addr, int cpu_num)
+{
+	int i;
+	char cpus[10];
+	int off, err, sub_off, id;
+
+	off = fdt_path_offset(fdt_addr, "/cpus");
+	if (off != -FDT_ERR_NOTFOUND) {
+		printf("error detecting cpus subnode: %s (%d)\n",
+		       fdt_strerror(off), off);
+		return;
+	}
+
+	off = fdt_add_subnode(fdt_addr, 0, "cpus");
+	if (off < 0) {
+		printf("error adding cpus subnode: %s (%d)\n",
+		       fdt_strerror(off), off);
+		return;
+	}
+
+	for (i = cpu_num - 1; i >= 0; i--) {
+		sprintf(cpus, "%s@%d", "cpu", i);
+		sub_off = fdt_add_subnode(fdt_addr, off, cpus);
+		if (sub_off < 0) {
+			printf("error adding subnode cpu %d: %s (%d)\n",
+			       i, fdt_strerror(sub_off), sub_off);
+			return;
+		}
+
+		id = cpu_to_fdt32(i);
+		err = fdt_setprop(fdt_addr, sub_off, "intel,apic-id",
+				(void *)&id, sizeof(id));
+		if (err < 0) {
+			printf("error adding apic-id %d: %s (%d)\n",
+			       i, fdt_strerror(err), err);
+			return;
+		}
+
+		err = fdt_setprop(fdt_addr, sub_off, "reg",
+				(void *)&id, sizeof(id));
+		if (err < 0) {
+			printf("error adding reg %d: %s (%d)\n",
+			       i, fdt_strerror(err), err);
+			return;
+		}
+
+		err = fdt_setprop(fdt_addr, sub_off, "compatible",
+				"cpu-qemu", sizeof("cpu-qemu"));
+		if (err < 0) {
+			printf("error adding compatible %d: %s (%d)\n",
+			       i, fdt_strerror(err), err);
+			return;
+		}
+
+		err = fdt_setprop(fdt_addr, sub_off, "device_type",
+				"cpu", sizeof("cpu"));
+		if (err < 0) {
+			printf("error adding device_type %d: %s (%d)\n",
+			       i, fdt_strerror(err), err);
+			return;
+		}
+	}
+}
+
 static int qemu_fwcfg_setup_kernel(void *load_addr)
 {
 	char *data_addr;
diff --git a/arch/x86/cpu/qemu/fw_cfg.h b/arch/x86/cpu/qemu/fw_cfg.h
index f3d06d1..a92c9e4 100644
--- a/arch/x86/cpu/qemu/fw_cfg.h
+++ b/arch/x86/cpu/qemu/fw_cfg.h
@@ -94,4 +94,15 @@ void qemu_fwcfg_init(void);
 
 int qemu_fwcfg_online_cpus(void);
 
+/**
+ * Fix 'cpu' node in device tree for qemu targets
+ *
+ * @fdt_addr: device tree blob address
+ * @cpu_num:  cpu number in system
+ *
+ * @retval:   None
+ */
+
+void qemu_fwcfg_fdt_fixup(void *fdt_addr, int cpu_num);
+
 #endif
-- 
1.9.1