[RFC PATCH v2 3/7] x86: kexec: Enable fdt modification in callbacks

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

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

This option allows kernel subsystems to modify (or create, if necessary)
the Flattened Device Tree (fdt) using registered callbacks and then pass
the modified version to the new kernel.

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

diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index efb472e267ec..90da51fbb8f8 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -2078,6 +2078,14 @@ config KEXEC_FILE_FDT
 	  This option enables passing existent Flattened Device Tree to the new
 	  kernel when kexec is invoked by the file based system call.
 
+config KEXEC_FILE_FDT_CALLBACK
+	bool "Enable kexec fdt modification support"
+	depends on KEXEC_FILE_FDT
+	select LIBFDT
+	help
+	  This option enables Flattened Device Tree modification (and creation
+	  if needed) by kernel subsystems, registered corresponding callback.
+
 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 ab9ae02c9a5f..3c6df28d3637 100644
--- a/arch/x86/kernel/kexec-bzimage64.c
+++ b/arch/x86/kernel/kexec-bzimage64.c
@@ -384,11 +384,50 @@ static int bzImage64_probe(const char *buf, unsigned long len)
 	return ret;
 }
 #ifdef CONFIG_KEXEC_FILE_FDT
+#ifdef CONFIG_KEXEC_FILE_FDT_CALLBACK
+static void *fdt_get_runtime(void)
+{
+	void *fdt;
+	size_t fdt_size = SZ_2M;
+	int status;
+
+	/* It's nothing to do without existent fdt and any callbacks */
+	if (!initial_boot_params && kexec_fdt_notify_list_empty())
+		return NULL;
+
+	fdt = kzalloc(fdt_size, GFP_KERNEL);
+	if (!fdt)
+		return NULL;
+
+	if (initial_boot_params)
+		status = fdt_open_into(initial_boot_params, fdt, fdt_size);
+	else
+		status = fdt_create_empty_tree(fdt, fdt_size);
+	if (status != 0) {
+		pr_err("failed to get fdt\n");
+		goto free_fdt;
+	}
+
+	status = kexec_fdt_notify(fdt);
+	if (status) {
+		pr_err("fdt notification failed\n");
+		goto free_fdt;
+	}
+
+	fdt_pack(fdt);
+
+	return fdt;
+
+free_fdt:
+	kfree(fdt);
+	return NULL;
+}
+#else
 static void *fdt_get_runtime(void)
 {
 	return initial_boot_params;
 }
-
+#endif
 static int kexec_setup_fdt(struct kexec_buf *kbuf, struct boot_params *params)
 {
 	void *fdt;