2 * Copyright 2016, Fabrice DESCLAUX <serpilliere@droids-corp.org>
3 * Copyright 2016, Olivier MATZ <zer0@droids-corp.org>
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
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.
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.
32 #include "ucg_reent_intr.h"
45 struct wrapper_stack {
56 __attribute__((naked)) uint32_t
57 ret_wrapper_stkalign_float(void)
66 "msr APSR_nzcvq, R0 \n"
68 "POP {R0-R3, R12, PC} \n"
73 __attribute__((naked)) uint32_t
74 ret_wrapper_stknoalign_float(void)
85 "msr APSR_nzcvq, R0 \n"
87 "POP {R0-R3, R12, PC} \n"
92 __attribute__((naked)) uint32_t
93 ret_wrapper_stkalign_nofloat(void)
102 "msr APSR_nzcvq, R0 \n"
103 /* restore PSR, LR */
104 "POP {R0-R3, R12, PC} \n"
108 __attribute__((naked)) uint32_t
109 ret_wrapper_stknoalign_nofloat(void)
116 /* restore PSR, LR */
120 "msr APSR_nzcvq, R0 \n"
121 /* restore PSR, LR */
122 "POP {R0-R3, R12, PC} \n"
126 void reent_intr(void);
128 void ucg_reent_intr(uint32_t *context, void *fct)
129 //void ucg_reent_intr(uint32_t *context)
131 struct saved_stack *stk_new = (void *)context;
132 struct saved_stack *stk_old = (void *)(&context[10]);
135 struct wrapper_stack wstack;
137 stk_padding = (stk_old->psr & 0x200)?1:0;
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;
149 exe_return = (uint32_t)context[-4];
152 stk_new->pc = (uint32_t)fct;
153 // stk_new->pc = (uint32_t)reent_intr;
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;
162 if (exe_return & 0x10) {
163 /* No Float stacked */
165 stk_new->lr = (uint32_t)ret_wrapper_stknoalign_nofloat;
166 memcpy(&context[10+1], &wstack, sizeof(wstack));
168 memcpy(&context[10], &wstack, sizeof(wstack));
169 stk_new->lr = (uint32_t)ret_wrapper_stkalign_nofloat;
173 //STM_EVAL_LEDOn(LED7);
174 memmove(&context[10], &context[18], 18*4);
176 //STM_EVAL_LEDOn(LED10);
177 memcpy(&context[10+18+1], &wstack, sizeof(wstack));
178 stk_new->lr = (uint32_t)ret_wrapper_stknoalign_float;
180 //STM_EVAL_LEDOn(LED9);
181 memcpy(&context[10+18], &wstack, sizeof(wstack));
182 stk_new->lr = (uint32_t)ret_wrapper_stkalign_float;
187 /* force return no fpu */
189 /* create exe_return for real state return */
190 context[8] = 0x1337beef;
191 context[8+1] = exe_return;