[linux-2.6-block.git] / drivers / soc / qcom / apr.c

// SPDX-License-Identifier: GPL-2.0
// Copyright (c) 2011-2017, The Linux Foundation. All rights reserved.
// Copyright (c) 2018, Linaro Limited

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/spinlock.h>
#include <linux/idr.h>
#include <linux/slab.h>
#include <linux/of_device.h>
#include <linux/soc/qcom/apr.h>
#include <linux/rpmsg.h>
#include <linux/of.h>

struct apr {
	struct rpmsg_endpoint *ch;
	struct device *dev;
	spinlock_t svcs_lock;
	struct idr svcs_idr;
	int dest_domain_id;
};

/**
 * apr_send_pkt() - Send a apr message from apr device
 *
 * @adev: Pointer to previously registered apr device.
 * @pkt: Pointer to apr packet to send
 *
 * Return: Will be an negative on packet size on success.
 */
int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)
{
	struct apr *apr = dev_get_drvdata(adev->dev.parent);
	struct apr_hdr *hdr;
	unsigned long flags;
	int ret;

	spin_lock_irqsave(&adev->lock, flags);

	hdr = &pkt->hdr;
	hdr->src_domain = APR_DOMAIN_APPS;
	hdr->src_svc = adev->svc_id;
	hdr->dest_domain = adev->domain_id;
	hdr->dest_svc = adev->svc_id;

	ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
	spin_unlock_irqrestore(&adev->lock, flags);

	return ret ? ret : hdr->pkt_size;
}
EXPORT_SYMBOL_GPL(apr_send_pkt);

static void apr_dev_release(struct device *dev)
{
	struct apr_device *adev = to_apr_device(dev);

	kfree(adev);
}

static int apr_callback(struct rpmsg_device *rpdev, void *buf,
				  int len, void *priv, u32 addr)
{
	struct apr *apr = dev_get_drvdata(&rpdev->dev);
	uint16_t hdr_size, msg_type, ver, svc_id;
	struct apr_device *svc = NULL;
	struct apr_driver *adrv = NULL;
	struct apr_resp_pkt resp;
	struct apr_hdr *hdr;
	unsigned long flags;

	if (len <= APR_HDR_SIZE) {
		dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n",
			buf, len);
		return -EINVAL;
	}

	hdr = buf;
	ver = APR_HDR_FIELD_VER(hdr->hdr_field);
	if (ver > APR_PKT_VER + 1)
		return -EINVAL;

	hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field);
	if (hdr_size < APR_HDR_SIZE) {
		dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size);
		return -EINVAL;
	}

	if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
		dev_err(apr->dev, "APR: Wrong packet size\n");
		return -EINVAL;
	}

	msg_type = APR_HDR_FIELD_MT(hdr->hdr_field);
	if (msg_type >= APR_MSG_TYPE_MAX) {
		dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type);
		return -EINVAL;
	}

	if (hdr->src_domain >= APR_DOMAIN_MAX ||
			hdr->dest_domain >= APR_DOMAIN_MAX ||
			hdr->src_svc >= APR_SVC_MAX ||
			hdr->dest_svc >= APR_SVC_MAX) {
		dev_err(apr->dev, "APR: Wrong APR header\n");
		return -EINVAL;
	}

	svc_id = hdr->dest_svc;
	spin_lock_irqsave(&apr->svcs_lock, flags);
	svc = idr_find(&apr->svcs_idr, svc_id);
	if (svc && svc->dev.driver)
		adrv = to_apr_driver(svc->dev.driver);
	spin_unlock_irqrestore(&apr->svcs_lock, flags);

	if (!adrv) {
		dev_err(apr->dev, "APR: service is not registered\n");
		return -EINVAL;
	}

	resp.hdr = *hdr;
	resp.payload_size = hdr->pkt_size - hdr_size;

	/*
	 * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include
	 * optional headers in to apr_hdr which should be ignored
	 */
	if (resp.payload_size > 0)
		resp.payload = buf + hdr_size;

	adrv->callback(svc, &resp);

	return 0;
}

static int apr_device_match(struct device *dev, struct device_driver *drv)
{
	struct apr_device *adev = to_apr_device(dev);
	struct apr_driver *adrv = to_apr_driver(drv);
	const struct apr_device_id *id = adrv->id_table;

	/* Attempt an OF style match first */
	if (of_driver_match_device(dev, drv))
		return 1;

	if (!id)
		return 0;

	while (id->domain_id != 0 || id->svc_id != 0) {
		if (id->domain_id == adev->domain_id &&
		    id->svc_id == adev->svc_id)
			return 1;
		id++;
	}

	return 0;
}

static int apr_device_probe(struct device *dev)
{
	struct apr_device *adev = to_apr_device(dev);
	struct apr_driver *adrv = to_apr_driver(dev->driver);

	return adrv->probe(adev);
}

static int apr_device_remove(struct device *dev)
{
	struct apr_device *adev = to_apr_device(dev);
	struct apr_driver *adrv;
	struct apr *apr = dev_get_drvdata(adev->dev.parent);

	if (dev->driver) {
		adrv = to_apr_driver(dev->driver);
		if (adrv->remove)
			adrv->remove(adev);
		spin_lock(&apr->svcs_lock);
		idr_remove(&apr->svcs_idr, adev->svc_id);
		spin_unlock(&apr->svcs_lock);
	}

	return 0;
}

static int apr_uevent(struct device *dev, struct kobj_uevent_env *env)
{
	struct apr_device *adev = to_apr_device(dev);
	int ret;

	ret = of_device_uevent_modalias(dev, env);
	if (ret != -ENODEV)
		return ret;

	return add_uevent_var(env, "MODALIAS=apr:%s", adev->name);
}

struct bus_type aprbus = {
	.name		= "aprbus",
	.match		= apr_device_match,
	.probe		= apr_device_probe,
	.uevent		= apr_uevent,
	.remove		= apr_device_remove,
};
EXPORT_SYMBOL_GPL(aprbus);

static int apr_add_device(struct device *dev, struct device_node *np,
			  const struct apr_device_id *id)
{
	struct apr *apr = dev_get_drvdata(dev);
	struct apr_device *adev = NULL;
	int ret;

	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
	if (!adev)
		return -ENOMEM;

	spin_lock_init(&adev->lock);

	adev->svc_id = id->svc_id;
	adev->domain_id = id->domain_id;
	adev->version = id->svc_version;
	if (np)
		snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
	else
		strscpy(adev->name, id->name, APR_NAME_SIZE);

	dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
		     id->domain_id, id->svc_id);

	adev->dev.bus = &aprbus;
	adev->dev.parent = dev;
	adev->dev.of_node = np;
	adev->dev.release = apr_dev_release;
	adev->dev.driver = NULL;

	spin_lock(&apr->svcs_lock);
	idr_alloc(&apr->svcs_idr, adev, id->svc_id,
		  id->svc_id + 1, GFP_ATOMIC);
	spin_unlock(&apr->svcs_lock);

	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));

	ret = device_register(&adev->dev);
	if (ret) {
		dev_err(dev, "device_register failed: %d\n", ret);
		put_device(&adev->dev);
	}

	return ret;
}

static void of_register_apr_devices(struct device *dev)
{
	struct apr *apr = dev_get_drvdata(dev);
	struct device_node *node;

	for_each_child_of_node(dev->of_node, node) {
		struct apr_device_id id = { {0} };

		if (of_property_read_u32(node, "reg", &id.svc_id))
			continue;

		id.domain_id = apr->dest_domain_id;

		if (apr_add_device(dev, node, &id))
			dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
	}
}

static int apr_probe(struct rpmsg_device *rpdev)
{
	struct device *dev = &rpdev->dev;
	struct apr *apr;
	int ret;

	apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
	if (!apr)
		return -ENOMEM;

	ret = of_property_read_u32(dev->of_node, "reg", &apr->dest_domain_id);
	if (ret) {
		dev_err(dev, "APR Domain ID not specified in DT\n");
		return ret;
	}

	dev_set_drvdata(dev, apr);
	apr->ch = rpdev->ept;
	apr->dev = dev;
	spin_lock_init(&apr->svcs_lock);
	idr_init(&apr->svcs_idr);
	of_register_apr_devices(dev);

	return 0;
}

static int apr_remove_device(struct device *dev, void *null)
{
	struct apr_device *adev = to_apr_device(dev);

	device_unregister(&adev->dev);

	return 0;
}

static void apr_remove(struct rpmsg_device *rpdev)
{
	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
}

/*
 * __apr_driver_register() - Client driver registration with aprbus
 *
 * @drv:Client driver to be associated with client-device.
 * @owner: owning module/driver
 *
 * This API will register the client driver with the aprbus
 * It is called from the driver's module-init function.
 */
int __apr_driver_register(struct apr_driver *drv, struct module *owner)
{
	drv->driver.bus = &aprbus;
	drv->driver.owner = owner;

	return driver_register(&drv->driver);
}
EXPORT_SYMBOL_GPL(__apr_driver_register);

/*
 * apr_driver_unregister() - Undo effect of apr_driver_register
 *
 * @drv: Client driver to be unregistered
 */
void apr_driver_unregister(struct apr_driver *drv)
{
	driver_unregister(&drv->driver);
}
EXPORT_SYMBOL_GPL(apr_driver_unregister);

static const struct of_device_id apr_of_match[] = {
	{ .compatible = "qcom,apr"},
	{ .compatible = "qcom,apr-v2"},
	{}
};
MODULE_DEVICE_TABLE(of, apr_of_match);

static struct rpmsg_driver apr_driver = {
	.probe = apr_probe,
	.remove = apr_remove,
	.callback = apr_callback,
	.drv = {
		.name = "qcom,apr",
		.of_match_table = apr_of_match,
	},
};

static int __init apr_init(void)
{
	int ret;

	ret = bus_register(&aprbus);
	if (!ret)
		ret = register_rpmsg_driver(&apr_driver);
	else
		bus_unregister(&aprbus);

	return ret;
}

static void __exit apr_exit(void)
{
	bus_unregister(&aprbus);
	unregister_rpmsg_driver(&apr_driver);
}

subsys_initcall(apr_init);
module_exit(apr_exit);

MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("Qualcomm APR Bus");
Commit	Line	Data
6adba21e SK	1	// SPDX-License-Identifier: GPL-2.0
	2	// Copyright (c) 2011-2017, The Linux Foundation. All rights reserved.
	3	// Copyright (c) 2018, Linaro Limited
	4
	5	#include <linux/kernel.h>
	6	#include <linux/module.h>
	7	#include <linux/device.h>
	8	#include <linux/spinlock.h>
	9	#include <linux/idr.h>
	10	#include <linux/slab.h>
	11	#include <linux/of_device.h>
	12	#include <linux/soc/qcom/apr.h>
	13	#include <linux/rpmsg.h>
	14	#include <linux/of.h>
	15
	16	struct apr {
	17	struct rpmsg_endpoint *ch;
	18	struct device *dev;
	19	spinlock_t svcs_lock;
	20	struct idr svcs_idr;
	21	int dest_domain_id;
	22	};
	23
	24	/**
	25	* apr_send_pkt() - Send a apr message from apr device
	26	*
	27	* @adev: Pointer to previously registered apr device.
	28	* @pkt: Pointer to apr packet to send
	29	*
	30	* Return: Will be an negative on packet size on success.
	31	*/
	32	int apr_send_pkt(struct apr_device adev, struct apr_pkt pkt)
	33	{
	34	struct apr *apr = dev_get_drvdata(adev->dev.parent);
	35	struct apr_hdr *hdr;
	36	unsigned long flags;
	37	int ret;
	38
	39	spin_lock_irqsave(&adev->lock, flags);
	40
	41	hdr = &pkt->hdr;
	42	hdr->src_domain = APR_DOMAIN_APPS;
	43	hdr->src_svc = adev->svc_id;
	44	hdr->dest_domain = adev->domain_id;
	45	hdr->dest_svc = adev->svc_id;
	46
	47	ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
	48	spin_unlock_irqrestore(&adev->lock, flags);
	49
	50	return ret ? ret : hdr->pkt_size;
	51	}
	52	EXPORT_SYMBOL_GPL(apr_send_pkt);
	53
	54	static void apr_dev_release(struct device *dev)
	55	{
	56	struct apr_device *adev = to_apr_device(dev);
	57
	58	kfree(adev);
	59	}
	60
	61	static int apr_callback(struct rpmsg_device rpdev, void buf,
	62	int len, void *priv, u32 addr)
	63	{
	64	struct apr *apr = dev_get_drvdata(&rpdev->dev);
65	uint16_t hdr_size, msg_type, ver, svc_id;
66	struct apr_device *svc = NULL;
67	struct apr_driver *adrv = NULL;
68	struct apr_resp_pkt resp;
69	struct apr_hdr *hdr;
70	unsigned long flags;
71
72	if (len <= APR_HDR_SIZE) {
73	dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n",
74	buf, len);
75	return -EINVAL;
76	}
77
78	hdr = buf;
79	ver = APR_HDR_FIELD_VER(hdr->hdr_field);
80	if (ver > APR_PKT_VER + 1)
81	return -EINVAL;
82
83	hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field);
84	if (hdr_size < APR_HDR_SIZE) {
85	dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size);
86	return -EINVAL;
87	}
88
89	if (hdr->pkt_size < APR_HDR_SIZE \|\| hdr->pkt_size != len) {
35aac0ba	90	dev_err(apr->dev, "APR: Wrong packet size\n");
6adba21e SK	91	return -EINVAL;
	92	}
	93
	94	msg_type = APR_HDR_FIELD_MT(hdr->hdr_field);
e744619d	95	if (msg_type >= APR_MSG_TYPE_MAX) {
6adba21e SK	96	dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type);
	97	return -EINVAL;
	98	}
	99
	100	if (hdr->src_domain >= APR_DOMAIN_MAX \|\|
	101	hdr->dest_domain >= APR_DOMAIN_MAX \|\|
	102	hdr->src_svc >= APR_SVC_MAX \|\|
	103	hdr->dest_svc >= APR_SVC_MAX) {
	104	dev_err(apr->dev, "APR: Wrong APR header\n");
	105	return -EINVAL;
	106	}
	107
	108	svc_id = hdr->dest_svc;
	109	spin_lock_irqsave(&apr->svcs_lock, flags);
	110	svc = idr_find(&apr->svcs_idr, svc_id);
	111	if (svc && svc->dev.driver)
	112	adrv = to_apr_driver(svc->dev.driver);
	113	spin_unlock_irqrestore(&apr->svcs_lock, flags);
	114
	115	if (!adrv) {
	116	dev_err(apr->dev, "APR: service is not registered\n");
	117	return -EINVAL;
	118	}
	119
	120	resp.hdr = *hdr;
	121	resp.payload_size = hdr->pkt_size - hdr_size;
	122
	123	/*
	124	* NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include
	125	* optional headers in to apr_hdr which should be ignored
	126	*/
	127	if (resp.payload_size > 0)
	128	resp.payload = buf + hdr_size;
	129
	130	adrv->callback(svc, &resp);
	131
	132	return 0;
	133	}
	134
	135	static int apr_device_match(struct device dev, struct device_driver drv)
	136	{
	137	struct apr_device *adev = to_apr_device(dev);
	138	struct apr_driver *adrv = to_apr_driver(drv);
	139	const struct apr_device_id *id = adrv->id_table;
	140
	141	/* Attempt an OF style match first */
	142	if (of_driver_match_device(dev, drv))
	143	return 1;
	144
	145	if (!id)
	146	return 0;
	147
	148	while (id->domain_id != 0 \|\| id->svc_id != 0) {
	149	if (id->domain_id == adev->domain_id &&
	150	id->svc_id == adev->svc_id)
	151	return 1;
	152	id++;
	153	}
	154
	155	return 0;
	156	}
	157
	158	static int apr_device_probe(struct device *dev)
	159	{
160	struct apr_device *adev = to_apr_device(dev);
161	struct apr_driver *adrv = to_apr_driver(dev->driver);
162
163	return adrv->probe(adev);
164	}
165
166	static int apr_device_remove(struct device *dev)
167	{
168	struct apr_device *adev = to_apr_device(dev);
169	struct apr_driver *adrv;
170	struct apr *apr = dev_get_drvdata(adev->dev.parent);
171
172	if (dev->driver) {
173	adrv = to_apr_driver(dev->driver);
174	if (adrv->remove)
175	adrv->remove(adev);
176	spin_lock(&apr->svcs_lock);
177	idr_remove(&apr->svcs_idr, adev->svc_id);
178	spin_unlock(&apr->svcs_lock);
179	}
180
181	return 0;
182	}
183
184	static int apr_uevent(struct device dev, struct kobj_uevent_env env)
185	{
186	struct apr_device *adev = to_apr_device(dev);
187	int ret;
188
189	ret = of_device_uevent_modalias(dev, env);
190	if (ret != -ENODEV)
191	return ret;
192
193	return add_uevent_var(env, "MODALIAS=apr:%s", adev->name);
194	}
195
196	struct bus_type aprbus = {
197	.name = "aprbus",
198	.match = apr_device_match,
199	.probe = apr_device_probe,
200	.uevent = apr_uevent,
201	.remove = apr_device_remove,
202	};
203	EXPORT_SYMBOL_GPL(aprbus);
204
205	static int apr_add_device(struct device dev, struct device_node np,
206	const struct apr_device_id *id)
207	{
208	struct apr *apr = dev_get_drvdata(dev);
209	struct apr_device *adev = NULL;
210	int ret;
211
212	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
213	if (!adev)
214	return -ENOMEM;
215
216	spin_lock_init(&adev->lock);
217
218	adev->svc_id = id->svc_id;
219	adev->domain_id = id->domain_id;
220	adev->version = id->svc_version;
221	if (np)
dc37a252	222	snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
6adba21e	223	else
4fadb265	224	strscpy(adev->name, id->name, APR_NAME_SIZE);
6adba21e SK	225
	226	dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
	227	id->domain_id, id->svc_id);
	228
	229	adev->dev.bus = &aprbus;
	230	adev->dev.parent = dev;
	231	adev->dev.of_node = np;
	232	adev->dev.release = apr_dev_release;
	233	adev->dev.driver = NULL;
	234
	235	spin_lock(&apr->svcs_lock);
	236	idr_alloc(&apr->svcs_idr, adev, id->svc_id,
	237	id->svc_id + 1, GFP_ATOMIC);
	238	spin_unlock(&apr->svcs_lock);
	239
	240	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
	241
	242	ret = device_register(&adev->dev);
	243	if (ret) {
	244	dev_err(dev, "device_register failed: %d\n", ret);
	245	put_device(&adev->dev);
	246	}
	247
	248	return ret;
	249	}
	250
	251	static void of_register_apr_devices(struct device *dev)
	252	{
	253	struct apr *apr = dev_get_drvdata(dev);
	254	struct device_node *node;
	255
	256	for_each_child_of_node(dev->of_node, node) {
	257	struct apr_device_id id = { {0} };
	258
	259	if (of_property_read_u32(node, "reg", &id.svc_id))
	260	continue;
	261
	262	id.domain_id = apr->dest_domain_id;
	263
	264	if (apr_add_device(dev, node, &id))
	265	dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
	266	}
	267	}
	268
	269	static int apr_probe(struct rpmsg_device *rpdev)
	270	{
	271	struct device *dev = &rpdev->dev;
	272	struct apr *apr;
	273	int ret;
	274
	275	apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
	276	if (!apr)
	277	return -ENOMEM;
	278
	279	ret = of_property_read_u32(dev->of_node, "reg", &apr->dest_domain_id);
	280	if (ret) {
	281	dev_err(dev, "APR Domain ID not specified in DT\n");
	282	return ret;
	283	}
	284
	285	dev_set_drvdata(dev, apr);
	286	apr->ch = rpdev->ept;
	287	apr->dev = dev;
	288	spin_lock_init(&apr->svcs_lock);
289	idr_init(&apr->svcs_idr);
290	of_register_apr_devices(dev);
291
292	return 0;
293	}
294
295	static int apr_remove_device(struct device dev, void null)
296	{
297	struct apr_device *adev = to_apr_device(dev);
298
299	device_unregister(&adev->dev);
300
301	return 0;
302	}
303
304	static void apr_remove(struct rpmsg_device *rpdev)
305	{
306	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
307	}
308
309	/*
310	* __apr_driver_register() - Client driver registration with aprbus
311	*
312	* @drv:Client driver to be associated with client-device.
313	* @owner: owning module/driver
314	*
315	* This API will register the client driver with the aprbus
316	* It is called from the driver's module-init function.
317	*/
318	int __apr_driver_register(struct apr_driver drv, struct module owner)
319	{
320	drv->driver.bus = &aprbus;
321	drv->driver.owner = owner;
322
323	return driver_register(&drv->driver);
324	}
325	EXPORT_SYMBOL_GPL(__apr_driver_register);
326
327	/*
328	* apr_driver_unregister() - Undo effect of apr_driver_register
329	*
330	* @drv: Client driver to be unregistered
331	*/
332	void apr_driver_unregister(struct apr_driver *drv)
333	{
334	driver_unregister(&drv->driver);
335	}
336	EXPORT_SYMBOL_GPL(apr_driver_unregister);
337
338	static const struct of_device_id apr_of_match[] = {
339	{ .compatible = "qcom,apr"},
340	{ .compatible = "qcom,apr-v2"},
341	{}
342	};
343	MODULE_DEVICE_TABLE(of, apr_of_match);
344
345	static struct rpmsg_driver apr_driver = {
346	.probe = apr_probe,
347	.remove = apr_remove,
348	.callback = apr_callback,
349	.drv = {
350	.name = "qcom,apr",
351	.of_match_table = apr_of_match,
352	},
353	};
354
355	static int __init apr_init(void)
356	{
357	int ret;
358
359	ret = bus_register(&aprbus);
360	if (!ret)
361	ret = register_rpmsg_driver(&apr_driver);
362	else
363	bus_unregister(&aprbus);
364
365	return ret;
366	}
367
368	static void __exit apr_exit(void)
369	{
370	bus_unregister(&aprbus);
371	unregister_rpmsg_driver(&apr_driver);
372	}
373
374	subsys_initcall(apr_init);
375	module_exit(apr_exit);
376
377	MODULE_LICENSE("GPL v2");
378	MODULE_DESCRIPTION("Qualcomm APR Bus");