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(res: 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 | arg: &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(res: r) != IORESOURCE_MEM) || (nr == 0)) |
60 | continue; |
61 | |
62 | gpfns = kcalloc(n: nr, size: sizeof(xen_pfn_t), GFP_KERNEL); |
63 | idxs = kcalloc(n: nr, size: sizeof(xen_ulong_t), GFP_KERNEL); |
64 | errs = kcalloc(n: nr, size: sizeof(int), GFP_KERNEL); |
65 | if (!gpfns || !idxs || !errs) { |
66 | kfree(objp: gpfns); |
67 | kfree(objp: idxs); |
68 | kfree(objp: 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, arg: &xatp); |
90 | kfree(objp: gpfns); |
91 | kfree(objp: idxs); |
92 | kfree(objp: errs); |
93 | if (rc) |
94 | goto unmap; |
95 | } |
96 | |
97 | return rc; |
98 | |
99 | unmap: |
100 | xen_unmap_device_mmio(resources, count: 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(resources: pdev->resource, count: pdev->num_resources); |
116 | break; |
117 | case BUS_NOTIFY_DEL_DEVICE: |
118 | r = xen_unmap_device_mmio(resources: pdev->resource, count: 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(bus: &platform_bus_type, nb: &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 | |