ao-bringup-avr: clean up alternate stack code
[fw/altos] / ao-bringup-avr / ao-switch.c
1 /*
2  * Copyright © 2011 Keith Packard <keithp@keithp.com>
3  *
4  * This program is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; version 2 of the License.
7  *
8  * This program is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
11  * General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License along
14  * with this program; if not, write to the Free Software Foundation, Inc.,
15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
16  */
17
18 #include "ao-bringup.h"
19
20 #define AO_STACK_SIZE   128
21
22 uint8_t new_stack[512];
23 int     stack_count;
24
25 #define PUSH8(stack, val)       (*((stack)--) = (val))
26
27 #define PUSH16(stack, val)      PUSH8(stack, ((uint16_t) (val))); PUSH8(stack, ((uint16_t) (val)) >> 8)
28
29 void
30 count(int count)
31 {
32
33 }
34
35 void
36 blink(void)
37 {
38         while (1) {
39                 LEDPORT ^= (1 << LEDOUT);
40                 _delay_ms(200);
41         }
42 }
43
44 void
45 function(void)
46 {
47         return;
48 }
49
50 void
51 init_stack(void (*f) (void))
52 {
53         uint8_t         *stack = new_stack + AO_STACK_SIZE - 1;
54         uint16_t        a;
55         uint8_t         h, l;
56
57         /* Return address */
58         a = (uint16_t) f;
59         l = a;
60         h = a >> 8;
61         PUSH8(stack, l);
62         PUSH8(stack, h);
63
64         /* Clear register values */
65         l = 32;
66         while (l--)
67                 PUSH8(stack, 0);
68
69         /* SREG with interrupts enabled */
70         PUSH8(stack, 0x80);
71         stack_count = stack - new_stack;
72         printf("Function is at %p. Stack[1] is %02x Stack[2] is %02x\n",
73                f, stack[1], stack[2]);
74         printf ("stack_count is %d\n", stack_count);
75         printf ("stack is %p\n", stack);
76 }
77
78 void
79 switch_stack(void) __attribute__((naked));
80
81 void show_stack(char *s, uint8_t h, uint8_t l)
82 {
83         printf ("SP at %s %02x %02x\n", s, h, l);
84 }
85
86 void
87 switch_stack(void)
88 {
89         uint8_t *sp = (new_stack + stack_count);
90         uint8_t sp_l, sp_h;
91
92         sp_l = (uint16_t) sp;
93         sp_h = ((uint16_t) sp) >> 8;
94         asm volatile ("out __SP_H__,%0" : : "r" (sp_h) );
95         asm volatile ("out __SP_L__,%0" : : "r" (sp_l) );
96         asm volatile("pop r0"   "\n\t"
97                      "out __SREG__, r0");
98         asm volatile("pop r0" "\n\t"
99                      "pop r1" "\n\t"
100                      "pop r2" "\n\t"
101                      "pop r3" "\n\t"
102                      "pop r4");
103         asm volatile("pop r5" "\n\t"
104                      "pop r6" "\n\t"
105                      "pop r7" "\n\t"
106                      "pop r8" "\n\t"
107                      "pop r9");
108         asm volatile("pop r10" "\n\t"
109                      "pop r11" "\n\t"
110                      "pop r12" "\n\t"
111                      "pop r13" "\n\t"
112                      "pop r14");
113         asm volatile("pop r15" "\n\t"
114                      "pop r16" "\n\t"
115                      "pop r17" "\n\t"
116                      "pop r18" "\n\t"
117                      "pop r19");
118         asm volatile("pop r20" "\n\t"
119                      "pop r21" "\n\t"
120                      "pop r22" "\n\t"
121                      "pop r23" "\n\t"
122                      "pop r24");
123         asm volatile("pop r25" "\n\t"
124                      "pop r26" "\n\t"
125                      "pop r27" "\n\t"
126                      "pop r28" "\n\t"
127                      "pop r29");
128         asm volatile("pop r30" "\n\t"
129                      "pop r31");
130         asm volatile("ret");
131 }
132
133 void back(void)
134 {
135         switch_stack();
136         blink();
137 }
138
139 void main(void)
140 {
141         ao_bringup_init();
142
143         printf("starting\n");
144
145         init_stack(blink);
146         switch_stack();
147 }