1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Trapped io support |
4 | * |
5 | * Copyright (C) 2008 Magnus Damm |
6 | * |
7 | * Intercept io operations by trapping. |
8 | */ |
9 | #include <linux/kernel.h> |
10 | #include <linux/mm.h> |
11 | #include <linux/bitops.h> |
12 | #include <linux/vmalloc.h> |
13 | #include <linux/module.h> |
14 | #include <linux/init.h> |
15 | #include <asm/mmu_context.h> |
16 | #include <linux/uaccess.h> |
17 | #include <asm/io.h> |
18 | #include <asm/io_trapped.h> |
19 | |
20 | #define TRAPPED_PAGES_MAX 16 |
21 | |
22 | #ifdef CONFIG_HAS_IOPORT_MAP |
23 | LIST_HEAD(trapped_io); |
24 | EXPORT_SYMBOL_GPL(trapped_io); |
25 | #endif |
26 | #ifdef CONFIG_HAS_IOMEM |
27 | LIST_HEAD(trapped_mem); |
28 | EXPORT_SYMBOL_GPL(trapped_mem); |
29 | #endif |
30 | static DEFINE_SPINLOCK(trapped_lock); |
31 | |
32 | static int trapped_io_disable __read_mostly; |
33 | |
34 | static int __init trapped_io_setup(char *__unused) |
35 | { |
36 | trapped_io_disable = 1; |
37 | return 1; |
38 | } |
39 | __setup("noiotrap" , trapped_io_setup); |
40 | |
41 | int register_trapped_io(struct trapped_io *tiop) |
42 | { |
43 | struct resource *res; |
44 | unsigned long len = 0, flags = 0; |
45 | struct page *pages[TRAPPED_PAGES_MAX]; |
46 | int k, n; |
47 | |
48 | if (unlikely(trapped_io_disable)) |
49 | return 0; |
50 | |
51 | /* structure must be page aligned */ |
52 | if ((unsigned long)tiop & (PAGE_SIZE - 1)) |
53 | goto bad; |
54 | |
55 | for (k = 0; k < tiop->num_resources; k++) { |
56 | res = tiop->resource + k; |
57 | len += roundup(resource_size(res), PAGE_SIZE); |
58 | flags |= res->flags; |
59 | } |
60 | |
61 | /* support IORESOURCE_IO _or_ MEM, not both */ |
62 | if (hweight_long(w: flags) != 1) |
63 | goto bad; |
64 | |
65 | n = len >> PAGE_SHIFT; |
66 | |
67 | if (n >= TRAPPED_PAGES_MAX) |
68 | goto bad; |
69 | |
70 | for (k = 0; k < n; k++) |
71 | pages[k] = virt_to_page(tiop); |
72 | |
73 | tiop->virt_base = vmap(pages, count: n, VM_MAP, PAGE_NONE); |
74 | if (!tiop->virt_base) |
75 | goto bad; |
76 | |
77 | len = 0; |
78 | for (k = 0; k < tiop->num_resources; k++) { |
79 | res = tiop->resource + k; |
80 | pr_info("trapped io 0x%08lx overrides %s 0x%08lx\n" , |
81 | (unsigned long)(tiop->virt_base + len), |
82 | res->flags & IORESOURCE_IO ? "io" : "mmio" , |
83 | (unsigned long)res->start); |
84 | len += roundup(resource_size(res), PAGE_SIZE); |
85 | } |
86 | |
87 | tiop->magic = IO_TRAPPED_MAGIC; |
88 | INIT_LIST_HEAD(list: &tiop->list); |
89 | spin_lock_irq(lock: &trapped_lock); |
90 | #ifdef CONFIG_HAS_IOPORT_MAP |
91 | if (flags & IORESOURCE_IO) |
92 | list_add(new: &tiop->list, head: &trapped_io); |
93 | #endif |
94 | #ifdef CONFIG_HAS_IOMEM |
95 | if (flags & IORESOURCE_MEM) |
96 | list_add(new: &tiop->list, head: &trapped_mem); |
97 | #endif |
98 | spin_unlock_irq(lock: &trapped_lock); |
99 | |
100 | return 0; |
101 | bad: |
102 | pr_warn("unable to install trapped io filter\n" ); |
103 | return -1; |
104 | } |
105 | |
106 | void __iomem *match_trapped_io_handler(struct list_head *list, |
107 | unsigned long offset, |
108 | unsigned long size) |
109 | { |
110 | unsigned long voffs; |
111 | struct trapped_io *tiop; |
112 | struct resource *res; |
113 | int k, len; |
114 | unsigned long flags; |
115 | |
116 | spin_lock_irqsave(&trapped_lock, flags); |
117 | list_for_each_entry(tiop, list, list) { |
118 | voffs = 0; |
119 | for (k = 0; k < tiop->num_resources; k++) { |
120 | res = tiop->resource + k; |
121 | if (res->start == offset) { |
122 | spin_unlock_irqrestore(lock: &trapped_lock, flags); |
123 | return tiop->virt_base + voffs; |
124 | } |
125 | |
126 | len = resource_size(res); |
127 | voffs += roundup(len, PAGE_SIZE); |
128 | } |
129 | } |
130 | spin_unlock_irqrestore(lock: &trapped_lock, flags); |
131 | return NULL; |
132 | } |
133 | |
134 | static struct trapped_io *lookup_tiop(unsigned long address) |
135 | { |
136 | pgd_t *pgd_k; |
137 | p4d_t *p4d_k; |
138 | pud_t *pud_k; |
139 | pmd_t *pmd_k; |
140 | pte_t *pte_k; |
141 | pte_t entry; |
142 | |
143 | pgd_k = swapper_pg_dir + pgd_index(address); |
144 | if (!pgd_present(pgd: *pgd_k)) |
145 | return NULL; |
146 | |
147 | p4d_k = p4d_offset(pgd: pgd_k, address); |
148 | if (!p4d_present(p4d: *p4d_k)) |
149 | return NULL; |
150 | |
151 | pud_k = pud_offset(p4d: p4d_k, address); |
152 | if (!pud_present(pud: *pud_k)) |
153 | return NULL; |
154 | |
155 | pmd_k = pmd_offset(pud: pud_k, address); |
156 | if (!pmd_present(pmd: *pmd_k)) |
157 | return NULL; |
158 | |
159 | pte_k = pte_offset_kernel(pmd: pmd_k, address); |
160 | entry = *pte_k; |
161 | |
162 | return pfn_to_kaddr(pfn: pte_pfn(pte: entry)); |
163 | } |
164 | |
165 | static unsigned long lookup_address(struct trapped_io *tiop, |
166 | unsigned long address) |
167 | { |
168 | struct resource *res; |
169 | unsigned long vaddr = (unsigned long)tiop->virt_base; |
170 | unsigned long len; |
171 | int k; |
172 | |
173 | for (k = 0; k < tiop->num_resources; k++) { |
174 | res = tiop->resource + k; |
175 | len = roundup(resource_size(res), PAGE_SIZE); |
176 | if (address < (vaddr + len)) |
177 | return res->start + (address - vaddr); |
178 | vaddr += len; |
179 | } |
180 | return 0; |
181 | } |
182 | |
183 | static unsigned long long copy_word(unsigned long src_addr, int src_len, |
184 | unsigned long dst_addr, int dst_len) |
185 | { |
186 | unsigned long long tmp = 0; |
187 | |
188 | switch (src_len) { |
189 | case 1: |
190 | tmp = __raw_readb(addr: src_addr); |
191 | break; |
192 | case 2: |
193 | tmp = __raw_readw(addr: src_addr); |
194 | break; |
195 | case 4: |
196 | tmp = __raw_readl(addr: src_addr); |
197 | break; |
198 | case 8: |
199 | tmp = __raw_readq(addr: src_addr); |
200 | break; |
201 | } |
202 | |
203 | switch (dst_len) { |
204 | case 1: |
205 | __raw_writeb(val: tmp, addr: dst_addr); |
206 | break; |
207 | case 2: |
208 | __raw_writew(val: tmp, addr: dst_addr); |
209 | break; |
210 | case 4: |
211 | __raw_writel(val: tmp, addr: dst_addr); |
212 | break; |
213 | case 8: |
214 | __raw_writeq(val: tmp, addr: dst_addr); |
215 | break; |
216 | } |
217 | |
218 | return tmp; |
219 | } |
220 | |
221 | static unsigned long from_device(void *dst, const void *src, unsigned long cnt) |
222 | { |
223 | struct trapped_io *tiop; |
224 | unsigned long src_addr = (unsigned long)src; |
225 | unsigned long long tmp; |
226 | |
227 | pr_debug("trapped io read 0x%08lx (%ld)\n" , src_addr, cnt); |
228 | tiop = lookup_tiop(address: src_addr); |
229 | WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC)); |
230 | |
231 | src_addr = lookup_address(address: tiop, level: src_addr); |
232 | if (!src_addr) |
233 | return cnt; |
234 | |
235 | tmp = copy_word(src_addr, |
236 | max_t(unsigned long, cnt, |
237 | (tiop->minimum_bus_width / 8)), |
238 | dst_addr: (unsigned long)dst, dst_len: cnt); |
239 | |
240 | pr_debug("trapped io read 0x%08lx -> 0x%08llx\n" , src_addr, tmp); |
241 | return 0; |
242 | } |
243 | |
244 | static unsigned long to_device(void *dst, const void *src, unsigned long cnt) |
245 | { |
246 | struct trapped_io *tiop; |
247 | unsigned long dst_addr = (unsigned long)dst; |
248 | unsigned long long tmp; |
249 | |
250 | pr_debug("trapped io write 0x%08lx (%ld)\n" , dst_addr, cnt); |
251 | tiop = lookup_tiop(address: dst_addr); |
252 | WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC)); |
253 | |
254 | dst_addr = lookup_address(address: tiop, level: dst_addr); |
255 | if (!dst_addr) |
256 | return cnt; |
257 | |
258 | tmp = copy_word(src_addr: (unsigned long)src, src_len: cnt, |
259 | dst_addr, max_t(unsigned long, cnt, |
260 | (tiop->minimum_bus_width / 8))); |
261 | |
262 | pr_debug("trapped io write 0x%08lx -> 0x%08llx\n" , dst_addr, tmp); |
263 | return 0; |
264 | } |
265 | |
266 | static struct mem_access trapped_io_access = { |
267 | from_device, |
268 | to_device, |
269 | }; |
270 | |
271 | int handle_trapped_io(struct pt_regs *regs, unsigned long address) |
272 | { |
273 | insn_size_t instruction; |
274 | int tmp; |
275 | |
276 | if (trapped_io_disable) |
277 | return 0; |
278 | if (!lookup_tiop(address)) |
279 | return 0; |
280 | |
281 | WARN_ON(user_mode(regs)); |
282 | |
283 | if (copy_from_kernel_nofault(&instruction, (void *)(regs->pc), |
284 | sizeof(instruction))) { |
285 | return 0; |
286 | } |
287 | |
288 | tmp = handle_unaligned_access(instruction, regs, |
289 | &trapped_io_access, 1, address); |
290 | return tmp == 0; |
291 | } |
292 | |