From 104a2ab39ac72c402d7463bec05aae4a6d09cfbf Mon Sep 17 00:00:00 2001
From: Tobias Waldekranz <tobias@waldekranz.com>
Date: Thu, 16 Feb 2023 15:06:12 +0100
Subject: [PATCH v2 3/9] blk: blkmap: Add basic infrastructure

blkmaps are loosely modeled on Linux's device mapper subsystem. The
basic idea is that you can create virtual block devices whose blocks
can be backed by a plethora of sources that are user configurable.

This change just adds the basic infrastructure for creating and
removing blkmap devices. Subsequent changes will extend this to add
support for actual mappings.

Signed-off-by: Tobias Waldekranz <tobias@waldekranz.com>
---
 MAINTAINERS                |   6 +
 drivers/block/Kconfig      |  18 ++
 drivers/block/Makefile     |   1 +
 drivers/block/blk-uclass.c |   1 +
 drivers/block/blkmap.c     | 343 +++++++++++++++++++++++++++++++++++++
 include/blkmap.h           |  35 ++++
 include/dm/uclass-id.h     |   1 +
 7 files changed, 405 insertions(+)
 create mode 100644 drivers/block/blkmap.c
 create mode 100644 include/blkmap.h

diff --git a/MAINTAINERS b/MAINTAINERS
index 6f53f9c2f6..3e47c9b34c 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -793,6 +793,12 @@ M:	Alper Nebi Yasak <alpernebiyasak@gmail.com>
 S:	Maintained
 F:	tools/binman/
 
+BLKMAP
+M:	Tobias Waldekranz <tobias@waldekranz.com>
+S:	Maintained
+F:	drivers/block/blkmap.c
+F:	include/blkmap.h
+
 BOOTDEVICE
 M:	Simon Glass <sjg@chromium.org>
 S:	Maintained
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index e95da48bdc..5a1aeb3d2b 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -67,6 +67,24 @@ config BLOCK_CACHE
 	  it will prevent repeated reads from directory structures and other
 	  filesystem data structures.
 
+config BLKMAP
+	bool "Composable virtual block devices (blkmap)"
+	depends on BLK
+	help
+ 	  Create virtual block devices that are backed by various sources,
+ 	  e.g. RAM, or parts of an existing block device. Though much more
+ 	  rudimentary, it borrows a lot of ideas from Linux's device mapper
+ 	  subsystem.
+
+	  Example use-cases:
+	  - Treat a region of RAM as a block device, i.e. a RAM disk. This let's
+            you extract files from filesystem images stored in RAM (perhaps as a
+            result of a TFTP transfer).
+	  - Create a virtual partition on an existing device. This let's you
+            access filesystems that aren't stored at an exact partition
+            boundary. A common example is a filesystem image embedded in an FIT
+            image.
+
 config SPL_BLOCK_CACHE
 	bool "Use block device cache in SPL"
 	depends on SPL_BLK
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index f12447d78d..a161d145fd 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_IDE) += ide.o
 endif
 obj-$(CONFIG_SANDBOX) += sandbox.o host-uclass.o host_dev.o
 obj-$(CONFIG_$(SPL_TPL_)BLOCK_CACHE) += blkcache.o
+obj-$(CONFIG_BLKMAP) += blkmap.o
 
 obj-$(CONFIG_EFI_MEDIA) += efi-media-uclass.o
 obj-$(CONFIG_EFI_MEDIA_SANDBOX) += sb_efi_media.o
diff --git a/drivers/block/blk-uclass.c b/drivers/block/blk-uclass.c
index c69fc4d518..cb73faaeda 100644
--- a/drivers/block/blk-uclass.c
+++ b/drivers/block/blk-uclass.c
@@ -32,6 +32,7 @@ static struct {
 	{ UCLASS_EFI_LOADER, "efiloader" },
 	{ UCLASS_VIRTIO, "virtio" },
 	{ UCLASS_PVBLOCK, "pvblock" },
+	{ UCLASS_BLKMAP, "blkmap" },
 };
 
 static enum uclass_id uclass_name_to_iftype(const char *uclass_idname)
diff --git a/drivers/block/blkmap.c b/drivers/block/blkmap.c
new file mode 100644
index 0000000000..acfc002ceb
--- /dev/null
+++ b/drivers/block/blkmap.c
@@ -0,0 +1,343 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2023 Addiva Elektronik
+ * Author: Tobias Waldekranz <tobias@waldekranz.com>
+ */
+
+#include <common.h>
+#include <blk.h>
+#include <blkmap.h>
+#include <dm.h>
+#include <malloc.h>
+#include <mapmem.h>
+#include <part.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+#include <dm/root.h>
+
+struct blkmap;
+
+/**
+ * struct blkmap_slice - Region mapped to a blkmap
+ *
+ * Common data for a region mapped to a blkmap, specialized by each
+ * map type.
+ *
+ * @node: List node used to associate this slice with a blkmap
+ * @blknr: Start block number of the mapping
+ * @blkcnt: Number of blocks covered by this mapping
+ */
+struct blkmap_slice {
+	struct list_head node;
+
+	lbaint_t blknr;
+	lbaint_t blkcnt;
+
+	/**
+	 * @read: - Read from slice
+	 *
+	 * @read.bm: Blkmap to which this slice belongs
+	 * @read.bms: This slice
+	 * @read.blknr: Start block number to read from
+	 * @read.blkcnt: Number of blocks to read
+	 * @read.buffer: Buffer to store read data to
+	 */
+	ulong (*read)(struct blkmap *bm, struct blkmap_slice *bms,
+		      lbaint_t blknr, lbaint_t blkcnt, void *buffer);
+
+	/**
+	 * @write: - Write to slice
+	 *
+	 * @write.bm: Blkmap to which this slice belongs
+	 * @write.bms: This slice
+	 * @write.blknr: Start block number to write to
+	 * @write.blkcnt: Number of blocks to write
+	 * @write.buffer: Data to be written
+	 */
+	ulong (*write)(struct blkmap *bm, struct blkmap_slice *bms,
+		       lbaint_t blknr, lbaint_t blkcnt, const void *buffer);
+
+	/**
+	 * @destroy: - Tear down slice
+	 *
+	 * @read.bm: Blkmap to which this slice belongs
+	 * @read.bms: This slice
+	 */
+	void (*destroy)(struct blkmap *bm, struct blkmap_slice *bms);
+};
+
+/**
+ * struct blkmap - Block map
+ *
+ * Data associated with a blkmap.
+ *
+ * @label: Human readable name of this blkmap
+ * @blk: Underlying block device
+ * @slices: List of slices associated with this blkmap
+ */
+struct blkmap {
+	char *label;
+	struct udevice *blk;
+	struct list_head slices;
+};
+
+static bool blkmap_slice_contains(struct blkmap_slice *bms, lbaint_t blknr)
+{
+	return (blknr >= bms->blknr) && (blknr < (bms->blknr + bms->blkcnt));
+}
+
+static bool blkmap_slice_available(struct blkmap *bm, struct blkmap_slice *new)
+{
+	struct blkmap_slice *bms;
+	lbaint_t first, last;
+
+	first = new->blknr;
+	last = new->blknr + new->blkcnt - 1;
+
+	list_for_each_entry(bms, &bm->slices, node) {
+		if (blkmap_slice_contains(bms, first) ||
+		    blkmap_slice_contains(bms, last) ||
+		    blkmap_slice_contains(new, bms->blknr) ||
+		    blkmap_slice_contains(new, bms->blknr + bms->blkcnt - 1))
+			return false;
+	}
+
+	return true;
+}
+
+static int blkmap_slice_add(struct blkmap *bm, struct blkmap_slice *new)
+{
+	struct blk_desc *bd = dev_get_uclass_plat(bm->blk);
+	struct list_head *insert = &bm->slices;
+	struct blkmap_slice *bms;
+
+	if (!blkmap_slice_available(bm, new))
+		return -EBUSY;
+
+	list_for_each_entry(bms, &bm->slices, node) {
+		if (bms->blknr < new->blknr)
+			continue;
+
+		insert = &bms->node;
+		break;
+	}
+
+	list_add_tail(&new->node, insert);
+
+	/* Disk might have grown, update the size */
+	bms = list_last_entry(&bm->slices, struct blkmap_slice, node);
+	bd->lba = bms->blknr + bms->blkcnt;
+	return 0;
+}
+
+static ulong blkmap_blk_read_slice(struct blkmap *bm, struct blkmap_slice *bms,
+				   lbaint_t blknr, lbaint_t blkcnt,
+				   void *buffer)
+{
+	lbaint_t nr, cnt;
+
+	nr = blknr - bms->blknr;
+	cnt = (blkcnt < bms->blkcnt) ? blkcnt : bms->blkcnt;
+	return bms->read(bm, bms, nr, cnt, buffer);
+}
+
+static ulong blkmap_blk_read(struct udevice *dev, lbaint_t blknr,
+			     lbaint_t blkcnt, void *buffer)
+{
+	struct blk_desc *bd = dev_get_uclass_plat(dev);
+	struct blkmap *bm = dev_get_plat(dev->parent);
+	struct blkmap_slice *bms;
+	lbaint_t cnt, total = 0;
+
+	list_for_each_entry(bms, &bm->slices, node) {
+		if (!blkmap_slice_contains(bms, blknr))
+			continue;
+
+		cnt = blkmap_blk_read_slice(bm, bms, blknr, blkcnt, buffer);
+		blknr += cnt;
+		blkcnt -= cnt;
+		buffer += cnt << bd->log2blksz;
+		total += cnt;
+	}
+
+	return total;
+}
+
+static ulong blkmap_blk_write_slice(struct blkmap *bm, struct blkmap_slice *bms,
+				    lbaint_t blknr, lbaint_t blkcnt,
+				    const void *buffer)
+{
+	lbaint_t nr, cnt;
+
+	nr = blknr - bms->blknr;
+	cnt = (blkcnt < bms->blkcnt) ? blkcnt : bms->blkcnt;
+	return bms->write(bm, bms, nr, cnt, buffer);
+}
+
+static ulong blkmap_blk_write(struct udevice *dev, lbaint_t blknr,
+			      lbaint_t blkcnt, const void *buffer)
+{
+	struct blk_desc *bd = dev_get_uclass_plat(dev);
+	struct blkmap *bm = dev_get_plat(dev->parent);
+	struct blkmap_slice *bms;
+	lbaint_t cnt, total = 0;
+
+	list_for_each_entry(bms, &bm->slices, node) {
+		if (!blkmap_slice_contains(bms, blknr))
+			continue;
+
+		cnt = blkmap_blk_write_slice(bm, bms, blknr, blkcnt, buffer);
+		blknr += cnt;
+		blkcnt -= cnt;
+		buffer += cnt << bd->log2blksz;
+		total += cnt;
+	}
+
+	return total;
+}
+
+static const struct blk_ops blkmap_blk_ops = {
+	.read	= blkmap_blk_read,
+	.write	= blkmap_blk_write,
+};
+
+U_BOOT_DRIVER(blkmap_blk) = {
+	.name		= "blkmap_blk",
+	.id		= UCLASS_BLK,
+	.ops		= &blkmap_blk_ops,
+};
+
+int blkmap_dev_bind(struct udevice *dev)
+{
+	struct blkmap *bm = dev_get_plat(dev);
+	struct blk_desc *bd;
+	int err;
+
+	err = blk_create_devicef(dev, "blkmap_blk", "blk", UCLASS_BLKMAP,
+				 dev_seq(dev), 512, 0, &bm->blk);
+	if (err)
+		return log_msg_ret("blk", err);
+
+	INIT_LIST_HEAD(&bm->slices);
+
+	bd = dev_get_uclass_plat(bm->blk);
+	snprintf(bd->vendor, BLK_VEN_SIZE, "U-Boot");
+	snprintf(bd->product, BLK_PRD_SIZE, "blkmap");
+	snprintf(bd->revision, BLK_REV_SIZE, "1.0");
+
+	/* EFI core isn't keen on zero-sized disks, so we lie. This is
+	 * updated with the correct size once the user adds a
+	 * mapping.
+	 */
+	bd->lba = 1;
+
+	return 0;
+}
+
+int blkmap_dev_unbind(struct udevice *dev)
+{
+	struct blkmap *bm = dev_get_plat(dev);
+	struct blkmap_slice *bms, *tmp;
+	int err;
+
+	list_for_each_entry_safe(bms, tmp, &bm->slices, node) {
+		list_del(&bms->node);
+		free(bms);
+	}
+
+	err = device_remove(bm->blk, DM_REMOVE_NORMAL);
+	if (err)
+		return err;
+
+	return device_unbind(bm->blk);
+}
+
+U_BOOT_DRIVER(blkmap_root) = {
+	.name		= "blkmap_dev",
+	.id		= UCLASS_BLKMAP,
+	.bind		= blkmap_dev_bind,
+	.unbind		= blkmap_dev_unbind,
+	.plat_auto	= sizeof(struct blkmap),
+};
+
+struct udevice *blkmap_from_label(const char *label)
+{
+	struct udevice *dev;
+	struct uclass *uc;
+	struct blkmap *bm;
+
+	uclass_id_foreach_dev(UCLASS_BLKMAP, dev, uc) {
+		bm = dev_get_plat(dev);
+		if (bm->label && !strcmp(label, bm->label))
+			return dev;
+	}
+
+	return NULL;
+}
+
+int blkmap_create(const char *label, struct udevice **devp)
+{
+	char *hname, *hlabel;
+	struct udevice *dev;
+	struct blkmap *bm;
+	size_t namelen;
+	int err;
+
+	dev = blkmap_from_label(label);
+	if (dev) {
+		err = -EBUSY;
+		goto err;
+	}
+
+	hlabel = strdup(label);
+	if (!hlabel) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	namelen = strlen("blkmap-") + strlen(label) + 1;
+	hname = malloc(namelen);
+	if (!hname) {
+		err = -ENOMEM;
+		goto err_free_hlabel;
+	}
+
+	strlcpy(hname, "blkmap-", namelen);
+	strlcat(hname, label, namelen);
+
+	err = device_bind_driver(dm_root(), "blkmap_dev", hname, &dev);
+	if (err)
+		goto err_free_hname;
+
+	device_set_name_alloced(dev);
+	bm = dev_get_plat(dev);
+	bm->label = hlabel;
+
+	if (devp)
+		*devp = dev;
+
+	return 0;
+
+err_free_hname:
+	free(hname);
+err_free_hlabel:
+	free(hlabel);
+err:
+	return err;
+}
+
+int blkmap_destroy(struct udevice *dev)
+{
+	int err;
+
+	err = device_remove(dev, DM_REMOVE_NORMAL);
+	if (err)
+		return err;
+
+	return device_unbind(dev);
+}
+
+UCLASS_DRIVER(blkmap) = {
+	.id		= UCLASS_BLKMAP,
+	.name		= "blkmap",
+};
diff --git a/include/blkmap.h b/include/blkmap.h
new file mode 100644
index 0000000000..3c7e36efab
--- /dev/null
+++ b/include/blkmap.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (c) 2023 Addiva Elektronik
+ * Author: Tobias Waldekranz <tobias@waldekranz.com>
+ */
+
+#ifndef _BLKMAP_H
+#define _BLKMAP_H
+
+/**
+ * blkmap_from_label() - Find blkmap from label
+ *
+ * @label: Label of the requested blkmap
+ * Returns: A pointer to the blkmap on success, NULL on failure
+ */
+struct udevice *blkmap_from_label(const char *label);
+
+/**
+ * blkmap_create() - Create new blkmap
+ *
+ * @label: Label of the new blkmap
+ * @devp: If not NULL, updated with the address of the resulting device
+ * Returns: 0 on success, negative error code on failure
+ */
+int blkmap_create(const char *label, struct udevice **devp);
+
+/**
+ * blkmap_destroy() - Destroy blkmap
+ *
+ * @dev: The blkmap to be destroyed
+ * Returns: 0 on success, negative error code on failure
+ */
+int blkmap_destroy(struct udevice *dev);
+
+#endif	/* _BLKMAP_H */
diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
index 33e43c20db..576237b954 100644
--- a/include/dm/uclass-id.h
+++ b/include/dm/uclass-id.h
@@ -37,6 +37,7 @@ enum uclass_id {
 	UCLASS_AUDIO_CODEC,	/* Audio codec with control and data path */
 	UCLASS_AXI,		/* AXI bus */
 	UCLASS_BLK,		/* Block device */
+	UCLASS_BLKMAP,		/* Composable virtual block device */
 	UCLASS_BOOTCOUNT,       /* Bootcount backing store */
 	UCLASS_BOOTDEV,		/* Boot device for locating an OS to boot */
 	UCLASS_BOOTMETH,	/* Bootmethod for booting an OS */
-- 
2.34.1

