Setting the stack pointer takes two instructions, so make sure we
don't get an interrupt in the middle of it. Let the restoration of
SREG set the interrupt flag as appropriate.
Signed-off-by: Keith Packard <keithp@keithp.com>
struct ao_task {
__xdata void *wchan; /* current wait channel (NULL if running) */
uint16_t alarm; /* abort ao_sleep time */
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 */
uint8_t stack_count; /* amount of saved stack */
uint8_t task_id; /* unique id */
__code char *name; /* task name */
uint8_t stack[AO_STACK_SIZE]; /* saved stack */
uint8_t task_id; /* unique id */
__code char *name; /* task name */
uint8_t stack[AO_STACK_SIZE]; /* saved stack */
static void
ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
{
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 */
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--)
/* Clear register values */
i = 32;
while (i--)
/* SREG with interrupts enabled */
/* SREG with interrupts enabled */
- PUSH8(stack, 0x80);
- task->stack_count = stack - task->stack;
+ PUSH8(sp, 0x80);
+ task->sp = sp;
ao_init_stack(task, start);
}
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
/* Task switching function. This must not use any stack variables */
void
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");
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");
asm("in r0, __SREG__" "\n\t" "push r0");
sei();
#else
asm("in r0, __SREG__" "\n\t" "push r0");
sei();
#else
{
#ifdef AVR
uint8_t sp_l, sp_h;
{
#ifdef AVR
uint8_t sp_l, sp_h;
asm("in %0,__SP_L__" : "=&r" (sp_l) );
asm("in %0,__SP_H__" : "=&r" (sp_h) );
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;
#else
uint8_t stack_len;
__data uint8_t *stack_ptr;
SP = AO_STACK_START - 1;
#endif
SP = AO_STACK_START - 1;
#endif
/* Find a task to run. If there isn't any runnable task,
* this loop will run forever, which is just fine
*/
/* Find a task to run. If there isn't any runnable task,
* this loop will run forever, which is just fine
*/
- uint8_t *sp = (ao_cur_task->stack + ao_cur_task->stack_count);
- 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"
asm("out __SP_H__,%0" : : "r" (sp_h) );
asm("out __SP_L__,%0" : : "r" (sp_l) );
asm("pop r0" "\n\t"
ao_task_info(void)
{
uint8_t i;
ao_task_info(void)
{
uint8_t i;
__xdata struct ao_task *task;
for (i = 0; i < ao_num_tasks; i++) {
task = ao_tasks[i];
__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",
- (int16_t) task->wchan,
- (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
+ (int16_t) task->wchan);