lib: add iomem emulation (logic_iomem)

Add IO memory emulation that uses callbacks for read/write to
the allocated regions. The callbacks can be registered by the
users using logic_iomem_alloc().

To use, an architecture must 'select LOGIC_IOMEM' in Kconfig
and then include <asm-generic/logic_io.h> into asm/io.h to get
the __raw_read*/__raw_write* functions.

Optionally, an architecture may 'select LOGIC_IOMEM_FALLBACK'
in which case non-emulated regions will 'fall back' to the
various real_* functions that must then be provided.

Cc: Arnd Bergmann <arnd@kernel.org>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: Richard Weinberger <richard@nod.at>

1 files changed, 318 insertions, 0 deletions

diff --git a/lib/logic_iomem.c b/lib/logic_iomem.c
new file mode 100644
index 000000000000..b76b92dd0f1f
--- /dev/null
+++ b/lib/logic_iomem.c
@@ -0,0 +1,318 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 Intel Corporation
+ * Author: Johannes Berg <johannes@sipsolutions.net>
+ */
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/logic_iomem.h>
+
+struct logic_iomem_region {
+	const struct resource *res;
+	const struct logic_iomem_region_ops *ops;
+	struct list_head list;
+};
+
+struct logic_iomem_area {
+	const struct logic_iomem_ops *ops;
+	void *priv;
+};
+
+#define AREA_SHIFT	24
+#define MAX_AREA_SIZE	(1 << AREA_SHIFT)
+#define MAX_AREAS	((1ULL<<32) / MAX_AREA_SIZE)
+#define AREA_BITS	((MAX_AREAS - 1) << AREA_SHIFT)
+#define AREA_MASK	(MAX_AREA_SIZE - 1)
+#ifdef CONFIG_64BIT
+#define IOREMAP_BIAS	0xDEAD000000000000UL
+#define IOREMAP_MASK	0xFFFFFFFF00000000UL
+#else
+#define IOREMAP_BIAS	0
+#define IOREMAP_MASK	0
+#endif
+
+static DEFINE_MUTEX(regions_mtx);
+static LIST_HEAD(regions_list);
+static struct logic_iomem_area mapped_areas[MAX_AREAS];
+
+int logic_iomem_add_region(struct resource *resource,
+			   const struct logic_iomem_region_ops *ops)
+{
+	struct logic_iomem_region *rreg;
+	int err;
+
+	if (WARN_ON(!resource || !ops))
+		return -EINVAL;
+
+	if (WARN_ON((resource->flags & IORESOURCE_TYPE_BITS) != IORESOURCE_MEM))
+		return -EINVAL;
+
+	rreg = kzalloc(sizeof(*rreg), GFP_KERNEL);
+	if (!rreg)
+		return -ENOMEM;
+
+	err = request_resource(&iomem_resource, resource);
+	if (err) {
+		kfree(rreg);
+		return -ENOMEM;
+	}
+
+	mutex_lock(&regions_mtx);
+	rreg->res = resource;
+	rreg->ops = ops;
+	list_add_tail(&rreg->list, &regions_list);
+	mutex_unlock(&regions_mtx);
+
+	return 0;
+}
+EXPORT_SYMBOL(logic_iomem_add_region);
+
+#ifndef CONFIG_LOGIC_IOMEM_FALLBACK
+static void __iomem *real_ioremap(phys_addr_t offset, size_t size)
+{
+	WARN(1, "invalid ioremap(0x%llx, 0x%zx)\n",
+	     (unsigned long long)offset, size);
+	return NULL;
+}
+
+static void real_iounmap(void __iomem *addr)
+{
+	WARN(1, "invalid iounmap for addr 0x%llx\n",
+	     (unsigned long long)addr);
+}
+#endif /* CONFIG_LOGIC_IOMEM_FALLBACK */
+
+void __iomem *ioremap(phys_addr_t offset, size_t size)
+{
+	void __iomem *ret = NULL;
+	struct logic_iomem_region *rreg, *found = NULL;
+	int i;
+
+	mutex_lock(&regions_mtx);
+	list_for_each_entry(rreg, &regions_list, list) {
+		if (rreg->res->start > offset)
+			continue;
+		if (rreg->res->end < offset + size - 1)
+			continue;
+		found = rreg;
+		break;
+	}
+
+	if (!found)
+		goto out;
+
+	for (i = 0; i < MAX_AREAS; i++) {
+		long offs;
+
+		if (mapped_areas[i].ops)
+			continue;
+
+		offs = rreg->ops->map(offset - found->res->start,
+				      size, &mapped_areas[i].ops,
+				      &mapped_areas[i].priv);
+		if (offs < 0) {
+			mapped_areas[i].ops = NULL;
+			break;
+		}
+
+		if (WARN_ON(!mapped_areas[i].ops)) {
+			mapped_areas[i].ops = NULL;
+			break;
+		}
+
+		ret = (void __iomem *)(IOREMAP_BIAS + (i << AREA_SHIFT) + offs);
+		break;
+	}
+out:
+	mutex_unlock(&regions_mtx);
+	if (ret)
+		return ret;
+	return real_ioremap(offset, size);
+}
+EXPORT_SYMBOL(ioremap);
+
+static inline struct logic_iomem_area *
+get_area(const volatile void __iomem *addr)
+{
+	unsigned long a = (unsigned long)addr;
+	unsigned int idx;
+
+	if (WARN_ON((a & IOREMAP_MASK) != IOREMAP_BIAS))
+		return NULL;
+
+	idx = (a & AREA_BITS) >> AREA_SHIFT;
+
+	if (mapped_areas[idx].ops)
+		return &mapped_areas[idx];
+
+	return NULL;
+}
+
+void iounmap(void __iomem *addr)
+{
+	struct logic_iomem_area *area = get_area(addr);
+
+	if (!area) {
+		real_iounmap(addr);
+		return;
+	}
+
+	if (area->ops->unmap)
+		area->ops->unmap(area->priv);
+
+	mutex_lock(&regions_mtx);
+	area->ops = NULL;
+	area->priv = NULL;
+	mutex_unlock(&regions_mtx);
+}
+EXPORT_SYMBOL(iounmap);
+
+#ifndef CONFIG_LOGIC_IOMEM_FALLBACK
+#define MAKE_FALLBACK(op, sz) 						\
+static u##sz real_raw_read ## op(const volatile void __iomem *addr)	\
+{									\
+	WARN(1, "Invalid read" #op " at address %llx\n",		\
+	     (unsigned long long)addr);					\
+	return (u ## sz)~0ULL;						\
+}									\
+									\
+void real_raw_write ## op(u ## sz val, volatile void __iomem *addr)	\
+{									\
+	WARN(1, "Invalid writeq" #op " of 0x%llx at address %llx\n",	\
+	     (unsigned long long)val, (unsigned long long)addr);	\
+}									\
+
+MAKE_FALLBACK(b, 8);
+MAKE_FALLBACK(w, 16);
+MAKE_FALLBACK(l, 32);
+#ifdef CONFIG_64BIT
+MAKE_FALLBACK(q, 64);
+#endif
+
+static void real_memset_io(volatile void __iomem *addr, int value, size_t size)
+{
+	WARN(1, "Invalid memset_io at address 0x%llx\n",
+	     (unsigned long long)addr);
+}
+
+static void real_memcpy_fromio(void *buffer, const volatile void __iomem *addr,
+			       size_t size)
+{
+	WARN(1, "Invalid memcpy_fromio at address 0x%llx\n",
+	     (unsigned long long)addr);
+
+	memset(buffer, 0xff, size);
+}
+
+static void real_memcpy_toio(volatile void __iomem *addr, const void *buffer,
+			     size_t size)
+{
+	WARN(1, "Invalid memcpy_toio at address 0x%llx\n",
+	     (unsigned long long)addr);
+}
+#endif /* CONFIG_LOGIC_IOMEM_FALLBACK */
+
+#define MAKE_OP(op, sz) 						\
+u##sz __raw_read ## op(const volatile void __iomem *addr)		\
+{									\
+	struct logic_iomem_area *area = get_area(addr);			\
+									\
+	if (!area)							\
+		return real_raw_read ## op(addr);			\
+									\
+	return (u ## sz) area->ops->read(area->priv,			\
+					 (unsigned long)addr & AREA_MASK,\
+					 sz / 8);			\
+}									\
+EXPORT_SYMBOL(__raw_read ## op);					\
+									\
+void __raw_write ## op(u ## sz val, volatile void __iomem *addr)	\
+{									\
+	struct logic_iomem_area *area = get_area(addr);			\
+									\
+	if (!area) {							\
+		real_raw_write ## op(val, addr);			\
+		return;							\
+	}								\
+									\
+	area->ops->write(area->priv,					\
+			 (unsigned long)addr & AREA_MASK,		\
+			 sz / 8, val);					\
+}									\
+EXPORT_SYMBOL(__raw_write ## op)
+
+MAKE_OP(b, 8);
+MAKE_OP(w, 16);
+MAKE_OP(l, 32);
+#ifdef CONFIG_64BIT
+MAKE_OP(q, 64);
+#endif
+
+void memset_io(volatile void __iomem *addr, int value, size_t size)
+{
+	struct logic_iomem_area *area = get_area(addr);
+	unsigned long offs, start;
+
+	if (!area) {
+		real_memset_io(addr, value, size);
+		return;
+	}
+
+	start = (unsigned long)addr & AREA_MASK;
+
+	if (area->ops->set) {
+		area->ops->set(area->priv, start, value, size);
+		return;
+	}
+
+	for (offs = 0; offs < size; offs++)
+		area->ops->write(area->priv, start + offs, 1, value);
+}
+EXPORT_SYMBOL(memset_io);
+
+void memcpy_fromio(void *buffer, const volatile void __iomem *addr,
+                   size_t size)
+{
+	struct logic_iomem_area *area = get_area(addr);
+	u8 *buf = buffer;
+	unsigned long offs, start;
+
+	if (!area) {
+		real_memcpy_fromio(buffer, addr, size);
+		return;
+	}
+
+	start = (unsigned long)addr & AREA_MASK;
+
+	if (area->ops->copy_from) {
+		area->ops->copy_from(area->priv, buffer, start, size);
+		return;
+	}
+
+	for (offs = 0; offs < size; offs++)
+		buf[offs] = area->ops->read(area->priv, start + offs, 1);
+}
+EXPORT_SYMBOL(memcpy_fromio);
+
+void memcpy_toio(volatile void __iomem *addr, const void *buffer, size_t size)
+{
+	struct logic_iomem_area *area = get_area(addr);
+	const u8 *buf = buffer;
+	unsigned long offs, start;
+
+	if (!area) {
+		real_memcpy_toio(addr, buffer, size);
+		return;
+	}
+
+	start = (unsigned long)addr & AREA_MASK;
+
+	if (area->ops->copy_to) {
+		area->ops->copy_to(area->priv, start, buffer, size);
+		return;
+	}
+
+	for (offs = 0; offs < size; offs++)
+		area->ops->write(area->priv, start + offs, 1, buf[offs]);
+}
+EXPORT_SYMBOL(memcpy_toio);