drivers/virtio/virtio_ring.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2018, Tuomas Tynkkynen <tuomas.tynkkynen@iki.fi>
   4  * Copyright (C) 2018, Bin Meng <bmeng.cn@gmail.com>
   5  *
   6  * virtio ring implementation
   7  */
   8
   9 #include <common.h>
  10 #include <dm.h>
  11 #include <malloc.h>
  12 #include <virtio_types.h>
  13 #include <virtio.h>
  14 #include <virtio_ring.h>
  15 #include <linux/compat.h>
  16
  17 int virtqueue_add(struct virtqueue *vq, struct virtio_sg *sgs[],
  18                   unsigned int out_sgs, unsigned int in_sgs)
  19 {
  20         struct vring_desc *desc;
  21         unsigned int total_sg = out_sgs + in_sgs;
  22         unsigned int i, n, avail, descs_used, uninitialized_var(prev);
  23         int head;
  24
  25         WARN_ON(total_sg == 0);
  26
  27         head = vq->free_head;
  28
  29         desc = vq->vring.desc;
  30         i = head;
  31         descs_used = total_sg;
  32
  33         if (vq->num_free < descs_used) {
  34                 debug("Can't add buf len %i - avail = %i\n",
  35                       descs_used, vq->num_free);
  36                 /*
  37                  * FIXME: for historical reasons, we force a notify here if
  38                  * there are outgoing parts to the buffer.  Presumably the
  39                  * host should service the ring ASAP.
  40                  */
  41                 if (out_sgs)
  42                         virtio_notify(vq->vdev, vq);
  43                 return -ENOSPC;
  44         }
  45
  46         for (n = 0; n < out_sgs; n++) {
  47                 struct virtio_sg *sg = sgs[n];
  48
  49                 desc[i].flags = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT);
  50                 desc[i].addr = cpu_to_virtio64(vq->vdev, (u64)(size_t)sg->addr);
  51                 desc[i].len = cpu_to_virtio32(vq->vdev, sg->length);
  52
  53                 prev = i;
  54                 i = virtio16_to_cpu(vq->vdev, desc[i].next);
  55         }
  56         for (; n < (out_sgs + in_sgs); n++) {
  57                 struct virtio_sg *sg = sgs[n];
  58
  59                 desc[i].flags = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT |
  60                                                 VRING_DESC_F_WRITE);
  61                 desc[i].addr = cpu_to_virtio64(vq->vdev,
  62                                                (u64)(uintptr_t)sg->addr);
  63                 desc[i].len = cpu_to_virtio32(vq->vdev, sg->length);
  64
  65                 prev = i;
  66                 i = virtio16_to_cpu(vq->vdev, desc[i].next);
  67         }
  68         /* Last one doesn't continue */
  69         desc[prev].flags &= cpu_to_virtio16(vq->vdev, ~VRING_DESC_F_NEXT);
  70
  71         /* We're using some buffers from the free list. */
  72         vq->num_free -= descs_used;
  73
  74         /* Update free pointer */
  75         vq->free_head = i;
  76
  77         /*
  78          * Put entry in available array (but don't update avail->idx
  79          * until they do sync).
  80          */
  81         avail = vq->avail_idx_shadow & (vq->vring.num - 1);
  82         vq->vring.avail->ring[avail] = cpu_to_virtio16(vq->vdev, head);
  83
  84         /*
  85          * Descriptors and available array need to be set before we expose the
  86          * new available array entries.
  87          */
  88         virtio_wmb();
  89         vq->avail_idx_shadow++;
  90         vq->vring.avail->idx = cpu_to_virtio16(vq->vdev, vq->avail_idx_shadow);
  91         vq->num_added++;
  92
  93         /*
  94          * This is very unlikely, but theoretically possible.
  95          * Kick just in case.
  96          */
  97         if (unlikely(vq->num_added == (1 << 16) - 1))
  98                 virtqueue_kick(vq);
  99
 100         return 0;
 101 }
 102
 103 static bool virtqueue_kick_prepare(struct virtqueue *vq)
 104 {
 105         u16 new, old;
 106         bool needs_kick;
 107
 108         /*
 109          * We need to expose available array entries before checking
 110          * avail event.
 111          */
 112         virtio_mb();
 113
 114         old = vq->avail_idx_shadow - vq->num_added;
 115         new = vq->avail_idx_shadow;
 116         vq->num_added = 0;
 117
 118         if (vq->event) {
 119                 needs_kick = vring_need_event(virtio16_to_cpu(vq->vdev,
 120                                 vring_avail_event(&vq->vring)), new, old);
 121         } else {
 122                 needs_kick = !(vq->vring.used->flags & cpu_to_virtio16(vq->vdev,
 123                                 VRING_USED_F_NO_NOTIFY));
 124         }
 125
 126         return needs_kick;
 127 }
 128
 129 void virtqueue_kick(struct virtqueue *vq)
 130 {
 131         if (virtqueue_kick_prepare(vq))
 132                 virtio_notify(vq->vdev, vq);
 133 }
 134
 135 static void detach_buf(struct virtqueue *vq, unsigned int head)
 136 {
 137         unsigned int i;
 138         __virtio16 nextflag = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT);
 139
 140         /* Put back on free list: unmap first-level descriptors and find end */
 141         i = head;
 142
 143         while (vq->vring.desc[i].flags & nextflag) {
 144                 i = virtio16_to_cpu(vq->vdev, vq->vring.desc[i].next);
 145                 vq->num_free++;
 146         }
 147
 148         vq->vring.desc[i].next = cpu_to_virtio16(vq->vdev, vq->free_head);
 149         vq->free_head = head;
 150
 151         /* Plus final descriptor */
 152         vq->num_free++;
 153 }
 154
 155 static inline bool more_used(const struct virtqueue *vq)
 156 {
 157         return vq->last_used_idx != virtio16_to_cpu(vq->vdev,
 158                         vq->vring.used->idx);
 159 }
 160
 161 void *virtqueue_get_buf(struct virtqueue *vq, unsigned int *len)
 162 {
 163         unsigned int i;
 164         u16 last_used;
 165
 166         if (!more_used(vq)) {
 167                 debug("(%s.%d): No more buffers in queue\n",
 168                       vq->vdev->name, vq->index);
 169                 return NULL;
 170         }
 171
 172         /* Only get used array entries after they have been exposed by host */
 173         virtio_rmb();
 174
 175         last_used = (vq->last_used_idx & (vq->vring.num - 1));
 176         i = virtio32_to_cpu(vq->vdev, vq->vring.used->ring[last_used].id);
 177         if (len) {
 178                 *len = virtio32_to_cpu(vq->vdev,
 179                                        vq->vring.used->ring[last_used].len);
 180                 debug("(%s.%d): last used idx %u with len %u\n",
 181                       vq->vdev->name, vq->index, i, *len);
 182         }
 183
 184         if (unlikely(i >= vq->vring.num)) {
 185                 printf("(%s.%d): id %u out of range\n",
 186                        vq->vdev->name, vq->index, i);
 187                 return NULL;
 188         }
 189
 190         detach_buf(vq, i);
 191         vq->last_used_idx++;
 192         /*
 193          * If we expect an interrupt for the next entry, tell host
 194          * by writing event index and flush out the write before
 195          * the read in the next get_buf call.
 196          */
 197         if (!(vq->avail_flags_shadow & VRING_AVAIL_F_NO_INTERRUPT))
 198                 virtio_store_mb(&vring_used_event(&vq->vring),
 199                                 cpu_to_virtio16(vq->vdev, vq->last_used_idx));
 200
 201         return (void *)(uintptr_t)virtio64_to_cpu(vq->vdev,
 202                                                   vq->vring.desc[i].addr);
 203 }
 204
 205 static struct virtqueue *__vring_new_virtqueue(unsigned int index,
 206                                                struct vring vring,
 207                                                struct udevice *udev)
 208 {
 209         unsigned int i;
 210         struct virtqueue *vq;
 211         struct virtio_dev_priv *uc_priv = dev_get_uclass_priv(udev);
 212         struct udevice *vdev = uc_priv->vdev;
 213
 214         vq = malloc(sizeof(*vq));
 215         if (!vq)
 216                 return NULL;
 217
 218         vq->vdev = vdev;
 219         vq->index = index;
 220         vq->num_free = vring.num;
 221         vq->vring = vring;
 222         vq->last_used_idx = 0;
 223         vq->avail_flags_shadow = 0;
 224         vq->avail_idx_shadow = 0;
 225         vq->num_added = 0;
 226         list_add_tail(&vq->list, &uc_priv->vqs);
 227
 228         vq->event = virtio_has_feature(vdev, VIRTIO_RING_F_EVENT_IDX);
 229
 230         /* Tell other side not to bother us */
 231         vq->avail_flags_shadow |= VRING_AVAIL_F_NO_INTERRUPT;
 232         if (!vq->event)
 233                 vq->vring.avail->flags = cpu_to_virtio16(vdev,
 234                                 vq->avail_flags_shadow);
 235
 236         /* Put everything in free lists */
 237         vq->free_head = 0;
 238         for (i = 0; i < vring.num - 1; i++)
 239                 vq->vring.desc[i].next = cpu_to_virtio16(vdev, i + 1);
 240
 241         return vq;
 242 }
 243
 244 struct virtqueue *vring_create_virtqueue(unsigned int index, unsigned int num,
 245                                          unsigned int vring_align,
 246                                          struct udevice *udev)
 247 {
 248         struct virtqueue *vq;
 249         void *queue = NULL;
 250         struct vring vring;
 251
 252         /* We assume num is a power of 2 */
 253         if (num & (num - 1)) {
 254                 printf("Bad virtqueue length %u\n", num);
 255                 return NULL;
 256         }
 257
 258         /* TODO: allocate each queue chunk individually */
 259         for (; num && vring_size(num, vring_align) > PAGE_SIZE; num /= 2) {
 260                 queue = memalign(PAGE_SIZE, vring_size(num, vring_align));
 261                 if (queue)
 262                         break;
 263         }
 264
 265         if (!num)
 266                 return NULL;
 267
 268         if (!queue) {
 269                 /* Try to get a single page. You are my only hope! */
 270                 queue = memalign(PAGE_SIZE, vring_size(num, vring_align));
 271         }
 272         if (!queue)
 273                 return NULL;
 274
 275         memset(queue, 0, vring_size(num, vring_align));
 276         vring_init(&vring, num, queue, vring_align);
 277
 278         vq = __vring_new_virtqueue(index, vring, udev);
 279         if (!vq) {
 280                 free(queue);
 281                 return NULL;
 282         }
 283         debug("(%s): created vring @ %p for vq @ %p with num %u\n", udev->name,
 284               queue, vq, num);
 285
 286         return vq;
 287 }
 288
 289 void vring_del_virtqueue(struct virtqueue *vq)
 290 {
 291         free(vq->vring.desc);
 292         list_del(&vq->list);
 293         free(vq);
 294 }
 295
 296 unsigned int virtqueue_get_vring_size(struct virtqueue *vq)
 297 {
 298         return vq->vring.num;
 299 }
 300
 301 ulong virtqueue_get_desc_addr(struct virtqueue *vq)
 302 {
 303         return (ulong)vq->vring.desc;
 304 }
 305
 306 ulong virtqueue_get_avail_addr(struct virtqueue *vq)
 307 {
 308         return (ulong)vq->vring.desc +
 309                ((char *)vq->vring.avail - (char *)vq->vring.desc);
 310 }
 311
 312 ulong virtqueue_get_used_addr(struct virtqueue *vq)
 313 {
 314         return (ulong)vq->vring.desc +
 315                ((char *)vq->vring.used - (char *)vq->vring.desc);
 316 }
 317
 318 bool virtqueue_poll(struct virtqueue *vq, u16 last_used_idx)
 319 {
 320         virtio_mb();
 321
 322         return last_used_idx != virtio16_to_cpu(vq->vdev, vq->vring.used->idx);
 323 }
 324
 325 void virtqueue_dump(struct virtqueue *vq)
 326 {
 327         unsigned int i;
 328
 329         printf("virtqueue %p for dev %s:\n", vq, vq->vdev->name);
 330         printf("\tindex %u, phys addr %p num %u\n",
 331                vq->index, vq->vring.desc, vq->vring.num);
 332         printf("\tfree_head %u, num_added %u, num_free %u\n",
 333                vq->free_head, vq->num_added, vq->num_free);
 334         printf("\tlast_used_idx %u, avail_flags_shadow %u, avail_idx_shadow %u\n",
 335                vq->last_used_idx, vq->avail_flags_shadow, vq->avail_idx_shadow);
 336
 337         printf("Descriptor dump:\n");
 338         for (i = 0; i < vq->vring.num; i++) {
 339                 printf("\tdesc[%u] = { 0x%llx, len %u, flags %u, next %u }\n",
 340                        i, vq->vring.desc[i].addr, vq->vring.desc[i].len,
 341                        vq->vring.desc[i].flags, vq->vring.desc[i].next);
 342         }
 343
 344         printf("Avail ring dump:\n");
 345         printf("\tflags %u, idx %u\n",
 346                vq->vring.avail->flags, vq->vring.avail->idx);
 347         for (i = 0; i < vq->vring.num; i++) {
 348                 printf("\tavail[%u] = %u\n",
 349                        i, vq->vring.avail->ring[i]);
 350         }
 351
 352         printf("Used ring dump:\n");
 353         printf("\tflags %u, idx %u\n",
 354                vq->vring.used->flags, vq->vring.used->idx);
 355         for (i = 0; i < vq->vring.num; i++) {
 356                 printf("\tused[%u] = { %u, %u }\n", i,
 357                        vq->vring.used->ring[i].id, vq->vring.used->ring[i].len);
 358         }
 359 }