123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572 |
- // SPDX-License-Identifier: GPL-2.0+
- #include <linux/device.h>
- #include <linux/interrupt.h>
- #include <linux/kernel.h>
- #include <linux/mailbox_controller.h>
- #include <linux/module.h>
- #include <linux/of.h>
- #include <linux/platform_device.h>
- #include <linux/virtio.h>
- #include <linux/virtio_mailbox.h>
- #include <linux/virtio_config.h>
- #include <linux/virtio_light.h>
- #include <linux/virtio_ids.h>
- #define DRIVER_NAME "virtio-mailbox"
- static inline u32 virtio_cread32_nosleep(struct virtio_device *vdev,
- unsigned int offset)
- {
- __virtio32 ret;
- vdev->config->get(vdev, offset, &ret, sizeof(ret));
- return virtio32_to_cpu(vdev, ret);
- }
- static inline void virtio_cwrite32_nosleep(struct virtio_device *vdev,
- unsigned int offset, u32 val)
- {
- __virtio32 v;
- v = cpu_to_virtio32(vdev, val);
- vdev->config->set(vdev, offset, &v, sizeof(v));
- }
- struct virtio_mbox {
- struct mbox_controller controller;
- struct virtio_device *vdev;
- unsigned int chan_num;
- unsigned int msg_size;
- struct mbox_chan *chans;
- struct virtio_chan *vchans;
- struct virtqueue **in_vqs;
- struct virtqueue **out_vqs;
- };
- struct virtio_chan {
- struct virtqueue *in_vq;
- struct virtqueue *out_vq;
- struct virtio_mbox *vmbox;
- struct mbox_chan *chan;
- bool in_use;
- u8 idx;
- spinlock_t lock;
- };
- static int add_invq_buf(struct virtqueue *vq, void *buf, u32 msg_size)
- {
- struct scatterlist sg[1];
- int ret;
- sg_init_one(sg, buf, msg_size);
- ret = virtqueue_add_inbuf(vq, sg, 1, buf, GFP_ATOMIC);
- if (!ret)
- ret = vq->num_free;
- return ret;
- }
- static int fill_invq(struct virtqueue *vq, spinlock_t *lock,
- unsigned int msg_size)
- {
- int nr_added_bufs;
- int ret;
- void *buf;
- nr_added_bufs = 0;
- do {
- buf = kmalloc(msg_size, GFP_KERNEL);
- if (!buf)
- return -ENOMEM;
- spin_lock_irq(lock);
- ret = add_invq_buf(vq, buf, msg_size);
- if (ret < 0) {
- spin_unlock_irq(lock);
- kfree(buf);
- return ret;
- }
- nr_added_bufs++;
- spin_unlock_irq(lock);
- } while (ret > 0);
- return nr_added_bufs;
- }
- /* callback for virtio inqueue */
- static void virtio_mbox_invq_cb(struct virtqueue *vq)
- {
- struct virtio_mbox *vmbox = vq->vdev->priv;
- struct mbox_chan *chan;
- struct virtio_chan *vchan;
- unsigned int len;
- u32 chan_idx, msg, val = 0;
- unsigned long flags;
- void *buf;
- /*
- * As vq->index is in range {0,1,2,...vmbox->chan_num*2},
- * vq->index in range {0,2,4,...} is for in_vqs;
- * vq->index in range {1,3,5,...} is for out_vqs;
- * one in and out vq pair is for one chan, eg,
- * {0,1} for chan 0;
- * {2,3} for chan 1;
- * {4,5} for chan 2,etc
- */
- chan_idx = vq->index/2;
- chan = &vmbox->chans[chan_idx];
- vchan = chan->con_priv;
- msg = virtio_cread32_nosleep(vmbox->vdev, offsets[MSG_H] + chan_idx * 4);
- spin_lock_irqsave(&vchan->lock, flags);
- if (!vchan->in_use || !msg) {
- spin_unlock_irqrestore(&vchan->lock, flags);
- return;
- }
- spin_unlock_irqrestore(&vchan->lock, flags);
- virtio_cwrite32_nosleep(vmbox->vdev, offsets[MSG_H] + chan_idx * 4, val);
- while (1) {
- buf = virtqueue_get_buf(vq, &len);
- if (buf == NULL)
- break;
- /*
- * assume client copied vmsg to its private buf
- * when this func return
- */
- mbox_chan_received_data(chan, buf);
- add_invq_buf(vq, buf, vmbox->msg_size);
- /* mark Ack */
- val = 1;
- virtio_cwrite32_nosleep(vmbox->vdev, offsets[ACK_G] + chan_idx * 4, val);
- /* tell backend to handle Ack */
- virtqueue_kick(vq);
- }
- }
- /*
- * callback for virtio outqueue
- * we use this func to reclaim used buffer and handle ack
- */
- static void virtio_mbox_outvq_cb(struct virtqueue *vq)
- {
- struct virtio_mbox *vmbox = vq->vdev->priv;
- struct mbox_chan *chan;
- struct virtio_chan *vchan;
- unsigned int len;
- int chan_idx;
- void *buf;
- unsigned long flags;
- u32 ack, val = 0;
- /* see comments in virtio_mbox_invq_cb() */
- chan_idx = vq->index/2;
- chan = &vmbox->chans[chan_idx];
- vchan = chan->con_priv;
- ack = virtio_cread32_nosleep(vmbox->vdev, offsets[ACK_H] + chan_idx * 4);
- spin_lock_irqsave(&vchan->lock, flags);
- if (!vchan->in_use || !ack) {
- spin_unlock_irqrestore(&vchan->lock, flags);
- return;
- }
- spin_unlock_irqrestore(&vchan->lock, flags);
- virtio_cwrite32_nosleep(vmbox->vdev, offsets[ACK_H] + chan_idx * 4, val);
- /*
- * don't warry, free used buf should be fast,
- * normally we only have one buf to free.
- */
- while (1) {
- buf = virtqueue_get_buf(vq, &len);
- if (buf == NULL)
- break;
- kfree(buf);
- }
- /*
- * backend Ack
- * when virtio_mbox_outvq_cb() is called, we think backend already
- * received last Tx msg we send by virtio_mbox_send_data(), so we
- * think it is Ack from backend, then we can mark last Tx done and
- * send next msg.
- */
- mbox_chan_txdone(chan, 0);
- }
- static int init_vqs(struct virtio_mbox *vmbox)
- {
- vq_callback_t **callbacks;
- struct virtqueue **vqs;
- int ret = -ENOMEM;
- int i, j, total_vqs;
- const char **names;
- char q_name[2][32];
- total_vqs = vmbox->chan_num * 2;
- vqs = kcalloc(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
- callbacks = kmalloc_array(total_vqs, sizeof(vq_callback_t *), GFP_KERNEL);
- names = kmalloc_array(total_vqs, sizeof(char *), GFP_KERNEL);
- vmbox->in_vqs = kmalloc_array(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
- vmbox->out_vqs = kmalloc_array(total_vqs, sizeof(struct virtqueue *), GFP_KERNEL);
- if (!vqs || !callbacks || !names || !vmbox->in_vqs || !vmbox->out_vqs)
- goto free;
- j = 0;
- for (i = 0; i < vmbox->chan_num; i++) {
- callbacks[j] = virtio_mbox_invq_cb;
- callbacks[j + 1] = virtio_mbox_outvq_cb;
- sprintf(q_name[0], "mailbox_input.%d", i);
- sprintf(q_name[1], "mailbox_output.%d", i);
- names[j] = q_name[0];
- names[j + 1] = q_name[1];
- j += 2;
- }
- ret = virtio_find_vqs(vmbox->vdev, total_vqs, vqs, callbacks, names, NULL);
- if (ret)
- goto free;
- j = 0;
- for (i = 0; i < vmbox->chan_num; i++) {
- vmbox->in_vqs[i] = vqs[j];
- vmbox->out_vqs[i] = vqs[j + 1];
- j += 2;
- }
- kfree(names);
- kfree(callbacks);
- kfree(vqs);
- return 0;
- free:
- kfree(vmbox->out_vqs);
- kfree(vmbox->in_vqs);
- kfree(names);
- kfree(callbacks);
- kfree(vqs);
- return ret;
- }
- static void virtio_mbox_del_vqs(struct virtio_mbox *vmbox)
- {
- struct virtio_device *vdev = vmbox->vdev;
- vdev->config->del_vqs(vdev);
- }
- static void virtio_mbox_reclaim_used_bufs(struct virtio_mbox *vmbox)
- {
- struct virtio_chan *vchans = vmbox->vchans;
- struct virtqueue *vq;
- unsigned long flags;
- unsigned int len;
- u32 chan_idx;
- void *buf;
- /*
- * go through all vqs with the order by vq->index:
- * 0, 1, 2, 3...
- */
- virtio_device_for_each_vq(vmbox->vdev, vq) {
- chan_idx = vq->index/2;
- spin_lock_irqsave(&vchans[chan_idx].lock, flags);
- if (vchans[chan_idx].in_use) {
- if (chan_idx & 1)
- vchans[chan_idx].in_use = false;
- while (1) {
- buf = virtqueue_get_buf(vq, &len);
- if (buf == NULL)
- break;
- kfree(buf);
- }
- }
- spin_unlock_irqrestore(&vchans[chan_idx].lock, flags);
- }
- }
- static void remove_vq_common(struct virtio_mbox *vmbox)
- {
- vmbox->vdev->config->reset(vmbox->vdev);
- virtio_mbox_reclaim_used_bufs(vmbox);
- virtio_mbox_del_vqs(vmbox);
- }
- static struct mbox_chan *virtio_mbox_xlate(struct mbox_controller *mbox,
- const struct of_phandle_args *sp)
- {
- struct mbox_chan *chan;
- unsigned int ch = sp->args[0];
- struct virtio_chan *vchan;
- unsigned long flags;
- if (ch >= mbox->num_chans) {
- dev_err(mbox->dev, "Invalid channel idx %d\n", ch);
- return ERR_PTR(-EINVAL);
- }
- chan = &mbox->chans[ch];
- vchan = chan->con_priv;
- spin_lock_irqsave(&vchan->lock, flags);
- if (vchan->in_use) {
- spin_unlock_irqrestore(&vchan->lock, flags);
- dev_err(mbox->dev, "Channel idx %d is in use\n", ch);
- return ERR_PTR(-EINVAL);
- }
- spin_unlock_irqrestore(&vchan->lock, flags);
- return chan;
- }
- static int virtio_mbox_send_data(struct mbox_chan *chan, void *vmsg)
- {
- struct virtio_chan *vchan = chan->con_priv;
- struct virtio_mbox *vmbox = vchan->vmbox;
- struct scatterlist sg[1];
- struct virtqueue *out_vq;
- void *data;
- u32 val = 1;
- int err = 0;
- out_vq = vchan->out_vq;
- data = kmemdup(vmsg, vmbox->msg_size, GFP_ATOMIC);
- if (!data) {
- dev_err(chan->mbox->dev, "memory alloc fialed\n");
- goto exit;
- }
- sg_init_one(sg, data, vmbox->msg_size);
- err = virtqueue_add_outbuf(out_vq, sg, 1, data, GFP_ATOMIC);
- if (err) {
- kfree(data);
- dev_err(chan->mbox->dev, "add outbuf failed %d\n", err);
- goto exit;
- }
- virtio_cwrite32_nosleep(vmbox->vdev, offsets[MSG_G] + vchan->idx * 4, val);
- /* tell backend for new data */
- virtqueue_kick(out_vq);
- exit:
- return 0;
- }
- static int virtio_mbox_startup(struct mbox_chan *chan)
- {
- struct virtio_chan *vchan = chan->con_priv;
- unsigned long flags;
- spin_lock_irqsave(&vchan->lock, flags);
- vchan->in_use = true;
- spin_unlock_irqrestore(&vchan->lock, flags);
- return 0;
- }
- static void virtio_mbox_shutdown(struct mbox_chan *chan)
- {
- struct virtio_chan *vchan = chan->con_priv;
- unsigned long flags;
- spin_lock_irqsave(&vchan->lock, flags);
- vchan->in_use = false;
- spin_unlock_irqrestore(&vchan->lock, flags);
- }
- static const struct mbox_chan_ops virtio_mbox_ops = {
- .send_data = virtio_mbox_send_data,
- .startup = virtio_mbox_startup,
- .shutdown = virtio_mbox_shutdown,
- };
- /* setup vqs and vmbox */
- static int virtio_mbox_setup(struct virtio_device *vdev)
- {
- struct virtio_mbox *vmbox;
- struct virtio_chan *vchans;
- int ret = -ENOMEM;
- int i;
- vmbox = devm_kzalloc(&vdev->dev, sizeof(*vmbox), GFP_KERNEL);
- if (!vmbox)
- return ret;
- vdev->priv = vmbox;
- vmbox->vdev = vdev;
- ret = virtio_cread_feature(vdev, VIRTIO_MAILBOX_F_CHAN_NUM,
- struct virtio_mailbox_config, chan_num,
- &vmbox->chan_num);
- if (ret)
- vmbox->chan_num = VIRTIO_MAILBOX_CHAN_NUM_DEFAULT;
- ret = virtio_cread_feature(vdev, VIRTIO_MAILBOX_F_MSG_SIZE,
- struct virtio_mailbox_config, msg_size,
- &vmbox->msg_size);
- if (ret)
- vmbox->msg_size = VIRTIO_MAILBOX_MSG_SIZE_DEFAULT;
- vmbox->chans = devm_kcalloc(&vmbox->vdev->dev, vmbox->chan_num,
- sizeof(*vmbox->chans), GFP_KERNEL);
- if (!vmbox->chans)
- goto free_vmbox;
- vchans = devm_kcalloc(&vmbox->vdev->dev, vmbox->chan_num,
- sizeof(*vchans), GFP_KERNEL);
- if (!vchans)
- goto free_chans;
- vmbox->vchans = vchans;
- /*
- * the actual dev is detected by virtio_light bus in
- * virtio_light_probe():
- * vm_dev->vdev.dev.parent = dev;
- */
- vmbox->controller.dev = vdev->dev.parent;
- vmbox->controller.num_chans = vmbox->chan_num;
- vmbox->controller.chans = &vmbox->chans[0];
- vmbox->controller.ops = &virtio_mbox_ops;
- vmbox->controller.of_xlate = &virtio_mbox_xlate;
- vmbox->controller.txdone_irq = true; /* see comments of mbox_chan_txdone() */
- ret = init_vqs(vmbox);
- if (ret)
- goto free_vchans;
- /* map chan to in,out queue pair */
- for (i = 0; i < vmbox->chan_num; i++) {
- vchans[i].vmbox = vmbox;
- vchans[i].chan = &vmbox->chans[i];
- vchans[i].idx = i;
- vchans[i].in_vq = vmbox->in_vqs[i];
- vchans[i].out_vq = vmbox->out_vqs[i];
- spin_lock_init(&(vchans[i].lock));
- /* The standard mailbox channel mapped to vq */
- vmbox->chans[i].con_priv = &vchans[i];
- ret = fill_invq(vmbox->in_vqs[i], &vchans[i].lock, vmbox->msg_size);
- if (ret == -ENOMEM)
- goto free_vchans;
- }
- ret = devm_mbox_controller_register(&vmbox->vdev->dev, &vmbox->controller);
- if (ret) {
- dev_err(&vmbox->vdev->dev, "Could not register mailbox controller\n");
- goto free_vchans;
- }
- dev_set_drvdata(&vmbox->vdev->dev, vmbox);
- dev_info(&vmbox->vdev->dev, "registered %d channels\n", vmbox->chan_num);
- return 0;
- free_vchans:
- kfree(vchans);
- free_chans:
- kfree(vmbox->chans);
- free_vmbox:
- kfree(vmbox);
- return ret;
- }
- static int virtio_mbox_probe(struct virtio_device *vdev)
- {
- struct device *dev = &vdev->dev;
- struct virtio_mbox *vmbox;
- int i, ret = 0;
- u32 val = 0;
- ret = virtio_mbox_setup(vdev);
- if (ret) {
- dev_err(dev, "virtio_mbox setup failed\n");
- return ret;
- }
- vmbox = vdev->priv;
- for (i = 0; i < vmbox->chan_num; i++) {
- virtio_cwrite32(vdev, offsets[ACK_G] + i * 4, val);
- virtio_cwrite32(vdev, offsets[MSG_G] + i * 4, val);
- }
- virtio_device_ready(vdev);
- dev_err(dev, "frontend driver init successfully\n");
- return ret;
- }
- static void virtio_mbox_remove(struct virtio_device *vdev)
- {
- struct virtio_mbox *vmbox = vdev->priv;
- kfree(vmbox->chans);
- kfree(vmbox->vchans);
- kfree(vmbox->out_vqs);
- kfree(vmbox->in_vqs);
- remove_vq_common(vmbox);
- kfree(vmbox);
- }
- static struct virtio_device_id id_table[] = {
- { VIRTIO_ID_MAILBOX, VIRTIO_DEV_ANY_ID },
- { 0 },
- };
- static unsigned int features[] = {
- VIRTIO_MAILBOX_F_CHAN_NUM, VIRTIO_MAILBOX_F_MSG_SIZE,
- };
- static struct virtio_driver virtio_mailbox_driver = {
- .feature_table = features,
- .feature_table_size = ARRAY_SIZE(features),
- .driver.name = KBUILD_MODNAME,
- .driver.owner = THIS_MODULE,
- .id_table = id_table,
- .probe = virtio_mbox_probe,
- .remove = virtio_mbox_remove,
- };
- static __init int virtio_mailbox_driver_init(void)
- {
- int ret;
- ret = register_virtio_driver(&virtio_mailbox_driver);
- return ret;
- }
- module_init(virtio_mailbox_driver_init);
- static __exit void virtio_mailbox_driver_exit(void)
- {
- unregister_virtio_driver(&virtio_mailbox_driver);
- }
- module_exit(virtio_mailbox_driver_exit);
- MODULE_DEVICE_TABLE(virtio, id_table);
- MODULE_AUTHOR("Xianting Tian <xianting.tian@linux.alibaba.com>");
- MODULE_DESCRIPTION("Virtio mailbox driver");
- MODULE_LICENSE("GPL");
|