apr.c 13 KB

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