125 lines
4.9 KiB
C
125 lines
4.9 KiB
C
/*
|
|
* Copyright (c) 2006-2021, RT-Thread Development Team
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Change Logs:
|
|
* Date Author Notes
|
|
* 2011-09-23 Bernard the first version
|
|
* 2011-10-05 Bernard add thumb mode
|
|
* 2021-11-04 GuEe-GUI set sp with SP_ELx
|
|
* 2021-12-28 GuEe-GUI add fpu support
|
|
*/
|
|
#include <rtthread.h>
|
|
#include <armv8.h>
|
|
|
|
#define INITIAL_SPSR_EL3 (PSTATE_EL3 | SP_ELx)
|
|
#define INITIAL_SPSR_EL2 (PSTATE_EL2 | SP_ELx)
|
|
#define INITIAL_SPSR_EL1 (PSTATE_EL1 | SP_ELx)
|
|
|
|
/**
|
|
* This function will initialize thread stack
|
|
*
|
|
* @param tentry the entry of thread
|
|
* @param parameter the parameter of entry
|
|
* @param stack_addr the beginning stack address
|
|
* @param texit the function will be called when thread exit
|
|
*
|
|
* @return stack address
|
|
*/
|
|
rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit)
|
|
{
|
|
rt_ubase_t *stk;
|
|
rt_ubase_t current_el;
|
|
|
|
stk = (rt_ubase_t*)stack_addr;
|
|
|
|
*(--stk) = (rt_ubase_t) 0; /* Q0 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q0 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q1 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q1 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q2 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q2 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q3 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q3 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q4 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q4 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q5 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q5 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q6 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q6 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q7 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q7 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q8 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q8 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q9 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q9 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q10 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q10 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q11 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q11 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q12 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q12 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q13 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q13 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q14 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q14 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q15 */
|
|
*(--stk) = (rt_ubase_t) 0; /* Q15 */
|
|
|
|
*(--stk) = ( rt_ubase_t ) 11; /* X1 */
|
|
*(--stk) = ( rt_ubase_t ) parameter; /* X0 */
|
|
*(--stk) = ( rt_ubase_t ) 33; /* X3 */
|
|
*(--stk) = ( rt_ubase_t ) 22; /* X2 */
|
|
*(--stk) = ( rt_ubase_t ) 55; /* X5 */
|
|
*(--stk) = ( rt_ubase_t ) 44; /* X4 */
|
|
*(--stk) = ( rt_ubase_t ) 77; /* X7 */
|
|
*(--stk) = ( rt_ubase_t ) 66; /* X6 */
|
|
*(--stk) = ( rt_ubase_t ) 99; /* X9 */
|
|
*(--stk) = ( rt_ubase_t ) 88; /* X8 */
|
|
*(--stk) = ( rt_ubase_t ) 11; /* X11 */
|
|
*(--stk) = ( rt_ubase_t ) 10; /* X10 */
|
|
*(--stk) = ( rt_ubase_t ) 13; /* X13 */
|
|
*(--stk) = ( rt_ubase_t ) 12; /* X12 */
|
|
*(--stk) = ( rt_ubase_t ) 15; /* X15 */
|
|
*(--stk) = ( rt_ubase_t ) 14; /* X14 */
|
|
*(--stk) = ( rt_ubase_t ) 17; /* X17 */
|
|
*(--stk) = ( rt_ubase_t ) 16; /* X16 */
|
|
*(--stk) = ( rt_ubase_t ) 19; /* X19 */
|
|
*(--stk) = ( rt_ubase_t ) 18; /* X18 */
|
|
*(--stk) = ( rt_ubase_t ) 21; /* X21 */
|
|
*(--stk) = ( rt_ubase_t ) 20; /* X20 */
|
|
*(--stk) = ( rt_ubase_t ) 23; /* X23 */
|
|
*(--stk) = ( rt_ubase_t ) 22; /* X22 */
|
|
*(--stk) = ( rt_ubase_t ) 25; /* X25 */
|
|
*(--stk) = ( rt_ubase_t ) 24; /* X24 */
|
|
*(--stk) = ( rt_ubase_t ) 27; /* X27 */
|
|
*(--stk) = ( rt_ubase_t ) 26; /* X26 */
|
|
*(--stk) = ( rt_ubase_t ) 29; /* X29 */
|
|
*(--stk) = ( rt_ubase_t ) 28; /* X28 */
|
|
*(--stk) = ( rt_ubase_t ) 0; /* FPSR */
|
|
*(--stk) = ( rt_ubase_t ) 0; /* FPCR */
|
|
*(--stk) = ( rt_ubase_t ) 0; /* XZR - has no effect, used so there are an even number of registers. */
|
|
*(--stk) = ( rt_ubase_t ) texit; /* X30 - procedure call link register. */
|
|
|
|
current_el = rt_hw_get_current_el();
|
|
|
|
if(current_el == 3)
|
|
{
|
|
*(--stk) = INITIAL_SPSR_EL3;
|
|
}
|
|
else if(current_el == 2)
|
|
{
|
|
*(--stk) = INITIAL_SPSR_EL2;
|
|
}
|
|
else
|
|
{
|
|
*(--stk) = INITIAL_SPSR_EL1;
|
|
}
|
|
|
|
*(--stk) = ( rt_ubase_t ) tentry; /* Exception return address. */
|
|
|
|
/* return task's current stack address */
|
|
return (rt_uint8_t *)stk;
|
|
}
|