kernel: msgq: adding support to k_msgq_put_front

This commit introduces the k_msgq_put_front API for sending
messages to a queue in a LIFO scheme.

Signed-off-by: Alexander Paschoaletto <axelpinheiro@gmail.com>

Author: alexpaschoaletto

include/zephyr/kernel.h

@@ -4785,7 +4785,7 @@ __syscall int k_msgq_alloc_init(struct k_msgq *msgq, size_t msg_size,
 int k_msgq_cleanup(struct k_msgq *msgq);
 
 /**
- * @brief Send a message to a message queue.
+ * @brief Send a message to the end of a message queue.
  *
  * This routine sends a message to message queue @a q.
  *
@@ -4806,6 +4806,33 @@ int k_msgq_cleanup(struct k_msgq *msgq);
  */
 __syscall int k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout);
 
+/**
+ * @brief Send a message to the front of a message queue.
+ *
+ * This routine sends a message to the beginning (head) of message queue @a q.
+ * Messages sent with this method will be retrieved before any pre-existing
+ * messages in the queue.
+ *
+ * @note if there is no space in the message queue, this function will
+ * behave the same as k_msgq_put.
+ *
+ * @note The message content is copied from @a data into @a msgq and the @a data
+ * pointer is not retained, so the message content will not be modified
+ * by this function.
+ *
+ * @funcprops \isr_ok
+ *
+ * @param msgq Address of the message queue.
+ * @param data Pointer to the message.
+ * @param timeout Waiting period to add the message, or one of the special
+ *                values K_NO_WAIT and K_FOREVER.
+ *
+ * @retval 0 Message sent.
+ * @retval -ENOMSG Returned without waiting or queue purged.
+ * @retval -EAGAIN Waiting period timed out.
+ */
+__syscall int k_msgq_put_front(struct k_msgq *msgq, const void *data, k_timeout_t timeout);
+
 /**
  * @brief Receive a message from a message queue.
  *

kernel/msg_q.c

@@ -124,8 +124,8 @@ int k_msgq_cleanup(struct k_msgq *msgq)
 	return 0;
 }
 
-
-int z_impl_k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout)
+static inline int put_msg_in_queue(struct k_msgq *msgq, const void *data,
+			k_timeout_t timeout, bool put_at_back)
 {
 	__ASSERT(!arch_is_in_isr() || K_TIMEOUT_EQ(timeout, K_NO_WAIT), "");
 
@@ -150,13 +150,31 @@ int z_impl_k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout
 			arch_thread_return_value_set(pending_thread, 0);
 			z_ready_thread(pending_thread);
 		} else {
-			/* put message in queue */
 			__ASSERT_NO_MSG(msgq->write_ptr >= msgq->buffer_start &&
 					msgq->write_ptr < msgq->buffer_end);
-			(void)memcpy(msgq->write_ptr, (char *)data, msgq->msg_size);
-			msgq->write_ptr += msgq->msg_size;
-			if (msgq->write_ptr == msgq->buffer_end) {
-				msgq->write_ptr = msgq->buffer_start;
+			if (put_at_back) {
+				/*
+				 * writing a message to the back of the queue is
+				 * simple and effective: copy the message, then
+				 * increment write_ptr.
+				 */
+				(void)memcpy(msgq->write_ptr, (char *)data, msgq->msg_size);
+				msgq->write_ptr += msgq->msg_size;
+				if (msgq->write_ptr == msgq->buffer_end) {
+					msgq->write_ptr = msgq->buffer_start;
+				}
+			} else {
+				/*
+				 * writing a message to the head of the queue can
+				 * be achieved by simply decrementing the read pointer
+				 * to open space at the front of the queue, then
+				 * copying the message to the newly created space.
+				 */
+				if (msgq->read_ptr == msgq->buffer_start) {
+					msgq->read_ptr = msgq->buffer_end;
+				}
+				msgq->read_ptr -= msgq->msg_size;
+				(void)memcpy(msgq->read_ptr, (char *)data, msgq->msg_size);
 			}
 			msgq->used_msgs++;
 			resched = handle_poll_events(msgq);
@@ -187,6 +205,17 @@ int z_impl_k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout
 	return result;
 }
 
+
+int z_impl_k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout)
+{
+	return put_msg_in_queue(msgq, data, timeout, true);
+}
+
+int z_impl_k_msgq_put_front(struct k_msgq *msgq, const void *data, k_timeout_t timeout)
+{
+	return put_msg_in_queue(msgq, data, timeout, false);
+}
+
 #ifdef CONFIG_USERSPACE
 static inline int z_vrfy_k_msgq_put(struct k_msgq *msgq, const void *data,
 				    k_timeout_t timeout)
@@ -197,6 +226,16 @@ static inline int z_vrfy_k_msgq_put(struct k_msgq *msgq, const void *data,
 	return z_impl_k_msgq_put(msgq, data, timeout);
 }
 #include <zephyr/syscalls/k_msgq_put_mrsh.c>
+
+static inline int z_vrfy_k_msgq_put_front(struct k_msgq *msgq, const void *data,
+				    k_timeout_t timeout)
+{
+	K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
+	K_OOPS(K_SYSCALL_MEMORY_READ(data, msgq->msg_size));
+
+	return z_impl_k_msgq_put_front(msgq, data, timeout);
+}
+#include <zephyr/syscalls/k_msgq_put_front_mrsh.c>
 #endif /* CONFIG_USERSPACE */
 
 void z_impl_k_msgq_get_attrs(struct k_msgq *msgq, struct k_msgq_attrs *attrs)