diff --git a/bootloader/rk27xx.c b/bootloader/rk27xx.c
index dfba970..44f0724 100644
--- a/bootloader/rk27xx.c
+++ b/bootloader/rk27xx.c
@@ -24,6 +24,45 @@
extern void show_logo( void );
+static void enter_dfu(void)
+{
+ asm volatile (
+ /* turn off cache */
+ "ldr r0, =0xefff0000 \n"
+ "ldrh r1, [r0] \n"
+ "strh r1, [r0] \n"
+
+ /* turn off interrupts */
+ "mrs r0, cpsr \n"
+ "bic r0, r0, #0x1f \n"
+ "orr r0, r0, #0xd3 \n"
+ "msr cpsr, r0 \n"
+
+ /* disable iram remap */
+ "mov r0, #0x18000000 \n"
+ "add r0, r0, #0x1c000 \n"
+ "mov r1, #0 \n"
+ "str r1, [r0, #4] \n"
+
+ /* setup stacks in unmapped
+ * iram just as rom will do
+ */
+ "msr cpsr, #0xd2 \n"
+ "ldr r1, =0x18200274 \n"
+ "add r1, r1, #0x200 \n"
+ "mov sp, r1 \n"
+ "msr cpsr, #0xd3 \n"
+ "add r1, r1, #0x400 \n"
+ "mov sp, r1 \n"
+
+ /* jump to main() in rom
+ * just before dfu handler
+ */
+ "ldr r0, =0xec0 \n"
+ "bx r0 \n"
+ );
+}
+
void main(void) NORETURN_ATTR;
void main(void)
{
@@ -102,8 +141,9 @@ void main(void)
kernel_entry();
printf("ERR: Failed to boot");
+ printf("Entering rockchip DFU mode");
sleep(5*HZ);
- power_off();
+ enter_dfu();
/* hang */
while(1);