1 /* 2 * Copyright (c) 2015, Linaro Limited, Shannon Zhao 3 * 4 * This program is free software; you can redistribute it and/or modify it 5 * under the terms and conditions of the GNU General Public License, 6 * version 2, as published by the Free Software Foundation. 7 * 8 * This program is distributed in the hope it will be useful, but WITHOUT 9 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 10 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 11 * more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program. If not, see <http://www.gnu.org/licenses/>. 15 */ 16 17 #include <linux/platform_device.h> 18 #include <linux/acpi.h> 19 #include <xen/xen.h> 20 #include <xen/page.h> 21 #include <xen/interface/memory.h> 22 #include <asm/xen/hypervisor.h> 23 #include <asm/xen/hypercall.h> 24 25 static int xen_unmap_device_mmio(const struct resource *resources, 26 unsigned int count) 27 { 28 unsigned int i, j, nr; 29 int rc = 0; 30 const struct resource *r; 31 struct xen_remove_from_physmap xrp; 32 33 for (i = 0; i < count; i++) { 34 r = &resources[i]; 35 nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE); 36 if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0)) 37 continue; 38 39 for (j = 0; j < nr; j++) { 40 xrp.domid = DOMID_SELF; 41 xrp.gpfn = XEN_PFN_DOWN(r->start) + j; 42 rc = HYPERVISOR_memory_op(XENMEM_remove_from_physmap, 43 &xrp); 44 if (rc) 45 return rc; 46 } 47 } 48 49 return rc; 50 } 51 52 static int xen_map_device_mmio(const struct resource *resources, 53 unsigned int count) 54 { 55 unsigned int i, j, nr; 56 int rc = 0; 57 const struct resource *r; 58 xen_pfn_t *gpfns; 59 xen_ulong_t *idxs; 60 int *errs; 61 62 for (i = 0; i < count; i++) { 63 struct xen_add_to_physmap_range xatp = { 64 .domid = DOMID_SELF, 65 .space = XENMAPSPACE_dev_mmio 66 }; 67 68 r = &resources[i]; 69 nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE); 70 if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0)) 71 continue; 72 73 gpfns = kcalloc(nr, sizeof(xen_pfn_t), GFP_KERNEL); 74 idxs = kcalloc(nr, sizeof(xen_ulong_t), GFP_KERNEL); 75 errs = kcalloc(nr, sizeof(int), GFP_KERNEL); 76 if (!gpfns || !idxs || !errs) { 77 kfree(gpfns); 78 kfree(idxs); 79 kfree(errs); 80 rc = -ENOMEM; 81 goto unmap; 82 } 83 84 for (j = 0; j < nr; j++) { 85 /* 86 * The regions are always mapped 1:1 to DOM0 and this is 87 * fine because the memory map for DOM0 is the same as 88 * the host (except for the RAM). 89 */ 90 gpfns[j] = XEN_PFN_DOWN(r->start) + j; 91 idxs[j] = XEN_PFN_DOWN(r->start) + j; 92 } 93 94 xatp.size = nr; 95 96 set_xen_guest_handle(xatp.gpfns, gpfns); 97 set_xen_guest_handle(xatp.idxs, idxs); 98 set_xen_guest_handle(xatp.errs, errs); 99 100 rc = HYPERVISOR_memory_op(XENMEM_add_to_physmap_range, &xatp); 101 kfree(gpfns); 102 kfree(idxs); 103 kfree(errs); 104 if (rc) 105 goto unmap; 106 } 107 108 return rc; 109 110 unmap: 111 xen_unmap_device_mmio(resources, i); 112 return rc; 113 } 114 115 static int xen_platform_notifier(struct notifier_block *nb, 116 unsigned long action, void *data) 117 { 118 struct platform_device *pdev = to_platform_device(data); 119 int r = 0; 120 121 if (pdev->num_resources == 0 || pdev->resource == NULL) 122 return NOTIFY_OK; 123 124 switch (action) { 125 case BUS_NOTIFY_ADD_DEVICE: 126 r = xen_map_device_mmio(pdev->resource, pdev->num_resources); 127 break; 128 case BUS_NOTIFY_DEL_DEVICE: 129 r = xen_unmap_device_mmio(pdev->resource, pdev->num_resources); 130 break; 131 default: 132 return NOTIFY_DONE; 133 } 134 if (r) 135 dev_err(&pdev->dev, "Platform: Failed to %s device %s MMIO!\n", 136 action == BUS_NOTIFY_ADD_DEVICE ? "map" : 137 (action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"), 138 pdev->name); 139 140 return NOTIFY_OK; 141 } 142 143 static struct notifier_block platform_device_nb = { 144 .notifier_call = xen_platform_notifier, 145 }; 146 147 static int __init register_xen_platform_notifier(void) 148 { 149 if (!xen_initial_domain() || acpi_disabled) 150 return 0; 151 152 return bus_register_notifier(&platform_bus_type, &platform_device_nb); 153 } 154 155 arch_initcall(register_xen_platform_notifier); 156 157 #ifdef CONFIG_ARM_AMBA 158 #include <linux/amba/bus.h> 159 160 static int xen_amba_notifier(struct notifier_block *nb, 161 unsigned long action, void *data) 162 { 163 struct amba_device *adev = to_amba_device(data); 164 int r = 0; 165 166 switch (action) { 167 case BUS_NOTIFY_ADD_DEVICE: 168 r = xen_map_device_mmio(&adev->res, 1); 169 break; 170 case BUS_NOTIFY_DEL_DEVICE: 171 r = xen_unmap_device_mmio(&adev->res, 1); 172 break; 173 default: 174 return NOTIFY_DONE; 175 } 176 if (r) 177 dev_err(&adev->dev, "AMBA: Failed to %s device %s MMIO!\n", 178 action == BUS_NOTIFY_ADD_DEVICE ? "map" : 179 (action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"), 180 adev->dev.init_name); 181 182 return NOTIFY_OK; 183 } 184 185 static struct notifier_block amba_device_nb = { 186 .notifier_call = xen_amba_notifier, 187 }; 188 189 static int __init register_xen_amba_notifier(void) 190 { 191 if (!xen_initial_domain() || acpi_disabled) 192 return 0; 193 194 return bus_register_notifier(&amba_bustype, &amba_device_nb); 195 } 196 197 arch_initcall(register_xen_amba_notifier); 198 #endif 199