Linux Audio

Check our new training course

Loading...
Note: File does not exist in v4.10.11.
  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/workqueue.h>
 12#include <linux/of_device.h>
 13#include <linux/soc/qcom/apr.h>
 14#include <linux/soc/qcom/pdr.h>
 15#include <linux/rpmsg.h>
 16#include <linux/of.h>
 17
 18struct apr {
 19	struct rpmsg_endpoint *ch;
 20	struct device *dev;
 21	spinlock_t svcs_lock;
 22	spinlock_t rx_lock;
 23	struct idr svcs_idr;
 24	int dest_domain_id;
 25	struct pdr_handle *pdr;
 26	struct workqueue_struct *rxwq;
 27	struct work_struct rx_work;
 28	struct list_head rx_list;
 29};
 30
 31struct apr_rx_buf {
 32	struct list_head node;
 33	int len;
 34	uint8_t buf[];
 35};
 36
 37/**
 38 * apr_send_pkt() - Send a apr message from apr device
 39 *
 40 * @adev: Pointer to previously registered apr device.
 41 * @pkt: Pointer to apr packet to send
 42 *
 43 * Return: Will be an negative on packet size on success.
 44 */
 45int apr_send_pkt(struct apr_device *adev, struct apr_pkt *pkt)
 46{
 47	struct apr *apr = dev_get_drvdata(adev->dev.parent);
 48	struct apr_hdr *hdr;
 49	unsigned long flags;
 50	int ret;
 51
 52	spin_lock_irqsave(&adev->lock, flags);
 53
 54	hdr = &pkt->hdr;
 55	hdr->src_domain = APR_DOMAIN_APPS;
 56	hdr->src_svc = adev->svc_id;
 57	hdr->dest_domain = adev->domain_id;
 58	hdr->dest_svc = adev->svc_id;
 59
 60	ret = rpmsg_trysend(apr->ch, pkt, hdr->pkt_size);
 61	spin_unlock_irqrestore(&adev->lock, flags);
 62
 63	return ret ? ret : hdr->pkt_size;
 64}
 65EXPORT_SYMBOL_GPL(apr_send_pkt);
 66
 67static void apr_dev_release(struct device *dev)
 68{
 69	struct apr_device *adev = to_apr_device(dev);
 70
 71	kfree(adev);
 72}
 73
 74static int apr_callback(struct rpmsg_device *rpdev, void *buf,
 75				  int len, void *priv, u32 addr)
 76{
 77	struct apr *apr = dev_get_drvdata(&rpdev->dev);
 78	struct apr_rx_buf *abuf;
 79	unsigned long flags;
 80
 81	if (len <= APR_HDR_SIZE) {
 82		dev_err(apr->dev, "APR: Improper apr pkt received:%p %d\n",
 83			buf, len);
 84		return -EINVAL;
 85	}
 86
 87	abuf = kzalloc(sizeof(*abuf) + len, GFP_ATOMIC);
 88	if (!abuf)
 89		return -ENOMEM;
 90
 91	abuf->len = len;
 92	memcpy(abuf->buf, buf, len);
 93
 94	spin_lock_irqsave(&apr->rx_lock, flags);
 95	list_add_tail(&abuf->node, &apr->rx_list);
 96	spin_unlock_irqrestore(&apr->rx_lock, flags);
 97
 98	queue_work(apr->rxwq, &apr->rx_work);
 99
100	return 0;
101}
102
103
104static int apr_do_rx_callback(struct apr *apr, struct apr_rx_buf *abuf)
105{
106	uint16_t hdr_size, msg_type, ver, svc_id;
107	struct apr_device *svc = NULL;
108	struct apr_driver *adrv = NULL;
109	struct apr_resp_pkt resp;
110	struct apr_hdr *hdr;
111	unsigned long flags;
112	void *buf = abuf->buf;
113	int len = abuf->len;
114
115	hdr = buf;
116	ver = APR_HDR_FIELD_VER(hdr->hdr_field);
117	if (ver > APR_PKT_VER + 1)
118		return -EINVAL;
119
120	hdr_size = APR_HDR_FIELD_SIZE_BYTES(hdr->hdr_field);
121	if (hdr_size < APR_HDR_SIZE) {
122		dev_err(apr->dev, "APR: Wrong hdr size:%d\n", hdr_size);
123		return -EINVAL;
124	}
125
126	if (hdr->pkt_size < APR_HDR_SIZE || hdr->pkt_size != len) {
127		dev_err(apr->dev, "APR: Wrong packet size\n");
128		return -EINVAL;
129	}
130
131	msg_type = APR_HDR_FIELD_MT(hdr->hdr_field);
132	if (msg_type >= APR_MSG_TYPE_MAX) {
133		dev_err(apr->dev, "APR: Wrong message type: %d\n", msg_type);
134		return -EINVAL;
135	}
136
137	if (hdr->src_domain >= APR_DOMAIN_MAX ||
138			hdr->dest_domain >= APR_DOMAIN_MAX ||
139			hdr->src_svc >= APR_SVC_MAX ||
140			hdr->dest_svc >= APR_SVC_MAX) {
141		dev_err(apr->dev, "APR: Wrong APR header\n");
142		return -EINVAL;
143	}
144
145	svc_id = hdr->dest_svc;
146	spin_lock_irqsave(&apr->svcs_lock, flags);
147	svc = idr_find(&apr->svcs_idr, svc_id);
148	if (svc && svc->dev.driver)
149		adrv = to_apr_driver(svc->dev.driver);
150	spin_unlock_irqrestore(&apr->svcs_lock, flags);
151
152	if (!adrv) {
153		dev_err(apr->dev, "APR: service is not registered\n");
154		return -EINVAL;
155	}
156
157	resp.hdr = *hdr;
158	resp.payload_size = hdr->pkt_size - hdr_size;
159
160	/*
161	 * NOTE: hdr_size is not same as APR_HDR_SIZE as remote can include
162	 * optional headers in to apr_hdr which should be ignored
163	 */
164	if (resp.payload_size > 0)
165		resp.payload = buf + hdr_size;
166
167	adrv->callback(svc, &resp);
168
169	return 0;
170}
171
172static void apr_rxwq(struct work_struct *work)
173{
174	struct apr *apr = container_of(work, struct apr, rx_work);
175	struct apr_rx_buf *abuf, *b;
176	unsigned long flags;
177
178	if (!list_empty(&apr->rx_list)) {
179		list_for_each_entry_safe(abuf, b, &apr->rx_list, node) {
180			apr_do_rx_callback(apr, abuf);
181			spin_lock_irqsave(&apr->rx_lock, flags);
182			list_del(&abuf->node);
183			spin_unlock_irqrestore(&apr->rx_lock, flags);
184			kfree(abuf);
185		}
186	}
187}
188
189static int apr_device_match(struct device *dev, struct device_driver *drv)
190{
191	struct apr_device *adev = to_apr_device(dev);
192	struct apr_driver *adrv = to_apr_driver(drv);
193	const struct apr_device_id *id = adrv->id_table;
194
195	/* Attempt an OF style match first */
196	if (of_driver_match_device(dev, drv))
197		return 1;
198
199	if (!id)
200		return 0;
201
202	while (id->domain_id != 0 || id->svc_id != 0) {
203		if (id->domain_id == adev->domain_id &&
204		    id->svc_id == adev->svc_id)
205			return 1;
206		id++;
207	}
208
209	return 0;
210}
211
212static int apr_device_probe(struct device *dev)
213{
214	struct apr_device *adev = to_apr_device(dev);
215	struct apr_driver *adrv = to_apr_driver(dev->driver);
216
217	return adrv->probe(adev);
218}
219
220static int apr_device_remove(struct device *dev)
221{
222	struct apr_device *adev = to_apr_device(dev);
223	struct apr_driver *adrv;
224	struct apr *apr = dev_get_drvdata(adev->dev.parent);
225
226	if (dev->driver) {
227		adrv = to_apr_driver(dev->driver);
228		if (adrv->remove)
229			adrv->remove(adev);
230		spin_lock(&apr->svcs_lock);
231		idr_remove(&apr->svcs_idr, adev->svc_id);
232		spin_unlock(&apr->svcs_lock);
233	}
234
235	return 0;
236}
237
238static int apr_uevent(struct device *dev, struct kobj_uevent_env *env)
239{
240	struct apr_device *adev = to_apr_device(dev);
241	int ret;
242
243	ret = of_device_uevent_modalias(dev, env);
244	if (ret != -ENODEV)
245		return ret;
246
247	return add_uevent_var(env, "MODALIAS=apr:%s", adev->name);
248}
249
250struct bus_type aprbus = {
251	.name		= "aprbus",
252	.match		= apr_device_match,
253	.probe		= apr_device_probe,
254	.uevent		= apr_uevent,
255	.remove		= apr_device_remove,
256};
257EXPORT_SYMBOL_GPL(aprbus);
258
259static int apr_add_device(struct device *dev, struct device_node *np,
260			  const struct apr_device_id *id)
261{
262	struct apr *apr = dev_get_drvdata(dev);
263	struct apr_device *adev = NULL;
264	int ret;
265
266	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
267	if (!adev)
268		return -ENOMEM;
269
270	spin_lock_init(&adev->lock);
271
272	adev->svc_id = id->svc_id;
273	adev->domain_id = id->domain_id;
274	adev->version = id->svc_version;
275	if (np)
276		snprintf(adev->name, APR_NAME_SIZE, "%pOFn", np);
277	else
278		strscpy(adev->name, id->name, APR_NAME_SIZE);
279
280	dev_set_name(&adev->dev, "aprsvc:%s:%x:%x", adev->name,
281		     id->domain_id, id->svc_id);
282
283	adev->dev.bus = &aprbus;
284	adev->dev.parent = dev;
285	adev->dev.of_node = np;
286	adev->dev.release = apr_dev_release;
287	adev->dev.driver = NULL;
288
289	spin_lock(&apr->svcs_lock);
290	idr_alloc(&apr->svcs_idr, adev, id->svc_id,
291		  id->svc_id + 1, GFP_ATOMIC);
292	spin_unlock(&apr->svcs_lock);
293
294	of_property_read_string_index(np, "qcom,protection-domain",
295				      1, &adev->service_path);
296
297	dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
298
299	ret = device_register(&adev->dev);
300	if (ret) {
301		dev_err(dev, "device_register failed: %d\n", ret);
302		put_device(&adev->dev);
303	}
304
305	return ret;
306}
307
308static int of_apr_add_pd_lookups(struct device *dev)
309{
310	const char *service_name, *service_path;
311	struct apr *apr = dev_get_drvdata(dev);
312	struct device_node *node;
313	struct pdr_service *pds;
314	int ret;
315
316	for_each_child_of_node(dev->of_node, node) {
317		ret = of_property_read_string_index(node, "qcom,protection-domain",
318						    0, &service_name);
319		if (ret < 0)
320			continue;
321
322		ret = of_property_read_string_index(node, "qcom,protection-domain",
323						    1, &service_path);
324		if (ret < 0) {
325			dev_err(dev, "pdr service path missing: %d\n", ret);
326			return ret;
327		}
328
329		pds = pdr_add_lookup(apr->pdr, service_name, service_path);
330		if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
331			dev_err(dev, "pdr add lookup failed: %d\n", ret);
332			return PTR_ERR(pds);
333		}
334	}
335
336	return 0;
337}
338
339static void of_register_apr_devices(struct device *dev, const char *svc_path)
340{
341	struct apr *apr = dev_get_drvdata(dev);
342	struct device_node *node;
343	const char *service_path;
344	int ret;
345
346	for_each_child_of_node(dev->of_node, node) {
347		struct apr_device_id id = { {0} };
348
349		/*
350		 * This function is called with svc_path NULL during
351		 * apr_probe(), in which case we register any apr devices
352		 * without a qcom,protection-domain specified.
353		 *
354		 * Then as the protection domains becomes available
355		 * (if applicable) this function is again called, but with
356		 * svc_path representing the service becoming available. In
357		 * this case we register any apr devices with a matching
358		 * qcom,protection-domain.
359		 */
360
361		ret = of_property_read_string_index(node, "qcom,protection-domain",
362						    1, &service_path);
363		if (svc_path) {
364			/* skip APR services that are PD independent */
365			if (ret)
366				continue;
367
368			/* skip APR services whose PD paths don't match */
369			if (strcmp(service_path, svc_path))
370				continue;
371		} else {
372			/* skip APR services whose PD lookups are registered */
373			if (ret == 0)
374				continue;
375		}
376
377		if (of_property_read_u32(node, "reg", &id.svc_id))
378			continue;
379
380		id.domain_id = apr->dest_domain_id;
381
382		if (apr_add_device(dev, node, &id))
383			dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
384	}
385}
386
387static int apr_remove_device(struct device *dev, void *svc_path)
388{
389	struct apr_device *adev = to_apr_device(dev);
390
391	if (svc_path && adev->service_path) {
392		if (!strcmp(adev->service_path, (char *)svc_path))
393			device_unregister(&adev->dev);
394	} else {
395		device_unregister(&adev->dev);
396	}
397
398	return 0;
399}
400
401static void apr_pd_status(int state, char *svc_path, void *priv)
402{
403	struct apr *apr = (struct apr *)priv;
404
405	switch (state) {
406	case SERVREG_SERVICE_STATE_UP:
407		of_register_apr_devices(apr->dev, svc_path);
408		break;
409	case SERVREG_SERVICE_STATE_DOWN:
410		device_for_each_child(apr->dev, svc_path, apr_remove_device);
411		break;
412	}
413}
414
415static int apr_probe(struct rpmsg_device *rpdev)
416{
417	struct device *dev = &rpdev->dev;
418	struct apr *apr;
419	int ret;
420
421	apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
422	if (!apr)
423		return -ENOMEM;
424
425	ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &apr->dest_domain_id);
426	if (ret) {
427		dev_err(dev, "APR Domain ID not specified in DT\n");
428		return ret;
429	}
430
431	dev_set_drvdata(dev, apr);
432	apr->ch = rpdev->ept;
433	apr->dev = dev;
434	apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
435	if (!apr->rxwq) {
436		dev_err(apr->dev, "Failed to start Rx WQ\n");
437		return -ENOMEM;
438	}
439	INIT_WORK(&apr->rx_work, apr_rxwq);
440
441	apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
442	if (IS_ERR(apr->pdr)) {
443		dev_err(dev, "Failed to init PDR handle\n");
444		ret = PTR_ERR(apr->pdr);
445		goto destroy_wq;
446	}
447
448	INIT_LIST_HEAD(&apr->rx_list);
449	spin_lock_init(&apr->rx_lock);
450	spin_lock_init(&apr->svcs_lock);
451	idr_init(&apr->svcs_idr);
452
453	ret = of_apr_add_pd_lookups(dev);
454	if (ret)
455		goto handle_release;
456
457	of_register_apr_devices(dev, NULL);
458
459	return 0;
460
461handle_release:
462	pdr_handle_release(apr->pdr);
463destroy_wq:
464	destroy_workqueue(apr->rxwq);
465	return ret;
466}
467
468static void apr_remove(struct rpmsg_device *rpdev)
469{
470	struct apr *apr = dev_get_drvdata(&rpdev->dev);
471
472	pdr_handle_release(apr->pdr);
473	device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
474	flush_workqueue(apr->rxwq);
475	destroy_workqueue(apr->rxwq);
476}
477
478/*
479 * __apr_driver_register() - Client driver registration with aprbus
480 *
481 * @drv:Client driver to be associated with client-device.
482 * @owner: owning module/driver
483 *
484 * This API will register the client driver with the aprbus
485 * It is called from the driver's module-init function.
486 */
487int __apr_driver_register(struct apr_driver *drv, struct module *owner)
488{
489	drv->driver.bus = &aprbus;
490	drv->driver.owner = owner;
491
492	return driver_register(&drv->driver);
493}
494EXPORT_SYMBOL_GPL(__apr_driver_register);
495
496/*
497 * apr_driver_unregister() - Undo effect of apr_driver_register
498 *
499 * @drv: Client driver to be unregistered
500 */
501void apr_driver_unregister(struct apr_driver *drv)
502{
503	driver_unregister(&drv->driver);
504}
505EXPORT_SYMBOL_GPL(apr_driver_unregister);
506
507static const struct of_device_id apr_of_match[] = {
508	{ .compatible = "qcom,apr"},
509	{ .compatible = "qcom,apr-v2"},
510	{}
511};
512MODULE_DEVICE_TABLE(of, apr_of_match);
513
514static struct rpmsg_driver apr_driver = {
515	.probe = apr_probe,
516	.remove = apr_remove,
517	.callback = apr_callback,
518	.drv = {
519		.name = "qcom,apr",
520		.of_match_table = apr_of_match,
521	},
522};
523
524static int __init apr_init(void)
525{
526	int ret;
527
528	ret = bus_register(&aprbus);
529	if (!ret)
530		ret = register_rpmsg_driver(&apr_driver);
531	else
532		bus_unregister(&aprbus);
533
534	return ret;
535}
536
537static void __exit apr_exit(void)
538{
539	bus_unregister(&aprbus);
540	unregister_rpmsg_driver(&apr_driver);
541}
542
543subsys_initcall(apr_init);
544module_exit(apr_exit);
545
546MODULE_LICENSE("GPL v2");
547MODULE_DESCRIPTION("Qualcomm APR Bus");