git.gag.com Git - fw/altos/blob - 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         l, h;
  91
  92 //      for (l = 0; l < AO_STACK_SIZE - stack_count; l++)
  93 //              printf ("stack[%2d] = %2x\n", l, sp[l]);
  94         l = new_stack[AO_STACK_SIZE - 1];
  95         h = new_stack[AO_STACK_SIZE - 2];
  96 //      printf ("Target return address: h %02x l %02x\n", h, l);
  97 #if 0
  98         asm("push %0" : : "r" (l));
  99         asm("push %0" : : "r" (h));
 100         asm("ret");
 101 #endif
 102 #if 0
 103         asm("push r31; push r30");
 104         asm("push r29; push r28; push r27; push r26; push r25");
 105         asm("push r24; push r23; push r22; push r21; push r20");
 106         asm("push r19; push r18; push r17; push r16; push r15");
 107         asm("push r14; push r13; push r12; push r11; push r10");
 108         asm("push r9; push r8; push r7; push r6; push r5");
 109         asm("push r4; push r3; push r2; push r1; push r0");
 110         asm("in r0, __SREG__" "\n\t"
 111             "push r0");
 112 #endif
 113
 114         uint8_t sp_l, sp_h;
 115         static  uint8_t reg[34];
 116         sp_l = (uint16_t) sp;
 117         sp_h = ((uint16_t) sp) >> 8;
 118         asm volatile ("out __SP_H__,%0" : : "r" (sp_h) );
 119         asm volatile ("out __SP_L__,%0" : : "r" (sp_l) );
 120
 121         asm volatile ("in %0,__SP_H__" : "=&r" (sp_h));
 122         asm volatile ("in %0,__SP_L__" : "=&r" (sp_l));
 123         asm volatile("pop r0"   "\n\t"
 124                      "out __SREG__, r0");
 125         asm volatile("pop r0" "\n\t"
 126                      "pop r1" "\n\t"
 127                      "pop r2" "\n\t"
 128                      "pop r3" "\n\t"
 129                      "pop r4");
 130         asm volatile("pop r5" "\n\t"
 131                      "pop r6" "\n\t"
 132                      "pop r7" "\n\t"
 133                      "pop r8" "\n\t"
 134                      "pop r9");
 135         asm volatile("pop r10" "\n\t"
 136                      "pop r11" "\n\t"
 137                      "pop r12" "\n\t"
 138                      "pop r13" "\n\t"
 139                      "pop r14");
 140         asm volatile("pop r15" "\n\t"
 141                      "pop r16" "\n\t"
 142                      "pop r17" "\n\t"
 143                      "pop r18" "\n\t"
 144                      "pop r19");
 145         asm volatile("pop r20" "\n\t"
 146                      "pop r21" "\n\t"
 147                      "pop r22" "\n\t"
 148                      "pop r23" "\n\t"
 149                      "pop r24");
 150         asm volatile("pop r25" "\n\t"
 151                      "pop r26" "\n\t"
 152                      "pop r27" "\n\t"
 153                      "pop r28" "\n\t"
 154                      "pop r29");
 155         asm volatile("pop r30" "\n\t"
 156                      "pop r31");
 157         asm volatile ("in %0,__SP_H__" : "=&r" (sp_h));
 158         asm volatile ("in %0,__SP_L__" : "=&r" (sp_l));
 159         asm volatile ("pop %0" : "=&r" (h));
 160         asm volatile ("pop %0" : "=&r" (l));
 161         show_stack("before ret", sp_h, sp_l);
 162         show_stack("returning to", h, l);
 163         asm volatile("push %0" : : "r" (l));
 164         asm volatile("push %0" : : "r" (h));
 165         asm volatile("ret");
 166 }
 167
 168 void back(void)
 169 {
 170         switch_stack();
 171         blink();
 172 }
 173
 174 void main(void)
 175 {
 176         ao_bringup_init();
 177
 178         printf("starting\n");
 179
 180         init_stack(blink);
 181         switch_stack();
 182 }