arch/sh/kernel/io_trapped.c

   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(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, 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(&tiop->list);
  89         spin_lock_irq(&trapped_lock);
  90 #ifdef CONFIG_HAS_IOPORT_MAP
  91         if (flags & IORESOURCE_IO)
  92                 list_add(&tiop->list, &trapped_io);
  93 #endif
  94 #ifdef CONFIG_HAS_IOMEM
  95         if (flags & IORESOURCE_MEM)
  96                 list_add(&tiop->list, &trapped_mem);
  97 #endif
  98         spin_unlock_irq(&trapped_lock);
  99
 100         return 0;
 101  bad:
 102         pr_warning("unable to install trapped io filter\n");
 103         return -1;
 104 }
 105 EXPORT_SYMBOL_GPL(register_trapped_io);
 106
 107 void __iomem *match_trapped_io_handler(struct list_head *list,
 108                                        unsigned long offset,
 109                                        unsigned long size)
 110 {
 111         unsigned long voffs;
 112         struct trapped_io *tiop;
 113         struct resource *res;
 114         int k, len;
 115         unsigned long flags;
 116
 117         spin_lock_irqsave(&trapped_lock, flags);
 118         list_for_each_entry(tiop, list, list) {
 119                 voffs = 0;
 120                 for (k = 0; k < tiop->num_resources; k++) {
 121                         res = tiop->resource + k;
 122                         if (res->start == offset) {
 123                                 spin_unlock_irqrestore(&trapped_lock, flags);
 124                                 return tiop->virt_base + voffs;
 125                         }
 126
 127                         len = resource_size(res);
 128                         voffs += roundup(len, PAGE_SIZE);
 129                 }
 130         }
 131         spin_unlock_irqrestore(&trapped_lock, flags);
 132         return NULL;
 133 }
 134 EXPORT_SYMBOL_GPL(match_trapped_io_handler);
 135
 136 static struct trapped_io *lookup_tiop(unsigned long address)
 137 {
 138         pgd_t *pgd_k;
 139         pud_t *pud_k;
 140         pmd_t *pmd_k;
 141         pte_t *pte_k;
 142         pte_t entry;
 143
 144         pgd_k = swapper_pg_dir + pgd_index(address);
 145         if (!pgd_present(*pgd_k))
 146                 return NULL;
 147
 148         pud_k = pud_offset(pgd_k, address);
 149         if (!pud_present(*pud_k))
 150                 return NULL;
 151
 152         pmd_k = pmd_offset(pud_k, address);
 153         if (!pmd_present(*pmd_k))
 154                 return NULL;
 155
 156         pte_k = pte_offset_kernel(pmd_k, address);
 157         entry = *pte_k;
 158
 159         return pfn_to_kaddr(pte_pfn(entry));
 160 }
 161
 162 static unsigned long lookup_address(struct trapped_io *tiop,
 163                                     unsigned long address)
 164 {
 165         struct resource *res;
 166         unsigned long vaddr = (unsigned long)tiop->virt_base;
 167         unsigned long len;
 168         int k;
 169
 170         for (k = 0; k < tiop->num_resources; k++) {
 171                 res = tiop->resource + k;
 172                 len = roundup(resource_size(res), PAGE_SIZE);
 173                 if (address < (vaddr + len))
 174                         return res->start + (address - vaddr);
 175                 vaddr += len;
 176         }
 177         return 0;
 178 }
 179
 180 static unsigned long long copy_word(unsigned long src_addr, int src_len,
 181                                     unsigned long dst_addr, int dst_len)
 182 {
 183         unsigned long long tmp = 0;
 184
 185         switch (src_len) {
 186         case 1:
 187                 tmp = __raw_readb(src_addr);
 188                 break;
 189         case 2:
 190                 tmp = __raw_readw(src_addr);
 191                 break;
 192         case 4:
 193                 tmp = __raw_readl(src_addr);
 194                 break;
 195         case 8:
 196                 tmp = __raw_readq(src_addr);
 197                 break;
 198         }
 199
 200         switch (dst_len) {
 201         case 1:
 202                 __raw_writeb(tmp, dst_addr);
 203                 break;
 204         case 2:
 205                 __raw_writew(tmp, dst_addr);
 206                 break;
 207         case 4:
 208                 __raw_writel(tmp, dst_addr);
 209                 break;
 210         case 8:
 211                 __raw_writeq(tmp, dst_addr);
 212                 break;
 213         }
 214
 215         return tmp;
 216 }
 217
 218 static unsigned long from_device(void *dst, const void *src, unsigned long cnt)
 219 {
 220         struct trapped_io *tiop;
 221         unsigned long src_addr = (unsigned long)src;
 222         unsigned long long tmp;
 223
 224         pr_debug("trapped io read 0x%08lx (%ld)\n", src_addr, cnt);
 225         tiop = lookup_tiop(src_addr);
 226         WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC));
 227
 228         src_addr = lookup_address(tiop, src_addr);
 229         if (!src_addr)
 230                 return cnt;
 231
 232         tmp = copy_word(src_addr,
 233                         max_t(unsigned long, cnt,
 234                               (tiop->minimum_bus_width / 8)),
 235                         (unsigned long)dst, cnt);
 236
 237         pr_debug("trapped io read 0x%08lx -> 0x%08llx\n", src_addr, tmp);
 238         return 0;
 239 }
 240
 241 static unsigned long to_device(void *dst, const void *src, unsigned long cnt)
 242 {
 243         struct trapped_io *tiop;
 244         unsigned long dst_addr = (unsigned long)dst;
 245         unsigned long long tmp;
 246
 247         pr_debug("trapped io write 0x%08lx (%ld)\n", dst_addr, cnt);
 248         tiop = lookup_tiop(dst_addr);
 249         WARN_ON(!tiop || (tiop->magic != IO_TRAPPED_MAGIC));
 250
 251         dst_addr = lookup_address(tiop, dst_addr);
 252         if (!dst_addr)
 253                 return cnt;
 254
 255         tmp = copy_word((unsigned long)src, cnt,
 256                         dst_addr, max_t(unsigned long, cnt,
 257                                         (tiop->minimum_bus_width / 8)));
 258
 259         pr_debug("trapped io write 0x%08lx -> 0x%08llx\n", dst_addr, tmp);
 260         return 0;
 261 }
 262
 263 static struct mem_access trapped_io_access = {
 264         from_device,
 265         to_device,
 266 };
 267
 268 int handle_trapped_io(struct pt_regs *regs, unsigned long address)
 269 {
 270         mm_segment_t oldfs;
 271         insn_size_t instruction;
 272         int tmp;
 273
 274         if (trapped_io_disable)
 275                 return 0;
 276         if (!lookup_tiop(address))
 277                 return 0;
 278
 279         WARN_ON(user_mode(regs));
 280
 281         oldfs = get_fs();
 282         set_fs(KERNEL_DS);
 283         if (copy_from_user(&instruction, (void *)(regs->pc),
 284                            sizeof(instruction))) {
 285                 set_fs(oldfs);
 286                 return 0;
 287         }
 288
 289         tmp = handle_unaligned_access(instruction, regs,
 290                                       &trapped_io_access, 1, address);
 291         set_fs(oldfs);
 292         return tmp == 0;
 293 }