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