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 struct 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
31 struct 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 */
apr_send_pkt(struct apr_device * adev,struct apr_pkt * pkt)45 int 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 }
65 EXPORT_SYMBOL_GPL(apr_send_pkt);
66
apr_dev_release(struct device * dev)67 static void apr_dev_release(struct device *dev)
68 {
69 struct apr_device *adev = to_apr_device(dev);
70
71 kfree(adev);
72 }
73
apr_callback(struct rpmsg_device * rpdev,void * buf,int len,void * priv,u32 addr)74 static 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
apr_do_rx_callback(struct apr * apr,struct apr_rx_buf * abuf)104 static 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
apr_rxwq(struct work_struct * work)172 static 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
apr_device_match(struct device * dev,struct device_driver * drv)189 static 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
apr_device_probe(struct device * dev)212 static 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
apr_device_remove(struct device * dev)220 static void 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
apr_uevent(struct device * dev,struct kobj_uevent_env * env)236 static 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
248 struct 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 };
255 EXPORT_SYMBOL_GPL(aprbus);
256
apr_add_device(struct device * dev,struct device_node * np,const struct apr_device_id * id)257 static 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 of_property_read_string_index(np, "qcom,protection-domain",
293 1, &adev->service_path);
294
295 dev_info(dev, "Adding APR dev: %s\n", dev_name(&adev->dev));
296
297 ret = device_register(&adev->dev);
298 if (ret) {
299 dev_err(dev, "device_register failed: %d\n", ret);
300 put_device(&adev->dev);
301 }
302
303 return ret;
304 }
305
of_apr_add_pd_lookups(struct device * dev)306 static int of_apr_add_pd_lookups(struct device *dev)
307 {
308 const char *service_name, *service_path;
309 struct apr *apr = dev_get_drvdata(dev);
310 struct device_node *node;
311 struct pdr_service *pds;
312 int ret;
313
314 for_each_child_of_node(dev->of_node, node) {
315 ret = of_property_read_string_index(node, "qcom,protection-domain",
316 0, &service_name);
317 if (ret < 0)
318 continue;
319
320 ret = of_property_read_string_index(node, "qcom,protection-domain",
321 1, &service_path);
322 if (ret < 0) {
323 dev_err(dev, "pdr service path missing: %d\n", ret);
324 return ret;
325 }
326
327 pds = pdr_add_lookup(apr->pdr, service_name, service_path);
328 if (IS_ERR(pds) && PTR_ERR(pds) != -EALREADY) {
329 dev_err(dev, "pdr add lookup failed: %ld\n", PTR_ERR(pds));
330 return PTR_ERR(pds);
331 }
332 }
333
334 return 0;
335 }
336
of_register_apr_devices(struct device * dev,const char * svc_path)337 static void of_register_apr_devices(struct device *dev, const char *svc_path)
338 {
339 struct apr *apr = dev_get_drvdata(dev);
340 struct device_node *node;
341 const char *service_path;
342 int ret;
343
344 for_each_child_of_node(dev->of_node, node) {
345 struct apr_device_id id = { {0} };
346
347 /*
348 * This function is called with svc_path NULL during
349 * apr_probe(), in which case we register any apr devices
350 * without a qcom,protection-domain specified.
351 *
352 * Then as the protection domains becomes available
353 * (if applicable) this function is again called, but with
354 * svc_path representing the service becoming available. In
355 * this case we register any apr devices with a matching
356 * qcom,protection-domain.
357 */
358
359 ret = of_property_read_string_index(node, "qcom,protection-domain",
360 1, &service_path);
361 if (svc_path) {
362 /* skip APR services that are PD independent */
363 if (ret)
364 continue;
365
366 /* skip APR services whose PD paths don't match */
367 if (strcmp(service_path, svc_path))
368 continue;
369 } else {
370 /* skip APR services whose PD lookups are registered */
371 if (ret == 0)
372 continue;
373 }
374
375 if (of_property_read_u32(node, "reg", &id.svc_id))
376 continue;
377
378 id.domain_id = apr->dest_domain_id;
379
380 if (apr_add_device(dev, node, &id))
381 dev_err(dev, "Failed to add apr %d svc\n", id.svc_id);
382 }
383 }
384
apr_remove_device(struct device * dev,void * svc_path)385 static int apr_remove_device(struct device *dev, void *svc_path)
386 {
387 struct apr_device *adev = to_apr_device(dev);
388
389 if (svc_path && adev->service_path) {
390 if (!strcmp(adev->service_path, (char *)svc_path))
391 device_unregister(&adev->dev);
392 } else {
393 device_unregister(&adev->dev);
394 }
395
396 return 0;
397 }
398
apr_pd_status(int state,char * svc_path,void * priv)399 static void apr_pd_status(int state, char *svc_path, void *priv)
400 {
401 struct apr *apr = (struct apr *)priv;
402
403 switch (state) {
404 case SERVREG_SERVICE_STATE_UP:
405 of_register_apr_devices(apr->dev, svc_path);
406 break;
407 case SERVREG_SERVICE_STATE_DOWN:
408 device_for_each_child(apr->dev, svc_path, apr_remove_device);
409 break;
410 }
411 }
412
apr_probe(struct rpmsg_device * rpdev)413 static int apr_probe(struct rpmsg_device *rpdev)
414 {
415 struct device *dev = &rpdev->dev;
416 struct apr *apr;
417 int ret;
418
419 apr = devm_kzalloc(dev, sizeof(*apr), GFP_KERNEL);
420 if (!apr)
421 return -ENOMEM;
422
423 ret = of_property_read_u32(dev->of_node, "qcom,apr-domain", &apr->dest_domain_id);
424 if (ret) {
425 dev_err(dev, "APR Domain ID not specified in DT\n");
426 return ret;
427 }
428
429 dev_set_drvdata(dev, apr);
430 apr->ch = rpdev->ept;
431 apr->dev = dev;
432 apr->rxwq = create_singlethread_workqueue("qcom_apr_rx");
433 if (!apr->rxwq) {
434 dev_err(apr->dev, "Failed to start Rx WQ\n");
435 return -ENOMEM;
436 }
437 INIT_WORK(&apr->rx_work, apr_rxwq);
438
439 apr->pdr = pdr_handle_alloc(apr_pd_status, apr);
440 if (IS_ERR(apr->pdr)) {
441 dev_err(dev, "Failed to init PDR handle\n");
442 ret = PTR_ERR(apr->pdr);
443 goto destroy_wq;
444 }
445
446 INIT_LIST_HEAD(&apr->rx_list);
447 spin_lock_init(&apr->rx_lock);
448 spin_lock_init(&apr->svcs_lock);
449 idr_init(&apr->svcs_idr);
450
451 ret = of_apr_add_pd_lookups(dev);
452 if (ret)
453 goto handle_release;
454
455 of_register_apr_devices(dev, NULL);
456
457 return 0;
458
459 handle_release:
460 pdr_handle_release(apr->pdr);
461 destroy_wq:
462 destroy_workqueue(apr->rxwq);
463 return ret;
464 }
465
apr_remove(struct rpmsg_device * rpdev)466 static void apr_remove(struct rpmsg_device *rpdev)
467 {
468 struct apr *apr = dev_get_drvdata(&rpdev->dev);
469
470 pdr_handle_release(apr->pdr);
471 device_for_each_child(&rpdev->dev, NULL, apr_remove_device);
472 flush_workqueue(apr->rxwq);
473 destroy_workqueue(apr->rxwq);
474 }
475
476 /*
477 * __apr_driver_register() - Client driver registration with aprbus
478 *
479 * @drv:Client driver to be associated with client-device.
480 * @owner: owning module/driver
481 *
482 * This API will register the client driver with the aprbus
483 * It is called from the driver's module-init function.
484 */
__apr_driver_register(struct apr_driver * drv,struct module * owner)485 int __apr_driver_register(struct apr_driver *drv, struct module *owner)
486 {
487 drv->driver.bus = &aprbus;
488 drv->driver.owner = owner;
489
490 return driver_register(&drv->driver);
491 }
492 EXPORT_SYMBOL_GPL(__apr_driver_register);
493
494 /*
495 * apr_driver_unregister() - Undo effect of apr_driver_register
496 *
497 * @drv: Client driver to be unregistered
498 */
apr_driver_unregister(struct apr_driver * drv)499 void apr_driver_unregister(struct apr_driver *drv)
500 {
501 driver_unregister(&drv->driver);
502 }
503 EXPORT_SYMBOL_GPL(apr_driver_unregister);
504
505 static const struct of_device_id apr_of_match[] = {
506 { .compatible = "qcom,apr"},
507 { .compatible = "qcom,apr-v2"},
508 {}
509 };
510 MODULE_DEVICE_TABLE(of, apr_of_match);
511
512 static struct rpmsg_driver apr_driver = {
513 .probe = apr_probe,
514 .remove = apr_remove,
515 .callback = apr_callback,
516 .drv = {
517 .name = "qcom,apr",
518 .of_match_table = apr_of_match,
519 },
520 };
521
apr_init(void)522 static int __init apr_init(void)
523 {
524 int ret;
525
526 ret = bus_register(&aprbus);
527 if (!ret)
528 ret = register_rpmsg_driver(&apr_driver);
529 else
530 bus_unregister(&aprbus);
531
532 return ret;
533 }
534
apr_exit(void)535 static void __exit apr_exit(void)
536 {
537 bus_unregister(&aprbus);
538 unregister_rpmsg_driver(&apr_driver);
539 }
540
541 subsys_initcall(apr_init);
542 module_exit(apr_exit);
543
544 MODULE_LICENSE("GPL v2");
545 MODULE_DESCRIPTION("Qualcomm APR Bus");
546
