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