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