1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
|
/*
* Copyright (C) 2015 Freescale Semiconductor, Inc. All Rights Reserved.
*
* 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.
*/
#include <linux/linkage.h>
#include <asm/smp_scu.h>
#include "hardware.h"
#ifdef CONFIG_SMP
.extern imx_scu_base
#endif
.globl wfe_smp_freq_change_start
.globl wfe_smp_freq_change_end
#ifdef CONFIG_SMP
.align 3
.macro disable_l1_dcache
/*
* Flush all data from the L1 data cache before disabling
* SCTLR.C bit.
*/
push {r0 - r11, lr}
ldr r7, =v7_flush_kern_cache_all
mov lr, pc
mov pc, r7
pop {r0 - r11, lr}
/* disable d-cache */
mrc p15, 0, r6, c1, c0, 0
bic r6, r6, #0x4
mcr p15, 0, r6, c1, c0, 0
dsb
isb
push {r0 - r11, lr}
ldr r7, =v7_flush_kern_cache_all
mov lr, pc
mov pc, r7
pop {r0 - r11, lr}
.endm
ENTRY(wfe_smp_freq_change)
wfe_smp_freq_change_start:
push {r4 - r11, lr}
mov r6, r0
mov r7, r1
dsb
isb
disable_l1_dcache
isb
/* Turn off SMP bit. */
mrc p15, 0, r8, c1, c0, 1
bic r8, r8, #0x40
mcr p15, 0, r8, c1, c0, 1
isb
/* Inform the SCU we are going to enter WFE. */
push {r0 - r11, lr}
ldr r0,=imx_scu_base
ldr r0, [r0]
mov r1, #SCU_PM_DORMANT
ldr r3, =scu_power_mode
mov lr, pc
mov pc, r3
pop {r0 - r11, lr}
go_back_wfe:
wfe
ldr r3, [r7]
cmp r3, #1
beq go_back_wfe
/* Turn ON SMP bit. */
mrc p15, 0, r8, c1, c0, 1
orr r8, r8, #0x40
mcr p15, 0, r8, c1, c0, 1
isb
/* Enable L1 data cache. */
mrc p15, 0, r8, c1, c0, 0
orr r8, r8, #0x4
mcr p15, 0, r8, c1, c0, 0
isb
/* Inform the SCU we have exited WFE. */
push {r0 - r11, lr}
ldr r0,=imx_scu_base
ldr r0, [r0]
mov r1, #SCU_PM_NORMAL
ldr r3, =scu_power_mode
mov lr, pc
mov pc, r3
pop {r0 - r11, lr}
/* Pop all saved registers. */
pop {r4 - r11, lr}
mov pc, lr
.ltorg
wfe_smp_freq_change_end:
ENDPROC(wfe_smp_freq_change)
#ifdef CONFIG_OPTEE
/**
* @brief Switch CPU in WFE mode while bus frequency change
* on-going
*
* @param[in] r0 CPU in WFE Status
* @param[in] r1 Bus frequency change status
*/
.globl imx_smp_wfe_optee_end
ENTRY(imx_smp_wfe_optee)
push {r4-r11, lr}
dsb
isb
disable_l1_dcache
isb
/* Set flag CPU entering WFE. */
mov r4, #1
str r4, [r0]
dsb
isb
1:
wfe
/* Check if busfreq is done, else loop */
ldr r4, [r1]
cmp r4, #1
beq 1b
/* Enable L1 data cache. */
mrc p15, 0, r4, c1, c0, 0
orr r4, r4, #0x4
mcr p15, 0, r4, c1, c0, 0
isb
/* Set flag CPU exiting WFE. */
mov r4, #0
str r4, [r0]
/* Pop all saved registers. */
pop {r4-r11, lr}
mov pc, lr
.ltorg
imx_smp_wfe_optee_end:
ENDPROC(imx_smp_wfe_optee)
#endif
#endif
|
