smd.c 2.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (c) 2015, Sony Mobile Communications Inc.
  4. * Copyright (c) 2013, The Linux Foundation. All rights reserved.
  5. */
  6. #include <linux/module.h>
  7. #include <linux/skbuff.h>
  8. #include <linux/rpmsg.h>
  9. #include "qrtr.h"
  10. struct qrtr_smd_dev {
  11. struct qrtr_endpoint ep;
  12. struct rpmsg_endpoint *channel;
  13. struct device *dev;
  14. };
  15. /* from smd to qrtr */
  16. static int qcom_smd_qrtr_callback(struct rpmsg_device *rpdev,
  17. void *data, int len, void *priv, u32 addr)
  18. {
  19. struct qrtr_smd_dev *qdev = dev_get_drvdata(&rpdev->dev);
  20. int rc;
  21. if (!qdev)
  22. return -EAGAIN;
  23. rc = qrtr_endpoint_post(&qdev->ep, data, len);
  24. if (rc == -EINVAL) {
  25. dev_err(qdev->dev, "invalid ipcrouter packet\n");
  26. /* return 0 to let smd drop the packet */
  27. rc = 0;
  28. }
  29. return rc;
  30. }
  31. /* from qrtr to smd */
  32. static int qcom_smd_qrtr_send(struct qrtr_endpoint *ep, struct sk_buff *skb)
  33. {
  34. struct qrtr_smd_dev *qdev = container_of(ep, struct qrtr_smd_dev, ep);
  35. int rc;
  36. rc = skb_linearize(skb);
  37. if (rc)
  38. goto out;
  39. rc = rpmsg_send(qdev->channel, skb->data, skb->len);
  40. out:
  41. if (rc)
  42. kfree_skb(skb);
  43. else
  44. consume_skb(skb);
  45. return rc;
  46. }
  47. static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev)
  48. {
  49. struct qrtr_smd_dev *qdev;
  50. int rc;
  51. qdev = devm_kzalloc(&rpdev->dev, sizeof(*qdev), GFP_KERNEL);
  52. if (!qdev)
  53. return -ENOMEM;
  54. qdev->channel = rpdev->ept;
  55. qdev->dev = &rpdev->dev;
  56. qdev->ep.xmit = qcom_smd_qrtr_send;
  57. rc = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NID_AUTO);
  58. if (rc)
  59. return rc;
  60. dev_set_drvdata(&rpdev->dev, qdev);
  61. dev_dbg(&rpdev->dev, "Qualcomm SMD QRTR driver probed\n");
  62. return 0;
  63. }
  64. static void qcom_smd_qrtr_remove(struct rpmsg_device *rpdev)
  65. {
  66. struct qrtr_smd_dev *qdev = dev_get_drvdata(&rpdev->dev);
  67. qrtr_endpoint_unregister(&qdev->ep);
  68. dev_set_drvdata(&rpdev->dev, NULL);
  69. }
  70. static const struct rpmsg_device_id qcom_smd_qrtr_smd_match[] = {
  71. { "IPCRTR" },
  72. {}
  73. };
  74. static struct rpmsg_driver qcom_smd_qrtr_driver = {
  75. .probe = qcom_smd_qrtr_probe,
  76. .remove = qcom_smd_qrtr_remove,
  77. .callback = qcom_smd_qrtr_callback,
  78. .id_table = qcom_smd_qrtr_smd_match,
  79. .drv = {
  80. .name = "qcom_smd_qrtr",
  81. },
  82. };
  83. module_rpmsg_driver(qcom_smd_qrtr_driver);
  84. MODULE_ALIAS("rpmsg:IPCRTR");
  85. MODULE_DESCRIPTION("Qualcomm IPC-Router SMD interface driver");
  86. MODULE_LICENSE("GPL v2");