From 14bcc859ca250dd313b74b26ec0629527e5539e5 Mon Sep 17 00:00:00 2001
From: romain pelletant <romainp@kickmaker.net>
Date: Wed, 28 Dec 2022 13:06:16 +0100
Subject: [PATCH] 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>
---
 include/zephyr/kernel.h | 18 ++++++++++++++++
 kernel/msg_q.c          | 46 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/include/zephyr/kernel.h b/include/zephyr/kernel.h
index 16ae789a936..957d1b0dee8 100644
--- a/include/zephyr/kernel.h
+++ b/include/zephyr/kernel.h
@@ -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.
  *
diff --git a/kernel/msg_q.c b/kernel/msg_q.c
index 0e6488df87d..6b68f7ac37d 100644
--- a/kernel/msg_q.c
+++ b/kernel/msg_q.c
@@ -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;