]>
Commit | Line | Data |
---|---|---|
e7cc9a73 MD |
1 | /* |
2 | * Trapped io support | |
3 | * | |
4 | * Copyright (C) 2008 Magnus Damm | |
5 | * | |
6 | * Intercept io operations by trapping. | |
7 | * | |
8 | * This file is subject to the terms and conditions of the GNU General Public | |
9 | * License. See the file "COPYING" in the main directory of this archive | |
10 | * for more details. | |
11 | */ | |
12 | #include <linux/kernel.h> | |
13 | #include <linux/mm.h> | |
14 | #include <linux/bitops.h> | |
15 | #include <linux/vmalloc.h> | |
ecc14e8c | 16 | #include <linux/module.h> |
eeee7853 | 17 | #include <linux/init.h> |
e7cc9a73 MD |
18 | #include <asm/mmu_context.h> |
19 | #include <asm/uaccess.h> | |
20 | #include <asm/io.h> | |
21 | #include <asm/io_trapped.h> | |
22 | ||
23 | #define TRAPPED_PAGES_MAX 16 | |
e7cc9a73 | 24 | |
ce816fa8 | 25 | #ifdef CONFIG_HAS_IOPORT_MAP |
e7cc9a73 | 26 | LIST_HEAD(trapped_io); |
ecc14e8c | 27 | EXPORT_SYMBOL_GPL(trapped_io); |
e7cc9a73 MD |
28 | #endif |
29 | #ifdef CONFIG_HAS_IOMEM | |
30 | LIST_HEAD(trapped_mem); | |
ecc14e8c | 31 | EXPORT_SYMBOL_GPL(trapped_mem); |
e7cc9a73 MD |
32 | #endif |
33 | static DEFINE_SPINLOCK(trapped_lock); | |
34 | ||
eeee7853 PM |
35 | static int trapped_io_disable __read_mostly; |
36 | ||
37 | static int __init trapped_io_setup(char *__unused) | |
38 | { | |
39 | trapped_io_disable = 1; | |
40 | return 1; | |
41 | } | |
42 | __setup("noiotrap", trapped_io_setup); | |
43 | ||
b2839ed8 | 44 | int register_trapped_io(struct trapped_io *tiop) |
e7cc9a73 MD |
45 | { |
46 | struct resource *res; | |
47 | unsigned long len = 0, flags = 0; | |
48 | struct page *pages[TRAPPED_PAGES_MAX]; | |
49 | int k, n; | |
50 | ||
eeee7853 PM |
51 | if (unlikely(trapped_io_disable)) |
52 | return 0; | |
53 | ||
e7cc9a73 MD |
54 | /* structure must be page aligned */ |
55 | if ((unsigned long)tiop & (PAGE_SIZE - 1)) | |
56 | goto bad; | |
57 | ||
58 | for (k = 0; k < tiop->num_resources; k++) { | |
59 | res = tiop->resource + k; | |
28f65c11 | 60 | len += roundup(resource_size(res), PAGE_SIZE); |
e7cc9a73 MD |
61 | flags |= res->flags; |
62 | } | |
63 | ||
64 | /* support IORESOURCE_IO _or_ MEM, not both */ | |
65 | if (hweight_long(flags) != 1) | |
66 | goto bad; | |
67 | ||
68 | n = len >> PAGE_SHIFT; | |
69 | ||
70 | if (n >= TRAPPED_PAGES_MAX) | |
71 | goto bad; | |
72 | ||
73 | for (k = 0; k < n; k++) | |
74 | pages[k] = virt_to_page(tiop); | |
75 | ||
76 | tiop->virt_base = vmap(pages, n, VM_MAP, PAGE_NONE); | |
77 | if (!tiop->virt_base) | |
78 | goto bad; | |
79 | ||
80 | len = 0; | |
81 | for (k = 0; k < tiop->num_resources; k++) { | |
82 | res = tiop->resource + k; | |
83 | pr_info("trapped io 0x%08lx overrides %s 0x%08lx\n", | |
84 | (unsigned long)(tiop->virt_base + len), | |
85 | res->flags & IORESOURCE_IO ? "io" : "mmio", | |
86 | (unsigned long)res->start); | |
28f65c11 | 87 | len += roundup(resource_size(res), PAGE_SIZE); |
e7cc9a73 MD |
88 | } |
89 | ||
90 | tiop->magic = IO_TRAPPED_MAGIC; | |
91 | INIT_LIST_HEAD(&tiop->list); | |
92 | spin_lock_irq(&trapped_lock); | |
ce816fa8 | 93 | #ifdef CONFIG_HAS_IOPORT_MAP |
e7cc9a73 MD |
94 | if (flags & IORESOURCE_IO) |
95 | list_add(&tiop->list, &trapped_io); | |
86e4dd5a PM |
96 | #endif |
97 | #ifdef CONFIG_HAS_IOMEM | |
e7cc9a73 MD |
98 | if (flags & IORESOURCE_MEM) |
99 | list_add(&tiop->list, &trapped_mem); | |
86e4dd5a | 100 | #endif |
e7cc9a73 MD |
101 | spin_unlock_irq(&trapped_lock); |
102 | ||
103 | return 0; | |
104 | bad: | |
105 | pr_warning("unable to install trapped io filter\n"); | |
106 | return -1; | |
107 | } | |
ecc14e8c | 108 | EXPORT_SYMBOL_GPL(register_trapped_io); |
e7cc9a73 MD |
109 | |
110 | void __iomem *match_trapped_io_handler(struct list_head *list, | |
111 | unsigned long offset, | |
112 | unsigned long size) | |
113 | { | |
114 | unsigned long voffs; | |
115 | struct trapped_io *tiop; | |
116 | struct resource *res; | |
117 | int k, len; | |
fd78a76a | 118 | unsigned long flags; |
e7cc9a73 | 119 | |
fd78a76a | 120 | spin_lock_irqsave(&trapped_lock, flags); |
e7cc9a73 MD |
121 | list_for_each_entry(tiop, list, list) { |
122 | voffs = 0; | |
123 | for (k = 0; k < tiop->num_resources; k++) { | |
124 | res = tiop->resource + k; | |
125 | if (res->start == offset) { | |
fd78a76a | 126 | spin_unlock_irqrestore(&trapped_lock, flags); |
e7cc9a73 MD |
127 | return tiop->virt_base + voffs; |
128 | } | |
129 | ||
28f65c11 | 130 | len = resource_size(res); |
e7cc9a73 MD |
131 | voffs += roundup(len, PAGE_SIZE); |
132 | } | |
133 | } | |
fd78a76a | 134 | spin_unlock_irqrestore(&trapped_lock, flags); |
e7cc9a73 MD |
135 | return NULL; |
136 | } | |
ecc14e8c | 137 | EXPORT_SYMBOL_GPL(match_trapped_io_handler); |
e7cc9a73 MD |
138 | |
139 | static struct trapped_io *lookup_tiop(unsigned long address) | |
140 | { | |
141 | pgd_t *pgd_k; | |
142 | pud_t *pud_k; | |
143 | pmd_t *pmd_k; | |
144 | pte_t *pte_k; | |
145 | pte_t entry; | |
146 | ||
147 | pgd_k = swapper_pg_dir + pgd_index(address); | |
148 | if (!pgd_present(*pgd_k)) | |
149 | return NULL; | |
150 | ||
151 | pud_k = pud_offset(pgd_k, address); | |
152 | if (!pud_present(*pud_k)) | |
153 | return NULL; | |
154 | ||
155 | pmd_k = pmd_offset(pud_k, address); | |
156 | if (!pmd_present(*pmd_k)) | |
157 | return NULL; | |
158 | ||
159 | pte_k = pte_offset_kernel(pmd_k, address); | |
160 | entry = *pte_k; | |
161 | ||
162 | return pfn_to_kaddr(pte_pfn(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; | |
28f65c11 | 175 | len = roundup(resource_size(res), PAGE_SIZE); |
e7cc9a73 MD |
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: | |
9d56dd3b | 190 | tmp = __raw_readb(src_addr); |
e7cc9a73 MD |
191 | break; |
192 | case 2: | |
9d56dd3b | 193 | tmp = __raw_readw(src_addr); |
e7cc9a73 MD |
194 | break; |
195 | case 4: | |
9d56dd3b | 196 | tmp = __raw_readl(src_addr); |
e7cc9a73 MD |
197 | break; |
198 | case 8: | |
9d56dd3b | 199 | tmp = __raw_readq(src_addr); |
e7cc9a73 MD |
200 | break; |
201 | } | |
202 | ||
203 | switch (dst_len) { | |
204 | case 1: | |
9d56dd3b | 205 | __raw_writeb(tmp, dst_addr); |
e7cc9a73 MD |
206 | break; |
207 | case 2: | |
9d56dd3b | 208 | __raw_writew(tmp, dst_addr); |
e7cc9a73 MD |
209 | break; |
210 | case 4: | |
9d56dd3b | 211 | __raw_writel(tmp, dst_addr); |
e7cc9a73 MD |
212 | break; |
213 | case 8: | |
9d56dd3b | 214 | __raw_writeq(tmp, dst_addr); |
e7cc9a73 MD |
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(src_addr); | |
229 | WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC)); | |
230 | ||
231 | src_addr = lookup_address(tiop, src_addr); | |
232 | if (!src_addr) | |
233 | return cnt; | |
234 | ||
f1cdd63f PM |
235 | tmp = copy_word(src_addr, |
236 | max_t(unsigned long, cnt, | |
237 | (tiop->minimum_bus_width / 8)), | |
e7cc9a73 MD |
238 | (unsigned long)dst, 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(dst_addr); | |
252 | WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC)); | |
253 | ||
254 | dst_addr = lookup_address(tiop, dst_addr); | |
255 | if (!dst_addr) | |
256 | return cnt; | |
257 | ||
258 | tmp = copy_word((unsigned long)src, cnt, | |
f1cdd63f PM |
259 | dst_addr, max_t(unsigned long, cnt, |
260 | (tiop->minimum_bus_width / 8))); | |
e7cc9a73 MD |
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 | mm_segment_t oldfs; | |
2bcfffa4 | 274 | insn_size_t instruction; |
e7cc9a73 MD |
275 | int tmp; |
276 | ||
08b36c4a PM |
277 | if (trapped_io_disable) |
278 | return 0; | |
e7cc9a73 MD |
279 | if (!lookup_tiop(address)) |
280 | return 0; | |
281 | ||
282 | WARN_ON(user_mode(regs)); | |
283 | ||
284 | oldfs = get_fs(); | |
285 | set_fs(KERNEL_DS); | |
286 | if (copy_from_user(&instruction, (void *)(regs->pc), | |
287 | sizeof(instruction))) { | |
288 | set_fs(oldfs); | |
289 | return 0; | |
290 | } | |
291 | ||
4aa5ac4e | 292 | tmp = handle_unaligned_access(instruction, regs, |
ace2dc7d | 293 | &trapped_io_access, 1, address); |
e7cc9a73 MD |
294 | set_fs(oldfs); |
295 | return tmp == 0; | |
296 | } |