arch/sh/kernel/io_trapped.c

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