2 files changed, 29 insertions, 18 deletions
diff --git a/os/rt/include/chthreads.h b/os/rt/include/chthreads.h
index ca2d3bcbc..fa093b09c 100644
--- a/os/rt/include/chthreads.h
+++ b/os/rt/include/chthreads.h
@@ -393,6 +393,31 @@ static inline bool chThdQueueIsEmptyI(threads_queue_t *tqp) {
   return queue_isempty(tqp);
 }
 
+
+/**
+ * @brief   Dequeues and wakes up one thread from the threads queue object.
+ * @details Dequeues one thread from the queue without checking if the queue
+ *          is empty.
+ * @pre     The queue must contain at least an object.
+ *
+ * @param[in] tqp       pointer to the threads queue object
+ * @param[in] msg       the message code
+ *
+ * @iclass
+ */
+static inline void chThdDoDequeueNextI(threads_queue_t *tqp, msg_t msg) {
+  thread_t *tp;
+
+  chDbgAssert(queue_notempty(tqp), "empty queue");
+
+  tp = queue_fifo_remove(tqp);
+
+  chDbgAssert(tp->p_state == CH_STATE_QUEUED, "invalid state");
+
+  tp->p_u.rdymsg = msg;
+  chSchReadyI(tp);
+}
+
 #endif /* _CHTHREADS_H_ */
 
 /** @} */
diff --git a/os/rt/src/chthreads.c b/os/rt/src/chthreads.c
index d7e9bc13d..3cff4dc61 100644
--- a/os/rt/src/chthreads.c
+++ b/os/rt/src/chthreads.c
@@ -640,15 +640,8 @@ msg_t chThdEnqueueTimeoutS(threads_queue_t *tqp, systime_t timeout) {
  */
 void chThdDequeueNextI(threads_queue_t *tqp, msg_t msg) {
 
-  if (queue_notempty(tqp)) {
-    thread_t *tp = queue_fifo_remove(tqp);
-
-    chDbgAssert(tp->p_state == CH_STATE_QUEUED,
-                "not CH_STATE_QUEUED");
-
-    tp->p_u.rdymsg = msg;
-    chSchReadyI(tp);
-  }
+  if (queue_notempty(tqp))
+    chThdDoDequeueNextI(tqp, msg);
 }
 
 /**
@@ -661,15 +654,8 @@ void chThdDequeueNextI(threads_queue_t *tqp, msg_t msg) {
  */
 void chThdDequeueAllI(threads_queue_t *tqp, msg_t msg) {
 
-  while (queue_notempty(tqp)) {
-    thread_t *tp = queue_fifo_remove(tqp);
-
-    chDbgAssert(tp->p_state == CH_STATE_QUEUED,
-                "not CH_STATE_QUEUED");
-
-    tp->p_u.rdymsg = msg;
-    chSchReadyI(tp);
-  }
+  while (queue_notempty(tqp))
+    chThdDoDequeueNextI(tqp, msg);
 }
 
 /** @} */