Skip to content

Commit 89da16e

Browse files
sg-sg-
authored
Merge pull request #3127 from TomoYamanaka/master
Fixed the issue about push/pop of VFP register.
2 parents 074555b + ecff422 commit 89da16e

File tree

2 files changed

+189
-9
lines changed

2 files changed

+189
-9
lines changed

‎targets/TARGET_RENESAS/TARGET_RZ_A1H/device/vfp_neon_push_pop.h

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
#ifndef __VFP_NEON_PUSH_POP_H__
2+
#define __VFP_NEON_PUSH_POP_H__
3+
4+
5+
#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
6+
/* ARM armcc specific functions */
7+
#pragma push
8+
#pragma arm
9+
__STATIC_ASM void __vfp_neon_push(void) {
10+
ARM
11+
12+
VMRS R2,FPSCR
13+
STMDB SP!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment
14+
VSTMDB SP!,{D0-D15}
15+
VSTMDB SP!,{D16-D31}
16+
BX LR
17+
}
18+
#pragma pop
19+
20+
#pragma push
21+
#pragma arm
22+
__STATIC_ASM void __vfp_neon_pop(void) {
23+
ARM
24+
25+
VLDMIA SP!,{D16-D31}
26+
VLDMIA SP!,{D0-D15}
27+
LDR R2,[SP]
28+
VMSR FPSCR,R2
29+
ADD SP,SP,#8
30+
BX LR
31+
}
32+
#pragma pop
33+
34+
35+
#pragma push
36+
#pragma arm
37+
__STATIC_ASM void __vfp_push(void) {
38+
ARM
39+
40+
VMRS R2,FPSCR
41+
STMDB SP!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment
42+
VSTMDB SP!,{D0-D15}
43+
BX LR
44+
}
45+
#pragma pop
46+
47+
#pragma push
48+
#pragma arm
49+
__STATIC_ASM void __vfp_pop(void) {
50+
ARM
51+
52+
VLDMIA SP!,{D0-D15}
53+
LDR R2,[SP]
54+
VMSR FPSCR,R2
55+
ADD SP,SP,#8
56+
BX LR
57+
}
58+
#pragma pop
59+
60+
#elif (defined (__ICCARM__)) /*---------------- ICC Compiler ---------------------*/
61+
62+
__arm static inline void __vfp_neon_push(void) {
63+
__asm(
64+
"ARM \n"
65+
"VMRS R2,FPSCR \n"
66+
"STMDB SP!,{R2,R4} \n" // Push FPSCR, maintain 8-byte alignment
67+
"VSTMDB SP!,{D0-D15} \n"
68+
"VSTMDB SP!,{D16-D31} \n"
69+
"BX lr \n" );
70+
}
71+
72+
__arm static inline void __vfp_neon_pop(void) {
73+
__asm(
74+
"ARM \n"
75+
"VLDMIA SP!,{D16-D31} \n"
76+
"VLDMIA SP!,{D0-D15} \n"
77+
"LDR R2,[SP] \n"
78+
"VMSR FPSCR,R2 \n"
79+
"ADD SP,SP,#8 \n"
80+
"BX lr \n" );
81+
}
82+
83+
__arm static inline void __vfp_push(void) {
84+
__asm(
85+
"ARM \n"
86+
"VMRS R2,FPSCR \n"
87+
"STMDB SP!,{R2,R4} \n" // Push FPSCR, maintain 8-byte alignment
88+
"VSTMDB SP!,{D0-D15} \n"
89+
"BX lr \n" );
90+
}
91+
92+
__arm static inline void __vfp_pop(void) {
93+
__asm(
94+
"ARM \n"
95+
"VLDMIA SP!,{D0-D15} \n"
96+
"LDR R2,[SP] \n"
97+
"VMSR FPSCR,R2 \n"
98+
"ADD SP,SP,#8 \n"
99+
"BX lr \n" );
100+
}
101+
102+
#elif (defined (__GNUC__)) /*------------------ GNU Compiler ---------------------*/
103+
104+
__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_neon_push(void)
105+
{
106+
__asm__ volatile (
107+
".ARM;"
108+
109+
"VMRS R2,FPSCR;"
110+
"STMDB SP!,{R2,R4};" // Push FPSCR, maintain 8-byte alignment
111+
"VSTMDB SP!,{D0-D15};"
112+
"VSTMDB SP!,{D16-D31};"
113+
:
114+
: "i"(MODE_USR)
115+
: );
116+
return;
117+
}
118+
119+
__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_neon_pop(void)
120+
{
121+
__asm__ volatile (
122+
".ARM;"
123+
124+
"VLDMIA SP!,{D16-D31};"
125+
"VLDMIA SP!,{D0-D15};"
126+
"LDR R2,[SP];"
127+
"VMSR FPSCR,R2;"
128+
"ADD SP,SP,#8;"
129+
:
130+
: "i"(MODE_USR)
131+
: );
132+
return;
133+
}
134+
135+
__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_push(void)
136+
{
137+
__asm__ volatile (
138+
".ARM;"
139+
140+
"VMRS R2,FPSCR;"
141+
"STMDB SP!,{R2,R4};" // Push FPSCR, maintain 8-byte alignment
142+
"VSTMDB SP!,{D0-D15};"
143+
:
144+
: "i"(MODE_USR)
145+
: );
146+
return;
147+
}
148+
149+
__attribute__( ( always_inline ) ) __STATIC_INLINE void __vfp_pop(void)
150+
{
151+
__asm__ volatile (
152+
".ARM;"
153+
154+
"VLDMIA SP!,{D0-D15};"
155+
"LDR R2,[SP];"
156+
"VMSR FPSCR,R2;"
157+
"ADD SP,SP,#8;"
158+
:
159+
: "i"(MODE_USR)
160+
: );
161+
return;
162+
}
163+
164+
#endif
165+
166+
#endif

‎targets/TARGET_RENESAS/TARGET_RZ_A1H/us_ticker.c

Lines changed: 23 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020

2121
#include "RZ_A1_Init.h"
2222
#include "MBRZA1H.h"
23+
#include "vfp_neon_push_pop.h"
2324

2425
#define US_TICKER_TIMER_IRQn (OSTMI1TINT_IRQn)
2526
#define CPG_STBCR5_BIT_MSTP50 (0x01u) /* OSTM1 */
@@ -31,6 +32,8 @@ static double count_clock = 0;
3132
static uint32_t last_read = 0;
3233
static uint32_t wrap_arround = 0;
3334
static uint64_t ticker_us_last64 = 0;
35+
static uint64_t set_cmp_val64 = 0;
36+
static uint64_t timestamp64 = 0;
3437

3538
void us_ticker_interrupt(void) {
3639
us_ticker_irq_handler();
@@ -80,9 +83,15 @@ static uint64_t ticker_read_counter64(void) {
8083
return cnt_val64;
8184
}
8285

83-
uint32_t us_ticker_read() {
86+
static void us_ticker_read_last(void) {
8487
uint64_t cnt_val64;
85-
uint64_t us_val64;
88+
89+
cnt_val64 = ticker_read_counter64();
90+
91+
ticker_us_last64 = (cnt_val64 / count_clock);
92+
}
93+
94+
uint32_t us_ticker_read() {
8695
int check_irq_masked;
8796

8897
#if defined ( __ICCARM__)
@@ -91,22 +100,24 @@ uint32_t us_ticker_read() {
91100
check_irq_masked = __disable_irq();
92101
#endif /* __ICCARM__ */
93102

94-
cnt_val64 = ticker_read_counter64();
95-
us_val64 = (cnt_val64 / count_clock);
96-
ticker_us_last64 = us_val64;
103+
__vfp_neon_push();
104+
us_ticker_read_last();
105+
__vfp_neon_pop();
97106

98107
if (!check_irq_masked) {
99108
__enable_irq();
100109
}
101110

102111
/* clock to us */
103-
return (uint32_t)us_val64;
112+
return (uint32_t)ticker_us_last64;
113+
}
114+
115+
static void us_ticker_calc_compare_match(void) {
116+
set_cmp_val64 = timestamp64 * count_clock;
104117
}
105118

106119
void us_ticker_set_interrupt(timestamp_t timestamp) {
107120
// set match value
108-
uint64_t timestamp64;
109-
uint64_t set_cmp_val64;
110121
volatile uint32_t set_cmp_val;
111122
uint64_t count_val_64;
112123

@@ -118,7 +129,10 @@ void us_ticker_set_interrupt(timestamp_t timestamp) {
118129
}
119130

120131
/* calc compare mach timestamp */
121-
set_cmp_val64 = timestamp64 * count_clock;
132+
__vfp_neon_push();
133+
us_ticker_calc_compare_match();
134+
__vfp_neon_pop();
135+
122136
set_cmp_val = (uint32_t)(set_cmp_val64 & 0x00000000FFFFFFFF);
123137
count_val_64 = ticker_read_counter64();
124138
if (set_cmp_val64 <= (count_val_64 + 500)) {

0 commit comments

Comments
 (0)