Orange Pi5 kernel

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