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 <assert.h> 14#include <errno.h> 15#include <stdbool.h> 16#include <stddef.h> 17#include <string.h> 18#include <virtqueue.h> 19 20#include <camkes/io.h> 21#include <camkes/msgqueue.h> 22#include <camkes/msgqueue_template.h> 23#include <camkes/virtqueue.h> 24#include <sel4/sel4.h> 25#include <utils/util.h> 26 27#include "virtqueue_common.h" 28 29int camkes_msgqueue_sender_init(int msgqueue_id, camkes_msgqueue_sender_t *sender) 30{ 31 if (!sender) { 32 ZF_LOGE("sender is NULL"); 33 return EINVAL; 34 } 35 36 if (sender->initialised) { 37 return 0; 38 } 39 40 camkes_msgqueue_channel_t *msgqueue_channel = camkes_msgqueue_channel_get(msgqueue_id, MSGQUEUE_SENDER); 41 if (!msgqueue_channel) { 42 ZF_LOGE("Failed to retrieve an initialised sender channel with ID %d", msgqueue_id); 43 return EINVAL; 44 } 45 46 size_t aligned_message_size = msgqueue_channel->message_size; 47 if (!IS_POWER_OF_2(msgqueue_channel->message_size)) { 48 aligned_message_size = NEXT_POWER_OF_2(msgqueue_channel->message_size); 49 } 50 51 int error = camkes_virtqueue_driver_init_common(&sender->sender_channel, msgqueue_channel->buffer, 52 msgqueue_channel->queue_len, 53 msgqueue_channel->buffer_size, msgqueue_channel->sender_funcs.notify, 54 aligned_message_size); 55 if (error) { 56 return error; 57 } 58 59 sender->message_size = msgqueue_channel->message_size; 60 sender->initialised = true; 61 62 return 0; 63} 64 65int camkes_msgqueue_receiver_init(int msgqueue_id, camkes_msgqueue_receiver_t *receiver) 66{ 67 if (!receiver) { 68 ZF_LOGE("receiver is NULL"); 69 return EINVAL; 70 } 71 72 if (receiver->initialised) { 73 return 0; 74 } 75 76 camkes_msgqueue_channel_t *msgqueue_channel = camkes_msgqueue_channel_get(msgqueue_id, MSGQUEUE_RECEIVER); 77 if (!msgqueue_channel) { 78 ZF_LOGE("Failed to retrieve an initialised receiver channel with ID %d", msgqueue_id); 79 return EINVAL; 80 } 81 82 int error = camkes_virtqueue_device_init_common(&receiver->receiver_channel, 83 msgqueue_channel->buffer, msgqueue_channel->queue_len, NULL); 84 if (error) { 85 return error; 86 } 87 88 receiver->message_size = msgqueue_channel->message_size; 89 receiver->poll = msgqueue_channel->receiver_funcs.poll; 90 receiver->wait = msgqueue_channel->receiver_funcs.wait; 91 receiver->initialised = true; 92 93 return 0; 94} 95 96static void *msgqueue_alloc_buffer(camkes_msgqueue_sender_t *sender) 97{ 98 void *ret_buffer = NULL; 99 virtqueue_ring_object_t handle = {0}; 100 unsigned len = 0; 101 UNUSED unsigned size = 0; 102 UNUSED vq_flags_t flag = {0}; 103 int error = 0; 104 105 /* Grab a used buffer if possible */ 106 if (virtqueue_get_used_buf(&sender->sender_channel, &handle, &len)) { 107 error = camkes_virtqueue_driver_gather_buffer(&sender->sender_channel, &handle, 108 (void **) &ret_buffer, &size, &flag); 109 ZF_LOGF_IF(error, "Failed to get a 'used' buffer even though there exists one"); 110 return ret_buffer; 111 } 112 113 /* Allocate a fresh buffer */ 114 error = camkes_virtqueue_buffer_alloc(&sender->sender_channel, &ret_buffer, sender->message_size); 115 if (error) { 116 return NULL; 117 } 118 119 return ret_buffer; 120} 121 122int camkes_msgqueue_send(camkes_msgqueue_sender_t *sender, void *message, size_t message_size) 123{ 124 if (!sender || !message) { 125 return EINVAL; 126 } 127 128 if (!sender->initialised) { 129 return EINVAL; 130 } 131 132 if (message_size > sender->message_size) { 133 ZF_LOGE("Message size is too large for this msgqueue"); 134 return EMSGSIZE; 135 } 136 137 /* Grab a buffer from the channel */ 138 void *msgqueue_buf = msgqueue_alloc_buffer(sender); 139 if (!msgqueue_buf) { 140 return ENOMEM; 141 } 142 143 /* Copy the contents of the received buffer */ 144 memcpy((void *) msgqueue_buf, message, message_size); 145 146 /* Now enqueue it on the virtqueue */ 147 int error = camkes_virtqueue_driver_send_buffer(&sender->sender_channel, msgqueue_buf, message_size); 148 if (error) { 149 memset((void *) msgqueue_buf, 0, message_size); 150 camkes_virtqueue_buffer_free(&sender->sender_channel, msgqueue_buf); 151 return ECOMM; 152 } 153 154 sender->sender_channel.notify(); 155 156 return 0; 157} 158 159int camkes_msgqueue_poll(camkes_msgqueue_receiver_t *receiver) 160{ 161 if (!receiver) { 162 return EINVAL; 163 } 164 165 if (!receiver->initialised) { 166 return EINVAL; 167 } 168 169 return receiver->poll(); 170} 171 172int camkes_msgqueue_wait(camkes_msgqueue_receiver_t *receiver) 173{ 174 if (!receiver) { 175 return EINVAL; 176 } 177 178 if (!receiver->initialised) { 179 return EINVAL; 180 } 181 182 receiver->wait(); 183 184 return 0; 185} 186 187int camkes_msgqueue_get(camkes_msgqueue_receiver_t *receiver, void *buffer, size_t buffer_size) 188{ 189 if (!receiver || !buffer) { 190 return EINVAL; 191 } 192 193 if (!receiver->initialised) { 194 return EINVAL; 195 } 196 197 if (buffer_size < receiver->message_size) { 198 ZF_LOGE("Buffer is too small for the message"); 199 return EMSGSIZE; 200 } 201 202 /* Try to get a message from the queue */ 203 virtqueue_ring_object_t handle = {0}; 204 205 if (!virtqueue_get_available_buf(&receiver->receiver_channel, &handle)) { 206 return ENOMSG; 207 } 208 209 void *message = NULL; 210 unsigned message_len = 0; 211 UNUSED vq_flags_t flags = {0}; 212 213 int error = camkes_virtqueue_device_gather_buffer(&receiver->receiver_channel, &handle, &message, &message_len, &flags); 214 ZF_LOGF_IF(error, "Failed to dequeue a message from the queue even though there is one"); 215 216 /* Copy it over and then add the buffer to the used ring */ 217 assert(buffer_size >= message_len); 218 memcpy(buffer, message, message_len); 219 220 virtqueue_add_used_buf(&receiver->receiver_channel, &handle, message_len); 221 222 return 0; 223} 224