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 #define PUSH8(stack, val)       (*((stack)--) = (val))
  23
  24 void
  25 blink(void)
  26 {
  27         while (1) {
  28                 LEDPORT ^= (1 << LEDOUT);
  29                 _delay_ms(200);
  30         }
  31 }
  32
  33
  34 uint8_t *
  35 init_stack(uint8_t *stack_base, int size, void (*f) (void))
  36 {
  37         uint8_t         *stack = stack_base + size - 1;
  38         uint16_t        a;
  39         uint8_t         h, l;
  40
  41         /* Return address */
  42         a = (uint16_t) f;
  43         l = a;
  44         h = a >> 8;
  45         PUSH8(stack, l);
  46         PUSH8(stack, h);
  47
  48         /* Clear register values */
  49         l = 32;
  50         while (l--)
  51                 PUSH8(stack, 0);
  52
  53         /* SREG with interrupts enabled */
  54         PUSH8(stack, 0x80);
  55         return stack;
  56 }
  57
  58 void
  59 switch_stack(uint8_t *sp) __attribute__((naked));
  60
  61 void
  62 switch_stack(uint8_t *sp)
  63 {
  64         uint8_t sp_l, sp_h;
  65
  66         sp_l = (uint16_t) sp;
  67         sp_h = ((uint16_t) sp) >> 8;
  68         asm volatile ("out __SP_H__,%0" : : "r" (sp_h) );
  69         asm volatile ("out __SP_L__,%0" : : "r" (sp_l) );
  70         asm volatile("pop r0"   "\n\t"
  71                      "out __SREG__, r0");
  72         asm volatile("pop r0" "\n\t"
  73                      "pop r1" "\n\t"
  74                      "pop r2" "\n\t"
  75                      "pop r3" "\n\t"
  76                      "pop r4");
  77         asm volatile("pop r5" "\n\t"
  78                      "pop r6" "\n\t"
  79                      "pop r7" "\n\t"
  80                      "pop r8" "\n\t"
  81                      "pop r9");
  82         asm volatile("pop r10" "\n\t"
  83                      "pop r11" "\n\t"
  84                      "pop r12" "\n\t"
  85                      "pop r13" "\n\t"
  86                      "pop r14");
  87         asm volatile("pop r15" "\n\t"
  88                      "pop r16" "\n\t"
  89                      "pop r17" "\n\t"
  90                      "pop r18" "\n\t"
  91                      "pop r19");
  92         asm volatile("pop r20" "\n\t"
  93                      "pop r21" "\n\t"
  94                      "pop r22" "\n\t"
  95                      "pop r23" "\n\t"
  96                      "pop r24");
  97         asm volatile("pop r25" "\n\t"
  98                      "pop r26" "\n\t"
  99                      "pop r27" "\n\t"
 100                      "pop r28" "\n\t"
 101                      "pop r29");
 102         asm volatile("pop r30" "\n\t"
 103                      "pop r31");
 104         asm volatile("ret");
 105 }
 106
 107 uint8_t new_stack[512];
 108
 109 void main(void)
 110 {
 111         uint8_t *stack;
 112         ao_bringup_init();
 113
 114         printf("starting\n");
 115
 116         stack = init_stack(new_stack, sizeof (new_stack), blink);
 117         switch_stack(stack);
 118 }