1
2
3
4
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 24
23#define MAX_AREA_SIZE (1 << AREA_SHIFT)
24#define MAX_AREAS ((1ULL<<32) / MAX_AREA_SIZE)
25#define AREA_BITS ((MAX_AREAS - 1) << AREA_SHIFT)
26#define AREA_MASK (MAX_AREA_SIZE - 1)
27#ifdef CONFIG_64BIT
28#define IOREMAP_BIAS 0xDEAD000000000000UL
29#define IOREMAP_MASK 0xFFFFFFFF00000000UL
30#else
31#define IOREMAP_BIAS 0
32#define IOREMAP_MASK 0
33#endif
34
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_LOGIC_IOMEM_FALLBACK
72static 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(void __iomem *addr)
80{
81 WARN(1, "invalid iounmap for addr 0x%llx\n",
82 (unsigned long long __force)addr);
83}
84#endif
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(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_LOGIC_IOMEM_FALLBACK
172#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 __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, (unsigned long long __force)addr);\
185} \
186
187MAKE_FALLBACK(b, 8);
188MAKE_FALLBACK(w, 16);
189MAKE_FALLBACK(l, 32);
190#ifdef CONFIG_64BIT
191MAKE_FALLBACK(q, 64);
192#endif
193
194static void real_memset_io(volatile void __iomem *addr, int value, size_t size)
195{
196 WARN(1, "Invalid memset_io at address 0x%llx\n",
197 (unsigned long long __force)addr);
198}
199
200static void real_memcpy_fromio(void *buffer, const volatile void __iomem *addr,
201 size_t size)
202{
203 WARN(1, "Invalid memcpy_fromio at address 0x%llx\n",
204 (unsigned long long __force)addr);
205
206 memset(buffer, 0xff, size);
207}
208
209static void real_memcpy_toio(volatile void __iomem *addr, const void *buffer,
210 size_t size)
211{
212 WARN(1, "Invalid memcpy_toio at address 0x%llx\n",
213 (unsigned long long __force)addr);
214}
215#endif
216
217#define MAKE_OP(op, sz) \
218u##sz __raw_read ## op(const volatile void __iomem *addr) \
219{ \
220 struct logic_iomem_area *area = get_area(addr); \
221 \
222 if (!area) \
223 return real_raw_read ## op(addr); \
224 \
225 return (u ## sz) area->ops->read(area->priv, \
226 (unsigned long)addr & AREA_MASK,\
227 sz / 8); \
228} \
229EXPORT_SYMBOL(__raw_read ## op); \
230 \
231void __raw_write ## op(u ## sz val, volatile void __iomem *addr) \
232{ \
233 struct logic_iomem_area *area = get_area(addr); \
234 \
235 if (!area) { \
236 real_raw_write ## op(val, addr); \
237 return; \
238 } \
239 \
240 area->ops->write(area->priv, \
241 (unsigned long)addr & AREA_MASK, \
242 sz / 8, val); \
243} \
244EXPORT_SYMBOL(__raw_write ## op)
245
246MAKE_OP(b, 8);
247MAKE_OP(w, 16);
248MAKE_OP(l, 32);
249#ifdef CONFIG_64BIT
250MAKE_OP(q, 64);
251#endif
252
253void memset_io(volatile void __iomem *addr, int value, size_t size)
254{
255 struct logic_iomem_area *area = get_area(addr);
256 unsigned long offs, start;
257
258 if (!area) {
259 real_memset_io(addr, value, size);
260 return;
261 }
262
263 start = (unsigned long)addr & AREA_MASK;
264
265 if (area->ops->set) {
266 area->ops->set(area->priv, start, value, size);
267 return;
268 }
269
270 for (offs = 0; offs < size; offs++)
271 area->ops->write(area->priv, start + offs, 1, value);
272}
273EXPORT_SYMBOL(memset_io);
274
275void memcpy_fromio(void *buffer, const volatile void __iomem *addr,
276 size_t size)
277{
278 struct logic_iomem_area *area = get_area(addr);
279 u8 *buf = buffer;
280 unsigned long offs, start;
281
282 if (!area) {
283 real_memcpy_fromio(buffer, addr, size);
284 return;
285 }
286
287 start = (unsigned long)addr & AREA_MASK;
288
289 if (area->ops->copy_from) {
290 area->ops->copy_from(area->priv, buffer, start, size);
291 return;
292 }
293
294 for (offs = 0; offs < size; offs++)
295 buf[offs] = area->ops->read(area->priv, start + offs, 1);
296}
297EXPORT_SYMBOL(memcpy_fromio);
298
299void memcpy_toio(volatile void __iomem *addr, const void *buffer, size_t size)
300{
301 struct logic_iomem_area *area = get_area(addr);
302 const u8 *buf = buffer;
303 unsigned long offs, start;
304
305 if (!area) {
306 real_memcpy_toio(addr, buffer, size);
307 return;
308 }
309
310 start = (unsigned long)addr & AREA_MASK;
311
312 if (area->ops->copy_to) {
313 area->ops->copy_to(area->priv, start, buffer, size);
314 return;
315 }
316
317 for (offs = 0; offs < size; offs++)
318 area->ops->write(area->priv, start + offs, 1, buf[offs]);
319}
320EXPORT_SYMBOL(memcpy_toio);
321
