diff --git a/os/rt/src/chmsg.c b/os/rt/src/chmsg.c
index 9c73ff200..92e6f7c38 100644
--- a/os/rt/src/chmsg.c
+++ b/os/rt/src/chmsg.c
@@ -64,9 +64,9 @@
 /*===========================================================================*/
 
 #if CH_CFG_USE_MESSAGES_PRIORITY == TRUE
-#define msg_insert(tp, qp) queue_prio_insert(tp, qp)
+#define msg_insert(tp, qp) ch_sch_prio_insert(&tp->hdr.queue, qp)
 #else
-#define msg_insert(tp, qp) ch_queue_insert((ch_queue_t *)tp, qp)
+#define msg_insert(tp, qp) ch_queue_insert(&tp->hdr.queue, qp)
 #endif
 
 /*===========================================================================*/
diff --git a/os/rt/src/chsem.c b/os/rt/src/chsem.c
index 79452d95b..c1e71ebcf 100644
--- a/os/rt/src/chsem.c
+++ b/os/rt/src/chsem.c
@@ -76,9 +76,9 @@
 /*===========================================================================*/
 
 #if CH_CFG_USE_SEMAPHORES_PRIORITY == TRUE
-#define sem_insert(tp, qp) queue_prio_insert(tp, qp)
+#define sem_insert(tp, qp) ch_sch_prio_insert(&tp->hdr.pqueue, qp)
 #else
-#define sem_insert(tp, qp) ch_queue_insert((ch_queue_t *)tp, qp)
+#define sem_insert(tp, qp) ch_queue_insert(&tp->hdr.queue, qp)
 #endif
 
 /*===========================================================================*/