ao-bringup-avr: cleaner stack switching code

Signed-off-by: Keith Packard <keithp@keithp.com>

1 files changed, 10 insertions, 39 deletions

diff --git a/ao-bringup-avr/ao-switch.c b/ao-bringup-avr/ao-switch.c
index 4b797c84..3ededd95 100644
--- a/ao-bringup-avr/ao-switch.c
+++ b/ao-bringup-avr/ao-switch.c
@@ -19,19 +19,8 @@
 
 #define AO_STACK_SIZE	128
 
-uint8_t	new_stack[512];
-int	stack_count;
-
 #define PUSH8(stack, val)	(*((stack)--) = (val))
 
-#define PUSH16(stack, val)	PUSH8(stack, ((uint16_t) (val))); PUSH8(stack, ((uint16_t) (val)) >> 8)
-
-void
-count(int count)
-{
-
-}
-
 void
 blink(void)
 {
@@ -41,16 +30,11 @@ blink(void)
 	}
 }
 
-void
-function(void)
-{
-	return;
-}
 
-void
-init_stack(void (*f) (void))
+uint8_t *
+init_stack(uint8_t *stack_base, int size, void (*f) (void))
 {
-	uint8_t		*stack = new_stack + AO_STACK_SIZE - 1;
+	uint8_t		*stack = stack_base + size - 1;
 	uint16_t	a;
 	uint8_t		h, l;
 
@@ -68,25 +52,15 @@ init_stack(void (*f) (void))
 
 	/* SREG with interrupts enabled */
 	PUSH8(stack, 0x80);
-	stack_count = stack - new_stack;
-	printf("Function is at %p. Stack[1] is %02x Stack[2] is %02x\n",
-	       f, stack[1], stack[2]);
-	printf ("stack_count is %d\n", stack_count);
-	printf ("stack is %p\n", stack);
+	return stack;
 }
 
 void
-switch_stack(void) __attribute__((naked));
-
-void show_stack(char *s, uint8_t h, uint8_t l)
-{
-	printf ("SP at %s %02x %02x\n", s, h, l);
-}
+switch_stack(uint8_t *sp) __attribute__((naked));
 
 void
-switch_stack(void)
+switch_stack(uint8_t *sp)
 {
-	uint8_t	*sp = (new_stack + stack_count);
 	uint8_t	sp_l, sp_h;
 
 	sp_l = (uint16_t) sp;
@@ -130,18 +104,15 @@ switch_stack(void)
 	asm volatile("ret");
 }
 
-void back(void)
-{
-	switch_stack();
-	blink();
-}
+uint8_t	new_stack[512];
 
 void main(void)
 {
+	uint8_t	*stack;
 	ao_bringup_init();
 
 	printf("starting\n");
 
-	init_stack(blink);
-	switch_stack();
+	stack = init_stack(new_stack, sizeof (new_stack), blink);
+	switch_stack(stack);
 }