4 files changed, 45 insertions, 1 deletions
diff --git a/utils/hwstub/stub/asm/mips/system.S b/utils/hwstub/stub/asm/mips/system.S
index ab134aed48..97b0207ec9 100644
--- a/utils/hwstub/stub/asm/mips/system.S
+++ b/utils/hwstub/stub/asm/mips/system.S
@@ -18,6 +18,7 @@
  *
  ****************************************************************************/
 #include "mips.h"
+#include "target-config.h"
 
 /* Handling of data abort:
  * the code can register a "longjmp" buffer to restore the context in case of
@@ -49,3 +50,27 @@ set_data_abort_jmp:
     jr ra
     move v0, zero
 .set reorder
+
+#ifdef CONFIG_FLUSH_CACHES
+.set noreorder
+.text
+.global target_flush_caches
+target_flush_caches:
+    /* commit dcache and invalidate icache */
+    la      t0, 0x80000000   /* an idx op should use an unmappable address */
+    ori     t1, t0, DCACHE_SIZE   /* cache size */
+reloc_dcache_loop:
+    cache   DCIndexWBInv, 0(t0) /* invalidate and write-back dcache index */
+    addiu   t0, t0, DCACHE_LINE_SIZE /* bytes per cache line */
+    bne     t0, t1, reloc_dcache_loop
+    nop
+    la      t0, 0x80000000   /* an idx op should use an unmappable address */
+    ori     t1, t0, ICACHE_SIZE   /* cache size */
+reloc_icache_loop:
+    cache   ICIndexInv, 0(t0) /* invalidate icache index */
+    addiu   t0, t0, ICACHE_LINE_SIZE  /* bytes per cache line */
+    bne     t0, t1, reloc_icache_loop
+    nop
+    jr      ra
+    nop
+#endif
diff --git a/utils/hwstub/stub/jz4760b/target-config.h b/utils/hwstub/stub/jz4760b/target-config.h
index fa018c14dc..681e17e6f6 100644
--- a/utils/hwstub/stub/jz4760b/target-config.h
+++ b/utils/hwstub/stub/jz4760b/target-config.h
@@ -3,6 +3,12 @@
 #define TCSM0_SIZE      0x4000
 #define CPU_MIPS
 #define STACK_SIZE      0x300
+#define DCACHE_SIZE     0x4000 /* 16 kB */
+#define DCACHE_LINE_SIZE    0x20   /* 32 B */
+#define ICACHE_SIZE     0x4000 /* 16 kB */
+#define ICACHE_LINE_SIZE    0x20   /* 32 B */
+/* we need to flush caches before executing */
+#define CONFIG_FLUSH_CACHES
 
 /* something provides define
  * #define mips 1
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c
index ee93ad4b63..c35872f320 100644
--- a/utils/hwstub/stub/main.c
+++ b/utils/hwstub/stub/main.c
@@ -518,10 +518,17 @@ static void handle_exec(struct usb_ctrlrequest *req)
     if(size != sizeof(struct hwstub_exec_req_t))
         return usb_drv_stall(EP_CONTROL, true, true);
     uint32_t addr = exec->dAddress;
+
+#if defined(CPU_ARM)
     if(exec->bmFlags & HWSTUB_EXEC_THUMB)
         addr |= 1;
     else
         addr &= ~1;
+#endif
+
+#ifdef CONFIG_FLUSH_CACHES
+    target_flush_caches();
+#endif
 
     if(exec->bmFlags & HWSTUB_EXEC_CALL)
     {
@@ -540,11 +547,13 @@ static void handle_exec(struct usb_ctrlrequest *req)
     else
     {
         /* in case of jump, respond immediately and disconnect usb */
+#if defined(CPU_ARM)
         usb_drv_send(EP_CONTROL, NULL, 0);
         usb_drv_exit();
-#if defined(CPU_ARM)
         asm volatile("bx %0\n" : : "r" (addr) : "memory");
 #elif defined(CPU_MIPS)
+        usb_drv_send(EP_CONTROL, NULL, 0);
+        usb_drv_exit();
         asm volatile("jr %0\nnop\n" : : "r" (addr) : "memory");
 #else 
 #warning jump is unsupported on this platform
diff --git a/utils/hwstub/stub/target.h b/utils/hwstub/stub/target.h
index 5cd049d04f..4992dd5bf8 100644
--- a/utils/hwstub/stub/target.h
+++ b/utils/hwstub/stub/target.h
@@ -41,6 +41,10 @@ uint32_t target_read32(const void *addr);
 void target_write8(void *addr, uint8_t val);
 void target_write16(void *addr, uint16_t val);
 void target_write32(void *addr, uint32_t val);
+#ifdef CONFIG_FLUSH_CACHES
+/* flush cache: commit dcache and invalidate icache */
+void target_flush_caches(void);
+#endif
 
 /* mandatory for all targets */
 extern struct hwstub_target_desc_t target_descriptor;