2 * Copyright (C) ST-Ericsson AB 2013
4 * Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
6 * License terms: GNU General Public License (GPL) version 2
8 #include <linux/module.h>
9 #include <linux/if_arp.h>
10 #include <linux/virtio.h>
11 #include <linux/vringh.h>
12 #include <linux/debugfs.h>
13 #include <linux/spinlock.h>
14 #include <linux/genalloc.h>
15 #include <linux/interrupt.h>
16 #include <linux/netdevice.h>
17 #include <linux/rtnetlink.h>
18 #include <linux/virtio_ids.h>
19 #include <linux/virtio_caif.h>
20 #include <linux/virtio_ring.h>
21 #include <linux/dma-mapping.h>
22 #include <net/caif/caif_dev.h>
23 #include <linux/virtio_config.h>
25 MODULE_LICENSE("GPL v2");
26 MODULE_AUTHOR("Vicram Arv");
27 MODULE_AUTHOR("Sjur Brendeland");
28 MODULE_DESCRIPTION("Virtio CAIF Driver");
30 /* NAPI schedule quota */
31 #define CFV_DEFAULT_QUOTA 32
33 /* Defaults used if virtio config space is unavailable */
34 #define CFV_DEF_MTU_SIZE 4096
35 #define CFV_DEF_HEADROOM 32
36 #define CFV_DEF_TAILROOM 32
38 /* Required IP header alignment */
39 #define IP_HDR_ALIGN 4
41 /* struct cfv_napi_contxt - NAPI context info
42 * @riov: IOV holding data read from the ring. Note that riov may
43 * still hold data when cfv_rx_poll() returns.
44 * @head: Last descriptor ID we received from vringh_getdesc_kern.
45 * We use this to put descriptor back on the used ring. USHRT_MAX is
46 * used to indicate invalid head-id.
48 struct cfv_napi_context {
49 struct vringh_kiov riov;
53 /* struct cfv_stats - statistics for debugfs
54 * @rx_napi_complete: Number of NAPI completions (RX)
55 * @rx_napi_resched: Number of calls where the full quota was used (RX)
56 * @rx_nomem: Number of SKB alloc failures (RX)
57 * @rx_kicks: Number of RX kicks
58 * @tx_full_ring: Number times TX ring was full
59 * @tx_no_mem: Number of times TX went out of memory
60 * @tx_flow_on: Number of flow on (TX)
61 * @tx_kicks: Number of TX kicks
74 /* struct cfv_info - Caif Virtio control structure
75 * @cfdev: caif common header
76 * @vdev: Associated virtio device
77 * @vr_rx: rx/downlink host vring
78 * @vq_tx: tx/uplink virtqueue
79 * @ndev: CAIF link layer device
80 * @watermark_tx: indicates number of free descriptors we need
81 * to reopen the tx-queues after overload.
82 * @tx_lock: protects vq_tx from concurrent use
83 * @tx_release_tasklet: Tasklet for freeing consumed TX buffers
84 * @napi: Napi context used in cfv_rx_poll()
85 * @ctx: Context data used in cfv_rx_poll()
86 * @tx_hr: transmit headroom
87 * @rx_hr: receive headroom
88 * @tx_tr: transmit tail room
89 * @rx_tr: receive tail room
90 * @mtu: transmit max size
91 * @mru: receive max size
92 * @allocsz: size of dma memory reserved for TX buffers
93 * @alloc_addr: virtual address to dma memory for TX buffers
94 * @alloc_dma: dma address to dma memory for TX buffers
95 * @genpool: Gen Pool used for allocating TX buffers
96 * @reserved_mem: Pointer to memory reserve allocated from genpool
97 * @reserved_size: Size of memory reserve allocated from genpool
98 * @stats: Statistics exposed in sysfs
99 * @debugfs: Debugfs dentry for statistic counters
102 struct caif_dev_common cfdev;
103 struct virtio_device *vdev;
104 struct vringh *vr_rx;
105 struct virtqueue *vq_tx;
106 struct net_device *ndev;
107 unsigned int watermark_tx;
108 /* Protect access to vq_tx */
110 struct tasklet_struct tx_release_tasklet;
111 struct napi_struct napi;
112 struct cfv_napi_context ctx;
121 dma_addr_t alloc_dma;
122 struct gen_pool *genpool;
123 unsigned long reserved_mem;
124 size_t reserved_size;
125 struct cfv_stats stats;
126 struct dentry *debugfs;
129 /* struct buf_info - maintains transmit buffer data handle
130 * @size: size of transmit buffer
131 * @dma_handle: handle to allocated dma device memory area
132 * @vaddr: virtual address mapping to allocated memory area
139 /* Called from virtio device, in IRQ context */
140 static void cfv_release_cb(struct virtqueue *vq_tx)
142 struct cfv_info *cfv = vq_tx->vdev->priv;
144 ++cfv->stats.tx_kicks;
145 tasklet_schedule(&cfv->tx_release_tasklet);
148 static void free_buf_info(struct cfv_info *cfv, struct buf_info *buf_info)
152 gen_pool_free(cfv->genpool, (unsigned long) buf_info->vaddr,
157 /* This is invoked whenever the remote processor completed processing
158 * a TX msg we just sent, and the buffer is put back to the used ring.
160 static void cfv_release_used_buf(struct virtqueue *vq_tx)
162 struct cfv_info *cfv = vq_tx->vdev->priv;
165 BUG_ON(vq_tx != cfv->vq_tx);
169 struct buf_info *buf_info;
171 /* Get used buffer from used ring to recycle used descriptors */
172 spin_lock_irqsave(&cfv->tx_lock, flags);
173 buf_info = virtqueue_get_buf(vq_tx, &len);
174 spin_unlock_irqrestore(&cfv->tx_lock, flags);
176 /* Stop looping if there are no more buffers to free */
180 free_buf_info(cfv, buf_info);
182 /* watermark_tx indicates if we previously stopped the tx
183 * queues. If we have enough free stots in the virtio ring,
184 * re-establish memory reserved and open up tx queues.
186 if (cfv->vq_tx->num_free <= cfv->watermark_tx)
189 /* Re-establish memory reserve */
190 if (cfv->reserved_mem == 0 && cfv->genpool)
192 gen_pool_alloc(cfv->genpool,
195 /* Open up the tx queues */
196 if (cfv->reserved_mem) {
198 virtqueue_get_vring_size(cfv->vq_tx);
199 netif_tx_wake_all_queues(cfv->ndev);
200 /* Buffers are recycled in cfv_netdev_tx, so
201 * disable notifications when queues are opened.
203 virtqueue_disable_cb(cfv->vq_tx);
204 ++cfv->stats.tx_flow_on;
206 /* if no memory reserve, wait for more free slots */
207 WARN_ON(cfv->watermark_tx >
208 virtqueue_get_vring_size(cfv->vq_tx));
210 virtqueue_get_vring_size(cfv->vq_tx) / 4;
215 /* Allocate a SKB and copy packet data to it */
216 static struct sk_buff *cfv_alloc_and_copy_skb(int *err,
217 struct cfv_info *cfv,
218 u8 *frm, u32 frm_len)
221 u32 cfpkt_len, pad_len;
224 /* Verify that packet size with down-link header and mtu size */
225 if (frm_len > cfv->mru || frm_len <= cfv->rx_hr + cfv->rx_tr) {
226 netdev_err(cfv->ndev,
227 "Invalid frmlen:%u mtu:%u hr:%d tr:%d\n",
228 frm_len, cfv->mru, cfv->rx_hr,
234 cfpkt_len = frm_len - (cfv->rx_hr + cfv->rx_tr);
235 pad_len = (unsigned long)(frm + cfv->rx_hr) & (IP_HDR_ALIGN - 1);
237 skb = netdev_alloc_skb(cfv->ndev, frm_len + pad_len);
243 skb_reserve(skb, cfv->rx_hr + pad_len);
245 memcpy(skb_put(skb, cfpkt_len), frm + cfv->rx_hr, cfpkt_len);
249 /* Get packets from the host vring */
250 static int cfv_rx_poll(struct napi_struct *napi, int quota)
252 struct cfv_info *cfv = container_of(napi, struct cfv_info, napi);
257 struct vringh_kiov *riov = &cfv->ctx.riov;
258 unsigned int skb_len;
264 /* Put the previous iovec back on the used ring and
265 * fetch a new iovec if we have processed all elements.
267 if (riov->i == riov->used) {
268 if (cfv->ctx.head != USHRT_MAX) {
269 vringh_complete_kern(cfv->vr_rx,
272 cfv->ctx.head = USHRT_MAX;
275 err = vringh_getdesc_kern(
286 buf = phys_to_virt((unsigned long) riov->iov[riov->i].iov_base);
287 /* TODO: Add check on valid buffer address */
289 skb = cfv_alloc_and_copy_skb(&err, cfv, buf,
290 riov->iov[riov->i].iov_len);
294 /* Push received packet up the stack. */
296 skb->protocol = htons(ETH_P_CAIF);
297 skb_reset_mac_header(skb);
298 skb->dev = cfv->ndev;
299 err = netif_receive_skb(skb);
301 ++cfv->ndev->stats.rx_dropped;
303 ++cfv->ndev->stats.rx_packets;
304 cfv->ndev->stats.rx_bytes += skb_len;
309 } while (rxcnt < quota);
311 ++cfv->stats.rx_napi_resched;
317 ++cfv->stats.rx_napi_complete;
319 /* Really out of patckets? (stolen from virtio_net)*/
321 if (unlikely(!vringh_notify_enable_kern(cfv->vr_rx)) &&
322 napi_schedule_prep(napi)) {
323 vringh_notify_disable_kern(cfv->vr_rx);
324 __napi_schedule(napi);
330 ++cfv->stats.rx_nomem;
332 /* Stop NAPI poll on OOM, we hope to be polled later */
334 vringh_notify_enable_kern(cfv->vr_rx);
338 /* We're doomed, any modem fault is fatal */
339 netdev_warn(cfv->ndev, "Bad ring, disable device\n");
340 cfv->ndev->stats.rx_dropped = riov->used - riov->i;
342 vringh_notify_disable_kern(cfv->vr_rx);
343 netif_carrier_off(cfv->ndev);
347 if (rxcnt && vringh_need_notify_kern(cfv->vr_rx) > 0)
348 vringh_notify(cfv->vr_rx);
352 static void cfv_recv(struct virtio_device *vdev, struct vringh *vr_rx)
354 struct cfv_info *cfv = vdev->priv;
356 ++cfv->stats.rx_kicks;
357 vringh_notify_disable_kern(cfv->vr_rx);
358 napi_schedule(&cfv->napi);
361 static void cfv_destroy_genpool(struct cfv_info *cfv)
364 dma_free_coherent(cfv->vdev->dev.parent->parent,
365 cfv->allocsz, cfv->alloc_addr,
370 gen_pool_free(cfv->genpool, cfv->reserved_mem,
372 gen_pool_destroy(cfv->genpool);
376 static int cfv_create_genpool(struct cfv_info *cfv)
380 /* dma_alloc can only allocate whole pages, and we need a more
381 * fine graned allocation so we use genpool. We ask for space needed
382 * by IP and a full ring. If the dma allcoation fails we retry with a
383 * smaller allocation size.
386 cfv->allocsz = (virtqueue_get_vring_size(cfv->vq_tx) *
387 (ETH_DATA_LEN + cfv->tx_hr + cfv->tx_tr) * 11)/10;
388 if (cfv->allocsz <= (num_possible_cpus() + 1) * cfv->ndev->mtu)
392 if (cfv->allocsz <= num_possible_cpus() * cfv->ndev->mtu) {
393 netdev_info(cfv->ndev, "Not enough device memory\n");
397 cfv->alloc_addr = dma_alloc_coherent(
398 cfv->vdev->dev.parent->parent,
399 cfv->allocsz, &cfv->alloc_dma,
404 cfv->allocsz = (cfv->allocsz * 3) >> 2;
407 netdev_dbg(cfv->ndev, "Allocated %zd bytes from dma-memory\n",
410 /* Allocate on 128 bytes boundaries (1 << 7)*/
411 cfv->genpool = gen_pool_create(7, -1);
415 err = gen_pool_add_virt(cfv->genpool, (unsigned long)cfv->alloc_addr,
416 (phys_addr_t)virt_to_phys(cfv->alloc_addr),
421 /* Reserve some memory for low memory situations. If we hit the roof
422 * in the memory pool, we stop TX flow and release the reserve.
424 cfv->reserved_size = num_possible_cpus() * cfv->ndev->mtu;
425 cfv->reserved_mem = gen_pool_alloc(cfv->genpool,
427 if (!cfv->reserved_mem) {
432 cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx);
435 cfv_destroy_genpool(cfv);
439 /* Enable the CAIF interface and allocate the memory-pool */
440 static int cfv_netdev_open(struct net_device *netdev)
442 struct cfv_info *cfv = netdev_priv(netdev);
444 if (cfv_create_genpool(cfv))
447 netif_carrier_on(netdev);
448 napi_enable(&cfv->napi);
450 /* Schedule NAPI to read any pending packets */
451 napi_schedule(&cfv->napi);
455 /* Disable the CAIF interface and free the memory-pool */
456 static int cfv_netdev_close(struct net_device *netdev)
458 struct cfv_info *cfv = netdev_priv(netdev);
460 struct buf_info *buf_info;
462 /* Disable interrupts, queues and NAPI polling */
463 netif_carrier_off(netdev);
464 virtqueue_disable_cb(cfv->vq_tx);
465 vringh_notify_disable_kern(cfv->vr_rx);
466 napi_disable(&cfv->napi);
468 /* Release any TX buffers on both used and avilable rings */
469 cfv_release_used_buf(cfv->vq_tx);
470 spin_lock_irqsave(&cfv->tx_lock, flags);
471 while ((buf_info = virtqueue_detach_unused_buf(cfv->vq_tx)))
472 free_buf_info(cfv, buf_info);
473 spin_unlock_irqrestore(&cfv->tx_lock, flags);
475 /* Release all dma allocated memory and destroy the pool */
476 cfv_destroy_genpool(cfv);
480 /* Allocate a buffer in dma-memory and copy skb to it */
481 static struct buf_info *cfv_alloc_and_copy_to_shm(struct cfv_info *cfv,
483 struct scatterlist *sg)
485 struct caif_payload_info *info = (void *)&skb->cb;
486 struct buf_info *buf_info = NULL;
492 if (unlikely(cfv->tx_hr + skb->len + cfv->tx_tr > cfv->mtu)) {
493 netdev_warn(cfv->ndev, "Invalid packet len (%d > %d)\n",
494 cfv->tx_hr + skb->len + cfv->tx_tr, cfv->mtu);
498 buf_info = kmalloc(sizeof(struct buf_info), GFP_ATOMIC);
499 if (unlikely(!buf_info))
502 /* Make the IP header aligned in tbe buffer */
503 hdr_ofs = cfv->tx_hr + info->hdr_len;
504 pad_len = hdr_ofs & (IP_HDR_ALIGN - 1);
505 buf_info->size = cfv->tx_hr + skb->len + cfv->tx_tr + pad_len;
507 /* allocate dma memory buffer */
508 buf_info->vaddr = (void *)gen_pool_alloc(cfv->genpool, buf_info->size);
509 if (unlikely(!buf_info->vaddr))
512 /* copy skbuf contents to send buffer */
513 skb_copy_bits(skb, 0, buf_info->vaddr + cfv->tx_hr + pad_len, skb->len);
514 sg_init_one(sg, buf_info->vaddr + pad_len,
515 skb->len + cfv->tx_hr + cfv->rx_hr);
523 /* Put the CAIF packet on the virtio ring and kick the receiver */
524 static int cfv_netdev_tx(struct sk_buff *skb, struct net_device *netdev)
526 struct cfv_info *cfv = netdev_priv(netdev);
527 struct buf_info *buf_info;
528 struct scatterlist sg;
530 bool flow_off = false;
533 /* garbage collect released buffers */
534 cfv_release_used_buf(cfv->vq_tx);
535 spin_lock_irqsave(&cfv->tx_lock, flags);
537 /* Flow-off check takes into account number of cpus to make sure
538 * virtqueue will not be overfilled in any possible smp conditions.
540 * Flow-on is triggered when sufficient buffers are freed
542 if (unlikely(cfv->vq_tx->num_free <= num_present_cpus())) {
544 cfv->stats.tx_full_ring++;
547 /* If we run out of memory, we release the memory reserve and retry
550 buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
551 if (unlikely(!buf_info)) {
552 cfv->stats.tx_no_mem++;
555 if (cfv->reserved_mem && cfv->genpool) {
556 gen_pool_free(cfv->genpool, cfv->reserved_mem,
558 cfv->reserved_mem = 0;
559 buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
563 if (unlikely(flow_off)) {
564 /* Turn flow on when a 1/4 of the descriptors are released */
565 cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx) / 4;
566 /* Enable notifications of recycled TX buffers */
567 virtqueue_enable_cb(cfv->vq_tx);
568 netif_tx_stop_all_queues(netdev);
571 if (unlikely(!buf_info)) {
572 /* If the memory reserve does it's job, this shouldn't happen */
573 netdev_warn(cfv->ndev, "Out of gen_pool memory\n");
577 ret = virtqueue_add_outbuf(cfv->vq_tx, &sg, 1, buf_info, GFP_ATOMIC);
578 if (unlikely((ret < 0))) {
579 /* If flow control works, this shouldn't happen */
580 netdev_warn(cfv->ndev, "Failed adding buffer to TX vring:%d\n",
585 /* update netdev statistics */
586 cfv->ndev->stats.tx_packets++;
587 cfv->ndev->stats.tx_bytes += skb->len;
588 spin_unlock_irqrestore(&cfv->tx_lock, flags);
590 /* tell the remote processor it has a pending message to read */
591 virtqueue_kick(cfv->vq_tx);
596 spin_unlock_irqrestore(&cfv->tx_lock, flags);
597 cfv->ndev->stats.tx_dropped++;
598 free_buf_info(cfv, buf_info);
603 static void cfv_tx_release_tasklet(unsigned long drv)
605 struct cfv_info *cfv = (struct cfv_info *)drv;
606 cfv_release_used_buf(cfv->vq_tx);
609 static const struct net_device_ops cfv_netdev_ops = {
610 .ndo_open = cfv_netdev_open,
611 .ndo_stop = cfv_netdev_close,
612 .ndo_start_xmit = cfv_netdev_tx,
615 static void cfv_netdev_setup(struct net_device *netdev)
617 netdev->netdev_ops = &cfv_netdev_ops;
618 netdev->type = ARPHRD_CAIF;
619 netdev->tx_queue_len = 100;
620 netdev->flags = IFF_POINTOPOINT | IFF_NOARP;
621 netdev->mtu = CFV_DEF_MTU_SIZE;
622 netdev->destructor = free_netdev;
625 /* Create debugfs counters for the device */
626 static inline void debugfs_init(struct cfv_info *cfv)
629 debugfs_create_dir(netdev_name(cfv->ndev), NULL);
631 if (IS_ERR(cfv->debugfs))
634 debugfs_create_u32("rx-napi-complete", S_IRUSR, cfv->debugfs,
635 &cfv->stats.rx_napi_complete);
636 debugfs_create_u32("rx-napi-resched", S_IRUSR, cfv->debugfs,
637 &cfv->stats.rx_napi_resched);
638 debugfs_create_u32("rx-nomem", S_IRUSR, cfv->debugfs,
639 &cfv->stats.rx_nomem);
640 debugfs_create_u32("rx-kicks", S_IRUSR, cfv->debugfs,
641 &cfv->stats.rx_kicks);
642 debugfs_create_u32("tx-full-ring", S_IRUSR, cfv->debugfs,
643 &cfv->stats.tx_full_ring);
644 debugfs_create_u32("tx-no-mem", S_IRUSR, cfv->debugfs,
645 &cfv->stats.tx_no_mem);
646 debugfs_create_u32("tx-kicks", S_IRUSR, cfv->debugfs,
647 &cfv->stats.tx_kicks);
648 debugfs_create_u32("tx-flow-on", S_IRUSR, cfv->debugfs,
649 &cfv->stats.tx_flow_on);
652 /* Setup CAIF for the a virtio device */
653 static int cfv_probe(struct virtio_device *vdev)
655 vq_callback_t *vq_cbs = cfv_release_cb;
656 vrh_callback_t *vrh_cbs = cfv_recv;
657 const char *names = "output";
658 const char *cfv_netdev_name = "cfvrt";
659 struct net_device *netdev;
660 struct cfv_info *cfv;
663 netdev = alloc_netdev(sizeof(struct cfv_info), cfv_netdev_name,
668 cfv = netdev_priv(netdev);
672 spin_lock_init(&cfv->tx_lock);
674 /* Get the RX virtio ring. This is a "host side vring". */
676 if (!vdev->vringh_config || !vdev->vringh_config->find_vrhs)
679 err = vdev->vringh_config->find_vrhs(vdev, 1, &cfv->vr_rx, &vrh_cbs);
683 /* Get the TX virtio ring. This is a "guest side vring". */
684 err = vdev->config->find_vqs(vdev, 1, &cfv->vq_tx, &vq_cbs, &names);
688 /* Get the CAIF configuration from virtio config space, if available */
689 if (vdev->config->get) {
690 virtio_cread(vdev, struct virtio_caif_transf_config, headroom,
692 virtio_cread(vdev, struct virtio_caif_transf_config, headroom,
694 virtio_cread(vdev, struct virtio_caif_transf_config, tailroom,
696 virtio_cread(vdev, struct virtio_caif_transf_config, tailroom,
698 virtio_cread(vdev, struct virtio_caif_transf_config, mtu,
700 virtio_cread(vdev, struct virtio_caif_transf_config, mtu,
703 cfv->tx_hr = CFV_DEF_HEADROOM;
704 cfv->rx_hr = CFV_DEF_HEADROOM;
705 cfv->tx_tr = CFV_DEF_TAILROOM;
706 cfv->rx_tr = CFV_DEF_TAILROOM;
707 cfv->mtu = CFV_DEF_MTU_SIZE;
708 cfv->mru = CFV_DEF_MTU_SIZE;
711 netdev->needed_headroom = cfv->tx_hr;
712 netdev->needed_tailroom = cfv->tx_tr;
714 /* Disable buffer release interrupts unless we have stopped TX queues */
715 virtqueue_disable_cb(cfv->vq_tx);
717 netdev->mtu = cfv->mtu - cfv->tx_tr;
720 /* Initialize NAPI poll context data */
721 vringh_kiov_init(&cfv->ctx.riov, NULL, 0);
722 cfv->ctx.head = USHRT_MAX;
723 netif_napi_add(netdev, &cfv->napi, cfv_rx_poll, CFV_DEFAULT_QUOTA);
725 tasklet_init(&cfv->tx_release_tasklet,
726 cfv_tx_release_tasklet,
729 /* Carrier is off until netdevice is opened */
730 netif_carrier_off(netdev);
732 /* register Netdev */
733 err = register_netdev(netdev);
735 dev_err(&vdev->dev, "Unable to register netdev (%d)\n", err);
743 netdev_warn(cfv->ndev, "CAIF Virtio probe failed:%d\n", err);
746 vdev->vringh_config->del_vrhs(cfv->vdev);
748 vdev->config->del_vqs(cfv->vdev);
753 static void cfv_remove(struct virtio_device *vdev)
755 struct cfv_info *cfv = vdev->priv;
758 dev_close(cfv->ndev);
761 tasklet_kill(&cfv->tx_release_tasklet);
762 debugfs_remove_recursive(cfv->debugfs);
764 vringh_kiov_cleanup(&cfv->ctx.riov);
765 vdev->config->reset(vdev);
766 vdev->vringh_config->del_vrhs(cfv->vdev);
768 vdev->config->del_vqs(cfv->vdev);
769 unregister_netdev(cfv->ndev);
772 static struct virtio_device_id id_table[] = {
773 { VIRTIO_ID_CAIF, VIRTIO_DEV_ANY_ID },
777 static unsigned int features[] = {
780 static struct virtio_driver caif_virtio_driver = {
781 .feature_table = features,
782 .feature_table_size = ARRAY_SIZE(features),
783 .driver.name = KBUILD_MODNAME,
784 .driver.owner = THIS_MODULE,
785 .id_table = id_table,
787 .remove = cfv_remove,
790 module_virtio_driver(caif_virtio_driver);
791 MODULE_DEVICE_TABLE(virtio, id_table);