From ecff4225d47e0c32f077e849c763e12915b0deca Mon Sep 17 00:00:00 2001 From: tomoyuki yamanaka Date: Tue, 25 Oct 2016 19:31:38 +0900 Subject: [PATCH] Fixed the issue about push/pop of VFP register. When the floating-point calculation is executed into the processing which is called from IRQ handlers, VFP register is overwritten. Therefore, we fixed the issue. we added the process to execute the saving (Push) and the returning (Pop) of VFP register in before and after the appropriate processing. --- .../TARGET_RZ_A1H/device/vfp_neon_push_pop.h | 166 ++++++++++++++++++ .../TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c | 32 +++- 2 files changed, 189 insertions(+), 9 deletions(-) create mode 100644 targets/TARGET_RENESAS/TARGET_RZ_A1H/device/vfp_neon_push_pop.h diff --git a/targets/TARGET_RENESAS/TARGET_RZ_A1H/device/vfp_neon_push_pop.h b/targets/TARGET_RENESAS/TARGET_RZ_A1H/device/vfp_neon_push_pop.h new file mode 100644 index 00000000000..15945a11e79 --- /dev/null +++ b/targets/TARGET_RENESAS/TARGET_RZ_A1H/device/vfp_neon_push_pop.h @@ -0,0 +1,166 @@ +#ifndef __VFP_NEON_PUSH_POP_H__ +#define __VFP_NEON_PUSH_POP_H__ + + +#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ +/* ARM armcc specific functions */ +#pragma push +#pragma arm +__STATIC_ASM void __vfp_neon_push(void) { + ARM + + VMRS R2,FPSCR + STMDB SP!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment + VSTMDB SP!,{D0-D15} + VSTMDB SP!,{D16-D31} + BX LR +} +#pragma pop + +#pragma push +#pragma arm +__STATIC_ASM void __vfp_neon_pop(void) { + ARM + + VLDMIA SP!,{D16-D31} + VLDMIA SP!,{D0-D15} + LDR R2,[SP] + VMSR FPSCR,R2 + ADD SP,SP,#8 + BX LR +} +#pragma pop + + +#pragma push +#pragma arm +__STATIC_ASM void __vfp_push(void) { + ARM + + VMRS R2,FPSCR + STMDB SP!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment + VSTMDB SP!,{D0-D15} + BX LR +} +#pragma pop + +#pragma push +#pragma arm +__STATIC_ASM void __vfp_pop(void) { + ARM + + VLDMIA SP!,{D0-D15} + LDR R2,[SP] + VMSR FPSCR,R2 + ADD SP,SP,#8 + BX LR +} +#pragma pop + +#elif (defined (__ICCARM__)) /*---------------- ICC Compiler ---------------------*/ + +__arm static inline void __vfp_neon_push(void) { +__asm( + "ARM \n" + "VMRS R2,FPSCR \n" + "STMDB SP!,{R2,R4} \n" // Push FPSCR, maintain 8-byte alignment + "VSTMDB SP!,{D0-D15} \n" + "VSTMDB SP!,{D16-D31} \n" + "BX lr \n" ); +} + +__arm static inline void __vfp_neon_pop(void) { +__asm( + "ARM \n" + "VLDMIA SP!,{D16-D31} \n" + "VLDMIA SP!,{D0-D15} \n" + "LDR R2,[SP] \n" + "VMSR FPSCR,R2 \n" + "ADD SP,SP,#8 \n" + "BX lr \n" ); +} + +__arm static inline void __vfp_push(void) { +__asm( + "ARM \n" + "VMRS R2,FPSCR \n" + "STMDB SP!,{R2,R4} \n" // Push FPSCR, maintain 8-byte alignment + "VSTMDB SP!,{D0-D15} \n" + "BX lr \n" ); +} + +__arm static inline void __vfp_pop(void) { +__asm( + "ARM \n" + "VLDMIA SP!,{D0-D15} \n" + "LDR R2,[SP] \n" + "VMSR FPSCR,R2 \n" + "ADD SP,SP,#8 \n" + "BX lr \n" ); +} + +#elif (defined (__GNUC__)) /*------------------ GNU Compiler ---------------------*/ + +__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_neon_push(void) +{ + __asm__ volatile ( + ".ARM;" + + "VMRS R2,FPSCR;" + "STMDB SP!,{R2,R4};" // Push FPSCR, maintain 8-byte alignment + "VSTMDB SP!,{D0-D15};" + "VSTMDB SP!,{D16-D31};" + : + : "i"(MODE_USR) + : ); + return; +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_neon_pop(void) +{ + __asm__ volatile ( + ".ARM;" + + "VLDMIA SP!,{D16-D31};" + "VLDMIA SP!,{D0-D15};" + "LDR R2,[SP];" + "VMSR FPSCR,R2;" + "ADD SP,SP,#8;" + : + : "i"(MODE_USR) + : ); + return; +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_push(void) +{ + __asm__ volatile ( + ".ARM;" + + "VMRS R2,FPSCR;" + "STMDB SP!,{R2,R4};" // Push FPSCR, maintain 8-byte alignment + "VSTMDB SP!,{D0-D15};" + : + : "i"(MODE_USR) + : ); + return; +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_pop(void) +{ + __asm__ volatile ( + ".ARM;" + + "VLDMIA SP!,{D0-D15};" + "LDR R2,[SP];" + "VMSR FPSCR,R2;" + "ADD SP,SP,#8;" + : + : "i"(MODE_USR) + : ); + return; +} + +#endif + +#endif diff --git a/targets/TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c b/targets/TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c index 70be974cd95..10ffe2a62c3 100644 --- a/targets/TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c +++ b/targets/TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c @@ -20,6 +20,7 @@ #include "RZ_A1_Init.h" #include "MBRZA1H.h" +#include "vfp_neon_push_pop.h" #define US_TICKER_TIMER_IRQn (OSTMI1TINT_IRQn) #define CPG_STBCR5_BIT_MSTP50 (0x01u) /* OSTM1 */ @@ -31,6 +32,8 @@ static double count_clock = 0; static uint32_t last_read = 0; static uint32_t wrap_arround = 0; static uint64_t ticker_us_last64 = 0; +static uint64_t set_cmp_val64 = 0; +static uint64_t timestamp64 = 0; void us_ticker_interrupt(void) { us_ticker_irq_handler(); @@ -80,9 +83,15 @@ static uint64_t ticker_read_counter64(void) { return cnt_val64; } -uint32_t us_ticker_read() { +static void us_ticker_read_last(void) { uint64_t cnt_val64; - uint64_t us_val64; + + cnt_val64 = ticker_read_counter64(); + + ticker_us_last64 = (cnt_val64 / count_clock); +} + +uint32_t us_ticker_read() { int check_irq_masked; #if defined ( __ICCARM__) @@ -91,22 +100,24 @@ uint32_t us_ticker_read() { check_irq_masked = __disable_irq(); #endif /* __ICCARM__ */ - cnt_val64 = ticker_read_counter64(); - us_val64 = (cnt_val64 / count_clock); - ticker_us_last64 = us_val64; + __vfp_neon_push(); + us_ticker_read_last(); + __vfp_neon_pop(); if (!check_irq_masked) { __enable_irq(); } /* clock to us */ - return (uint32_t)us_val64; + return (uint32_t)ticker_us_last64; +} + +static void us_ticker_calc_compare_match(void) { + set_cmp_val64 = timestamp64 * count_clock; } void us_ticker_set_interrupt(timestamp_t timestamp) { // set match value - uint64_t timestamp64; - uint64_t set_cmp_val64; volatile uint32_t set_cmp_val; uint64_t count_val_64; @@ -118,7 +129,10 @@ void us_ticker_set_interrupt(timestamp_t timestamp) { } /* calc compare mach timestamp */ - set_cmp_val64 = timestamp64 * count_clock; + __vfp_neon_push(); + us_ticker_calc_compare_match(); + __vfp_neon_pop(); + set_cmp_val = (uint32_t)(set_cmp_val64 & 0x00000000FFFFFFFF); count_val_64 = ticker_read_counter64(); if (set_cmp_val64 <= (count_val_64 + 500)) {