322 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) 2021 Intel Corporation4 * Author: Johannes Berg <johannes@sipsolutions.net>5 */6#include <linux/types.h>7#include <linux/slab.h>8#include <linux/logic_iomem.h>9#include <asm/io.h>10 11struct logic_iomem_region {12 const struct resource *res;13 const struct logic_iomem_region_ops *ops;14 struct list_head list;15};16 17struct logic_iomem_area {18 const struct logic_iomem_ops *ops;19 void *priv;20};21 22#define AREA_SHIFT 2423#define MAX_AREA_SIZE (1 << AREA_SHIFT)24#define MAX_AREAS ((1U << 31) / MAX_AREA_SIZE)25#define AREA_BITS ((MAX_AREAS - 1) << AREA_SHIFT)26#define AREA_MASK (MAX_AREA_SIZE - 1)27#ifdef CONFIG_64BIT28#define IOREMAP_BIAS 0xDEAD000000000000UL29#define IOREMAP_MASK 0xFFFFFFFF00000000UL30#else31#define IOREMAP_BIAS 0x80000000UL32#define IOREMAP_MASK 0x80000000UL33#endif34 35static DEFINE_MUTEX(regions_mtx);36static LIST_HEAD(regions_list);37static struct logic_iomem_area mapped_areas[MAX_AREAS];38 39int logic_iomem_add_region(struct resource *resource,40 const struct logic_iomem_region_ops *ops)41{42 struct logic_iomem_region *rreg;43 int err;44 45 if (WARN_ON(!resource || !ops))46 return -EINVAL;47 48 if (WARN_ON((resource->flags & IORESOURCE_TYPE_BITS) != IORESOURCE_MEM))49 return -EINVAL;50 51 rreg = kzalloc(sizeof(*rreg), GFP_KERNEL);52 if (!rreg)53 return -ENOMEM;54 55 err = request_resource(&iomem_resource, resource);56 if (err) {57 kfree(rreg);58 return -ENOMEM;59 }60 61 mutex_lock(®ions_mtx);62 rreg->res = resource;63 rreg->ops = ops;64 list_add_tail(&rreg->list, ®ions_list);65 mutex_unlock(®ions_mtx);66 67 return 0;68}69EXPORT_SYMBOL(logic_iomem_add_region);70 71#ifndef CONFIG_INDIRECT_IOMEM_FALLBACK72static void __iomem *real_ioremap(phys_addr_t offset, size_t size)73{74 WARN(1, "invalid ioremap(0x%llx, 0x%zx)\n",75 (unsigned long long)offset, size);76 return NULL;77}78 79static void real_iounmap(volatile void __iomem *addr)80{81 WARN(1, "invalid iounmap for addr 0x%llx\n",82 (unsigned long long)(uintptr_t __force)addr);83}84#endif /* CONFIG_INDIRECT_IOMEM_FALLBACK */85 86void __iomem *ioremap(phys_addr_t offset, size_t size)87{88 void __iomem *ret = NULL;89 struct logic_iomem_region *rreg, *found = NULL;90 int i;91 92 mutex_lock(®ions_mtx);93 list_for_each_entry(rreg, ®ions_list, list) {94 if (rreg->res->start > offset)95 continue;96 if (rreg->res->end < offset + size - 1)97 continue;98 found = rreg;99 break;100 }101 102 if (!found)103 goto out;104 105 for (i = 0; i < MAX_AREAS; i++) {106 long offs;107 108 if (mapped_areas[i].ops)109 continue;110 111 offs = rreg->ops->map(offset - found->res->start,112 size, &mapped_areas[i].ops,113 &mapped_areas[i].priv);114 if (offs < 0) {115 mapped_areas[i].ops = NULL;116 break;117 }118 119 if (WARN_ON(!mapped_areas[i].ops)) {120 mapped_areas[i].ops = NULL;121 break;122 }123 124 ret = (void __iomem *)(IOREMAP_BIAS + (i << AREA_SHIFT) + offs);125 break;126 }127out:128 mutex_unlock(®ions_mtx);129 if (ret)130 return ret;131 return real_ioremap(offset, size);132}133EXPORT_SYMBOL(ioremap);134 135static inline struct logic_iomem_area *136get_area(const volatile void __iomem *addr)137{138 unsigned long a = (unsigned long)addr;139 unsigned int idx;140 141 if (WARN_ON((a & IOREMAP_MASK) != IOREMAP_BIAS))142 return NULL;143 144 idx = (a & AREA_BITS) >> AREA_SHIFT;145 146 if (mapped_areas[idx].ops)147 return &mapped_areas[idx];148 149 return NULL;150}151 152void iounmap(volatile void __iomem *addr)153{154 struct logic_iomem_area *area = get_area(addr);155 156 if (!area) {157 real_iounmap(addr);158 return;159 }160 161 if (area->ops->unmap)162 area->ops->unmap(area->priv);163 164 mutex_lock(®ions_mtx);165 area->ops = NULL;166 area->priv = NULL;167 mutex_unlock(®ions_mtx);168}169EXPORT_SYMBOL(iounmap);170 171#ifndef CONFIG_INDIRECT_IOMEM_FALLBACK172#define MAKE_FALLBACK(op, sz) \173static u##sz real_raw_read ## op(const volatile void __iomem *addr) \174{ \175 WARN(1, "Invalid read" #op " at address %llx\n", \176 (unsigned long long)(uintptr_t __force)addr); \177 return (u ## sz)~0ULL; \178} \179 \180static void real_raw_write ## op(u ## sz val, \181 volatile void __iomem *addr) \182{ \183 WARN(1, "Invalid writeq" #op " of 0x%llx at address %llx\n", \184 (unsigned long long)val, \185 (unsigned long long)(uintptr_t __force)addr);\186} \187 188MAKE_FALLBACK(b, 8);189MAKE_FALLBACK(w, 16);190MAKE_FALLBACK(l, 32);191#ifdef CONFIG_64BIT192MAKE_FALLBACK(q, 64);193#endif194 195static void real_memset_io(volatile void __iomem *addr, int value, size_t size)196{197 WARN(1, "Invalid memset_io at address 0x%llx\n",198 (unsigned long long)(uintptr_t __force)addr);199}200 201static void real_memcpy_fromio(void *buffer, const volatile void __iomem *addr,202 size_t size)203{204 WARN(1, "Invalid memcpy_fromio at address 0x%llx\n",205 (unsigned long long)(uintptr_t __force)addr);206 207 memset(buffer, 0xff, size);208}209 210static void real_memcpy_toio(volatile void __iomem *addr, const void *buffer,211 size_t size)212{213 WARN(1, "Invalid memcpy_toio at address 0x%llx\n",214 (unsigned long long)(uintptr_t __force)addr);215}216#endif /* CONFIG_INDIRECT_IOMEM_FALLBACK */217 218#define MAKE_OP(op, sz) \219u##sz __raw_read ## op(const volatile void __iomem *addr) \220{ \221 struct logic_iomem_area *area = get_area(addr); \222 \223 if (!area) \224 return real_raw_read ## op(addr); \225 \226 return (u ## sz) area->ops->read(area->priv, \227 (unsigned long)addr & AREA_MASK,\228 sz / 8); \229} \230EXPORT_SYMBOL(__raw_read ## op); \231 \232void __raw_write ## op(u ## sz val, volatile void __iomem *addr) \233{ \234 struct logic_iomem_area *area = get_area(addr); \235 \236 if (!area) { \237 real_raw_write ## op(val, addr); \238 return; \239 } \240 \241 area->ops->write(area->priv, \242 (unsigned long)addr & AREA_MASK, \243 sz / 8, val); \244} \245EXPORT_SYMBOL(__raw_write ## op)246 247MAKE_OP(b, 8);248MAKE_OP(w, 16);249MAKE_OP(l, 32);250#ifdef CONFIG_64BIT251MAKE_OP(q, 64);252#endif253 254void memset_io(volatile void __iomem *addr, int value, size_t size)255{256 struct logic_iomem_area *area = get_area(addr);257 unsigned long offs, start;258 259 if (!area) {260 real_memset_io(addr, value, size);261 return;262 }263 264 start = (unsigned long)addr & AREA_MASK;265 266 if (area->ops->set) {267 area->ops->set(area->priv, start, value, size);268 return;269 }270 271 for (offs = 0; offs < size; offs++)272 area->ops->write(area->priv, start + offs, 1, value);273}274EXPORT_SYMBOL(memset_io);275 276void memcpy_fromio(void *buffer, const volatile void __iomem *addr,277 size_t size)278{279 struct logic_iomem_area *area = get_area(addr);280 u8 *buf = buffer;281 unsigned long offs, start;282 283 if (!area) {284 real_memcpy_fromio(buffer, addr, size);285 return;286 }287 288 start = (unsigned long)addr & AREA_MASK;289 290 if (area->ops->copy_from) {291 area->ops->copy_from(area->priv, buffer, start, size);292 return;293 }294 295 for (offs = 0; offs < size; offs++)296 buf[offs] = area->ops->read(area->priv, start + offs, 1);297}298EXPORT_SYMBOL(memcpy_fromio);299 300void memcpy_toio(volatile void __iomem *addr, const void *buffer, size_t size)301{302 struct logic_iomem_area *area = get_area(addr);303 const u8 *buf = buffer;304 unsigned long offs, start;305 306 if (!area) {307 real_memcpy_toio(addr, buffer, size);308 return;309 }310 311 start = (unsigned long)addr & AREA_MASK;312 313 if (area->ops->copy_to) {314 area->ops->copy_to(area->priv, start, buffer, size);315 return;316 }317 318 for (offs = 0; offs < size; offs++)319 area->ops->write(area->priv, start + offs, 1, buf[offs]);320}321EXPORT_SYMBOL(memcpy_toio);322