3 files changed, 36 insertions, 10 deletions
diff --git a/os/kernel/include/chschd.h b/os/kernel/include/chschd.h
index d1b69cad5..3847c948e 100644
--- a/os/kernel/include/chschd.h
+++ b/os/kernel/include/chschd.h
@@ -92,11 +92,33 @@ typedef struct {
 extern ReadyList rlist;
 #endif /* !defined(PORT_OPTIMIZED_RLIST_EXT) */
 
-#ifdef CH_CURRP_REGISTER_CACHE
-register Thread *currp asm(CH_CURRP_REGISTER_CACHE);
-#else
+/**
+ * @brief   Current thread pointer access macro.
+ * @note    This macro is not meant to be used in the application code but
+ *          only from within the kernel, use the @p chThdSelf() API instead.
+ * @note    It is forbidden to use this macro in order to change the pointer
+ *          (currp = something), use @p setcurrp() instead.
+ */
+#if !defined(PORT_OPTIMIZED_CURRP) || defined(__DOXYGEN__)
+#if !defined(CH_CURRP_REGISTER_CACHE) || defined(__DOXYGEN__)
 #define currp rlist.r_current
-#endif
+#else /* defined(CH_CURRP_REGISTER_CACHE) */
+register Thread *currp asm(CH_CURRP_REGISTER_CACHE);
+#endif /* defined(CH_CURRP_REGISTER_CACHE) */
+#endif /* !defined(PORT_OPTIMIZED_CURRP) */
+
+/**
+ * @brief   Current thread pointer change macro.
+ * @note    This macro is not meant to be used in the application code but
+ *          only from within the kernel.
+ */
+#if !defined(PORT_OPTIMIZED_SETCURRP) || defined(__DOXYGEN__)
+#if !defined(CH_CURRP_REGISTER_CACHE) || defined(__DOXYGEN__)
+#define setcurrp(tp) (rlist.r_current = (tp))
+#else /* defined(CH_CURRP_REGISTER_CACHE) */
+(currp = (tp))
+#endif /* defined(CH_CURRP_REGISTER_CACHE) */
+#endif /* !defined(PORT_OPTIMIZED_SETCURRP) */
 
 /*
  * Scheduler APIs.
@@ -135,7 +157,7 @@ extern "C" {
  * @details This function returns @p TRUE if there is a ready thread with
  *          higher priority.
  */
-#if !defined(PORT_OPTIMIZED_ISRESCHREQUIREDI) && !defined(__DOXYGEN__)
+#if !defined(PORT_OPTIMIZED_ISRESCHREQUIREDI) || defined(__DOXYGEN__)
 #define chSchIsRescRequiredI() (firstprio(&rlist.r_queue) > currp->p_prio)
 #endif /* !defined(PORT_OPTIMIZED_ISRESCHREQUIREDI) */
 
@@ -144,7 +166,7 @@ extern "C" {
  * @details This function returns @p TRUE if there is a ready thread with
  *          equal or higher priority.
  */
-#if !defined(PORT_OPTIMIZED_CANYIELDS) && !defined(__DOXYGEN__)
+#if !defined(PORT_OPTIMIZED_CANYIELDS) || defined(__DOXYGEN__)
 #define chSchCanYieldS() (firstprio(&rlist.r_queue) >= currp->p_prio)
 #endif /* !defined(PORT_OPTIMIZED_CANYIELDS) */
 
diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c
index d5c8a0a5a..7170fb0ec 100644
--- a/os/kernel/src/chschd.c
+++ b/os/kernel/src/chschd.c
@@ -96,7 +96,8 @@ void chSchGoSleepS(tstate_t newstate) {
   Thread *otp;
 
   (otp = currp)->p_state = newstate;
-  (currp = fifo_remove(&rlist.r_queue))->p_state = THD_STATE_CURRENT;
+  setcurrp(fifo_remove(&rlist.r_queue));
+  currp->p_state = THD_STATE_CURRENT;
 #if CH_TIME_QUANTUM > 0
   rlist.r_preempt = CH_TIME_QUANTUM;
 #endif
@@ -196,7 +197,8 @@ void chSchWakeupS(Thread *ntp, msg_t msg) {
 #if CH_TIME_QUANTUM > 0
     rlist.r_preempt = CH_TIME_QUANTUM;
 #endif
-    (currp = ntp)->p_state = THD_STATE_CURRENT;
+    setcurrp(ntp);
+    ntp->p_state = THD_STATE_CURRENT;
     chDbgTrace(ntp, otp);
     chSysSwitchI(ntp, otp);
   }
@@ -217,7 +219,8 @@ void chSchDoRescheduleI(void) {
 #endif
   otp = currp;
   /* Picks the first thread from the ready queue and makes it current.*/
-  (currp = ntp = fifo_remove(&rlist.r_queue))->p_state = THD_STATE_CURRENT;
+  (ntp = fifo_remove(&rlist.r_queue))->p_state = THD_STATE_CURRENT;
+  setcurrp(ntp);
   chSchReadyI(otp);
   chDbgTrace(ntp, otp);
   chSysSwitchI(ntp, otp);
diff --git a/os/kernel/src/chsys.c b/os/kernel/src/chsys.c
index eeed4a998..1a66c8e98 100644
--- a/os/kernel/src/chsys.c
+++ b/os/kernel/src/chsys.c
@@ -80,7 +80,8 @@ void chSysInit(void) {
 #endif
 
   /* Now this instructions flow becomes the main thread.*/
-  (currp = init_thread(&mainthread, NORMALPRIO))->p_state = THD_STATE_CURRENT;
+  setcurrp(init_thread(&mainthread, NORMALPRIO));
+  currp->p_state = THD_STATE_CURRENT;
   chSysEnable();
 
   /* This thread has the lowest priority in the system, its role is just to