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