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
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	((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
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(&regions_mtx);
62	rreg->res = resource;
63	rreg->ops = ops;
64	list_add_tail(&rreg->list, &regions_list);
65	mutex_unlock(&regions_mtx);
66
67	return 0;
68}
69EXPORT_SYMBOL(logic_iomem_add_region);
70
71#ifndef CONFIG_INDIRECT_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(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(&regions_mtx);
93	list_for_each_entry(rreg, &regions_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(&regions_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(&regions_mtx);
165	area->ops = NULL;
166	area->priv = NULL;
167	mutex_unlock(&regions_mtx);
168}
169EXPORT_SYMBOL(iounmap);
170
171#ifndef CONFIG_INDIRECT_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)(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_64BIT
192MAKE_FALLBACK(q, 64);
193#endif
194
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_64BIT
251MAKE_OP(q, 64);
252#endif
253
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