3 files changed, 15 insertions, 11 deletions
diff --git a/src/chschd.c b/src/chschd.c
index 78f051bf8..18a4e000f 100644
--- a/src/chschd.c
+++ b/src/chschd.c
@@ -35,7 +35,7 @@ ReadyList rlist;
 void chSchInit(void) {
 
   fifo_init(&rlist.r_queue);
-//  rlist.r_prio = ABSPRIO;
+  rlist.r_prio = NOPRIO;
   rlist.r_preempt = CH_TIME_QUANTUM;
 #ifdef CH_USE_SYSTEMTIME
   rlist.r_stime = 0;
@@ -58,10 +58,17 @@ INLINE void chSchReadyI(Thread *tp, t_msg msg) {
 #else
 void chSchReadyI(Thread *tp, t_msg msg) {
 #endif
+  Thread *cp = rlist.r_queue.p_next;
 
   tp->p_state = PRREADY;
   tp->p_rdymsg = msg;
-  prio_insert(tp, &rlist.r_queue);
+//  prio_insert(tp, &rlist.r_queue);
+  while (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;
+
 }
 
 /**
@@ -106,13 +113,7 @@ void chSchWakeupS(Thread *ntp, t_msg msg) {
     chSchReadyI(ntp, msg);
   else {
     Thread *otp = currp;
-    /* Note, does a prio_insert() instead of a chSchReadyI() because of the
-       relative priority between the two threads, prio_insert() scans the
-       list starting from the highest priority end downward.*/
-/*    chSchReadyI(otp, RDY_OK);*/
-    otp->p_state = PRREADY;
-    otp->p_rdymsg = RDY_OK;
-    prio_insert(otp, &rlist.r_queue);
+    chSchReadyI(otp, RDY_OK);
     (currp = ntp)->p_state = PRCURR;
     ntp->p_rdymsg = msg;
     rlist.r_preempt = CH_TIME_QUANTUM;
diff --git a/src/include/scheduler.h b/src/include/scheduler.h
index 565b75ccb..183e3af79 100644
--- a/src/include/scheduler.h
+++ b/src/include/scheduler.h
@@ -39,6 +39,7 @@
  */
 typedef struct {
   ThreadsQueue          r_queue;
+  t_prio                r_prio;
   t_cnt                 r_preempt;
 #ifndef CH_CURRP_REGISTER_CACHE
   Thread                *r_current;
diff --git a/src/include/threads.h b/src/include/threads.h
index 241b44442..50468e063 100644
--- a/src/include/threads.h
+++ b/src/include/threads.h
@@ -136,10 +136,12 @@ struct Thread {
 /** Thread option: Create suspended thread.*/
 #define P_SUSPENDED 2
 
+/** Pseudo priority used by the ready list header, do not use.*/
+#define NOPRIO      0
 /** Idle thread priority.*/
-#define IDLEPRIO    0
+#define IDLEPRIO    1
 /** Lowest user priority.*/
-#define LOWPRIO     1
+#define LOWPRIO     2
 /** Normal user priority.*/
 #define NORMALPRIO  64
 /** Highest user priority.*/