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 <errno.h> 14#include <camkes/msgqueue_template.h> 15#include <utils/util.h> 16 17camkes_msgqueue_channel_t camkes_msgqueue_channels[MAX_CAMKES_MSGQUEUE_ID]; 18int num_registered_msgqueue_channels; 19 20static inline int msgqueue_channel_check_common(int msgqueue_id, void *buffer, size_t buffer_size, 21 size_t message_size) 22{ 23 if (msgqueue_id >= MAX_CAMKES_MSGQUEUE_ID) { 24 ZF_LOGE("Supplied msgqueue_id is greater or equal to the maximum number of IDs"); 25 return -EINVAL; 26 } 27 28 if (num_registered_msgqueue_channels >= MAX_CAMKES_MSGQUEUE_ID) { 29 ZF_LOGE("Trying to register more msgqueue channels than allowed"); 30 return -EINVAL; 31 } 32 33 if (camkes_msgqueue_channels[msgqueue_id].role != MSGQUEUE_UNASSIGNED) { 34 ZF_LOGE("Trying to overwrite an initialised msgqueue channel"); 35 return -EINVAL; 36 } 37 38 if (!buffer) { 39 ZF_LOGE("Trying to register a msgqueue channel with an empty buffer"); 40 return -EINVAL; 41 } 42 43 if (message_size >= buffer_size) { 44 ZF_LOGE("The mesasage size of the channel is greater than the size of the buffer"); 45 return -EINVAL; 46 } 47 48 return 0; 49} 50 51static inline void msgqueue_channel_init_common(int msgqueue_id, void *buffer, unsigned queue_len, 52 size_t buffer_size, size_t message_size) 53{ 54 camkes_msgqueue_channels[msgqueue_id] = (camkes_msgqueue_channel_t) { 55 0 56 }; 57 camkes_msgqueue_channels[msgqueue_id].buffer = buffer; 58 camkes_msgqueue_channels[msgqueue_id].queue_len = queue_len; 59 camkes_msgqueue_channels[msgqueue_id].buffer_size = buffer_size; 60 camkes_msgqueue_channels[msgqueue_id].message_size = message_size; 61} 62 63int camkes_msgqueue_channel_register_sender(int msgqueue_id, void *buffer, unsigned queue_len, 64 size_t buffer_size, size_t message_size, void (*notify)(void)) 65{ 66 int res = msgqueue_channel_check_common(msgqueue_id, buffer, buffer_size, message_size); 67 if (res) { 68 return res; 69 } 70 71 if (!notify) { 72 ZF_LOGE("Registering a sender msgqueue channel with empty 'notify' function pointer"); 73 return -EINVAL; 74 } 75 76 msgqueue_channel_init_common(msgqueue_id, buffer, queue_len, buffer_size, message_size); 77 78 camkes_msgqueue_channels[msgqueue_id].role = MSGQUEUE_SENDER; 79 camkes_msgqueue_channels[msgqueue_id].sender_funcs.notify = notify; 80 81 return 0; 82} 83 84int camkes_msgqueue_channel_register_receiver(int msgqueue_id, void *buffer, unsigned queue_len, 85 size_t buffer_size, size_t message_size, int (*poll)(void), void (*wait)(void)) 86{ 87 int res = msgqueue_channel_check_common(msgqueue_id, buffer, buffer_size, message_size); 88 if (res) { 89 return res; 90 } 91 92 if (!poll || !wait) { 93 ZF_LOGE("Registering a receiver msgqueue channel with empty 'poll' and 'wait' function pointers"); 94 return -EINVAL; 95 } 96 97 msgqueue_channel_init_common(msgqueue_id, buffer, queue_len, buffer_size, message_size); 98 99 camkes_msgqueue_channels[msgqueue_id].role = MSGQUEUE_RECEIVER; 100 camkes_msgqueue_channels[msgqueue_id].receiver_funcs.poll = poll; 101 camkes_msgqueue_channels[msgqueue_id].receiver_funcs.wait = wait; 102 103 return 0; 104} 105 106camkes_msgqueue_channel_t *camkes_msgqueue_channel_get(int msgqueue_id, msgqueue_role_t role) 107{ 108 if (msgqueue_id >= MAX_CAMKES_MSGQUEUE_ID) { 109 ZF_LOGE("Trying to retrieve a channel with an ID greater than the maximum ID"); 110 return NULL; 111 } 112 113 if (role == MSGQUEUE_UNASSIGNED) { 114 ZF_LOGE("Trying to retrieve an uninitialised channel"); 115 return NULL; 116 } 117 118 if (camkes_msgqueue_channels[msgqueue_id].role != role) { 119 ZF_LOGE("Requested msgqueue channel doesn't match the role passed in"); 120 return NULL; 121 } 122 123 return &camkes_msgqueue_channels[msgqueue_id]; 124} 125