1 files changed, 24 insertions, 2 deletions

diff --git a/rbutil/mkimxboot/dualboot/dualboot.S b/rbutil/mkimxboot/dualboot/dualboot.S
index 8302829a81..1b9edb015f 100644
--- a/rbutil/mkimxboot/dualboot/dualboot.S
+++ b/rbutil/mkimxboot/dualboot/dualboot.S
@@ -23,6 +23,7 @@
 .global start
 @ int start(uint32_t arg, uint32_t *result_id)
 start:
+    stmfd   sp!, {r4-r6,lr}
 #if defined(SANSA_FUZEPLUS)
     /* If volume down key is hold, return so that the OF can boot */
     ldr     r2, =0x80018610 @ HW_PINCTRL_DIN1
@@ -30,10 +31,31 @@ start:
     tst     r2, #0x40000000 @ bit 30, active low
     moveq   r0, #0 @ return 0, continue boot
     bxeq    lr
-    /* otherwise jump to section given as argument */
+    /* otherwise monitor the power button for a short time */
+    ldr     r2, =550000 @ loop count
+    ldr     r4, =0 @ number of times PSWITCH was 1
+pswitch_monitor_loop:
+    ldr     r3, =0x800440c0 @ HW_POWER_STS
+    ldr     r3, [r3]
+    mov     r3, r3, lsl#10
+    mov     r3, r3, lsr#30 @ extract PSWITCH
+    cmp     r3, #1
+    addeq   r4, r3 @ add one if PSWITCH=1 (means power hold)
+    subs    r2, #1
+    bne     pswitch_monitor_loop
+    /* power down if power wasn't hold long enough */
+    ldr     r2, =400000
+    cmp     r4, r2
+    bcc     power_down
+    /* jump to section given as argument */
     str     r0, [r1]
     mov     r0, #1
-    bx      lr
+    ldmfd   sp!, {r4-r6,pc}
+power_down:
+    ldr     r0, =0x80044100 @ HW_POWER_RESET
+    ldr     r1, =0x3E770001 @ unlock key + power down
+    str     r1, [r0]
+    b       power_down
 #else
 #error No target defined !
 #endif