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