diff options
author | Johannes Berg <johannes.berg@intel.com> | 2021-03-05 13:19:52 +0100 |
---|---|---|
committer | Richard Weinberger <richard@nod.at> | 2021-06-17 21:44:51 +0200 |
commit | ca2e334232b6cd4ae5af9da2df83c009d042aefb (patch) | |
tree | 1827fb3faf57865ec599ee74e574a2bfd6a45edd /lib/logic_iomem.c | |
parent | 0bbadafdc49d11a1836e5946f517d18cceaea6c8 (diff) | |
download | linux-stable-ca2e334232b6cd4ae5af9da2df83c009d042aefb.tar.gz linux-stable-ca2e334232b6cd4ae5af9da2df83c009d042aefb.tar.bz2 linux-stable-ca2e334232b6cd4ae5af9da2df83c009d042aefb.zip |
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>
Diffstat (limited to 'lib/logic_iomem.c')
-rw-r--r-- | lib/logic_iomem.c | 318 |
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(®ions_mtx); + rreg->res = resource; + rreg->ops = ops; + list_add_tail(&rreg->list, ®ions_list); + mutex_unlock(®ions_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(®ions_mtx); + list_for_each_entry(rreg, ®ions_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(®ions_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(®ions_mtx); + area->ops = NULL; + area->priv = NULL; + mutex_unlock(®ions_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); |