[U-Boot] [PATCH 5/7] 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>
---
 arch/x86/cpu/qemu/fw_cfg.c | 66 ++++++++++++++++++++++++++++++++++++++++++++++
 arch/x86/cpu/qemu/fw_cfg.h |  1 +
 2 files changed, 67 insertions(+)

diff --git a/arch/x86/cpu/qemu/fw_cfg.c b/arch/x86/cpu/qemu/fw_cfg.c
index e7615d1..d6b0276 100644
--- a/arch/x86/cpu/qemu/fw_cfg.c
+++ b/arch/x86/cpu/qemu/fw_cfg.c
@@ -12,6 +12,8 @@
 #include <errno.h>
 #include <fs.h>
 #include <asm/io.h>
+#include <libfdt.h>
+#include <libfdt_env.h>
 #include "fw_cfg.h"
 
 static bool fwcfg_present;
@@ -92,6 +94,70 @@ uint16_t qemu_fwcfg_online_cpus(void)
 	return nb_cpus;
 }
 
+void qemu_fwcfg_fdt_fixup(void *fdt_addr, uint16_t 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: %s (%d)\n",
+			       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: %s (%d)\n",
+			       fdt_strerror(err), err);
+			return;
+		}
+
+		err = fdt_setprop(fdt_addr, sub_off, "reg",
+				(void *)&id, sizeof(id));
+		if (err < 0) {
+			printf("error adding reg: %s (%d)\n",
+			       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: %s (%d)\n",
+			       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: %s (%d)\n",
+			       fdt_strerror(err), err);
+			return;
+		}
+	}
+}
+
 static int qemu_fwcfg_setup_kernel(void *load_addr)
 {
 	char *cmd_addr;
diff --git a/arch/x86/cpu/qemu/fw_cfg.h b/arch/x86/cpu/qemu/fw_cfg.h
index 66e0c8a..d4bd46f 100644
--- a/arch/x86/cpu/qemu/fw_cfg.h
+++ b/arch/x86/cpu/qemu/fw_cfg.h
@@ -80,5 +80,6 @@ struct fw_cfg_dma_access {
 
 void qemu_fwcfg_init(void);
 uint16_t qemu_fwcfg_online_cpus(void);
+void qemu_fwcfg_fdt_fixup(void *fdt_addr, uint16_t cpu_num);
 
 #endif
-- 
1.9.1