[mirror_ubuntu-zesty-kernel.git] / drivers / xen / arm-device.c

/*
 * Copyright (c) 2015, Linaro Limited, Shannon Zhao
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <linux/platform_device.h>
#include <linux/acpi.h>
#include <xen/xen.h>
#include <xen/page.h>
#include <xen/interface/memory.h>
#include <asm/xen/hypervisor.h>
#include <asm/xen/hypercall.h>

static int xen_unmap_device_mmio(const struct resource *resources,
				 unsigned int count)
{
	unsigned int i, j, nr;
	int rc = 0;
	const struct resource *r;
	struct xen_remove_from_physmap xrp;

	for (i = 0; i < count; i++) {
		r = &resources[i];
		nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE);
		if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0))
			continue;

		for (j = 0; j < nr; j++) {
			xrp.domid = DOMID_SELF;
			xrp.gpfn = XEN_PFN_DOWN(r->start) + j;
			rc = HYPERVISOR_memory_op(XENMEM_remove_from_physmap,
						  &xrp);
			if (rc)
				return rc;
		}
	}

	return rc;
}

static int xen_map_device_mmio(const struct resource *resources,
			       unsigned int count)
{
	unsigned int i, j, nr;
	int rc = 0;
	const struct resource *r;
	xen_pfn_t *gpfns;
	xen_ulong_t *idxs;
	int *errs;
	struct xen_add_to_physmap_range xatp;

	for (i = 0; i < count; i++) {
		r = &resources[i];
		nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE);
		if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0))
			continue;

		gpfns = kzalloc(sizeof(xen_pfn_t) * nr, GFP_KERNEL);
		idxs = kzalloc(sizeof(xen_ulong_t) * nr, GFP_KERNEL);
		errs = kzalloc(sizeof(int) * nr, GFP_KERNEL);
		if (!gpfns || !idxs || !errs) {
			kfree(gpfns);
			kfree(idxs);
			kfree(errs);
			rc = -ENOMEM;
			goto unmap;
		}

		for (j = 0; j < nr; j++) {
			/*
			 * The regions are always mapped 1:1 to DOM0 and this is
			 * fine because the memory map for DOM0 is the same as
			 * the host (except for the RAM).
			 */
			gpfns[j] = XEN_PFN_DOWN(r->start) + j;
			idxs[j] = XEN_PFN_DOWN(r->start) + j;
		}

		xatp.domid = DOMID_SELF;
		xatp.size = nr;
		xatp.space = XENMAPSPACE_dev_mmio;

		set_xen_guest_handle(xatp.gpfns, gpfns);
		set_xen_guest_handle(xatp.idxs, idxs);
		set_xen_guest_handle(xatp.errs, errs);

		rc = HYPERVISOR_memory_op(XENMEM_add_to_physmap_range, &xatp);
		kfree(gpfns);
		kfree(idxs);
		kfree(errs);
		if (rc)
			goto unmap;
	}

	return rc;

unmap:
	xen_unmap_device_mmio(resources, i);
	return rc;
}

static int xen_platform_notifier(struct notifier_block *nb,
				 unsigned long action, void *data)
{
	struct platform_device *pdev = to_platform_device(data);
	int r = 0;

	if (pdev->num_resources == 0 || pdev->resource == NULL)
		return NOTIFY_OK;

	switch (action) {
	case BUS_NOTIFY_ADD_DEVICE:
		r = xen_map_device_mmio(pdev->resource, pdev->num_resources);
		break;
	case BUS_NOTIFY_DEL_DEVICE:
		r = xen_unmap_device_mmio(pdev->resource, pdev->num_resources);
		break;
	default:
		return NOTIFY_DONE;
	}
	if (r)
		dev_err(&pdev->dev, "Platform: Failed to %s device %s MMIO!\n",
			action == BUS_NOTIFY_ADD_DEVICE ? "map" :
			(action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
			pdev->name);

	return NOTIFY_OK;
}

static struct notifier_block platform_device_nb = {
	.notifier_call = xen_platform_notifier,
};

static int __init register_xen_platform_notifier(void)
{
	if (!xen_initial_domain() || acpi_disabled)
		return 0;

	return bus_register_notifier(&platform_bus_type, &platform_device_nb);
}

arch_initcall(register_xen_platform_notifier);

#ifdef CONFIG_ARM_AMBA
#include <linux/amba/bus.h>

static int xen_amba_notifier(struct notifier_block *nb,
			     unsigned long action, void *data)
{
	struct amba_device *adev = to_amba_device(data);
	int r = 0;

	switch (action) {
	case BUS_NOTIFY_ADD_DEVICE:
		r = xen_map_device_mmio(&adev->res, 1);
		break;
	case BUS_NOTIFY_DEL_DEVICE:
		r = xen_unmap_device_mmio(&adev->res, 1);
		break;
	default:
		return NOTIFY_DONE;
	}
	if (r)
		dev_err(&adev->dev, "AMBA: Failed to %s device %s MMIO!\n",
			action == BUS_NOTIFY_ADD_DEVICE ? "map" :
			(action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
			adev->dev.init_name);

	return NOTIFY_OK;
}

static struct notifier_block amba_device_nb = {
	.notifier_call = xen_amba_notifier,
};

static int __init register_xen_amba_notifier(void)
{
	if (!xen_initial_domain() || acpi_disabled)
		return 0;

	return bus_register_notifier(&amba_bustype, &amba_device_nb);
}

arch_initcall(register_xen_amba_notifier);
#endif
Commit	Line	Data
4ba04bec SZ	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	struct xen_add_to_physmap_range xatp;
	62
	63	for (i = 0; i < count; i++) {
	64	r = &resources[i];
65	nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE);
66	if ((resource_type(r) != IORESOURCE_MEM) \|\| (nr == 0))
67	continue;
68
69	gpfns = kzalloc(sizeof(xen_pfn_t) * nr, GFP_KERNEL);
70	idxs = kzalloc(sizeof(xen_ulong_t) * nr, GFP_KERNEL);
71	errs = kzalloc(sizeof(int) * nr, GFP_KERNEL);
72	if (!gpfns \|\| !idxs \|\| !errs) {
73	kfree(gpfns);
74	kfree(idxs);
75	kfree(errs);
76	rc = -ENOMEM;
77	goto unmap;
78	}
79
80	for (j = 0; j < nr; j++) {
81	/*
82	* The regions are always mapped 1:1 to DOM0 and this is
83	* fine because the memory map for DOM0 is the same as
84	* the host (except for the RAM).
85	*/
86	gpfns[j] = XEN_PFN_DOWN(r->start) + j;
87	idxs[j] = XEN_PFN_DOWN(r->start) + j;
88	}
89
90	xatp.domid = DOMID_SELF;
91	xatp.size = nr;
92	xatp.space = XENMAPSPACE_dev_mmio;
93
94	set_xen_guest_handle(xatp.gpfns, gpfns);
95	set_xen_guest_handle(xatp.idxs, idxs);
96	set_xen_guest_handle(xatp.errs, errs);
97
98	rc = HYPERVISOR_memory_op(XENMEM_add_to_physmap_range, &xatp);
99	kfree(gpfns);
100	kfree(idxs);
101	kfree(errs);
102	if (rc)
103	goto unmap;
104	}
105
106	return rc;
107
108	unmap:
109	xen_unmap_device_mmio(resources, i);
110	return rc;
111	}
112
113	static int xen_platform_notifier(struct notifier_block *nb,
114	unsigned long action, void *data)
115	{
116	struct platform_device *pdev = to_platform_device(data);
117	int r = 0;
118
119	if (pdev->num_resources == 0 \|\| pdev->resource == NULL)
120	return NOTIFY_OK;
121
122	switch (action) {
123	case BUS_NOTIFY_ADD_DEVICE:
124	r = xen_map_device_mmio(pdev->resource, pdev->num_resources);
125	break;
126	case BUS_NOTIFY_DEL_DEVICE:
127	r = xen_unmap_device_mmio(pdev->resource, pdev->num_resources);
128	break;
129	default:
130	return NOTIFY_DONE;
131	}
132	if (r)
133	dev_err(&pdev->dev, "Platform: Failed to %s device %s MMIO!\n",
134	action == BUS_NOTIFY_ADD_DEVICE ? "map" :
135	(action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
136	pdev->name);
137
138	return NOTIFY_OK;
139	}
140
141	static struct notifier_block platform_device_nb = {
142	.notifier_call = xen_platform_notifier,
143	};
144
145	static int __init register_xen_platform_notifier(void)
146	{
147	if (!xen_initial_domain() \|\| acpi_disabled)
148	return 0;
149
150	return bus_register_notifier(&platform_bus_type, &platform_device_nb);
151	}
152
153	arch_initcall(register_xen_platform_notifier);
5789afeb SZ	154
	155	#ifdef CONFIG_ARM_AMBA
	156	#include <linux/amba/bus.h>
	157
	158	static int xen_amba_notifier(struct notifier_block *nb,
	159	unsigned long action, void *data)
	160	{
	161	struct amba_device *adev = to_amba_device(data);
	162	int r = 0;
	163
	164	switch (action) {
	165	case BUS_NOTIFY_ADD_DEVICE:
	166	r = xen_map_device_mmio(&adev->res, 1);
	167	break;
	168	case BUS_NOTIFY_DEL_DEVICE:
	169	r = xen_unmap_device_mmio(&adev->res, 1);
	170	break;
	171	default:
	172	return NOTIFY_DONE;
	173	}
	174	if (r)
	175	dev_err(&adev->dev, "AMBA: Failed to %s device %s MMIO!\n",
	176	action == BUS_NOTIFY_ADD_DEVICE ? "map" :
	177	(action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
	178	adev->dev.init_name);
	179
	180	return NOTIFY_OK;
	181	}
	182
	183	static struct notifier_block amba_device_nb = {
	184	.notifier_call = xen_amba_notifier,
	185	};
	186
	187	static int __init register_xen_amba_notifier(void)
	188	{
	189	if (!xen_initial_domain() \|\| acpi_disabled)
	190	return 0;
	191
	192	return bus_register_notifier(&amba_bustype, &amba_device_nb);
	193	}
	194
	195	arch_initcall(register_xen_amba_notifier);
	196	#endif