diff --git a/demos/LPC214x-GCC/Makefile b/demos/LPC214x-GCC/Makefile
index e96550727..7f75b31b2 100644
--- a/demos/LPC214x-GCC/Makefile
+++ b/demos/LPC214x-GCC/Makefile
@@ -64,9 +64,9 @@ UADEFS =
 # List ARM-mode C source files here
 ASRC = chcore.c main.c buzzer.c \
        ../../ports/ARM7-LPC214x/GCC/lpc214x_serial.c  \
-       ../../src/chinit.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/chinit.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
 
 # List THUMB-mode C sources here
 # NOTE: If any module is compiled in thumb mode then -mthumb-interwork is
@@ -112,7 +112,7 @@ ADEFS   = $(DADEFS) $(UADEFS)
 AOBJS   = $(ASRC:.c=.o)
 TOBJS   = $(TSRC:.c=.o)
 OBJS	= $(ASMOBJS) $(AOBJS) $(TOBJS)
-ASMOBJS = $(ASMSRC:.s=.o) 
+ASMOBJS = $(ASMSRC:.s=.o)
 LIBS    = $(DLIBS) $(ULIBS)
 MCFLAGS = -mcpu=$(MCU)
 
diff --git a/demos/LPC214x-GCC/chtypes.h b/demos/LPC214x-GCC/chtypes.h
index 49c7c6c38..803b5ead4 100644
--- a/demos/LPC214x-GCC/chtypes.h
+++ b/demos/LPC214x-GCC/chtypes.h
@@ -25,28 +25,22 @@
  */
 #define BOOL        char
 #define BYTE8       unsigned char
+#define SBYTE8      char
 #define WORD16      short
 #define UWORD16     unsigned short
 #define LONG32      int
 #define ULONG32     unsigned int
-#define PTR_EQ      int
 
 typedef BYTE8       t_tmode;
 typedef BYTE8       t_tstate;
-typedef LONG32      t_prio;
-typedef PTR_EQ      t_msg;
+typedef ULONG32     t_prio;
+typedef LONG32      t_msg;
 typedef LONG32      t_eventid;
 typedef ULONG32     t_eventmask;
 typedef ULONG32     t_time;
-typedef LONG32      t_semcnt;
+typedef LONG32      t_cnt;
 typedef ULONG32     t_size;
 
-#define MINPRIO     0x8000
-#define MAXPRIO     0x7fff
-
-#define MINDELTA    0
-#define MAXDELTA    0xffff
-
 #define INLINE      inline
 
 #endif /* _CHTYPES_H_ */
diff --git a/demos/Win32-MSVS/chtypes.h b/demos/Win32-MSVS/chtypes.h
index f7d05b855..6481c22a8 100644
--- a/demos/Win32-MSVS/chtypes.h
+++ b/demos/Win32-MSVS/chtypes.h
@@ -25,28 +25,22 @@
  */
 #define BOOL        char
 #define BYTE8       unsigned char
+#define SBYTE8      char
 #define WORD16      short
 #define UWORD16     unsigned short
 #define LONG32      int
 #define ULONG32     unsigned int
-#define PTR_EQ      int
 
 typedef BYTE8       t_tmode;
 typedef BYTE8       t_tstate;
-typedef LONG32      t_prio;
-typedef PTR_EQ      t_msg;
+typedef ULONG32     t_prio;
+typedef LONG32      t_msg;
 typedef LONG32      t_eventid;
 typedef ULONG32     t_eventmask;
 typedef ULONG32     t_time;
-typedef LONG32      t_semcnt;
+typedef LONG32      t_cnt;
 typedef ULONG32     t_size;
 
-#define MINPRIO     0x8000
-#define MAXPRIO     0x7fff
-
-#define MINDELTA    0
-#define MAXDELTA    0xffff
-
 #define INLINE      __inline
 
 #endif /* _CHTYPES_H_ */
diff --git a/demos/Win32-MinGW/Makefile b/demos/Win32-MinGW/Makefile
index b7dc8b024..4ab603d8e 100644
--- a/demos/Win32-MinGW/Makefile
+++ b/demos/Win32-MinGW/Makefile
@@ -58,9 +58,9 @@ UADEFS =
 # List C source files here
 SRC  = chcore.c demo.c \
        ../../ports/win32/simcom.c \
-       ../../src/chinit.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/chinit.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
 
 # List ASM source files here
 ASRC = chcore2.s
diff --git a/demos/Win32-MinGW/chcore.h b/demos/Win32-MinGW/chcore.h
index b8872c819..7f0021854 100644
--- a/demos/Win32-MinGW/chcore.h
+++ b/demos/Win32-MinGW/chcore.h
@@ -63,7 +63,7 @@ typedef struct {
 
 #define INT_REQUIRED_STACK 0x0
 
-#define UserStackSize(n) (sizeof(Thread) + sizeof(PTR_EQ) + sizeof(PTR_EQ) + \
+#define UserStackSize(n) (sizeof(Thread) + sizeof(void *) * 2 + \
                           sizeof(struct stackregs) + (n) + (INT_REQUIRED_STACK))
 
 __attribute__((fastcall)) void chSysHalt(void);
diff --git a/demos/Win32-MinGW/chtypes.h b/demos/Win32-MinGW/chtypes.h
index 49c7c6c38..803b5ead4 100644
--- a/demos/Win32-MinGW/chtypes.h
+++ b/demos/Win32-MinGW/chtypes.h
@@ -25,28 +25,22 @@
  */
 #define BOOL        char
 #define BYTE8       unsigned char
+#define SBYTE8      char
 #define WORD16      short
 #define UWORD16     unsigned short
 #define LONG32      int
 #define ULONG32     unsigned int
-#define PTR_EQ      int
 
 typedef BYTE8       t_tmode;
 typedef BYTE8       t_tstate;
-typedef LONG32      t_prio;
-typedef PTR_EQ      t_msg;
+typedef ULONG32     t_prio;
+typedef LONG32      t_msg;
 typedef LONG32      t_eventid;
 typedef ULONG32     t_eventmask;
 typedef ULONG32     t_time;
-typedef LONG32      t_semcnt;
+typedef LONG32      t_cnt;
 typedef ULONG32     t_size;
 
-#define MINPRIO     0x8000
-#define MAXPRIO     0x7fff
-
-#define MINDELTA    0
-#define MAXDELTA    0xffff
-
 #define INLINE      inline
 
 #endif /* _CHTYPES_H_ */
diff --git a/readme.txt b/readme.txt
index 4508b2a21..347262f18 100644
--- a/readme.txt
+++ b/readme.txt
@@ -32,6 +32,25 @@ LPC214x-GCC         - ChibiOS/RT port for ARM7 LPC2148, the demo targets the
 *** Releases                                                              ***
 *****************************************************************************
 
+*** 0.3.1 ***
+- Lists code moved into chlists.c from various other places and reorganized.
+- The list of the threads waiting in chThdWait() is now a single link list,
+  this saves some space.
+- Cleaned the template files code, the files contained some obsolete
+  declarations.
+- Code optimization in scSemSignalWait().
+- Code optimization in chEvtSend().
+- Added a Semaphore pointer to the Thread structure, this allows to know on
+  which semaphore a thread is waiting on. It takes no space because it is
+  located in the union inside the Thread structure. This also allowed a minor
+  optimization inside chSemWaitTimeout() and chSemWaitTimeoutS().
+- Changed the priority constants in order to make the t_prio type compatible
+  with a signed byte, this is very important for 8 bits architectures.
+  Now the threads priorities can range from 1 to 63, more than enough anyway.
+- Fixed bug in chEvtWaitTimeout(), the timeout code performed a useless
+  dequeue operation.
+- Fixed bug on RT semaphores, the priority queuing was broken.
+
 *** 0.3.0 ***
 - ChibiOS/RT goes beta.
 - Diet for the threads code, some simple APIs become macros.
diff --git a/src/chdelta.c b/src/chdelta.c
index 9a4f8b21d..e627cbf19 100644
--- a/src/chdelta.c
+++ b/src/chdelta.c
@@ -33,7 +33,7 @@ DeltaList dlist;
 void chVTInit(void) {
 
   dlist.dl_next = dlist.dl_prev = (VirtualTimer *)&dlist;
-  dlist.dl_dtime = MAXDELTA;
+  dlist.dl_dtime = (t_time)-1;
 }
 
 /**
diff --git a/src/chevents.c b/src/chevents.c
index 87f990c16..4dcf37a22 100644
--- a/src/chevents.c
+++ b/src/chevents.c
@@ -92,7 +92,6 @@ void chEvtClear(t_eventmask mask) {
  */
 void chEvtSend(EventSource *esp) {
   EventListener *elp;
-  BOOL flag = FALSE;
 
   chSysLock();
 
@@ -102,11 +101,10 @@ void chEvtSend(EventSource *esp) {
 
     tp->p_epending |= EventMask(elp->el_id);
     if ((tp->p_state == PRWTEVENT) && (tp->p_epending & tp->p_ewmask))
-      chSchReadyI(tp), flag = TRUE;
+      chSchReadyI(tp);
     elp = elp->el_next;
   }
-  if (flag)
-    chSchRescheduleI();
+  chSchRescheduleI();
 
   chSysUnlock();
 }
@@ -174,7 +172,7 @@ static void unwait(void *p) {
 
 // Test removed, it should never happen.
 //  if (((Thread *)p)->p_state == PRWTEVENT)
-  chSchReadyI(dequeue(p))->p_rdymsg = RDY_TIMEOUT;
+  chSchReadyI(p)->p_rdymsg = RDY_TIMEOUT;
 }
 
 /**
diff --git a/src/chlists.c b/src/chlists.c
new file mode 100644
index 000000000..d01c8c37c
--- /dev/null
+++ b/src/chlists.c
@@ -0,0 +1,89 @@
+/*
+    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 Messages
+ * @{
+ */
+#include <ch.h>
+
+#ifndef CH_OPTIMIZE_SPEED
+/*
+ * Inserts a thread into a FIFO queue.
+ * @param tp the pointer to the thread to be inserted in the list
+ * @param tqp the pointer to the threads list header
+ */
+void fifo_insert(Thread *tp, ThreadsQueue *tqp) {
+
+  tp->p_next = (Thread *)tqp;
+  tp->p_prev = tqp->p_prev;
+  tqp->p_prev->p_next = tp;
+  tqp->p_prev = tp;
+}
+
+/*
+ * Removes a thread from a FIFO queue.
+ * @param tqp the pointer to the threads list header
+ */
+Thread *fifo_remove(ThreadsQueue *tqp) {
+  Thread *tp = tqp->p_next;
+
+  (tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
+  return tp;
+}
+
+/*
+ * Removes a Thread from a FIFO list and returns it.
+ * @param tp the pointer to the thread to be removed from the list
+ * @param tqp the pointer to the list header
+ * @return the removed thread pointer
+ */
+Thread *dequeue(Thread *tp) {
+
+  tp->p_prev->p_next = tp->p_next;
+  tp->p_next->p_prev = tp->p_prev;
+  return tp;
+}
+
+/*
+ * Inserts a Thread into a stack list.
+ * @param tp the pointer to the thread to be inserted in the list
+ * @param tlp the pointer to the threads list header
+ */
+void list_insert(Thread *tp, ThreadsList *tlp) {
+
+  tp->p_next = tlp->p_next;
+  tlp->p_next = tp;
+}
+
+
+/*
+ * Removes a Thread from a stack list and returns it.
+ * @param tlp the pointer to the threads list header
+ * @return the removed thread pointer
+ */
+Thread *list_remove(ThreadsList *tlp) {
+
+  Thread *tp = tlp->p_next;
+  tlp->p_next = tp->p_next;
+  return tp;
+}
+#endif /* CH_OPTIMIZE_SPEED */
+
+/** @} */
diff --git a/src/chmsg.c b/src/chmsg.c
index 7b22a69b2..7a25502b0 100644
--- a/src/chmsg.c
+++ b/src/chmsg.c
@@ -36,7 +36,7 @@ t_msg chMsgSend(Thread *tp, t_msg msg) {
 
   chSysLock();
 
-  enqueue(currp, &tp->p_msgqueue);
+  fifo_insert(currp, &tp->p_msgqueue);
   if (tp->p_state == PRWTMSG)
     chSchReadyI(tp);
   currp->p_msg = msg;
@@ -66,7 +66,7 @@ t_msg chMsgSendWithEvent(Thread *tp, t_msg msg, EventSource *esp) {
 
   chSysLock();
 
-  enqueue(currp, &tp->p_msgqueue);
+  fifo_insert(currp, &tp->p_msgqueue);
 //  if (tp->p_state == PRWTMSG)
 //    chSchReadyI(tp);
   chEvtSendI(esp);
@@ -109,7 +109,7 @@ t_msg chMsgSendTimeout(Thread *tp, t_msg msg, t_time time) {
   chSysLock();
 
   chVTSetI(&vt, time, unsend, currp);
-  enqueue(currp, &tp->p_msgqueue);
+  fifo_insert(currp, &tp->p_msgqueue);
   if (tp->p_state == PRWTMSG)
     chSchReadyI(tp);
   currp->p_msg = msg;
@@ -183,7 +183,7 @@ void chMsgRelease(t_msg msg) {
   chSysLock();
 
 //  if (!chMsgIsPendingI(currp)
-  chSchWakeupI(dequeue(currp->p_msgqueue.p_next), msg);
+  chSchWakeupI(fifo_remove(&currp->p_msgqueue), msg);
 
   chSysUnlock();
 }
diff --git a/src/chqueues.c b/src/chqueues.c
index 2cccc3120..225492e60 100644
--- a/src/chqueues.c
+++ b/src/chqueues.c
@@ -212,7 +212,7 @@ void chOQReset(Queue *qp) {
   chSysLock();
 
   qp->q_rdptr = qp->q_wrptr = qp->q_buffer;
-  chSemResetI(&qp->q_sem, (t_semcnt)(qp->q_top - qp->q_buffer));
+  chSemResetI(&qp->q_sem, (t_cnt)(qp->q_top - qp->q_buffer));
 
   chSysUnlock();
 }
diff --git a/src/chschd.c b/src/chschd.c
index a046f4fbf..81031f8c4 100644
--- a/src/chschd.c
+++ b/src/chschd.c
@@ -32,7 +32,7 @@ static ReadyList rlist;
 Thread *currp;
 #endif
 
-static UWORD16 preempt;
+static t_cnt preempt;
 #ifdef CH_USE_SYSTEMTIME
 t_time stime;
 #endif
@@ -46,7 +46,7 @@ t_time stime;
 void chSchInit(void) {
 
   rlist.p_next = rlist.p_prev = (Thread *)&rlist;
-  rlist.p_prio = MAXPRIO;
+  rlist.p_prio = ABSPRIO;
   preempt = CH_TIME_QUANTUM;
 #ifdef CH_USE_SYSTEMTIME
   stime = 0;
@@ -68,11 +68,12 @@ Thread *chSchReadyI(Thread *tp) {
 
   tp->p_state = PRREADY;
   tp->p_rdymsg = RDY_OK;
-  cp = rlist.p_next;
+  cp = rlist.p_prev;
   while (cp->p_prio < prio)
-    cp = cp->p_next;
-  tp->p_prev = (tp->p_next = cp)->p_prev;
-  tp->p_prev->p_next = cp->p_prev = tp;
+    cp = cp->p_prev;
+  // Insertion on p_next
+  tp->p_next = (tp->p_prev = cp)->p_next;
+  tp->p_next->p_prev = cp->p_next = tp;
   return tp;
 }
 
@@ -83,7 +84,7 @@ Thread *chSchReadyI(Thread *tp) {
 static void nextready(void) {
   Thread *otp = currp;
 
-  (currp = dequeue(rlist.p_prev))->p_state = PRCURR;
+  (currp = fifo_remove((ThreadsQueue *)&rlist))->p_state = PRCURR;
   preempt = CH_TIME_QUANTUM;
   chSysSwitchI(&otp->p_ctx, &currp->p_ctx);
 }
@@ -135,7 +136,7 @@ void chSchWakeupI(Thread *tp, t_msg msg) {
  */
 void chSchRescheduleI(void) {
 
-  if (isempty(&rlist) || lastprio(&rlist) <= currp->p_prio)
+  if (isempty(&rlist) || firstprio(&rlist) <= currp->p_prio)
     return;
 
   chSchDoRescheduleI();
@@ -162,11 +163,11 @@ BOOL chSchRescRequiredI(void) {
     return FALSE;
 
   if (preempt) {
-    if (lastprio(&rlist) <= currp->p_prio)
+    if (firstprio(&rlist) <= currp->p_prio)
       return FALSE;
   }
   else { /* Time quantum elapsed. */
-    if (lastprio(&rlist) < currp->p_prio)
+    if (firstprio(&rlist) < currp->p_prio)
       return FALSE;
   }
   return TRUE;
diff --git a/src/chsem.c b/src/chsem.c
index 058187050..5c1ff0544 100644
--- a/src/chsem.c
+++ b/src/chsem.c
@@ -31,10 +31,10 @@
  * @param n initial value of the semaphore counter. Must be non-negative.
  * @note Can be called with interrupts disabled or enabled.
  */
-void chSemInit(Semaphore *sp, t_semcnt n) {
+void chSemInit(Semaphore *sp, t_cnt n) {
 
+  fifo_init(&sp->s_queue);
   sp->s_cnt = n;
-  sp->s_queue.p_next = sp->s_queue.p_prev = (Thread *)&sp->s_queue;
 }
 
 /**
@@ -45,8 +45,8 @@ void chSemInit(Semaphore *sp, t_semcnt n) {
  *       instead than a signal because the \p p_rdymsg field is set to
  *       \p RDY_RESET.
  */
-void chSemReset(Semaphore *sp, t_semcnt n) {
-  t_semcnt cnt;
+void chSemReset(Semaphore *sp, t_cnt n) {
+  t_cnt cnt;
 
   chSysLock();
 
@@ -54,7 +54,7 @@ void chSemReset(Semaphore *sp, t_semcnt n) {
   sp->s_cnt = n;
   if (cnt < 0) {
     while (cnt++)
-      chSchReadyI(dequeue(sp->s_queue.p_next))->p_rdymsg = RDY_RESET;
+      chSchReadyI(fifo_remove(&sp->s_queue))->p_rdymsg = RDY_RESET;
     chSchRescheduleI();
   }
 
@@ -70,13 +70,13 @@ void chSemReset(Semaphore *sp, t_semcnt n) {
  *       \p RDY_RESET.
  * @note This function must be called with interrupts disabled.
  */
-void chSemResetI(Semaphore *sp, t_semcnt n) {
-  t_semcnt cnt;
+void chSemResetI(Semaphore *sp, t_cnt n) {
+  t_cnt cnt;
 
   cnt = sp->s_cnt;
   sp->s_cnt = n;
   while (cnt++ < 0)
-    chSchReadyI(dequeue(sp->s_queue.p_next))->p_rdymsg = RDY_RESET;
+    chSchReadyI(fifo_remove(&sp->s_queue))->p_rdymsg = RDY_RESET;
 }
 
 /**
@@ -88,7 +88,8 @@ void chSemWait(Semaphore *sp) {
   chSysLock();
 
   if (--sp->s_cnt < 0) {
-    enqueue(currp, &sp->s_queue);
+    fifo_insert(currp, &sp->s_queue);
+    currp->p_semp = sp;
     chSchGoSleepI(PRWTSEM);
   }
 
@@ -104,7 +105,8 @@ void chSemWait(Semaphore *sp) {
 void chSemWaitS(Semaphore *sp) {
 
   if (--sp->s_cnt < 0) {
-    enqueue(currp, &sp->s_queue);
+    fifo_insert(currp, &sp->s_queue);
+    currp->p_semp = sp;
     chSchGoSleepI(PRWTSEM);
   }
 }
@@ -114,6 +116,7 @@ static void unwait(void *p) {
 
 // Test removed, it should never happen.
 //  if (((Thread *)p)->p_state == PRWTSEM)
+  chSemFastSignalI(((Thread *)p)->p_semp);
   chSchReadyI(dequeue(p))->p_rdymsg = RDY_TIMEOUT;
 }
 
@@ -132,11 +135,11 @@ t_msg chSemWaitTimeout(Semaphore *sp, t_time time) {
     VirtualTimer vt;
 
     chVTSetI(&vt, time, unwait, currp);
-    enqueue(currp, &sp->s_queue);
+    fifo_insert(currp, &sp->s_queue);
+    currp->p_semp = sp;
     chSchGoSleepI(PRWTSEM);
     msg = currp->p_rdymsg; // Note, got value *before* invoking CH_LEAVE_SYSTEM().
     if (!vt.vt_func) {
-      sp->s_cnt++;
 
       chSysUnlock();
       return msg;
@@ -167,12 +170,11 @@ t_msg chSemWaitTimeoutS(Semaphore *sp, t_time time) {
     VirtualTimer vt;
 
     chVTSetI(&vt, time, unwait, currp);
-    enqueue(currp, &sp->s_queue);
+    fifo_insert(currp, &sp->s_queue);
+    currp->p_semp = sp;
     chSchGoSleepI(PRWTSEM);
-    if (!vt.vt_func) {
-      sp->s_cnt++;
+    if (!vt.vt_func)
       return currp->p_rdymsg;
-    }
     chVTResetI(&vt);
     return currp->p_rdymsg;
   }
@@ -191,7 +193,7 @@ void chSemSignal(Semaphore *sp) {
   chSysLock();
 
   if (sp->s_cnt++ < 0)
-    chSchWakeupI(dequeue(sp->s_queue.p_next), RDY_OK);
+    chSchWakeupI(fifo_remove(&sp->s_queue), RDY_OK);
 
   chSysUnlock();
 }
@@ -206,7 +208,7 @@ void chSemSignal(Semaphore *sp) {
 void chSemSignalI(Semaphore *sp) {
 
   if (sp->s_cnt++ < 0)
-    chSchReadyI(dequeue(sp->s_queue.p_next));
+    chSchReadyI(fifo_remove(&sp->s_queue));
 }
 
 /**
@@ -217,20 +219,18 @@ void chSemSignalI(Semaphore *sp) {
  *       option is enabled in \p chconf.h.
  */
 void chSemSignalWait(Semaphore *sps, Semaphore *spw) {
-  BOOL flag;
 
   chSysLock();
 
   if (sps->s_cnt++ < 0)
-    chSchReadyI(dequeue(sps->s_queue.p_next)), flag = TRUE;
-  else
-    flag = FALSE;
+    chSchReadyI(fifo_remove(&sps->s_queue));
 
   if (--spw->s_cnt < 0) {
-    enqueue(currp, &spw->s_queue);
+    fifo_insert(currp, &spw->s_queue);
+    currp->p_semp = spw;
     chSchGoSleepI(PRWTSEM);
   }
-  else if (flag)
+  else
     chSchRescheduleI();
 
   chSysUnlock();
@@ -245,15 +245,14 @@ void chSemSignalWait(Semaphore *sps, Semaphore *spw) {
  *       you want to create some custom threads synchronization mechanism.
  */
 static void prioenq(Thread *tp, ThreadsQueue *tqp) {
-  Thread *p;
+  Thread *cp;
 
-  p = tqp->p_next;
-  while ((p != (Thread *)tqp) && (p->p_prio >= tp->p_prio))
-    p = p->p_next;
-  tp->p_next = p;
-  tp->p_prev = tqp->p_prev;
-  p->p_prev->p_next = tp;
-  p->p_prev = tp;
+  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;
 }
 
 /**
@@ -268,6 +267,7 @@ void chSemRaisePrioWait(Semaphore *sp) {
 
   if (--sp->s_cnt < 0) {
     prioenq(currp, &sp->s_queue);
+    currp->p_semp = sp;
     chSchGoSleepI(PRWTSEM);
   }
 
@@ -290,11 +290,11 @@ void chSemLowerPrioSignal(Semaphore *sp) {
   if (!--currp->p_rtcnt) {
     currp->p_prio -= MEPRIO;
     if (sp->s_cnt++ < 0)
-      chSchReadyI(dequeue(sp->s_queue.p_next));
+      chSchReadyI(fifo_remove(&sp->s_queue));
     chSchRescheduleI();
   }
   else if (sp->s_cnt++ < 0)
-    chSchWakeupI(dequeue(sp->s_queue.p_next), RDY_OK);
+    chSchWakeupI(fifo_remove(&sp->s_queue), RDY_OK);
 
   chSysUnlock();
 }
@@ -312,10 +312,11 @@ void chSemRaisePrioSignalWait(Semaphore *sps, Semaphore *spw) {
   chSysLock();
 
   if (sps->s_cnt++ < 0)
-    chSchReadyI(dequeue(sps->s_queue.p_next));
+    chSchReadyI(fifo_remove(&sps->s_queue));
 
   if (--spw->s_cnt < 0) {
     prioenq(currp, &spw->s_queue);
+    currp->p_semp = spw;
     chSchGoSleepI(PRWTSEM);
 
     if (!currp->p_rtcnt++)
@@ -347,10 +348,11 @@ void chSemLowerPrioSignalWait(Semaphore *sps, Semaphore *spw) {
     currp->p_prio -= MEPRIO;
 
   if (sps->s_cnt++ < 0)
-    chSchReadyI(dequeue(sps->s_queue.p_next));
+    chSchReadyI(fifo_remove(&sps->s_queue));
 
   if (--spw->s_cnt < 0) {
-    enqueue(currp, &spw->s_queue); // enqueue() because the spw is a normal sem.
+    fifo_insert(currp, &spw->s_queue); // fifo_insert() because the spw is a normal sem.
+    currp->p_semp = spw;
     chSchGoSleepI(PRWTSEM);
   }
   else
diff --git a/src/chthreads.c b/src/chthreads.c
index a439eb5b4..608dabe89 100644
--- a/src/chthreads.c
+++ b/src/chthreads.c
@@ -24,33 +24,6 @@
 
 #include <ch.h>
 
-#ifndef CH_OPTIMIZE_SPEED
-/*
- * Removes a Thread from a list and returns it.
- * @param tp the pointer to the thread to be removed from the list
- * @return the removed thread pointer
- */
-Thread *dequeue(Thread *tp) {
-
-  tp->p_prev->p_next = tp->p_next;
-  tp->p_next->p_prev = tp->p_prev;
-  return tp;
-}
-
-/*
- * Inserts a thread into a bi-directional list in FIFO order.
- * @param tp the pointer to the thread to be inserted in the list
- * @param tqp the pointer to the threads list header
- */
-void enqueue(Thread *tp, ThreadsQueue *tqp) {
-
-  tp->p_next = (Thread *)tqp;
-  tp->p_prev = tqp->p_prev;
-  tqp->p_prev->p_next = tp;
-  tqp->p_prev = tp;
-}
-#endif /* CH_OPTIMIZE_SPEED */
-
 /*
  * Initializes a thread structure.
  */
@@ -63,12 +36,10 @@ void _InitThread(t_prio prio, t_tmode mode, Thread *tp) {
   tp->p_rtcnt = 0;
 #endif
 #ifdef CH_USE_WAITEXIT
-  tp->p_waiting.p_next = (Thread *)&tp->p_waiting;
-  tp->p_waiting.p_prev = (Thread *)&tp->p_waiting;
+  list_init(&tp->p_waiting);
 #endif
 #ifdef CH_USE_MESSAGES
-  tp->p_msgqueue.p_next = (Thread *)&tp->p_msgqueue;
-  tp->p_msgqueue.p_prev = (Thread *)&tp->p_msgqueue;
+  fifo_init(&tp->p_msgqueue);
 #endif
 #ifdef CH_USE_EVENTS
   tp->p_epending = 0;
@@ -179,7 +150,7 @@ void chThdExit(t_msg msg) {
   currp->p_exitcode = msg;              /* Post mortem info.            */
 #ifdef CH_USE_WAITEXIT
   while (notempty(&currp->p_waiting))
-    chSchReadyI(dequeue(currp->p_waiting.p_next));
+    chSchReadyI(list_remove(&currp->p_waiting));
 #endif
 #ifdef CH_USE_EXIT_EVENT
   chEvtSendI(&currp->p_exitesource);
@@ -203,7 +174,7 @@ t_msg chThdWait(Thread *tp) {
   chSysLock();
 
   if (tp->p_state != PREXIT) {
-    enqueue(currp, &tp->p_waiting);
+    list_insert(currp, &tp->p_waiting);
     chSchGoSleepI(PRWAIT);
   }
 
diff --git a/src/include/ch.h b/src/include/ch.h
index 80204bb2d..aa77df544 100644
--- a/src/include/ch.h
+++ b/src/include/ch.h
@@ -25,8 +25,6 @@
 #ifndef _CH_H_
 #define _CH_H_
 
-typedef struct Thread Thread;
-
 #ifndef __DOXIGEN__
 #ifndef _CHCONF_H_
 #include <chconf.h>
@@ -41,14 +39,22 @@ typedef struct Thread Thread;
 #endif
 #endif /* __DOXIGEN__ */
 
-#ifndef _DELTA_H_
+#ifndef _LISTS_H_
 #include "delta.h"
 #endif
 
+#ifndef _LISTS_H_
+#include "lists.h"
+#endif
+
 #ifndef _SCHEDULER_H_
 #include "scheduler.h"
 #endif
 
+#ifndef _SEMAPHORES_H_
+#include "semaphores.h"
+#endif
+
 #ifndef _EVENTS_H_
 #include "events.h"
 #endif
@@ -65,10 +71,6 @@ typedef struct Thread Thread;
 #include "sleep.h"
 #endif
 
-#ifndef _SEMAPHORES_H_
-#include "semaphores.h"
-#endif
-
 #ifndef _QUEUES_H_
 #include "queues.h"
 #endif
diff --git a/src/include/lists.h b/src/include/lists.h
new file mode 100644
index 000000000..477ab1061
--- /dev/null
+++ b/src/include/lists.h
@@ -0,0 +1,67 @@
+/*
+    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 Threads
+ * @{
+ */
+
+#ifndef _LISTS_H_
+#define _LISTS_H_
+
+typedef struct Thread Thread;
+
+/* Macros good with both ThreadsQueue and ThreadsList.*/
+#define isempty(p)     ((p)->p_next == (Thread *)(p))
+#define notempty(p)    ((p)->p_next != (Thread *)(p))
+
+/**
+ * Generic threads FIFO queue header and element.
+ */
+typedef struct {
+  /** Next \p Thread in the queue, in FIFO order.*/
+  Thread            *p_next;
+  /** Last \p Thread in the queue, in FIFO order.*/
+  Thread            *p_prev;
+} ThreadsQueue;
+
+/**
+ * Generic threads single link list, it works like a stack.
+ */
+typedef struct {
+  Thread            *p_next;
+} ThreadsList;
+
+/*
+ * Threads Lists functions and macros.
+ */
+#define fifo_init(tqp) ((tqp)->p_next = (tqp)->p_prev = (Thread *)(tqp));
+#define list_init(tlp) ((tlp)->p_next = (Thread *)(tlp))
+
+#ifndef CH_OPTIMIZE_SPEED
+void fifo_insert(Thread *tp, ThreadsQueue *tqp);
+Thread *fifo_remove(ThreadsQueue *tqp);
+Thread *dequeue(Thread *tp);
+void list_insert(Thread *tp, ThreadsList *tlp);
+Thread *list_remove(ThreadsList *tlp);
+#endif
+
+#endif  /* _LISTS_H_ */
+
+/** @} */
diff --git a/src/include/scheduler.h b/src/include/scheduler.h
index 78fbb4a72..2efabbcd2 100644
--- a/src/include/scheduler.h
+++ b/src/include/scheduler.h
@@ -32,6 +32,8 @@
 /** Returned if the thread was made ready because a reset.*/
 #define RDY_RESET   -2
 
+#define firstprio(qp)   ((qp)->p_next->p_prio)
+
 /**
  * Ready list header.
  */
diff --git a/src/include/semaphores.h b/src/include/semaphores.h
index 83d4613de..8179627f4 100644
--- a/src/include/semaphores.h
+++ b/src/include/semaphores.h
@@ -34,12 +34,12 @@ typedef struct {
   /** Queue of the threads sleeping on this Semaphore.*/
   ThreadsQueue    s_queue;
   /** The Semaphore counter.*/
-  t_semcnt        s_cnt;
+  t_cnt           s_cnt;
 } Semaphore;
 
-void chSemInit(Semaphore *sp, t_semcnt n);
-void chSemReset(Semaphore *sp, t_semcnt n);
-void chSemResetI(Semaphore *sp, t_semcnt n);
+void chSemInit(Semaphore *sp, t_cnt n);
+void chSemReset(Semaphore *sp, t_cnt n);
+void chSemResetI(Semaphore *sp, t_cnt n);
 void chSemWait(Semaphore *sp);
 void chSemWaitS(Semaphore *sp);
 t_msg chSemWaitTimeout(Semaphore *sp, t_time time);
@@ -59,21 +59,18 @@ void chSemLowerPrioSignalWait(Semaphore *sps, Semaphore *spw);
  * Decreases the semaphore counter, this macro can be used when it is ensured
  * that the counter would not become negative.
  */
-#define chSemFastWaitS(sp) \
-				((sp)->s_cnt--)
+#define chSemFastWaitS(sp) ((sp)->s_cnt--)
 
 /**
  * Increases the semaphore counter, this macro can be used when the counter is
  * not negative.
  */
-#define chSemFastSignalI(sp) \
-				((sp)->s_cnt++)
+#define chSemFastSignalI(sp) ((sp)->s_cnt++)
 
 /**
  * Returns the semaphore counter current value.
  */
-#define chSemGetCounter(sp) \
-				((sp)->s_cnt)
+#define chSemGetCounter(sp) ((sp)->s_cnt)
 
 #endif /* CH_USE_SEMAPHORES */
 
diff --git a/src/include/threads.h b/src/include/threads.h
index 688ce579a..beb41aae7 100644
--- a/src/include/threads.h
+++ b/src/include/threads.h
@@ -25,21 +25,6 @@
 #ifndef _THREADS_H_
 #define _THREADS_H_
 
-#define isempty(qp)     ((qp)->p_next == (Thread *)(qp))
-#define notempty(qp)    ((qp)->p_next != (Thread *)(qp))
-#define firstprio(qp)   ((qp)->p_next->p_prio)
-#define lastprio(qp)    ((qp)->p_prev->p_prio)
-
-/**
- * Generic threads queue header and element.
- */
-typedef struct {
-  /** First \p Thread in the queue.*/
-  Thread            *p_next;
-  /** Last \p Thread in the queue.*/
-  Thread            *p_prev;
-} ThreadsQueue;
-
 /**
  * Structure representing a thread.
  * @note Not all the listed fields are always needed, by switching off some
@@ -49,6 +34,7 @@ typedef struct {
 struct Thread {
   /** Next \p Thread in the threads list.*/
   Thread            *p_next;
+  /* End of the fields shared with the ThreadsList structure. */
   /** Previous \p Thread in the threads list.*/
   Thread            *p_prev;
   /* End of the fields shared with the ThreadsQueue structure. */
@@ -66,10 +52,14 @@ struct Thread {
    */
   union {
     /** Thread wakeup code, normally set to \p RDY_OK by the \p chSchReadyI()
-     * (only while in \p PRCURR or \p PRREADY states).*/
+     * (only while in \p PRREADY state).*/
     t_msg           p_rdymsg;
     /** The thread exit code (only while in \p PREXIT state).*/
     t_msg           p_exitcode;
+#ifdef CH_USE_SEMAPHORES
+    /** Semaphore where the thread is waiting on (only in \p PRWTSEM state).*/
+    Semaphore       *p_semp;
+#endif
 #ifdef CH_USE_EVENTS
     /** Enabled events mask (only while in \p PRWTEVENT state).*/
     t_eventmask     p_ewmask;
@@ -89,7 +79,7 @@ struct Thread {
    */
 #ifdef CH_USE_WAITEXIT
   /** The queue of the threads waiting for this thread termination.*/
-  ThreadsQueue      p_waiting;
+  ThreadsList       p_waiting;
 #endif
 #ifdef CH_USE_EXIT_EVENT
   /** The thread termination \p EventSource.*/
@@ -103,9 +93,8 @@ struct Thread {
   t_eventmask       p_epending;
 #endif
 #ifdef CH_USE_RT_SEMAPHORES
-  /** Priority backup after acquiring a RT semaphore.*/
   /** RT semaphores depth counter.*/
-  int               p_rtcnt;
+  t_cnt             p_rtcnt;
 #endif
 };
 
@@ -142,13 +131,13 @@ struct Thread {
 /** Lowest user priority.*/
 #define LOWPRIO     1
 /** Normal user priority.*/
-#define NORMALPRIO  128
+#define NORMALPRIO  64
 /** Highest user priority.*/
-#define HIGHPRIO    255
+#define HIGHPRIO    127
 /** Boosted base priority.*/
-#define MEPRIO      256
-/** Absolute priority.*/
-#define ABSPRIO     512
+#define MEPRIO      128
+/** Greatest possible priority.*/
+#define ABSPRIO     255
 
 /* Not an API, don't use into the application code.*/
 void _InitThread(t_prio prio, t_tmode mode, Thread *tp);
@@ -157,12 +146,24 @@ void _InitThread(t_prio prio, t_tmode mode, Thread *tp);
 typedef t_msg (*t_tfunc)(void *);
 
 /*
- * Threads Lists functions.
+ * Inlined functions if CH_OPTIMIZE_SPEED is enabled.
  */
-#ifndef CH_OPTIMIZE_SPEED
-void enqueue(Thread *tp, ThreadsQueue *tqp);
-Thread *dequeue(Thread *tp);
-#else
+#ifdef CH_OPTIMIZE_SPEED
+static INLINE void fifo_insert(Thread *tp, ThreadsQueue *tqp) {
+
+  tp->p_next = (Thread *)tqp;
+  tp->p_prev = tqp->p_prev;
+  tqp->p_prev->p_next = tp;
+  tqp->p_prev = tp;
+}
+
+static INLINE Thread *fifo_remove(ThreadsQueue *tqp) {
+  Thread *tp = tqp->p_next;
+
+  (tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
+  return tp;
+}
+
 static INLINE Thread *dequeue(Thread *tp) {
 
   tp->p_prev->p_next = tp->p_next;
@@ -170,12 +171,17 @@ static INLINE Thread *dequeue(Thread *tp) {
   return tp;
 }
 
-static INLINE void enqueue(Thread *tp, ThreadsQueue *tqp) {
+static INLINE void list_insert(Thread *tp, ThreadsList *tlp) {
 
-  tp->p_next = (Thread *)tqp;
-  tp->p_prev = tqp->p_prev;
-  tqp->p_prev->p_next = tp;
-  tqp->p_prev = tp;
+  tp->p_next = tlp->p_next;
+  tlp->p_next = tp;
+}
+
+static INLINE Thread *list_remove(ThreadsList *tlp) {
+
+  Thread *tp = tlp->p_next;
+  tlp->p_next = tp->p_next;
+  return tp;
 }
 #endif
 
diff --git a/src/templates/chconf.h b/src/templates/chconf.h
index abb996899..87f9a18b6 100644
--- a/src/templates/chconf.h
+++ b/src/templates/chconf.h
@@ -147,7 +147,7 @@
  *        ChibiOS/RT code <b>must</b> be recompiled with the GCC option \p
  *        -ffixed-\<reg\>.
  */
-#define CH_CURRP_REGISTER_CACHE "reg"
+//#define CH_CURRP_REGISTER_CACHE "reg"
 
 #endif  /* _CHCONF_H_ */
 
diff --git a/src/templates/chcore.c b/src/templates/chcore.c
index e23cf1bfe..f39441ed5 100644
--- a/src/templates/chcore.c
+++ b/src/templates/chcore.c
@@ -45,6 +45,6 @@ void chSysHalt(void) {}
 /**
  * Context switch.
  */
-void chSysSwitchI(struct ctxswc **oldp, struct ctxswc *new) {}
+void chSysSwitchI(Context *oldp, Context *newp) {}
 
 /** @} */
diff --git a/src/templates/chcore.h b/src/templates/chcore.h
index bb901b1ec..a9bfcce63 100644
--- a/src/templates/chcore.h
+++ b/src/templates/chcore.h
@@ -31,6 +31,10 @@
 struct stackregs {
 };
 
+typedef struct {
+  struct stackregs *sp;
+} Context;
+
 /**
  * Platform dependent part of the \p chThdCreate() API.
  */
@@ -38,6 +42,11 @@ struct stackregs {
 {                                                                  \
 }
 
+#define INT_REQUIRED_STACK 0  // Must include registers and stack frames.
+
+#define UserStackSize(n) (sizeof(Thread) + \
+                          sizeof(struct stackregs) + (n) + (INT_REQUIRED_STACK))
+
 /**
  * Enters the ChibiOS/RT system mutual exclusion zone, the implementation is
  * architecture dependent, on single core systems usually this function
@@ -62,7 +71,7 @@ struct stackregs {
 
 void chSysHalt(void);
 void chSysPause(void);
-void chSysSwitchI(struct ctxswc **oldp, struct ctxswc *new);
+void chSysSwitchI(Context *oldp, Context *newp);
 
 #endif /* _CHCORE_H_ */
 
diff --git a/src/templates/chtypes.h b/src/templates/chtypes.h
index 891b850c2..4d8b2a18e 100644
--- a/src/templates/chtypes.h
+++ b/src/templates/chtypes.h
@@ -30,29 +30,21 @@
  */
 #define BOOL        char
 #define BYTE8       unsigned char
+#define SBYTE8      char
 #define WORD16      short
 #define UWORD16     unsigned short
 #define LONG32      int
 #define ULONG32     unsigned int
-#define PTR_EQ      int
 
-#define INT_REQUIRED_STACK 0
-
-typedef BYTE8       t_tmode;
-typedef BYTE8       t_tstate;
-typedef LONG32      t_prio;
-typedef PTR_EQ      t_msg;
-typedef LONG32      t_eventid;
-typedef ULONG32     t_eventmask;
-typedef ULONG32     t_time;
-typedef LONG32      t_semcnt;
-typedef ULONG32     t_size;
-
-#define MINPRIO     0x8000
-#define MAXPRIO     0x7fff
-
-#define MINDELTA    0
-#define MAXDELTA    0xffff
+typedef BYTE8       t_tmode;    /* Thread mode flags, BYTE8 is ok. */
+typedef BYTE8       t_tstate;   /* Thread state, BYTE8 is ok. */
+typedef ULONG32     t_prio;     /* Priority, use the fastest unsigned type. */
+typedef LONG32      t_msg;      /* Message, use signed pointer equivalent.*/
+typedef LONG32      t_eventid;  /* Event Id, use fastest signed.*/
+typedef ULONG32     t_eventmask;/* Event Mask, recommended fastest unsigned.*/
+typedef ULONG32     t_time;     /* Time, recommended fastest unsigned.*/
+typedef LONG32      t_cnt;      /* Counter, recommended fastest signed.*/
+typedef ULONG32     t_size;     /* Size, use unsigned pointer equivalent.*/
 
 #define THREAD
 #define INLINE      inline