/*
* Copyright (c) 2013, 2017 embedded brains GmbH. All rights reserved.
*
* embedded brains GmbH
* Dornierstr. 4
* 82178 Puchheim
* Germany
* <rtems@embedded-brains.de>
*
* The license and distribution terms for this file may be
* found in the file LICENSE in this distribution or at
* http://www.rtems.org/license/LICENSE.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <rtems/asm.h>
#include <rtems/score/cpu.h>
#define FRAME_OFFSET_R4 0
#define FRAME_OFFSET_R5 4
#define FRAME_OFFSET_R6 8
#define FRAME_OFFSET_R7 12
#define FRAME_OFFSET_R8 16
#define FRAME_OFFSET_R9 20
#define FRAME_OFFSET_R10 24
#define FRAME_OFFSET_R11 28
#define FRAME_OFFSET_LR 32
#ifdef ARM_MULTILIB_VFP
#define FRAME_OFFSET_D8 40
#define FRAME_OFFSET_D9 48
#define FRAME_OFFSET_D10 56
#define FRAME_OFFSET_D11 64
#define FRAME_OFFSET_D12 72
#define FRAME_OFFSET_D13 80
#define FRAME_OFFSET_D14 88
#define FRAME_OFFSET_D15 96
#define FRAME_SIZE (FRAME_OFFSET_D15 + 8)
#else
#define FRAME_SIZE (FRAME_OFFSET_LR + 4)
#endif
.syntax unified
.section .text
#ifdef __thumb2__
FUNCTION_THUMB_ENTRY(_CPU_Context_validate)
#else
FUNCTION_ENTRY(_CPU_Context_validate)
#endif
/* Save */
sub sp, sp, #FRAME_SIZE
mov r1, r4
str r1, [sp, #FRAME_OFFSET_R4]
mov r1, r5
str r1, [sp, #FRAME_OFFSET_R5]
mov r1, r6
str r1, [sp, #FRAME_OFFSET_R6]
mov r1, r7
str r1, [sp, #FRAME_OFFSET_R7]
mov r1, r8
str r1, [sp, #FRAME_OFFSET_R8]
mov r1, r9
str r1, [sp, #FRAME_OFFSET_R9]
mov r1, r10
str r1, [sp, #FRAME_OFFSET_R10]
mov r1, r11
str r1, [sp, #FRAME_OFFSET_R11]
mov r1, lr
str r1, [sp, #FRAME_OFFSET_LR]
#ifdef ARM_MULTILIB_VFP
vstr d8, [sp, #FRAME_OFFSET_D8]
vstr d9, [sp, #FRAME_OFFSET_D9]
vstr d10, [sp, #FRAME_OFFSET_D10]
vstr d11, [sp, #FRAME_OFFSET_D11]
vstr d12, [sp, #FRAME_OFFSET_D12]
vstr d13, [sp, #FRAME_OFFSET_D13]
vstr d14, [sp, #FRAME_OFFSET_D14]
vstr d15, [sp, #FRAME_OFFSET_D15]
#endif
/* Fill */
/* R1 is used for temporary values */
mov r1, r0
/* R2 contains the stack pointer */
mov r2, sp
.macro fill_register reg
add r1, r1, #1
mov \reg, r1
.endm
#ifdef ARM_MULTILIB_VFP
/* R3 contains the FPSCR */
vmrs r3, FPSCR
ldr r4, =0xf000001f
bic r3, r3, r4
and r4, r4, r0
orr r3, r3, r4
vmsr FPSCR, r3
#else
fill_register r3
#endif
fill_register r4
fill_register r5
fill_register r6
fill_register r7
fill_register r8
fill_register r9
fill_register r10
fill_register r11
fill_register r12
fill_register lr
#ifdef ARM_MULTILIB_VFP
.macro fill_vfp_register reg
add r1, r1, #1
vmov \reg, r1, r1
.endm
fill_vfp_register d0
fill_vfp_register d1
fill_vfp_register d2
fill_vfp_register d3
fill_vfp_register d4
fill_vfp_register d5
fill_vfp_register d6
fill_vfp_register d7
fill_vfp_register d8
fill_vfp_register d9
fill_vfp_register d10
fill_vfp_register d11
fill_vfp_register d12
fill_vfp_register d13
fill_vfp_register d14
fill_vfp_register d15
#ifdef ARM_MULTILIB_VFP_D32
fill_vfp_register d16
fill_vfp_register d17
fill_vfp_register d18
fill_vfp_register d19
fill_vfp_register d20
fill_vfp_register d21
fill_vfp_register d22
fill_vfp_register d23
fill_vfp_register d24
fill_vfp_register d25
fill_vfp_register d26
fill_vfp_register d27
fill_vfp_register d28
fill_vfp_register d29
fill_vfp_register d30
fill_vfp_register d31
#endif /* ARM_MULTILIB_VFP_D32 */
#endif /* ARM_MULTILIB_VFP */
/* Check */
check:
.macro check_register reg
add r1, r1, #1
cmp \reg, r1
bne restore
.endm
/* A compare involving the stack pointer is deprecated */
mov r1, sp
cmp r2, r1
bne restore
mov r1, r0
#ifdef __thumb2__
cmp r1, r1
itttt eq
addeq r1, #1
addeq r1, #2
addeq r1, #4
addeq r1, #8
subs r1, #15
cmp r1, r0
bne restore
cmp r1, r1
iteee eq
addeq r1, #1
addne r1, #2
addne r1, #4
addne r1, #8
subs r1, #1
cmp r1, r0
bne restore
#endif
#ifndef ARM_MULTILIB_VFP
check_register r3
#endif
check_register r4
check_register r5
check_register r6
check_register r7
check_register r8
check_register r9
check_register r10
check_register r11
check_register r12
check_register lr
#ifdef ARM_MULTILIB_VFP
b check_vfp
#endif
b check
/* Restore */
restore:
ldr r1, [sp, #FRAME_OFFSET_R4]
mov r4, r1
ldr r1, [sp, #FRAME_OFFSET_R5]
mov r5, r1
ldr r1, [sp, #FRAME_OFFSET_R6]
mov r6, r1
ldr r1, [sp, #FRAME_OFFSET_R7]
mov r7, r1
ldr r1, [sp, #FRAME_OFFSET_R8]
mov r8, r1
ldr r1, [sp, #FRAME_OFFSET_R9]
mov r9, r1
ldr r1, [sp, #FRAME_OFFSET_R10]
mov r10, r1
ldr r1, [sp, #FRAME_OFFSET_R11]
mov r11, r1
ldr r1, [sp, #FRAME_OFFSET_LR]
mov lr, r1
#ifdef ARM_MULTILIB_VFP
vldr d8, [sp, #FRAME_OFFSET_D8]
vldr d9, [sp, #FRAME_OFFSET_D9]
vldr d10, [sp, #FRAME_OFFSET_D10]
vldr d11, [sp, #FRAME_OFFSET_D11]
vldr d12, [sp, #FRAME_OFFSET_D12]
vldr d13, [sp, #FRAME_OFFSET_D13]
vldr d14, [sp, #FRAME_OFFSET_D14]
vldr d15, [sp, #FRAME_OFFSET_D15]
#endif
add sp, sp, #FRAME_SIZE
bx lr
FUNCTION_END(_CPU_Context_validate)
#ifdef ARM_MULTILIB_VFP
check_vfp:
.macro check_vfp_register reg
add r1, r1, #1
vmov r4, r5, \reg
cmp r4, r5
bne 1f
cmp r1, r4
bne 1f
b 2f
1:
b restore
2:
.endm
vmrs r4, FPSCR
cmp r4, r3
bne restore
check_vfp_register d0
check_vfp_register d1
check_vfp_register d2
check_vfp_register d3
check_vfp_register d4
check_vfp_register d5
check_vfp_register d6
check_vfp_register d7
check_vfp_register d8
check_vfp_register d9
check_vfp_register d10
check_vfp_register d11
check_vfp_register d12
check_vfp_register d13
check_vfp_register d14
check_vfp_register d15
#ifdef ARM_MULTILIB_VFP_D32
check_vfp_register d16
check_vfp_register d17
check_vfp_register d18
check_vfp_register d19
check_vfp_register d20
check_vfp_register d21
check_vfp_register d22
check_vfp_register d23
check_vfp_register d24
check_vfp_register d25
check_vfp_register d26
check_vfp_register d27
check_vfp_register d28
check_vfp_register d29
check_vfp_register d30
check_vfp_register d31
#endif /* ARM_MULTILIB_VFP_D32 */
/* Restore r4 and r5 */
mov r1, r0
fill_register r4
fill_register r5
b check
#endif /* ARM_MULTILIB_VFP */
