kernel: k_msgq: add peek at function

Make message queue able to peek data at the specified index.
Related to issue #53360

Signed-off-by: romain pelletant <romainp@kickmaker.net>
This commit is contained in:
romain pelletant 2022-12-28 13:06:16 +01:00 committed by Fabio Baltieri
parent 0321ac8dbf
commit 14bcc859ca
2 changed files with 64 additions and 0 deletions

View File

@ -4449,6 +4449,24 @@ __syscall int k_msgq_get(struct k_msgq *msgq, void *data, k_timeout_t timeout);
*/
__syscall int k_msgq_peek(struct k_msgq *msgq, void *data);
/**
* @brief Peek/read a message from a message queue at the specified index
*
* This routine reads a message from message queue at the specified index
* and leaves the message in the queue.
* k_msgq_peek_at(msgq, data, 0) is equivalent to k_msgq_peek(msgq, data)
*
* @funcprops \isr_ok
*
* @param msgq Address of the message queue.
* @param data Address of area to hold the message read from the queue.
* @param idx Message queue index at which to peek
*
* @retval 0 Message read.
* @retval -ENOMSG Returned when the queue has no message at index.
*/
__syscall int k_msgq_peek_at(struct k_msgq *msgq, void *data, uint32_t idx);
/**
* @brief Purge a message queue.
*

View File

@ -315,6 +315,52 @@ static inline int z_vrfy_k_msgq_peek(struct k_msgq *msgq, void *data)
#include <syscalls/k_msgq_peek_mrsh.c>
#endif
int z_impl_k_msgq_peek_at(struct k_msgq *msgq, void *data, uint32_t idx)
{
k_spinlock_key_t key;
int result;
uint32_t bytes_to_end;
uint32_t byte_offset;
char *start_addr;
key = k_spin_lock(&msgq->lock);
if (msgq->used_msgs > idx) {
bytes_to_end = (msgq->buffer_end - msgq->read_ptr);
byte_offset = idx * msgq->msg_size;
start_addr = msgq->read_ptr;
/* check item available in start/end of ring buffer */
if (bytes_to_end <= byte_offset) {
/* Tweak the values in case */
byte_offset -= bytes_to_end;
/* wrap-around is required */
start_addr = msgq->buffer_start;
}
(void)memcpy(data, start_addr + byte_offset, msgq->msg_size);
result = 0;
} else {
/* don't wait for a message to become available */
result = -ENOMSG;
}
SYS_PORT_TRACING_OBJ_FUNC(k_msgq, peek, msgq, result);
k_spin_unlock(&msgq->lock, key);
return result;
}
#ifdef CONFIG_USERSPACE
static inline int z_vrfy_k_msgq_peek_at(struct k_msgq *msgq, void *data, uint32_t idx)
{
Z_OOPS(Z_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
Z_OOPS(Z_SYSCALL_MEMORY_WRITE(data, msgq->msg_size));
return z_impl_k_msgq_peek_at(msgq, data, idx);
}
#include <syscalls/k_msgq_peek_at_mrsh.c>
#endif
void z_impl_k_msgq_purge(struct k_msgq *msgq)
{
k_spinlock_key_t key;