From 430010715e7a9af17185412273165674f3b58f20 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sun, 16 Dec 2007 19:01:30 +0000
Subject: [PATCH] git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@141
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 demos/ARM7-LPC214x-GCC-minimal/Makefile       |   5 +-
 demos/ARM7-LPC214x-GCC-minimal/Makefile.thumb |   5 +-
 demos/ARM7-LPC214x-GCC-minimal/chconf.h       |   7 +-
 demos/ARM7-LPC214x-GCC/Makefile               |   5 +-
 demos/ARM7-LPC214x-GCC/Makefile.thumb         |   5 +-
 demos/ARM7-LPC214x-GCC/chconf.h               |   7 +-
 demos/AVR-AT90CANx-GCC/chconf.h               |   7 +-
 demos/Win32-MSVS/chconf.h                     |   7 +-
 demos/Win32-MinGW/chconf.h                    |   7 +-
 docs/Doxyfile                                 |   4 +-
 docs/ch.txt                                   |  73 +++--
 docs/index.html                               |   2 +-
 readme.txt                                    |  12 +
 src/chmtx.c                                   | 249 ++++++++++++++++++
 src/chsem.c                                   | 129 ---------
 src/chthreads.c                               |  14 +-
 src/include/ch.h                              |   4 +
 src/include/mutexes.h                         |  66 +++++
 src/include/semaphores.h                      |   9 +-
 src/include/threads.h                         |  26 +-
 src/templates/chconf.h                        |   7 +-
 test/test.c                                   | 238 ++++++++++++-----
 22 files changed, 618 insertions(+), 270 deletions(-)
 create mode 100644 src/chmtx.c
 create mode 100644 src/include/mutexes.h

diff --git a/demos/ARM7-LPC214x-GCC-minimal/Makefile b/demos/ARM7-LPC214x-GCC-minimal/Makefile
index e74d2f382..ac7bb1000 100644
--- a/demos/ARM7-LPC214x-GCC-minimal/Makefile
+++ b/demos/ARM7-LPC214x-GCC-minimal/Makefile
@@ -64,8 +64,9 @@ UADEFS =
 # List ARM-mode C source files here
 ASRC = chcore.c \
        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chevents.c \
-       ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c ../../src/chserial.c \
+       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+       ../../src/chserial.c \
        ../../ports/ARM7-LPC214x/GCC/vic.c \
        main.c
 
diff --git a/demos/ARM7-LPC214x-GCC-minimal/Makefile.thumb b/demos/ARM7-LPC214x-GCC-minimal/Makefile.thumb
index 68f308449..17239c741 100644
--- a/demos/ARM7-LPC214x-GCC-minimal/Makefile.thumb
+++ b/demos/ARM7-LPC214x-GCC-minimal/Makefile.thumb
@@ -69,8 +69,9 @@ ASRC =
 #       enabled for all modules and that lowers performance.
 TSRC = chcore.c \
        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chevents.c \
-       ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c ../../src/chserial.c \
+       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+       ../../src/chserial.c \
        ../../ports/ARM7-LPC214x/GCC/vic.c \
        main.c
 
diff --git a/demos/ARM7-LPC214x-GCC-minimal/chconf.h b/demos/ARM7-LPC214x-GCC-minimal/chconf.h
index fc81d571f..11081acc4 100644
--- a/demos/ARM7-LPC214x-GCC-minimal/chconf.h
+++ b/demos/ARM7-LPC214x-GCC-minimal/chconf.h
@@ -77,10 +77,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 //#define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-//#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+//#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/demos/ARM7-LPC214x-GCC/Makefile b/demos/ARM7-LPC214x-GCC/Makefile
index 60c2df008..a2f229617 100644
--- a/demos/ARM7-LPC214x-GCC/Makefile
+++ b/demos/ARM7-LPC214x-GCC/Makefile
@@ -64,8 +64,9 @@ UADEFS =
 # List ARM-mode C source files here
 ASRC = chcore.c \
        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chevents.c \
-       ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c ../../src/chserial.c \
+       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+       ../../src/chserial.c \
        ../../ports/ARM7-LPC214x/GCC/vic.c  ../../ports/ARM7-LPC214x/GCC/lpc214x_serial.c  \
        ../../ports/ARM7-LPC214x/GCC/lpc214x_ssp.c  \
        ../../src/lib/evtimer.c ../../test/test.c \
diff --git a/demos/ARM7-LPC214x-GCC/Makefile.thumb b/demos/ARM7-LPC214x-GCC/Makefile.thumb
index 111fa8c7b..518169cd8 100644
--- a/demos/ARM7-LPC214x-GCC/Makefile.thumb
+++ b/demos/ARM7-LPC214x-GCC/Makefile.thumb
@@ -69,8 +69,9 @@ ASRC =
 #       enabled for all modules and that lowers performance.
 TSRC = chcore.c \
        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chevents.c \
-       ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c ../../src/chserial.c \
+       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+       ../../src/chserial.c \
        ../../ports/ARM7-LPC214x/GCC/vic.c  ../../ports/ARM7-LPC214x/GCC/lpc214x_serial.c  \
        ../../ports/ARM7-LPC214x/GCC/lpc214x_ssp.c  \
        ../../src/lib/evtimer.c ../../test/test.c \
diff --git a/demos/ARM7-LPC214x-GCC/chconf.h b/demos/ARM7-LPC214x-GCC/chconf.h
index 87402547c..28db4b351 100644
--- a/demos/ARM7-LPC214x-GCC/chconf.h
+++ b/demos/ARM7-LPC214x-GCC/chconf.h
@@ -77,10 +77,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 #define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/demos/AVR-AT90CANx-GCC/chconf.h b/demos/AVR-AT90CANx-GCC/chconf.h
index 2bc89dc2f..b4ddeeb05 100644
--- a/demos/AVR-AT90CANx-GCC/chconf.h
+++ b/demos/AVR-AT90CANx-GCC/chconf.h
@@ -78,10 +78,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 #define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/demos/Win32-MSVS/chconf.h b/demos/Win32-MSVS/chconf.h
index 5941cc856..037e3d8a0 100644
--- a/demos/Win32-MSVS/chconf.h
+++ b/demos/Win32-MSVS/chconf.h
@@ -82,10 +82,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 #define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/demos/Win32-MinGW/chconf.h b/demos/Win32-MinGW/chconf.h
index af19dd995..3e741bb8c 100644
--- a/demos/Win32-MinGW/chconf.h
+++ b/demos/Win32-MinGW/chconf.h
@@ -82,10 +82,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 #define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/docs/Doxyfile b/docs/Doxyfile
index 8abfd62b4..24ab33ea9 100644
--- a/docs/Doxyfile
+++ b/docs/Doxyfile
@@ -4,7 +4,7 @@
 # Project related configuration options
 #---------------------------------------------------------------------------
 PROJECT_NAME           = ChibiOS/RT
-PROJECT_NUMBER         = "0.4.5 beta"
+PROJECT_NUMBER         = "0.5.0 beta"
 OUTPUT_DIRECTORY       = .
 CREATE_SUBDIRS         = NO
 OUTPUT_LANGUAGE        = English
@@ -231,7 +231,7 @@ PREDEFINED             = __JUST_STUBS__ \
                          CH_USE_TERMINATE \
                          CH_USE_WAITEXIT \
                          CH_USE_SEMAPHORES \
-                         CH_USE_RT_SEMAPHORES \
+                         CH_USE_MUTEXES \
                          CH_USE_EVENTS \
                          CH_USE_EVENTS_TIMEOUT \
                          CH_USE_EXIT_EVENT \
diff --git a/docs/ch.txt b/docs/ch.txt
index 02152047b..993f77102 100644
--- a/docs/ch.txt
+++ b/docs/ch.txt
@@ -13,7 +13,7 @@
  * <li>Easily portable.</li>
  * <li>Mixed programming model:</li>
  *   <ul>
- *   <li>Synchronous, using semaphores and/or messages.</li>
+ *   <li>Synchronous, using semaphores/mutexes and/or messages.</li>
  *   <li>Asynchronous, using event sources.</li>
  *   <li>Mix of the above models, multiple threads listening to multiple event
  *       sources while serving message queues.</li>
@@ -27,6 +27,7 @@
  * <li>Unlimited number of threads.</li>
  * <li>Unlimited number of virtual timers.</li>
  * <li>Unlimited number of semaphores.</li>
+ * <li>Unlimited number of mutexes.</li>
  * <li>Unlimited number of event sources.</li>
  * <li>Unlimited number of messages in queue.</li>
  * <li>Unlimited number of I/O queues.</li>
@@ -211,38 +212,66 @@
  * A semaphore is a threads synchronization object, some operations
  * are defined on semaphores:<br>
  * <ul>
- * <li><b>Signal</b>: The semaphore counter is increased and if the result
+ *   <li><b>Signal</b>: The semaphore counter is increased and if the result
  *     is non-positive then a waiting thread is removed from the semaphore
- *     queue and made ready for execution.</li>
- * <li><b>Wait</b>: The semaphore counter is decreased and if the result
+ *     queue and made ready for execution.
+ *   </li>
+ *   <li><b>Wait</b>: The semaphore counter is decreased and if the result
  *     becomes negative the thread is queued in the semaphore and suspended.
- *     </li>
- * <li><b>Reset</b>: The semaphore counter is reset to a non-negative value
- *     and all the threads in the queue are released.</li>
+ *   </li>
+ *   <li><b>Reset</b>: The semaphore counter is reset to a non-negative value
+ *     and all the threads in the queue are released.
+ *   </li>
  * </ul>
- * Semaphores are mainly used as guards for mutual exclusion code zones but
+ * Semaphores can be used as guards for mutual exclusion code zones but
  * also have other uses, queues guards and counters as example.<br>
  * In order to use the Semaphores APIs the \p CH_USE_SEMAPHORES
  * option must be specified in \p chconf.h.<br><br>
- * <b>Realtime Semaphores</b><br><br>
- * The RT Semaphores work exactly like normal semaphores except that the
- * thread gaining access to a mutex zone receives a priority boost, the
- * priority is increased by \p MEPRIO and becomes higher than any thread that
- * does not have the boost, note that all the boosted threads still keep their
- * relative priorities.<br>
- * Another difference is that the threads are queued by priority when trying
- * to acquire RT semaphores, normal semaphores use a FIFO strategy.<br>
- * The reason of this mechanism is to make the threads leave very critical
- * guarded zones in a predictable time. Use this mechanism if your application
- * has very strong realtime requirements.<br>
- * In order to use the RT Semaphores APIs the \p CH_USE_RT_SEMAPHORES
- * option must be specified in \p chconf.h.<br>
- * This mechanism is <b>experimental</b>.
  * @file semaphores.h Semaphores macros and structures.
  * @file chsem.c Semaphores code.
  */
 /** @} */
 
+/**
+ * @defgroup Mutexes Mutexes
+ * @{
+ * Mutexes and threads synchronization.
+ * <b>Operation mode</b><br><br>
+ * A mutex is a threads synchronization object, some operations are defined
+ * on mutexes:<br>
+ * <ul>
+ *   <li><b>Lock</b>: The mutex is checked, if the mutex is not owned by some
+ *     other thread then it is locked else the current thread is queued on the
+ *     mutex in a list ordered by priority.
+ *   </li>
+ *   <li><b>Unlock</b>: The mutex is released by the owner and the highest
+ *     priority thread waiting in the queue, if any, is resumed and made owner
+ *     of the mutex.
+ *   </li>
+ * </ul>
+ * In order to use the Event APIs the \p CH_USE_MUTEXES option must be
+ * specified in \p chconf.h.<br><br>
+ *
+ * <b>Constraints</b><br><br>
+ * In ChibiOS/RT the Unlock operations are always performed in Lock-reverse
+ * order. The Unlock API does not even have a parameter, the mutex to unlock
+ * is taken from an internal stack of owned mutexes.
+ * This both improves the performance and is required by the priority
+ * inheritance mechanism.
+ *
+ * <b>The priority inversion problem</b><br><br>
+ * The mutexes in ChibiOS/RT implements the <b>full recursive<b> priority
+ * inheritance mechanism in order handle the priority inversion problem.<br>
+ * When a thread is queued on a mutex, any thread, directly or indirectly,
+ * holding the mutex gains the same priority of the waiting thread (if their
+ * priority was not already equal or higher). The mechanism works with any
+ * number of nested mutexes and any number of involved threads. The algorithm
+ * complexity (worst case) is N with N equal to the number of nested mutexes.
+ * @file mutexes.h Mutexes macros and structures.
+ * @file chmtx.c Mutexes functions.
+ */
+/** @} */
+
 /**
  * @defgroup Events Events
  * @{
diff --git a/docs/index.html b/docs/index.html
index 4ac525ae0..10b088bbb 100644
--- a/docs/index.html
+++ b/docs/index.html
@@ -13,7 +13,7 @@ Homepage</h2>
     </tr>
     <tr>
       <td style="text-align: center; vertical-align: top; width: 150px;">Current
-Version 0.4.5<br>
+Version 0.5.0<br>
 -<br>
       <a href="http://sourceforge.net/projects/chibios/" rel="me" target="_top">Project on SourceForge</a><br>
       <a href="html/index.html" target="_top" rel="me">Documentation</a><br>
diff --git a/readme.txt b/readme.txt
index 4348c6cba..18ba71eb5 100644
--- a/readme.txt
+++ b/readme.txt
@@ -39,6 +39,18 @@ AVR-AT90CANx-GCC       - Port on AVR AT90CAN128, not complete yet.
 *** Releases                                                              ***
 *****************************************************************************
 
+*** 0.5.0 ***
+- NEW: Binary Mutexes, the new mechanism provides a complete implementation
+  of the "priority inheritance" algorithm as a tool for work around the
+  priority inversion problem.
+  The Mutexes are not meant to replace the Semaphores that are still the best
+  mechanism for synchronization between interrupt handlers and high level
+  code, something Mutexes cannot do.
+  Soon an article will be added to the wiki describing pro and cons of the
+  various mechanisms and the correct use cases.
+- RT Semaphores subsystem removed, the Mutexes implements a better solution
+  for the same problem.
+
 *** 0.4.5 ***
 - Moved the serial IRQ handlers and VIC vectors initialization inside the
   serial drivers. Now the serial-related code is all inside the driver.
diff --git a/src/chmtx.c b/src/chmtx.c
new file mode 100644
index 000000000..1c0055acd
--- /dev/null
+++ b/src/chmtx.c
@@ -0,0 +1,249 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @addtogroup Mutexes
+ * @{
+ */
+
+#include <ch.h>
+
+#ifdef CH_USE_MUTEXES
+
+/**
+ * Initializes s \p Mutex structure.
+ * @param mp pointer to a \p Mutex structure
+ */
+void chMtxInit(Mutex *mp) {
+
+  fifo_init(&mp->m_queue);
+  mp->m_owner = NULL;
+}
+
+/*
+ * Inserts a thread into a list ordering it by priority.
+ * @param tp the pointer to the thread to be inserted in the list
+ * @param tqp the pointer to the threads list header
+ */
+#ifdef CH_OPTIMIZE_SPEED
+static INLINE void prio_enq(Thread *tp, ThreadsQueue *tqp) {
+#else
+static void prio_enq(Thread *tp, ThreadsQueue *tqp) {
+#endif
+
+  Thread *cp = tqp->p_next;
+  while ((cp != (Thread *)tqp) && (cp->p_prio >= tp->p_prio))
+    cp = cp->p_next;
+  /* Insertion on p_prev.*/
+  tp->p_prev = (tp->p_next = cp)->p_prev;
+  tp->p_prev->p_next = cp->p_prev = tp;
+}
+
+/*
+ * 0 +++BA++------------------2+++--8++++++++++BR0----------------------------
+ * 1 .......++AA++--2+++++++++BA....8..........++++++++BR8++++AR1-------------
+ * 2 .............++AA..............................................----++AR++
+ * 8 .............................++AA........................++++++AR++......
+ *
+ *
+ * 5 ++++++++++++++++++AA+....9++++++++++++++AR5------------------------------
+ * 7 .....................++--------------------------------------------------
+ * 9 .......................++AA.............+++++++++AR++++++++++++++++++++++
+ */
+
+/**
+ * Locks the specified mutex.
+ * @param mp pointer to the \p Mutex structure
+ */
+void chMtxLock(Mutex *mp) {
+
+  chSysLock();
+
+  chMtxLockS(mp);
+
+  chSysUnlock();
+}
+
+/**
+ * Locks the specified mutex.
+ * @param mp pointer to the \p Mutex structure
+ * @note This function must be called within a \p chSysLock() / \p chSysUnlock()
+ *       block.
+ */
+void chMtxLockS(Mutex *mp) {
+
+  if (mp->m_owner != NULL) {
+    /*
+     * Inheritance, explores the thread-mutex dependances adjusting
+     * the priority of all the affected threads.
+     */
+    Thread *tp = mp->m_owner;
+    while (tp->p_prio < currp->p_prio) {
+      tp->p_prio = currp->p_prio;
+      switch (tp->p_state) {
+      case PRWTMTX:
+        prio_enq(dequeue(tp), &tp->p_mtxp->m_queue);
+        tp = tp->p_mtxp->m_owner;
+        continue;
+      case PRREADY:
+        chSchReadyI(dequeue(tp), RDY_OK);
+      }
+      break;
+    }
+    /*
+     * Goes to sleep on the mutex.
+     */
+    prio_enq(currp, &mp->m_queue);
+    currp->p_mtxp = mp;
+    chSchGoSleepS(PRWTMTX);
+    chDbgAssert(mp->m_owner == NULL, "chmtx.c, chMtxLockS()");
+  }
+  /*
+   * The mutex is now inserted in the owned mutexes list.
+   */
+  mp->m_owner = currp;
+  mp->m_next = currp->p_mtxlist;
+  currp->p_mtxlist = mp;
+}
+
+/**
+ * Tries to lock a mutex. This function does not have any overhead related to
+ * the priority inheritance mechanism because it does not try to enter a sleep
+ * state on the mutex.
+ * @param mp pointer to the \p Mutex structure
+ * @return \p TRUE if the mutex was successfully acquired else \p FALSE
+ */
+BOOL chMtxTryLock(Mutex *mp) {
+
+  chSysLock();
+
+  BOOL b = chMtxTryLockS(mp);
+
+  chSysUnlock();
+  return b;
+}
+
+/**
+ * Tries to lock a mutex. This function does not have any overhead related to
+ * the priority inheritance mechanism because it does not try to enter a sleep
+ * state on the mutex.
+ * @param mp pointer to the \p Mutex structure
+ * @return \p TRUE if the mutex was successfully acquired else \p FALSE
+ * @note This function must be called within a \p chSysLock() / \p chSysUnlock()
+ *       block.
+ */
+BOOL chMtxTryLockS(Mutex *mp) {
+
+  if (mp->m_owner != NULL)
+    return FALSE;
+  mp->m_owner = currp;
+  mp->m_next = currp->p_mtxlist;
+  currp->p_mtxlist = mp;
+  return TRUE;
+}
+
+/**
+ * Unlocks the next owned mutex in reverse lock order.
+ */
+void chMtxUnlock(void) {
+
+  chSysLock();
+
+  chMtxUnlockS();
+
+  chSysUnlock();
+}
+
+/**
+ * Unlocks the next owned mutex in reverse lock order.
+ * @note This function must be called within a \p chSysLock() / \p chSysUnlock()
+ *       block.
+ */
+void chMtxUnlockS(void) {
+
+  chDbgAssert((currp->p_mtxlist != NULL) && (currp->p_mtxlist->m_owner == currp),
+              "chmtx.c, chMtxUnlockS()");
+
+  /*
+   * Removes the top Mutex from the owned mutexes list and marks it as not owned.
+   */
+  Mutex *mp = currp->p_mtxlist;
+  currp->p_mtxlist = mp->m_next;
+  mp->m_owner = NULL;
+  /*
+   * If a thread is waiting on the mutex then the hard part begins.
+   */
+  if (chMtxQueueNotEmptyS(mp)) {
+    Thread *tp = fifo_remove(&mp->m_queue);
+    /*
+     * Recalculates the optimal thread priority by scanning the owned mutexes list.
+     */
+    t_prio newprio = currp->p_realprio;
+    mp = currp->p_mtxlist;
+    while (mp != NULL) {
+      if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio))
+        newprio = mp->m_queue.p_next->p_prio;
+      mp = mp->m_next;
+    }
+    currp->p_prio = newprio;
+    chSchWakeupS(tp, RDY_OK);
+  }
+}
+
+/**
+ * Unlocks all the mutexes owned by the invoking thread, this is <b>MUCH MORE</b>
+ * efficient than releasing the mutexes one by one and not just because the
+ * call overhead, this function does not have any overhead related to the
+ * priority inheritance mechanism.
+ */
+void chMtxUnlockAll(void) {
+
+  chSysLock();
+
+  chMtxUnlockAllS();
+
+  chSysUnlock();
+}
+
+/**
+ * Unlocks all the mutexes owned by the invoking thread, this is <b>MUCH MORE</b>
+ * efficient than releasing the mutexes one by one and not just because the
+ * call overhead, this function does not have any overhead related to the
+ * priority inheritance mechanism.
+ * @note This function must be called within a \p chSysLock() / \p chSysUnlock()
+ *       block.
+ */
+void chMtxUnlockAllS(void) {
+
+  if (currp->p_mtxlist != NULL) {
+    do {
+      Mutex *mp = currp->p_mtxlist;
+      currp->p_mtxlist = mp->m_next;
+      mp->m_owner = NULL;
+      if (chMtxQueueNotEmptyS(mp))
+        chSchReadyI(fifo_remove(&mp->m_queue), RDY_OK);
+    } while (currp->p_mtxlist != NULL);
+    currp->p_prio = currp->p_realprio;
+    chSchRescheduleS();
+  }
+}
+
+#endif /* CH_USE_MUTEXES */
+
+/** @} */
diff --git a/src/chsem.c b/src/chsem.c
index 083e25412..8e5073248 100644
--- a/src/chsem.c
+++ b/src/chsem.c
@@ -225,135 +225,6 @@ void chSemSignalWait(Semaphore *sps, Semaphore *spw) {
 }
 #endif /* CH_USE_SEMSW */
 
-#ifdef CH_USE_RT_SEMAPHORES
-/*
- * Inserts a thread into a list ordering it by priority.
- * @param tp the pointer to the thread to be inserted in the list
- * @param tqp the pointer to the threads list header
- * @note Usually you dont need to use this function in the user code unless
- *       you want to create some custom threads synchronization mechanism.
- */
-static void prioenq(Thread *tp, ThreadsQueue *tqp) {
-  Thread *cp;
-
-  cp = tqp->p_next;
-  while ((cp != (Thread *)tqp) && (cp->p_prio >= tp->p_prio))
-    cp = cp->p_next;
-  // Insertion on p_prev
-  tp->p_prev = (tp->p_next = cp)->p_prev;
-  tp->p_prev->p_next = cp->p_prev = tp;
-}
-
-/**
- * Performs a wait operation on a semaphore with priority boost.
- * @param sp pointer to a \p Semaphore structure
- * @note The function is available only if the \p CH_USE_RT_SEMAPHORES
- *       option is enabled in \p chconf.h.
- */
-void chSemRaisePrioWait(Semaphore *sp) {
-
-  chSysLock();
-
-  if (--sp->s_cnt < 0) {
-    prioenq(currp, &sp->s_queue);
-    currp->p_semp = sp;
-    chSchGoSleepS(PRWTSEM);
-  }
-
-  if (!currp->p_rtcnt++)
-    currp->p_prio += MEPRIO;
-
-  chSysUnlock();
-}
-
-/**
- * Performs a signal operation on a semaphore with return to normal priority.
- * @param sp pointer to a \p Semaphore structure
- * @note The function is available only if the \p CH_USE_RT_SEMAPHORES
- *       option is enabled in \p chconf.h.
- */
-void chSemLowerPrioSignal(Semaphore *sp) {
-
-  chSysLock();
-
-  if (!--currp->p_rtcnt) {
-    currp->p_prio -= MEPRIO;
-    if (sp->s_cnt++ < 0)
-      chSchReadyI(fifo_remove(&sp->s_queue), RDY_OK);
-    chSchRescheduleS();
-  }
-  else if (sp->s_cnt++ < 0)
-    chSchWakeupS(fifo_remove(&sp->s_queue), RDY_OK);
-
-  chSysUnlock();
-}
-
-#ifdef CH_USE_SEMSW
-/**
- * Performs atomic signal and wait operations on two semaphores with priority
- * boost.
- * @param sps pointer to a \p Semaphore structure to be signaled
- * @param spw pointer to a \p Semaphore structure to be wait on
- * @note The function is available only if the \p CH_USE_RT_SEMAPHORES
- *       option is enabled in \p chconf.h.
- */
-void chSemRaisePrioSignalWait(Semaphore *sps, Semaphore *spw) {
-
-  chSysLock();
-
-  if (sps->s_cnt++ < 0)
-    chSchReadyI(fifo_remove(&sps->s_queue), RDY_OK);
-
-  if (--spw->s_cnt < 0) {
-    prioenq(currp, &spw->s_queue);
-    currp->p_semp = spw;
-    chSchGoSleepS(PRWTSEM);
-
-    if (!currp->p_rtcnt++)
-      currp->p_prio += MEPRIO;
-  }
-  else {
-    if (!currp->p_rtcnt++)
-      currp->p_prio += MEPRIO;
-
-    chSchRescheduleS(); // Really needed ?
-  }
-
-  chSysUnlock();
-}
-
-/**
- * Performs atomic signal and wait operations on two semaphores with return
- * to normal priority.
- * @param sps pointer to a \p Semaphore structure to be signaled
- * @param spw pointer to a \p Semaphore structure to be wait on
- * @note The function is available only if the \p CH_USE_RT_SEMAPHORES
- *       option is enabled in \p chconf.h.
- */
-void chSemLowerPrioSignalWait(Semaphore *sps, Semaphore *spw) {
-
-  chSysLock();
-
-  if (!--currp->p_rtcnt)
-    currp->p_prio -= MEPRIO;
-
-  if (sps->s_cnt++ < 0)
-    chSchReadyI(fifo_remove(&sps->s_queue), RDY_OK);
-
-  if (--spw->s_cnt < 0) {
-    fifo_insert(currp, &spw->s_queue); // fifo_insert() because the spw is a normal sem.
-    currp->p_semp = spw;
-    chSchGoSleepS(PRWTSEM);
-  }
-  else
-    chSchRescheduleS();
-
-  chSysUnlock();
-}
-#endif /* CH_USE_SEMSW */
-
-#endif /* CH_USE_RT_SEMAPHORES */
-
 #endif /* CH_USE_SEMAPHORES */
 
 /** @} */
diff --git a/src/chthreads.c b/src/chthreads.c
index 1f37bb808..8a27acdb9 100644
--- a/src/chthreads.c
+++ b/src/chthreads.c
@@ -34,8 +34,9 @@ void _InitThread(t_prio prio, t_tmode mode, Thread *tp) {
   tp->p_flags = mode;
   tp->p_prio = prio;
   tp->p_rdymsg = RDY_OK;
-#ifdef CH_USE_RT_SEMAPHORES
-  tp->p_rtcnt = 0;
+#ifdef CH_USE_MUTEXES
+  tp->p_mtxlist = NULL;
+  tp->p_realprio = prio;
 #endif
 #ifdef CH_USE_WAITEXIT
   list_init(&tp->p_waiting);
@@ -125,11 +126,14 @@ void chThdSetPriority(t_prio newprio) {
   chDbgAssert(newprio <= HIGHPRIO, "chthreads.c, chThdSetPriority()")
   chSysLock();
 
-#ifdef CH_USE_RT_SEMAPHORES
-  if (currp->p_rtcnt)
-    currp->p_prio = newprio + MEPRIO;
+#ifdef CH_USE_MUTEXES
+  if (currp->p_prio != currp->p_realprio) {
+    if (newprio > currp->p_prio)
+      currp->p_prio = newprio;
+  }
   else
     currp->p_prio = newprio;
+  currp->p_realprio = newprio;
 #else
   currp->p_prio = newprio;
 #endif
diff --git a/src/include/ch.h b/src/include/ch.h
index be251415f..2c2bb2f7c 100644
--- a/src/include/ch.h
+++ b/src/include/ch.h
@@ -57,6 +57,10 @@
 #include "semaphores.h"
 #endif
 
+#ifndef _MUTEXES_H_
+#include "mutexes.h"
+#endif
+
 #ifndef _EVENTS_H_
 #include "events.h"
 #endif
diff --git a/src/include/mutexes.h b/src/include/mutexes.h
new file mode 100644
index 000000000..e2881423d
--- /dev/null
+++ b/src/include/mutexes.h
@@ -0,0 +1,66 @@
+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+ * @addtogroup Mutexes
+ * @{
+ */
+
+#ifndef _MUTEXES_H_
+#define _MUTEXES_H_
+
+#ifdef CH_USE_MUTEXES
+
+typedef struct Mutex Mutex;
+
+struct Mutex {
+  /** Queue of the threads sleeping on this Mutex.*/
+  ThreadsQueue  m_queue;
+  /** Owner \p Thread pointer or \p NULL.*/
+  Thread        *m_owner;
+  /** Next \p Mutex into an owner-list, \p NULL if none.*/
+  Mutex         *m_next;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chMtxInit(Mutex *mp);
+  void chMtxLock(Mutex *mp);
+  void chMtxLockS(Mutex *mp);
+  BOOL chMtxTryLock(Mutex *mp);
+  BOOL chMtxTryLockS(Mutex *mp);
+  void chMtxUnlock(void);
+  void chMtxUnlockS(void);
+  void chMtxUnlockAll(void);
+  void chMtxUnlockAllS(void);
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * Returns \p TRUE if the mutex queus contains at least a waiting thread.
+ */
+#define chMtxQueueNotEmptyS(mp) notempty(&(mp)->m_queue)
+
+#endif /* CH_USE_MUTEXES */
+
+#endif /* _MUTEXES_H_ */
+
+/** @} */
diff --git a/src/include/semaphores.h b/src/include/semaphores.h
index e17d03a6a..0457da81d 100644
--- a/src/include/semaphores.h
+++ b/src/include/semaphores.h
@@ -52,13 +52,6 @@ extern "C" {
   void chSemSignal(Semaphore *sp);
   void chSemSignalI(Semaphore *sp);
   void chSemSignalWait(Semaphore *sps, Semaphore *spw);
-
-#ifdef CH_USE_RT_SEMAPHORES
-  void chSemRaisePrioWait(Semaphore *sp);
-  void chSemLowerPrioSignal(Semaphore *sp);
-  void chSemRaisePrioSignalWait(Semaphore *sps, Semaphore *spw);
-  void chSemLowerPrioSignalWait(Semaphore *sps, Semaphore *spw);
-#endif
 #ifdef __cplusplus
 }
 #endif
@@ -82,6 +75,6 @@ extern "C" {
 
 #endif /* CH_USE_SEMAPHORES */
 
-#endif /* _SEM_H_ */
+#endif /* _SEMAPHORES_H_ */
 
 /** @} */
diff --git a/src/include/threads.h b/src/include/threads.h
index 0783c4e9a..241b44442 100644
--- a/src/include/threads.h
+++ b/src/include/threads.h
@@ -62,6 +62,9 @@ struct Thread {
     /** Semaphore where the thread is waiting on (only in \p PRWTSEM state).*/
     Semaphore       *p_semp;
 #endif
+#ifdef CH_USE_MUTEXES
+    Mutex           *p_mtxp;
+#endif
 #ifdef CH_USE_EVENTS
     /** Enabled events mask (only while in \p PRWTEVENT state).*/
     t_eventmask     p_ewmask;
@@ -96,9 +99,10 @@ struct Thread {
   /** Pending events mask.*/
   t_eventmask       p_epending;
 #endif
-#ifdef CH_USE_RT_SEMAPHORES
-  /** RT semaphores depth counter.*/
-  t_cnt             p_rtcnt;
+#ifdef CH_USE_MUTEXES
+  /** Owner mutexes list, \p NULL terminated.*/
+  Mutex             *p_mtxlist;
+  t_prio            p_realprio;
 #endif
 };
 
@@ -112,18 +116,20 @@ struct Thread {
 #define PRSUSPENDED 3
 /** Thread state: Waiting on a semaphore.*/
 #define PRWTSEM     4
+/** Thread state: Waiting on a mutex.*/
+#define PRWTMTX     5
 /** Thread state: Waiting in \p chThdSleep() or \p chThdSleepUntil().*/
-#define PRSLEEP     5
+#define PRSLEEP     6
 /** Thread state: Waiting in \p chThdWait().*/
-#define PRWAIT      6
+#define PRWAIT      7
 /** Thread state: Waiting in \p chEvtWait().*/
-#define PRWTEVENT   7
+#define PRWTEVENT   8
 /** Thread state: Waiting in \p chMsgSend().*/
-#define PRSNDMSG    8
+#define PRSNDMSG    9
 /** Thread state: Waiting in \p chMsgWait().*/
-#define PRWTMSG     9
+#define PRWTMSG     10
 /** Thread state: After termination.*/
-#define PREXIT      10
+#define PREXIT      11
 
 /** Thread option: Termination requested flag.*/
 #define P_TERMINATE 1
@@ -138,8 +144,6 @@ struct Thread {
 #define NORMALPRIO  64
 /** Highest user priority.*/
 #define HIGHPRIO    127
-/** Boosted base priority.*/
-#define MEPRIO      128
 /** Greatest possible priority.*/
 #define ABSPRIO     255
 
diff --git a/src/templates/chconf.h b/src/templates/chconf.h
index 187eb2373..fb8ae6a69 100644
--- a/src/templates/chconf.h
+++ b/src/templates/chconf.h
@@ -78,10 +78,9 @@
  * @note requires \p CH_USE_VIRTUAL_TIMERS.*/
 #define CH_USE_SEMAPHORES_TIMEOUT
 
-/** Configuration option: if specified then the Semaphores APIs with priority
- *  shift are included in the kernel.
- * @note requires \p CH_USE_SEMAPHORES.*/
-#define CH_USE_RT_SEMAPHORES
+/** Configuration option: if specified then the Mutexes APIs are included in
+ * the kernel.*/
+#define CH_USE_MUTEXES
 
 /** Configuration option: if specified then the Events APIs are included in
  *  the kernel.*/
diff --git a/test/test.c b/test/test.c
index 15b70b9d4..5f6736843 100644
--- a/test/test.c
+++ b/test/test.c
@@ -40,6 +40,7 @@ static Thread *t1, *t2, *t3, *t4, *t5;
 
 static FullDuplexDriver *comp;
 static Semaphore sem1;
+static Mutex m1, m2;
 
 static void wait(void) {
 
@@ -92,8 +93,9 @@ t_msg Thread2(void *p) {
 
 t_msg Thread3(void *p) {
 
-  chSemRaisePrioWait(&sem1);
+  chMtxLock(&m1);
   chFDDPut(comp, *(BYTE8 *)p);
+  chMtxUnlock();
   return 0;
 }
 
@@ -129,25 +131,8 @@ t_msg Thread7(void *p) {
   return (unsigned int)p + 1;
 }
 
-/**
- * Tester thread, this thread must be created with priority \p NORMALPRIO.
- */
-t_msg TestThread(void *p) {
-  static BYTE8 ib[16];
-  static Queue iq;
-  t_msg msg;
-  unsigned int i;
-  t_time time;
+static void testrdy1(void) {
 
-  comp = p;
-  println("*****************************");
-  println("*** ChibiOS/RT test suite ***");
-  println("*****************************");
-  println("");
-
-  /*
-   * Ready list ordering test.
-   */
   println("*** Ready List, priority enqueuing test #1, you should read ABCDE:");
   t5 = chThdCreate(chThdGetPriority()-5, 0, wsT5, sizeof(wsT5), Thread1, "E");
   t4 = chThdCreate(chThdGetPriority()-4, 0, wsT4, sizeof(wsT4), Thread1, "D");
@@ -156,6 +141,10 @@ t_msg TestThread(void *p) {
   t1 = chThdCreate(chThdGetPriority()-1, 0, wsT1, sizeof(wsT1), Thread1, "A");
   wait();
   println("");
+}
+
+static void testrdy2(void) {
+
   println("*** Ready List, priority enqueuing test #2, you should read ABCDE:");
   t4 = chThdCreate(chThdGetPriority()-4, 0, wsT4, sizeof(wsT4), Thread1, "D");
   t5 = chThdCreate(chThdGetPriority()-5, 0, wsT5, sizeof(wsT5), Thread1, "E");
@@ -164,12 +153,12 @@ t_msg TestThread(void *p) {
   t3 = chThdCreate(chThdGetPriority()-3, 0, wsT3, sizeof(wsT3), Thread1, "C");
   wait();
   println("");
+}
+
+static void testsem1(void) {
 
-  /*
-   * Semaphores test.
-   */
-  chSemInit(&sem1, 0);
   println("*** Semaphores, FIFO enqueuing test, you should read ABCDE:");
+  chSemInit(&sem1, 0);
   t1 = chThdCreate(chThdGetPriority()+5, 0, wsT1, sizeof(wsT1), Thread2, "A");
   t2 = chThdCreate(chThdGetPriority()+1, 0, wsT2, sizeof(wsT2), Thread2, "B");
   t3 = chThdCreate(chThdGetPriority()+3, 0, wsT3, sizeof(wsT3), Thread2, "C");
@@ -182,45 +171,111 @@ t_msg TestThread(void *p) {
   chSemSignal(&sem1);
   wait();
   println("");
-  println("*** Semaphores, priority enqueuing test #1, you should read ABCDE:");
-  chSemInit(&sem1, 0);
-  t5 = chThdCreate(chThdGetPriority()+1, 0, wsT5, sizeof(wsT5), Thread3, "E");
-  t4 = chThdCreate(chThdGetPriority()+2, 0, wsT4, sizeof(wsT4), Thread3, "D");
-  t3 = chThdCreate(chThdGetPriority()+3, 0, wsT3, sizeof(wsT3), Thread3, "C");
-  t2 = chThdCreate(chThdGetPriority()+4, 0, wsT2, sizeof(wsT2), Thread3, "B");
-  t1 = chThdCreate(chThdGetPriority()+5, 0, wsT1, sizeof(wsT1), Thread3, "A");
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  wait();
-  println("");
-  println("*** Semaphores, priority enqueuing test #2, you should read ABCDE:");
-  chSemInit(&sem1, 0);
-  t4 = chThdCreate(chThdGetPriority()+2, 0, wsT4, sizeof(wsT4), Thread3, "D");
-  t5 = chThdCreate(chThdGetPriority()+1, 0, wsT5, sizeof(wsT5), Thread3, "E");
-  t1 = chThdCreate(chThdGetPriority()+5, 0, wsT1, sizeof(wsT1), Thread3, "A");
-  t2 = chThdCreate(chThdGetPriority()+4, 0, wsT2, sizeof(wsT2), Thread3, "B");
-  t3 = chThdCreate(chThdGetPriority()+3, 0, wsT3, sizeof(wsT3), Thread3, "C");
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  chSemLowerPrioSignal(&sem1);
-  wait();
-  println("");
+}
+
+static void testsem2(void) {
+  unsigned int i;
+
   println("*** Semaphores, timeout test, you should read ABCDE (slowly):");
   chSemInit(&sem1, 0);
   for (i = 0; i < 5; i++) {
-    chFDDPut(comp, 'A'+i);
+    chFDDPut(comp, 'A' + i);
     chSemWaitTimeout(&sem1, 500);
   }
   println("");
+}
+
+static void testmtx1(void) {
+
+  chMtxInit(&m1);
+  println("*** Mutexes, priority enqueuing test, you should read ABCDE:");
+  chMtxLock(&m1);
+  t5 = chThdCreate(chThdGetPriority()+1, 0, wsT5, sizeof(wsT5), Thread3, "E");
+  t4 = chThdCreate(chThdGetPriority()+3, 0, wsT4, sizeof(wsT4), Thread3, "D");
+  t3 = chThdCreate(chThdGetPriority()+3, 0, wsT3, sizeof(wsT3), Thread3, "C");
+  t2 = chThdCreate(chThdGetPriority()+4, 0, wsT2, sizeof(wsT2), Thread3, "B");
+  t1 = chThdCreate(chThdGetPriority()+5, 0, wsT1, sizeof(wsT1), Thread3, "A");
+  chMtxUnlock();
+  wait();
+  println("");
+}
+
+t_msg Thread8(void *p) {
+
+  chThdSleep(5);
+  chMtxLock(&m1);
+  chMtxUnlock();
+  chFDDPut(comp, *(BYTE8 *)p);
+  return 0;
+}
+
+t_msg Thread9(void *p) {
+
+  chMtxLock(&m1);
+  chThdSleep(20);
+  chMtxUnlock();
+  chFDDPut(comp, *(BYTE8 *)p);
+  return 0;
+}
+
+t_msg Thread10(void *p) {
+
+  chThdSleep(10);
+  /* 50mS CPU pulse */
+  t_time time = chSysGetTime() + 50;
+  while (chSysGetTime() != time)
+    ;
+  chFDDPut(comp, *(BYTE8 *)p);
+  return 0;
+}
+
+t_msg Thread11(void *p) {
+
+  chThdSleep(5);
+  chSemWait(&sem1);
+  chSemSignal(&sem1);
+  chFDDPut(comp, *(BYTE8 *)p);
+  return 0;
+}
+
+t_msg Thread12(void *p) {
+
+  chSemWait(&sem1);
+  chThdSleep(20);
+  chSemSignal(&sem1);
+  chFDDPut(comp, *(BYTE8 *)p);
+  return 0;
+}
+
+static void testmtx2(void) {
+
+  chMtxInit(&m1);
+  println("*** Mutexes, mutex with inheritance, you should read ABC:");
+  t1 = chThdCreate(chThdGetPriority()-1, 0, wsT1, sizeof(wsT1), Thread8, "A");
+  t2 = chThdCreate(chThdGetPriority()-3, 0, wsT2, sizeof(wsT2), Thread9, "C");
+  t3 = chThdCreate(chThdGetPriority()-2, 0, wsT3, sizeof(wsT3), Thread10, "B");
+  chThdWait(t1);
+  chThdWait(t2);
+  chThdWait(t3);
+  println("");
+}
+
+static void testmtx3(void) {
+
+  chSemInit(&sem1, 1);
+  println("*** Mutexes, mutex without inheritance, inversion happens, you should read BAC:");
+  t1 = chThdCreate(chThdGetPriority()-1, 0, wsT1, sizeof(wsT1), Thread11, "A");
+  t2 = chThdCreate(chThdGetPriority()-3, 0, wsT2, sizeof(wsT2), Thread12, "C");
+  t3 = chThdCreate(chThdGetPriority()-2, 0, wsT3, sizeof(wsT3), Thread10, "B");
+  chThdWait(t1);
+  chThdWait(t2);
+  chThdWait(t3);
+  println("");
+}
+
+static void testmsg1(void) {
+  t_msg msg;
 
-  /*
-   * Messages test.
-   */
   println("*** Messages, dispatch test, you should read AABBCCDDEE:");
   t1 = chThdCreate(chThdGetPriority()-1, 0, wsT1, sizeof(wsT1), Thread4, chThdSelf());
   do {
@@ -230,19 +285,26 @@ t_msg TestThread(void *p) {
   } while (msg);
   chThdWait(t1);
   println("");
+}
+
+static void testmsg2(void) {
+  unsigned int i;
+
   println("*** Messages, timeout test, you should read ABCDE (slowly):");
   t1 = chThdCreate(chThdGetPriority()-1, 0, wsT1, sizeof(wsT1), Thread5, chThdSelf());
   for (i = 0; i < 5; i++) {
-    chFDDPut(comp, 'A'+i);
-    chMsgSendTimeout(t1, 'A'+i, 500);
+    chFDDPut(comp, 'A' + i);
+    chMsgSendTimeout(t1, 'A' + i, 500);
   }
   chMsgSendTimeout(t1, 0, 500);
   chThdWait(t1);
   println("");
+}
+
+static void bench1(void) {
+  unsigned int i;
+  t_time time;
 
-  /*
-   * Kernel benchmarks.
-   */
   println("*** Kernel Benchmark, context switch stress test:");
   time = chSysGetTime() + 1;
   while (chSysGetTime() < time) {
@@ -266,6 +328,11 @@ t_msg TestThread(void *p) {
   print(" msgs/S, ");
   printn(i << 1);
   println(" ctxsws/S");
+}
+
+static void bench2(void) {
+  unsigned int i;
+  t_time time;
 
   println("*** Kernel Benchmark, threads creation/termination:");
   time = chSysGetTime() + 1;
@@ -286,6 +353,13 @@ t_msg TestThread(void *p) {
   print("Threads throughput = ");
   printn(i);
   println(" threads/S");
+}
+
+static void bench3(void) {
+  static BYTE8 ib[16];
+  static Queue iq;
+  unsigned int i;
+  t_time time;
 
   println("*** Kernel Benchmark, I/O Queues throughput:");
   chIQInit(&iq, ib, sizeof(ib), NULL);
@@ -311,6 +385,50 @@ t_msg TestThread(void *p) {
   print("Queues throughput = ");
   printn(i * 4);
   println(" bytes/S");
+}
+
+/**
+ * Tester thread, this thread must be created with priority \p NORMALPRIO.
+ */
+t_msg TestThread(void *p) {
+
+  comp = p;
+  println("*****************************");
+  println("*** ChibiOS/RT test suite ***");
+  println("*****************************");
+  println("");
+
+  /*
+   * Ready list ordering tests.
+   */
+  testrdy1();
+  testrdy2();
+
+  /*
+   * Semaphores tests.
+   */
+  testsem1();
+  testsem2();
+
+  /*
+   * Mutexes tests.
+   */
+  testmtx1();
+  testmtx2();
+  testmtx3();
+
+  /*
+   * Messages tests.
+   */
+  testmsg1();
+  testmsg2();
+
+  /*
+   * Kernel benchmarks.
+   */
+  bench1();
+  bench2();
+  bench3();
 
   println("\r\nTest complete");
   return 0;