Commit | Line | Data |
---|---|---|
ca2e3342 JB |
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> | |
68fdb644 | 9 | #include <asm/io.h> |
ca2e3342 JB |
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 ((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 | ||
35 | static DEFINE_MUTEX(regions_mtx); | |
36 | static LIST_HEAD(regions_list); | |
37 | static struct logic_iomem_area mapped_areas[MAX_AREAS]; | |
38 | ||
39 | 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 | 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 | } | |
69 | EXPORT_SYMBOL(logic_iomem_add_region); | |
70 | ||
71 | #ifndef CONFIG_LOGIC_IOMEM_FALLBACK | |
72 | static 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 | ||
79 | static void real_iounmap(void __iomem *addr) | |
80 | { | |
81 | WARN(1, "invalid iounmap for addr 0x%llx\n", | |
68fdb644 | 82 | (unsigned long long __force)addr); |
ca2e3342 JB |
83 | } |
84 | #endif /* CONFIG_LOGIC_IOMEM_FALLBACK */ | |
85 | ||
86 | void __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 | } | |
127 | out: | |
128 | mutex_unlock(®ions_mtx); | |
129 | if (ret) | |
130 | return ret; | |
131 | return real_ioremap(offset, size); | |
132 | } | |
133 | EXPORT_SYMBOL(ioremap); | |
134 | ||
135 | static inline struct logic_iomem_area * | |
136 | get_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 | ||
152 | void 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 | } | |
169 | EXPORT_SYMBOL(iounmap); | |
170 | ||
171 | #ifndef CONFIG_LOGIC_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", \ | |
68fdb644 | 176 | (unsigned long long __force)addr); \ |
ca2e3342 JB |
177 | return (u ## sz)~0ULL; \ |
178 | } \ | |
179 | \ | |
68fdb644 JB |
180 | static void real_raw_write ## op(u ## sz val, \ |
181 | volatile void __iomem *addr) \ | |
ca2e3342 JB |
182 | { \ |
183 | WARN(1, "Invalid writeq" #op " of 0x%llx at address %llx\n", \ | |
68fdb644 | 184 | (unsigned long long)val, (unsigned long long __force)addr);\ |
ca2e3342 JB |
185 | } \ |
186 | ||
187 | MAKE_FALLBACK(b, 8); | |
188 | MAKE_FALLBACK(w, 16); | |
189 | MAKE_FALLBACK(l, 32); | |
190 | #ifdef CONFIG_64BIT | |
191 | MAKE_FALLBACK(q, 64); | |
192 | #endif | |
193 | ||
194 | static 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", | |
68fdb644 | 197 | (unsigned long long __force)addr); |
ca2e3342 JB |
198 | } |
199 | ||
200 | static 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", | |
68fdb644 | 204 | (unsigned long long __force)addr); |
ca2e3342 JB |
205 | |
206 | memset(buffer, 0xff, size); | |
207 | } | |
208 | ||
209 | static 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", | |
68fdb644 | 213 | (unsigned long long __force)addr); |
ca2e3342 JB |
214 | } |
215 | #endif /* CONFIG_LOGIC_IOMEM_FALLBACK */ | |
216 | ||
217 | #define MAKE_OP(op, sz) \ | |
218 | u##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 | } \ | |
229 | EXPORT_SYMBOL(__raw_read ## op); \ | |
230 | \ | |
231 | void __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 | } \ | |
244 | EXPORT_SYMBOL(__raw_write ## op) | |
245 | ||
246 | MAKE_OP(b, 8); | |
247 | MAKE_OP(w, 16); | |
248 | MAKE_OP(l, 32); | |
249 | #ifdef CONFIG_64BIT | |
250 | MAKE_OP(q, 64); | |
251 | #endif | |
252 | ||
253 | void 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 | } | |
273 | EXPORT_SYMBOL(memset_io); | |
274 | ||
275 | void 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 | } | |
297 | EXPORT_SYMBOL(memcpy_fromio); | |
298 | ||
299 | void 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 | } | |
320 | EXPORT_SYMBOL(memcpy_toio); |