Add support for multiple versions to KHO

index cc68a3692905..29a05cec2625 100644
@@ -722,6 +722,21 @@ static void __init kho_reserve_scratch(void)
 	kho_enable = false;
 }
 
+static int kho_add_subtree_node(const char *name, void *root_fdt, void *fdt, int off)
+{
+	int err;
+	phys_addr_t phys = virt_to_phys(fdt);
+
+	err = fdt_setprop(root_fdt, off, KHO_FDT_SUB_TREE_PROP_NAME,
+			  &phys, sizeof(phys));
+	if (err)
+		return err;
+
+	WARN_ON_ONCE(kho_debugfs_fdt_add(&kho_out.dbg, name, fdt, false));
+
+	return 0;
+}
+
 /**
  * kho_add_subtree - record the physical address of a sub FDT in KHO root tree.
  * @name: name of the sub tree.

@@ -739,7 +754,6 @@ static void __init kho_reserve_scratch(void)
  */
 int kho_add_subtree(const char *name, void *fdt)
 {
-	phys_addr_t phys = virt_to_phys(fdt);
 	void *root_fdt = kho_out.fdt;
 	int err = -ENOMEM;
 	int off, fdt_err;

@@ -757,12 +771,7 @@ int kho_add_subtree(const char *name, void *fdt)
 		goto out_pack;
 	}
 
-	err = fdt_setprop(root_fdt, off, KHO_FDT_SUB_TREE_PROP_NAME,
-			  &phys, sizeof(phys));
-	if (err < 0)
-		goto out_pack;
-
-	WARN_ON_ONCE(kho_debugfs_fdt_add(&kho_out.dbg, name, fdt, false));
+	err = kho_add_subtree_node(name, root_fdt, fdt, off);
 
 out_pack:
 	fdt_pack(root_fdt);

index ac4129d1d741..1d47ab76b02b 100644
@@ -33,8 +33,11 @@ struct folio *kho_restore_folio(phys_addr_t phys);
 struct page *kho_restore_pages(phys_addr_t phys, unsigned long nr_pages);
 void *kho_restore_vmalloc(const struct kho_vmalloc *preservation);
 int kho_add_subtree(const char *name, void *fdt);
+int kho_add_subtree_version(const char *name, int version, void *fdt);
 void kho_remove_subtree(void *fdt);
+void kho_remove_subtree_version(const char *name, int version, void *fdt);
 int kho_retrieve_subtree(const char *name, phys_addr_t *phys);
+int kho_retrieve_subtree_version(const char *name, int version, phys_addr_t *phys);
 
 void kho_memory_init(void);
 

@@ -102,13 +105,24 @@ static inline int kho_add_subtree(const char *name, void *fdt)
 	return -EOPNOTSUPP;
 }
 
+static inline int kho_add_subtree_version(const char *name, int version, void *fdt)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline void kho_remove_subtree(void *fdt) { }
+static inline void kho_remove_subtree_version(const char *name, int version, void *fdt) { }
 
 static inline int kho_retrieve_subtree(const char *name, phys_addr_t *phys)
 {
 	return -EOPNOTSUPP;
 }
 
+static inline int kho_retrieve_subtree_version(const char *name, int version, phys_addr_t *phys)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline void kho_memory_init(void) { }
 
 static inline void kho_populate(phys_addr_t fdt_phys, u64 fdt_len,

index 2201a0d2c159..55acb73b1d27 100644
@@ -73,6 +73,22 @@
  *
  *       Physical address pointing to a subnode FDT blob that is also
  *       being preserved.
+ *
+ *   Versioned Subnodes (<subnode-name-N>):
+ *     Alternatively, a subnode can group multiple versions of the same
+ *     data. In this case, the subnode <subnode-name-N> does not contain
+ *     an 'fdt' property directly. Instead, it contains child nodes named
+ *     after the version number (e.g., '1', '2'), which in turn contain
+ *     the 'fdt' property.
+ *
+ *     <subnode-name-N> {
+ *         <version-1> {
+ *             fdt = <0x...>;
+ *         };
+ *         <version-2> {
+ *             fdt = <0x...>;
+ *         };
+ *     };
  */
 
 /* The compatible string for the KHO FDT root node. */

274 lines not on this page · raw message

index 2f93939168ab..2ca055219900 100644
@@ -13,6 +13,7 @@
 #include <linux/io.h>
 #include <linux/libfdt.h>
 #include <linux/mm.h>
+#include <linux/unaligned.h>
 #include "kexec_handover_internal.h"
 
 static struct dentry *debugfs_root;

@@ -140,19 +141,49 @@ __init void kho_in_debugfs_init(struct kho_debugfs *dbg, const void *fdt)
 		const u64 *fdt_phys;
 
 		fdt_phys = fdt_getprop(fdt, child, "fdt", &len);
-		if (!fdt_phys)
-			continue;
-		if (len != sizeof(*fdt_phys)) {
-			pr_warn("node %s prop fdt has invalid length: %d\n",
-				name, len);
-			continue;
-		}
-		err = __kho_debugfs_fdt_add(&dbg->fdt_list, sub_fdt_dir, name,
-					    phys_to_virt(*fdt_phys));
-		if (err) {
-			pr_warn("failed to add fdt %s to debugfs: %pe\n", name,
-				ERR_PTR(err));
-			continue;
+		if (fdt_phys) {
+			if (len != sizeof(*fdt_phys)) {
+				pr_warn("node %s prop fdt has invalid length: %d\n",
+					name, len);
+				continue;
+			}
+			err = __kho_debugfs_fdt_add(&dbg->fdt_list, sub_fdt_dir, name,
+						    phys_to_virt(get_unaligned(fdt_phys)));
+			if (err) {
+				pr_warn("failed to add fdt %s to debugfs: %pe\n", name,
+					ERR_PTR(err));
+			}
+		} else {
+			int grandchild;
+
+			fdt_for_each_subnode(grandchild, fdt, child) {
+				const char *gc_name = fdt_get_name(fdt, grandchild, NULL);
+				const u64 *gc_fdt_phys;
+				char *combined_name;
+
+				gc_fdt_phys = fdt_getprop(fdt, grandchild, "fdt", &len);
+				if (!gc_fdt_phys)
+					continue;
+
+				if (len != sizeof(*gc_fdt_phys)) {
+					pr_warn("node %s/%s prop fdt has invalid length: %d\n",
+						name, gc_name, len);
+					continue;
+				}
+
+				combined_name = kasprintf(GFP_KERNEL, "%s-%s", name, gc_name);
+				if (!combined_name)
+					continue;
+
+				err = __kho_debugfs_fdt_add(&dbg->fdt_list, sub_fdt_dir,
+							combined_name,
+							phys_to_virt(get_unaligned(gc_fdt_phys)));
+				kfree(combined_name);
+				if (err) {
+					pr_warn("failed to add fdt %s-%s to debugfs: %pe\n",
+						name, gc_name, ERR_PTR(err));
+				}
+			}
 		}
 	}
 

index 7ef9e4061869..db531da63576 100644
@@ -310,52 +310,276 @@ static int kho_test_restore(phys_addr_t fdt_phys)
 	return 0;
 }
 
+static void kho_test_cleanup(void)
+{
+	/* unpreserve and free the data stored in folios */
+	kho_test_unpreserve_data(&kho_test_state);
+	for (int i = 0; i < kho_test_state.nr_folios; i++)
+		folio_put(kho_test_state.folios[i]);
+
+	kvfree(kho_test_state.folios);
+
+	/* Unpreserve and release the FDT folio */
+	kho_unpreserve_folio(kho_test_state.fdt);
+	folio_put(kho_test_state.fdt);
+}
+
+#define KHO_TEST_VER_NAME	"kho_test_ver"
+#define KHO_TEST_VER_1		1
+#define KHO_TEST_VER_2		2
+#define KHO_TEST_VER_MAGIC_1	0x11111111
+#define KHO_TEST_VER_MAGIC_2	0x22222222
+
+struct kho_test_versioned_state {
+	struct folio *fdt1;
+	struct folio *fdt2;
+};
+
+static struct kho_test_versioned_state kho_test_ver_state;
+
+static int kho_test_prepare_ver_fdt(struct folio *folio, int version, unsigned int magic)
+{
+	void *fdt = folio_address(folio);
+	int err;
+
+	err = fdt_create(fdt, PAGE_SIZE);
+	err |= fdt_finish_reservemap(fdt);
+	err |= fdt_begin_node(fdt, "");
+	err |= fdt_property(fdt, "compatible", KHO_TEST_COMPAT, sizeof(KHO_TEST_COMPAT));
+	err |= fdt_property(fdt, "magic", &magic, sizeof(magic));
+	err |= fdt_property(fdt, "version", &version, sizeof(version));
+	err |= fdt_end_node(fdt);
+	err |= fdt_finish(fdt);
+
+	return err;
+}
+
+static int kho_test_ver_save(void)
+{
+	struct kho_test_versioned_state *state = &kho_test_ver_state;
+	int err;
+
+	state->fdt1 = folio_alloc(GFP_KERNEL, 0);
+	if (!state->fdt1)
+		return -ENOMEM;
+
+	state->fdt2 = folio_alloc(GFP_KERNEL, 0);
+	if (!state->fdt2) {
+		err = -ENOMEM;
+		goto err_free_fdt1;
+	}
+
+	err = kho_preserve_folio(state->fdt1);
+	if (err)
+		goto err_free_fdt2;
+
+	err = kho_preserve_folio(state->fdt2);
+	if (err)
+		goto err_unpreserve_fdt1;
+
+	err = kho_test_prepare_ver_fdt(state->fdt1, KHO_TEST_VER_1, KHO_TEST_VER_MAGIC_1);
+	if (err)
+		goto err_unpreserve_fdt2;
+
+	err = kho_test_prepare_ver_fdt(state->fdt2, KHO_TEST_VER_2, KHO_TEST_VER_MAGIC_2);
+	if (err)
+		goto err_unpreserve_fdt2;
+
+	err = kho_add_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_1,
+				      folio_address(state->fdt1));
+	if (err)
+		goto err_unpreserve_fdt2;
+
+	err = kho_add_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_2,
+				      folio_address(state->fdt2));
+	if (err)
+		goto err_remove_ver1;
+
+	pr_info("KHO versioned test save succeeded\n");
+	return 0;
+
+err_remove_ver1:
+	kho_remove_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_1, folio_address(state->fdt1));
+err_unpreserve_fdt2:
+	kho_unpreserve_folio(state->fdt2);
+err_unpreserve_fdt1:
+	kho_unpreserve_folio(state->fdt1);
+err_free_fdt2:
+	folio_put(state->fdt2);
+	state->fdt2 = NULL;
+err_free_fdt1:
+	folio_put(state->fdt1);
+	state->fdt1 = NULL;
+	return err;
+}
+
+static int kho_test_ver_restore_one(phys_addr_t fdt_phys, int expected_version,
+				    unsigned int expected_magic)
+{
+	void *fdt = phys_to_virt(fdt_phys);
+	const unsigned int *magic;
+	const int *version;
+	int node, len;
+
+	node = fdt_path_offset(fdt, "/");
+	if (node < 0)
+		return -EINVAL;
+
+	if (fdt_node_check_compatible(fdt, node, KHO_TEST_COMPAT))
+		return -EINVAL;
+
+	magic = fdt_getprop(fdt, node, "magic", &len);
+	if (!magic || len != sizeof(*magic) || *magic != expected_magic)
+		return -EINVAL;
+
+	version = fdt_getprop(fdt, node, "version", &len);
+	if (!version || len != sizeof(*version) || *version != expected_version)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int kho_test_ver_restore(void)
+{
+	phys_addr_t fdt_phys1, fdt_phys2;
+	struct folio *fdt1 = NULL, *fdt2 = NULL;
+	int err;
+
+	err = kho_retrieve_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_1, &fdt_phys1);
+	if (err) {
+		if (err != -ENOENT)
+			pr_err("Failed to retrieve version 1: %d\n", err);
+		return err;
+	}
+
+	fdt1 = kho_restore_folio(fdt_phys1);
+	if (!fdt1) {
+		pr_err("Failed to restore FDT 1 folio\n");
+		return -EINVAL;
+	}
+
+	err = kho_retrieve_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_2, &fdt_phys2);
+	if (err) {
+		pr_err("Failed to retrieve version 2: %d\n", err);
+		goto out_put_fdt1;
+	}
+
+	fdt2 = kho_restore_folio(fdt_phys2);
+	if (!fdt2) {
+		pr_err("Failed to restore FDT 2 folio\n");
+		err = -EINVAL;
+		goto out_put_fdt1;
+	}
+
+	err = kho_test_ver_restore_one(fdt_phys1, KHO_TEST_VER_1, KHO_TEST_VER_MAGIC_1);
+	if (err) {
+		pr_err("Version 1 verification failed: %d\n", err);
+		goto out_put_fdt2;
+	}
+
+	err = kho_test_ver_restore_one(fdt_phys2, KHO_TEST_VER_2, KHO_TEST_VER_MAGIC_2);
+	if (err) {
+		pr_err("Version 2 verification failed: %d\n", err);
+		goto out_put_fdt2;
+	}
+
+	pr_info("KHO versioned test restore succeeded\n");
+
+out_put_fdt2:
+	folio_put(fdt2);
+out_put_fdt1:
+	folio_put(fdt1);
+	return err;
+}
+
+static void kho_test_ver_cleanup(void)
+{
+	struct kho_test_versioned_state *state = &kho_test_ver_state;
+
+	if (state->fdt1) {
+		kho_unpreserve_folio(state->fdt1);
+		folio_put(state->fdt1);
+		state->fdt1 = NULL;
+	}
+	if (state->fdt2) {
+		kho_unpreserve_folio(state->fdt2);
+		folio_put(state->fdt2);
+		state->fdt2 = NULL;
+	}
+}
+
+static void kho_test_ver_exit(void)
+{
+	struct kho_test_versioned_state *state = &kho_test_ver_state;
+
+	if (state->fdt1)
+		kho_remove_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_1,
+					   folio_address(state->fdt1));
+	if (state->fdt2)
+		kho_remove_subtree_version(KHO_TEST_VER_NAME, KHO_TEST_VER_2,
+					   folio_address(state->fdt2));
+	kho_test_ver_cleanup();
+}
+
 static int __init kho_test_init(void)
 {
 	phys_addr_t fdt_phys;
-	int err;
+	int err, ver_err;
+	bool restored = false;
 
 	if (!kho_is_enabled())
 		return 0;
 
+	// Try to restore non-versioned
 	err = kho_retrieve_subtree(KHO_TEST_FDT, &fdt_phys);
 	if (!err) {
 		err = kho_test_restore(fdt_phys);
-		if (err)
+		if (err) {
 			pr_err("KHO restore failed\n");
-		else
-			pr_info("KHO restore succeeded\n");
-
+			return err;
+		}
+		pr_info("KHO restore succeeded\n");
+		restored = true;
+	} else if (err != -ENOENT) {
+		pr_warn("failed to retrieve %s FDT: %d\n", KHO_TEST_FDT, err);
 		return err;
 	}
 
-	if (err != -ENOENT) {
-		pr_warn("failed to retrieve %s FDT: %d\n", KHO_TEST_FDT, err);
-		return err;
+	// Try to restore versioned
+	ver_err = kho_test_ver_restore();
+	if (!ver_err) {
+		restored = true;
+	} else if (ver_err != -ENOENT) {
+		pr_warn("KHO versioned restore failed: %d\n", ver_err);
+		return ver_err;
 	}
 
-	return kho_test_save();
-}
-module_init(kho_test_init);
+	if (restored)
+		return 0;
 
-static void kho_test_cleanup(void)
-{
-	/* unpreserve and free the data stored in folios */
-	kho_test_unpreserve_data(&kho_test_state);
-	for (int i = 0; i < kho_test_state.nr_folios; i++)
-		folio_put(kho_test_state.folios[i]);
+	// If we got here, it means both returned -ENOENT (first boot).
+	// So we save both.
 
-	kvfree(kho_test_state.folios);
+	err = kho_test_save();
+	if (err)
+		return err;
 
-	/* Unpreserve and release the FDT folio */
-	kho_unpreserve_folio(kho_test_state.fdt);
-	folio_put(kho_test_state.fdt);
+	ver_err = kho_test_ver_save();
+	if (ver_err) {
+		kho_remove_subtree(folio_address(kho_test_state.fdt));
+		kho_test_cleanup();
+		return ver_err;
+	}
+
+	return 0;
 }
+module_init(kho_test_init);
 
 static void __exit kho_test_exit(void)
 {
 	kho_remove_subtree(folio_address(kho_test_state.fdt));
 	kho_test_cleanup();
+	kho_test_ver_exit();
 }
 module_exit(kho_test_exit);