1 files changed, 16 insertions, 2 deletions

diff --git a/firmware/target/arm/tcc780x/cowond2/power-cowond2.c b/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
index d8a58570e4..2439676acc 100644
--- a/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
+++ b/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
@@ -16,9 +16,7 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
-#include "config.h"
 #include "cpu.h"
-#include <stdbool.h>
 #include "kernel.h"
 #include "system.h"
 #include "power.h"
@@ -61,6 +59,22 @@ void charger_enable(bool on)
 
 void power_off(void)
 {
+    /* Disable interrupts on this core */
+    set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS);
+
+    /* Mask them on both cores */
+    CPU_INT_CLR = -1;
+    COP_INT_CLR = -1;
+
+    /* Shutdown: stop XIN oscillator */
+    CLKCTRL &= ~(1 << 31);
+
+    /* Halt everything and wait for device to power off */
+    while (1)
+    {
+        CPU_CTL = PROC_SLEEP;
+        COP_CTL = PROC_SLEEP;
+    }
 }
 
 void ide_power_enable(bool on)