8 files changed, 536 insertions, 50 deletions

diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-target.h b/firmware/target/mips/ingenic_jz47xx/lcd-target.h
index e643608d56..e643608d56 100644
--- a/firmware/target/mips/ingenic_jz47xx/onda_vx747/lcd-target.h
+++ b/firmware/target/mips/ingenic_jz47xx/lcd-target.h
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/adc-target.h b/firmware/target/mips/ingenic_jz47xx/onda_vx767/adc-target.h
new file mode 100644
index 0000000000..e74f008a3e
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/adc-target.h
@@ -0,0 +1,28 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef _ADC_TARGET_H_
+#define _ADC_TARGET_H_
+
+#define NUM_ADC_CHANNELS 4
+
+#define ADC_BUTTONS 0
+
+#endif /* _ADC_TARGET_H_ */
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-ondavx767.c b/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-ondavx767.c
new file mode 100644
index 0000000000..9deab7712a
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-ondavx767.c
@@ -0,0 +1,80 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+ 
+#include "config.h"
+#include "jz4740.h"
+#include "backlight-target.h"
+
+#define GPIO_PWM    123
+#define PWM_CHN     7
+#define PWM_FULL    101
+
+static void set_backlight(int unk, int val)
+{
+    if(val == 0)
+        __gpio_as_pwm7();
+    else
+    {
+        REG_TCU_TCSR(7) |= 2;
+        REG_TCU_TCSR(7) &= ~0x100;
+        int tmp;
+        tmp = (unk/2 + __cpm_get_rtcclk()) / unk;
+        if(tmp > 0xFFFF)
+            tmp = 0xFFFF;
+        
+        __tcu_set_half_data(7, (tmp * unk * 1374389535) >> 5);
+        __tcu_set_full_data(7, tmp);
+        
+        REG_TCU_TSCR = (1 << 7);
+        REG_TCU_TESR = (1 << 7);
+
+        __tcu_enable_pwm_output(7);
+    }
+    __tcu_set_count(7, 0);
+}
+
+bool _backlight_init(void)
+{
+    __gpio_as_pwm7();
+    
+    __tcu_stop_counter(7);
+    __tcu_disable_pwm_output(7);
+    
+    set_backlight(300, 7);
+    
+    return true;
+}
+void _backlight_on(void)
+{
+    set_backlight(300, 7);
+}
+void _backlight_off(void)
+{
+    set_backlight(300, 0);
+}
+
+#ifdef HAVE_BACKLIGHT_BRIGHTNESS
+void _backlight_set_brightness(int brightness)
+{
+    (void)brightness;
+    return;
+}
+#endif
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-target.h b/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-target.h
new file mode 100644
index 0000000000..4170f96cc0
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/backlight-target.h
@@ -0,0 +1,37 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef BACKLIGHT_TARGET_H
+#define BACKLIGHT_TARGET_H
+
+#ifdef BOOTLOADER
+#define BACKLIGHT_DRIVER_CLOSE
+/* Force the whole driver to be built */
+#define BACKLIGHT_FULL_INIT
+#endif
+
+#include <stdbool.h>
+
+bool _backlight_init(void);
+void _backlight_on(void);
+void _backlight_off(void);
+void _backlight_set_brightness(int brightness);
+
+#endif /* BACKLIGHT_TARGET_H */
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-ondavx767.c b/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-ondavx767.c
new file mode 100644
index 0000000000..8b033a1bee
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-ondavx767.c
@@ -0,0 +1,70 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+ 
+#include "config.h"
+#include "system.h"
+#include "jz4740.h"
+#include "button-target.h"
+
+#define BTN_VOL_DOWN (1 << 27)
+#define BTN_VOL_UP   (1 << 0)
+#define BTN_MENU     (1 << 1)
+#define BTN_OFF      (1 << 29)
+#define BTN_HOLD     (1 << 16)
+#define BTN_MASK     (BTN_VOL_DOWN | BTN_VOL_UP \
+                      | BTN_MENU | BTN_OFF )
+
+bool button_hold(void)
+{
+    return (~REG_GPIO_PXPIN(3) & BTN_HOLD ? 1 : 0);
+}
+
+void button_init_device(void)
+{  
+    __gpio_port_as_input(3, 29);
+    __gpio_port_as_input(3, 27);
+    __gpio_port_as_input(3, 16);
+    __gpio_port_as_input(3, 1);
+    __gpio_port_as_input(3, 0);
+}
+
+int button_read_device(void)
+{
+    if(button_hold())
+        return 0;
+    
+    unsigned int key = ~(__gpio_get_port(3));
+    int ret = 0;
+    
+    if(key & BTN_MASK)
+    {
+        if(key & BTN_VOL_DOWN)
+            ret |= BUTTON_VOL_DOWN;
+        if(key & BTN_VOL_UP)
+            ret |= BUTTON_VOL_UP;
+        if(key & BTN_MENU)
+            ret |= BUTTON_MENU;
+        if(key & BTN_OFF)
+            ret |= BUTTON_POWER;
+    }
+
+    return ret;
+}
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-target.h b/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-target.h
new file mode 100644
index 0000000000..32325d1423
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/button-target.h
@@ -0,0 +1,54 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef BUTTON_TARGET_H
+#define BUTTON_TARGET_H
+
+#include <stdbool.h>
+#include "config.h"
+
+#define HAS_BUTTON_HOLD
+
+bool button_hold(void);
+void button_init_device(void);
+int button_read_device(void);
+
+/* Main unit's buttons */
+#define BUTTON_POWER      0x00000001
+#define BUTTON_VOL_UP     0x00000002
+#define BUTTON_VOL_DOWN   0x00000004
+#define BUTTON_MENU       0x00000008
+
+/* Compatibility hacks for flipping. Needs a somewhat better fix. */
+#define BUTTON_LEFT  0
+#define BUTTON_RIGHT 0
+#define BUTTON_UP    0
+#define BUTTON_DOWN  0
+
+#define BUTTON_MAIN  (BUTTON_POWER | BUTTON_VOL_UP | BUTTON_VOL_DOWN | BUTTON_MENU)
+
+/* No remote */
+#define BUTTON_REMOTE 0
+
+/* Software power-off */
+#define POWEROFF_BUTTON BUTTON_POWER
+#define POWEROFF_COUNT 10
+
+#endif /* BUTTON_TARGET_H */
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx767/lcd-ondavx767.c b/firmware/target/mips/ingenic_jz47xx/onda_vx767/lcd-ondavx767.c
new file mode 100644
index 0000000000..63d1736ebe
--- /dev/null
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx767/lcd-ondavx767.c
@@ -0,0 +1,195 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+ 
+#include "config.h"
+#include "jz4740.h"
+#include "lcd-target.h"
+
+#define PIN_CS_N        (32*1+17) /* Chip select */
+#define PIN_RESET_N (32*1+18) /* Reset */
+#define PIN_UNK_N   (32*2+19)
+
+#define my__gpio_as_lcd_16bit()                 \
+do {                                            \
+        REG_GPIO_PXFUNS(2) = 0x0014ffff;        \
+        REG_GPIO_PXSELC(2) = 0x0014ffff;        \
+        REG_GPIO_PXPES(2) = 0x0014ffff;         \
+} while (0)
+
+
+#define SLEEP(x) for(i=0; i<x; i++) asm("nop"); asm("nop");
+#define DELAY    SLEEP(700000);
+static void _display_pin_init(void)
+{
+    int i;
+    my__gpio_as_lcd_16bit();
+    __gpio_as_output(PIN_UNK_N);
+    __gpio_set_pin(PIN_UNK_N);
+    __gpio_as_output(PIN_CS_N);
+    __gpio_as_output(PIN_RESET_N);
+    DELAY; /* delay_ms(10); */
+    
+    __gpio_clear_pin(PIN_CS_N);
+    DELAY; /* delay_ms(10); */
+    
+        __gpio_set_pin(PIN_RESET_N);
+        DELAY; /* delay_ms(10); */      
+        __gpio_clear_pin(PIN_RESET_N);
+        DELAY; /* delay_ms(10); */
+        __gpio_set_pin(PIN_RESET_N);
+        DELAY; /* delay_ms(10); */
+}
+
+#define WAIT_ON_SLCD while(REG_SLCD_STATE & SLCD_STATE_BUSY);
+#define SLCD_SET_DATA(x) WAIT_ON_SLCD; REG_SLCD_DATA = (x) | SLCD_DATA_RS_DATA;
+#define SLCD_SET_COMMAND(x) WAIT_ON_SLCD; REG_SLCD_DATA = (x) | SLCD_DATA_RS_COMMAND;
+#define SLCD_SEND_COMMAND(cmd,val) SLCD_SET_COMMAND(cmd); __gpio_set_pin(PIN_UNK_N); SLCD_SET_DATA(val); __gpio_clear_pin(PIN_UNK_N);
+static void _display_init(void)
+{
+    int i;
+    
+    SLCD_SEND_COMMAND(0xE3, 0x8);
+    SLCD_SEND_COMMAND(0xE4, 0x1411);
+    SLCD_SEND_COMMAND(0xE5, 0x8000);
+    SLCD_SEND_COMMAND(0x0, 0x1);
+    DELAY; /* delay_ms(10); */
+    
+    SLCD_SEND_COMMAND(0x1, 0x100);
+    SLCD_SEND_COMMAND(0x2, 0x400);
+    SLCD_SEND_COMMAND(0x3, 0x1028);
+    SLCD_SEND_COMMAND(0x4, 0);
+    SLCD_SEND_COMMAND(0x8, 0x202);
+    SLCD_SEND_COMMAND(0x9, 0);
+    SLCD_SEND_COMMAND(0xA, 0);
+    SLCD_SEND_COMMAND(0xC, 0);
+    SLCD_SEND_COMMAND(0xD, 0);
+    SLCD_SEND_COMMAND(0xF, 0);
+    SLCD_SEND_COMMAND(0x10, 0);
+    SLCD_SEND_COMMAND(0x11, 0x7);
+    SLCD_SEND_COMMAND(0x12, 0);
+    SLCD_SEND_COMMAND(0x13, 0);
+    SLCD_SEND_COMMAND(0x10, 0x17B0);
+    SLCD_SEND_COMMAND(0x11, 0x4);
+    SLCD_SEND_COMMAND(0x12, 0x13C);
+    SLCD_SEND_COMMAND(0x13, 0x1B00);
+    SLCD_SEND_COMMAND(0x29, 0x16);
+    SLCD_SEND_COMMAND(0x20, 0);
+    SLCD_SEND_COMMAND(0x21, 0);
+    SLCD_SEND_COMMAND(0x2B, 0x20);
+    SLCD_SEND_COMMAND(0x30, 0);
+    SLCD_SEND_COMMAND(0x31, 0x403);
+    SLCD_SEND_COMMAND(0x32, 0x400);
+    SLCD_SEND_COMMAND(0x35, 0x5);
+    SLCD_SEND_COMMAND(0x36, 0x6);
+    SLCD_SEND_COMMAND(0x37, 0x606);
+    SLCD_SEND_COMMAND(0x38, 0x106);
+    SLCD_SEND_COMMAND(0x39, 0x7);
+    SLCD_SEND_COMMAND(0x3C, 0x700);
+    SLCD_SEND_COMMAND(0x3D, 0x707);
+    SLCD_SEND_COMMAND(0x50, 0);
+    SLCD_SEND_COMMAND(0x51, 239);
+    SLCD_SEND_COMMAND(0x52, 0);
+    SLCD_SEND_COMMAND(0x53, 319);
+    SLCD_SEND_COMMAND(0x60, 0x2700);
+    SLCD_SEND_COMMAND(0x61, 0x1);
+    SLCD_SEND_COMMAND(0x6A, 0);
+    SLCD_SEND_COMMAND(0x80, 0);
+    SLCD_SEND_COMMAND(0x81, 0);
+    SLCD_SEND_COMMAND(0x82, 0);
+    SLCD_SEND_COMMAND(0x83, 0);
+    SLCD_SEND_COMMAND(0x84, 0);
+    SLCD_SEND_COMMAND(0x85, 0);
+    SLCD_SEND_COMMAND(0x90, 0x10);
+    SLCD_SEND_COMMAND(0x92, 0);
+    SLCD_SEND_COMMAND(0x93, 0x3);
+    SLCD_SEND_COMMAND(0x95, 0x110);
+    SLCD_SEND_COMMAND(0x97, 0);
+    SLCD_SEND_COMMAND(0x98, 0);
+    SLCD_SEND_COMMAND(0x7, 0x173);
+    
+    SLCD_SET_COMMAND(0x22);
+    __gpio_set_pin(PIN_UNK_N);
+}
+
+static void _display_on(void)
+{
+}
+
+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_SLCD_CFG = (SLCD_CFG_BURST_4_WORD | SLCD_CFG_DWIDTH_18 | SLCD_CFG_CWIDTH_18BIT
+                   | SLCD_CFG_CS_ACTIVE_LOW | SLCD_CFG_RS_CMD_LOW | SLCD_CFG_CLK_ACTIVE_FALLING
+                   | SLCD_CFG_TYPE_PARALLEL);
+    
+    REG_SLCD_CTRL = SLCD_CTRL_DMA_EN;
+}
+
+static void _set_lcd_clock(void)
+{
+        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);
+    __cpm_start_lcd();
+}
+
+void lcd_init_controller(void)
+{
+    int i;
+    _display_pin_init();
+    _set_lcd_bus();
+    _set_lcd_clock();
+    SLEEP(1000);
+    _display_init();
+}
+
+void lcd_set_target(short x, short y, short width, short height)
+{
+    SLCD_SEND_COMMAND(0x50, y);
+    SLCD_SEND_COMMAND(0x51, y+height-1);
+    SLCD_SEND_COMMAND(0x52, x);
+    SLCD_SEND_COMMAND(0x53, x+width-1);
+    /* TODO */
+}
+
+void lcd_on(void)
+{
+    _display_on();
+}
+
+void lcd_off(void)
+{
+    _display_off();
+}
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
index 8f1c3f5c1a..61be6c60de 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
@@ -555,21 +555,7 @@ void dma_cache_wback_inv(unsigned long addr, unsigned long size)
     }
 }
 
-extern int main(void);
-extern void except_common_entry(void);
-
-#define mtc0_tlbw_hazard()                 \
-    __asm__ __volatile__(                  \
-    "    .set    noreorder          \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    .set    reorder            \n");
-
-#define tlbw_use_hazard()                  \
+#define BARRIER                            \
     __asm__ __volatile__(                  \
     "    .set    noreorder          \n"    \
     "    nop                        \n"    \
@@ -580,10 +566,13 @@ extern void except_common_entry(void);
     "    nop                        \n"    \
     "    .set    reorder            \n");
 
-
-#define PAGE_SHIFT           PL_4K
-#define PM_DEFAULT_MASK      PM_4K
-#define UNIQUE_ENTRYHI(idx)  (A_K0BASE + ((idx) << (PAGE_SHIFT + 1)))
+#define DEFAULT_PAGE_SHIFT       PL_4K
+#define DEFAULT_PAGE_MASK        PM_4K
+#define UNIQUE_ENTRYHI(idx, ps)  (A_K0BASE + ((idx) << (ps + 1)))
+#define ASID_MASK                M_EntryHiASID
+#define VPN2_SHIFT               S_EntryHiVPN2
+#define PFN_SHIFT                S_EntryLoPFN
+#define PFN_MASK                 0xffffff
 static void local_flush_tlb_all(void)
 {
     unsigned long old_ctx;
@@ -594,59 +583,92 @@ static void local_flush_tlb_all(void)
     old_ctx = read_c0_entryhi();
     write_c0_entrylo0(0);
     write_c0_entrylo1(0);
+    BARRIER;
 
     /* Blast 'em all away. */
-    for(entry = read_c0_wired(); entry < 32; entry++)
+    for(entry = 0; entry < 32; entry++)
     {
         /* Make sure all entries differ. */
-        write_c0_entryhi(UNIQUE_ENTRYHI(entry));
+        write_c0_entryhi(UNIQUE_ENTRYHI(entry, DEFAULT_PAGE_SHIFT));
         write_c0_index(entry);
-        mtc0_tlbw_hazard();
+        BARRIER;
         tlb_write_indexed();
     }
-    tlbw_use_hazard();
+    BARRIER;
     write_c0_entryhi(old_ctx);
     
     restore_irq(old_irq);
 }
 
+static void add_wired_entry(unsigned long entrylo0, unsigned long entrylo1,
+                            unsigned long entryhi,  unsigned long pagemask)
+{
+    unsigned long wired;
+    unsigned long old_pagemask;
+    unsigned long old_ctx;
+    unsigned int  old_irq = disable_irq_save();
+    
+    old_ctx = read_c0_entryhi() & ASID_MASK;
+    old_pagemask = read_c0_pagemask();
+    wired = read_c0_wired();
+    write_c0_wired(wired + 1);
+    write_c0_index(wired);
+    BARRIER;
+    write_c0_pagemask(pagemask);
+    write_c0_entryhi(entryhi);
+    write_c0_entrylo0(entrylo0);
+    write_c0_entrylo1(entrylo1);
+    BARRIER;
+    tlb_write_indexed();
+    BARRIER;
+
+    write_c0_entryhi(old_ctx);
+    BARRIER;
+    write_c0_pagemask(old_pagemask);
+    local_flush_tlb_all();
+    restore_irq(old_irq);
+}
+
+static void map_address(unsigned long virtual, unsigned long physical, unsigned long length)
+{
+    unsigned long entry0  = (physical & PFN_MASK) << PFN_SHIFT;
+    unsigned long entry1  = ((physical+length) & PFN_MASK) << PFN_SHIFT;
+    unsigned long entryhi = virtual & ~VPN2_SHIFT;
+    
+    entry0 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
+    entry1 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
+    
+    add_wired_entry(entry0, entry1, entryhi, DEFAULT_PAGE_MASK);
+}
+
 
 static void tlb_init(void)
 {
-    write_c0_pagemask(PM_DEFAULT_MASK);
+    write_c0_pagemask(DEFAULT_PAGE_MASK);
     write_c0_wired(0);
     write_c0_framemask(0);
     
     local_flush_tlb_all();
+/*
+    map_address(0x80000000, 0x80000000, 0x4000);
+    map_address(0x80004000, 0x80004000, MEM * 0x100000);
+*/
 }
 
-static void tlb_refill_handler(void)
+void tlb_refill_handler(void)
 {
-#if 1
-    panicf("TLB refill handler! [0x%x] [0x%x]", read_c0_badvaddr(), read_c0_epc());
-#else
-    __asm__ __volatile__(
-    "mfc0 k0, C0_BADVADDR\n"
-    "lui  k1, pgdc\n"
-    "lw   k1, pgdc>>16(k0)\n"
-    "srl  k0, k0, 22\n"
-    "sll  k0, k0, 2\n"
-    "addu k1, k1, k0\n"
-    "mfc0 k0, C0_CONTEXT\n"
-    "lw   k1, 0(k1)\n"
-    "andi k0, k0, 0xFFC\n"
-    "addu k1, k1, k0\n"
-    "lw   k0, 0(k1)\n"
-    "nop\n"
-    "mtc0 k0, C0_ENTRYLO0\n"
-    "mfc0 k1, C0_EPC\n"
-    "tlbwr\n"
-    "jr   k1\n"
-    "rfe\n"
-    );
-#endif
+    panicf("TLB refill handler! [0x%x] [0x%lx]", read_c0_badvaddr(), read_c0_epc());
 }
 
+static void tlb_call_refill(void)
+{
+    asm("la $8, tlb_refill_handler \n"
+        "jr $8                     \n");
+}
+
+extern int main(void);
+extern void except_common_entry(void);
+
 void system_main(void)
 {
     int i;
@@ -657,7 +679,7 @@ void system_main(void)
      * 0x180 - Exception/Interrupt handler
      * 0x200 - Special Exception Interrupt handler (when IV is set in CP0_CAUSE)
      */
-    memcpy((void *)A_K0BASE, (void *)&tlb_refill_handler, 0x20);
+    memcpy((void *)A_K0BASE, (void *)&tlb_call_refill, 0x20);
     memcpy((void *)(A_K0BASE + 0x100), (void *)&except_common_entry, 0x20);
     memcpy((void *)(A_K0BASE + 0x180), (void *)&except_common_entry, 0x20);
     memcpy((void *)(A_K0BASE + 0x200), (void *)&except_common_entry, 0x20);
@@ -671,7 +693,7 @@ void system_main(void)
     for(i=0; i<IRQ_MAX; i++)
         dis_irq(i);
     
-    tlb_init();
+    //tlb_init();
     
     sti();