diff --git a/os/rt/include/ch.h b/os/rt/include/ch.h
index 21c172ec9..e96bc797c 100644
--- a/os/rt/include/ch.h
+++ b/os/rt/include/ch.h
@@ -82,8 +82,8 @@ extern "C" {
 #include "chdebug.h"
 #include "chtm.h"
 #include "chstats.h"
-#include "chsys.h"
 #include "chglobal.h"
+#include "chsys.h"
 #include "chvt.h"
 #include "chlists.h"
 #include "chschd.h"
diff --git a/os/rt/include/chdebug.h b/os/rt/include/chdebug.h
index 6ae948bc3..44f44116f 100644
--- a/os/rt/include/chdebug.h
+++ b/os/rt/include/chdebug.h
@@ -71,6 +71,13 @@
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
 
+#if CH_DBG_ENABLE_ASSERTS     || CH_DBG_ENABLE_CHECKS      ||               \
+    CH_DBG_ENABLE_STACK_CHECK || CH_DBG_SYSTEM_STATE_CHECK
+#define CH_DBG_ENABLED              TRUE
+#else
+#define CH_DBG_ENABLED              FALSE
+#endif
+
 /*===========================================================================*/
 /* Module data structures and types.                                         */
 /*===========================================================================*/
diff --git a/os/rt/include/chsys.h b/os/rt/include/chsys.h
index 6255b7d5a..9a74788ae 100644
--- a/os/rt/include/chsys.h
+++ b/os/rt/include/chsys.h
@@ -299,6 +299,14 @@ static inline void chSysUnlock(void) {
 
   _dbg_check_unlock();
   _stats_stop_measure_crit_thd();
+
+  /* The following condition can be triggered by the use of i-class functions
+     in a critical section not followed by a chSchResceduleS(), this means
+     that the current thread has a lower priority than the next thread in
+     the ready list.*/
+  chDbgAssert(ch.rlist.r_current->p_prio >= ch.rlist.r_queue.p_next->p_prio,
+              "priority violation, missing reschedule");
+
   port_unlock();
 }
 
diff --git a/test/testmtx.c b/test/testmtx.c
index b40242d3d..c9ba9e093 100644
--- a/test/testmtx.c
+++ b/test/testmtx.c
@@ -399,6 +399,7 @@ static void mtx4_execute(void) {
   test_assert(11, chThdGetPriorityX() == p2, "wrong priority level");
   chSysLock();
   chMtxUnlockS();
+  chSchRescheduleS();
   chSysUnlock();
   test_assert(12, chThdGetPriorityX() == p1, "wrong priority level");
   chThdSleepMilliseconds(100);
diff --git a/test/testsem.c b/test/testsem.c
index 604a6954d..cb7f13bc2 100644
--- a/test/testsem.c
+++ b/test/testsem.c
@@ -102,6 +102,7 @@ static void sem1_execute(void) {
   threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+5, thread1, "A");
   chSysLock();
   chSemAddCounterI(&sem1, 2);
+  chSchRescheduleS();
   chSysUnlock();
   test_wait_threads();
   test_assert_lock(2, chSemGetCounterI(&sem1) == 1, "invalid counter");