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