[RFC PATCH 2/4] kho: Add support for multiple versions in subtrees

[Logan Odell <loganodell@google.com>]

Add explicit version support to KHO by creating subnodes with
the version number. The first caller to kho_add_subtree_version will
create the parent node and versioned subnode. Subsequent calls with a
different version will add more subnodes with the version as the name.
Remove is updated to handle cleanup when the final version is removed.

Example Tree:

       / (Root Node)
       ├── compatible = "kho-v1"
       ├── preserved-memory-map = <0xphys_map>
       ├── some_legacy_node (Legacy Node)
       │   ├── preserved-data = <0xphys_legacy>
       │   └── blob-size = <0xsize_legacy>
       └── LUO (Versioned Parent Node)
           ├── 1 (Version 1 Node)
           │   ├── preserved-data = <0xphys_luo_v1>
           │   └── blob-size = <0xsize_luo_v1>
           └── 2 (Version 2 Node)
               ├── preserved-data = <0xphys_luo_v2>
               └── blob-size = <0xsize_luo_v2>

Signed-off-by: Logan Odell <loganodell@google.com>
---
 include/linux/kexec_handover.h         |  14 ++
 include/linux/kho/abi/kexec_handover.h |  16 ++
 kernel/liveupdate/kexec_handover.c     | 215 ++++++++++++++++++++++++-
 3 files changed, 237 insertions(+), 8 deletions(-)

diff --git a/include/linux/kexec_handover.h b/include/linux/kexec_handover.h
index ac4129d1d741..1d47ab76b02b 100644
--- a/include/linux/kexec_handover.h
+++ b/include/linux/kexec_handover.h
@@ -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,
diff --git a/include/linux/kho/abi/kexec_handover.h b/include/linux/kho/abi/kexec_handover.h
index 2201a0d2c159..55acb73b1d27 100644
--- a/include/linux/kho/abi/kexec_handover.h
+++ b/include/linux/kho/abi/kexec_handover.h
@@ -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. */
diff --git a/kernel/liveupdate/kexec_handover.c b/kernel/liveupdate/kexec_handover.c
index 29a05cec2625..6dc7c328a6e2 100644
--- a/kernel/liveupdate/kexec_handover.c
+++ b/kernel/liveupdate/kexec_handover.c
@@ -20,6 +20,7 @@
 #include <linux/list.h>
 #include <linux/memblock.h>
 #include <linux/page-isolation.h>
+#include <linux/slab.h>
 #include <linux/unaligned.h>
 #include <linux/vmalloc.h>
 
@@ -722,15 +723,31 @@ static void __init kho_reserve_scratch(void)
 	kho_enable = false;
 }
 
+static int fdt_err_to_errno(int fdt_err)
+{
+	switch (fdt_err) {
+	case 0:
+		return 0;
+	case -FDT_ERR_NOSPACE:
+		return -ENOSPC;
+	case -FDT_ERR_EXISTS:
+		return -EEXIST;
+	case -FDT_ERR_NOTFOUND:
+		return -ENOENT;
+	default:
+		return -EINVAL;
+	}
+}
+
 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);
+	u64 phys = virt_to_phys(fdt);
 
 	err = fdt_setprop(root_fdt, off, KHO_FDT_SUB_TREE_PROP_NAME,
 			  &phys, sizeof(phys));
 	if (err)
-		return err;
+		return fdt_err_to_errno(err);
 
 	WARN_ON_ONCE(kho_debugfs_fdt_add(&kho_out.dbg, name, fdt, false));
 
@@ -762,12 +779,11 @@ int kho_add_subtree(const char *name, void *fdt)
 
 	fdt_err = fdt_open_into(root_fdt, root_fdt, PAGE_SIZE);
 	if (fdt_err < 0)
-		return err;
+		return fdt_err_to_errno(fdt_err);
 
 	off = fdt_add_subnode(root_fdt, 0, name);
 	if (off < 0) {
-		if (off == -FDT_ERR_EXISTS)
-			err = -EEXIST;
+		err = fdt_err_to_errno(off);
 		goto out_pack;
 	}
 
@@ -780,6 +796,93 @@ int kho_add_subtree(const char *name, void *fdt)
 }
 EXPORT_SYMBOL_GPL(kho_add_subtree);
 
+/**
+ * kho_add_subtree_version - record physical address of a sub FDT in KHO root tree with version.
+ * @name: name of the sub tree group.
+ * @version: version of the sub tree.
+ * @fdt: the sub tree blob.
+ *
+ * Finds or creates a child node named @name in KHO root FDT, and then
+ * creates a child node named @version under @name, and records
+ * the physical address of @fdt there.
+ *
+ * A debugfs blob entry is also created at
+ * ``/sys/kernel/debug/kho/out/sub_fdts/@name-@version`` when kernel is configured with
+ * CONFIG_KEXEC_HANDOVER_DEBUGFS
+ *
+ * Return: 0 on success, error code on failure
+ */
+int kho_add_subtree_version(const char *name, int version, void *fdt)
+{
+	void *root_fdt = kho_out.fdt;
+	int err = -ENOMEM;
+	int off, sub_off, fdt_err;
+	char version_str[12];
+	char *dbg_name = NULL;
+	bool created_parent = false;
+
+	mutex_lock(&kho_out.lock);
+
+	fdt_err = fdt_open_into(root_fdt, root_fdt, PAGE_SIZE);
+	if (fdt_err < 0) {
+		err = fdt_err_to_errno(fdt_err);
+		goto out;
+	}
+
+	off = fdt_subnode_offset(root_fdt, 0, name);
+	if (off == -FDT_ERR_NOTFOUND) {
+		off = fdt_add_subnode(root_fdt, 0, name);
+		if (off < 0) {
+			err = fdt_err_to_errno(off);
+			goto out_pack;
+		}
+		created_parent = true;
+	} else if (off < 0) {
+		err = fdt_err_to_errno(off);
+		goto out_pack;
+	}
+
+	snprintf(version_str, sizeof(version_str), "%d", version);
+	sub_off = fdt_add_subnode(root_fdt, off, version_str);
+	if (sub_off < 0) {
+		err = fdt_err_to_errno(sub_off);
+		goto err_del_parent;
+	}
+
+	dbg_name = kasprintf(GFP_KERNEL, "%s-%s", name, version_str);
+	if (!dbg_name) {
+		err = -ENOMEM;
+		goto err_del_subnode;
+	}
+
+	err = kho_add_subtree_node(dbg_name, root_fdt, fdt, sub_off);
+	if (err)
+		goto err_free_dbg_name;
+
+	kfree(dbg_name);
+
+out_pack:
+	fdt_pack(root_fdt);
+out:
+	mutex_unlock(&kho_out.lock);
+	return err;
+
+err_free_dbg_name:
+	kfree(dbg_name);
+err_del_subnode:
+	if (created_parent)
+		fdt_del_node(root_fdt, off);
+	else
+		fdt_del_node(root_fdt, sub_off);
+	goto out_pack;
+
+err_del_parent:
+	if (created_parent)
+		fdt_del_node(root_fdt, off);
+	goto out_pack;
+}
+EXPORT_SYMBOL_GPL(kho_add_subtree_version);
+
 void kho_remove_subtree(void *fdt)
 {
 	phys_addr_t target_phys = virt_to_phys(fdt);
@@ -799,10 +902,10 @@ void kho_remove_subtree(void *fdt)
 		int len;
 
 		val = fdt_getprop(root_fdt, off, KHO_FDT_SUB_TREE_PROP_NAME, &len);
-		if (!val || len != sizeof(phys_addr_t))
+		if (!val || len != sizeof(*val))
 			continue;
 
-		if ((phys_addr_t)*val == target_phys) {
+		if ((phys_addr_t)get_unaligned(val) == target_phys) {
 			fdt_del_node(root_fdt, off);
 			kho_debugfs_fdt_remove(&kho_out.dbg, fdt);
 			break;
@@ -813,6 +916,59 @@ void kho_remove_subtree(void *fdt)
 }
 EXPORT_SYMBOL_GPL(kho_remove_subtree);
 
+/**
+ * kho_remove_subtree_version - remove a versioned sub FDT from KHO root tree.
+ * @name: name of the sub tree group.
+ * @version: version of the sub tree.
+ * @fdt: the sub tree blob to remove.
+ *
+ * Removes the @version subnode under @name node and its associated debugfs entry.
+ * If @name node becomes empty, it is also removed.
+ */
+void kho_remove_subtree_version(const char *name, int version, void *fdt)
+{
+	phys_addr_t target_phys = virt_to_phys(fdt);
+	void *root_fdt = kho_out.fdt;
+	int off, sub_off;
+	int err;
+	char version_str[12];
+
+	mutex_lock(&kho_out.lock);
+
+	err = fdt_open_into(root_fdt, root_fdt, PAGE_SIZE);
+	if (err < 0)
+		goto out;
+
+	off = fdt_subnode_offset(root_fdt, 0, name);
+	if (off < 0)
+		goto out_pack;
+
+	snprintf(version_str, sizeof(version_str), "%d", version);
+	sub_off = fdt_subnode_offset(root_fdt, off, version_str);
+	if (sub_off < 0)
+		goto out_pack;
+
+	{
+		const u64 *val;
+		int len;
+
+		val = fdt_getprop(root_fdt, sub_off, KHO_FDT_SUB_TREE_PROP_NAME, &len);
+		if (val && len == sizeof(*val) && (phys_addr_t)get_unaligned(val) == target_phys) {
+			fdt_del_node(root_fdt, sub_off);
+			kho_debugfs_fdt_remove(&kho_out.dbg, fdt);
+
+			if (fdt_first_subnode(root_fdt, off) == -FDT_ERR_NOTFOUND)
+				fdt_del_node(root_fdt, off);
+		}
+	}
+
+out_pack:
+	fdt_pack(root_fdt);
+out:
+	mutex_unlock(&kho_out.lock);
+}
+EXPORT_SYMBOL_GPL(kho_remove_subtree_version);
+
 /**
  * kho_preserve_folio - preserve a folio across kexec.
  * @folio: folio to preserve.
@@ -1336,12 +1492,55 @@ int kho_retrieve_subtree(const char *name, phys_addr_t *phys)
 	if (!val || len != sizeof(*val))
 		return -EINVAL;
 
-	*phys = (phys_addr_t)*val;
+	*phys = (phys_addr_t)get_unaligned(val);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(kho_retrieve_subtree);
 
+/**
+ * kho_retrieve_subtree_version - retrieve a preserved versioned sub FDT.
+ * @name: the name of the sub FDT group passed to kho_add_subtree_version().
+ * @version: the version of the sub FDT.
+ * @phys: if found, the physical address of the sub FDT is stored in @phys.
+ *
+ * Retrieve a preserved sub FDT named @version under @name and store its
+ * physical address in @phys.
+ *
+ * Return: 0 on success, error code on failure
+ */
+int kho_retrieve_subtree_version(const char *name, int version, phys_addr_t *phys)
+{
+	const void *fdt = kho_get_fdt();
+	const u64 *val;
+	int offset, sub_offset, len;
+	char version_str[12];
+
+	if (!fdt)
+		return -ENOENT;
+
+	if (!phys)
+		return -EINVAL;
+
+	offset = fdt_subnode_offset(fdt, 0, name);
+	if (offset < 0)
+		return -ENOENT;
+
+	snprintf(version_str, sizeof(version_str), "%d", version);
+	sub_offset = fdt_subnode_offset(fdt, offset, version_str);
+	if (sub_offset < 0)
+		return -ENOENT;
+
+	val = fdt_getprop(fdt, sub_offset, KHO_FDT_SUB_TREE_PROP_NAME, &len);
+	if (!val || len != sizeof(*val))
+		return -EINVAL;
+
+	*phys = (phys_addr_t)get_unaligned(val);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(kho_retrieve_subtree_version);
+
 static __init int kho_out_fdt_setup(void)
 {
 	void *root = kho_out.fdt;
-- 
2.55.0.508.g3f0d502094-goog