linux/arch/sh/kernel/io_trapped.c
<<
>>
Prefs
   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
  23LIST_HEAD(trapped_io);
  24EXPORT_SYMBOL_GPL(trapped_io);
  25#endif
  26#ifdef CONFIG_HAS_IOMEM
  27LIST_HEAD(trapped_mem);
  28EXPORT_SYMBOL_GPL(trapped_mem);
  29#endif
  30static DEFINE_SPINLOCK(trapped_lock);
  31
  32static int trapped_io_disable __read_mostly;
  33
  34static int __init trapped_io_setup(char *__unused)
  35{
  36        trapped_io_disable = 1;
  37        return 1;
  38}
  39__setup("noiotrap", trapped_io_setup);
  40
  41int 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_warn("unable to install trapped io filter\n");
 103        return -1;
 104}
 105
 106void __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(&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(&trapped_lock, flags);
 131        return NULL;
 132}
 133
 134static 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_k))
 145                return NULL;
 146
 147        p4d_k = p4d_offset(pgd_k, address);
 148        if (!p4d_present(*p4d_k))
 149                return NULL;
 150
 151        pud_k = pud_offset(p4d_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
 165static 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
 183static 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(src_addr);
 191                break;
 192        case 2:
 193                tmp = __raw_readw(src_addr);
 194                break;
 195        case 4:
 196                tmp = __raw_readl(src_addr);
 197                break;
 198        case 8:
 199                tmp = __raw_readq(src_addr);
 200                break;
 201        }
 202
 203        switch (dst_len) {
 204        case 1:
 205                __raw_writeb(tmp, dst_addr);
 206                break;
 207        case 2:
 208                __raw_writew(tmp, dst_addr);
 209                break;
 210        case 4:
 211                __raw_writel(tmp, dst_addr);
 212                break;
 213        case 8:
 214                __raw_writeq(tmp, dst_addr);
 215                break;
 216        }
 217
 218        return tmp;
 219}
 220
 221static 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
 235        tmp = copy_word(src_addr,
 236                        max_t(unsigned long, cnt,
 237                              (tiop->minimum_bus_width / 8)),
 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
 244static 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,
 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
 266static struct mem_access trapped_io_access = {
 267        from_device,
 268        to_device,
 269};
 270
 271int handle_trapped_io(struct pt_regs *regs, unsigned long address)
 272{
 273        mm_segment_t oldfs;
 274        insn_size_t instruction;
 275        int tmp;
 276
 277        if (trapped_io_disable)
 278                return 0;
 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
 292        tmp = handle_unaligned_access(instruction, regs,
 293                                      &trapped_io_access, 1, address);
 294        set_fs(oldfs);
 295        return tmp == 0;
 296}
 297