1/* 2 * Copyright 2019, Data61 3 * Commonwealth Scientific and Industrial Research Organisation (CSIRO) 4 * ABN 41 687 119 230. 5 * 6 * This software may be distributed and modified according to the terms of 7 * the BSD 2-Clause license. Note that NO WARRANTY is provided. 8 * See "LICENSE_BSD2.txt" for details. 9 * 10 * @TAG(DATA61_BSD) 11 */ 12 13#include <utils/util.h> 14#include <virtqueue.h> 15 16void virtqueue_init_driver(virtqueue_driver_t *vq, unsigned queue_len, vq_vring_avail_t *avail_ring, 17 vq_vring_used_t *used_ring, vq_vring_desc_t *desc, void (*notify)(void), 18 void *cookie) 19{ 20 if (!IS_POWER_OF_2(queue_len)) { 21 ZF_LOGE("Invalid queue_len: %d, must be a power of 2.", queue_len); 22 } 23 vq->free_desc_head = 0; 24 vq->queue_len = queue_len; 25 vq->u_ring_last_seen = vq->queue_len - 1; 26 vq->avail_ring = avail_ring; 27 vq->used_ring = used_ring; 28 vq->desc_table = desc; 29 vq->notify = notify; 30 vq->cookie = cookie; 31 virtqueue_init_desc_table(desc, vq->queue_len); 32 virtqueue_init_avail_ring(avail_ring); 33 virtqueue_init_used_ring(used_ring); 34 35} 36 37void virtqueue_init_device(virtqueue_device_t *vq, unsigned queue_len, vq_vring_avail_t *avail_ring, 38 vq_vring_used_t *used_ring, vq_vring_desc_t *desc, void (*notify)(void), 39 void *cookie) 40{ 41 if (!IS_POWER_OF_2(queue_len)) { 42 ZF_LOGE("Invalid queue_len: %d, must be a power of 2.", queue_len); 43 } 44 vq->queue_len = queue_len; 45 vq->a_ring_last_seen = vq->queue_len - 1; 46 vq->avail_ring = avail_ring; 47 vq->used_ring = used_ring; 48 vq->desc_table = desc; 49 vq->notify = notify; 50 vq->cookie = cookie; 51} 52 53void virtqueue_init_desc_table(vq_vring_desc_t *table, unsigned queue_len) 54{ 55 unsigned i; 56 for (i = 0; i < queue_len; i++) { 57 table[i].addr = 0; 58 table[i].len = 0; 59 table[i].flags = 0; 60 table[i].next = i + 1; 61 } 62} 63 64void virtqueue_init_avail_ring(vq_vring_avail_t *ring) 65{ 66 ring->flags = 0; 67 ring->idx = 0; 68} 69 70void virtqueue_init_used_ring(vq_vring_used_t *ring) 71{ 72 ring->flags = 0; 73 ring->idx = 0; 74} 75 76static unsigned vq_add_desc(virtqueue_driver_t *vq, void *buf, unsigned len, 77 vq_flags_t flag, int prev) 78{ 79 unsigned new; 80 vq_vring_desc_t *desc; 81 82 new = vq->free_desc_head; 83 if (new == vq->queue_len) { 84 return new; 85 } 86 vq->free_desc_head = vq->desc_table[new].next; 87 desc = vq->desc_table + new; 88 89 // casting pointers to integers directly is not allowed, must cast the 90 // pointer to a uintptr_t first 91 desc->addr = (uintptr_t)buf; 92 93 desc->len = len; 94 desc->flags = flag; 95 desc->next = vq->queue_len; 96 97 if (prev < vq->queue_len) { 98 desc = vq->desc_table + prev; 99 desc->next = new; 100 } 101 return new; 102} 103 104static unsigned vq_pop_desc(virtqueue_driver_t *vq, unsigned idx, 105 void **buf, unsigned *len, vq_flags_t *flag) 106{ 107 unsigned next = vq->desc_table[idx].next; 108 109 // casting integers to pointers directly is not allowed, must cast the 110 // integer to a uintptr_t first 111 *buf = (void *)(uintptr_t)(vq->desc_table[idx].addr); 112 113 *len = vq->desc_table[idx].len; 114 *flag = vq->desc_table[idx].flags; 115 vq->desc_table[idx].next = vq->free_desc_head; 116 vq->free_desc_head = idx; 117 118 return next; 119} 120 121int virtqueue_add_available_buf(virtqueue_driver_t *vq, virtqueue_ring_object_t *obj, 122 void *buf, unsigned len, vq_flags_t flag) 123{ 124 unsigned idx; 125 126 /* If descriptor table full */ 127 if ((idx = vq_add_desc(vq, buf, len, flag, obj->cur)) == vq->queue_len) { 128 return 0; 129 } 130 obj->cur = idx; 131 132 /* If this is the first buffer in the descriptor chain */ 133 if (obj->first >= vq->queue_len) { 134 obj->first = idx; 135 vq->avail_ring->ring[vq->avail_ring->idx] = idx; 136 vq->avail_ring->idx = (vq->avail_ring->idx + 1) & (vq->queue_len - 1); 137 } 138 return 1; 139} 140 141int virtqueue_get_used_buf(virtqueue_driver_t *vq, virtqueue_ring_object_t *obj, uint32_t *len) 142{ 143 unsigned next = (vq->u_ring_last_seen + 1) & (vq->queue_len - 1); 144 145 if (next == vq->used_ring->idx) { 146 return 0; 147 } 148 obj->first = vq->used_ring->ring[next].id; 149 *len = vq->used_ring->ring[next].len; 150 obj->cur = obj->first; 151 vq->u_ring_last_seen = next; 152 return 1; 153} 154 155int virtqueue_add_used_buf(virtqueue_device_t *vq, virtqueue_ring_object_t *robj, uint32_t len) 156{ 157 unsigned cur = vq->used_ring->idx; 158 159 vq->used_ring->ring[cur].id = robj->first; 160 vq->used_ring->ring[cur].len = len; 161 vq->used_ring->idx = (cur + 1) & (vq->queue_len - 1); 162 return 1; 163} 164 165int virtqueue_get_available_buf(virtqueue_device_t *vq, virtqueue_ring_object_t *robj) 166{ 167 unsigned next = (vq->a_ring_last_seen + 1) & (vq->queue_len - 1); 168 169 if (next == vq->avail_ring->idx) { 170 return 0; 171 } 172 robj->first = vq->avail_ring->ring[next]; 173 robj->cur = robj->first; 174 vq->a_ring_last_seen = next; 175 return 1; 176} 177 178void virtqueue_init_ring_object(virtqueue_ring_object_t *obj) 179{ 180 obj->cur = (uint32_t) -1; 181 obj->first = (uint32_t) -1; 182} 183 184uint32_t virtqueue_scattered_available_size(virtqueue_device_t *vq, virtqueue_ring_object_t *robj) 185{ 186 uint32_t ret = 0; 187 unsigned cur = robj->first; 188 189 while (cur < vq->queue_len) { 190 ret += vq->desc_table[cur].len; 191 cur = vq->desc_table[cur].next; 192 } 193 return ret; 194} 195 196int virtqueue_gather_available(virtqueue_device_t *vq, virtqueue_ring_object_t *robj, 197 void **buf, unsigned *len, vq_flags_t *flag) 198{ 199 unsigned idx = robj->cur; 200 201 if (idx >= vq->queue_len) { 202 return 0; 203 } 204 205 // casting integers to pointers directly is not allowed, must cast the 206 // integer to a uintptr_t first 207 *buf = (void *)(uintptr_t)(vq->desc_table[idx].addr); 208 209 *len = vq->desc_table[idx].len; 210 *flag = vq->desc_table[idx].flags; 211 robj->cur = vq->desc_table[idx].next; 212 return 1; 213} 214 215int virtqueue_gather_used(virtqueue_driver_t *vq, virtqueue_ring_object_t *robj, 216 void **buf, unsigned *len, vq_flags_t *flag) 217{ 218 if (robj->cur >= vq->queue_len) { 219 return 0; 220 } 221 robj->cur = vq_pop_desc(vq, robj->cur, buf, len, flag); 222 return 1; 223} 224