arch/stm32/ucg_reent_intr.c

   1 /*
   2  * Copyright 2016, Fabrice DESCLAUX <serpilliere@droids-corp.org>
   3  * Copyright 2016, Olivier MATZ <zer0@droids-corp.org>
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *
   8  *     * Redistributions of source code must retain the above copyright
   9  *       notice, this list of conditions and the following disclaimer.
  10  *     * Redistributions in binary form must reproduce the above copyright
  11  *       notice, this list of conditions and the following disclaimer in the
  12  *       documentation and/or other materials provided with the distribution.
  13  *     * Neither the name of the University of California, Berkeley nor the
  14  *       names of its contributors may be used to endorse or promote products
  15  *       derived from this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
  18  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  19  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  20  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
  21  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  22  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  23  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  24  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  25  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  26  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  27  */
  28
  29 #include <stdint.h>
  30 #include <string.h>
  31
  32 #include "ucg_reent_intr.h"
  33
  34 struct saved_stack {
  35         uint32_t r0;
  36         uint32_t r1;
  37         uint32_t r2;
  38         uint32_t r3;
  39         uint32_t r12;
  40         uint32_t lr;
  41         uint32_t pc;
  42         uint32_t psr;
  43 };
  44
  45 struct wrapper_stack {
  46         uint32_t psr;
  47         uint32_t lr;
  48         uint32_t r0;
  49         uint32_t r1;
  50         uint32_t r2;
  51         uint32_t r3;
  52         uint32_t r12;
  53         uint32_t pc;
  54 };
  55
  56 __attribute__((naked)) uint32_t
  57 ret_wrapper_stkalign_float(void)
  58 {
  59         __asm__ volatile (
  60                 /* restore float */
  61                 "ADD    SP, SP, 0x50       \n"
  62                 /* restore PSR, LR */
  63                 "POP   {R0, LR}        \n"
  64                 /* restore PSR */
  65                 //"MSR   PSR, R0       \n"
  66                 "msr APSR_nzcvq, R0 \n"
  67                 /* restore PSR, LR */
  68                 "POP   {R0-R3, R12, PC}        \n"
  69         );
  70 }
  71
  72
  73 __attribute__((naked)) uint32_t
  74 ret_wrapper_stknoalign_float(void)
  75 {
  76         __asm__ volatile (
  77                 /* restore float */
  78                 "ADD    SP, SP, 0x50       \n"
  79                 /* remove padding */
  80                 "ADD    SP, SP, 0x4       \n"
  81                 /* restore PSR, LR */
  82                 "POP   {R0, LR}        \n"
  83                 /* restore PSR */
  84                 //"MSR   PSR, R0       \n"
  85                 "msr APSR_nzcvq, R0 \n"
  86                 /* restore PSR, LR */
  87                 "POP   {R0-R3, R12, PC}        \n"
  88         );
  89 }
  90
  91
  92 __attribute__((naked)) uint32_t
  93 ret_wrapper_stkalign_nofloat(void)
  94 {
  95         __asm__ volatile (
  96                 /* restore float */
  97                 "ADD    SP, SP, 0x8       \n"
  98                 /* restore PSR, LR */
  99                 "POP   {R0, LR}        \n"
 100                 /* restore PSR */
 101                 //"MSR   PSR, R0       \n"
 102                 "msr APSR_nzcvq, R0 \n"
 103                 /* restore PSR, LR */
 104                 "POP   {R0-R3, R12, PC}        \n"
 105         );
 106 }
 107
 108 __attribute__((naked)) uint32_t
 109 ret_wrapper_stknoalign_nofloat(void)
 110 {
 111         __asm__ volatile (
 112                 /* restore float */
 113                 "ADD    SP, SP, 0x8       \n"
 114                 /* remove padding */
 115                 "ADD    SP, SP, 0x4       \n"
 116                 /* restore PSR, LR */
 117                 "POP   {R0, LR}        \n"
 118                 /* restore PSR */
 119                 //"MSR   PSR, R0       \n"
 120                 "msr APSR_nzcvq, R0 \n"
 121                 /* restore PSR, LR */
 122                 "POP   {R0-R3, R12, PC}        \n"
 123         );
 124 }
 125
 126 void reent_intr(void);
 127
 128 void ucg_reent_intr(uint32_t *context, void *fct)
 129 //void ucg_reent_intr(uint32_t *context)
 130 {
 131         struct saved_stack *stk_new = (void *)context;
 132         struct saved_stack *stk_old = (void *)(&context[10]);
 133         uint32_t exe_return;
 134         int stk_padding;
 135         struct wrapper_stack wstack;
 136
 137         stk_padding = (stk_old->psr & 0x200)?1:0;
 138
 139         /* copy saved ctxt */
 140         wstack.r0 = stk_old->r0;
 141         wstack.r1 = stk_old->r1;
 142         wstack.r2 = stk_old->r2;
 143         wstack.r3 = stk_old->r3;
 144         wstack.r12 = stk_old->r12;
 145         wstack.lr = stk_old->lr;
 146         wstack.psr = stk_old->psr & ~0x200;
 147         wstack.pc = stk_old->pc | 1;
 148
 149         exe_return = (uint32_t)context[-4];
 150
 151
 152         stk_new->pc = (uint32_t)fct;
 153 //      stk_new->pc = (uint32_t)reent_intr;
 154         /* set arguments */
 155         /* stk_new->r0 = 0; */
 156         /* stk_new->r1 = 1; */
 157         /* stk_new->r2 = 2; */
 158         /* stk_new->r3 = 3; */
 159         stk_new->r12 = wstack.r12;
 160         stk_new->psr = 0x21000000;
 161
 162         if (exe_return & 0x10) {
 163                 /*  No Float stacked */
 164                 if (stk_padding) {
 165                         stk_new->lr = (uint32_t)ret_wrapper_stknoalign_nofloat;
 166                         memcpy(&context[10+1], &wstack, sizeof(wstack));
 167                 } else {
 168                         memcpy(&context[10], &wstack, sizeof(wstack));
 169                         stk_new->lr = (uint32_t)ret_wrapper_stkalign_nofloat;
 170                 }
 171         } else {
 172                 /* Float stacked */
 173                 //STM_EVAL_LEDOn(LED7);
 174                 memmove(&context[10], &context[18], 18*4);
 175                 if (stk_padding) {
 176                         //STM_EVAL_LEDOn(LED10);
 177                         memcpy(&context[10+18+1], &wstack, sizeof(wstack));
 178                         stk_new->lr = (uint32_t)ret_wrapper_stknoalign_float;
 179                 } else {
 180                         //STM_EVAL_LEDOn(LED9);
 181                         memcpy(&context[10+18], &wstack, sizeof(wstack));
 182                         stk_new->lr = (uint32_t)ret_wrapper_stkalign_float;
 183                 }
 184
 185         }
 186
 187         /* force return no fpu */
 188         context[-4] |= 0x10;
 189         /* create exe_return for real state return */
 190         context[8] = 0x1337beef;
 191         context[8+1] = exe_return;
 192 }