src-avr: Add 'sleep_cpu' to reduce power usage while idle

[fw/altos] / src-avr / ao_task.c

diff --git a/src-avr/ao_task.c b/src-avr/ao_task.c
index 3c6b34a42afd7534bef9c7f3d2a97394184180c8..514671334ec4903da97fdc3c4ef760dd28039948 100644 (file)
--- a/src-avr/ao_task.c
+++ b/src-avr/ao_task.c
@@ -31,22 +31,22 @@ __xdata struct ao_task *__data ao_cur_task;
 static void
 ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
 {
-       uint8_t         *stack = task->stack + AO_STACK_SIZE - 1;
+       uint8_t         *sp = task->stack + AO_STACK_SIZE - 1;
        uint16_t        a = (uint16_t) start;
        int             i;
 
        /* Return address */
-       PUSH8(stack, a);
-       PUSH8(stack, (a >> 8));
+       PUSH8(sp, a);
+       PUSH8(sp, (a >> 8));
 
        /* Clear register values */
        i = 32;
        while (i--)
-               PUSH8(stack, 0);
+               PUSH8(sp, 0);
 
        /* SREG with interrupts enabled */
-       PUSH8(stack, 0x80);
-       task->stack_count = stack - task->stack;
+       PUSH8(sp, 0x80);
+       task->sp = sp;
 }
 #else
 static void
@@ -114,6 +114,7 @@ ao_yield(void) __naked
        asm("push r14" "\n\t" "push r13" "\n\t" "push r12" "\n\t" "push r11" "\n\t" "push r10");
        asm("push r9" "\n\t" "push r8" "\n\t" "push r7" "\n\t" "push r6" "\n\t" "push r5");
        asm("push r4" "\n\t" "push r3" "\n\t" "push r2" "\n\t" "push r1" "\n\t" "push r0");
+       cli();
        asm("in r0, __SREG__" "\n\t" "push r0");
        sei();
 #else
@@ -150,11 +151,9 @@ ao_yield(void) __naked
        {
 #ifdef AVR
                uint8_t sp_l, sp_h;
-               uint8_t *sp;
                asm("in %0,__SP_L__" : "=&r" (sp_l) );
                asm("in %0,__SP_H__" : "=&r" (sp_h) );
-               sp = (uint8_t *) ((uint16_t) sp_l | ((uint16_t) sp_h << 8));
-               ao_cur_task->stack_count = sp - ao_cur_task->stack;
+               ao_cur_task->sp = (uint8_t *) ((uint16_t) sp_l | ((uint16_t) sp_h << 8));
 #else
                uint8_t stack_len;
                __data uint8_t *stack_ptr;
@@ -198,10 +197,11 @@ ao_yield(void) __naked
                                break;
                        }
 
-#ifdef AVR
-#else
                        /* Enter lower power mode when there isn't anything to do */
                        if (ao_next_task_index == ao_cur_task_index)
+#ifdef AVR
+                               sleep_cpu();
+#else
                                PCON = PCON_IDLE;
 #endif
                }
@@ -209,10 +209,10 @@ ao_yield(void) __naked
 
 #ifdef AVR
        {
-               uint8_t *sp = (ao_cur_task->stack + ao_cur_task->stack_count);
                uint8_t sp_l, sp_h;
-               sp_l = (uint16_t) sp;
-               sp_h = ((uint16_t) sp) >> 8;
+               sp_l = (uint16_t) ao_cur_task->sp;
+               sp_h = ((uint16_t) ao_cur_task->sp) >> 8;
+               cli();
                asm("out __SP_H__,%0" : : "r" (sp_h) );
                asm("out __SP_L__,%0" : : "r" (sp_l) );
                asm("pop r0"    "\n\t"
@@ -277,9 +277,11 @@ ao_yield(void) __naked
 uint8_t
 ao_sleep(__xdata void *wchan)
 {
+       cli();
        __critical {
                ao_cur_task->wchan = wchan;
        }
+       sei();
        ao_yield();
        ao_cur_task->alarm = 0;
        if (ao_cur_task->wchan) {
@@ -325,16 +327,13 @@ void
 ao_task_info(void)
 {
        uint8_t i;
-       uint8_t pc_loc;
        __xdata struct ao_task *task;
 
        for (i = 0; i < ao_num_tasks; i++) {
                task = ao_tasks[i];
-               pc_loc = task->stack_count - 17;
-               printf("%12s: wchan %04x pc %04x\n",
+               printf("%12s: wchan %04x\n",
                       task->name,
-                      (int16_t) task->wchan,
-                      (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
+                      (int16_t) task->wchan);
        }
 }