diff --git a/utils/hwstub/stub/crt0.S b/utils/hwstub/stub/crt0.S
index e2d4742d36..5ba71905c9 100644
--- a/utils/hwstub/stub/crt0.S
+++ b/utils/hwstub/stub/crt0.S
@@ -3,8 +3,30 @@
 .align      0x04
 .global     start
 start:
+    sub     r7, pc, #8 /* Copy running address */
     msr     cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
+    /* Disable MMU, disable caching and buffering;
+     * use low exception range address */
+    mrc     p15, 0, r0, c1, c0, 0
+    ldr     r1, =0x3005
+    bic     r0, r1
+    mcr     p15, 0, r0, c1, c0, 0
     ldr     sp, =oc_stackend
+    /* Relocate to right address */
+    mov    r2, r7
+    ldr    r3, =_copystart
+    ldr    r4, =_copyend
+1:
+    cmp     r4, r3
+    ldrhi   r5, [r2], #4
+    strhi   r5, [r3], #4
+    bhi     1b
+
+    mov     r2, #0
+    mcr     p15, 0, r2, c7, c5, 0   @ Invalidate ICache
+    /* Jump to real location */
+    ldr     pc, =remap
+remap:
     /* clear bss */
     ldr     r2, =bss_start
     ldr     r3, =bss_end
diff --git a/utils/hwstub/stub/hwstub.lds b/utils/hwstub/stub/hwstub.lds
index 61504a3e75..549e4ab96c 100644
--- a/utils/hwstub/stub/hwstub.lds
+++ b/utils/hwstub/stub/hwstub.lds
@@ -36,11 +36,13 @@ SECTIONS
 {
     .octext :
     {
+        _copystart = .;
         oc_codestart = .;
         *(.text*)
         *(.icode*)
         *(.data*)
         *(.rodata*)
+        _copyend = .;
     } > OCRAM
 
     .bss (NOLOAD) :
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c
index c3cc429949..0923bf85ce 100644
--- a/utils/hwstub/stub/main.c
+++ b/utils/hwstub/stub/main.c
@@ -496,7 +496,7 @@ void main(uint32_t arg)
     {
         struct usb_ctrlrequest req;
         usb_drv_recv_setup(&req);
-        
+
         switch(req.bRequestType & USB_TYPE_MASK)
         {
             case USB_TYPE_STANDARD: