8 files changed, 70 insertions, 26 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 79193d9800..e65200eea2 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -111,7 +111,8 @@ static int wait_for_bsy(void)
 {
     int timeout = current_tick + HZ*10;
     while (TIME_BEFORE(current_tick, timeout) && (ATA_ALT_STATUS & STATUS_BSY))
-        yield();
+        sleep_thread();
+    wake_up_thread();
 
     if (TIME_BEFORE(current_tick, timeout))
         return 1;
@@ -131,7 +132,8 @@ static int wait_for_rdy(void)
     
     while (TIME_BEFORE(current_tick, timeout) &&
            !(ATA_ALT_STATUS & STATUS_RDY))
-        yield();
+        sleep_thread();
+    wake_up_thread();
 
     if (TIME_BEFORE(current_tick, timeout))
         return STATUS_RDY;
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index 2c33440fef..69d041f65e 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -163,19 +163,8 @@ int button_get(bool block)
 int button_get_w_tmo(int ticks)
 {
     struct event ev;
-    unsigned int timeout = current_tick + ticks;
-
-    while (TIME_BEFORE( current_tick, timeout ))
-    {
-        if(!queue_empty(&button_queue))
-        {
-            queue_wait(&button_queue, &ev);
-            return ev.id;
-        }
-        yield();
-    }
-
-    return BUTTON_NONE;
+    queue_wait_w_tmo(&button_queue, &ev, ticks);
+    return (ev.id != SYS_TIMEOUT)? ev.id: BUTTON_NONE;
 }
 
 #ifdef HAVE_RECORDER_KEYPAD
diff --git a/firmware/drivers/i2c.c b/firmware/drivers/i2c.c
index 97d7a511e1..f0b5907be8 100644
--- a/firmware/drivers/i2c.c
+++ b/firmware/drivers/i2c.c
@@ -19,6 +19,7 @@
 #include "lcd.h"
 #include "sh7034.h"
 #include "kernel.h"
+#include "thread.h"
 #include "debug.h"
 
 #define PB13 0x2000
@@ -108,7 +109,8 @@ void i2c_ack(int bit)
     
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the MAS to release it */
-        yield();
+        sleep_thread();
+    wake_up_thread();
 
     DELAY;
     SCL_OUTPUT;
@@ -130,7 +132,8 @@ int i2c_getack(void)
     SDA_INPUT;   /* And set to input */
     SCL_INPUT;   /* Set the clock to input */
     while(!SCL)  /* and wait for the MAS to release it */
-        yield();
+        sleep_thread();
+    wake_up_thread();
     
     if (SDA)
         /* ack failed */
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index 7940ddfcf6..7be58d0931 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -19,12 +19,17 @@
 #ifndef THREAD_H
 #define THREAD_H
 
+#include <stdbool.h>
+
 #define MAXTHREADS      16
 #define DEFAULT_STACK_SIZE 0x400 /* Bytes */
 
 int create_thread(void* function, void* stack, int stack_size, char *name);
 void switch_thread(void);
+void sleep_thread(void);
+void wake_up_thread(void);
 void init_threads(void);
 int thread_stack_usage(int threadnum);
+void cpu_sleep(bool enabled);
 
 #endif
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 4d2f519171..3255ba0b20 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -59,13 +59,15 @@ void sleep(int ticks)
     int timeout = current_tick + ticks + 1;
 
     while (TIME_BEFORE( current_tick, timeout )) {
-        yield();
+        sleep_thread();
     }
+    wake_up_thread();
 }
 
 void yield(void)
 {
     switch_thread();
+    wake_up_thread();
 }
 
 /****************************************************************************
@@ -96,8 +98,9 @@ void queue_wait(struct event_queue *q, struct event *ev)
 {
     while(q->read == q->write)
     {
-        switch_thread();
+        sleep_thread();
     }
+    wake_up_thread();
 
     *ev = q->events[(q->read++) & QUEUE_LENGTH_MASK];
 }
@@ -108,8 +111,9 @@ void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks)
 
     while(q->read == q->write && TIME_BEFORE( current_tick, timeout ))
     {
-        switch_thread();
+        sleep_thread();
     }
+    wake_up_thread();
 
     if(q->read != q->write)
     {
@@ -201,6 +205,7 @@ void IMIA0(void)
     }
 
     current_tick++;
+    wake_up_thread();
 
     TSR0 &= ~0x01;
 }
@@ -257,7 +262,8 @@ void mutex_lock(struct mutex *m)
 {
     /* Wait until the lock is open... */
     while(m->locked)
-        yield();
+        sleep_thread();
+    wake_up_thread();
 
     /* ...and lock it */
     m->locked = true;
diff --git a/firmware/mpeg.c b/firmware/mpeg.c
index 0518dd24e0..6fe60c99c9 100644
--- a/firmware/mpeg.c
+++ b/firmware/mpeg.c
@@ -845,6 +845,7 @@ static void dma_tick(void)
             {
                 saving = true;
                 queue_post(&mpeg_queue, MPEG_SAVE_DATA, 0);
+                wake_up_thread();
             }
         }
     }
@@ -948,6 +949,7 @@ void DEI3(void)
     }
 
     CHCR3 &= ~0x0002; /* Clear DMA interrupt */
+    wake_up_thread();
 }
 
 #ifdef HAVE_MAS3587F
@@ -1791,9 +1793,10 @@ static void mpeg_thread(void)
         }
         else
         {
+            /* This doesn't look neccessary...
             yield();
             if(!queue_empty(&mpeg_queue))
-            {
+            {*/
                 queue_wait(&mpeg_queue, &ev);
                 switch(ev.id)
                 {
@@ -1897,7 +1900,7 @@ static void mpeg_thread(void)
                         init_playback_done = true;
                         break;
                 }
-            }
+            /*}*/
         }
 #endif
     }
@@ -1974,7 +1977,8 @@ void mpeg_init_recording(void)
     queue_post(&mpeg_queue, MPEG_INIT_RECORDING, NULL);
 
     while(!init_recording_done)
-        yield();
+        sleep_thread();
+    wake_up_thread();
 }
 
 void mpeg_init_playback(void)
@@ -1983,7 +1987,8 @@ void mpeg_init_playback(void)
     queue_post(&mpeg_queue, MPEG_INIT_PLAYBACK, NULL);
 
     while(!init_playback_done)
-        yield();
+        sleep_thread();
+    wake_up_thread();
 }
 
 static void init_recording(void)
diff --git a/firmware/test/fat/test.sh b/firmware/test/fat/test.sh
index 6a26e7ff59..44ff6bb849 100644
--- a/firmware/test/fat/test.sh
+++ b/firmware/test/fat/test.sh
@@ -75,7 +75,6 @@ runtests() {
     try mkfile /cpa.rock 0
     check
     try chkfile /cpa.rock
-    try chkfile /apa.rock
     try chkfile /bpa.rock
 
     LOOP=50
diff --git a/firmware/thread.c b/firmware/thread.c
index 2651a4f2d7..ea518ca4ee 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -16,8 +16,11 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
+#include <stdbool.h>
 #include "thread.h"
 #include "panic.h"
+#include "kernel.h"
+#include "sh7034.h"
 
 struct regs
 {
@@ -30,6 +33,8 @@ struct regs
 };
 
 int num_threads;
+bool cpu_sleep_enabled;
+static volatile int num_sleepers;
 static int current_thread;
 static struct regs thread_contexts[MAXTHREADS] __attribute__ ((section(".idata")));
 char *thread_name[MAXTHREADS];
@@ -95,6 +100,18 @@ void switch_thread(void)
     int next;
     unsigned int *stackptr;
 
+#ifdef SIMULATOR
+    /* Do nothing */
+#else
+
+    while (cpu_sleep_enabled && num_sleepers == num_threads)
+    {
+        /* Enter sleep mode, woken up on interrupt */
+        SBYCR &= 0x7F;
+        asm volatile ("sleep");
+    }
+    
+#endif
     next = current = current_thread;
     if (++next >= num_threads)
         next = 0;
@@ -108,6 +125,22 @@ void switch_thread(void)
        panicf("Stkov %s", thread_name[next]);
 }
 
+void cpu_sleep(bool enabled)
+{
+    cpu_sleep_enabled = enabled;
+}
+
+void sleep_thread(void)
+{
+    ++num_sleepers;
+    switch_thread();
+}
+
+void wake_up_thread(void)
+{
+    num_sleepers = 0;
+}
+
 /*--------------------------------------------------------------------------- 
  * Create thread.
  * Return 0 if context area could be allocated, else -1.
@@ -144,6 +177,7 @@ int create_thread(void* function, void* stack, int stack_size, char *name)
         regs->sr = 0;
         regs->pr = function;
     }
+    wake_up_thread();
     return 0;
 }
 
@@ -154,6 +188,7 @@ void init_threads(void)
     thread_name[0] = main_thread_name;
     thread_stack[0] = stackbegin;
     thread_stack_size[0] = (int)stackend - (int)stackbegin;
+    num_sleepers = 0;
 }
 
 int thread_stack_usage(int threadnum)