the-tree/lib/logic_iomem.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2021 Intel Corporation
0004  * Author: Johannes Berg <johannes@sipsolutions.net>
0005  */
0006 #include <linux/types.h>
0007 #include <linux/slab.h>
0008 #include <linux/logic_iomem.h>
0009 #include <asm/io.h>
0010
0011 struct logic_iomem_region {
0012     const struct resource *res;
0013     const struct logic_iomem_region_ops *ops;
0014     struct list_head list;
0015 };
0016
0017 struct logic_iomem_area {
0018     const struct logic_iomem_ops *ops;
0019     void *priv;
0020 };
0021
0022 #define AREA_SHIFT  24
0023 #define MAX_AREA_SIZE   (1 << AREA_SHIFT)
0024 #define MAX_AREAS   ((1U << 31) / MAX_AREA_SIZE)
0025 #define AREA_BITS   ((MAX_AREAS - 1) << AREA_SHIFT)
0026 #define AREA_MASK   (MAX_AREA_SIZE - 1)
0027 #ifdef CONFIG_64BIT
0028 #define IOREMAP_BIAS    0xDEAD000000000000UL
0029 #define IOREMAP_MASK    0xFFFFFFFF00000000UL
0030 #else
0031 #define IOREMAP_BIAS    0x80000000UL
0032 #define IOREMAP_MASK    0x80000000UL
0033 #endif
0034
0035 static DEFINE_MUTEX(regions_mtx);
0036 static LIST_HEAD(regions_list);
0037 static struct logic_iomem_area mapped_areas[MAX_AREAS];
0038
0039 int logic_iomem_add_region(struct resource *resource,
0040                const struct logic_iomem_region_ops *ops)
0041 {
0042     struct logic_iomem_region *rreg;
0043     int err;
0044
0045     if (WARN_ON(!resource || !ops))
0046         return -EINVAL;
0047
0048     if (WARN_ON((resource->flags & IORESOURCE_TYPE_BITS) != IORESOURCE_MEM))
0049         return -EINVAL;
0050
0051     rreg = kzalloc(sizeof(*rreg), GFP_KERNEL);
0052     if (!rreg)
0053         return -ENOMEM;
0054
0055     err = request_resource(&iomem_resource, resource);
0056     if (err) {
0057         kfree(rreg);
0058         return -ENOMEM;
0059     }
0060
0061     mutex_lock(&regions_mtx);
0062     rreg->res = resource;
0063     rreg->ops = ops;
0064     list_add_tail(&rreg->list, &regions_list);
0065     mutex_unlock(&regions_mtx);
0066
0067     return 0;
0068 }
0069 EXPORT_SYMBOL(logic_iomem_add_region);
0070
0071 #ifndef CONFIG_INDIRECT_IOMEM_FALLBACK
0072 static void __iomem *real_ioremap(phys_addr_t offset, size_t size)
0073 {
0074     WARN(1, "invalid ioremap(0x%llx, 0x%zx)\n",
0075          (unsigned long long)offset, size);
0076     return NULL;
0077 }
0078
0079 static void real_iounmap(volatile void __iomem *addr)
0080 {
0081     WARN(1, "invalid iounmap for addr 0x%llx\n",
0082          (unsigned long long)(uintptr_t __force)addr);
0083 }
0084 #endif /* CONFIG_INDIRECT_IOMEM_FALLBACK */
0085
0086 void __iomem *ioremap(phys_addr_t offset, size_t size)
0087 {
0088     void __iomem *ret = NULL;
0089     struct logic_iomem_region *rreg, *found = NULL;
0090     int i;
0091
0092     mutex_lock(&regions_mtx);
0093     list_for_each_entry(rreg, &regions_list, list) {
0094         if (rreg->res->start > offset)
0095             continue;
0096         if (rreg->res->end < offset + size - 1)
0097             continue;
0098         found = rreg;
0099         break;
0100     }
0101
0102     if (!found)
0103         goto out;
0104
0105     for (i = 0; i < MAX_AREAS; i++) {
0106         long offs;
0107
0108         if (mapped_areas[i].ops)
0109             continue;
0110
0111         offs = rreg->ops->map(offset - found->res->start,
0112                       size, &mapped_areas[i].ops,
0113                       &mapped_areas[i].priv);
0114         if (offs < 0) {
0115             mapped_areas[i].ops = NULL;
0116             break;
0117         }
0118
0119         if (WARN_ON(!mapped_areas[i].ops)) {
0120             mapped_areas[i].ops = NULL;
0121             break;
0122         }
0123
0124         ret = (void __iomem *)(IOREMAP_BIAS + (i << AREA_SHIFT) + offs);
0125         break;
0126     }
0127 out:
0128     mutex_unlock(&regions_mtx);
0129     if (ret)
0130         return ret;
0131     return real_ioremap(offset, size);
0132 }
0133 EXPORT_SYMBOL(ioremap);
0134
0135 static inline struct logic_iomem_area *
0136 get_area(const volatile void __iomem *addr)
0137 {
0138     unsigned long a = (unsigned long)addr;
0139     unsigned int idx;
0140
0141     if (WARN_ON((a & IOREMAP_MASK) != IOREMAP_BIAS))
0142         return NULL;
0143
0144     idx = (a & AREA_BITS) >> AREA_SHIFT;
0145
0146     if (mapped_areas[idx].ops)
0147         return &mapped_areas[idx];
0148
0149     return NULL;
0150 }
0151
0152 void iounmap(volatile void __iomem *addr)
0153 {
0154     struct logic_iomem_area *area = get_area(addr);
0155
0156     if (!area) {
0157         real_iounmap(addr);
0158         return;
0159     }
0160
0161     if (area->ops->unmap)
0162         area->ops->unmap(area->priv);
0163
0164     mutex_lock(&regions_mtx);
0165     area->ops = NULL;
0166     area->priv = NULL;
0167     mutex_unlock(&regions_mtx);
0168 }
0169 EXPORT_SYMBOL(iounmap);
0170
0171 #ifndef CONFIG_INDIRECT_IOMEM_FALLBACK
0172 #define MAKE_FALLBACK(op, sz)                       \
0173 static u##sz real_raw_read ## op(const volatile void __iomem *addr) \
0174 {                                   \
0175     WARN(1, "Invalid read" #op " at address %llx\n",        \
0176          (unsigned long long)(uintptr_t __force)addr);      \
0177     return (u ## sz)~0ULL;                      \
0178 }                                   \
0179                                     \
0180 static void real_raw_write ## op(u ## sz val,               \
0181                  volatile void __iomem *addr)       \
0182 {                                   \
0183     WARN(1, "Invalid writeq" #op " of 0x%llx at address %llx\n",    \
0184          (unsigned long long)val,                   \
0185          (unsigned long long)(uintptr_t __force)addr);\
0186 }                                   \
0187
0188 MAKE_FALLBACK(b, 8);
0189 MAKE_FALLBACK(w, 16);
0190 MAKE_FALLBACK(l, 32);
0191 #ifdef CONFIG_64BIT
0192 MAKE_FALLBACK(q, 64);
0193 #endif
0194
0195 static void real_memset_io(volatile void __iomem *addr, int value, size_t size)
0196 {
0197     WARN(1, "Invalid memset_io at address 0x%llx\n",
0198          (unsigned long long)(uintptr_t __force)addr);
0199 }
0200
0201 static void real_memcpy_fromio(void *buffer, const volatile void __iomem *addr,
0202                    size_t size)
0203 {
0204     WARN(1, "Invalid memcpy_fromio at address 0x%llx\n",
0205          (unsigned long long)(uintptr_t __force)addr);
0206
0207     memset(buffer, 0xff, size);
0208 }
0209
0210 static void real_memcpy_toio(volatile void __iomem *addr, const void *buffer,
0211                  size_t size)
0212 {
0213     WARN(1, "Invalid memcpy_toio at address 0x%llx\n",
0214          (unsigned long long)(uintptr_t __force)addr);
0215 }
0216 #endif /* CONFIG_INDIRECT_IOMEM_FALLBACK */
0217
0218 #define MAKE_OP(op, sz)                         \
0219 u##sz __raw_read ## op(const volatile void __iomem *addr)       \
0220 {                                   \
0221     struct logic_iomem_area *area = get_area(addr);         \
0222                                     \
0223     if (!area)                          \
0224         return real_raw_read ## op(addr);           \
0225                                     \
0226     return (u ## sz) area->ops->read(area->priv,            \
0227                      (unsigned long)addr & AREA_MASK,\
0228                      sz / 8);           \
0229 }                                   \
0230 EXPORT_SYMBOL(__raw_read ## op);                    \
0231                                     \
0232 void __raw_write ## op(u ## sz val, volatile void __iomem *addr)    \
0233 {                                   \
0234     struct logic_iomem_area *area = get_area(addr);         \
0235                                     \
0236     if (!area) {                            \
0237         real_raw_write ## op(val, addr);            \
0238         return;                         \
0239     }                               \
0240                                     \
0241     area->ops->write(area->priv,                    \
0242              (unsigned long)addr & AREA_MASK,       \
0243              sz / 8, val);                  \
0244 }                                   \
0245 EXPORT_SYMBOL(__raw_write ## op)
0246
0247 MAKE_OP(b, 8);
0248 MAKE_OP(w, 16);
0249 MAKE_OP(l, 32);
0250 #ifdef CONFIG_64BIT
0251 MAKE_OP(q, 64);
0252 #endif
0253
0254 void memset_io(volatile void __iomem *addr, int value, size_t size)
0255 {
0256     struct logic_iomem_area *area = get_area(addr);
0257     unsigned long offs, start;
0258
0259     if (!area) {
0260         real_memset_io(addr, value, size);
0261         return;
0262     }
0263
0264     start = (unsigned long)addr & AREA_MASK;
0265
0266     if (area->ops->set) {
0267         area->ops->set(area->priv, start, value, size);
0268         return;
0269     }
0270
0271     for (offs = 0; offs < size; offs++)
0272         area->ops->write(area->priv, start + offs, 1, value);
0273 }
0274 EXPORT_SYMBOL(memset_io);
0275
0276 void memcpy_fromio(void *buffer, const volatile void __iomem *addr,
0277                    size_t size)
0278 {
0279     struct logic_iomem_area *area = get_area(addr);
0280     u8 *buf = buffer;
0281     unsigned long offs, start;
0282
0283     if (!area) {
0284         real_memcpy_fromio(buffer, addr, size);
0285         return;
0286     }
0287
0288     start = (unsigned long)addr & AREA_MASK;
0289
0290     if (area->ops->copy_from) {
0291         area->ops->copy_from(area->priv, buffer, start, size);
0292         return;
0293     }
0294
0295     for (offs = 0; offs < size; offs++)
0296         buf[offs] = area->ops->read(area->priv, start + offs, 1);
0297 }
0298 EXPORT_SYMBOL(memcpy_fromio);
0299
0300 void memcpy_toio(volatile void __iomem *addr, const void *buffer, size_t size)
0301 {
0302     struct logic_iomem_area *area = get_area(addr);
0303     const u8 *buf = buffer;
0304     unsigned long offs, start;
0305
0306     if (!area) {
0307         real_memcpy_toio(addr, buffer, size);
0308         return;
0309     }
0310
0311     start = (unsigned long)addr & AREA_MASK;
0312
0313     if (area->ops->copy_to) {
0314         area->ops->copy_to(area->priv, start, buffer, size);
0315         return;
0316     }
0317
0318     for (offs = 0; offs < size; offs++)
0319         area->ops->write(area->priv, start + offs, 1, buf[offs]);
0320 }
0321 EXPORT_SYMBOL(memcpy_toio);