qmsgq_gunyah.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (c) 2023 Qualcomm Innovation Center, Inc. All rights reserved.
  4. */
  5. #include <linux/gunyah/gh_rm_drv.h>
  6. #include <linux/gunyah/gh_msgq.h>
  7. #include <linux/kthread.h>
  8. #include <linux/list.h>
  9. #include <linux/platform_device.h>
  10. #include <linux/pm_wakeup.h>
  11. #include <linux/skbuff.h>
  12. #include <linux/sizes.h>
  13. #include <linux/types.h>
  14. #include <linux/of.h>
  15. #include "af_qmsgq.h"
  16. #define QMSGQ_GH_PROTO_VER_1 1
  17. #define MAX_PKT_SZ SZ_64K
  18. #define QMSGQ_SKB_WAKEUP_MS 500
  19. enum qmsgq_gh_pkt_type {
  20. QMSGQ_GH_TYPE_DATA = 1,
  21. };
  22. /**
  23. * struct qmsgq_gh_hdr - qmsgq gunyah packet header
  24. * @version: protocol version
  25. * @type: packet type; one of qmsgq_gh_pkt_type
  26. * @flags: Reserved for future use
  27. * @optlen: length of optional header data
  28. * @size: length of packet, excluding this header and optlen
  29. * @src_node_id: source cid, reserved
  30. * @src_port_id: source port
  31. * @dst_node_id: destination cid, reserved
  32. * @dst_port_id: destination port
  33. */
  34. struct qmsgq_gh_hdr {
  35. u8 version;
  36. u8 type;
  37. u8 flags;
  38. u8 optlen;
  39. __le32 size;
  40. __le32 src_rsvd;
  41. __le32 src_port_id;
  42. __le32 dst_rsvd;
  43. __le32 dst_port_id;
  44. };
  45. /* gh_transport_buf: gunyah transport buffer
  46. * @lock: lock for the buffer
  47. * @len: hdrlen + packet size
  48. * @copied: size of buffer copied
  49. * @hdr_received: true if the header is already saved, else false
  50. * @buf: buffer saved
  51. */
  52. struct qmsgq_gh_recv_buf {
  53. /* @lock: lock for the buffer */
  54. struct mutex lock;
  55. size_t len;
  56. size_t copied;
  57. bool hdr_received;
  58. char buf[MAX_PKT_SZ];
  59. };
  60. /* qmsgq_gh_device: vm devices attached to this transport
  61. * @item: list item of all vm devices
  62. * @dev: device from platform_device.
  63. * @peer_cid: remote cid
  64. * @master: primary vm indicator
  65. * @msgq_label: msgq label
  66. * @msgq_hdl: msgq handle
  67. * @rm_nb: notifier block for vm status from rm
  68. * @tx_lock: tx lock to queue only one packet at a time
  69. * @rx_thread: rx thread to receive incoming packets
  70. * @ep: qmsq endpoint
  71. * @sock_ws: wakeup source
  72. */
  73. struct qmsgq_gh_device {
  74. struct list_head item;
  75. struct device *dev;
  76. struct qmsgq_endpoint ep;
  77. unsigned int peer_cid;
  78. bool master;
  79. enum gh_msgq_label msgq_label;
  80. void *msgq_hdl;
  81. struct notifier_block rm_nb;
  82. struct wakeup_source *sock_ws;
  83. /* @tx_lock: tx lock to queue only one packet at a time */
  84. struct mutex tx_lock;
  85. struct task_struct *rx_thread;
  86. struct qmsgq_gh_recv_buf rx_buf;
  87. };
  88. static void reset_buf(struct qmsgq_gh_recv_buf *rx_buf)
  89. {
  90. memset(rx_buf->buf, 0, MAX_PKT_SZ);
  91. rx_buf->hdr_received = false;
  92. rx_buf->copied = 0;
  93. rx_buf->len = 0;
  94. }
  95. static int qmsgq_gh_post(struct qmsgq_gh_device *qdev, struct qmsgq_gh_recv_buf *rx_buf)
  96. {
  97. unsigned int cid, port, len;
  98. struct qmsgq_gh_hdr *hdr;
  99. struct sockaddr_vm src;
  100. struct sockaddr_vm dst;
  101. void *data;
  102. int rc;
  103. if (rx_buf->len < sizeof(*hdr)) {
  104. pr_err("%s: len: %d < hdr size\n", __func__, rx_buf->len);
  105. return -EINVAL;
  106. }
  107. hdr = (struct qmsgq_gh_hdr *)rx_buf->buf;
  108. if (hdr->type != QMSGQ_GH_TYPE_DATA)
  109. return -EINVAL;
  110. cid = le32_to_cpu(hdr->src_rsvd);
  111. port = le32_to_cpu(hdr->src_port_id);
  112. vsock_addr_init(&src, cid, port);
  113. cid = le32_to_cpu(hdr->dst_rsvd);
  114. port = le32_to_cpu(hdr->dst_port_id);
  115. vsock_addr_init(&dst, cid, port);
  116. data = rx_buf->buf + sizeof(*hdr);
  117. len = rx_buf->len - sizeof(*hdr);
  118. rc = qmsgq_post(&qdev->ep, &src, &dst, data, len);
  119. return rc;
  120. }
  121. static void qmsgq_process_recv(struct qmsgq_gh_device *qdev, void *buf, size_t len)
  122. {
  123. struct qmsgq_gh_recv_buf *rx_buf = &qdev->rx_buf;
  124. struct qmsgq_gh_hdr *hdr;
  125. size_t n;
  126. mutex_lock(&rx_buf->lock);
  127. /* Copy message into the local buffer */
  128. n = (rx_buf->copied + len < MAX_PKT_SZ) ? len : MAX_PKT_SZ - rx_buf->copied;
  129. memcpy(rx_buf->buf + rx_buf->copied, buf, n);
  130. rx_buf->copied += n;
  131. if (!rx_buf->hdr_received) {
  132. hdr = (struct qmsgq_gh_hdr *)rx_buf->buf;
  133. if (hdr->version != QMSGQ_GH_PROTO_VER_1) {
  134. pr_err("%s: Incorrect version:%d\n", __func__, hdr->version);
  135. goto err;
  136. }
  137. if (hdr->type != QMSGQ_GH_TYPE_DATA) {
  138. pr_err("%s: Incorrect type:%d\n", __func__, hdr->type);
  139. goto err;
  140. }
  141. if (hdr->size > MAX_PKT_SZ - sizeof(*hdr)) {
  142. pr_err("%s: Packet size too big:%d\n", __func__, hdr->size);
  143. goto err;
  144. }
  145. rx_buf->len = sizeof(*hdr) + hdr->size;
  146. rx_buf->hdr_received = true;
  147. }
  148. /* Check len size, can not be smaller than amount copied*/
  149. if (rx_buf->len < rx_buf->copied) {
  150. pr_err("%s: Size mismatch len:%d, copied:%d\n", __func__,
  151. rx_buf->len, rx_buf->copied);
  152. goto err;
  153. }
  154. if (rx_buf->len == rx_buf->copied) {
  155. qmsgq_gh_post(qdev, rx_buf);
  156. reset_buf(rx_buf);
  157. }
  158. mutex_unlock(&rx_buf->lock);
  159. return;
  160. err:
  161. reset_buf(rx_buf);
  162. mutex_unlock(&rx_buf->lock);
  163. }
  164. static int qmsgq_gh_msgq_recv(void *data)
  165. {
  166. struct qmsgq_gh_device *qdev = data;
  167. size_t size;
  168. void *buf;
  169. int rc;
  170. buf = kzalloc(GH_MSGQ_MAX_MSG_SIZE_BYTES, GFP_KERNEL);
  171. if (!buf)
  172. return -ENOMEM;
  173. while (!kthread_should_stop()) {
  174. rc = gh_msgq_recv(qdev->msgq_hdl, buf, GH_MSGQ_MAX_MSG_SIZE_BYTES, &size,
  175. GH_MSGQ_TX_PUSH);
  176. if (rc)
  177. continue;
  178. if (size <= 0)
  179. continue;
  180. qmsgq_process_recv(qdev, buf, size);
  181. pm_wakeup_ws_event(qdev->sock_ws, QMSGQ_SKB_WAKEUP_MS, true);
  182. }
  183. kfree(buf);
  184. return 0;
  185. }
  186. static int qmsgq_gh_send(struct qmsgq_gh_device *qdev, void *buf, size_t len)
  187. {
  188. size_t left, chunk, offset;
  189. int rc = 0;
  190. left = len;
  191. chunk = 0;
  192. offset = 0;
  193. mutex_lock(&qdev->tx_lock);
  194. while (left > 0) {
  195. chunk = (left > GH_MSGQ_MAX_MSG_SIZE_BYTES) ? GH_MSGQ_MAX_MSG_SIZE_BYTES : left;
  196. rc = gh_msgq_send(qdev->msgq_hdl, buf + offset, chunk, GH_MSGQ_TX_PUSH);
  197. if (rc) {
  198. if (rc == -ERESTARTSYS) {
  199. continue;
  200. } else {
  201. pr_err("%s: gh_msgq_send failed: %d\n", __func__, rc);
  202. mutex_unlock(&qdev->tx_lock);
  203. goto err;
  204. }
  205. }
  206. left -= chunk;
  207. offset += chunk;
  208. }
  209. mutex_unlock(&qdev->tx_lock);
  210. return 0;
  211. err:
  212. return rc;
  213. }
  214. static int qmsgq_gh_dgram_enqueue(struct qmsgq_sock *qsk, struct sockaddr_vm *remote,
  215. struct msghdr *msg, size_t len)
  216. {
  217. struct sockaddr_vm *local_addr = &qsk->local_addr;
  218. const struct qmsgq_endpoint *ep;
  219. struct qmsgq_gh_device *qdev;
  220. struct qmsgq_gh_hdr *hdr;
  221. char *buf;
  222. int rc;
  223. ep = qsk->ep;
  224. if (!ep)
  225. return -ENXIO;
  226. qdev = container_of(ep, struct qmsgq_gh_device, ep);
  227. if (!qdev->msgq_hdl) {
  228. pr_err("%s: Transport not ready\n", __func__);
  229. return -ENODEV;
  230. }
  231. if (len > MAX_PKT_SZ - sizeof(*hdr)) {
  232. pr_err("%s: Invalid pk size: len: %lu\n", __func__, len);
  233. return -EMSGSIZE;
  234. }
  235. /* Allocate a buffer for the user's message and our packet header. */
  236. buf = kmalloc(len + sizeof(*hdr), GFP_KERNEL);
  237. if (!buf)
  238. return -ENOMEM;
  239. /* Populate Header */
  240. hdr = (struct qmsgq_gh_hdr *)buf;
  241. hdr->version = QMSGQ_GH_PROTO_VER_1;
  242. hdr->type = QMSGQ_GH_TYPE_DATA;
  243. hdr->flags = 0;
  244. hdr->optlen = 0;
  245. hdr->size = len;
  246. hdr->src_rsvd = 0;
  247. hdr->src_port_id = local_addr->svm_port;
  248. hdr->dst_rsvd = 0;
  249. hdr->dst_port_id = remote->svm_port;
  250. rc = memcpy_from_msg((void *)buf + sizeof(*hdr), msg, len);
  251. if (rc) {
  252. pr_err("%s failed: memcpy_from_msg rc: %d\n", __func__, rc);
  253. goto send_err;
  254. }
  255. pr_debug("TX DATA: Len:0x%x src[0x%x] dst[0x%x]\n", len, hdr->src_port_id,
  256. hdr->dst_port_id);
  257. rc = qmsgq_gh_send(qdev, buf, len + sizeof(*hdr));
  258. if (rc < 0) {
  259. pr_err("%s: failed to send msg rc: %d\n", __func__, rc);
  260. goto send_err;
  261. }
  262. kfree(buf);
  263. return 0;
  264. send_err:
  265. kfree(buf);
  266. return rc;
  267. }
  268. static int qmsgq_gh_socket_init(struct qmsgq_sock *qsk, struct qmsgq_sock *psk)
  269. {
  270. return 0;
  271. }
  272. static void qmsgq_gh_destruct(struct qmsgq_sock *qsk)
  273. {
  274. }
  275. static void qmsgq_gh_release(struct qmsgq_sock *qsk)
  276. {
  277. }
  278. static bool qmsgq_gh_allow_rsvd_cid(u32 cid)
  279. {
  280. /* Allowing for cid 0 as of now as af_qmsgq sends 0 if no cid is
  281. * passed by the client.
  282. */
  283. if (cid == 0)
  284. return true;
  285. return false;
  286. }
  287. static bool qmsgq_gh_dgram_allow(u32 cid, u32 port)
  288. {
  289. if (qmsgq_gh_allow_rsvd_cid(cid) || cid == VMADDR_CID_ANY || cid == VMADDR_CID_HOST)
  290. return true;
  291. pr_err("%s: dgram not allowed for cid 0x%x\n", __func__, cid);
  292. return false;
  293. }
  294. static int qmsgq_gh_shutdown(struct qmsgq_sock *qsk, int mode)
  295. {
  296. return 0;
  297. }
  298. static u32 qmsgq_gh_get_local_cid(void)
  299. {
  300. return VMADDR_CID_HOST;
  301. }
  302. static int qmsgq_gh_msgq_start(struct qmsgq_gh_device *qdev)
  303. {
  304. struct device *dev = qdev->dev;
  305. int rc;
  306. if (qdev->msgq_hdl) {
  307. dev_err(qdev->dev, "Already have msgq handle!\n");
  308. return NOTIFY_DONE;
  309. }
  310. qdev->msgq_hdl = gh_msgq_register(qdev->msgq_label);
  311. if (IS_ERR_OR_NULL(qdev->msgq_hdl)) {
  312. rc = PTR_ERR(qdev->msgq_hdl);
  313. dev_err(dev, "msgq register failed rc:%d\n", rc);
  314. return rc;
  315. }
  316. qdev->rx_thread = kthread_run(qmsgq_gh_msgq_recv, qdev, "qmsgq_gh_rx");
  317. if (IS_ERR_OR_NULL(qdev->rx_thread)) {
  318. rc = PTR_ERR(qdev->rx_thread);
  319. dev_err(dev, "Failed to create rx thread rc:%d\n", rc);
  320. return rc;
  321. }
  322. return 0;
  323. }
  324. static int qmsgq_gh_rm_cb(struct notifier_block *nb, unsigned long cmd, void *data)
  325. {
  326. struct qmsgq_gh_device *qdev = container_of(nb, struct qmsgq_gh_device, rm_nb);
  327. struct gh_rm_notif_vm_status_payload *vm_status_payload = data;
  328. u8 vm_status = vm_status_payload->vm_status;
  329. int rc;
  330. if (cmd != GH_RM_NOTIF_VM_STATUS)
  331. return NOTIFY_DONE;
  332. /* TODO - check for peer */
  333. switch (vm_status) {
  334. case GH_RM_VM_STATUS_READY:
  335. rc = qmsgq_gh_msgq_start(qdev);
  336. break;
  337. default:
  338. pr_debug("Unknown notification for vmid = %d vm_status = %d\n",
  339. vm_status_payload->vmid, vm_status);
  340. }
  341. return NOTIFY_DONE;
  342. }
  343. static int qmsgq_gh_probe(struct platform_device *pdev)
  344. {
  345. struct device_node *np = pdev->dev.of_node;
  346. struct device *dev = &pdev->dev;
  347. struct qmsgq_gh_device *qdev;
  348. int rc;
  349. qdev = devm_kzalloc(dev, sizeof(*qdev), GFP_KERNEL);
  350. if (!qdev)
  351. return -ENOMEM;
  352. qdev->dev = dev;
  353. dev_set_drvdata(&pdev->dev, qdev);
  354. mutex_init(&qdev->tx_lock);
  355. mutex_init(&qdev->rx_buf.lock);
  356. qdev->rx_buf.len = 0;
  357. qdev->rx_buf.copied = 0;
  358. qdev->rx_buf.hdr_received = false;
  359. qdev->ep.module = THIS_MODULE;
  360. qdev->ep.init = qmsgq_gh_socket_init;
  361. qdev->ep.destruct = qmsgq_gh_destruct;
  362. qdev->ep.release = qmsgq_gh_release;
  363. qdev->ep.dgram_enqueue = qmsgq_gh_dgram_enqueue;
  364. qdev->ep.dgram_allow = qmsgq_gh_dgram_allow;
  365. qdev->ep.shutdown = qmsgq_gh_shutdown;
  366. qdev->ep.get_local_cid = qmsgq_gh_get_local_cid;
  367. //TODO properly set this
  368. qdev->peer_cid = 0;
  369. qdev->sock_ws = wakeup_source_register(NULL, "qmsgq_sock_ws");
  370. rc = of_property_read_u32(np, "msgq-label", &qdev->msgq_label);
  371. if (rc) {
  372. dev_err(dev, "failed to read msgq-label info %d\n", rc);
  373. return rc;
  374. }
  375. qdev->master = of_property_read_bool(np, "qcom,master");
  376. if (qdev->master) {
  377. qdev->rm_nb.notifier_call = qmsgq_gh_rm_cb;
  378. gh_rm_register_notifier(&qdev->rm_nb);
  379. } else {
  380. rc = qmsgq_gh_msgq_start(qdev);
  381. }
  382. qmsgq_endpoint_register(&qdev->ep);
  383. return rc;
  384. }
  385. static int qmsgq_gh_remove(struct platform_device *pdev)
  386. {
  387. struct qmsgq_gh_device *qdev = dev_get_drvdata(&pdev->dev);
  388. if (qdev->master)
  389. gh_rm_unregister_notifier(&qdev->rm_nb);
  390. if (qdev->rx_thread)
  391. kthread_stop(qdev->rx_thread);
  392. qmsgq_endpoint_unregister(&qdev->ep);
  393. return 0;
  394. }
  395. static const struct of_device_id qmsgq_gh_of_match[] = {
  396. { .compatible = "qcom,qmsgq-gh" },
  397. {}
  398. };
  399. MODULE_DEVICE_TABLE(of, qmsgq_gh_of_match);
  400. static struct platform_driver qmsgq_gh_driver = {
  401. .probe = qmsgq_gh_probe,
  402. .remove = qmsgq_gh_remove,
  403. .driver = {
  404. .name = "qmsgq-gh",
  405. .of_match_table = qmsgq_gh_of_match,
  406. }
  407. };
  408. module_platform_driver(qmsgq_gh_driver);
  409. MODULE_ALIAS("gunyah:QMSGQ");
  410. MODULE_DESCRIPTION("Gunyah QMSGQ Transport driver");
  411. MODULE_LICENSE("GPL");