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)) {