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:
parent
0321ac8dbf
commit
14bcc859ca
|
@ -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.
|
||||
*
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue