aboutsummaryrefslogtreecommitdiffstats
path: root/arch/arm/mach-imx/smp_wfe_imx6.S
diff options
context:
space:
mode:
Diffstat (limited to 'arch/arm/mach-imx/smp_wfe_imx6.S')
-rw-r--r--arch/arm/mach-imx/smp_wfe_imx6.S54
1 files changed, 54 insertions, 0 deletions
diff --git a/arch/arm/mach-imx/smp_wfe_imx6.S b/arch/arm/mach-imx/smp_wfe_imx6.S
index cdee616..7695d89 100644
--- a/arch/arm/mach-imx/smp_wfe_imx6.S
+++ b/arch/arm/mach-imx/smp_wfe_imx6.S
@@ -129,4 +129,58 @@ go_back_wfe:
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