src-avr: Suspend interrupts while switching stacks

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>

1 files changed, 28 insertions, 17 deletions

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);
 	}
 }