mtk_rpmsg.c 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408
  1. // SPDX-License-Identifier: GPL-2.0
  2. //
  3. // Copyright 2019 Google LLC.
  4. #include <linux/kernel.h>
  5. #include <linux/module.h>
  6. #include <linux/of.h>
  7. #include <linux/platform_device.h>
  8. #include <linux/remoteproc.h>
  9. #include <linux/rpmsg/mtk_rpmsg.h>
  10. #include <linux/slab.h>
  11. #include <linux/workqueue.h>
  12. #include "rpmsg_internal.h"
  13. struct mtk_rpmsg_rproc_subdev {
  14. struct platform_device *pdev;
  15. struct mtk_rpmsg_info *info;
  16. struct rpmsg_endpoint *ns_ept;
  17. struct rproc_subdev subdev;
  18. struct work_struct register_work;
  19. struct list_head channels;
  20. struct mutex channels_lock;
  21. };
  22. #define to_mtk_subdev(d) container_of(d, struct mtk_rpmsg_rproc_subdev, subdev)
  23. struct mtk_rpmsg_channel_info {
  24. struct rpmsg_channel_info info;
  25. bool registered;
  26. struct list_head list;
  27. };
  28. /**
  29. * struct rpmsg_ns_msg - dynamic name service announcement message
  30. * @name: name of remote service that is published
  31. * @addr: address of remote service that is published
  32. *
  33. * This message is sent across to publish a new service. When we receive these
  34. * messages, an appropriate rpmsg channel (i.e device) is created. In turn, the
  35. * ->probe() handler of the appropriate rpmsg driver will be invoked
  36. * (if/as-soon-as one is registered).
  37. */
  38. struct rpmsg_ns_msg {
  39. char name[RPMSG_NAME_SIZE];
  40. u32 addr;
  41. } __packed;
  42. struct mtk_rpmsg_device {
  43. struct rpmsg_device rpdev;
  44. struct mtk_rpmsg_rproc_subdev *mtk_subdev;
  45. };
  46. struct mtk_rpmsg_endpoint {
  47. struct rpmsg_endpoint ept;
  48. struct mtk_rpmsg_rproc_subdev *mtk_subdev;
  49. };
  50. #define to_mtk_rpmsg_device(r) container_of(r, struct mtk_rpmsg_device, rpdev)
  51. #define to_mtk_rpmsg_endpoint(r) container_of(r, struct mtk_rpmsg_endpoint, ept)
  52. static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops;
  53. static void __mtk_ept_release(struct kref *kref)
  54. {
  55. struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint,
  56. refcount);
  57. kfree(to_mtk_rpmsg_endpoint(ept));
  58. }
  59. static void mtk_rpmsg_ipi_handler(void *data, unsigned int len, void *priv)
  60. {
  61. struct mtk_rpmsg_endpoint *mept = priv;
  62. struct rpmsg_endpoint *ept = &mept->ept;
  63. int ret;
  64. ret = (*ept->cb)(ept->rpdev, data, len, ept->priv, ept->addr);
  65. if (ret)
  66. dev_warn(&ept->rpdev->dev, "rpmsg handler return error = %d",
  67. ret);
  68. }
  69. static struct rpmsg_endpoint *
  70. __mtk_create_ept(struct mtk_rpmsg_rproc_subdev *mtk_subdev,
  71. struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv,
  72. u32 id)
  73. {
  74. struct mtk_rpmsg_endpoint *mept;
  75. struct rpmsg_endpoint *ept;
  76. struct platform_device *pdev = mtk_subdev->pdev;
  77. int ret;
  78. mept = kzalloc(sizeof(*mept), GFP_KERNEL);
  79. if (!mept)
  80. return NULL;
  81. mept->mtk_subdev = mtk_subdev;
  82. ept = &mept->ept;
  83. kref_init(&ept->refcount);
  84. ept->rpdev = rpdev;
  85. ept->cb = cb;
  86. ept->priv = priv;
  87. ept->ops = &mtk_rpmsg_endpoint_ops;
  88. ept->addr = id;
  89. ret = mtk_subdev->info->register_ipi(pdev, id, mtk_rpmsg_ipi_handler,
  90. mept);
  91. if (ret) {
  92. dev_err(&pdev->dev, "IPI register failed, id = %d", id);
  93. kref_put(&ept->refcount, __mtk_ept_release);
  94. return NULL;
  95. }
  96. return ept;
  97. }
  98. static struct rpmsg_endpoint *
  99. mtk_rpmsg_create_ept(struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv,
  100. struct rpmsg_channel_info chinfo)
  101. {
  102. struct mtk_rpmsg_rproc_subdev *mtk_subdev =
  103. to_mtk_rpmsg_device(rpdev)->mtk_subdev;
  104. return __mtk_create_ept(mtk_subdev, rpdev, cb, priv, chinfo.src);
  105. }
  106. static void mtk_rpmsg_destroy_ept(struct rpmsg_endpoint *ept)
  107. {
  108. struct mtk_rpmsg_rproc_subdev *mtk_subdev =
  109. to_mtk_rpmsg_endpoint(ept)->mtk_subdev;
  110. mtk_subdev->info->unregister_ipi(mtk_subdev->pdev, ept->addr);
  111. kref_put(&ept->refcount, __mtk_ept_release);
  112. }
  113. static int mtk_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len)
  114. {
  115. struct mtk_rpmsg_rproc_subdev *mtk_subdev =
  116. to_mtk_rpmsg_endpoint(ept)->mtk_subdev;
  117. return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data,
  118. len, 0);
  119. }
  120. static int mtk_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len)
  121. {
  122. struct mtk_rpmsg_rproc_subdev *mtk_subdev =
  123. to_mtk_rpmsg_endpoint(ept)->mtk_subdev;
  124. /*
  125. * TODO: This currently is same as mtk_rpmsg_send, and wait until SCP
  126. * received the last command.
  127. */
  128. return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data,
  129. len, 0);
  130. }
  131. static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops = {
  132. .destroy_ept = mtk_rpmsg_destroy_ept,
  133. .send = mtk_rpmsg_send,
  134. .trysend = mtk_rpmsg_trysend,
  135. };
  136. static void mtk_rpmsg_release_device(struct device *dev)
  137. {
  138. struct rpmsg_device *rpdev = to_rpmsg_device(dev);
  139. struct mtk_rpmsg_device *mdev = to_mtk_rpmsg_device(rpdev);
  140. kfree(mdev);
  141. }
  142. static const struct rpmsg_device_ops mtk_rpmsg_device_ops = {
  143. .create_ept = mtk_rpmsg_create_ept,
  144. };
  145. static struct device_node *
  146. mtk_rpmsg_match_device_subnode(struct device_node *node, const char *channel)
  147. {
  148. struct device_node *child;
  149. const char *name;
  150. int ret;
  151. for_each_available_child_of_node(node, child) {
  152. ret = of_property_read_string(child, "mtk,rpmsg-name", &name);
  153. if (ret)
  154. continue;
  155. if (strcmp(name, channel) == 0)
  156. return child;
  157. }
  158. return NULL;
  159. }
  160. static int mtk_rpmsg_register_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev,
  161. struct rpmsg_channel_info *info)
  162. {
  163. struct rpmsg_device *rpdev;
  164. struct mtk_rpmsg_device *mdev;
  165. struct platform_device *pdev = mtk_subdev->pdev;
  166. mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
  167. if (!mdev)
  168. return -ENOMEM;
  169. mdev->mtk_subdev = mtk_subdev;
  170. rpdev = &mdev->rpdev;
  171. rpdev->ops = &mtk_rpmsg_device_ops;
  172. rpdev->src = info->src;
  173. rpdev->dst = info->dst;
  174. strscpy(rpdev->id.name, info->name, RPMSG_NAME_SIZE);
  175. rpdev->dev.of_node =
  176. mtk_rpmsg_match_device_subnode(pdev->dev.of_node, info->name);
  177. rpdev->dev.parent = &pdev->dev;
  178. rpdev->dev.release = mtk_rpmsg_release_device;
  179. return rpmsg_register_device(rpdev);
  180. }
  181. static void mtk_register_device_work_function(struct work_struct *register_work)
  182. {
  183. struct mtk_rpmsg_rproc_subdev *subdev = container_of(
  184. register_work, struct mtk_rpmsg_rproc_subdev, register_work);
  185. struct platform_device *pdev = subdev->pdev;
  186. struct mtk_rpmsg_channel_info *info;
  187. int ret;
  188. mutex_lock(&subdev->channels_lock);
  189. list_for_each_entry(info, &subdev->channels, list) {
  190. if (info->registered)
  191. continue;
  192. ret = mtk_rpmsg_register_device(subdev, &info->info);
  193. if (ret) {
  194. dev_err(&pdev->dev, "Can't create rpmsg_device\n");
  195. continue;
  196. }
  197. info->registered = true;
  198. }
  199. mutex_unlock(&subdev->channels_lock);
  200. }
  201. static int mtk_rpmsg_create_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev,
  202. char *name, u32 addr)
  203. {
  204. struct mtk_rpmsg_channel_info *info;
  205. info = kzalloc(sizeof(*info), GFP_KERNEL);
  206. if (!info)
  207. return -ENOMEM;
  208. strscpy(info->info.name, name, RPMSG_NAME_SIZE);
  209. info->info.src = addr;
  210. info->info.dst = RPMSG_ADDR_ANY;
  211. mutex_lock(&mtk_subdev->channels_lock);
  212. list_add(&info->list, &mtk_subdev->channels);
  213. mutex_unlock(&mtk_subdev->channels_lock);
  214. schedule_work(&mtk_subdev->register_work);
  215. return 0;
  216. }
  217. static int mtk_rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len,
  218. void *priv, u32 src)
  219. {
  220. struct rpmsg_ns_msg *msg = data;
  221. struct mtk_rpmsg_rproc_subdev *mtk_subdev = priv;
  222. struct device *dev = &mtk_subdev->pdev->dev;
  223. int ret;
  224. if (len != sizeof(*msg)) {
  225. dev_err(dev, "malformed ns msg (%d)\n", len);
  226. return -EINVAL;
  227. }
  228. /*
  229. * the name service ept does _not_ belong to a real rpmsg channel,
  230. * and is handled by the rpmsg bus itself.
  231. * for sanity reasons, make sure a valid rpdev has _not_ sneaked
  232. * in somehow.
  233. */
  234. if (rpdev) {
  235. dev_err(dev, "anomaly: ns ept has an rpdev handle\n");
  236. return -EINVAL;
  237. }
  238. /* don't trust the remote processor for null terminating the name */
  239. msg->name[RPMSG_NAME_SIZE - 1] = '\0';
  240. dev_info(dev, "creating channel %s addr 0x%x\n", msg->name, msg->addr);
  241. ret = mtk_rpmsg_create_device(mtk_subdev, msg->name, msg->addr);
  242. if (ret) {
  243. dev_err(dev, "create rpmsg device failed\n");
  244. return ret;
  245. }
  246. return 0;
  247. }
  248. static int mtk_rpmsg_prepare(struct rproc_subdev *subdev)
  249. {
  250. struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);
  251. /* a dedicated endpoint handles the name service msgs */
  252. if (mtk_subdev->info->ns_ipi_id >= 0) {
  253. mtk_subdev->ns_ept =
  254. __mtk_create_ept(mtk_subdev, NULL, mtk_rpmsg_ns_cb,
  255. mtk_subdev,
  256. mtk_subdev->info->ns_ipi_id);
  257. if (!mtk_subdev->ns_ept) {
  258. dev_err(&mtk_subdev->pdev->dev,
  259. "failed to create name service endpoint\n");
  260. return -ENOMEM;
  261. }
  262. }
  263. return 0;
  264. }
  265. static void mtk_rpmsg_unprepare(struct rproc_subdev *subdev)
  266. {
  267. struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);
  268. if (mtk_subdev->ns_ept) {
  269. mtk_rpmsg_destroy_ept(mtk_subdev->ns_ept);
  270. mtk_subdev->ns_ept = NULL;
  271. }
  272. }
  273. static void mtk_rpmsg_stop(struct rproc_subdev *subdev, bool crashed)
  274. {
  275. struct mtk_rpmsg_channel_info *info, *next;
  276. struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);
  277. struct device *dev = &mtk_subdev->pdev->dev;
  278. /*
  279. * Destroy the name service endpoint here, to avoid new channel being
  280. * created after the rpmsg_unregister_device loop below.
  281. */
  282. if (mtk_subdev->ns_ept) {
  283. mtk_rpmsg_destroy_ept(mtk_subdev->ns_ept);
  284. mtk_subdev->ns_ept = NULL;
  285. }
  286. cancel_work_sync(&mtk_subdev->register_work);
  287. mutex_lock(&mtk_subdev->channels_lock);
  288. list_for_each_entry(info, &mtk_subdev->channels, list) {
  289. if (!info->registered)
  290. continue;
  291. if (rpmsg_unregister_device(dev, &info->info)) {
  292. dev_warn(
  293. dev,
  294. "rpmsg_unregister_device failed for %s.%d.%d\n",
  295. info->info.name, info->info.src,
  296. info->info.dst);
  297. }
  298. }
  299. list_for_each_entry_safe(info, next,
  300. &mtk_subdev->channels, list) {
  301. list_del(&info->list);
  302. kfree(info);
  303. }
  304. mutex_unlock(&mtk_subdev->channels_lock);
  305. }
  306. struct rproc_subdev *
  307. mtk_rpmsg_create_rproc_subdev(struct platform_device *pdev,
  308. struct mtk_rpmsg_info *info)
  309. {
  310. struct mtk_rpmsg_rproc_subdev *mtk_subdev;
  311. mtk_subdev = kzalloc(sizeof(*mtk_subdev), GFP_KERNEL);
  312. if (!mtk_subdev)
  313. return NULL;
  314. mtk_subdev->pdev = pdev;
  315. mtk_subdev->subdev.prepare = mtk_rpmsg_prepare;
  316. mtk_subdev->subdev.stop = mtk_rpmsg_stop;
  317. mtk_subdev->subdev.unprepare = mtk_rpmsg_unprepare;
  318. mtk_subdev->info = info;
  319. INIT_LIST_HEAD(&mtk_subdev->channels);
  320. INIT_WORK(&mtk_subdev->register_work,
  321. mtk_register_device_work_function);
  322. mutex_init(&mtk_subdev->channels_lock);
  323. return &mtk_subdev->subdev;
  324. }
  325. EXPORT_SYMBOL_GPL(mtk_rpmsg_create_rproc_subdev);
  326. void mtk_rpmsg_destroy_rproc_subdev(struct rproc_subdev *subdev)
  327. {
  328. struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);
  329. kfree(mtk_subdev);
  330. }
  331. EXPORT_SYMBOL_GPL(mtk_rpmsg_destroy_rproc_subdev);
  332. MODULE_LICENSE("GPL v2");
  333. MODULE_DESCRIPTION("MediaTek scp rpmsg driver");