9 files changed, 212 insertions, 228 deletions

diff --git a/firmware/drivers/rtc/rtc_jz4740.c b/firmware/drivers/rtc/rtc_jz4740.c
index dadc93a8e0..a1a864d27a 100644
--- a/firmware/drivers/rtc/rtc_jz4740.c
+++ b/firmware/drivers/rtc/rtc_jz4740.c
@@ -52,6 +52,7 @@ static const unsigned int sum_monthday[13] = {
     365
 };
 
+#if 0
 static unsigned int jz_mktime(int year, int mon, int day, int hour, int min, int sec)
 {
     unsigned int seccounter;
@@ -78,6 +79,7 @@ static unsigned int jz_mktime(int year, int mon, int day, int hour, int min, int
 
     return seccounter;
 }
+#endif
 
 static void jz_gettime(unsigned int rtc, int *year, int *mon, int *day, int *hour,
          int *min, int *sec, int *weekday)
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index ecba179693..c4dfbf4ed3 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -112,10 +112,10 @@ struct regs
 #elif defined(CPU_MIPS)
 struct regs
 {
-    uint32_t r[27]; /* 0-104 - Registers $1, v0-v1, a0-a3, t0-t9, s0-s7, gp, fp */
-    uint32_t sp;    /*   108 - Stack pointer */
-    uint32_t ra;    /*   112 - Return address */
-    uint32_t start; /*   116 - Thread start address, or NULL when started */
+    uint32_t r[10]; /* 0-36 - Registers s0-s7, gp, fp */
+    uint32_t sp;    /*   40 - Stack pointer */
+    uint32_t ra;    /*   44 - Return address */
+    uint32_t start; /*   48 - Thread start address, or NULL when started */
 };
 #endif /* CONFIG_CPU */
 #else
diff --git a/firmware/target/mips/ingenic_jz47xx/crt0.S b/firmware/target/mips/ingenic_jz47xx/crt0.S
index 97e01d8566..5cfd49141a 100644
--- a/firmware/target/mips/ingenic_jz47xx/crt0.S
+++ b/firmware/target/mips/ingenic_jz47xx/crt0.S
@@ -66,17 +66,16 @@ _start:
 
     li      t0, (M_StatusBEV  | M_StatusIM7 | M_StatusIM6 \
                 | M_StatusIM5 | M_StatusIM4 | M_StatusIM3 \
-                | M_StatusIM2 | M_StatusERL | M_StatusSM)
+                | M_StatusIM2 | M_StatusERL)
              /*
                  BEV = Enable Boot Exception Vectors
                  IMx = Interrupt mask
                  ERL = Denotes error level
-                 SM  = Supervisor Mode
              */
     mtc0    t0, C0_STATUS
 
-    li      t1, M_CauseIV
-    mtc0    t1, C0_CAUSE
+    li      t0, M_CauseIV
+    mtc0    t0, C0_CAUSE
 
     /*
       ----------------------------------------------------
@@ -85,7 +84,7 @@ _start:
     */
     li      t0, 3            // enable cache for kseg0 accesses
     mtc0    t0, C0_CONFIG    // CONFIG reg
-    la      t0, 0x80000000   // an idx op should use a unmappable address
+    la      t0, 0x80000000   // an idx op should use an unmappable address
     ori     t1, t0, 0x4000   // 16kB cache
     mtc0    zero, C0_TAGLO   // TAGLO reg
     mtc0    zero, C0_TAGHI   // TAGHI reg
@@ -215,25 +214,26 @@ real_exception_handler:
     sw      v1, 0x64(sp)
     sw      v0, 0x68(sp)
     sw      $1, 0x6C(sp)
-    mflo    t0             # Move From LO
+    mflo    k0             # Move From LO
     nop
-    sw      t0, 0x70(sp)
-    mfhi    t0             # Move From HI
+    sw      k0, 0x70(sp)
+    mfhi    k0             # Move From HI
     nop
-    sw      t0, 0x74(sp)
-    mfc0    t0, C0_STATUS  # Status register
+    sw      k0, 0x74(sp)
+    mfc0    k0, C0_STATUS  # Status register
     sll     zero, 1
     sll     zero, 1
     sll     zero, 1
     sll     zero, 1
-    sw      t0, 0x78(sp)
-    mfc0    t0, C0_EPC     # Exception Program Counter
+    sw      k0, 0x78(sp)
+    mfc0    k0, C0_EPC     # Exception Program Counter
     sll     zero, 1
     sll     zero, 1
     sll     zero, 1
     sll     zero, 1
-    sw      t0, 0x7C(sp)
-    li      k1, 0x7C
+    sw      k0, 0x7C(sp)
+    
+    li      k1, M_CauseExcCode
     mfc0    k0, C0_CAUSE   # C0_CAUSE of last exception
     and     k0, k1
     beq     zero, k0, _int
@@ -273,7 +273,7 @@ _int:
     lw      a0, 0x60(sp)
     lw      v1, 0x64(sp)
     lw      v0, 0x68(sp)
-    lw      v1, 0x6C(sp)
+    lw      $1, 0x6C(sp)
     lw      k0, 0x70(sp)
     mtlo    k0             # Move To LO
     nop
@@ -313,7 +313,57 @@ _exception:
     sll     zero, 1
     sll     zero, 1
     sll     zero, 1
-    la      k0, exception_handler
-    jr      k0
+    jal     exception_handler
+    nop
+    lw      ra, 0(sp)
+    lw      fp, 4(sp)
+    sw      gp, 8(sp)
+    lw      t9, 0xC(sp)
+    lw      t8, 0x10(sp)
+    lw      s7, 0x14(sp)
+    lw      s6, 0x18(sp)
+    lw      s5, 0x1C(sp)
+    lw      s4, 0x20(sp)
+    lw      s3, 0x24(sp)
+    lw      s2, 0x28(sp)
+    lw      s1, 0x2C(sp)
+    lw      s0, 0x30(sp)
+    lw      t7, 0x34(sp)
+    lw      t6, 0x38(sp)
+    lw      t5, 0x3C(sp)
+    lw      t4, 0x40(sp)
+    lw      t3, 0x44(sp)
+    lw      t2, 0x48(sp)
+    lw      t1, 0x4C(sp)
+    lw      t0, 0x50(sp)
+    lw      a3, 0x54(sp)
+    lw      a2, 0x58(sp)
+    lw      a1, 0x5C(sp)
+    lw      a0, 0x60(sp)
+    lw      v1, 0x64(sp)
+    lw      v0, 0x68(sp)
+    lw      $1, 0x6C(sp)
+    lw      k0, 0x70(sp)
+    mtlo    k0             # Move To LO
+    nop
+    lw      k0, 0x74(sp)
+    mthi    k0             # Move To HI
+    nop
+    lw      k0, 0x78(sp)
+    nop
+    mtc0    k0, C0_STATUS  # Status register
+    sll     zero, 1
+    sll     zero, 1
+    sll     zero, 1
+    sll     zero, 1
+    lw      k0, 0x7C(sp)
+    nop
+    mtc0    k0, C0_EPC     # Exception Program Counter
+    sll     zero, 1
+    sll     zero, 1
+    sll     zero, 1
+    sll     zero, 1
+    addiu   sp, 0x80
+    eret                   # Exception Return
     nop
     .set reorder
diff --git a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
index 6bb7db8d9b..90ecfc6a3a 100644
--- a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
@@ -68,7 +68,7 @@ void lcd_update_rect(int x, int y, int width, int height)
                        | DMAC_DCMD_DWDH_16 | DMAC_DCMD_DS_16BIT);             /* | (2 << 12) | (3 << 8) */
     REG_DMAC_DCCSR(0) = (DMAC_DCCSR_NDES | DMAC_DCCSR_EN);                     /* (1 << 31) | (1 << 0) */
     
-    dma_cache_wback_inv((unsigned long)&lcd_framebuffer[y][x], width*height);
+    __dcache_writeback_all(); /* Size of framebuffer is way bigger than cache size */
     
     REG_DMAC_DMACR = DMAC_DMACR_DMAE;
 
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-onda_vx747.c b/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-onda_vx747.c
index 98fa8e7a20..3846987026 100644
--- a/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-onda_vx747.c
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-onda_vx747.c
@@ -24,14 +24,14 @@
 #include "r61509.h"
 #include "lcd-target.h"
 
-#define PIN_CS_N        (32*1+17) /* Chip select */
+#define PIN_CS_N    (32*1+17) /* Chip select */
 #define PIN_RESET_N (32*1+18) /* Reset */
 
-#define my__gpio_as_lcd_16bit()                 \
-do {                                            \
-        REG_GPIO_PXFUNS(2) = 0x001cffff;        \
-        REG_GPIO_PXSELC(2) = 0x001cffff;        \
-        REG_GPIO_PXPES(2) = 0x001cffff;         \
+#define my__gpio_as_lcd_16bit()         \
+do {                                    \
+    REG_GPIO_PXFUNS(2) = 0x001cffff;    \
+    REG_GPIO_PXSELC(2) = 0x001cffff;    \
+    REG_GPIO_PXPES(2) = 0x001cffff;     \
 } while (0)
 
 
@@ -45,12 +45,12 @@ static void _display_pin_init(void)
     __gpio_as_output(PIN_RESET_N);
     __gpio_clear_pin(PIN_CS_N);
     
-        __gpio_set_pin(PIN_RESET_N);
-        DELAY;          
-        __gpio_clear_pin(PIN_RESET_N);
-        DELAY;
-        __gpio_set_pin(PIN_RESET_N);
-        DELAY;
+    __gpio_set_pin(PIN_RESET_N);
+    DELAY;        
+    __gpio_clear_pin(PIN_RESET_N);
+    DELAY;
+    __gpio_set_pin(PIN_RESET_N);
+    DELAY;
 }
 
 #define WAIT_ON_SLCD while(REG_SLCD_STATE & SLCD_STATE_BUSY);
@@ -189,7 +189,7 @@ static void _display_off(void)
 static void _set_lcd_bus(void)
 {
     REG_LCD_CFG &= ~LCD_CFG_LCDPIN_MASK;
-        REG_LCD_CFG |= LCD_CFG_LCDPIN_SLCD;
+    REG_LCD_CFG |= LCD_CFG_LCDPIN_SLCD;
     
     REG_SLCD_CFG = (SLCD_CFG_BURST_8_WORD | SLCD_CFG_DWIDTH_16 | SLCD_CFG_CWIDTH_16BIT
                    | SLCD_CFG_CS_ACTIVE_LOW | SLCD_CFG_RS_CMD_LOW | SLCD_CFG_CLK_ACTIVE_FALLING
@@ -200,17 +200,17 @@ static void _set_lcd_bus(void)
 
 static void _set_lcd_clock(void)
 {
-        unsigned int val;
-        int pll_div;
+    unsigned int val;
+    int pll_div;
     
     __cpm_stop_lcd();
-        pll_div = ( REG_CPM_CPCCR & CPM_CPCCR_PCS ); /* clock source, 0:pllout/2 1: pllout */
-        pll_div = pll_div ? 1 : 2 ;
-        val = ( __cpm_get_pllout()/pll_div ) / 336000000;
-        val--;
-        if ( val > 0x1ff )
-                val = 0x1ff; /* CPM_LPCDR is too large, set it to 0x1ff */
-        __cpm_set_pixdiv(val);
+    pll_div = ( REG_CPM_CPCCR & CPM_CPCCR_PCS ); /* clock source, 0:pllout/2 1: pllout */
+    pll_div = pll_div ? 1 : 2 ;
+    val = ( __cpm_get_pllout()/pll_div ) / 336000000;
+    val--;
+    if ( val > 0x1ff )
+        val = 0x1ff; /* CPM_LPCDR is too large, set it to 0x1ff */
+    __cpm_set_pixdiv(val);
     __cpm_start_lcd();
 }
 
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx747/sadc-onda_vx747.c b/firmware/target/mips/ingenic_jz47xx/onda_vx747/sadc-onda_vx747.c
index 0f11782d3f..dd169c3a44 100644
--- a/firmware/target/mips/ingenic_jz47xx/onda_vx747/sadc-onda_vx747.c
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx747/sadc-onda_vx747.c
@@ -122,13 +122,16 @@ int button_read_device(int *data)
             ret |= BUTTON_POWER;
     }
     
-    if(pendown_flag)
+    if(data != NULL)
     {
-        *data = touch_to_pixels(stable_x_pos, stable_y_pos);
-        ret |= BUTTON_TOUCH;
+        if(pendown_flag)
+        {
+            *data = touch_to_pixels(stable_x_pos, stable_y_pos);
+            ret |= BUTTON_TOUCH;
+        }
+        else
+            *data = 0;
     }
-    else
-        *data = 0;
 
     return ret;
 }
@@ -199,7 +202,7 @@ void SADC(void)
         
         xData = (dat >>  0) & 0xfff;
         yData = (dat >> 16) & 0xfff;
-            
+        
         dat = REG_SADC_TSDAT;
         tsz1Data = (dat >>  0) & 0xfff;
         tsz2Data = (dat >> 16) & 0xfff;
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
index 61be6c60de..1a41723ffc 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
@@ -324,7 +324,7 @@ static int get_irq_number(void)
 
 void intr_handler(void)
 {
-    irq = get_irq_number();
+    int irq = get_irq_number();
     if(irq < 0)
         return;
     
@@ -361,7 +361,7 @@ static char* parse_exception(unsigned int cause)
 }
 
 void exception_handler(void* stack_ptr, unsigned int cause, unsigned int epc)
-{
+{  
     panicf("Exception occurred: %s [0x%08x] at 0x%08x (stack at 0x%08x)", parse_exception(cause), cause, epc, (unsigned int)stack_ptr);
 }
 
@@ -457,23 +457,6 @@ void __icache_invalidate_all(void)
 {
     unsigned int i;
 
-/*
-    do 
-    {
-        unsigned long __k0_addr;
-        
-        __asm__ __volatile__(
-                             "la    %0, 1f      \n"
-                             "or    %0, %0, %1  \n"
-                             "jr    %0          \n"
-                             "nop               \n"
-                             "1: nop            \n"
-                             : "=&r"(__k0_addr)
-                             : "r" (0x20000000)
-                             );
-    } while(0);
-*/
-
     asm volatile (".set   noreorder  \n"
                   ".set   mips32     \n"
                   "mtc0   $0, $28    \n" /* TagLo */
@@ -484,34 +467,16 @@ void __icache_invalidate_all(void)
     for(i=KSEG0; i<KSEG0+CACHE_SIZE; i+=CACHE_LINE_SIZE)
         __CACHE_OP(Index_Store_Tag_I, i);
 
-/*
-    do
-    {
-        unsigned long __k0_addr;
-        __asm__ __volatile__(
-                             "nop;nop;nop;nop;nop;nop;nop  \n"
-                             "la    %0, 1f                 \n"
-                             "jr    %0                     \n"
-                             "nop                          \n"
-                             "1:    nop                    \n"
-                             : "=&r" (__k0_addr)
-                             );
-    } while(0);
-*/
-
-    do
-    {
-        unsigned long tmp;
-        __asm__ __volatile__(
-        ".set mips32       \n"
-        "mfc0 %0, $16, 7   \n" /* Config */
-        "nop               \n"
-        "ori  %0, 2        \n"
-        "mtc0 %0, $16, 7   \n" /* Config */
-        "nop               \n"
-        ".set mips0        \n"
-        : "=&r" (tmp));
-    } while(0);
+    /* invalidate btb */
+    asm volatile (
+        ".set mips32        \n"
+        "mfc0 %0, $16, 7    \n"
+        "nop                \n"
+        "ori  %0, 2         \n"
+        "mtc0 %0, $16, 7    \n"
+        ".set mips0         \n"
+        :
+        : "r" (i));
 }
 
 void __dcache_invalidate_all(void)
@@ -657,13 +622,14 @@ static void tlb_init(void)
 
 void tlb_refill_handler(void)
 {
-    panicf("TLB refill handler! [0x%x] [0x%lx]", read_c0_badvaddr(), read_c0_epc());
+    panicf("TLB refill handler at 0x%08lx! [0x%x]", read_c0_epc(), read_c0_badvaddr());
 }
 
 static void tlb_call_refill(void)
 {
     asm("la $8, tlb_refill_handler \n"
-        "jr $8                     \n");
+        "jr $8                     \n"
+       );
 }
 
 extern int main(void);
@@ -687,18 +653,18 @@ void system_main(void)
     __dcache_writeback_all();
     __icache_invalidate_all();
     
-    write_c0_status(1 << 28 | 1 << 10 | 1 << 3); /* Enable CP | Mask interrupt 2 | Supervisor mode */
+    write_c0_status(1 << 28 | 1 << 10 ); /* Enable CP | Mask interrupt 2 */
     
     /* Disable all interrupts */
     for(i=0; i<IRQ_MAX; i++)
         dis_irq(i);
     
-    //tlb_init();
-    
-    sti();
+    tlb_init();
     
     detect_clock();
     
+    sti();
+    
     main();
     
     while(1);
diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
index 92a2f3b801..cf107397e3 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
@@ -824,25 +824,21 @@ void usbHandleStandDevReq(u8 *buf)
 
 extern char printfbuf[256];
 
-int GET_CUP_INFO_Handle()
-{
-        HW_SendPKT(0, printfbuf, 64);
-        udc_state = IDLE;
-        return 0;
-}
-
 void usbHandleVendorReq(u8 *buf)
 {
-        int ret_state;
+        int ret_state, i;
         USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
-        switch (dreq->bRequest) {
-        case 0xAB:
-                ret_state=GET_CUP_INFO_Handle();
-                break;
-    case 0x12:
-        HW_SendPKT(0, "TEST", 4);
-        udc_state = IDLE;
-        break;
+        switch (dreq->bRequest)
+    {
+        case 0xAB:
+            //for(i=0; i<256; i+=64)
+                HW_SendPKT(0, printfbuf, 64);
+                udc_state = IDLE;
+                break;
+        case 0x12:
+                HW_SendPKT(0, "TEST", 4);
+                udc_state = IDLE;
+            break;
         }
 }
 
@@ -1032,6 +1028,9 @@ void __udc_start(void)
     system_enable_irq(IRQ_UDC);
 }
 
-void usb_init_device(void){}
+void usb_init_device(void)
+{
+    __udc_start();
+}
 
 #endif
diff --git a/firmware/thread.c b/firmware/thread.c
index f70eb5af7f..58d1b39d74 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -991,34 +991,31 @@ static inline void core_sleep(void)
  * Start the thread running and terminate it if it returns
  *---------------------------------------------------------------------------
  */
+
 void start_thread(void); /* Provide C access to ASM label */
-#if 0
-static void __attribute__((used)) __start_thread(void)
+static void __attribute__((used)) _start_thread(void)
 {
 
-    /* $v0 = context */
+    /* $t1 = context */
     asm volatile (
-        ".set noreorder        \n"
-    "_start_thread:            \n" /* Start here - no naked attribute */
-        "lw     $8,   (4)$2    \n" /* Fetch thread function pointer ($8 = $t0, $2 = $v0) */
-        "lw     $29,   (108)$2 \n" /* Set initial sp(=$29) */
-        "jalr   $8             \n" /* Start the thread  ($8 = $t0,)*/
-        "sw     $0,    (116)$2 \n" /* Clear start address ($2 = $v0) */
-        ".set reorder          \n"
+      "start_thread:          \n"
+        ".set noreorder       \n"
+        ".set noat            \n"
+        "lw     $8,    4($9)  \n" /* Fetch thread function pointer ($8 = $t0, $9 = $t1) */
+        "lw     $29,  40($9)  \n" /* Set initial sp(=$29) */
+        "sw     $0,   48($9)  \n" /* Clear start address */
+        "jalr   $8            \n" /* Start the thread */
+        "nop                  \n"
+        ".set at              \n"
+        ".set reorder         \n"
     );
     thread_exit();
 
 }
-#else
-void start_thread(void)
-{
-    return;
-}
-#endif
 
-/* Place context pointer in $v0 slot, function pointer in $v1 slot, and
+/* Place context pointer in $s0 slot, function pointer in $s1 slot, and
  * start_thread pointer in context_start */
-#define THREAD_STARTUP_INIT(core, thread, function) \
+#define THREAD_STARTUP_INIT(core, thread, function)           \
     ({ (thread)->context.r[0] = (uint32_t)&(thread)->context, \
        (thread)->context.r[1] = (uint32_t)(function),         \
        (thread)->context.start = (uint32_t)start_thread; })
@@ -1029,43 +1026,26 @@ void start_thread(void)
  */
 static inline void store_context(void* addr)
 {
-#if 0
     asm volatile (
-        ".set noreorder       \n"
-        ".set noat            \n"
-        "sw    $1, (0)%0      \n"
-        "sw    $2,(4)%0       \n" /* $v0 */
-        "sw    $3,(8)%0       \n" /* $v1 */
-        "sw    $4,(12)%0      \n" /* $a0 */
-        "sw    $5,(16)%0      \n" /* $a1 */
-        "sw    $6,(20)%0      \n" /* $a2 */
-        "sw    $7,(24)%0      \n" /* $a3 */
-        "sw    $8,(28)%0      \n" /* $t0 */
-        "sw    $9,(32)%0      \n" /* $t1 */
-        "sw    $10,(36)%0     \n" /* $t2 */
-        "sw    $11,(40)%0     \n" /* $t3 */
-        "sw    $12,(44)%0     \n" /* $t4 */
-        "sw    $13,(48)%0     \n" /* $t5 */
-        "sw    $14,(52)%0     \n" /* $t6 */
-        "sw    $15,(56)%0     \n" /* $t7 */
-        "sw    $24,(60)%0     \n" /* $t8 */
-        "sw    $25,(64)%0     \n" /* $t9 */
-        "sw    $16,(68)%0     \n" /* $s0 */
-        "sw    $17,(72)%0     \n" /* $s1 */
-        "sw    $18,(76)%0     \n" /* $s2 */
-        "sw    $19,(80)%0     \n" /* $s3 */
-        "sw    $20,(84)%0     \n" /* $s4 */
-        "sw    $21,(88)%0     \n" /* $s5 */
-        "sw    $22,(92)%0     \n" /* $s6 */
-        "sw    $23,(96)%0     \n" /* $s7 */
-        "sw    $28,(100)%0    \n" /* gp */
-        "sw    $30,(104)%0    \n" /* fp */
-        "sw    $29,(108)%0    \n" /* sp */
-        "sw    $31,(112)%0    \n" /* ra */
-        ".set reorder         \n"
-        : : "r" (addr)
+        ".set noreorder        \n"
+        ".set noat             \n"
+        "move  $8, %0          \n"
+        "sw    $16,  0($8)     \n" /* $s0 */
+        "sw    $17,  4($8)     \n" /* $s1 */
+        "sw    $18,  8($8)     \n" /* $s2 */
+        "sw    $19, 12($8)     \n" /* $s3 */
+        "sw    $20, 16($8)     \n" /* $s4 */
+        "sw    $21, 20($8)     \n" /* $s5 */
+        "sw    $22, 24($8)     \n" /* $s6 */
+        "sw    $23, 28($8)     \n" /* $s7 */
+        "sw    $28, 32($8)     \n" /* gp */
+        "sw    $30, 36($8)     \n" /* fp */
+        "sw    $29, 40($8)     \n" /* sp */
+        "sw    $31, 44($8)     \n" /* ra */
+        ".set at               \n"
+        ".set reorder          \n"
+        : : "r" (addr) : "t0"
     );
-#endif
 }
 
 /*---------------------------------------------------------------------------
@@ -1074,48 +1054,33 @@ static inline void store_context(void* addr)
  */
 static inline void load_context(const void* addr)
 {
-#if 0
     asm volatile (
-        ".set noat            \n"
-        ".set noreorder       \n"
-        "lw     $8, 116(%0)   \n" /* Get start address ($8 = $t0) */
-        //"tst    r0, r0        \n"
-        "j      .running      \n" /* NULL -> already running */
-        "jr     $8            \n" /* $t0 = $8 = context */
-    ".running:                \n"
-        "lw    $1, (0)%0      \n"
-        "lw    $2,(4)%0       \n" /* $v0 */
-        "lw    $3,(8)%0       \n" /* $v1 */
-        "lw    $4,(12)%0      \n" /* $a0 */
-        "lw    $5,(16)%0      \n" /* $a1 */
-        "lw    $6,(20)%0      \n" /* $a2 */
-        "lw    $7,(24)%0      \n" /* $a3 */
-        "lw    $8,(28)%0      \n" /* $t0 */
-        "lw    $9,(32)%0      \n" /* $t1 */
-        "lw    $10,(36)%0     \n" /* $t2 */
-        "lw    $11,(40)%0     \n" /* $t3 */
-        "lw    $12,(44)%0     \n" /* $t4 */
-        "lw    $13,(48)%0     \n" /* $t5 */
-        "lw    $14,(52)%0     \n" /* $t6 */
-        "lw    $15,(56)%0     \n" /* $t7 */
-        "lw    $24,(60)%0     \n" /* $t8 */
-        "lw    $25,(64)%0     \n" /* $t9 */
-        "lw    $16,(68)%0     \n" /* $s0 */
-        "lw    $17,(72)%0     \n" /* $s1 */
-        "lw    $18,(76)%0     \n" /* $s2 */
-        "lw    $19,(80)%0     \n" /* $s3 */
-        "lw    $20,(84)%0     \n" /* $s4 */
-        "lw    $21,(88)%0     \n" /* $s5 */
-        "lw    $22,(92)%0     \n" /* $s6 */
-        "lw    $23,(96)%0     \n" /* $s7 */
-        "lw    $28,(100)%0    \n" /* gp */
-        "lw    $30,(104)%0    \n" /* fp */
-        "lw    $29,(108)%0    \n" /* sp */
-        "lw    $31,(112)%0    \n" /* ra */
-        ".set reorder         \n"
-        : : "r" (addr) : "v0" /* only! */
+        ".set noat             \n"
+        ".set noreorder        \n"
+        "lw    $8, 48(%0)      \n" /* Get start address ($8 = $t0) */
+        "beqz  $8, running     \n" /* NULL -> already running */
+        "nop                   \n"
+        "move  $9, %0          \n" /* $t1 = context */
+        "jr    $8              \n"
+        "nop                   \n"
+    "running:                  \n"
+        "move  $8, %0          \n"
+        "lw    $16,  0($8)     \n" /* $s0 */
+        "lw    $17,  4($8)     \n" /* $s1 */
+        "lw    $18,  8($8)     \n" /* $s2 */
+        "lw    $19, 12($8)     \n" /* $s3 */
+        "lw    $20, 16($8)     \n" /* $s4 */
+        "lw    $21, 20($8)     \n" /* $s5 */
+        "lw    $22, 24($8)     \n" /* $s6 */
+        "lw    $23, 28($8)     \n" /* $s7 */
+        "lw    $28, 32($8)     \n" /* gp */
+        "lw    $30, 36($8)     \n" /* fp */
+        "lw    $29, 40($8)     \n" /* sp */
+        "lw    $31, 44($8)     \n" /* ra */
+        ".set at               \n"
+        ".set reorder          \n"
+        : : "r" (addr) : "t0" /* only! */
     );
-#endif
 }
 
 /*---------------------------------------------------------------------------
@@ -1124,26 +1089,27 @@ static inline void load_context(const void* addr)
  */
 static inline void core_sleep(void)
 {
-    /*
+#if 0
+#if CONFIG_CPU == JZ4732
         REG_CPM_LCR &= ~CPM_LCR_LPM_MASK;
         REG_CPM_LCR |= CPM_LCR_LPM_SLEEP;
-    */
-    #if 0
-        asm volatile(".set   mips32             \n"
-                 "mfc0   t0, 12             \n"
-                 "move   t1, t0             \n"
-                 "ori    t0, t0, 0x8000000  \n" /* Enable reduced power mode */
-                 "mtc0   t0, 12             \n"
+#endif
+        asm volatile(".set   mips32r2           \n"
+                 "mfc0   $8, $12            \n" /* mfc $t0, $12 */
+                 "move   $9, $8             \n" /* move $t1, $t0 */
+                 "la     $10, 0x8000000     \n" /* la $t2, 0x8000000 */
+                 "or     $8, $8, $10        \n" /* Enable reduced power mode */
+                 "mtc0   $8, $12            \n"
                  "wait                      \n"
-                 "mtc0   t1, 12             \n"
+                 "mtc0   $9, $12            \n"
                  ".set   mips0              \n"
-                 ::: "t0", "t1"
+                 ::: "t0", "t1", "t2"
                  );
-     #endif
-        /*
+#if CONFIG_CPU == JZ4732
     REG_CPM_LCR &= ~CPM_LCR_LPM_MASK;
         REG_CPM_LCR |= CPM_LCR_LPM_IDLE;
-    */
+#endif
+#endif
 }
 
 
@@ -2071,7 +2037,6 @@ static inline void block_thread_on_l(struct thread_entry *thread,
  */
 void switch_thread(void)
 {
-#ifndef ONDA_VX747
 
     const unsigned int core = CURRENT_CORE;
     struct thread_entry *block = cores[core].block_task;
@@ -2208,7 +2173,6 @@ void switch_thread(void)
     profile_thread_started(thread - threads);
 #endif
 
-#endif
 }
 
 /*---------------------------------------------------------------------------