Line data Source code
1 : // SPDX-License-Identifier: GPL-2.0
2 : /*
3 : * Copyright (C) 2021 Intel Corporation
4 : * 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 :
11 : struct logic_iomem_region {
12 : const struct resource *res;
13 : const struct logic_iomem_region_ops *ops;
14 : struct list_head list;
15 : };
16 :
17 : struct 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 ((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_64BIT
28 : #define IOREMAP_BIAS 0xDEAD000000000000UL
29 : #define IOREMAP_MASK 0xFFFFFFFF00000000UL
30 : #else
31 : #define IOREMAP_BIAS 0x80000000UL
32 : #define IOREMAP_MASK 0x80000000UL
33 : #endif
34 :
35 : static DEFINE_MUTEX(regions_mtx);
36 : static LIST_HEAD(regions_list);
37 : static struct logic_iomem_area mapped_areas[MAX_AREAS];
38 :
39 3 : int 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 3 : if (WARN_ON(!resource || !ops))
46 : return -EINVAL;
47 :
48 3 : if (WARN_ON((resource->flags & IORESOURCE_TYPE_BITS) != IORESOURCE_MEM))
49 : return -EINVAL;
50 :
51 3 : rreg = kzalloc(sizeof(*rreg), GFP_KERNEL);
52 3 : if (!rreg)
53 : return -ENOMEM;
54 :
55 3 : err = request_resource(&iomem_resource, resource);
56 3 : if (err) {
57 0 : kfree(rreg);
58 0 : return -ENOMEM;
59 : }
60 :
61 3 : mutex_lock(®ions_mtx);
62 3 : rreg->res = resource;
63 3 : rreg->ops = ops;
64 6 : list_add_tail(&rreg->list, ®ions_list);
65 3 : mutex_unlock(®ions_mtx);
66 :
67 3 : return 0;
68 : }
69 : EXPORT_SYMBOL(logic_iomem_add_region);
70 :
71 : #ifndef CONFIG_INDIRECT_IOMEM_FALLBACK
72 : static void __iomem *real_ioremap(phys_addr_t offset, size_t size)
73 : {
74 0 : WARN(1, "invalid ioremap(0x%llx, 0x%zx)\n",
75 : (unsigned long long)offset, size);
76 : return NULL;
77 : }
78 :
79 : static void real_iounmap(volatile void __iomem *addr)
80 : {
81 0 : 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 :
86 0 : void __iomem *ioremap(phys_addr_t offset, size_t size)
87 : {
88 0 : void __iomem *ret = NULL;
89 0 : struct logic_iomem_region *rreg, *found = NULL;
90 : int i;
91 :
92 0 : mutex_lock(®ions_mtx);
93 0 : list_for_each_entry(rreg, ®ions_list, list) {
94 0 : if (rreg->res->start > offset)
95 0 : continue;
96 0 : if (rreg->res->end < offset + size - 1)
97 0 : continue;
98 : found = rreg;
99 : break;
100 : }
101 :
102 0 : if (!found)
103 : goto out;
104 :
105 0 : for (i = 0; i < MAX_AREAS; i++) {
106 : long offs;
107 :
108 0 : if (mapped_areas[i].ops)
109 0 : continue;
110 :
111 0 : offs = rreg->ops->map(offset - found->res->start,
112 : size, &mapped_areas[i].ops,
113 : &mapped_areas[i].priv);
114 0 : if (offs < 0) {
115 0 : mapped_areas[i].ops = NULL;
116 0 : break;
117 : }
118 :
119 0 : if (WARN_ON(!mapped_areas[i].ops)) {
120 0 : mapped_areas[i].ops = NULL;
121 0 : break;
122 : }
123 :
124 0 : ret = (void __iomem *)(IOREMAP_BIAS + (i << AREA_SHIFT) + offs);
125 0 : break;
126 : }
127 : out:
128 0 : mutex_unlock(®ions_mtx);
129 0 : if (ret)
130 : return ret;
131 0 : return real_ioremap(offset, size);
132 : }
133 : EXPORT_SYMBOL(ioremap);
134 :
135 : static inline struct logic_iomem_area *
136 0 : get_area(const volatile void __iomem *addr)
137 : {
138 0 : unsigned long a = (unsigned long)addr;
139 : unsigned int idx;
140 :
141 0 : if (WARN_ON((a & IOREMAP_MASK) != IOREMAP_BIAS))
142 : return NULL;
143 :
144 0 : idx = (a & AREA_BITS) >> AREA_SHIFT;
145 :
146 0 : if (mapped_areas[idx].ops)
147 0 : return &mapped_areas[idx];
148 :
149 : return NULL;
150 : }
151 :
152 0 : void iounmap(volatile void __iomem *addr)
153 : {
154 0 : struct logic_iomem_area *area = get_area(addr);
155 :
156 0 : if (!area) {
157 : real_iounmap(addr);
158 : return;
159 : }
160 :
161 0 : if (area->ops->unmap)
162 0 : area->ops->unmap(area->priv);
163 :
164 0 : mutex_lock(®ions_mtx);
165 0 : area->ops = NULL;
166 0 : area->priv = NULL;
167 0 : mutex_unlock(®ions_mtx);
168 : }
169 : EXPORT_SYMBOL(iounmap);
170 :
171 : #ifndef CONFIG_INDIRECT_IOMEM_FALLBACK
172 : #define MAKE_FALLBACK(op, sz) \
173 : static 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 : \
180 : static 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 :
188 0 : MAKE_FALLBACK(b, 8);
189 0 : MAKE_FALLBACK(w, 16);
190 0 : MAKE_FALLBACK(l, 32);
191 : #ifdef CONFIG_64BIT
192 0 : MAKE_FALLBACK(q, 64);
193 : #endif
194 :
195 : static void real_memset_io(volatile void __iomem *addr, int value, size_t size)
196 : {
197 0 : WARN(1, "Invalid memset_io at address 0x%llx\n",
198 : (unsigned long long)(uintptr_t __force)addr);
199 : }
200 :
201 0 : static void real_memcpy_fromio(void *buffer, const volatile void __iomem *addr,
202 : size_t size)
203 : {
204 0 : WARN(1, "Invalid memcpy_fromio at address 0x%llx\n",
205 : (unsigned long long)(uintptr_t __force)addr);
206 :
207 0 : memset(buffer, 0xff, size);
208 0 : }
209 :
210 : static void real_memcpy_toio(volatile void __iomem *addr, const void *buffer,
211 : size_t size)
212 : {
213 0 : 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) \
219 : u##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 : } \
230 : EXPORT_SYMBOL(__raw_read ## op); \
231 : \
232 : void __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 : } \
245 : EXPORT_SYMBOL(__raw_write ## op)
246 :
247 0 : MAKE_OP(b, 8);
248 0 : MAKE_OP(w, 16);
249 0 : MAKE_OP(l, 32);
250 : #ifdef CONFIG_64BIT
251 0 : MAKE_OP(q, 64);
252 : #endif
253 :
254 0 : void memset_io(volatile void __iomem *addr, int value, size_t size)
255 : {
256 0 : struct logic_iomem_area *area = get_area(addr);
257 : unsigned long offs, start;
258 :
259 0 : if (!area) {
260 0 : real_memset_io(addr, value, size);
261 : return;
262 : }
263 :
264 0 : start = (unsigned long)addr & AREA_MASK;
265 :
266 0 : if (area->ops->set) {
267 0 : area->ops->set(area->priv, start, value, size);
268 0 : return;
269 : }
270 :
271 0 : for (offs = 0; offs < size; offs++)
272 0 : area->ops->write(area->priv, start + offs, 1, value);
273 : }
274 : EXPORT_SYMBOL(memset_io);
275 :
276 0 : void memcpy_fromio(void *buffer, const volatile void __iomem *addr,
277 : size_t size)
278 : {
279 0 : struct logic_iomem_area *area = get_area(addr);
280 0 : u8 *buf = buffer;
281 : unsigned long offs, start;
282 :
283 0 : if (!area) {
284 0 : real_memcpy_fromio(buffer, addr, size);
285 0 : return;
286 : }
287 :
288 0 : start = (unsigned long)addr & AREA_MASK;
289 :
290 0 : if (area->ops->copy_from) {
291 0 : area->ops->copy_from(area->priv, buffer, start, size);
292 0 : return;
293 : }
294 :
295 0 : for (offs = 0; offs < size; offs++)
296 0 : buf[offs] = area->ops->read(area->priv, start + offs, 1);
297 : }
298 : EXPORT_SYMBOL(memcpy_fromio);
299 :
300 0 : void memcpy_toio(volatile void __iomem *addr, const void *buffer, size_t size)
301 : {
302 0 : struct logic_iomem_area *area = get_area(addr);
303 0 : const u8 *buf = buffer;
304 : unsigned long offs, start;
305 :
306 0 : if (!area) {
307 0 : real_memcpy_toio(addr, buffer, size);
308 : return;
309 : }
310 :
311 0 : start = (unsigned long)addr & AREA_MASK;
312 :
313 0 : if (area->ops->copy_to) {
314 0 : area->ops->copy_to(area->priv, start, buffer, size);
315 0 : return;
316 : }
317 :
318 0 : for (offs = 0; offs < size; offs++)
319 0 : area->ops->write(area->priv, start + offs, 1, buf[offs]);
320 : }
321 : EXPORT_SYMBOL(memcpy_toio);
|
