[RFC PATCH v2 2/7] x86: kexec: Transfer existing fdt to the new kernel

[Stanislav Kinsburskii <skinsburskii@linux.microsoft.com>]

From: Stanislav Kinsburskii <stanislav.kinsburskii@gmail.com>

Enable passing of the Flattened Device Tree (fdt) over kexec for x86
architecture, as outlined in Documentation/x86/booting-dt.rst.

Signed-off-by: Stanislav Kinsburskii <skinsburskii@linux.microsoft.com>
---
 arch/x86/Kconfig                  |    8 +++++
 arch/x86/kernel/kexec-bzimage64.c |   58 +++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index e36261b4ea14..efb472e267ec 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -2070,6 +2070,14 @@ config KEXEC_FILE
 	  for kernel and initramfs as opposed to list of segments as
 	  accepted by previous system call.
 
+config KEXEC_FILE_FDT
+	bool "Pass fdt over kexec"
+	depends on KEXEC_FILE && X86_64
+	depends on OF_FLATTREE
+	help
+	  This option enables passing existent Flattened Device Tree to the new
+	  kernel when kexec is invoked by the file based system call.
+
 config ARCH_HAS_KEXEC_PURGATORY
 	def_bool KEXEC_FILE
 
diff --git a/arch/x86/kernel/kexec-bzimage64.c b/arch/x86/kernel/kexec-bzimage64.c
index a61c12c01270..ab9ae02c9a5f 100644
--- a/arch/x86/kernel/kexec-bzimage64.c
+++ b/arch/x86/kernel/kexec-bzimage64.c
@@ -18,6 +18,8 @@
 #include <linux/mm.h>
 #include <linux/efi.h>
 #include <linux/random.h>
+#include <linux/of_fdt.h>
+#include <linux/libfdt.h>
 
 #include <asm/bootparam.h>
 #include <asm/setup.h>
@@ -381,7 +383,59 @@ static int bzImage64_probe(const char *buf, unsigned long len)
 
 	return ret;
 }
+#ifdef CONFIG_KEXEC_FILE_FDT
+static void *fdt_get_runtime(void)
+{
+	return initial_boot_params;
+}
+
+static int kexec_setup_fdt(struct kexec_buf *kbuf, struct boot_params *params)
+{
+	void *fdt;
+	struct setup_data *sd;
+	unsigned long fdt_load_addr, fdt_sz;
+	int ret;
+
+	fdt = fdt_get_runtime();
+	if (!fdt)
+		return 0;
+
+	fdt_sz = fdt_totalsize(fdt);
+
+	kbuf->bufsz = kbuf->memsz = sizeof(struct setup_data) + fdt_sz;
+
+	sd = kzalloc(kbuf->bufsz, GFP_KERNEL);
+	if (!sd)
+		return -ENOMEM;
+
+	kbuf->buffer = sd;
+	kbuf->buf_align = PAGE_SIZE;
+	kbuf->buf_min = MIN_INITRD_LOAD_ADDR;
+	kbuf->mem = KEXEC_BUF_MEM_UNKNOWN;
+	ret = kexec_add_buffer(kbuf);
+	if (ret)
+		return ret;
+
+	fdt_load_addr = kbuf->mem;
 
+	pr_debug("Loaded fdt at 0x%lx bufsz=0x%lx memsz=0x%lx\n",
+		fdt_load_addr, fdt_sz, fdt_sz);
+
+	sd->type = SETUP_DTB;
+	sd->len = fdt_sz;
+	memcpy(sd->data, fdt, fdt_sz);
+
+	sd->next = params->hdr.setup_data;
+	params->hdr.setup_data = fdt_load_addr;
+
+	return 0;
+}
+#else
+static int kexec_setup_fdt(struct kexec_buf *kbuf, struct boot_params *params)
+{
+	return 0;
+}
+#endif
 static void *bzImage64_load(struct kimage *image, char *kernel,
 			    unsigned long kernel_len, char *initrd,
 			    unsigned long initrd_len, char *cmdline,
@@ -561,6 +615,10 @@ static void *bzImage64_load(struct kimage *image, char *kernel,
 	if (ret)
 		goto out_free_params;
 
+	ret = kexec_setup_fdt(&kbuf, params);
+	if (ret)
+		goto out_free_params;
+
 	/* Allocate loader specific data */
 	ldata = kzalloc(sizeof(struct bzimage64_data), GFP_KERNEL);
 	if (!ldata) {