summaryrefslogtreecommitdiff
path: root/ao-bringup-avr/ao-switch.c
blob: 263a70d5b0e701e047c41971bff5f9dd057637e1 (plain) (blame)
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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
/*
 * 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

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)
{
	while (1) {
		LEDPORT ^= (1 << LEDOUT);
		_delay_ms(200);
	}
}

void
function(void)
{
	return;
}

void
init_stack(void (*f) (void))
{
	uint8_t		*stack = new_stack + AO_STACK_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);
	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);
}

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

void
switch_stack(void)
{
	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"
		     "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 ("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();
}

void main(void)
{
	ao_bringup_init();

	printf("starting\n");

	init_stack(blink);
	switch_stack();
}