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 <stdio.h>
14#include <string.h>
15#include <camkes/virtqueue.h>
16#include <utils/util.h>
17#include <platsupport/io.h>
18
19#include "virtqueue_common.h"
20
21#define BLOCK_SIZE 128
22#define IDX_TO_OFFSET(block_size, idx) (block_size * idx)
23#define OFFSET_TO_IDX(block_size, offset) ((offset) / block_size)
24
25int camkes_virtqueue_buffer_alloc(virtqueue_driver_t *virtqueue, void **buf, size_t alloc_size)
26{
27    if (!virtqueue) {
28        return -1;
29    }
30
31    struct vq_buf_alloc *allocator = virtqueue->cookie;
32
33    if (alloc_size > allocator->block_size) {
34        ZF_LOGE("Error: invalid alloc size");
35        return -1;
36    }
37    if (allocator->head == allocator->free_list_size) {
38        ZF_LOGE("Error: ran out of memory");
39        return -1;
40    }
41    *buf = allocator->buffer + IDX_TO_OFFSET(allocator->block_size, allocator->head);
42    allocator->head = allocator->free_list[allocator->head];
43    return 0;
44}
45
46void camkes_virtqueue_buffer_free(virtqueue_driver_t *virtqueue, void *buffer)
47{
48    if (!virtqueue) {
49        ZF_LOGE("virtqueue is NULL");
50        return;
51    }
52
53    struct vq_buf_alloc *allocator = virtqueue->cookie;
54    int idx = OFFSET_TO_IDX(allocator->block_size, (uintptr_t)buffer - (uintptr_t)(allocator->buffer));
55
56    if (idx < 0 || idx >= allocator->free_list_size) {
57        ZF_LOGE("Warning: invalid buffer");
58        return;
59    }
60    allocator->free_list[idx] = allocator->head;
61    allocator->head = idx;
62}
63
64static camkes_virtqueue_channel_t *get_virtqueue_channel(virtqueue_role_t role, unsigned int camkes_virtqueue_id)
65{
66    /* Check that the virtqueue id is in a valid range */
67    if (camkes_virtqueue_id > MAX_CAMKES_VIRTQUEUE_ID) {
68        return NULL;
69    }
70    /* Return error if the given virtqueue channel hasn't been initialized */
71    if (camkes_virtqueue_channels[camkes_virtqueue_id].role == VIRTQUEUE_UNASSIGNED) {
72        return NULL;
73    }
74    camkes_virtqueue_channel_t *channel = &camkes_virtqueue_channels[camkes_virtqueue_id];
75    /* Check that the buffer is not NULL */
76    if (channel->channel_buffer == NULL) {
77        return NULL;
78    }
79
80    if (channel->role != role) {
81        ZF_LOGE("role provided does not match role trying to bind to.");
82        return NULL;
83    }
84
85    return channel;
86}
87
88
89int camkes_virtqueue_get_id_from_name(const char *interface_name)
90{
91    for (int i = 0; i < MAX_CAMKES_VIRTQUEUE_ID; i++) {
92        if (strncmp(interface_name, camkes_virtqueue_channels[i].interface_name, 200) == 0) {
93            return i;
94        }
95    }
96    return -1;
97}
98
99int camkes_virtqueue_driver_init_with_recv(virtqueue_driver_t *driver, unsigned int camkes_virtqueue_id,
100                                           seL4_CPtr *recv_notification, seL4_CPtr *recv_badge)
101{
102    if (driver == NULL) {
103        return -1;
104    }
105
106    camkes_virtqueue_channel_t *channel = get_virtqueue_channel(VIRTQUEUE_DRIVER, camkes_virtqueue_id);
107    if (channel == NULL) {
108        ZF_LOGE("Failed to get channel");
109        return -1;
110    }
111
112    if (recv_notification != NULL) {
113        *recv_notification = channel->recv_notification;
114    }
115    if (recv_badge != NULL) {
116        *recv_badge = channel->recv_badge;
117    }
118
119    return camkes_virtqueue_driver_init_common(driver, channel->channel_buffer, channel->queue_len, channel->channel_buffer_size,
120                                               channel->notify, BLOCK_SIZE) ? -1 : 0;
121}
122
123int camkes_virtqueue_device_init_with_recv(virtqueue_device_t *device, unsigned int camkes_virtqueue_id,
124                                           seL4_CPtr *recv_notification, seL4_CPtr *recv_badge)
125{
126    if (device == NULL) {
127        return -1;
128    }
129
130    camkes_virtqueue_channel_t *channel = get_virtqueue_channel(VIRTQUEUE_DEVICE, camkes_virtqueue_id);
131    if (channel == NULL) {
132        ZF_LOGE("Failed to get channel");
133        return -1;
134    }
135
136    if (recv_notification != NULL) {
137        *recv_notification = channel->recv_notification;
138    }
139    if (recv_badge != NULL) {
140        *recv_badge = channel->recv_badge;
141    }
142
143    return camkes_virtqueue_device_init_common(device, channel->channel_buffer, channel->queue_len, channel->notify) ? -1 : 0;
144}
145
146void *camkes_virtqueue_device_offset_to_buffer(virtqueue_device_t *virtqueue, uintptr_t offset)
147{
148    return virtqueue->cookie + offset;
149}
150
151void *camkes_virtqueue_driver_offset_to_buffer(virtqueue_driver_t *virtqueue, uintptr_t offset)
152{
153    struct vq_buf_alloc *allocator = virtqueue->cookie;
154
155    return allocator->buffer + offset;
156}
157
158int camkes_virtqueue_driver_send_buffer(virtqueue_driver_t *vq, void *buffer, size_t size)
159{
160    uintptr_t base_offset = (uintptr_t)(((struct vq_buf_alloc *)vq->cookie)->buffer);
161    uintptr_t buf_offset = (uintptr_t)buffer - base_offset;
162    virtqueue_ring_object_t handle;
163
164    virtqueue_init_ring_object(&handle);
165    if (!virtqueue_add_available_buf(vq, &handle, (void *)buf_offset, size, VQ_RW)) {
166        ZF_LOGE("Error while enqueuing available buffer");
167        return -1;
168    }
169    return 0;
170}
171
172static int chain_vq_buf(virtqueue_driver_t *vq, virtqueue_ring_object_t *handle,
173                        void *buffer, size_t size)
174{
175    struct vq_buf_alloc *allocator = vq->cookie;
176    uintptr_t offset = (uintptr_t)buffer - (uintptr_t)(allocator->buffer);
177
178    if (!virtqueue_add_available_buf(vq, handle, (void *)offset, size, VQ_RW)) {
179        ZF_LOGE("Error while chaining available buffer");
180        return -1;
181    }
182    return 0;
183}
184
185int camkes_virtqueue_driver_scatter_send_buffer(virtqueue_driver_t *vq, void *buffer, size_t size)
186{
187    size_t sent = 0;
188    virtqueue_ring_object_t handle;
189    virtqueue_init_ring_object(&handle);
190
191    while (sent < size) {
192        void *vq_buf = NULL;
193        size_t to_send = 0;
194
195        to_send = size - sent < BLOCK_SIZE ? size - sent : BLOCK_SIZE;
196        if (camkes_virtqueue_buffer_alloc(vq, &vq_buf, to_send)) {
197            ZF_LOGE("Error: could not allocate virtqueue buffer");
198            return -1;
199        }
200        if (buffer) {
201            memcpy(vq_buf, buffer + sent, to_send);
202        }
203        if (chain_vq_buf(vq, &handle, vq_buf, to_send)) {
204            return -1;
205        }
206        sent += to_send;
207    }
208    return 0;
209}
210
211int camkes_virtqueue_driver_gather_copy_buffer(virtqueue_driver_t *vq, virtqueue_ring_object_t *handle,
212                                               void *buffer, size_t size)
213{
214    size_t copied = 0;
215    void *used_buf;
216    unsigned buf_size;
217    vq_flags_t flag;
218
219    while (camkes_virtqueue_driver_gather_buffer(vq, handle, &used_buf, &buf_size, &flag) == 0) {
220        size_t to_copy = copied + buf_size > size ? size - copied : buf_size;
221
222        if (to_copy) {
223            memcpy(buffer + copied, used_buf, to_copy);
224        }
225        copied += to_copy;
226        camkes_virtqueue_buffer_free(vq, used_buf);
227    }
228    return 0;
229}
230
231int camkes_virtqueue_device_scatter_copy_buffer(virtqueue_device_t *vq, virtqueue_ring_object_t *handle,
232                                                void *buffer, size_t size)
233{
234    size_t sent = 0;
235
236    while (sent < size) {
237        void *avail_buf;
238        unsigned buf_size;
239        size_t to_copy;
240        vq_flags_t flag;
241
242        if (camkes_virtqueue_device_gather_buffer(vq, handle, &avail_buf, &buf_size, &flag)) {
243            virtqueue_add_used_buf(vq, handle, sent);
244            return -1;
245        }
246        to_copy = size - sent < buf_size ? size - sent : buf_size;
247        memcpy(avail_buf, buffer + sent, to_copy);
248        sent += to_copy;
249    }
250    virtqueue_add_used_buf(vq, handle, sent);
251    return 0;
252}
253
254int camkes_virtqueue_device_gather_copy_buffer(virtqueue_device_t *vq, virtqueue_ring_object_t *handle,
255                                               void *buffer, size_t size)
256{
257    size_t sent = 0;
258
259    while (sent < size) {
260        void *avail_buf;
261        unsigned buf_size;
262        size_t to_copy;
263        vq_flags_t flag;
264
265        if (camkes_virtqueue_device_gather_buffer(vq, handle, &avail_buf, &buf_size, &flag)) {
266            virtqueue_add_used_buf(vq, handle, sent);
267            return -1;
268        }
269        to_copy = size - sent < buf_size ? size - sent : buf_size;
270        memcpy(buffer + sent, avail_buf, to_copy);
271        sent += to_copy;
272    }
273    virtqueue_add_used_buf(vq, handle, sent);
274    return 0;
275}
276
277int camkes_virtqueue_driver_gather_buffer(virtqueue_driver_t *vq, virtqueue_ring_object_t *handle,
278                                          void **buffer, unsigned *size, vq_flags_t *flag)
279{
280    uintptr_t buf_offset;
281    if (!virtqueue_gather_used(vq, handle, (void **)&buf_offset, size, flag)) {
282        return -1;
283    }
284    *buffer = camkes_virtqueue_driver_offset_to_buffer(vq, (uintptr_t) buf_offset);
285    return 0;
286}
287
288int camkes_virtqueue_device_gather_buffer(virtqueue_device_t *vq, virtqueue_ring_object_t *handle,
289                                          void **buffer, unsigned *size, vq_flags_t *flag)
290{
291    uintptr_t buf_offset;
292    if (!virtqueue_gather_available(vq, handle, (void **)&buf_offset, size, flag)) {
293        return -1;
294    }
295    *buffer = camkes_virtqueue_device_offset_to_buffer(vq, (uintptr_t) buf_offset);
296    return 0;
297}
298
299int camkes_virtqueue_channel_num(void)
300{
301    /* Return number of registered virtqueue channels */
302    return num_registered_virtqueue_channels;
303}
304