virtio-mailbox.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572
  1. // SPDX-License-Identifier: GPL-2.0+
  2. #include <linux/device.h>
  3. #include <linux/interrupt.h>
  4. #include <linux/kernel.h>
  5. #include <linux/mailbox_controller.h>
  6. #include <linux/module.h>
  7. #include <linux/of.h>
  8. #include <linux/platform_device.h>
  9. #include <linux/virtio.h>
  10. #include <linux/virtio_mailbox.h>
  11. #include <linux/virtio_config.h>
  12. #include <linux/virtio_light.h>
  13. #include <linux/virtio_ids.h>
  14. #define DRIVER_NAME "virtio-mailbox"
  15. static inline u32 virtio_cread32_nosleep(struct virtio_device *vdev,
  16. unsigned int offset)
  17. {
  18. __virtio32 ret;
  19. vdev->config->get(vdev, offset, &ret, sizeof(ret));
  20. return virtio32_to_cpu(vdev, ret);
  21. }
  22. static inline void virtio_cwrite32_nosleep(struct virtio_device *vdev,
  23. unsigned int offset, u32 val)
  24. {
  25. __virtio32 v;
  26. v = cpu_to_virtio32(vdev, val);
  27. vdev->config->set(vdev, offset, &v, sizeof(v));
  28. }
  29. struct virtio_mbox {
  30. struct mbox_controller controller;
  31. struct virtio_device *vdev;
  32. unsigned int chan_num;
  33. unsigned int msg_size;
  34. struct mbox_chan *chans;
  35. struct virtio_chan *vchans;
  36. struct virtqueue **in_vqs;
  37. struct virtqueue **out_vqs;
  38. };
  39. struct virtio_chan {
  40. struct virtqueue *in_vq;
  41. struct virtqueue *out_vq;
  42. struct virtio_mbox *vmbox;
  43. struct mbox_chan *chan;
  44. bool in_use;
  45. u8 idx;
  46. spinlock_t lock;
  47. };
  48. static int add_invq_buf(struct virtqueue *vq, void *buf, u32 msg_size)
  49. {
  50. struct scatterlist sg[1];
  51. int ret;
  52. sg_init_one(sg, buf, msg_size);
  53. ret = virtqueue_add_inbuf(vq, sg, 1, buf, GFP_ATOMIC);
  54. if (!ret)
  55. ret = vq->num_free;
  56. return ret;
  57. }
  58. static int fill_invq(struct virtqueue *vq, spinlock_t *lock,
  59. unsigned int msg_size)
  60. {
  61. int nr_added_bufs;
  62. int ret;
  63. void *buf;
  64. nr_added_bufs = 0;
  65. do {
  66. buf = kmalloc(msg_size, GFP_KERNEL);
  67. if (!buf)
  68. return -ENOMEM;
  69. spin_lock_irq(lock);
  70. ret = add_invq_buf(vq, buf, msg_size);
  71. if (ret < 0) {
  72. spin_unlock_irq(lock);
  73. kfree(buf);
  74. return ret;
  75. }
  76. nr_added_bufs++;
  77. spin_unlock_irq(lock);
  78. } while (ret > 0);
  79. return nr_added_bufs;
  80. }
  81. /* callback for virtio inqueue */
  82. static void virtio_mbox_invq_cb(struct virtqueue *vq)
  83. {
  84. struct virtio_mbox *vmbox = vq->vdev->priv;
  85. struct mbox_chan *chan;
  86. struct virtio_chan *vchan;
  87. unsigned int len;
  88. u32 chan_idx, msg, val = 0;
  89. unsigned long flags;
  90. void *buf;
  91. /*
  92. * As vq->index is in range {0,1,2,...vmbox->chan_num*2},
  93. * vq->index in range {0,2,4,...} is for in_vqs;
  94. * vq->index in range {1,3,5,...} is for out_vqs;
  95. * one in and out vq pair is for one chan, eg,
  96. * {0,1} for chan 0;
  97. * {2,3} for chan 1;
  98. * {4,5} for chan 2,etc
  99. */
  100. chan_idx = vq->index/2;
  101. chan = &vmbox->chans[chan_idx];
  102. vchan = chan->con_priv;
  103. msg = virtio_cread32_nosleep(vmbox->vdev, offsets[MSG_H] + chan_idx * 4);
  104. spin_lock_irqsave(&vchan->lock, flags);
  105. if (!vchan->in_use || !msg) {
  106. spin_unlock_irqrestore(&vchan->lock, flags);
  107. return;
  108. }
  109. spin_unlock_irqrestore(&vchan->lock, flags);
  110. virtio_cwrite32_nosleep(vmbox->vdev, offsets[MSG_H] + chan_idx * 4, val);
  111. while (1) {
  112. buf = virtqueue_get_buf(vq, &len);
  113. if (buf == NULL)
  114. break;
  115. /*
  116. * assume client copied vmsg to its private buf
  117. * when this func return
  118. */
  119. mbox_chan_received_data(chan, buf);
  120. add_invq_buf(vq, buf, vmbox->msg_size);
  121. /* mark Ack */
  122. val = 1;
  123. virtio_cwrite32_nosleep(vmbox->vdev, offsets[ACK_G] + chan_idx * 4, val);
  124. /* tell backend to handle Ack */
  125. virtqueue_kick(vq);
  126. }
  127. }
  128. /*
  129. * callback for virtio outqueue
  130. * we use this func to reclaim used buffer and handle ack
  131. */
  132. static void virtio_mbox_outvq_cb(struct virtqueue *vq)
  133. {
  134. struct virtio_mbox *vmbox = vq->vdev->priv;
  135. struct mbox_chan *chan;
  136. struct virtio_chan *vchan;
  137. unsigned int len;
  138. int chan_idx;
  139. void *buf;
  140. unsigned long flags;
  141. u32 ack, val = 0;
  142. /* see comments in virtio_mbox_invq_cb() */
  143. chan_idx = vq->index/2;
  144. chan = &vmbox->chans[chan_idx];
  145. vchan = chan->con_priv;
  146. ack = virtio_cread32_nosleep(vmbox->vdev, offsets[ACK_H] + chan_idx * 4);
  147. spin_lock_irqsave(&vchan->lock, flags);
  148. if (!vchan->in_use || !ack) {
  149. spin_unlock_irqrestore(&vchan->lock, flags);
  150. return;
  151. }
  152. spin_unlock_irqrestore(&vchan->lock, flags);
  153. virtio_cwrite32_nosleep(vmbox->vdev, offsets[ACK_H] + chan_idx * 4, val);
  154. /*
  155. * don't warry, free used buf should be fast,
  156. * normally we only have one buf to free.
  157. */
  158. while (1) {
  159. buf = virtqueue_get_buf(vq, &len);
  160. if (buf == NULL)
  161. break;
  162. kfree(buf);
  163. }
  164. /*
  165. * backend Ack
  166. * when virtio_mbox_outvq_cb() is called, we think backend already
  167. * received last Tx msg we send by virtio_mbox_send_data(), so we
  168. * think it is Ack from backend, then we can mark last Tx done and
  169. * send next msg.
  170. */
  171. mbox_chan_txdone(chan, 0);
  172. }
  173. static int init_vqs(struct virtio_mbox *vmbox)
  174. {
  175. vq_callback_t **callbacks;
  176. struct virtqueue **vqs;
  177. int ret = -ENOMEM;
  178. int i, j, total_vqs;
  179. const char **names;
  180. char q_name[2][32];
  181. total_vqs = vmbox->chan_num * 2;
  182. vqs = kcalloc(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
  183. callbacks = kmalloc_array(total_vqs, sizeof(vq_callback_t *), GFP_KERNEL);
  184. names = kmalloc_array(total_vqs, sizeof(char *), GFP_KERNEL);
  185. vmbox->in_vqs = kmalloc_array(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
  186. vmbox->out_vqs = kmalloc_array(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
  187. if (!vqs || !callbacks || !names || !vmbox->in_vqs || !vmbox->out_vqs)
  188. goto free;
  189. j = 0;
  190. for (i = 0; i < vmbox->chan_num; i++) {
  191. callbacks[j] = virtio_mbox_invq_cb;
  192. callbacks[j + 1] = virtio_mbox_outvq_cb;
  193. sprintf(q_name[0], "mailbox_input.%d", i);
  194. sprintf(q_name[1], "mailbox_output.%d", i);
  195. names[j] = q_name[0];
  196. names[j + 1] = q_name[1];
  197. j += 2;
  198. }
  199. ret = virtio_find_vqs(vmbox->vdev, total_vqs, vqs, callbacks, names, NULL);
  200. if (ret)
  201. goto free;
  202. j = 0;
  203. for (i = 0; i < vmbox->chan_num; i++) {
  204. vmbox->in_vqs[i] = vqs[j];
  205. vmbox->out_vqs[i] = vqs[j + 1];
  206. j += 2;
  207. }
  208. kfree(names);
  209. kfree(callbacks);
  210. kfree(vqs);
  211. return 0;
  212. free:
  213. kfree(vmbox->out_vqs);
  214. kfree(vmbox->in_vqs);
  215. kfree(names);
  216. kfree(callbacks);
  217. kfree(vqs);
  218. return ret;
  219. }
  220. static void virtio_mbox_del_vqs(struct virtio_mbox *vmbox)
  221. {
  222. struct virtio_device *vdev = vmbox->vdev;
  223. vdev->config->del_vqs(vdev);
  224. }
  225. static void virtio_mbox_reclaim_used_bufs(struct virtio_mbox *vmbox)
  226. {
  227. struct virtio_chan *vchans = vmbox->vchans;
  228. struct virtqueue *vq;
  229. unsigned long flags;
  230. unsigned int len;
  231. u32 chan_idx;
  232. void *buf;
  233. /*
  234. * go through all vqs with the order by vq->index:
  235. * 0, 1, 2, 3...
  236. */
  237. virtio_device_for_each_vq(vmbox->vdev, vq) {
  238. chan_idx = vq->index/2;
  239. spin_lock_irqsave(&vchans[chan_idx].lock, flags);
  240. if (vchans[chan_idx].in_use) {
  241. if (chan_idx & 1)
  242. vchans[chan_idx].in_use = false;
  243. while (1) {
  244. buf = virtqueue_get_buf(vq, &len);
  245. if (buf == NULL)
  246. break;
  247. kfree(buf);
  248. }
  249. }
  250. spin_unlock_irqrestore(&vchans[chan_idx].lock, flags);
  251. }
  252. }
  253. static void remove_vq_common(struct virtio_mbox *vmbox)
  254. {
  255. vmbox->vdev->config->reset(vmbox->vdev);
  256. virtio_mbox_reclaim_used_bufs(vmbox);
  257. virtio_mbox_del_vqs(vmbox);
  258. }
  259. static struct mbox_chan *virtio_mbox_xlate(struct mbox_controller *mbox,
  260. const struct of_phandle_args *sp)
  261. {
  262. struct mbox_chan *chan;
  263. unsigned int ch = sp->args[0];
  264. struct virtio_chan *vchan;
  265. unsigned long flags;
  266. if (ch >= mbox->num_chans) {
  267. dev_err(mbox->dev, "Invalid channel idx %d\n", ch);
  268. return ERR_PTR(-EINVAL);
  269. }
  270. chan = &mbox->chans[ch];
  271. vchan = chan->con_priv;
  272. spin_lock_irqsave(&vchan->lock, flags);
  273. if (vchan->in_use) {
  274. spin_unlock_irqrestore(&vchan->lock, flags);
  275. dev_err(mbox->dev, "Channel idx %d is in use\n", ch);
  276. return ERR_PTR(-EINVAL);
  277. }
  278. spin_unlock_irqrestore(&vchan->lock, flags);
  279. return chan;
  280. }
  281. static int virtio_mbox_send_data(struct mbox_chan *chan, void *vmsg)
  282. {
  283. struct virtio_chan *vchan = chan->con_priv;
  284. struct virtio_mbox *vmbox = vchan->vmbox;
  285. struct scatterlist sg[1];
  286. struct virtqueue *out_vq;
  287. void *data;
  288. u32 val = 1;
  289. int err = 0;
  290. out_vq = vchan->out_vq;
  291. data = kmemdup(vmsg, vmbox->msg_size, GFP_ATOMIC);
  292. if (!data) {
  293. dev_err(chan->mbox->dev, "memory alloc fialed\n");
  294. goto exit;
  295. }
  296. sg_init_one(sg, data, vmbox->msg_size);
  297. err = virtqueue_add_outbuf(out_vq, sg, 1, data, GFP_ATOMIC);
  298. if (err) {
  299. kfree(data);
  300. dev_err(chan->mbox->dev, "add outbuf failed %d\n", err);
  301. goto exit;
  302. }
  303. virtio_cwrite32_nosleep(vmbox->vdev, offsets[MSG_G] + vchan->idx * 4, val);
  304. /* tell backend for new data */
  305. virtqueue_kick(out_vq);
  306. exit:
  307. return 0;
  308. }
  309. static int virtio_mbox_startup(struct mbox_chan *chan)
  310. {
  311. struct virtio_chan *vchan = chan->con_priv;
  312. unsigned long flags;
  313. spin_lock_irqsave(&vchan->lock, flags);
  314. vchan->in_use = true;
  315. spin_unlock_irqrestore(&vchan->lock, flags);
  316. return 0;
  317. }
  318. static void virtio_mbox_shutdown(struct mbox_chan *chan)
  319. {
  320. struct virtio_chan *vchan = chan->con_priv;
  321. unsigned long flags;
  322. spin_lock_irqsave(&vchan->lock, flags);
  323. vchan->in_use = false;
  324. spin_unlock_irqrestore(&vchan->lock, flags);
  325. }
  326. static const struct mbox_chan_ops virtio_mbox_ops = {
  327. .send_data = virtio_mbox_send_data,
  328. .startup = virtio_mbox_startup,
  329. .shutdown = virtio_mbox_shutdown,
  330. };
  331. /* setup vqs and vmbox */
  332. static int virtio_mbox_setup(struct virtio_device *vdev)
  333. {
  334. struct virtio_mbox *vmbox;
  335. struct virtio_chan *vchans;
  336. int ret = -ENOMEM;
  337. int i;
  338. vmbox = devm_kzalloc(&vdev->dev, sizeof(*vmbox), GFP_KERNEL);
  339. if (!vmbox)
  340. return ret;
  341. vdev->priv = vmbox;
  342. vmbox->vdev = vdev;
  343. ret = virtio_cread_feature(vdev, VIRTIO_MAILBOX_F_CHAN_NUM,
  344. struct virtio_mailbox_config, chan_num,
  345. &vmbox->chan_num);
  346. if (ret)
  347. vmbox->chan_num = VIRTIO_MAILBOX_CHAN_NUM_DEFAULT;
  348. ret = virtio_cread_feature(vdev, VIRTIO_MAILBOX_F_MSG_SIZE,
  349. struct virtio_mailbox_config, msg_size,
  350. &vmbox->msg_size);
  351. if (ret)
  352. vmbox->msg_size = VIRTIO_MAILBOX_MSG_SIZE_DEFAULT;
  353. vmbox->chans = devm_kcalloc(&vmbox->vdev->dev, vmbox->chan_num,
  354. sizeof(*vmbox->chans), GFP_KERNEL);
  355. if (!vmbox->chans)
  356. goto free_vmbox;
  357. vchans = devm_kcalloc(&vmbox->vdev->dev, vmbox->chan_num,
  358. sizeof(*vchans), GFP_KERNEL);
  359. if (!vchans)
  360. goto free_chans;
  361. vmbox->vchans = vchans;
  362. /*
  363. * the actual dev is detected by virtio_light bus in
  364. * virtio_light_probe():
  365. * vm_dev->vdev.dev.parent = dev;
  366. */
  367. vmbox->controller.dev = vdev->dev.parent;
  368. vmbox->controller.num_chans = vmbox->chan_num;
  369. vmbox->controller.chans = &vmbox->chans[0];
  370. vmbox->controller.ops = &virtio_mbox_ops;
  371. vmbox->controller.of_xlate = &virtio_mbox_xlate;
  372. vmbox->controller.txdone_irq = true; /* see comments of mbox_chan_txdone() */
  373. ret = init_vqs(vmbox);
  374. if (ret)
  375. goto free_vchans;
  376. /* map chan to in,out queue pair */
  377. for (i = 0; i < vmbox->chan_num; i++) {
  378. vchans[i].vmbox = vmbox;
  379. vchans[i].chan = &vmbox->chans[i];
  380. vchans[i].idx = i;
  381. vchans[i].in_vq = vmbox->in_vqs[i];
  382. vchans[i].out_vq = vmbox->out_vqs[i];
  383. spin_lock_init(&(vchans[i].lock));
  384. /* The standard mailbox channel mapped to vq */
  385. vmbox->chans[i].con_priv = &vchans[i];
  386. ret = fill_invq(vmbox->in_vqs[i], &vchans[i].lock, vmbox->msg_size);
  387. if (ret == -ENOMEM)
  388. goto free_vchans;
  389. }
  390. ret = devm_mbox_controller_register(&vmbox->vdev->dev, &vmbox->controller);
  391. if (ret) {
  392. dev_err(&vmbox->vdev->dev, "Could not register mailbox controller\n");
  393. goto free_vchans;
  394. }
  395. dev_set_drvdata(&vmbox->vdev->dev, vmbox);
  396. dev_info(&vmbox->vdev->dev, "registered %d channels\n", vmbox->chan_num);
  397. return 0;
  398. free_vchans:
  399. kfree(vchans);
  400. free_chans:
  401. kfree(vmbox->chans);
  402. free_vmbox:
  403. kfree(vmbox);
  404. return ret;
  405. }
  406. static int virtio_mbox_probe(struct virtio_device *vdev)
  407. {
  408. struct device *dev = &vdev->dev;
  409. struct virtio_mbox *vmbox;
  410. int i, ret = 0;
  411. u32 val = 0;
  412. ret = virtio_mbox_setup(vdev);
  413. if (ret) {
  414. dev_err(dev, "virtio_mbox setup failed\n");
  415. return ret;
  416. }
  417. vmbox = vdev->priv;
  418. for (i = 0; i < vmbox->chan_num; i++) {
  419. virtio_cwrite32(vdev, offsets[ACK_G] + i * 4, val);
  420. virtio_cwrite32(vdev, offsets[MSG_G] + i * 4, val);
  421. }
  422. virtio_device_ready(vdev);
  423. dev_err(dev, "frontend driver init successfully\n");
  424. return ret;
  425. }
  426. static void virtio_mbox_remove(struct virtio_device *vdev)
  427. {
  428. struct virtio_mbox *vmbox = vdev->priv;
  429. kfree(vmbox->chans);
  430. kfree(vmbox->vchans);
  431. kfree(vmbox->out_vqs);
  432. kfree(vmbox->in_vqs);
  433. remove_vq_common(vmbox);
  434. kfree(vmbox);
  435. }
  436. static struct virtio_device_id id_table[] = {
  437. { VIRTIO_ID_MAILBOX, VIRTIO_DEV_ANY_ID },
  438. { 0 },
  439. };
  440. static unsigned int features[] = {
  441. VIRTIO_MAILBOX_F_CHAN_NUM, VIRTIO_MAILBOX_F_MSG_SIZE,
  442. };
  443. static struct virtio_driver virtio_mailbox_driver = {
  444. .feature_table = features,
  445. .feature_table_size = ARRAY_SIZE(features),
  446. .driver.name = KBUILD_MODNAME,
  447. .driver.owner = THIS_MODULE,
  448. .id_table = id_table,
  449. .probe = virtio_mbox_probe,
  450. .remove = virtio_mbox_remove,
  451. };
  452. static __init int virtio_mailbox_driver_init(void)
  453. {
  454. int ret;
  455. ret = register_virtio_driver(&virtio_mailbox_driver);
  456. return ret;
  457. }
  458. module_init(virtio_mailbox_driver_init);
  459. static __exit void virtio_mailbox_driver_exit(void)
  460. {
  461. unregister_virtio_driver(&virtio_mailbox_driver);
  462. }
  463. module_exit(virtio_mailbox_driver_exit);
  464. MODULE_DEVICE_TABLE(virtio, id_table);
  465. MODULE_AUTHOR("Xianting Tian <xianting.tian@linux.alibaba.com>");
  466. MODULE_DESCRIPTION("Virtio mailbox driver");
  467. MODULE_LICENSE("GPL");