diff options
| -rw-r--r-- | src-avr/ao.h | 4 | ||||
| -rw-r--r-- | src-avr/ao_task.c | 45 |
2 files changed, 32 insertions, 17 deletions
diff --git a/src-avr/ao.h b/src-avr/ao.h index 242c26c7..90298a3e 100644 --- a/src-avr/ao.h +++ b/src-avr/ao.h @@ -50,7 +50,11 @@ struct ao_task { __xdata void *wchan; /* current wait channel (NULL if running) */ uint16_t alarm; /* abort ao_sleep time */ +#ifdef AVR + uint8_t *sp; /* saved stack pointer */ +#else uint8_t stack_count; /* amount of saved stack */ +#endif uint8_t task_id; /* unique id */ __code char *name; /* task name */ uint8_t stack[AO_STACK_SIZE]; /* saved stack */ diff --git a/src-avr/ao_task.c b/src-avr/ao_task.c index 3c6b34a4..b3bee6c9 100644 --- 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 @@ -102,6 +102,19 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam ao_init_stack(task, start); } +void +ao_show_task_from(void) +{ + if (ao_cur_task) + printf("switching from %s\n", ao_cur_task->name); +} + +void +ao_show_task_to(void) +{ + printf("switching to %s\n", ao_cur_task->name); +} + /* Task switching function. This must not use any stack variables */ void ao_yield(void) __naked @@ -114,6 +127,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 +164,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; @@ -175,6 +187,7 @@ ao_yield(void) __naked SP = AO_STACK_START - 1; #endif + ao_show_task_from(); /* Find a task to run. If there isn't any runnable task, * this loop will run forever, which is just fine */ @@ -209,10 +222,11 @@ 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; + ao_show_task_to(); + cli(); asm("out __SP_H__,%0" : : "r" (sp_h) ); asm("out __SP_L__,%0" : : "r" (sp_l) ); asm("pop r0" "\n\t" @@ -325,16 +339,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); } } |