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