From ace7332c8c0162cca48ca1bf25441cb8a5d5d1f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mikolai=20G=C3=BCtschow?= <mikolai.guetschow@tu-dresden.de>
Date: Tue, 26 Nov 2024 21:30:49 +0100
Subject: [PATCH] core/msg: re-enable IRQs before printing for highlevel_stdio

copy message queue information to stack before
---
 core/include/msg.h |  9 +++++++++
 core/msg.c         | 40 +++++++++++++++++++++++++++++++---------
 2 files changed, 40 insertions(+), 9 deletions(-)

diff --git a/core/include/msg.h b/core/include/msg.h
index 96ab47ea2e97..90119bcdb513 100644
--- a/core/include/msg.h
+++ b/core/include/msg.h
@@ -411,8 +411,17 @@ unsigned msg_queue_capacity(kernel_pid_t pid);
  */
 void msg_init_queue(msg_t *array, int num);
 
+/**
+ * @brief Number of messages to be maximally printed through @ref msg_queue_print
+ */
+#ifndef CONFIG_MSG_QUEUE_PRINT_MAX
+#  define CONFIG_MSG_QUEUE_PRINT_MAX 16U
+#endif
+
 /**
  * @brief   Prints the message queue of the current thread.
+ *
+ * @note    Prints at most CONFIG_MSG_QUEUE_PRINT_MAX messages.
  */
 void msg_queue_print(void);
 
diff --git a/core/msg.c b/core/msg.c
index 13d2b4eece4f..958f0b205e23 100644
--- a/core/msg.c
+++ b/core/msg.c
@@ -21,8 +21,10 @@
  */
 
 #include <stddef.h>
+#include <string.h>
 #include <inttypes.h>
 #include <assert.h>
+#include "macros/utils.h"
 #include "sched.h"
 #include "msg.h"
 #include "msg_bus.h"
@@ -495,26 +497,46 @@ void msg_queue_print(void)
     unsigned state = irq_disable();
     thread_t *thread = thread_get_active();
 
-    unsigned msg_counter = msg_avail();
+    unsigned msg_count = msg_avail();
 
-    if (msg_counter < 1) {
+    if (msg_count < 1) {
         /* no msg queue */
         irq_restore(state);
         printf("No messages or no message queue\n");
         return;
     }
+
     cib_t *msg_queue = &thread->msg_queue;
-    msg_t *msg_array = thread->msg_array;
-    int first_msg = cib_peek(msg_queue);
+
+    /* copy message queue information to stack
+       before re-enabling IRQs (needed for highlevel_stdio) */
+    unsigned size = msg_queue->mask + 1;
+    unsigned msg_count_print = MIN(msg_count, CONFIG_MSG_QUEUE_PRINT_MAX);
+    int msg_idx_first = cib_peek(msg_queue);
+    int msg_idx_last = (msg_idx_first + msg_count) & msg_queue->mask;
+    unsigned msg_count_to_end = size - msg_idx_first;
+
+    static msg_t msg_array[CONFIG_MSG_QUEUE_PRINT_MAX];
+    if (msg_idx_last > msg_idx_first || msg_count_to_end > msg_count_print) {
+        memcpy(msg_array, &thread->msg_array[msg_idx_first], sizeof(msg_t)*(msg_count_print));
+    }
+    else {
+        unsigned msg_count_from_start = MIN((unsigned)(msg_idx_last + 1), msg_count_print-msg_count_to_end);
+        memcpy(&msg_array[0], &thread->msg_array[msg_idx_first], sizeof(msg_t)*msg_count_to_end);
+        memcpy(&msg_array[msg_count_to_end], &thread->msg_array[0], sizeof(msg_t)*msg_count_from_start);
+    }
+    irq_restore(state);
 
     printf("Message queue of thread %" PRIkernel_pid "\n", thread->pid);
-    printf("    size: %u (avail: %u)\n", msg_queue->mask + 1, msg_counter);
+    printf("    size: %u (avail: %u)\n", size, msg_count);
 
-    for (unsigned i = 0; i < msg_counter; i++) {
-        msg_t *m = &msg_array[(first_msg + i) & msg_queue->mask];
-        printf("    * %u: sender: %" PRIkernel_pid ", type: 0x%04" PRIu16
+    for (unsigned i = 0; i < msg_count_print; i++) {
+        msg_t *m = &msg_array[i];
+        printf("    * %u: sender: %" PRIkernel_pid ", type: 0x%04" PRIx16
                ", content: %" PRIu32 " (%p)\n", i, m->sender_pid, m->type,
                m->content.value, m->content.ptr);
     }
-    irq_restore(state);
+    if (msg_count > msg_count_print) {
+        puts("    ... (print more by changing CONFIG_MSG_QUEUE_PRINT_MAX)");
+    }
 }