3 files changed, 13 insertions, 1 deletions

diff --git a/firmware/target/arm/imx233/clkctrl-imx233.h b/firmware/target/arm/imx233/clkctrl-imx233.h
index fc835c813a..8a52620f7e 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.h
+++ b/firmware/target/arm/imx233/clkctrl-imx233.h
@@ -98,6 +98,17 @@
 #define HW_CLKCTRL_RESET_CHIP   0x2
 #define HW_CLKCTRL_RESET_DIG    0x1
 
+static inline void core_sleep(void)
+{
+    __REG_SET(HW_CLKCTRL_CPU) = HW_CLKCTRL_CPU__INTERRUPT_WAIT;
+    asm volatile (
+        "mcr p15, 0, %0, c7, c0, 4 \n" /* Wait for interrupt */
+        "nop\n" /* Datasheet unclear: "The lr sent to handler points here after RTI"*/
+        : : "r"(0)
+    );
+    enable_irq();
+}
+
 enum imx233_clock_t
 {
     CLK_PIX, /* freq, div, frac, bypass, enable */
diff --git a/firmware/target/arm/imx233/system-target.h b/firmware/target/arm/imx233/system-target.h
index 87063510ad..90a2966f2f 100644
--- a/firmware/target/arm/imx233/system-target.h
+++ b/firmware/target/arm/imx233/system-target.h
@@ -26,6 +26,7 @@
 #include "panic.h"
 
 #include "clock-target.h" /* CPUFREQ_* are defined here */
+#include "clkctrl-imx233.h"
 #include "power-imx233.h"
 
 /* Digital control */
diff --git a/firmware/target/arm/thread-arm.c b/firmware/target/arm/thread-arm.c
index 0b46ca2558..88dac2542a 100644
--- a/firmware/target/arm/thread-arm.c
+++ b/firmware/target/arm/thread-arm.c
@@ -109,7 +109,7 @@ static inline void core_sleep(void)
 }
 #else
 /* Skip this if special code is required and implemented */
-#if !(defined(CPU_PP)) && CONFIG_CPU != RK27XX
+#if !(defined(CPU_PP)) && CONFIG_CPU != RK27XX && CONFIG_CPU != IMX233
 static inline void core_sleep(void)
 {
     /* TODO: core_sleep not implemented, battery life will be decreased */