libs/rt-thread
libs/rt-thread
4
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
rt-thread/libcpu/arm/armv6/vfp_entry_gcc.S
Bernard Xiong 43f68131ce [BSP] Add fh8620 bsp from Shanghai Fullhan Microelectronics Co., Ltd. 
FH8620 BSP
Copyright (c) 2016 Shanghai Fullhan Microelectronics Co., Ltd.
All rights reserved
2016-04-18 13:52:39 +08:00

240 lines
7.5 KiB
ArmAsm
Raw Blame History

/*
* File : vfp_entry_gcc.S
* This file is part of RT-Thread RTOS
* COPYRIGHT (C) 2006, RT-Thread Development Team
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
* Change Logs:
* Date Author Notes
* 2014-11-07 weety first version
*/
#include <rtconfig.h>
#ifdef RT_USING_VFP
#include "armv6.h"
#include "vfp.h"
//#define DEBUG
.macro PRINT, str
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro PRINT1, str, arg
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
mov r1, \arg
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro PRINT3, str, arg1, arg2, arg3
#ifdef DEBUG
stmfd sp!, {r0-r3, ip, lr}
mov r3, \arg3
mov r2, \arg2
mov r1, \arg1
add r0, pc, #4
bl rt_kprintf
b 1f
.asciz "VFP: \str\n"
.balign 4
1: ldmfd sp!, {r0-r3, ip, lr}
#endif
.endm
.macro get_vfpregs_offset, rd
ldr \rd, .vfp_offset
ldr \rd, [\rd]
.endm
.vfp_offset:
.word vfpregs_offset
.macro vfp_restore_working_reg, base, rd0
vldmia \base!, {d0-d15}
#ifdef RT_USING_VFPv3
vmrs \rd0, mvfr0
and \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
cmp \rd0, #2 @ 0b0000 Not supported.
@ 0b0001 Supported, 16 <20><>64-bit registers.
@ 0b0010 Supported, 32 <20><>64-bit registers.
vldmiaeq \base!, {d16-d31}
addne \base, \base, #32*4 @ skip unused registers
#endif
.endm
.macro vfp_save_working_reg, base, rd0
vstmia \base!, {d0-d15} @ save the working registers
#ifdef RT_USING_VFPv3
vmrs \rd0, mvfr0
and \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
cmp \rd0, #2 @ 0b0000 Not supported.
@ 0b0001 Supported, 16 <20><>64-bit registers.
@ 0b0010 Supported, 32 <20><>64-bit registers.
vstmiaeq \base!, {d16-d31}
addne \base, \base, #32*4 @ skip unused registers
#endif
.endm
.macro vfp_restore_state, base, fpexc_rd, rd0, rd1, rd2
ldmia \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ load FPEXC, FPSCR, FPINST, FPINST2
tst \fpexc_rd, #FPEXC_EX @ vfp is in the exceptional state?
beq 1f
vmsr fpinst, \rd1 @ restore fpinst
tst \fpexc_rd, #FPEXC_FP2V @ FPINST2 instruction valid
beq 1f
vmsr fpinst2, \rd2 @ restore fpinst2
1:
vmsr fpscr, \rd0 @ restore fpscr
.endm
.macro vfp_save_state, base, fpexc_rd, rd0, rd1, rd2
vmrs \rd0, fpscr @ current status
tst \fpexc_rd, #FPEXC_EX @ vfp is in the exceptional state?
beq 1f
vmrs \rd1, fpinst @ get fpinst
tst \fpexc_rd, #FPEXC_FP2V @ FPINST2 instruction valid
beq 1f
vmrs \rd2, fpinst2 @ get fpinst2
1:
stmia \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ save FPEXC, FPSCR, FPINST, FPINST2
.endm
/*
* VFP hardware support entry point.
* r0 = faulted instruction
* r2 = faulted PC+4
* r9 = successful return
* r10 = rt_thread structure
* lr = failure return
*/
.globl vfp_entry
vfp_entry:
ldr r1, =rt_interrupt_nest
ldr r1, [r1] @ get rt_interrupt_nest
cmp r1, #0 @ rt_interrupt_nest == 0?
bne irq_vfp_entry @ irq handler used VFP
get_vfpregs_offset r11
add r10, r10, r11 @ r10 = vfpregs
vmrs r1, fpexc
tst r1, #FPEXC_EN
bne __lookup_vfp_exceptions @ if the VFP already enabled, now checking vfp exceptions
ldr r3, last_vfp_context_address
orr r1, r1, #FPEXC_EN @ set VFP enable bit
ldr r4, [r3] @ get last_vfp_context pointer
bic r5, r1, #FPEXC_EX @ clear exceptions status
cmp r4, r10
beq __switch_to_the_same_thread @ switch to the same thread, checking pending exception.
vmsr fpexc, r5 @ enable VFP, clear any pending exceptions
/* Save the current VFP registers to the old thread context */
cmp r4, #0
beq __no_last_vfp_context
vfp_save_working_reg r4, r5 @ save the working registers
vfp_save_state r4, r1, r5, r6, r8 @ save vfp state registers
__no_last_vfp_context:
str r10, [r3] @ update the last_vfp_context pointer
vfp_restore_working_reg r10, r5 @ restore the working registers
vfp_restore_state r10, r1, r5, r6, r8 @ restore vfp state registers
__switch_to_the_same_thread:
tst r1, #FPEXC_EX
bne __do_exception
vmsr fpexc, r1 @ restore fpexc last
sub r2, r2, #4
str r2, [sp, #S_PC] @ retry the faulted instruction
PRINT1 "return instr=0x%08x", r2
mov pc, r9
__lookup_vfp_exceptions:
tst r1, #FPEXC_EX | FPEXC_DEX @ Check for synchronous or asynchronous exception
bne __do_exception
vmrs r5, fpscr
tst r5, #FPSCR_IXE
bne __do_exception
PRINT "__lookup_vfp_exceptions"
mov pc, lr
__do_exception:
PRINT "__do_exception"
push {lr}
mov r5, r1
bic r5, #FPEXC_EX @ clear exception
vmsr fpexc, r5
bl vfp_exception @ r0 = faulted instruction, r1 = fpexc
pop {pc}
@mov pc, lr
irq_vfp_entry:
vmrs r1, fpexc
tst r1, #FPEXC_EN
bne __lookup_vfp_exceptions @ if the VFP already enabled, now checking vfp exceptions
ldr r3, last_vfp_context_address
orr r1, r1, #FPEXC_EN @ set VFP enable bit
ldr r4, [r3] @ get last_vfp_context pointer
bic r5, r1, #FPEXC_EX @ clear exceptions status
vmsr fpexc, r5 @ enable VFP, clear any pending exceptions
/* Save the current VFP registers to the old thread context */
cmp r4, #0 @ last_vfp_context != NULL ?
beq __no_save_vfp_context
vfp_save_working_reg r4, r5 @ save the working registers
vfp_save_state r4, r1, r5, r6, r8 @ save vfp state registers
mov r4, #0
str r4, [r3] @ update the last_vfp_context pointer
@ last_vfp_context = NULL
__no_save_vfp_context:
sub r2, r2, #4
str r2, [sp, #S_PC] @ retry the faulted instruction
PRINT1 "return instr=0x%08x", r2
mov pc, r9
.align
last_vfp_context_address:
.word last_vfp_context
#endif
View Git Blame Copy Permalink