ao-bringup-avr: cleaner stack switching code

[fw/altos] / ao-bringup-avr / ao-switch.c

diff --git a/ao-bringup-avr/ao-switch.c b/ao-bringup-avr/ao-switch.c
index 263a70d5b0e701e047c41971bff5f9dd057637e1..3ededd95185a9ee7b6079f93940c08e1665b28bb 100644 (file)
--- 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,58 +52,21 @@ 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         l, h;
-
-//     for (l = 0; l < AO_STACK_SIZE - stack_count; l++)
-//             printf ("stack[%2d] = %2x\n", l, sp[l]);
-       l = new_stack[AO_STACK_SIZE - 1];
-       h = new_stack[AO_STACK_SIZE - 2];
-//     printf ("Target return address: h %02x l %02x\n", h, l);
-#if 0
-       asm("push %0" : : "r" (l));
-       asm("push %0" : : "r" (h));
-       asm("ret");
-#endif
-#if 0
-       asm("push r31; push r30");
-       asm("push r29; push r28; push r27; push r26; push r25");
-       asm("push r24; push r23; push r22; push r21; push r20");
-       asm("push r19; push r18; push r17; push r16; push r15");
-       asm("push r14; push r13; push r12; push r11; push r10");
-       asm("push r9; push r8; push r7; push r6; push r5");
-       asm("push r4; push r3; push r2; push r1; push r0");
-       asm("in r0, __SREG__" "\n\t"
-           "push r0");
-#endif
-
        uint8_t sp_l, sp_h;
-       static  uint8_t reg[34];
+
        sp_l = (uint16_t) sp;
        sp_h = ((uint16_t) sp) >> 8;
        asm volatile ("out __SP_H__,%0" : : "r" (sp_h) );
        asm volatile ("out __SP_L__,%0" : : "r" (sp_l) );
-
-       asm volatile ("in %0,__SP_H__" : "=&r" (sp_h));
-       asm volatile ("in %0,__SP_L__" : "=&r" (sp_l));
        asm volatile("pop r0"   "\n\t"
                     "out __SREG__, r0");
        asm volatile("pop r0" "\n\t"
@@ -154,29 +101,18 @@ switch_stack(void)
                     "pop r29");
        asm volatile("pop r30" "\n\t"
                     "pop r31");
-       asm volatile ("in %0,__SP_H__" : "=&r" (sp_h));
-       asm volatile ("in %0,__SP_L__" : "=&r" (sp_l));
-       asm volatile ("pop %0" : "=&r" (h));
-       asm volatile ("pop %0" : "=&r" (l));
-       show_stack("before ret", sp_h, sp_l);
-       show_stack("returning to", h, l);
-       asm volatile("push %0" : : "r" (l));
-       asm volatile("push %0" : : "r" (h));
        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);
 }