kernel: msgq: adding support to k_msgq_put_urgent

This commit introduces the k_msgq_put_urgent 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,50 @@ 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 is
+				 * less obvious, but it can be achieved by simply
+				 * following these steps:
+				 *
+				 * 1. decrementing the read pointer. This effectively
+				 * opens space for the incoming message in the head of
+				 * the queue.
+				 *
+				 * 2. temporarily matching the write pointer with the
+				 * read pointer. This way the message will be written
+				 * in the recently opened space.
+				 *
+				 * 3. reverting the write pointer after the write to its
+				 * original value. The read pointer, by its turn, should
+				 * be always at the head, so it doesn't need reverting
+				 * because we just wrote to the head.
+				 *
+				 * ...but this is inefficient because we need to go back
+				 * and forth with the value of write_ptr; note that it
+				 * becomes equal to read_ptr just to be restored afterwards.
+				 * A much more efficient solution is to simply write the
+				 * message directly to read_ptr's address after step 1,
+				 * which avoids needing to perform steps 2 and 3 altogether.
+				 */
+				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 +224,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)
@@ -196,6 +244,15 @@ static inline int z_vrfy_k_msgq_put(struct k_msgq *msgq, const void *data,
 
 	return z_impl_k_msgq_put(msgq, data, timeout);
 }
+
+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_mrsh.c>
 #endif /* CONFIG_USERSPACE */