- initial import

git-svn-id: http://moon:8086/svn/mips@1 a8ebac50-d88d-4704-bea3-6648445a41b3
2014-07-20 15:01:37 +00:00
commit 26291bca3d
1549 changed files with 3997969 additions and 0 deletions
@@ -0,0 +1,45 @@
+ENDIAN_FLAGS=-EL
+CFLAGS=$(ENDIAN_FLAGS) -msoft-float -O2 -march=r3000 -I. -Wl,-M
+
+ifeq ($(TLB),yes)
+  CFLAGS+=-DSYS_IO_BASE=0xAC000000 -DSDRAM_BASE=0x80000000 -DFLASH_BASE_IO=0xA4000000 -DFLASH_BASE_MEM=0x88000000
+else
+  CFLAGS+=-DSYS_IO_BASE=0xA0000000 -DSDRAM_BASE=0x40000000 -DFLASH_BASE_IO=0xA4000000 -DFLASH_BASE_MEM=0x00000000
+endif
+
+AS=mipsel-elf-as
+AR=mipsel-elf-ar
+CC=mipsel-elf-gcc
+LD=mipsel-elf-ld
+OBJDUMP=mipsel-elf-objdump
+OBJCOPY=mipsel-elf-objcopy
+FLASHGEN=flashgen
+
+SRCS=libsys.c xcpt.c syscalls.c irq.c mips_dbg.c mips_dis.c mips_gfx.c mips_ps2.c gpio.c
+OBJS=libsys.o xcpt.o syscalls.o irq.o mips_dbg.o mips_dis.o mips_gfx.o mips_ps2.o gpio.o
+
+.PHONY : libsys.a libsys_sim.a
+
+all: libsys.a libsys_sim.a
+	$(MAKE) -C eb
+	
+$(OBJS): $(SRCS)
+	$(CC) $(CFLAGS) -G 0 -c $(SRCS)
+
+libsys.a: $(OBJS) $(SRCS)
+	$(AR) -r libsys.a $(OBJS)
+	$(CC) $(CFLAGS) -G 0 -c startup.S
+	$(CC) $(CFLAGS) -G 0 -c kernel.S
+
+%.o: %.c
+	$(CC) $(CFLAGS) -G 0 -c  *.c
+
+libsys_sim.a: libsys_sim.o xcpt.o syscalls.o irq.o gpio.o
+	$(AR) -r libsys_sim.a libsys_sim.o xcpt.o syscalls.o irq.o gpio.o
+	$(CC) $(CFLAGS) -G 0 -c startup.S
+	$(CC) $(CFLAGS) -G 0 -c kernel.S
+	
+clean:
+	rm -rf *.a *.o *.map *.dis *.elf > /dev/null
+	$(MAKE) -C eb clean
+	
@@ -0,0 +1,47 @@
+ENDIAN_FLAGS=-EB
+CFLAGS=$(ENDIAN_FLAGS) -msoft-float -O2 -march=r3000 -I../ -Wl,-M
+
+ifeq ($(TLB),yes)
+  CFLAGS+=-DSYS_IO_BASE=0xAC000000 -DSDRAM_BASE=0x80000000 -DFLASH_BASE_IO=0xA4000000 -DFLASH_BASE_MEM=0x88000000
+else
+  CFLAGS+=-DSYS_IO_BASE=0xA0000000 -DSDRAM_BASE=0x40000000 -DFLASH_BASE_IO=0xA4000000 -DFLASH_BASE_MEM=0x00000000
+endif
+
+AS=mipsel-elf-as
+AR=mipsel-elf-ar
+CC=mipsel-elf-gcc
+LD=mipsel-elf-ld
+OBJDUMP=mipsel-elf-objdump
+OBJCOPY=mipsel-elf-objcopy
+FLASHGEN=flashgen
+
+SRCS=../libsys.c ../xcpt.c ../syscalls.c ../irq.c ../mips_dbg.c ../mips_dis.c ../mips_gfx.c ../mips_ps2.c ../gpio.c
+OBJS=libsys.o xcpt.o syscalls.o irq.o mips_dbg.o mips_dis.o mips_gfx.o mips_ps2.o gpio.o
+
+OBJS_SIM=libsys_sim.o xcpt.o syscalls.o irq.o gpio.o
+
+.PHONY : libsys.a libsys_sim.a
+
+all: libsys.a libsys_sim.a
+	
+$(OBJS): $(SRCS)
+	$(CC) $(CFLAGS) -G 0 -c $(SRCS)
+
+libsys.a: $(OBJS) $(SRCS)
+	$(AR) -r libsys.a $(OBJS)
+	$(CC) $(CFLAGS) -G 0 -c ../startup.S
+	$(CC) $(CFLAGS) -G 0 -c ../kernel.S
+
+libsys_sim.o: ../libsys_sim.c
+	$(CC) $(CFLAGS) -G 0 -c ../libsys_sim.c
+
+%.o: ../%.c
+	$(CC) $(CFLAGS) -G 0 -c  ../*.c
+
+libsys_sim.a: libsys_sim.o xcpt.o syscalls.o irq.o gpio.o
+	$(AR) -r libsys_sim.a libsys_sim.o xcpt.o syscalls.o irq.o gpio.o
+	$(CC) $(CFLAGS) -G 0 -c ../startup.S
+	$(CC) $(CFLAGS) -G 0 -c ../kernel.S
+
+clean:
+	rm -rf *.a *.o *.map *.dis *.elf > /dev/null
@@ -0,0 +1,28 @@
+#include "libsys.h"
+#include "gpio.h"
+
+// ---------------------------------------------------------
+UINT32 gpio_init(gpio_if_t *pGPIO, UINT32 base_addr)
+{
+	pGPIO->pBase = (UINT32*)base_addr;
+	return 0;
+}
+
+UINT32 gpio_clean(gpio_if_t *pGPIO)
+{
+	return 0;
+}
+
+UINT32 gpio_write(gpio_if_t *pGPIO, UINT32 mask, UINT32 shift, UINT32 offset, UINT32 value)
+{
+	*(pGPIO->pBase + offset) = (*(pGPIO->pBase + offset) & ~(mask << shift)) | ((value & mask) << shift);
+	return 0;
+}
+
+UINT32 gpio_read(gpio_if_t *pGPIO, UINT32 mask, UINT32 shift, UINT32 offset, UINT32 *pValue)
+{
+	*pValue = (*(pGPIO->pBase + offset) & (mask << shift)) >> shift;
+	return 0;
+}
+
+// ---------------------------------------------------------
@@ -0,0 +1,41 @@
+#ifndef GPIO_H
+#define GPIO_H
+
+#define GPIO_DATA_OFFSET 		0	/* Offset for data */
+#define GPIO_DIR_OFFSET			1	/* Offset from any channel */
+
+// ---------------------------------------------------------
+// Types
+// ---------------------------------------------------------
+typedef struct _sgpio_if_t
+{
+	volatile UINT32 *pBase;
+
+} gpio_if_t;
+
+/* Some quick macros */
+
+/* Write/Read from the DATA register */
+
+#define gpio_write_data(gpio,mask,shift,value) \
+        gpio_write(gpio, mask, shift, GPIO_DATA_OFFSET, value)
+
+#define gpio_read_data(gpio,mask,shift,ret) \
+        gpio_read(gpio, mask, shift, GPIO_DATA_OFFSET, ret)
+
+/* Write/Read from the TRISTATE register ( for bidirectional GPIOs) */
+
+#define gpio_set_dir(gpio,mask,shift,value) \
+        gpio_write(gpio, mask, shift, GPIO_DIR_OFFSET, value)
+
+#define gpio_get_dir(gpio,mask,shift,ret) \
+        gpio_read(gpio, mask, shift, GPIO_DIR_OFFSET, ret)
+
+
+UINT32 gpio_init(gpio_if_t *pGPIO, UINT32 base_addr);
+UINT32 gpio_clean(gpio_if_t *pGPIO);
+
+UINT32 gpio_write(gpio_if_t *pGPIO, UINT32 mask, UINT32 shift, UINT32 offset, UINT32 value);
+UINT32 gpio_read(gpio_if_t *pGPIO, UINT32 mask, UINT32 shift, UINT32 offset, UINT32 *pValue);
+
+#endif // GPIO_H
@@ -0,0 +1,98 @@
+#include <sys/times.h>
+#include <sys/time.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <stdlib.h>
+#include "libsys.h"
+#include "regdef.h"
+#include "xcpt.h"
+#include "irq.h"
+
+static fp_irq_t g_irq_handler[MAX_NUM_IRQ] = {NULL};
+
+int _irq_dispatch(struct xcptcontext * xcp)
+{
+	int i, ip, cause;
+
+	cause = xcp->cr & xcp->sr;
+	ip =  (cause >> 8) & 0xFF;
+
+	for (i=0; i < MAX_NUM_IRQ; i++)
+	{
+		if (ip & 1)
+			if (g_irq_handler[i])
+				(g_irq_handler[i])(xcp);
+
+		ip >>= 1;
+	}
+
+	return 0;
+}
+
+void interrupt_register(int irq_num, fp_irq_t fp)
+{
+	if ((irq_num >= 0) && (irq_num < MAX_NUM_IRQ))
+		g_irq_handler[irq_num] = fp;
+
+}
+
+void interrupt_enable(int irq_num)
+{
+	reg_t reg, im;
+
+	if ((irq_num >= 0) && (irq_num < MAX_NUM_IRQ))
+	{
+		im = 1 << irq_num;
+		reg = CP0_SR_read();
+		reg |= (SR_MASK_IM & (im << 8));
+		CP0_SR_write(reg);
+	}
+
+}
+
+void interrupt_disable(int irq_num)
+{
+	reg_t reg, im;
+
+	if ((irq_num >= 0) && (irq_num < MAX_NUM_IRQ))
+	{
+		im = 1 << irq_num;
+		reg = CP0_SR_read();
+		reg &= ~(SR_MASK_IM & (im << 8));
+		CP0_SR_write(reg);
+	}
+}
+
+void interrupt_set(int irq_num)
+{
+	reg_t reg, ip;
+
+	if ((irq_num >= 0) && (irq_num < MAX_NUM_IRQ))
+	{
+		ip = 1 << irq_num;
+		reg = CP0_CR_read();
+		reg |= (CR_MASK_IP & (ip << 8));
+		CP0_CR_write(reg);
+	}
+}
+
+void interrupt_clr(int irq_num)
+{
+	reg_t reg, ip;
+
+	if ((irq_num >= 0) && (irq_num < MAX_NUM_IRQ))
+	{
+		ip = 1 << irq_num;
+		reg = CP0_CR_read();
+		reg &= ~(CR_MASK_IP & (ip << 8));
+		CP0_CR_write(reg);
+	}
+}
+
+void interrupt_init(void)
+{
+	// register interrupt dispatcher
+	xcpt_register(EXC_INT, _irq_dispatch);
+
+}
+
@@ -0,0 +1,16 @@
+#ifndef IRQ_H
+#define IRQ_H
+
+#include "xcpt.h"
+
+#define MAX_NUM_IRQ				8
+
+typedef void (*fp_irq_t)(struct xcptcontext * xcp);
+
+void interrupt_init(void);
+int _irq_dispatch(struct xcptcontext * xcp);
+void interrupt_register(int irq_num, fp_irq_t fp);
+void interrupt_enable(int irq_num);
+void interrupt_disable(int irq_num);
+
+#endif // IRQ_H
@@ -0,0 +1,184 @@
+#include <regdef.h>
+#include <xcpt_asm.h>
+
+		.section .exc_vect
+_xcpt_vector_tbl:
+		.fill	16, 4, 0		
+			
+		.text
+
+LEAF(_xcpt_handler)
+		.set noreorder
+		.set noat
+		/* Note: exceptions do not save and restore registers k0 and k1.
+		* on entry, k1 = exception class.
+		*/
+		
+		/* allocate exception stack frame (on 8-byte boundary) */
+		subu k1, sp, XCP_SIZE
+		srl k1, 3 	/* shift right/left -> alligned on boundary */
+		sll k1, 3
+
+		/* save enough registers to get by */
+		sw AT, XCP_AT(k1)
+		sw v0, XCP_V0(k1)
+		sw v1, XCP_V1(k1)
+		sw a0, XCP_A0(k1)
+		sw a1, XCP_A1(k1)
+		sw a2, XCP_A2(k1)
+		sw a3, XCP_A3(k1)
+		sw sp, XCP_SP(k1)
+		sw ra, XCP_RA(k1)
+		
+		/* get coprocessor 0 exception state */
+		mfc0 a0, CP0_CR
+		mfc0 a1, CP0_SR
+		mfc0 a2, CP0_BADDR
+		mfc0 a3, CP0_EPC
+
+		/* we can safely use AT now */
+		.set at
+
+		/* switch to using sp to point at exception frame */
+		move sp, k1
+		
+		/* nothing sensible to store for k0/k1, store zero */
+		sw zero, XCP_K0(sp)
+		sw zero, XCP_K1(sp)		
+
+		/* we are now interruptible: dump all remaining state
+		* into the exception stack frame.
+		*/
+		/* coprocessor exception state */
+		sw a0, XCP_CR(sp)
+		sw a1, XCP_SR(sp)
+		sw a2, XCP_BADDR(sp)
+		sw a3, XCP_EPC(sp)
+
+		/* mdhi and mdlo */
+		mfhi v0
+		mflo v1
+		sw v0, XCP_MDHI(sp)
+		sw v1, XCP_MDLO(sp)
+
+		/* Save all the other general registers.
+		* We save zero, s0-s7 and s8 as well, as instruction emulators (e.g. FP
+		* operations) and debuggers rely on all registers stored together in
+		* well-defined structure.
+		*/
+		sw zero, XCP_ZERO(sp)
+		sw t0, XCP_T0(sp)
+		sw t1, XCP_T1(sp)
+		sw t2, XCP_T2(sp)
+		sw t3, XCP_T3(sp)
+		sw t4, XCP_T4(sp)
+		sw t5, XCP_T5(sp)
+		sw t6, XCP_T6(sp)
+		sw t7, XCP_T7(sp)
+		sw s0, XCP_S0(sp)
+		sw s1, XCP_S1(sp)
+		sw s2, XCP_S2(sp)
+		sw s3, XCP_S3(sp)
+		sw s4, XCP_S4(sp)
+		sw s5, XCP_S5(sp)
+		sw s6, XCP_S6(sp)
+		sw s7, XCP_S7(sp)
+		sw t8, XCP_T8(sp)
+		sw t9, XCP_T9(sp)
+		sw gp, XCP_GP(sp)
+		sw s8, XCP_S8(sp)
+
+		/* I don't know what the following does. [rb] */
+		/* load our _gp pointer */
+#		la gp, _gp
+
+		/* and call the C exception handler */
+		move a0, sp	# arg1 = &xcp
+		subu sp, 16	# (arg save area)
+		j _xcpt_call
+		move ra, zero	# fake return address
+		
+		/* This strange call to _xcpt_call with zero return address is to
+		* help exception-aware debuggers to trace back over the exception event.
+		* We are basically interposing a bogus stackframe (with a zero return
+		* address) between the C exception handler and the actual machine
+		* exception.
+		*/
+$xcptrest:
+		.set noat
+		add AT, sp, 16
+		
+		/* at points to exception frame */
+$xcptrestother:
+		/* restore all state */
+		/* restore most general registers */
+		lw t0, XCP_T0(AT)
+		lw t1, XCP_T1(AT)
+		lw t2, XCP_T2(AT)
+		lw t3, XCP_T3(AT)
+		lw t4, XCP_T4(AT)
+		lw t5, XCP_T5(AT)
+		lw t6, XCP_T6(AT)
+		lw t7, XCP_T7(AT)
+		lw s0, XCP_S0(AT)
+		lw s1, XCP_S1(AT)
+		lw s2, XCP_S2(AT)
+		lw s3, XCP_S3(AT)
+		lw s4, XCP_S4(AT)
+		lw s5, XCP_S5(AT)
+		lw s6, XCP_S6(AT)
+		lw s7, XCP_S7(AT)
+		lw t8, XCP_T8(AT)
+		lw t9, XCP_T9(AT)
+		lw gp, XCP_GP(AT)
+		lw s8, XCP_S8(AT)
+
+		/* mdhi and mdlo */
+		lw v0, XCP_MDHI(AT)
+		lw v1, XCP_MDLO(AT)
+		mthi v0
+		mtlo v1
+
+		/* remaining general registers */
+		lw a0, XCP_A0(AT)
+		lw a1, XCP_A1(AT)
+		lw a2, XCP_A2(AT)
+		lw a3, XCP_A3(AT)
+		lw ra, XCP_RA(AT)
+
+		/* restore the exception-time status register */
+		.set noreorder
+		lw v0, XCP_SR(AT)
+		nop			
+		mtc0 v0, CP0_SR
+		lw v1, XCP_V1(AT)
+		lw v0, XCP_V0(AT)
+		lw sp, XCP_SP(AT)
+
+		/* we are not uninterruptible and can use k1 safely */
+		lw k1, XCP_EPC(AT)
+		lw AT, XCP_AT(AT)
+		mtc0 k1, CP0_EPC
+		j	k1
+		rfe
+
+		.set reorder
+		.set at
+END(_xcpt_handler)
+		
+		
+LEAF(_xcpt_call)
+		/* on entry: a0 == &xcp */
+		subu sp, 24
+		sw ra, 16(sp)
+		
+		/* punt out to _xcpt_deliver */
+		jal _xcpt_deliver
+		nop
+		lw ra, 16(sp)
+		addu sp, 24
+		beqz ra, $xcptrest
+		nop
+		j ra
+		nop
+END(_xcpt_call)	
@@ -0,0 +1,402 @@
+#ifndef LIBSYS_H
+#define LIBSYS_H
+
+// ---------------------------------------------------------
+// Types
+// ---------------------------------------------------------
+#define INT8							signed char
+#define INT16							signed short
+#define INT32							signed int
+#define INT64							signed long long
+#define UINT8							unsigned char
+#define UINT16							unsigned short
+#define UINT32							unsigned int
+#define UINT64							unsigned long long
+#define INT								INT32
+#define UINT							UINT32
+#define FLOAT32							float
+#define FLOAT64							double
+
+#define int8_t							signed char
+#define int16_t							signed short
+#define int32_t							signed int
+#define int64_t							signed long long
+#define uint8_t							unsigned char
+#define uint16_t						unsigned short
+#define uint32_t						unsigned int
+#define uint64_t						unsigned long long
+
+#define	NO_ERROR						LSYS_SUCCESS
+#define	ERROR							LSYS_ERR_BASE
+#define	LSYS_SUCCESS					0
+#define	LSYS_ERR_BASE					0x80000000
+
+#define	IS_ERROR(e)						((e & LSYS_ERR_BASE) == LSYS_ERR_BASE)
+
+// ---------------------------------------------------------
+// IRQs
+// ---------------------------------------------------------
+#define	SYS_INT_TIMER					7
+#define	SYS_INT_PS2						6
+#define	SYS_INT_EMAC					6
+#define	SYS_INT_VGA						5
+#define	SYS_INT_AC97					5
+#define	SYS_INT_USB						4
+#define	SYS_INT_UART					3
+#define	SYS_INT_DBG						2
+#define	SYS_INT_SOFT_1					1
+#define	SYS_INT_SOFT_0					0
+
+// ---------------------------------------------------------
+// Memory-mapped registers
+// ---------------------------------------------------------
+#ifndef SYS_IO_BASE
+#define SYS_IO_BASE			0xA0000000
+#endif
+#ifndef SDRAM_BASE
+#define SDRAM_BASE			0x40000000
+#endif
+#ifndef FLASH_BASE_IO
+#define FLASH_BASE_IO		0xA4000000
+#endif
+#ifndef FLASH_BASE_MEM
+#define FLASH_BASE_MEM		0x00000000
+#endif
+
+// GPIO
+#define	SYS_GPIO_0_BASE					(SYS_IO_BASE + 0)
+#define	SYS_GPIO_0_DATA					(SYS_GPIO_0_BASE + 0)
+#define	SYS_GPIO_0_DIR					(SYS_GPIO_0_BASE + 4)
+
+#define	GPIO_0_DFLT_DIR					0x00000000		// Inputs
+#define	GPIO_0_DFLT_DATA				0x00000000
+
+#define	GPIO_0_MASK_LED					0x000001FF
+#define	GPIO_0_ALIGN_LED				0
+
+#define	GPIO_0_MASK_USB					0x00000003
+#define	GPIO_0_ALIGN_USB				12
+#define	GPIO_0_USB_RST					0x00000001
+#define	GPIO_0_USB_INTEN				0x00000002
+
+#define	GPIO_0_MASK_AC97				0x00000001
+#define	GPIO_0_ALIGN_AC97				15
+#define	GPIO_0_AC97_INTEN				0x00000001
+
+#define	GPIO_0_MASK_DIP					0x000000FF
+#define	GPIO_0_ALIGN_DIP				16
+
+#define	GPIO_0_MASK_BTN					0x0000001F
+#define	GPIO_0_ALIGN_BTN				24
+
+#define	GPIO_0_MASK_MDIO				0x00000007
+#define	GPIO_0_ALIGN_MDIO				29
+#define	GPIO_0_MDIO_INTEN				0x00000001		// ToDo: better grouping
+#define	GPIO_0_MDIO_RST					0x00000001
+#define	GPIO_0_MDIO_MDC					0x00000002
+#define	GPIO_0_MDIO_MDIO				0x00000004
+
+
+// RTC
+#define	SYS_TIMER_USEC					(SYS_IO_BASE + 0x8)
+#define	SYS_TIMER_SEC					(SYS_IO_BASE + 0xC)
+
+// TIMERs
+#define	SYS_ITIM_CTRL					(SYS_IO_BASE + 0x18)
+#define	SYS_ITIM_STAT					(SYS_IO_BASE + 0x1C)
+#define	SYS_ITIM0_CNT					(SYS_IO_BASE + 0x20)
+#define	SYS_ITIM1_CNT					(SYS_IO_BASE + 0x24)
+#define	SYS_ITIM2_CNT					(SYS_IO_BASE + 0x28)
+#define	SYS_ITIM3_CNT					(SYS_IO_BASE + 0x2C)
+#define	SYS_ITIM0_CMP					(SYS_IO_BASE + 0x30)
+#define	SYS_ITIM1_CMP					(SYS_IO_BASE + 0x34)
+#define	SYS_ITIM2_CMP					(SYS_IO_BASE + 0x38)
+#define	SYS_ITIM3_CMP					(SYS_IO_BASE + 0x3C)
+
+// UARTs
+#define SYS_UART_BIT_TX_HALFFULL		0x00000001
+#define SYS_UART_BIT_TX_FULL			0x00000002
+#define SYS_UART_BIT_RX_HALFFULL		0x00000004
+#define SYS_UART_BIT_RX_FULL			0x00000008
+#define SYS_UART_BIT_RX_AVAIL			0x00000010
+#define SYS_UART_BIT_TX_INTEN			0x00000020
+#define SYS_UART_BIT_RX_INTEN			0x00000040
+#define SYS_UART_BIT_TX_IRQ				0x00000100
+#define SYS_UART_BIT_RX_IRQ				0x00000200
+#define	SYS_UART_DATA					SYS_UART0_DATA
+#define	SYS_UART_STAT					SYS_UART0_STAT
+#define	SYS_UART_BAUD					SYS_UART0_BAUD
+#define	SYS_UART0_DATA					(SYS_IO_BASE + 0x10000)
+#define	SYS_UART0_STAT					(SYS_IO_BASE + 0x10004)
+#define	SYS_UART0_BAUD					(SYS_IO_BASE + 0x10008)
+#define	SYS_UART1_DATA					(SYS_IO_BASE + 0x10100)
+#define	SYS_UART1_STAT					(SYS_IO_BASE + 0x10104)
+#define	SYS_UART1_BAUD					(SYS_IO_BASE + 0x10108)
+
+// PS2s
+#define SYS_PS2_BIT_TX_EMPTY			0x00000001
+#define SYS_PS2_BIT_RX_AVAIL			0x00000004
+#define SYS_PS2_BIT_PIN_CLOCK			0x00000008
+#define SYS_PS2_BIT_PIN_DATA			0x00000010
+#define SYS_PS2_BIT_TX_INTEN			0x00000020
+#define SYS_PS2_BIT_RX_INTEN			0x00000040
+#define SYS_PS2_BIT_TX_IRQ				0x00000100
+#define SYS_PS2_BIT_RX_IRQ				0x00000200
+#define SYS_PS2_BIT_RX_ERR_PAR			0x00000800
+#define SYS_PS2_BIT_RX_ERR_FLAG			0x00008000
+#define	SYS_PS2_0_DATA					(SYS_IO_BASE + 0x10200)
+#define	SYS_PS2_0_STAT					(SYS_IO_BASE + 0x10204)
+#define	SYS_PS2_1_DATA					(SYS_IO_BASE + 0x10300)
+#define	SYS_PS2_1_STAT					(SYS_IO_BASE + 0x10304)
+
+// USB
+#define	SYS_USB_DATA					(SYS_IO_BASE + 0x20000)
+#define	SYS_USB_MBX						(SYS_IO_BASE + 0x20004)
+#define	SYS_USB_ADDR					(SYS_IO_BASE + 0x20008)
+#define	SYS_USB_STATUS					(SYS_IO_BASE + 0x2000C)
+
+// VGA
+#define	SYS_VGA_CTRL					(SYS_IO_BASE + 0x30000)	// R/W
+#define	SYS_VGA_BIT_CGRDY				0x00000001	// RO
+#define	SYS_VGA_BIT_MSTEN				0x00000002	// R/W
+#define	SYS_VGA_BIT_BUFCHG_INTEN		0x00000004	// RO
+#define	SYS_VGA_BIT_BUFCHG_FLAG			0x00000008	// RO
+#define	SYS_VGA_BIT_CLRSCR				0x00000010	// WO
+#define	SYS_VGA_BIT_CLRLINE				0x00000020	// WO
+#define	SYS_VGA_BIT_BLIT_REQ			0x00000100	// R/W
+#define	SYS_VGA_BIT_BLIT_BSY			0x00000100	// R/W
+#define	SYS_VGA_BIT_BLIT_FIN			0x00000200	// R/W
+#define	SYS_VGA_BIT_BLIT_FIN_ACK		0x00000200	// R/W
+#define	SYS_VGA_BIT_BLIT_FIN_INTEN		0x00000400	// R/W
+#define	SYS_VGA_BIT_BLIT_EMPTY			0x00000800	// R/W
+#define	SYS_VGA_BIT_BLIT_EMPTY_ACK		0x00000800	// R/W
+#define	SYS_VGA_BIT_BLIT_EMPTY_INTEN	0x00001000	// R/W
+#define	SYS_VGA_BIT_BLIT_OP_CONSTCOL	0x00010000	// R/W
+#define	SYS_VGA_BIT_BLIT_ACTIVE			0x80000000	// RO
+#define	SYS_VGA_ASCII					(SYS_IO_BASE + 0x30004)	// R/W
+#define	SYS_VGA_POSX					(SYS_IO_BASE + 0x30008)	// R/W
+#define	SYS_VGA_POSY					(SYS_IO_BASE + 0x3000C)	// R/W
+#define	SYS_VGA_CGCOL					(SYS_IO_BASE + 0x30010)	// R/W
+#define	SYS_VGA_RES						(SYS_IO_BASE + 0x30014)	// RO
+#define	SYS_VGA_FB_FRONT				(SYS_IO_BASE + 0x30018)	// R/W
+#define	SYS_VGA_FB_BACK					(SYS_IO_BASE + 0x3001C)	// R/W
+
+#define	SYS_VGA_BLIT_SRC0_ADDR			(SYS_IO_BASE + 0x30020)	// R/W
+#define	SYS_VGA_BLIT_SRC0_DIMX			(SYS_IO_BASE + 0x30024)	// R/W
+#define	SYS_VGA_BLIT_SRC1_ADDR			(SYS_IO_BASE + 0x30030)	// R/W
+#define	SYS_VGA_BLIT_SRC1_DIMX			(SYS_IO_BASE + 0x30034)	// R/W
+#define	SYS_VGA_BLIT_DST_ADDR			(SYS_IO_BASE + 0x30040)	// R/W
+#define	SYS_VGA_BLIT_DST_DIMX			(SYS_IO_BASE + 0x30044)	// R/W
+#define	SYS_VGA_BLIT_NX					(SYS_IO_BASE + 0x30050)	// R/W
+#define	SYS_VGA_BLIT_NY					(SYS_IO_BASE + 0x30054)	// R/W
+#define	SYS_VGA_BLIT_COLOR				(SYS_IO_BASE + 0x30058)	// R/W
+
+// AC'97
+#define	SYS_AC97_STAT					(SYS_IO_BASE + 0x40000)
+#define	SYS_AC97_CTRL					(SYS_IO_BASE + 0x40000)
+#define	SYS_AC97_ACSTAT					(SYS_IO_BASE + 0x40400)
+#define	SYS_AC97_ACCTRL					(SYS_IO_BASE + 0x40600)
+#define	SYS_AC97_PCM					(SYS_IO_BASE + 0x40800)
+#define	SYS_AC97_WAVIN					SYS_AC97_PCM
+#define	SYS_AC97_WAVOUT					SYS_AC97_PCM
+
+// Ethernet MAC
+#define	SYS_EMAC_RX_CTRL				(SYS_IO_BASE + 0x50000)
+#define	EMAC_RX_CTRL_RESET				(1 << 31)
+#define	EMAC_RX_CTRL_GIGABIT			(1 << 30)
+#define	EMAC_RX_CTRL_FCS_CHECK			(1 << 29)
+#define	EMAC_RX_CTRL_PROMISCIOUS		(1 << 23)
+#define	EMAC_RX_CTRL_PKT_REQ			(1 << 17)
+#define	EMAC_RX_CTRL_PKT_FREE			(1 << 16)
+#define	EMAC_RX_CTRL_INTEN				(1 << 4)
+
+#define	SYS_EMAC_RX_STAT				SYS_EMAC_RX_CTRL
+#define	EMAC_RX_STAT_RESET				(1 << 31)
+#define	EMAC_RX_STAT_GIGABIT			(1 << 30)
+#define	EMAC_RX_STAT_FCS_CHECK			(1 << 29)
+#define	EMAC_RX_STAT_PROMISCIOUS		(1 << 23)
+#define	EMAC_RX_STAT_PKT_BCAST			(1 << 19)
+#define	EMAC_RX_STAT_PKT_MACOK			(1 << 18)
+#define	EMAC_RX_STAT_PKT_VALID			(1 << 17)
+#define	EMAC_RX_STAT_PKT_AVAIL			(1 << 16)
+#define	EMAC_RX_STAT_INTEN				(1 << 4)
+
+#define	SYS_EMAC_RX_SIZE				(SYS_IO_BASE + 0x50004)
+
+#define	SYS_EMAC_TX_CTRL				(SYS_IO_BASE + 0x50008)
+#define	EMAC_TX_CTRL_RESET				(1 << 31)
+#define	EMAC_TX_CTRL_GIGABIT			(1 << 30)
+#define	EMAC_TX_CTRL_FCS_APPEND			(1 << 29)
+#define	EMAC_TX_CTRL_PKT_ALLOC			(1 << 17)
+#define	EMAC_TX_CTRL_PKT_COMMIT			(1 << 16)
+#define	EMAC_TX_CTRL_INTEN				(1 << 4)
+
+#define	SYS_EMAC_TX_STAT				SYS_EMAC_TX_CTRL
+#define	EMAC_TX_STAT_RESET				(1 << 31)
+#define	EMAC_TX_STAT_GIGABIT			(1 << 30)
+#define	EMAC_TX_STAT_FCS_APPEND			(1 << 29)
+#define	EMAC_TX_STAT_PKT_DONE			(1 << 24)
+#define	EMAC_TX_STAT_PKT_ALLOC_BSY		(1 << 17)
+#define	EMAC_TX_STAT_PKT_ARMED			(1 << 16)
+#define	EMAC_TX_STAT_INTEN				(1 << 4)
+
+#define	SYS_EMAC_TX_SIZE				(SYS_IO_BASE + 0x5000C)
+#define	SYS_EMAC_MADDR					(SYS_IO_BASE + 0x50018)
+#define	SYS_EMAC_MADDR_0				SYS_EMAC_MADDR_0
+#define	SYS_EMAC_MADDR_1				(SYS_IO_BASE + 0x5001C)
+
+#define	SYS_EMAC_DATA					(SYS_IO_BASE + 0x58000)
+#define	SYS_EMAC_RX_DATA				SYS_EMAC_DATA
+#define	SYS_EMAC_TX_DATA				SYS_EMAC_DATA
+#define	SYS_EMAC_DATA_CACHED			(SYS_IO_BASE + 0x10008000)
+
+// Sync. SRAM
+#define	SYS_SSRAM_IO					0xA8000000
+
+// ---------------------------------------------------------
+// Chip registers
+// ---------------------------------------------------------
+// COP0 registers
+#define COP0_REG_INVAT_POINTER			31
+
+// COP0 instructions
+#define COP0_INSTR_INVALL_ICACHE		32
+#define COP0_INSTR_INVAT_ICACHE			33
+#define COP0_INSTR_INVALL_DCACHE		34
+#define COP0_INSTR_INVAT_DCACHE			35
+
+/*
+void ICACHE_invalidate_all(void)
+{
+	__asm__
+	(
+		"cop0	32\n"
+	);
+}
+
+void ICACHE_invalidate_at(UINT32* pPtr)
+{
+	__asm__
+	(
+		"mtc0   %[pPtr], $31\n"
+		"cop0	33\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+
+void DCACHE_invalidate_all(void)
+{
+	__asm__
+	(
+		"cop0	34\n"
+	);
+}
+
+void DCACHE_invalidate_at(UINT32* pPtr)
+{
+	__asm__
+	(
+		"mtc0   %[pPtr], $31\n"
+		"cop0	35\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+*/
+
+#define CP0_read(idx) \
+({									\
+	UINT32 result;					\
+	__asm__	__volatile__			\
+	(								\
+		"mfc0\t%0,$"#idx"\n"		\
+		: "=r" (result)				\
+	);								\
+	result;							\
+})
+
+#define CP0_write(idx, val) \
+({									\
+	__asm__	__volatile__			\
+	(								\
+		"mtc0\t%0,$"#idx"\n"		\
+		"nop\n"						\
+		: /* no output */			\
+		: "r" (val)					\
+	);								\
+})
+
+#define CP0_TLBR					 \
+({									\
+	__asm__	__volatile__			\
+	(								\
+		"tlbr\n"					\
+		: /* no output */			\
+		: /* no input */			\
+	);								\
+})
+
+#define CP0_TLBWI					 \
+({									\
+	__asm__	__volatile__			\
+	(								\
+		"tlbwi\n"					\
+		: /* no output */			\
+		: /* no input */			\
+	);								\
+})
+
+UINT32 CP0_SR_read(void);
+void CP0_SR_write(UINT32 val);
+UINT32 CP0_CR_read(void);
+void CP0_CR_write(UINT32 val);
+UINT32 CP0_PRID_read(void);
+UINT32 CP0_TR_read(void);
+void CP0_TR_write(UINT32 val);
+void CP0_TR_write_ptr(UINT32* pPtr);
+void CP0_TR_read_ptr(UINT32* pPtr);
+void ICACHE_invalidate_all(void);
+void ICACHE_invalidate_at(UINT32* pPtr);
+void DCACHE_invalidate_all(void);
+void DCACHE_invalidate_at(UINT32* pPtr);
+
+#define CALC_BAUD(b) \
+	((UINT32)((float)CPU_FREQ_HZ/(16*b) + 0.5f) - 1);
+
+#define	UART0_setbaud(b) \
+	*((UINT32*)SYS_UART0_BAUD) = CALC_BAUD(b);
+
+#define	UART1_setbaud(b) \
+	*((UINT32*)SYS_UART1_BAUD) = CALC_BAUD(b);
+
+#define	UART2_setbaud(b) \
+	*((UINT32*)SYS_UART2_BAUD) = CALC_BAUD(b);
+
+char readchar(UINT32 port);
+void writechar(UINT32 port, char c);
+int write(int file, char *ptr, int len);
+int sputs(char *pStr);
+void print_byte(char byte);
+void print_word(int word);
+void _exit (int exitcode);
+void sleep(unsigned ms);
+void usleep(unsigned us);
+
+char _getchar(void);
+
+void PrintBuffer8(UINT8 *pBuf, int nbpr, int len);
+void memdump(UINT8 *pBuf, int print_offset_only, int num_bytes_per_row, int len);
+
+void Screen_clr(void);
+void Screen_line_clr(void);
+void Screen_csr_set_x(UINT32 coord);
+void Screen_csr_set_y(UINT32 coord);
+UINT32 Screen_get_resx(void);
+UINT32 Screen_get_resy(void);
+UINT32 Screen_get_fps(void);
+
+
+
+#endif // LIBSYS_H
+
@@ -0,0 +1,738 @@
+#include <sys/times.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "libsys.h"
+
+
+// ---------------------------------------------------------------------------------
+// Forward declarations
+// ---------------------------------------------------------------------------------
+void flush(UINT32 port);
+UINT32 GetPort(int file);
+
+// ---------------------------------------------------------------------------------
+// Types
+// ---------------------------------------------------------------------------------
+void libsys_init(void)
+{
+	UINT32 volatile *pITIM_ctrl = (UINT32*)SYS_ITIM_CTRL;
+
+	// Disable timers
+	*pITIM_ctrl = 0;
+
+	// Initialzie syscall support
+	syscalls_init();
+
+	// Initialzie interrupt support
+	interrupt_init();
+
+	// flush stdin
+	flush(GetPort(0));
+
+	// Do some initializations here
+
+}
+
+enum
+{
+	PORT_IN_UART_0 = 0,
+	PORT_IN_UART_1
+};
+
+enum
+{
+	PORT_OUT_UART0 = 0,
+	PORT_OUT_UART1
+};
+
+
+UINT32 GetPort(int file)
+{
+	if (file == 0)		// STDIN
+		return PORT_IN_UART_0;
+
+	if (file == 1)		// STDOUT
+		return PORT_OUT_UART0;
+
+	if (file == 2)		// STDERR
+		return PORT_OUT_UART0;
+
+	if (file == 10)		// AUX0
+		return PORT_IN_UART_1;
+
+	if (file == 11)		// AUX1
+		return PORT_OUT_UART1;
+}
+
+enum
+{
+	IF_TYPE_INVALID = 0,
+	IF_TYPE_UART
+};
+
+typedef struct _suart_if_t
+{
+	volatile UINT32 *pCTRL;
+	volatile UINT32 *pDATA;
+	volatile UINT32 *pBAUD;
+} uart_if_t;
+
+typedef struct _scon_if_in_t
+{
+	UINT32 type;
+	void *pIF;
+} con_if_in_t;
+
+typedef struct _scon_if_out_t
+{
+	UINT32 type;
+	void *pIF;
+} con_if_out_t;
+
+// ---------------------------------------------------------------------------------
+// Globals
+// ---------------------------------------------------------------------------------
+static uart_if_t uart_if[2] =
+{
+	{(UINT32*)SYS_UART0_STAT, (UINT32*)SYS_UART0_DATA, (UINT32*)SYS_UART0_BAUD},
+	{(UINT32*)SYS_UART1_STAT, (UINT32*)SYS_UART1_DATA, (UINT32*)SYS_UART1_BAUD}
+};
+
+static con_if_in_t con_if_in[] =
+{
+	{IF_TYPE_UART, &uart_if[0]},
+	{IF_TYPE_UART, &uart_if[1]},
+	{IF_TYPE_INVALID, NULL}
+};
+
+static con_if_out_t con_if_out[] =
+{
+	{IF_TYPE_UART, &uart_if[0]},
+	{IF_TYPE_UART, &uart_if[1]},
+	{IF_TYPE_INVALID, NULL}
+};
+
+// ---------------------------------------------------------------------------------
+// MIPS specific
+// ---------------------------------------------------------------------------------
+
+UINT32 CP0_SR_read(void)
+{
+	UINT32 result;
+
+	__asm__
+	(
+		"mfc0   %[val], $12\n"
+		: [val] "=r" (result)
+	);
+
+	return result;
+}
+
+void CP0_SR_write(UINT32 val)
+{
+	__asm__
+	(
+		"mtc0   %[val], $12\n"
+		: /* no output */
+		: [val] "r" (val)
+	);
+}
+
+UINT32 CP0_CR_read(void)
+{
+	UINT32 result;
+
+	__asm__
+	(
+		"mfc0   %[val], $13\n"
+		: [val] "=r" (result)
+	);
+
+	return result;
+}
+
+void CP0_CR_write(UINT32 val)
+{
+	__asm__
+	(
+		"mtc0   %[val], $13\n"
+		:
+		: [val] "r" (val)
+	);
+}
+
+UINT32 CP0_TR_read(void)
+{
+	UINT32 result;
+
+	__asm__
+	(
+		"mfc0   %[val], $31\n"
+		: [val] "=r" (result)
+	);
+
+	return result;
+}
+
+void CP0_TR_write(UINT32 val)
+{
+	__asm__
+	(
+		"mtc0   %[val], $31\n"
+		:
+		: [val] "r" (val)
+	);
+}
+
+UINT32 CP0_PRID_read(void)
+{
+	UINT32 result;
+
+	__asm__
+	(
+		"mfc0   %[val], $15\n"
+		: [val] "=r" (result)
+	);
+
+	return result;
+}
+
+void CP0_TR_write_ptr(UINT32* pPtr)
+{
+	__asm__
+	(
+		"swc0   $31, 0(%[pPtr])\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+
+void CP0_TR_read_ptr(UINT32* pPtr)
+{
+	__asm__
+	(
+		"lwc0   $31, 0(%[pPtr])\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+
+void ICACHE_invalidate_all(void)
+{
+	__asm__
+	(
+		"cop0	32\n"
+	);
+}
+
+void ICACHE_invalidate_at(UINT32* pPtr)
+{
+	__asm__
+	(
+		"mtc0   %[pPtr], $31\n"
+		"cop0	33\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+
+void DCACHE_invalidate_all(void)
+{
+	__asm__
+	(
+		"cop0	34\n"
+	);
+}
+
+void DCACHE_invalidate_at(UINT32* pPtr)
+{
+	__asm__
+	(
+		"mtc0   %[pPtr], $31\n"
+		"cop0	35\n"
+		:
+		: [pPtr] "r" (pPtr)
+	);
+}
+
+// ------------------------------------
+// Low-level I/O
+// ------------------------------------
+int UART_readchar(uart_if_t *pIF)
+{
+	if (!pIF)
+		return -1;
+
+	if (SYS_UART_BIT_RX_AVAIL & *pIF->pCTRL)
+		return (*pIF->pDATA & 0xFF);
+
+	return -1;
+}
+
+void UART_writechar(uart_if_t *pIF, char c)
+{
+	if (!pIF)
+		return;
+
+	while((SYS_UART_BIT_TX_HALFFULL & *pIF->pCTRL) != 0);
+
+	*pIF->pDATA = (UINT32)c;
+}
+
+// ---------------------------------------------------------------------------------
+void flush(UINT32 port)
+{
+	int c;
+	con_if_in_t *pIF;
+
+	pIF = &con_if_in[port];
+
+	do
+	{
+		switch(pIF->type)
+		{
+			case IF_TYPE_UART:
+			c = UART_readchar((uart_if_t*)pIF->pIF);
+			break;
+
+			default:
+			break;
+		}
+
+	} while(c > 0);
+}
+
+char readchar(UINT32 port)
+{
+	int c;
+	con_if_in_t *pIF;
+
+	pIF = &con_if_in[port];
+
+	do
+	{
+		switch(pIF->type)
+		{
+			case IF_TYPE_UART:
+			c = UART_readchar((uart_if_t*)pIF->pIF);
+			break;
+
+			default:
+			break;
+		}
+
+	} while(c < 0);
+
+	return (char)c;
+}
+
+void writechar(UINT32 port, char c)
+{
+	con_if_out_t *pIF;
+
+	pIF = &con_if_out[port];
+
+	switch(pIF->type)
+	{
+		case IF_TYPE_UART:
+		UART_writechar((uart_if_t*)pIF->pIF, c);
+		break;
+
+		default:
+		break;
+	}
+}
+
+void _putchar(UINT32 port, char c)
+{
+
+	con_if_out_t *pIF;
+
+	pIF = &con_if_out[port];
+
+	switch(pIF->type)
+	{
+		case IF_TYPE_UART:
+		if (c == 0x0A)
+			UART_writechar((uart_if_t*)pIF->pIF, 0x0D);
+		UART_writechar((uart_if_t*)pIF->pIF, c);
+		break;
+
+		default:
+		break;
+	}
+}
+
+char _getchar(void)
+{
+	return readchar(GetPort(0));
+}
+
+void _exit (int exitcode)
+{
+	fflush(stdout);
+	fflush(stderr);
+	while(1);
+}
+
+void sleep(unsigned ms)
+{
+	unsigned stop;
+	struct tms t;
+
+	times(&t);
+	stop = t.tms_utime + ms;
+
+	do
+	{
+		times(&t);
+	} while (t.tms_utime < stop);
+
+}
+
+void usleep(unsigned us)
+{
+	unsigned stop_us, stop_s, curr_us, curr_s;
+	struct timeval t;
+
+	if (!us)
+		return;
+
+	gettimeofday(&t, NULL);
+	stop_s = (unsigned)t.tv_sec;
+	stop_us = (unsigned)t.tv_usec + us;
+	if (stop_us >= 1E6)
+	{
+		stop_us -= 1E6;
+		stop_s++;
+	}
+
+	do
+	{
+		gettimeofday(&t, NULL);
+		curr_s = (unsigned)t.tv_sec;
+		curr_us = (unsigned)t.tv_usec;
+	} while ((curr_s < stop_s) || (curr_us < stop_us));
+
+}
+
+int getrusage(int who, struct rusage *usage)
+{
+	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
+	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
+
+	// who: RUSAGE_SELF or RUSAGE_CHILDREN
+	usage->ru_utime.tv_usec = *pReg_usec;
+	usage->ru_utime.tv_sec = *pReg_sec;
+	usage->ru_stime.tv_usec = *pReg_usec;
+	usage->ru_stime.tv_sec = *pReg_sec;
+}
+
+clock_t times(struct tms *buffer)
+{
+	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
+	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
+	long sec;
+	long usec;
+
+	sec = *pReg_sec;
+	usec = *pReg_usec;
+
+	if (buffer)
+	{
+		buffer->tms_utime = sec*1000 + usec/1000;
+		buffer->tms_stime = 0;
+		buffer->tms_cutime = 0;
+		buffer->tms_cstime = 0;
+	}
+
+	return (clock_t)sec;
+}
+
+int gettimeofday(struct timeval *tp, void *tzp)
+{
+	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
+	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
+
+	if (tp)
+	{
+		tp->tv_sec = *pReg_sec;
+		tp->tv_usec = *pReg_usec;
+	}
+
+	return 0;
+}
+
+int settimeofday(const struct timeval *tp, const struct timezone *tzp)
+{
+	volatile long *pReg_usec = (long*)SYS_TIMER_USEC;
+	volatile long *pReg_sec = (long*)SYS_TIMER_SEC;
+	div_t res;
+
+	res = div(tp->tv_usec, 1E6);
+
+	if (tp)
+	{
+		*pReg_usec = res.rem;
+		*pReg_sec  = tp->tv_sec + res.quot;
+	}
+
+	return 0;
+}
+
+caddr_t sbrk(int incr)
+{
+	extern char end;
+	extern char stack_ptr;
+	static char *heap_end;
+	char *prev_heap_end;
+
+	if (heap_end == 0)
+	{
+		heap_end = &end;
+	}
+
+	prev_heap_end = heap_end;
+	if (heap_end + incr > &stack_ptr)
+	{
+//		sputs("Heap and stack collision\n");
+//		sputs("Stack Ptr = ");print_word(&stack_ptr);sputs("\n");
+//		sputs("Heap end  = ");print_word(heap_end);sputs("\n");
+//		sputs("Increment = ");print_word(incr);sputs("\n");
+		exit(1);
+	}
+	heap_end += incr;
+	return (caddr_t) prev_heap_end;
+}
+
+int fstat(int file, struct stat *st)
+{
+//	sputs("fstat ("); print_word(file); sputs(")\n");
+	st->st_mode = S_IFCHR;
+	st->st_blksize = 0;
+	return 0;
+}
+
+int lseek(int file, int ptr, int dir)
+{
+//	sputs("lseek ("); print_word(file); sputs(")\n");
+	errno = ESPIPE;
+	return -1;
+}
+
+int open(const char *name, int flags, int mode)
+{
+//	sputs("open (\""); sputs(name); sputs("\")\n");
+	errno = EIO;
+	return -1;
+}
+
+int close(int file)
+{
+//	sputs("close ("); print_word(file); sputs(")\n");
+	return 0;
+}
+
+int read(int file, char *ptr, int len)
+{
+	UINT32 port;
+	int i;
+	char c;
+
+//	sputs("read (file="); print_word(file); sputs(", len ="); print_word(len); sputs(")\n");
+	i = 0;
+	port = GetPort(file);
+
+	while (i < len)
+	{
+		c = readchar(port);
+
+		if (c == 0x04)
+			break;
+
+		if ((c == 0x0D) || (c == 0x0A))
+		{
+			writechar(port, 0x0D);
+			ptr[i++] = 0x0D;
+			writechar(port, 0x0A);
+			ptr[i++] = 0x0A;
+			break;
+		}
+
+		writechar(port, c);
+		ptr[i++] = c;
+
+	}
+	return i;
+}
+
+int write(int file, char *ptr, int len)
+{
+	UINT32 port;
+	int i = 0;
+
+//	sputs("write ("); print_word(file); sputs(")\n");
+
+	port = GetPort(file);
+	for (i=0; i < len; i++)
+		_putchar(port, *ptr++);
+
+	return i;
+}
+
+int isatty(int file)
+{
+//	sputs("isatty ("); print_word(file); sputs(")\n");
+	return 1;
+}
+
+int getpid(void)
+{
+	return 1;
+}
+
+#include <errno.h>
+#undef errno
+extern int errno;
+int kill(int pid, int sig)
+{
+	errno = EINVAL;
+	return -1;
+}
+
+#include <errno.h>
+#undef errno
+extern int errno;
+int link(char *old, char *new)
+{
+	errno = EMLINK;
+	return -1;
+}
+
+#include <errno.h>
+#undef errno
+extern int errno;
+int unlink(char *name)
+{
+	errno = ENOENT;
+	return -1;
+}
+
+int sputs(char *pStr)
+{
+	char *start;
+	start = pStr;
+
+	while(*pStr)
+		_putchar(GetPort(1), *(pStr++));
+
+	return pStr - start;
+}
+
+void print_byte(char byte)
+{
+	int i;
+	unsigned char c, nibble;
+
+	for (i=0; i < 2; i++)
+	{
+		nibble = (char)((byte >> 4) & 0xF);
+		byte <<= 4;
+
+		if (nibble < 10)
+			c = nibble + '0';
+		else
+			c = nibble + 'A' - 10;
+
+		_putchar(GetPort(1), c);
+	}
+}
+
+void print_word(int word)
+{
+	int i;
+	unsigned char c, nibble;
+
+	for (i=0; i < 4; i++)
+	{
+		c = (char) (word >> 24);
+		print_byte(c);
+		word <<= 8;
+	}
+}
+
+// ---------------------------------------------------------------------------------
+// PrintBuffer8()
+// Prints byte buffer as hex and ascii interpretation
+// ---------------------------------------------------------------------------------
+// _Parameters :
+// pBuf:       : IN: Buffer to display
+// nbpr        : Number of bytes per row to display
+// len         : Length of input buffer
+// _Return:      none
+//
+// ---------------------------------------------------------------------------
+void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
+{
+	memdump(pBuf, 1, nbpr, len);
+}
+
+void memdump(UINT8 *pBuf, int print_offset_only, int num_bytes_per_row, int len)
+{
+    int i, j, cnt_hex, cnt_asc, base;
+    unsigned char c;
+
+    i = j = 0;
+    cnt_hex = len;
+    cnt_asc = len;
+    base = 0;
+	if (!print_offset_only)
+		base = (int)pBuf;
+
+    do
+    {
+        print_word(base + i);
+        sputs(": ");
+        for (j=0; j < num_bytes_per_row; j++)
+        {
+            if (cnt_hex)
+            {
+    		    print_byte(pBuf[i+j]);
+		        sputs(" ");
+                cnt_hex--;
+            }
+            else
+            {
+                sputs("   ");
+            }
+
+        }
+
+        sputs("   ");
+        for (j=0; j < num_bytes_per_row; j++)
+        {
+            if (cnt_asc)
+            {
+                c = pBuf[i+j];
+                if((c < 0x20) || (c > 0x7F))
+                    c = '.';
+
+                _putchar(GetPort(1), c);
+                cnt_asc--;
+            }
+            else
+            {
+                sputs(" ");
+            }
+        }
+        sputs("\n");
+        i += num_bytes_per_row;
+
+    } while (cnt_hex);
+}
+
@@ -0,0 +1,495 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "irq.h"
+#include "libsys.h"
+#include "mips_dis.h"
+
+#define	MAX_USER_BP				16
+#define	BP_MASK_ID				0x00FFF
+#define	BP_MASK_TYPE			0xFF000
+
+#define	BP_GET_ID(bp)			((bp >> 6) & 0xFFF)
+
+#define	BP_USER_BASE			0x10000
+#define	BP_USER(id)				(((BP_USER_BASE + id) << 6) | 0x0D)
+
+#define	BP_MGMT_BASE_SS			0x20000
+#define	BP_MGMT_SS(id)			(((BP_MGMT_BASE_SS + id) << 6) | 0x0D)
+
+#define	BP_MGMT_BASE_RU			0x30000
+#define	BP_MGMT_RU(id)			(((BP_MGMT_BASE_RU + id) << 6) | 0x0D)
+
+static char g_buf[80];
+
+enum
+{
+	STATE_INIT = 0,
+	STATE_RUN,
+	STATE_STEP_SINGLE,
+	STATE_STEP_PROC,
+	STATE_NUM_ENTRIES
+};
+
+static int g_state;
+static char *g_state_names[STATE_NUM_ENTRIES] = {"Init", "Run", "Single step", "Precedure step"};
+
+
+typedef struct _sbp_t
+{
+	UINT32 *pAddr;
+	UINT32 instr;
+} bp_t;
+
+static EXCEPTION_CONTEXT xcp_last;
+static bp_t user_bp[MAX_USER_BP];
+static bp_t mgmt_bp_ss[2];
+static bp_t mgmt_bp_ru[2];
+static int g_is_ss;
+static int g_bp_count;
+
+int IsBreak(UINT32 instr)
+{
+	return ((instr & 0xFC00003F) == 0x0000000D);
+}
+
+UINT32 BreakGetType(UINT32 instr)
+{
+	return (instr >> 6) & BP_MASK_TYPE;
+}
+
+char* reg_changed(UINT32 reg1, UINT32 reg2)
+{
+	static char chg_str[2] = {0};
+
+	if (reg1 != reg2)
+		chg_str[0] = '*';
+	else
+		chg_str[0] = ' ';
+
+	return chg_str;
+}
+
+void set_mgmt_break(UINT32 *pAddr_break)
+{
+	mgmt_bp_ss[0].pAddr = NULL;
+	mgmt_bp_ss[0].pAddr = 0;
+
+	// Don't overwrite user breaks
+	if (IsBreak(*pAddr_break) && (BreakGetType(*pAddr_break) == BP_USER_BASE))
+	{
+//		sputs("Instruction at address ");print_word((long)pAddr_break);sputs(" is user break\n");
+	}
+	else
+	{
+		// Save original instruction
+		mgmt_bp_ss[0].pAddr = pAddr_break;
+		mgmt_bp_ss[0].instr = *(mgmt_bp_ss[0].pAddr);
+		// Replace instruction with managment breakpoint
+		*(mgmt_bp_ss[0].pAddr) = BP_MGMT_SS(1);
+		ICACHE_invalidate_at(mgmt_bp_ss[0].pAddr);
+	}
+}
+
+void singlestep(struct xcptcontext * xcp, UINT32 *pAddr_curr, UINT32 is_branch_shadow)
+{
+
+    int junk;
+ 	UINT32 branch_addr, reg, result, bp_type;
+	UINT32 *pInstr, *pAddr_prev, *pAddr_next;
+
+	pAddr_prev = pAddr_curr - 1;
+	pAddr_next = pAddr_curr + 1;
+
+	set_mgmt_break(pAddr_next);
+
+	mgmt_bp_ss[1].pAddr = NULL;
+	mgmt_bp_ss[1].pAddr = 0;
+
+	// We have two possible management breaks, if previous instruction was branch or jump
+	if (is_branch_shadow)
+	{
+		result = tdisasm(g_buf, (long)pAddr_prev, *pAddr_prev, 0, &junk, &branch_addr, &reg);
+		// Is jump target stored in register
+		if (result == 3)
+		{
+			branch_addr = xcp->regs[reg&0x1F];
+		}
+
+	// Don't overwrite user breaks
+		if (IsBreak(branch_addr) && (BreakGetType(branch_addr) == BP_USER_BASE))
+		{
+//			sputs("Instruction at branch address ");print_word((long)branch_addr);sputs(" is user break\n");
+		}
+		else
+		{
+			// Save original instruction
+			mgmt_bp_ss[1].pAddr = (UINT32*)branch_addr;
+			mgmt_bp_ss[1].instr = *(mgmt_bp_ss[1].pAddr);
+			// Replace instruction with managment breakpoint
+			*(mgmt_bp_ss[1].pAddr) = BP_MGMT_SS(2);
+			ICACHE_invalidate_at(mgmt_bp_ss[1].pAddr);
+		}
+	}
+}
+
+void dbg_handler(struct xcptcontext * xcp)
+{
+
+ 	UINT32 bp_addr, bp_index = 0, branch_addr, reg, result, bp_type;
+    int junk, i, leave_isr, is_branch_shadow, is_user_bp;
+	UINT32 *pInstr, *pAddr_curr, *pAddr_prev, *pAddr_next;
+	instr_info_t info;
+
+	is_user_bp = 0;
+	pAddr_curr = (UINT32*)xcp->epc;
+	is_branch_shadow = ((xcp->cr & 0x80000000) == 0x80000000);
+	if (is_branch_shadow)
+		pAddr_curr++;
+
+	pAddr_prev = pAddr_curr - 1;
+	pAddr_next = pAddr_curr + 1;
+
+	if (IsBreak(*pAddr_curr))
+	{
+		bp_type = BreakGetType(*pAddr_curr);
+
+		switch (bp_type)
+		{
+			case BP_MGMT_BASE_SS:
+
+			// Restore original instructions after single-step
+//			sputs("1. Restore after single-step at ");print_word((long)mgmt_bp_ss[0].pAddr);sputs("\n");
+			*(mgmt_bp_ss[0].pAddr) = mgmt_bp_ss[0].instr;
+			ICACHE_invalidate_at(mgmt_bp_ss[0].pAddr);
+			if (mgmt_bp_ss[1].pAddr)
+			{
+//				sputs("2. Restore after single-step at ");print_word((long)mgmt_bp_ss[1].pAddr);sputs("\n");
+				*(mgmt_bp_ss[1].pAddr) = mgmt_bp_ss[1].instr;
+				ICACHE_invalidate_at(mgmt_bp_ss[1].pAddr);
+			}
+
+			break;
+
+			case BP_MGMT_BASE_RU:
+
+			bp_index = BP_GET_ID(*pAddr_curr);
+//			sputs("Restore user break at ");print_word((long)user_bp[bp_index].pAddr);sputs("\n");
+
+			*(user_bp[bp_index].pAddr) = BP_USER(bp_index);
+			ICACHE_invalidate_at(user_bp[bp_index].pAddr);
+
+			*(mgmt_bp_ru[0].pAddr) = mgmt_bp_ru[0].instr;
+			ICACHE_invalidate_at(mgmt_bp_ru[0].pAddr);
+//			sputs("1. Restore after RU at ");print_word((long)mgmt_bp_ru[0].pAddr);sputs("\n");
+			if (mgmt_bp_ru[1].pAddr)
+			{
+				*(mgmt_bp_ru[1].pAddr) = mgmt_bp_ru[1].instr;
+				ICACHE_invalidate_at(mgmt_bp_ru[1].pAddr);
+//				sputs("2. Restore after RU at ");print_word((long)mgmt_bp_ru[1].pAddr);sputs("\n");
+			}
+			if (!g_is_ss)
+				return;
+
+			break;
+
+			case BP_USER_BASE:
+
+			is_user_bp = 1;
+			bp_index = BP_GET_ID(*pAddr_curr);
+//			sputs("Restore instruction at ");print_word((long)user_bp[bp_index].pAddr);sputs("\n");
+			*(user_bp[bp_index].pAddr) = user_bp[bp_index].instr;
+			ICACHE_invalidate_at(user_bp[bp_index].pAddr);
+			break;
+
+			default:
+//			sputs("Found ordinary break at ");print_word((long)pAddr_curr);sputs("\n");
+			break;
+
+		}
+	}
+
+	sputs("\n");
+	PRINT_REG_CHG(" Status : ", xcp->sr, reg_changed(xcp->sr, xcp_last.sr));
+	PRINT_REG_CHG("  Cause : ", xcp->cr, reg_changed(xcp->cr, xcp_last.cr));
+	PRINT_REG_CHG("    EPC : ", xcp->epc, reg_changed(xcp->epc, xcp_last.epc));
+	PRINT_REG_CHG("BadAddr : ", xcp->baddr, reg_changed(xcp->baddr, xcp_last.baddr));
+	sputs("\n");
+	PRINT_REG_CHG("   MDLO : ", xcp->mdlo, reg_changed(xcp->mdlo, xcp_last.mdlo));
+	PRINT_REG_CHG("   MDHI : ", xcp->mdhi, reg_changed(xcp->mdhi, xcp_last.mdhi));
+
+	sputs("\n");
+
+	sputs("Registers:\n");
+	PRINT_REG_CHG(" 0 (ze) : ", xcp->regs[0], reg_changed(xcp->regs[0], xcp_last.regs[0]));
+	PRINT_REG_CHG(" 1 (at) : ", xcp->regs[1], reg_changed(xcp->regs[1], xcp_last.regs[1]));
+	PRINT_REG_CHG(" 2 (v0) : ", xcp->regs[2], reg_changed(xcp->regs[2], xcp_last.regs[2]));
+	PRINT_REG_CHG(" 3 (v1) : ", xcp->regs[3], reg_changed(xcp->regs[3], xcp_last.regs[3]));
+	sputs("\n");
+	PRINT_REG_CHG(" 4 (a0) : ", xcp->regs[4], reg_changed(xcp->regs[4], xcp_last.regs[4]));
+	PRINT_REG_CHG(" 5 (a1) : ", xcp->regs[5], reg_changed(xcp->regs[5], xcp_last.regs[5]));
+	PRINT_REG_CHG(" 6 (a2) : ", xcp->regs[6], reg_changed(xcp->regs[6], xcp_last.regs[6]));
+	PRINT_REG_CHG(" 7 (a3) : ", xcp->regs[7], reg_changed(xcp->regs[7], xcp_last.regs[7]));
+	sputs("\n");
+	PRINT_REG_CHG(" 8 (t0) : ", xcp->regs[8], reg_changed(xcp->regs[8], xcp_last.regs[8]));
+	PRINT_REG_CHG(" 9 (t1) : ", xcp->regs[9], reg_changed(xcp->regs[9], xcp_last.regs[9]));
+	PRINT_REG_CHG("10 (t2) : ", xcp->regs[10], reg_changed(xcp->regs[10], xcp_last.regs[10]));
+	PRINT_REG_CHG("11 (t3) : ", xcp->regs[11], reg_changed(xcp->regs[11], xcp_last.regs[11]));
+	sputs("\n");
+	PRINT_REG_CHG("12 (t4) : ", xcp->regs[12], reg_changed(xcp->regs[12], xcp_last.regs[12]));
+	PRINT_REG_CHG("13 (t5) : ", xcp->regs[13], reg_changed(xcp->regs[13], xcp_last.regs[13]));
+	PRINT_REG_CHG("14 (t6) : ", xcp->regs[14], reg_changed(xcp->regs[14], xcp_last.regs[14]));
+	PRINT_REG_CHG("15 (t7) : ", xcp->regs[15], reg_changed(xcp->regs[15], xcp_last.regs[15]));
+	sputs("\n");
+	PRINT_REG_CHG("16 (s0) : ", xcp->regs[16], reg_changed(xcp->regs[16], xcp_last.regs[16]));
+	PRINT_REG_CHG("17 (s1) : ", xcp->regs[17], reg_changed(xcp->regs[17], xcp_last.regs[17]));
+	PRINT_REG_CHG("18 (s2) : ", xcp->regs[18], reg_changed(xcp->regs[18], xcp_last.regs[18]));
+	PRINT_REG_CHG("19 (s3) : ", xcp->regs[19], reg_changed(xcp->regs[19], xcp_last.regs[19]));
+	sputs("\n");
+	PRINT_REG_CHG("20 (s4) : ", xcp->regs[20], reg_changed(xcp->regs[20], xcp_last.regs[20]));
+	PRINT_REG_CHG("21 (s5) : ", xcp->regs[21], reg_changed(xcp->regs[21], xcp_last.regs[21]));
+	PRINT_REG_CHG("22 (s6) : ", xcp->regs[22], reg_changed(xcp->regs[22], xcp_last.regs[22]));
+	PRINT_REG_CHG("23 (s7) : ", xcp->regs[23], reg_changed(xcp->regs[23], xcp_last.regs[23]));
+	sputs("\n");
+	PRINT_REG_CHG("24 (s8) : ", xcp->regs[24], reg_changed(xcp->regs[24], xcp_last.regs[24]));
+	PRINT_REG_CHG("25 (s9) : ", xcp->regs[25], reg_changed(xcp->regs[25], xcp_last.regs[25]));
+	PRINT_REG_CHG("26 (k0) : ", xcp->regs[26], reg_changed(xcp->regs[26], xcp_last.regs[26]));
+	PRINT_REG_CHG("27 (k1) : ", xcp->regs[27], reg_changed(xcp->regs[27], xcp_last.regs[27]));
+	sputs("\n");
+	PRINT_REG_CHG("28 (gp) : ", xcp->regs[28], reg_changed(xcp->regs[28], xcp_last.regs[28]));
+	PRINT_REG_CHG("29 (sp) : ", xcp->regs[29], reg_changed(xcp->regs[29], xcp_last.regs[29]));
+	PRINT_REG_CHG("30 (fp) : ", xcp->regs[30], reg_changed(xcp->regs[30], xcp_last.regs[30]));
+	PRINT_REG_CHG("31 (ra) : ", xcp->regs[31], reg_changed(xcp->regs[31], xcp_last.regs[31]));
+	sputs("\n");
+	sputs("\n");
+
+//	sputs(g_state_names[g_state]);sputs("\n");
+//	sputs("\n");
+
+	pInstr = pAddr_prev;
+	// Disassemble instructions
+    for (i=0; i < 3; i++)
+    {
+		print_word((long)pInstr);
+		if (pInstr != pAddr_curr)
+		{
+			if (IsBreak(*pInstr) && (BreakGetType(*pInstr) == BP_USER_BASE))
+			{
+				tdisasm(g_buf, (long)user_bp[BP_GET_ID(*pInstr)].pAddr, user_bp[BP_GET_ID(*pInstr)].instr, 0, &junk, &junk, &junk);
+				sputs(": b("); print_byte((char)BP_GET_ID(*pInstr)); sputs(")   ");
+			}
+			else
+			{
+				tdisasm(g_buf, (long)pInstr, *pInstr, 0, &junk, &junk, &junk);
+				sputs(":         ");
+			}
+		}
+		else
+		{
+			tdisasm(g_buf, (long)pInstr, *pInstr, 0, &junk, &junk, &junk);
+			if (is_user_bp)
+			{
+				sputs(": b("); print_byte((char)bp_index); sputs(")-> ");
+			}
+			else
+			{
+				sputs(":      -> ");
+			}
+		}
+		sputs(g_buf);
+		sputs("\n");
+		pInstr++;
+	}
+
+	if (is_user_bp)
+	{
+		// Save original instruction
+		mgmt_bp_ru[0].pAddr = pAddr_next;
+		mgmt_bp_ru[0].instr = *(mgmt_bp_ru[0].pAddr);
+		// Replace instruction with managment breakpoint
+		*(mgmt_bp_ru[0].pAddr) = BP_MGMT_RU(bp_index);
+		ICACHE_invalidate_at(mgmt_bp_ru[0].pAddr);
+		mgmt_bp_ru[1].pAddr = NULL;
+		mgmt_bp_ru[1].pAddr = 0;
+//		sputs("1. Insert RU-break at ");print_word((long)mgmt_bp_ru[0].pAddr);sputs("\n");
+
+		// We have two possible management breaks, if previous instruction was branch or jump
+		if (is_branch_shadow)
+		{
+			result = tdisasm(g_buf, (long)pAddr_prev, *pAddr_prev, 0, &junk, &branch_addr, &reg);
+			// Is jump target stored in register
+			if (result == 3)
+			{
+				branch_addr = xcp->regs[reg&0x1F];
+			}
+			// Save original instruction
+			mgmt_bp_ru[1].pAddr = (UINT32*)branch_addr;
+			mgmt_bp_ru[1].instr = *(mgmt_bp_ru[1].pAddr);
+			// Replace instruction with managment breakpoint
+			*(mgmt_bp_ru[1].pAddr) = BP_MGMT_RU(bp_index);
+			ICACHE_invalidate_at(mgmt_bp_ru[1].pAddr);
+//			sputs("2. Insert RU-break at ");print_word((long)mgmt_bp_ru[1].pAddr);sputs("\n");
+		}
+	}
+
+	do
+	{
+		leave_isr = 0;
+		switch(_getchar())
+		{
+			case 'g':
+			g_state = STATE_RUN;
+			leave_isr = 1;
+			g_is_ss = 0;
+			if (IsBreak(*pAddr_curr))
+			{
+				if (BreakGetType(*pAddr_curr) != BP_USER_BASE)
+				{
+					xcp->epc += 4;		// Skip ordinary program breaks
+				}
+			}
+			break;
+
+			case 'b':
+			printf("Enter breakpoint: ");
+			scanf("%x", &bp_addr);
+			printf("Insert breakpoint at %.8X\n", bp_addr);
+
+			user_bp[g_bp_count].pAddr = (UINT32*)bp_addr;
+			user_bp[g_bp_count].instr = *(user_bp[g_bp_count].pAddr);
+			*(user_bp[g_bp_count].pAddr) = BP_USER(g_bp_count);
+
+		    tdisasm(g_buf, (long)user_bp[g_bp_count].pAddr, user_bp[g_bp_count].instr, 0, &junk, &junk, &junk);
+			print_word((long)user_bp[g_bp_count].pAddr);sputs(": b   ");sputs(g_buf);sputs("\n");
+			ICACHE_invalidate_at(user_bp[g_bp_count].pAddr);
+			g_bp_count++;
+			break;
+
+			case 's':
+			g_state = STATE_STEP_SINGLE;
+			g_is_ss = 1;
+			leave_isr = 1;
+
+			if (is_user_bp)
+				break;
+
+			if (IsBreak(*pAddr_curr))
+			{
+				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
+				{
+					break;
+				}
+				else
+				{
+					xcp->epc += 4;		// Skip ordinary program breaks
+				}
+			}
+
+			singlestep(xcp, pAddr_curr, is_branch_shadow);
+			break;
+
+			case 'e':
+			g_state = STATE_STEP_SINGLE;
+			g_is_ss = 1;
+			leave_isr = 1;
+
+			if (is_user_bp)
+				break;
+
+			if (IsBreak(*pAddr_curr))
+			{
+				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
+				{
+					break;
+				}
+				else
+				{
+					xcp->epc += 4;		// Skip ordinary program breaks
+				}
+			}
+
+			pInstr = pAddr_curr;
+
+			do
+			{
+				result = instr_info(*pInstr, &info);
+				if (result == INSTR_TYPE_JUMP)
+				{
+					if (info.rs == 0x1F)
+						break;
+				}
+				else if (result == INSTR_TYPE_LOAD)
+				{
+					if (info.rs == 0x1F)
+						break;
+				}
+				pInstr++;
+			} while(1);
+
+//			sputs("Return found at "); print_word((long)pInstr);sputs("\n");
+			set_mgmt_break(pInstr);
+			break;
+
+			case 'p':
+			g_state = STATE_STEP_PROC;
+			g_is_ss = 1;
+			leave_isr = 1;
+
+			if (is_user_bp)
+				break;
+
+			if (IsBreak(*pAddr_curr))
+			{
+				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
+				{
+					break;
+				}
+				else
+				{
+					xcp->epc += 4;		// Skip ordinary program breaks
+				}
+			}
+
+			result = instr_info(*pAddr_curr, &info);
+			if ((result == INSTR_TYPE_JUMP) && (info.flags & INSTR_FLAGS_JUMP_LINK))
+			{
+				singlestep(xcp, pAddr_next, 0);
+//				sputs("Skip sub-routine\n");
+			}
+			else
+			{
+				singlestep(xcp, pAddr_curr, is_branch_shadow);
+			}
+			break;
+
+			default:
+			break;
+		}
+	} while (!leave_isr);
+
+	memcpy(&xcp_last, xcp, sizeof(EXCEPTION_CONTEXT));
+}
+
+void debug_int(struct xcptcontext * xcp)
+{
+	dbg_handler(xcp);
+
+}
+
+int debug_break(struct xcptcontext * xcp)
+{
+	dbg_handler(xcp);
+	return 0;
+
+}
+
+void dbg_init(void)
+{
+	memset(&xcp_last, 0, sizeof(EXCEPTION_CONTEXT));
+	interrupt_register(2, debug_int);
+	interrupt_enable(2);
+	g_state = STATE_INIT;
+
+	xcpt_register(EXC_BP, debug_break);
+}
+
@@ -0,0 +1,825 @@
+/* +----------------------------------------------------------------+ */
+/* |            Copyright (c) 1994 Stanford University.             | */
+/* |                      All Rights Reserved.                      | */
+/* |                                                                | */
+/* |   This software is distributed with *ABSOLUTELY NO SUPPORT*    | */
+/* |   and *NO WARRANTY*.   Use or reproduction of this code for    | */
+/* |   commerical gains is strictly prohibited.   Otherwise, you    | */
+/* |   are given permission to use or modify this code as long      | */
+/* |   as you do not remove this notice.                            | */
+/* +----------------------------------------------------------------+ */
+/* --------------------------------------------------- */
+/* | Copyright (c) 1986 MIPS Computer Systems, Inc.  | */
+/* | All Rights Reserved.                            | */
+/* --------------------------------------------------- */
+
+/*
+ * Copyright 1985 by MIPS Computer Systems, Inc.
+ */
+
+/* TORCH disassembler */
+/* derived from MIPS instruction dissassembler by PGL October 91 */
+
+#include <stdio.h>
+#include <string.h>
+#include "mips_dis.h"
+
+#define false 0
+#define true 1
+typedef int boolean;
+char *strcat();
+
+#define ZERO 0
+
+#define COMPILER_NAMES tdis_reg_names[0]
+#define HARDWARE_NAMES tdis_reg_names[1]
+#define ASSEMBLER_NAMES tdis_reg_names[2]
+#define DIS_REG_NAMES(x)	tdis_reg_names[x]
+
+union extension_byte {
+    unsigned char byte;
+    struct {	/* extension byte bits in little-endian order */
+#if (defined __MIPSEB || defined _MIPSEB || defined MIPSEB)
+	unsigned reserved:1;
+	unsigned dyn_nop:1;
+	unsigned rs_boost:2;
+	unsigned rt_boost:2;
+	unsigned rd_boost:2;
+#elif (defined __MIPSEL || defined _MIPSEL || defined MIPSEL)
+	unsigned rd_boost:2;
+	unsigned rt_boost:2;
+	unsigned rs_boost:2;
+	unsigned dyn_nop:1;
+	unsigned reserved:1;
+#endif /*MIPSEB*/
+    } e;
+};
+
+#define blez_op_T	blez_op
+#define blez_op_N	(blez_op | 0x10)
+#define bgtz_op_T	bgtz_op
+#define bgtz_op_N	(bgtz_op | 0x10)
+#define beq_op_T	beq_op
+#define beq_op_N	(beq_op | 0x10)
+#define bne_op_T	bne_op
+#define bne_op_N	(bne_op | 0x10)
+#define bc_op_T		bc_op
+#define bc_op_N		(bc_op | 0x02)
+
+static char *op_name[64] = {
+/* 0 */	"special",	"regimm","j",	"jal",	"beq",  "bne",  "blez", "bgtz",
+/* 8 */	"addi",	"addiu","slti",	"sltiu","andi",	"ori",	"xori",	"lui",
+/*16 */	"cop0",	"cop1",	"cop2",	"cop3",	"beql","bnel","blezl","bgtzl",
+/*24 */	"op60",	"op64",	"op68",	"op6c",	"op70",	"op74",	"op78",	"op7c",
+/*32 */	"lb",	"lh",	"lwl",	"lw",	"lbu",	"lhu",	"lwr",	"ld",
+/*40 */	"sb",	"sh",	"swl",	"sw",	"opb0",	"opb4",	"swr",	"sd",
+/*48 */	"lwc0",	"lwc1",	"lwc2",	"lwc3",	"ldc0",	"ldc1",	"ldc2",	"ldc3",
+/*56 */	"swc0",	"swc1",	"swc2",	"swc3",	"sdc0",	"sdc1",	"sdc2",	"sdc3"
+};
+
+static char *spec_name[64] = {
+/* 0*/ "sll",	"spec01","srl",	"sra",	"sllv",	"spec05","srlv","srav",
+/* 8*/ "jr",	"jalr",	"spec12","spec13","syscall","break","vcall","spec17",
+/*16*/ "mfhi",	"mthi",	"mflo",	"mtlo",	"spec24","spec25","spec26","spec27",
+/*24*/ "mult",	"multu","div",	"divu",	"spec34","spec35","spec36","spec37",
+/*32*/ "add",	"addu",	"sub",	"subu",	"and",	"or",	"xor",	"nor",
+/*40*/ "spec50","spec51","slt","sltu",	"spec54","spec55","spec56","spec57",
+/*48*/ "spec60","spec61","spec62","spec63","spec64","spec65","spec66","spec67",
+/*56*/ "spec70","spec71","spec72","spec73","spec74","spec75","spec76","spec77"
+};
+
+static char *bcond_name[32] = {
+	"bltz",
+	"bgez",
+	"bltzl",
+	"bgezl",
+	"bcond04",
+	"bcond05",
+	"bcond06",
+	"bcond07",
+	"tgei",
+	"tgeiu",
+	"tlti",
+	"tltiu",
+	"teqi",
+	"bcond0d",
+	"tnei",
+	"bcond0f",
+	"bltzal",
+	"bgezal",
+	"bltzall",
+	"bgezall",
+	"bcond14",
+	"bcond15",
+	"bcond16",
+	"bcond17",
+	"bcond18",
+	"bcond19",
+	"bcond1a",
+	"bcond1b",
+	"bcond1c",
+	"bcond1d",
+	"bcond1e",
+	"bcond1f"
+};
+
+static char *cop1_name[64] = {
+/* 0 */	"add",	"sub",	"mul",	"div",	"sqrt", "abs",	"mov",	"neg",
+/* 8 */	"fop08","fop09","fop0a","fop0b","fop0c","fop0d","fop0e","fop0f",
+/*16 */	"fop10","fop11","fop12","fop13","fop14","fop15","fop16","fop17",
+/*24 */	"fop18","fop19","fop1a","fop1b","fop1c","fop1d","fop1e","fop1f",
+/*32 */	"cvt.s","cvt.d","cvt.e","fop23","cvt.w","fop25","fop26","fop27",
+/*40 */	"fop28","fop29","fop2a","fop2b","fop2c","fop2d","fop2e","fop2f",
+/*48 */	"c.f", "c.un","c.eq","c.ueq","c.olt","c.ult","c.ole","c.ule",
+/*56 */	"c.sf","c.ngle","c.seq","c.ngl","c.lt","c.nge","c.le","c.ngt"
+};
+
+static char *fmt_name[16] = {
+	"s",	"d",	"e",	"q",
+	"w",	"fmt5",	"fmt6",	"fmt7",
+	"fmt8",	"fmt9",	"fmta",	"fmtb",
+	"fmtc",	"fmtd",	"fmte",	"fmtf"
+};
+
+/* public */
+/* Three sets of commonly used register names */
+const char *tdis_reg_names[3][64] = {
+    {   /* compiler names */
+	"zero",	"at",	"v0",	"v1",	"a0",	"a1",	"a2",	"a3",
+	"t0",	"t1",	"t2",	"t3",	"t4",	"t5",	"t6",	"t7",
+	"s0",	"s1",	"s2",	"s3",	"s4",	"s5",	"s6",	"s7",
+	"t8",	"t9",	"k0",	"k1",	"gp",	"sp",	"s8",	"ra",
+	"zero.B","at.B","v0.B",	"v1.B",	"a0.B",	"a1.B",	"a2.B",	"a3.B",
+	"t0.B",	"t1.B",	"t2.B",	"t3.B",	"t4.B",	"t5.B",	"t6.B",	"t7.B",
+	"s0.B",	"s1.B",	"s2.B",	"s3.B",	"s4.B",	"s5.B",	"s6.B",	"s7.B",
+	"t8.B",	"t9.B",	"k0.B",	"k1.B",	"gp.B",	"sp.B",	"s8.B",	"ra.B"
+    },
+    {	/* hardware names */
+	"r0",	"r1",	"r2",	"r3",	"r4",	"r5",	"r6",	"r7",
+	"r8",	"r9",	"r10",	"r11",	"r12",	"r13",	"r14",	"r15",
+	"r16",	"r17",	"r18",	"r19",	"r20",	"r21",	"r22",	"r23",
+	"r24",	"r25",	"r26",	"r27",	"gp",	"sp",	"r30",	"r31",
+	"r0.B",	"r1.B",	"r2.B",	"r3.B",	"r4.B",	"r5.B",	"r6.B",	"r7.B",
+	"r8.B",	"r9.B",	"r10.B","r11.B","r12.B","r13.B","r14.B","r15.B",
+	"r16.B","r17.B","r18.B","r19.B","r20.B","r21.B","r22.B","r23.B",
+	"r24.B","r25.B","r26.B","r27.B","gp.B",	"sp.B",	"r30.B","r31.B"
+    },
+    {	/* assembler names */
+	"$0",	"$at",	"$2",	"$3",	"$4",	"$5",	"$6",	"$7",
+	"$8",	"$9",	"$10",	"$11",	"$12",	"$13",	"$14",	"$15",
+	"$16",	"$17",	"$18",	"$19",	"$20",	"$21",	"$22",	"$23",
+	"$24",	"$25",	"$26",	"$27",	"$gp",	"$sp",	"$30",	"$31",
+	"$0.B",	"$at.B","$2.B",	"$3.B",	"$4.B",	"$5.B",	"$6.B",	"$7.B",
+	"$8.B",	"$9.B",	"$10.B","$11.B","$12.B","$13.B","$14.B","$15.B",
+	"$16.B","$17.B","$18.B","$19.B","$20.B","$21.B","$22.B","$23.B",
+	"$24.B","$25.B","$26.B","$27.B","$gp.B","$sp.B","$30.B","$31.B"
+    }
+};
+
+static char *c0_opname[64] = {
+    "c0op0","tlbr","tlbwi","c0op3","c0op4","c0op5","tlbwr","c0op7",
+    "tlbp","c0op9","c0op10","c0op11","c0op12","c0op13","c0op14","c0op15",
+    "rfe","c0op17","c0op18","c0op19","c0op20","c0op21","c0op22","c0op23",
+    "c0op24","c0op25","c0op26","c0op27","c0op28","c0op29","c0op30","c0op31",
+    "c0op32","c0op33","c0op34","c0op35","c0op36","c0op37","c0op38","c0op39",
+    "c0op40","c0op41","c0op42","c0op43","c0op44","c0op45","c0op46","c0op47",
+    "c0op48","c0op49","c0op50","c0op51","c0op52","c0op53","c0op54","c0op55",
+    "c0op56","c0op57","c0op58","c0op59","c0op60","c0op61","c0op62","c0op63"
+};
+
+static char *c0_reg[32] = {
+    "index","random","tlblo","c0r3","context","c0r5","c0r6","c0r7",
+    "badvaddr","c0r9","tlbhi","epcn","sr",	"cause","epc",	"c0r15",
+    "c0r16","c0r17","c0r18","c0r19","c0r20","c0r21","c0r22","c0r23",
+    "c0r24","c0r25","c0r26","c0r27","c0r28","c0r29","c0r30","c0r31"
+};
+
+/* Remember the options set by dis_init */
+//#define ADDR_DEFAULT "%#010x:\t"
+#define ADDR_DEFAULT 0
+//#define VALUE_DEFAULT "%#010x\t"
+#define VALUE_DEFAULT 0
+#define NAME_DEFAULT COMPILER_NAMES
+static struct {
+    char *addr_format;
+    char *value_format;
+    const char **reg_names;
+    int print_jal_targets;
+} save = {
+    ADDR_DEFAULT,
+    VALUE_DEFAULT,
+    NAME_DEFAULT,
+    true
+};
+
+/* Update regmask to reflect the use of this general-purpose (not fp)
+  register, and return its name */
+static const char *
+register_name(ireg, boosted, regmask)
+unsigned ireg, *regmask;
+int boosted;
+{
+    if (!boosted)
+	*regmask |= (1 << ireg);
+    return save.reg_names[boosted ? ireg + 32 : ireg];
+}
+
+int instr_info(unsigned iword, instr_info_t *pInfo)
+{
+	unsigned address, *regmask, *symbol_value, *ls_register;
+
+    int return_value = 0;
+    boolean do_b_displacement = false;
+    boolean do_loadstore = false;
+    union mips_instruction i;
+    union extension_byte e;
+
+    i.word = iword;
+    *regmask = *symbol_value = *ls_register = 0;
+
+    /* decode the instruction */
+    switch (i.j_format.opcode)
+    {
+
+		case spec_op:
+
+			if (i.word == 0x20)
+			{
+				pInfo->type = INSTR_TYPE_NOP;
+				break;
+
+			}
+			else if (i.r_format.func == addu_op && i.r_format.rt == ZERO)
+			{
+				pInfo->type = INSTR_TYPE_MOVE;
+				pInfo->rd = i.r_format.rd;
+				pInfo->rs = i.r_format.rs;
+				break;
+			}
+
+			switch (i.r_format.func)
+			{
+				case sll_op:
+				case srl_op:
+				case sra_op:
+					pInfo->type = INSTR_TYPE_ARITH;
+					pInfo->rd = i.r_format.rd;
+					pInfo->rt = i.r_format.rt;
+					pInfo->re = i.r_format.re;
+					break;
+				case sllv_op:
+				case srlv_op:
+				case srav_op:
+					pInfo->type = INSTR_TYPE_ARITH;
+					pInfo->rd = i.r_format.rd;
+					pInfo->rt = i.r_format.rt;
+					pInfo->rs = i.r_format.rs;
+					break;
+				case mfhi_op:
+				case mflo_op:
+					pInfo->type = INSTR_TYPE_ARITH;
+					pInfo->rd = i.r_format.rd;
+					break;
+				case jalr_op:
+					pInfo->type = INSTR_TYPE_JUMP;
+					pInfo->flags = INSTR_FLAGS_JUMP_LINK | INSTR_FLAGS_JUMP_REGISTER;
+					pInfo->rd = i.r_format.rd;
+					pInfo->rs = i.r_format.rs;
+					break;
+				case jr_op:
+					pInfo->type = INSTR_TYPE_JUMP;
+					pInfo->flags = INSTR_FLAGS_JUMP_REGISTER;
+					pInfo->rs = i.r_format.rs;
+					break;
+				case mtlo_op:
+				case mthi_op:
+					pInfo->rs = i.r_format.rs;
+				case mult_op:
+				case multu_op:
+				case div_op:
+				case divu_op:
+					pInfo->type = INSTR_TYPE_ARITH;
+					pInfo->rt = i.r_format.rt;
+					break;
+				case syscall_op:
+					pInfo->type = INSTR_TYPE_SYSCALL;
+					break;
+				case break_op:
+				case vcall_op:
+					pInfo->type = INSTR_TYPE_BREAK;
+					break;
+				default:
+					break;
+			}
+			break;
+
+		case bcond_op:
+			pInfo->type = INSTR_TYPE_BRANCH;
+			pInfo->rs = i.i_format.rs;
+			do_b_displacement = true;
+		break;
+
+		case blez_op_T:
+		case bgtz_op_T:
+		case blez_op_N:
+		case bgtz_op_N:
+			pInfo->type = INSTR_TYPE_BRANCH;
+			pInfo->rs = i.i_format.rs;
+			do_b_displacement = true;
+		break;
+		case beq_op_T:
+		case beq_op_N:
+		case bne_op_T:
+		case bne_op_N:
+			pInfo->type = INSTR_TYPE_BRANCH;
+			pInfo->rs = i.i_format.rs;
+			pInfo->rt = i.i_format.rt;
+			do_b_displacement = true;
+		break;
+
+		case cop0_op:
+		case cop1_op:
+		case cop2_op:
+		case cop3_op:
+			pInfo->type = INSTR_TYPE_COP;
+		break;			/* End of cop0, cop1, cop2, cop3 */
+
+		case jal_op:
+			pInfo->type = INSTR_TYPE_JUMP;
+			pInfo->flags = INSTR_FLAGS_JUMP_LINK | INSTR_FLAGS_JUMP_IMMEDIATE;
+			break;
+		case j_op:
+			pInfo->type = INSTR_TYPE_JUMP;
+			pInfo->flags = INSTR_FLAGS_JUMP_IMMEDIATE;
+			break;
+
+		case swc1_op:
+		case sdc1_op:
+			pInfo->type = INSTR_TYPE_STORE;
+			pInfo->flags = INSTR_FLAGS_STORE_COP;
+			do_loadstore = true;
+			break;
+
+		case lwc1_op:
+		case ldc1_op:
+			pInfo->type = INSTR_TYPE_LOAD;
+			pInfo->flags = INSTR_FLAGS_LOAD_COP;
+			do_loadstore = true;
+			break;
+		break;
+
+		case lb_op:
+		case lh_op:
+		case lw_op:
+		case ld_op:
+		case lbu_op:
+		case lhu_op:
+		case lwl_op:
+		case lwr_op:
+			pInfo->type = INSTR_TYPE_LOAD;
+			pInfo->flags = INSTR_FLAGS_LOAD_MEMORY;
+			pInfo->rt = i.i_format.rt;
+			do_loadstore = true;
+			break;
+		case sb_op:
+		case sh_op:
+		case sw_op:
+		case sd_op:
+			pInfo->type = INSTR_TYPE_STORE;
+			pInfo->flags = INSTR_FLAGS_STORE_MEMORY;
+			pInfo->rt = i.i_format.rt;
+			do_loadstore = true;
+			break;
+
+		case ori_op:
+		case xori_op:
+			pInfo->type = INSTR_TYPE_ARITH;
+			pInfo->flags = INSTR_FLAGS_ARITH_IMMEDIATE;
+			pInfo->rt = i.i_format.rt;
+			break;
+
+		/* fall through */
+		case andi_op:
+			pInfo->type = INSTR_TYPE_ARITH;
+			pInfo->flags = INSTR_FLAGS_ARITH_IMMEDIATE;
+			pInfo->rt = i.u_format.rt;
+			pInfo->rs = i.u_format.rs;
+			break;
+
+		case lui_op:
+			pInfo->type = INSTR_TYPE_LOAD;
+			pInfo->flags = INSTR_FLAGS_LOAD_IMMEDIATE;
+			pInfo->rt = i.u_format.rt;
+		break;
+		case addi_op:
+		case addiu_op:
+			pInfo->type = INSTR_TYPE_ARITH;
+			pInfo->flags = INSTR_FLAGS_ARITH_IMMEDIATE;
+			pInfo->rt = i.i_format.rt;
+		/* fall through */
+		default:
+			break;
+    }
+
+    return pInfo->type;
+}
+
+/* public -- see dissassembler.h */
+int
+tdisasm(buffer, address, iword, ext, regmask, symbol_value, ls_register)
+char *buffer;
+unsigned char ext;	/* extension byte */
+unsigned address, iword, *regmask, *symbol_value, *ls_register;
+{
+    int return_value = 0;
+    char *bufptr = buffer;
+    boolean do_b_displacement = false;
+    boolean do_loadstore = false;
+    union mips_instruction i;
+    union extension_byte e;
+
+	*bufptr = 0;
+    i.word = iword;
+    e.byte = ext;
+    *regmask = *symbol_value = *ls_register = 0;
+
+    /* Put out the address and hex value of the instruction,
+       leaving bufptr set at the end */
+    if (save.addr_format) {
+	sprintf(bufptr, save.addr_format, address);
+	bufptr += strlen(bufptr);
+    }
+    if (save.value_format) {
+	sprintf(bufptr, save.value_format, iword);
+	bufptr += strlen(bufptr);
+    }
+
+    /* check for dynamic NOP */
+    if (e.e.dyn_nop) {
+	strcat(bufptr, "D.");
+	bufptr += 2;
+    }
+
+    /* decode the instruction */
+    switch (i.j_format.opcode) {
+
+    case spec_op:
+
+	if (i.word == 0x20) {
+	    strcat(bufptr, "nop");
+	    if (e.e.rd_boost)
+		strcat(bufptr, ".B");
+	    bufptr += strlen(bufptr);
+	    break;
+	} else if (i.r_format.func == addu_op && i.r_format.rt == ZERO) {
+	    sprintf(bufptr, "move%s\t%s,%s",
+		    e.e.rd_boost ? ".B" : "",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask),
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask));
+	    bufptr += strlen(bufptr);
+	    break;
+	}
+	strcat(bufptr, spec_name[i.r_format.func]);
+	if (e.e.rd_boost)
+	    strcat(bufptr, ".B");
+	bufptr += strlen(bufptr);
+
+	switch (i.r_format.func) {
+	case sll_op:
+	case srl_op:
+	case sra_op:
+	    sprintf(bufptr, "\t%s,%s,%d",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask),
+		    register_name(i.r_format.rt, e.e.rt_boost, regmask),
+		    i.r_format.re);
+	    break;
+	case sllv_op:
+	case srlv_op:
+	case srav_op:
+	    sprintf(bufptr, "\t%s,%s,%s",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask),
+		    register_name(i.r_format.rt, e.e.rt_boost, regmask),
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask));
+	    break;
+	case mfhi_op:
+	case mflo_op:
+	    sprintf(bufptr, "\t%s",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask));
+	    break;
+	case jalr_op:
+	    return_value = 3;
+	    sprintf(bufptr, "\t%s,%s",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask),
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask));
+	    *ls_register = i.r_format.rs;
+	    break;
+	case jr_op:
+	    return_value = 3;
+	    *ls_register = i.r_format.rs;
+	    /* fall through */
+	case mtlo_op:
+	case mthi_op:
+	    sprintf(bufptr, "\t%s",
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask));
+	    break;
+	case mult_op:
+	case multu_op:
+	case div_op:
+	case divu_op:
+	    sprintf(bufptr, "\t%s,%s",
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask),
+		    register_name(i.r_format.rt, e.e.rt_boost, regmask));
+	    break;
+	case syscall_op:
+	    break;
+	case break_op:
+	case vcall_op:
+	    {
+		char *format = "\t%d";
+		unsigned op2 = i.r_format.rd * 32 + i.r_format.re;
+
+		if (op2)
+		    format = "\t%d,%d";
+		sprintf(bufptr, format, i.r_format.rs*32+i.r_format.rt,
+			op2);
+	    }
+	    break;
+	default:
+	    sprintf(bufptr, "\t%s,%s,%s",
+		    register_name(i.r_format.rd, e.e.rd_boost, regmask),
+		    register_name(i.r_format.rs, e.e.rs_boost, regmask),
+		    register_name(i.r_format.rt, e.e.rt_boost, regmask));
+	    break;
+	};
+	break;
+
+    case bcond_op:
+	sprintf(bufptr, "%s%s\t%s,",
+		bcond_name[i.i_format.rt],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.i_format.rs, e.e.rs_boost, regmask));
+	do_b_displacement = true;
+	break;
+
+    case blez_op_T:
+    case bgtz_op_T:
+    case blez_op_N:
+    case bgtz_op_N:
+	sprintf(bufptr, "%s%s\t%s,", op_name[i.i_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.i_format.rs, e.e.rs_boost, regmask));
+	do_b_displacement = true;
+	break;
+    case beq_op_T:
+    case beq_op_N:
+	if (i.i_format.rs == ZERO && i.i_format.rt == ZERO) {
+	    strcat(bufptr, "b");
+	    if (i.j_format.opcode == beq_op_T)
+		strcat(bufptr, ".T");
+	    else
+		strcat(bufptr, ".N");
+	    if (e.e.rd_boost)
+		strcat(bufptr, ".B\t");
+	    else
+		strcat(bufptr, "\t");
+	    do_b_displacement = true;
+	    break;
+	}
+	/* fall through */
+    case bne_op_T:
+    case bne_op_N:
+	sprintf(bufptr, "%s%s\t%s,%s,", op_name[i.i_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.i_format.rs, e.e.rs_boost, regmask),
+		register_name(i.i_format.rt, e.e.rt_boost, regmask));
+	do_b_displacement = true;
+	break;
+
+    case cop0_op:
+    case cop1_op:
+    case cop2_op:
+    case cop3_op:
+	{
+	    unsigned which_cop = i.j_format.opcode - cop0_op;
+	    char *f_or_r = "rf";
+
+	    switch (i.r_format.rs) {
+	    case bc_op_T:
+		sprintf(bufptr, "bc%d%c.T%s\t", which_cop,
+			"ft"[i.r_format.rt],
+			e.e.rd_boost ? ".B" : "");
+		do_b_displacement = true;
+		break;
+	    case bc_op_N:
+		sprintf(bufptr, "bc%d%c.N%s\t", which_cop,
+			"ft"[i.r_format.rt],
+			e.e.rd_boost ? ".B" : "");
+		do_b_displacement = true;
+		break;
+	    case mtc_op:
+		if (which_cop == 0)
+		    sprintf(bufptr, "mtc%d%s\t%s,%s", which_cop,
+			    e.e.rd_boost ? ".B" : "",
+			    register_name(i.r_format.rt, e.e.rt_boost,regmask),
+			    c0_reg[i.f_format.rd]);
+		else
+		    sprintf(bufptr, "mtc%d%s\t%s,%c%d", which_cop,
+			    e.e.rd_boost ? ".B" : "",
+			    register_name(i.r_format.rt, e.e.rt_boost,regmask),
+			    f_or_r[(which_cop == 1)],
+			    i.f_format.rd);
+		break;
+	    case mfc_op:
+		if (which_cop == 0)
+		    sprintf(bufptr, "mfc%d%s\t%s,%s", which_cop,
+			    e.e.rd_boost ? ".B" : "",
+			    register_name(i.r_format.rt, e.e.rt_boost,regmask),
+			    c0_reg[i.f_format.rd]);
+		else
+		    sprintf(bufptr, "mfc%d%s\t%s,%c%d", which_cop,
+			    e.e.rd_boost ? ".B" : "",
+			    register_name(i.r_format.rt, e.e.rt_boost,regmask),
+			    f_or_r[(which_cop == 1)],
+			    i.f_format.rd);
+		break;
+	    case cfc_op:
+		sprintf(bufptr, "cfc%d%s\t%s,%c%d", which_cop,
+			e.e.rd_boost ? ".B" : "",
+			register_name(i.r_format.rt, e.e.rt_boost, regmask),
+			f_or_r[(which_cop == 1)],
+			i.f_format.rd);
+		break;
+	    case ctc_op:
+		sprintf(bufptr, "ctc%d%s\t%s,%c%d", which_cop,
+			e.e.rd_boost ? ".B" : "",
+			register_name(i.r_format.rt, e.e.rt_boost, regmask),
+			f_or_r[(which_cop == 1)],
+			i.f_format.rd);
+		break;
+	    default:
+		if (which_cop != 1)
+		    sprintf(bufptr, "c0%s\t%s", e.e.rd_boost ? ".B" : "",
+			    c0_opname[i.f_format.func]);
+		else
+		    {
+			sprintf(bufptr, "%s.%s%s\t",
+				cop1_name[i.f_format.func],
+				fmt_name[i.f_format.fmt],
+				e.e.rd_boost ? ".B" : "");
+			bufptr += strlen(bufptr);
+			switch (i.f_format.func) {
+			case fsqrt_op:
+			case fabs_op:
+			case fmov_op:
+			case fcvts_op:
+			case fcvtd_op:
+			case fcvte_op:
+			case fcvtw_op:
+			    sprintf(bufptr, "f%d,f%d",
+				    i.f_format.re,
+				    i.f_format.rd);
+			    break;
+			case fcmp_op+0x0:
+			case fcmp_op+0x1:
+			case fcmp_op+0x2:
+			case fcmp_op+0x3:
+			case fcmp_op+0x4:
+			case fcmp_op+0x5:
+			case fcmp_op+0x6:
+			case fcmp_op+0x7:
+			case fcmp_op+0x8:
+			case fcmp_op+0x9:
+			case fcmp_op+0xa:
+			case fcmp_op+0xb:
+			case fcmp_op+0xc:
+			case fcmp_op+0xd:
+			case fcmp_op+0xe:
+			case fcmp_op+0xf:
+			    sprintf(bufptr, "f%d,f%d",
+				    i.f_format.rd,
+				    i.f_format.rt);
+			    break;
+			default:
+			    sprintf(bufptr, "f%d,f%d,f%d",
+				    i.f_format.re,
+				    i.f_format.rd,
+				    i.f_format.rt);
+			    break;
+			}	/* switch on func */
+		    }
+		break;		/* End of default */
+	    }			/* switch on rs */
+	}
+	break;			/* End of cop0, cop1, cop2, cop3 */
+
+    case jal_op:
+    case j_op:
+	sprintf(bufptr, "%s%s\t", op_name[i.j_format.opcode],
+		e.e.rd_boost ? ".B" : "");
+	*symbol_value = (address & 0xF0000000) | i.j_format.target << 2;
+	if (save.print_jal_targets)
+	    {
+		bufptr += strlen(bufptr);
+		sprintf(bufptr, "%#x", *symbol_value);
+	    }
+	return_value = 1;
+	break;
+
+    case swc1_op:
+    case sdc1_op:
+    case lwc1_op:
+    case ldc1_op:
+	sprintf(bufptr, "%s%s\tf%d,", op_name[i.i_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		i.i_format.rt);
+	do_loadstore = true;
+	break;
+
+    case lb_op:
+    case lh_op:
+    case lw_op:
+    case ld_op:
+    case lbu_op:
+    case lhu_op:
+    case sb_op:
+    case sh_op:
+    case sw_op:
+    case sd_op:
+    case lwl_op:
+    case lwr_op:
+	sprintf(bufptr, "%s%s\t%s,", op_name[i.i_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.i_format.rt, e.e.rt_boost, regmask));
+	do_loadstore = true;
+	break;
+
+    case ori_op:
+    case xori_op:
+	if (i.u_format.rs == ZERO) {
+	    sprintf(bufptr, "li%s\t%s,%d",
+		    e.e.rd_boost ? ".B" : "",
+		    register_name(i.u_format.rt, e.e.rt_boost, regmask),
+		    i.u_format.uimmediate);
+	    break;
+	}
+	/* fall through */
+    case andi_op:
+	sprintf(bufptr, "%s%s\t%s,%s,%#x", op_name[i.u_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.u_format.rt, e.e.rt_boost, regmask),
+		register_name(i.u_format.rs, e.e.rs_boost, regmask),
+		i.u_format.uimmediate);
+	break;
+    case lui_op:
+	sprintf(bufptr, "%s%s\t%s,%#x", op_name[i.u_format.opcode],
+		e.e.rd_boost ? ".B" : "",
+		register_name(i.u_format.rt, e.e.rt_boost, regmask),
+		i.u_format.uimmediate);
+	break;
+    case addi_op:
+    case addiu_op:
+	if (i.i_format.rs == ZERO) {
+	    short sign_extender = i.i_format.simmediate;
+	    sprintf(bufptr, "li%s\t%s,%d",
+		    e.e.rd_boost ? ".B" : "",
+		    register_name(i.i_format.rt, e.e.rt_boost, regmask),
+		    sign_extender);
+	    break;
+	}
+	/* fall through */
+    default:
+	{
+	    short sign_extender = i.i_format.simmediate;
+	    sprintf(bufptr, "%s%s\t%s,%s,%d", op_name[i.i_format.opcode],
+		    e.e.rd_boost ? ".B" : "",
+		    register_name(i.i_format.rt, e.e.rt_boost, regmask),
+		    register_name(i.i_format.rs, e.e.rs_boost, regmask),
+		    sign_extender);
+	}
+	break;
+    }
+
+    /* Some instructions require more than just registers */
+
+    if (do_loadstore)
+	{
+	    short sign_extender = i.i_format.simmediate;
+	    *symbol_value = sign_extender;
+	    *ls_register = i.i_format.rs;
+	    bufptr += strlen(bufptr);
+	    sprintf(bufptr, "%d(%s)", sign_extender,
+		    register_name(i.i_format.rs, e.e.rs_boost, regmask));
+	    return_value = -1;
+	}
+    else if (do_b_displacement)
+	{
+	    short sign_extender = i.i_format.simmediate;
+	    bufptr += strlen(bufptr);
+	    *symbol_value = 4 + address + ((((long)sign_extender & 0xFFFD0000) | (0x3FFFF & (sign_extender << 2))));
+	    sprintf(bufptr, "%#x", *symbol_value);
+	    return_value = 2;
+	}
+
+    return return_value;
+}
+
@@ -0,0 +1,489 @@
+/* ------------------------------------------------------------------ */
+/* | Copyright Unpublished, MIPS Computer Systems, Inc.  All Rights | */
+/* | Reserved.  This software contains proprietary and confidential | */
+/* | information of MIPS and its suppliers.  Use, disclosure or     | */
+/* | reproduction is prohibited without the prior express written   | */
+/* | consent of MIPS.                                               | */
+/* ------------------------------------------------------------------ */
+/*  inst.h 4.2 */
+/*
+ * inst.h -- instruction format defines
+ */
+
+/**
+ ** UPDATE - 19 Sept 89 by Michael Smith
+ **
+ ** Removed signed type to remove obnoxious C++ warning message.
+ **/
+
+#ifdef LANGUAGE_C
+#ifdef MIPSEB
+union mips_instruction {
+	unsigned word;
+	unsigned char byte[4];
+	struct {
+		unsigned opcode : 6;
+		unsigned target : 26;
+	} j_format;
+	struct {
+		unsigned opcode : 6;
+		unsigned rs : 5;
+		unsigned rt : 5;
+		unsigned simmediate : 16;
+	} i_format;
+	struct {
+		unsigned opcode : 6;
+		unsigned rs : 5;
+		unsigned rt : 5;
+		unsigned uimmediate : 16;
+	} u_format;
+	struct {
+		unsigned opcode : 6;
+		unsigned rs : 5;
+		unsigned rt : 5;
+		unsigned rd : 5;
+		unsigned re : 5;
+		unsigned func : 6;
+	} r_format;
+	struct {
+		unsigned opcode : 6;
+		unsigned : 1;
+		unsigned fmt : 4;
+		unsigned rt : 5;
+		unsigned rd : 5;
+		unsigned re : 5;
+		unsigned func : 6;
+	} f_format;
+};
+#endif
+
+#ifdef MIPSEL
+union mips_instruction {
+	unsigned word;
+	unsigned char byte[4];
+	struct {
+		unsigned target : 26;
+		unsigned opcode : 6;
+	} j_format;
+	struct {
+		unsigned simmediate : 16;
+		unsigned rt : 5;
+		unsigned rs : 5;
+		unsigned opcode : 6;
+	} i_format;
+	struct {
+		unsigned uimmediate : 16;
+		unsigned rt : 5;
+		unsigned rs : 5;
+		unsigned opcode : 6;
+	} u_format;
+	struct {
+		unsigned func : 6;
+		unsigned re : 5;
+		unsigned rd : 5;
+		unsigned rt : 5;
+		unsigned rs : 5;
+		unsigned opcode : 6;
+	} r_format;
+	struct {
+		unsigned func : 6;
+		unsigned re : 5;
+		unsigned rd : 5;
+		unsigned rt : 5;
+		unsigned fmt : 4;
+		unsigned : 1;
+		unsigned opcode : 6;
+	} f_format;
+};
+#endif
+
+#define	INSTR_FLAGS_ARITH_IMMEDIATE			0x0000001
+#define	INSTR_FLAGS_ARITH_REGISTER			0x0000002
+#define	INSTR_FLAGS_JUMP_LINK				0x8000000
+#define	INSTR_FLAGS_JUMP_IMMEDIATE			0x0000001
+#define	INSTR_FLAGS_JUMP_REGISTER			0x0000002
+#define	INSTR_FLAGS_BRANCH_LINK				0x8000000
+#define	INSTR_FLAGS_BRANCH_IMMEDIATE		0x0000001
+#define	INSTR_FLAGS_BRANCH_REGISTER			0x0000002
+#define	INSTR_FLAGS_LOAD_IMMEDIATE			0x0000001
+#define	INSTR_FLAGS_LOAD_MEMORY				0x0000002
+#define	INSTR_FLAGS_LOAD_COP				0x0000004
+#define	INSTR_FLAGS_STORE_MEMORY			0x0000002
+#define	INSTR_FLAGS_STORE_COP				0x0000004
+
+enum
+{
+	INSTR_TYPE_NOP = 0,
+	INSTR_TYPE_MOVE,
+	INSTR_TYPE_ARITH,
+	INSTR_TYPE_JUMP,
+	INSTR_TYPE_BRANCH,
+	INSTR_TYPE_LOAD,
+	INSTR_TYPE_STORE,
+	INSTR_TYPE_COP,
+	INSTR_TYPE_MUL,
+	INSTR_TYPE_DIV,
+	INSTR_TYPE_BREAK,
+	INSTR_TYPE_SYSCALL
+};
+
+typedef struct _sinstr_info_t
+{
+	unsigned type;
+	unsigned flags;
+	unsigned rd, rt, rs, re;
+
+} instr_info_t;
+
+
+#define	spec_op		0x00
+#define	bcond_op	0x01
+#define	j_op		0x02
+#define	jal_op		0x03
+
+#define	beq_op		0x04
+#define	bne_op		0x05
+#define	blez_op		0x06
+#define	bgtz_op		0x07
+
+#define	addi_op		0x08
+#define	addiu_op	0x09
+#define	slti_op		0x0A
+#define	sltiu_op	0x0B
+
+#define	andi_op		0x0C
+#define	ori_op		0x0D
+#define	xori_op		0x0E
+#define	lui_op		0x0F
+
+#define	lb_op		0x20
+#define	lh_op		0x21
+#define	lw_op		0x23
+#define	lbu_op		0x24
+#define	lhu_op		0x25
+#define	ld_op		0x27
+#define	sb_op		0x28
+#define	sh_op		0x29
+#define	sw_op		0x2B
+#define	sd_op		0x2F
+#define	lwl_op		0x22
+#define	lwr_op		0x26
+#define	swl_op		0x2a
+#define	swr_op		0x2e
+
+/* Co-processor sub-opcodes */
+#define	bc_op		0x08
+#define	mfc_op		0x00
+#define cfc_op		0x02
+#define	mtc_op		0x04
+#define ctc_op		0x06
+
+/* Co-processor 0 opcodes */
+#define	cop0_op		0x10
+#define	lwc0_op		0x30
+#define	ldc0_op		0x34
+#define	swc0_op		0x38
+#define	sdc0_op		0x3c
+
+/* Co-processor 0 sub-opcodes */
+#define	tlbr_op		0x1
+#define	tlbwi_op	0x2
+#define	tlbwr_op	0x6
+#define	tlbp_op		0x8
+#define	rfe_op		0x10
+
+/* Co-processor 1 opcodes */
+#define	cop1_op		0x11
+#define	lwc1_op		0x31
+#define	ldc1_op		0x35
+#define	swc1_op		0x39
+#define	sdc1_op		0x3D
+
+/* Co-processor 1 sub-opcodes */
+#define	fadd_op		0x00
+#define	fsub_op		0x01
+#define	fmpy_op		0x02
+#define	fdiv_op		0x03
+#define	fsqrt_op	0x04
+#define	fabs_op		0x05
+#define	fmov_op		0x06
+#define fneg_op		0x07
+#define	fcvts_op	0x20
+#define	fcvtd_op	0x21
+#define	fcvte_op	0x22
+#define	fcvtw_op	0x24
+#define	fcmp_op		0x30
+#define	s_fmt		0
+#define	d_fmt		1
+#define	e_fmt		2
+#define	w_fmt		4
+
+/* Other coprocessor opcodes */
+#define	cop2_op		0x12
+#define	lwc2_op		0x32
+#define	ldc2_op		0x36
+#define	swc2_op		0x3a
+#define	sdc2_op		0x3e
+
+#define	cop3_op		0x13
+#define	lwc3_op		0x33
+#define	ldc3_op		0x37
+#define	swc3_op		0x3b
+#define	sdc3_op		0x3f
+
+
+/* bcond subopcodes */
+#define	bltz_op		0x00
+#define	bgez_op		0x01
+#define	bltzal_op	0x10
+#define	bgezal_op	0x11
+
+/* special subopcodes */
+#define	sll_op		0x00
+#define	srl_op		0x02
+#define	sra_op		0x03
+#define	sllv_op		0x04
+#define	srlv_op		0x06
+#define	srav_op		0x07
+#define	jr_op		0x08
+#define	jalr_op		0x09
+#define	syscall_op	0x0C
+#define	break_op	0x0D
+#define	vcall_op	0x0E
+
+#define	mfhi_op		0x10
+#define	mthi_op		0x11
+#define	mflo_op		0x12
+#define	mtlo_op		0x13
+#define	mult_op		0x18
+#define	multu_op	0x19
+#define	div_op		0x1A
+#define	divu_op		0x1B
+
+#define	add_op		0x20
+#define	addu_op		0x21
+#define	and_op		0x24
+#define	or_op		0x25
+#define	xor_op		0x26
+#define	nor_op		0x27
+#define	sub_op		0x22
+#define	subu_op		0x23
+#define	slt_op		0x2A
+#define	sltu_op		0x2B
+
+#endif /* LANGUAGE_C */
+
+#ifdef LANGUAGE_PASCAL
+
+#ifdef MIPSEB
+type
+    mips_instruction =
+      packed record
+        case cardinal of
+	  0: (
+	    word: cardinal;
+	    );
+	  1: (
+	    byte: packed array[0..3] of 0..255;
+	    );
+	  2: (
+	    opcode: 0..63;
+	    target: 0..67108863;
+	    );
+	  3: (
+	    opcode3: 0..63;
+	    rs: 0..31;
+	    rt: 0..31;
+	    simmediate: -32768..32767;
+	    );
+	  4: (
+	    opcode4: 0..63;
+	    rs4: 0..63;
+	    rt4: 0..63;
+	    uimmediate: 0..65535;
+	    );
+	  5: (
+	    opcode5: 0..63;
+	    rs5: 0..63;
+	    rt5: 0..63;
+	    rd5: 0..63;
+	    re5: 0..63;
+	    func: 0..63;
+	    );
+      end {record};
+#endif
+
+#ifdef MIPSEL
+type
+    mips_instruction =
+      packed record
+        case cardinal of
+	  0: (
+	    word: cardinal;
+	    );
+	  1: (
+	    byte: packed array[0..3] of 0..255;
+	    );
+	  2: (
+	    target: 0..67108863;
+	    opcode: 0..63;
+	    );
+	  3: (
+	    simmediate: -32768..32767;
+	    rt: 0..31;
+	    rs: 0..31;
+	    opcode3: 0..63;
+	    );
+	  4: (
+	    uimmediate: 0..65535;
+	    rt4: 0..63;
+	    rs4: 0..63;
+	    opcode4: 0..63;
+	    );
+	  5: (
+	    func: 0..63;
+	    re5: 0..63;
+	    rd5: 0..63;
+	    rt5: 0..63;
+	    rs5: 0..63;
+	    opcode5: 0..63;
+	    );
+      end {record};
+#endif
+
+#define	spec_op		16#00
+#define	bcond_op	16#01
+#define	j_op		16#02
+#define	jal_op		16#03
+
+#define	beq_op		16#04
+#define	bne_op		16#05
+#define	blez_op		16#06
+#define	bgtz_op		16#07
+
+#define	addi_op		16#08
+#define	addiu_op	16#09
+#define	slti_op		16#0A
+#define	sltiu_op	16#0B
+
+#define	andi_op		16#0C
+#define	ori_op		16#0D
+#define	xori_op		16#0E
+#define	lui_op		16#0F
+
+#define	lb_op		16#20
+#define	lh_op		16#21
+#define	lw_op		16#23
+#define	lbu_op		16#24
+#define	lhu_op		16#25
+#define	ld_op		16#27
+#define	sb_op		16#28
+#define	sh_op		16#29
+#define	sw_op		16#2B
+#define	sd_op		16#2F
+#define	lwl_op		16#22
+#define	lwr_op		16#26
+#define	swl_op		16#2a
+#define	swr_op		16#2e
+
+/* Co-processor sub-opcodes */
+#define	bc_op		16#08
+#define	mfc_op		16#00
+#define cfc_op		16#02
+#define	mtc_op		16#04
+#define ctc_op		16#06
+
+/* Co-processor 0 opcodes */
+#define	cop0_op		16#10
+#define	lwc0_op		16#30
+#define	ldc0_op		16#34
+#define	swc0_op		16#38
+#define	sdc0_op		16#3c
+
+/* Co-processor 0 sub-opcodes */
+#define	tlbr_op		16#1
+#define	tlbwi_op	16#2
+#define	tlbwr_op	16#6
+#define	tlbp_op		16#8
+#define	rfe_op		16#10
+
+/* Co-processor 1 opcodes */
+#define	cop1_op		16#11
+#define	lwc1_op		16#31
+#define	ldc1_op		16#35
+#define	swc1_op		16#39
+#define	sdc1_op		16#3D
+
+/* Co-processor 1 sub-opcodes */
+#define	fadd_op		16#00
+#define	fsub_op		16#01
+#define	fmpy_op		16#02
+#define	fdiv_op		16#03
+#define	fsqrt_op	16#04
+#define	fabs_op		16#05
+#define	fmov_op		16#06
+#define fneg_op		16#07
+#define	fcvts_op	16#20
+#define	fcvtd_op	16#21
+#define	fcvte_op	16#22
+#define	fcvtw_op	16#24
+#define	fcmp_op		16#30
+#define	s_fmt		0
+#define	d_fmt		1
+#define	e_fmt		2
+#define	w_fmt		4
+
+/* Other coprocessor opcodes */
+#define	cop2_op		16#12
+#define	lwc2_op		16#32
+#define	ldc2_op		16#36
+#define	swc2_op		16#3a
+#define	sdc2_op		16#3e
+
+#define	cop3_op		16#13
+#define	lwc3_op		16#33
+#define	ldc3_op		16#37
+#define	swc3_op		16#3b
+#define	sdc3_op		16#3f
+
+
+/* bcond subopcodes */
+#define	bltz_op		16#00
+#define	bgez_op		16#01
+#define	bltzal_op	16#10
+#define	bgezal_op	16#11
+
+/* special subopcodes */
+#define	sll_op		16#00
+#define	srl_op		16#02
+#define	sra_op		16#03
+#define	sllv_op		16#04
+#define	srlv_op		16#06
+#define	srav_op		16#07
+#define	jr_op		16#08
+#define	jalr_op		16#09
+#define	syscall_op	16#0C
+#define	break_op	16#0D
+#define	vcall_op	16#0E
+
+#define	mfhi_op		16#10
+#define	mthi_op		16#11
+#define	mflo_op		16#12
+#define	mtlo_op		16#13
+#define	mult_op		16#18
+#define	multu_op	16#19
+#define	div_op		16#1A
+#define	divu_op		16#1B
+
+#define	add_op		16#20
+#define	addu_op		16#21
+#define	and_op		16#24
+#define	or_op		16#25
+#define	xor_op		16#26
+#define	nor_op		16#27
+#define	sub_op		16#22
+#define	subu_op		16#23
+#define	slt_op		16#2A
+#define	sltu_op		16#2B
+
+#endif /* LANGUAGE_PASCAL */
@@ -0,0 +1,831 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "libsys.h"
+#include "mips_gfx.h"
+
+#define USE_HW_BLITTER
+
+
+static vga_hw_t *g_pRegs;
+
+// -------------------------------------------------------------
+// Functions
+// -------------------------------------------------------------
+void __gfx_block_set_8(UINT32* pDst, UINT32 value)
+{
+	__asm__ volatile
+	(
+		"sw   %[value], 0(%[pDst])\n"
+		"sw   %[value], 4(%[pDst])\n"
+		"sw   %[value], 8(%[pDst])\n"
+		"sw   %[value], 12(%[pDst])\n"
+		"sw   %[value], 16(%[pDst])\n"
+		"sw   %[value], 20(%[pDst])\n"
+		"sw   %[value], 24(%[pDst])\n"
+		"sw   %[value], 28(%[pDst])\n"
+		:
+		: [pDst] "r" (pDst), [value] "r" (value)
+	);
+}
+
+void __gfx_block_copy_8(UINT32* pDst, UINT32* pSrc)
+{
+	 __asm__ volatile
+	(
+		".set noreorder\n"
+		".set nomacro\n"
+		"lw   $s0, 0(%[pSrc])\n"
+		"lw   $s1, 4(%[pSrc])\n"
+		"sw   $s0, 0(%[pDst])\n"
+		"lw   $s0, 8(%[pSrc])\n"
+		"sw   $s1, 4(%[pDst])\n"
+		"lw   $s1, 12(%[pSrc])\n"
+		"sw   $s0, 8(%[pDst])\n"
+		"lw   $s0, 16(%[pSrc])\n"
+		"sw   $s1, 12(%[pDst])\n"
+		"lw   $s1, 20(%[pSrc])\n"
+		"sw   $s0, 16(%[pDst])\n"
+		"lw   $s0, 24(%[pSrc])\n"
+		"sw   $s1, 20(%[pDst])\n"
+		"lw   $s1, 28(%[pSrc])\n"
+		"sw   $s0, 24(%[pDst])\n"
+		"sw   $s1, 28(%[pDst])\n"
+		".set macro\n"
+		".set reorder\n"
+		:
+		: [pDst] "r" (pDst), [pSrc] "r" (pSrc)
+		: "s0", "s1"
+	);
+}
+
+void _GFX_copy32(UINT32* pDst, UINT32* pSrc, UINT32 ndwords)
+{
+	while (ndwords > 3)
+	{
+		__gfx_block_copy_8(pDst, pSrc);
+		pDst += 8;
+		pSrc += 8;
+		ndwords -= 8;
+	}
+	while (ndwords)
+	{
+		*(pDst++) = *(pSrc++);
+		ndwords -= 1;
+	}
+}
+
+void _GFX_memset32(UINT32* pDst, UINT32 value, UINT32 ndwords)
+{
+	while (ndwords > 3)
+	{
+		__gfx_block_set_8(pDst, value);
+		pDst += 8;
+		ndwords -= 8;
+	}
+	while (ndwords)
+	{
+		*(pDst++) = value;
+		ndwords -= 1;
+	}
+}
+
+void _GFX_copy32_hw(UINT32* pDst, UINT32* pSrc, UINT32 ndwords, UINT32 wait_finish)
+{
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = 0;
+	g_pRegs->pVGA_dst_dimx = 0;
+	g_pRegs->pVGA_nx = ndwords;
+	g_pRegs->pVGA_ny = 1;
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+	if (!wait_finish)
+		return;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
+
+}
+
+void _GFX_memset32_hw(UINT32* pDst, UINT32 value, UINT32 ndwords, UINT32 wait_finish)
+{
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = 0;
+	g_pRegs->pVGA_dst_dimx = 0;
+	g_pRegs->pVGA_nx = ndwords;
+	g_pRegs->pVGA_ny = 1;
+	g_pRegs->pVGA_color = value;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);
+
+	if (!wait_finish)
+		return;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
+
+}
+
+void _GFX_BLIT_const(UINT32* pDst, UINT32 xdim_dst, UINT32 xmin, UINT32 ymin, UINT32 xmax, UINT32 ymax, UINT32 value)
+{
+	UINT32 xmax8, x, y;
+	UINT32 *pStart, *pEnd;
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += xdim_dst)
+	{
+		pStart = &pDst[xmin + y];
+		pEnd = &pDst[xmax8 + y];
+		while(pStart < pEnd)
+		{
+			__gfx_block_set_8(pStart, value);
+			pStart += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pDst[x + y] = value;
+		}
+	}
+}
+
+void _GFX_BLIT_const_hw(UINT32* pDst, UINT32 xdim_dst, UINT32 xmin, UINT32 ymin, UINT32 xmax, UINT32 ymax, UINT32 value, UINT32 wait_finish)
+{
+	UINT32 *pStart, *pEnd;
+
+	pStart = (UINT32*)&pDst[xmin + ymin*xdim_dst];
+
+	g_pRegs->pVGA_dst = (UINT32)pStart;
+	g_pRegs->pVGA_dst_dimx = xdim_dst;
+	g_pRegs->pVGA_nx = xmax - xmin + 1;
+	g_pRegs->pVGA_ny = ymax - ymin + 1;
+	g_pRegs->pVGA_color = value;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);
+
+	if (!wait_finish)
+		return;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
+}
+
+void GFX_DirtyRectSetClean(gfx_t *pObj, buf_t *pBuf)
+{
+	pBuf->dirty.xmin = pBuf->w;
+	pBuf->dirty.xmax = 0;
+	pBuf->dirty.ymin = pBuf->h;
+	pBuf->dirty.ymax = 0;
+	pBuf->is_dirty = 0;
+}
+
+void GFX_DirtyRectInit(gfx_t *pObj, buf_t *pBuf, UINT32 w, UINT32 h)
+{
+	pBuf->w = w;
+	pBuf->h = h;
+	GFX_DirtyRectSetClean(pObj, pBuf);
+}
+
+void GFX_DirtyRectUpdate(gfx_t *pObj, buf_t *pBuf, UINT32 xmin, UINT32 xmax, UINT32 ymin, UINT32 ymax)
+{
+
+	if (xmin < pBuf->dirty.xmin)
+	{
+		pBuf->is_dirty = 1;
+		pBuf->dirty.xmin = xmin;
+	}
+
+	if (xmax > pBuf->dirty.xmax)
+	{
+		pBuf->is_dirty = 1;
+		pBuf->dirty.xmax = xmax;
+	}
+
+	if (ymin < pBuf->dirty.ymin)
+	{
+		pBuf->is_dirty = 1;
+		pBuf->dirty.ymin = ymin;
+	}
+
+	if (ymax > pBuf->dirty.ymax)
+	{
+		pBuf->is_dirty = 1;
+		pBuf->dirty.ymax = ymax;
+	}
+
+	if (pBuf->is_dirty && ((pBuf->dirty.xmax-pBuf->dirty.xmin+1)%8))
+		pBuf->dirty.xmax = pBuf->dirty.xmin + ((pBuf->dirty.xmax-pBuf->dirty.xmin)/8 + 1) * 8; // Make len_x multiple of 8
+
+	pObj->restore.xmin = pBuf->dirty.xmin;
+	pObj->restore.xmax = pBuf->dirty.xmax;
+	pObj->restore.ymin = pBuf->dirty.ymin;
+	pObj->restore.ymax = pBuf->dirty.ymax;
+
+}
+
+void GFX_DirtyRectCopy(gfx_t *pObj, buf_t *pBuf)
+{
+	UINT32 xmin, xmax, xmax8, ymin, ymax, x, y;
+	UINT32 *pDst, *pDst_end;
+	UINT32 *pSrc, *pSrc_end;
+
+	xmin = pBuf->dirty.xmin;
+	xmax = pBuf->dirty.xmax;
+
+	ymin = pBuf->dirty.ymin;
+	ymax = pBuf->dirty.ymax;
+
+	ymin *= pBuf->w;
+	ymax *= pBuf->w;
+
+#ifdef USE_HW_BLITTER
+	pSrc = (UINT32*)&pBuf->pLast->pFB[xmin + ymin];
+	pDst = (UINT32*)&pBuf->pFB[xmin + ymin];
+
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = (UINT32)pBuf->w;
+	g_pRegs->pVGA_dst_dimx = (UINT32)pBuf->w;
+	g_pRegs->pVGA_nx = (UINT32)(pBuf->dirty.xmax-pBuf->dirty.xmin);
+	g_pRegs->pVGA_ny = (UINT32)(pBuf->dirty.ymax-pBuf->dirty.ymin);
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+#else
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += pBuf->w)
+	{
+		pSrc = &pBuf->pLast->pFB[xmin + y];
+		pSrc_end = &pBuf->pLast->pFB[xmax8 + y];
+		pDst = &pBuf->pFB[xmin + y];
+		pDst_end = &pBuf->pFB[xmax8 + y];
+		while(pDst < pDst_end)
+		{
+			__gfx_block_copy_8(pDst, pSrc);
+			pDst += 8;
+			pSrc += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pBuf->pFB[x + y] = pBuf->pLast->pFB[x + y];
+		}
+	}
+#endif
+
+}
+
+void GFX_init(gfx_t *pObj)
+{
+	int i;
+	buf_t *pBuf;
+	UINT32 w, h, fps;
+
+	g_pRegs = (vga_hw_t*)SYS_VGA_CTRL;
+
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;
+
+	w = Screen_get_resx();
+	h = Screen_get_resy();
+	fps = Screen_get_fps();
+
+//	printf("VGA screen = %d x %d @ %d fps\n", w, h, fps);
+
+	pObj->w = w;
+	pObj->h = h;
+
+	// Init background buffer
+	pObj->pBG = (UINT32*)calloc(w * h, sizeof(buf_t));
+
+	// Init framebuffers
+	pObj->pBuf = (buf_t*)calloc(1, sizeof(buf_t));
+	pBuf = pObj->pBuf;
+	pBuf->pFB = (UINT32*)calloc((w+8) * h, sizeof(UINT32));		// w+8 because of dirty rectangle 8 byte min. x-size
+	for (i=1; i < GFX_NUM_FRAME_BUFFERS; i++)
+	{
+
+		pBuf->pNext = (buf_t*)calloc(1, sizeof(buf_t));
+		pBuf->pNext->pLast = pBuf;
+		pBuf = pBuf->pNext;
+		pBuf->pFB = (UINT32*)calloc((w+8) * h, sizeof(UINT32));	// w+8 because of dirty rectangle 8 byte min. x-size
+	}
+	pObj->pBuf->pLast = pBuf;
+	pBuf->pNext = pObj->pBuf;
+
+	pBuf = pObj->pBuf;
+	for (i=0; i < GFX_NUM_FRAME_BUFFERS; i++)
+	{
+		GFX_DirtyRectInit(pObj, pBuf, w, h);
+
+//		fprintf(stderr, "pBuf = %08X, pLast = %08X, pNext = %08X\n", pBuf, pBuf->pLast, pBuf->pNext);
+		pBuf = pBuf->pNext;
+	}
+
+	g_pRegs->pVGA_front = (UINT32)pObj->pBuf->pLast->pFB;
+	g_pRegs->pVGA_back = (UINT32)pObj->pBuf->pLast->pFB;
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_MSTEN;
+
+
+}
+
+void GFX_set_background(gfx_t *pObj, UINT32 *pImage, UINT32 image_nx, UINT32 image_ny, UINT32 image_scale_x, UINT32 image_scale_y)
+{
+
+	UINT32 off_x, off_y;
+	UINT32 i, j;
+	UINT32 *pDst, *pSrc;
+	buf_t *pBuf;
+
+	if (!pImage)
+		return;
+
+	off_x = (pObj->w-(image_scale_x*image_nx))/2;
+	off_y = (pObj->h-(image_scale_y*image_ny))/2;
+
+#ifdef USE_HW_BLITTER
+
+	pDst = ((UINT32*)pObj->pBG) + pObj->w*off_y + off_x;
+	pSrc = pImage;
+
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = (UINT32)0;
+	g_pRegs->pVGA_dst_dimx = (UINT32)0;
+	g_pRegs->pVGA_nx = (UINT32)image_ny*image_nx;
+	g_pRegs->pVGA_ny = (UINT32)1;
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+	pBuf = pObj->pBuf;
+
+	pDst = pBuf->pFB;
+	pSrc = pObj->pBG;
+
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = (UINT32)0;
+	g_pRegs->pVGA_dst_dimx = (UINT32)0;
+	g_pRegs->pVGA_nx = (UINT32)image_ny*image_nx;
+	g_pRegs->pVGA_ny = (UINT32)1;
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
+
+#else
+
+	pDst = ((UINT32*)pObj->pBG) + pObj->w*off_y + off_x;
+	pSrc = pImage;
+
+	for (i=0; i < image_ny*image_nx; i += 8)
+	{
+		__gfx_block_copy_8(pDst, pSrc);
+		pDst += 8;
+		pSrc += 8;
+	}
+
+	pBuf = pObj->pBuf;
+
+	pDst = pBuf->pFB;
+	pSrc = pObj->pBG;
+
+	for (i=0; i < image_ny*image_nx; i += 8)
+	{
+		__gfx_block_copy_8(pDst, pSrc);
+		pDst += 8;
+		pSrc += 8;
+	}
+
+#endif
+
+	do
+	{
+		GFX_DirtyRectUpdate(pObj, pBuf, 0, pObj->w-1, 0, pObj->h-1);
+		pBuf = pBuf->pNext;
+
+	} while(pBuf != pObj->pBuf);
+
+}
+
+void GFX_frame(gfx_t *pObj, UINT32 sync_mode)
+{
+
+	// Wait until back buffer becomes front
+	if (sync_mode == GFX_FRAME_SYNC)
+	{
+		while(g_pRegs->pVGA_front == (UINT32)pObj->pBuf->pFB);
+	}
+
+	GFX_copy_front2back(pObj);
+
+	// Invalidate D-Cache after blitting has finished
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+#ifdef USE_HW_BLITTER
+	DCACHE_invalidate_all();
+#endif
+}
+
+void GFX_show(gfx_t *pObj)
+{
+
+	g_pRegs->pVGA_back = (UINT32)pObj->pBuf->pFB;
+	pObj->pBuf = pObj->pBuf->pNext;
+
+}
+
+void GFX_copy_front2back(gfx_t *pObj)
+{
+	buf_t *pBuf;
+
+	pBuf = pObj->pBuf;
+
+	// Lopp over all frame buffers, except front buffer
+	while(pBuf != pObj->pBuf->pLast)
+	{
+		if (pBuf->is_dirty)
+		{
+//			fprintf(stderr, "Dirty rect [%08X]: w=%d, h=%d\n", (int)pBuf, pBuf->dirty.xmax - pBuf->dirty.xmin, pBuf->dirty.ymax - pBuf->dirty.ymin);
+			GFX_DirtyRectCopy(pObj, pBuf);
+			GFX_DirtyRectSetClean(pObj, pBuf);
+		}
+		pBuf = pBuf->pNext;
+	}
+}
+
+void GFX_get_FB_front(gfx_t *pObj, UINT32 **ppFB)
+{
+	*ppFB = pObj->pBuf->pLast->pFB;
+}
+
+void box_create(box_t *pObj, UINT32 w, UINT32 h)
+{
+	pObj->w = w;
+	pObj->h = h;
+}
+
+void GFX_restore(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy)
+{
+	UINT32 xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
+	UINT32 *pDst, *pDst_end;
+	UINT32 *pSrc, *pSrc_end;
+	buf_t *pBuf;
+
+	if (pBox)
+	{
+		xmin = posx;
+		xmax = pBox->w + posx - 0;
+		if (xmax >= pObj->w)
+			xmax = pObj->w - 0;
+
+		ymin = posy;
+		ymax = pBox->h + posy - 0;
+		if (ymax >= pObj->h)
+			ymax = pObj->h - 0;
+	}
+	else
+	{
+		xmin = pObj->restore.xmin;
+		xmax = pObj->restore.xmax;
+		ymin = pObj->restore.ymin;
+		ymax = pObj->restore.ymax;
+	}
+	pObj->restore.xmin = 0;
+	pObj->restore.xmax = 0;
+	pObj->restore.ymin = 0;
+	pObj->restore.ymax = 0;
+	pBuf = pObj->pBuf->pNext;
+
+	// Lopp over all frame buffers, except current back buffer
+	while(pBuf != pObj->pBuf)
+	{
+		GFX_DirtyRectUpdate(pObj, pBuf, xmin, xmax, ymin, ymax);
+		pBuf = pBuf->pNext;
+
+	}
+
+	nx = xmax - xmin;
+	ny = ymax - ymin;
+
+	pBuf = pObj->pBuf;
+
+	ymin *= pObj->w;
+	ymax *= pObj->w;
+
+#ifdef USE_HW_BLITTER
+	pSrc = (UINT32*)&pObj->pBG[xmin + ymin];
+	pDst = (UINT32*)&pBuf->pFB[xmin + ymin];
+
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_src0_dimx = (UINT32)pObj->w;
+	g_pRegs->pVGA_dst_dimx = (UINT32)pObj->w;
+	g_pRegs->pVGA_nx = (UINT32)nx;
+	g_pRegs->pVGA_ny = (UINT32)ny;
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+#else
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += pBuf->w)
+	{
+		pSrc = &pObj->pBG[xmin + y];
+		pSrc_end = &pObj->pBG[xmax8 + y];
+		pDst = &pBuf->pFB[xmin + y];
+		pDst_end = &pBuf->pFB[xmax8 + y];
+		while(pDst < pDst_end)
+		{
+			__gfx_block_copy_8(pDst, pSrc);
+			pDst += 8;
+			pSrc += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pBuf->pFB[x + y] = pObj->pBG[x + y];
+		}
+	}
+#endif
+
+}
+
+void GFX_box_draw_direct(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color)
+{
+	UINT32 xmin, xmax, xmax8, ymin, ymax, x, y;
+	UINT32 *pFB, *pFB_end;
+	buf_t *pBuf;
+
+	pBuf = pObj->pBuf->pLast;
+
+	xmin = posx;
+	xmax = pBox->w + posx - 0;
+	if (xmax >= pObj->w)
+		xmax = pObj->w - 0;
+
+	ymin = posy;
+	ymax = pBox->h + posy - 0;
+	if (ymax >= pObj->h)
+		ymax = pObj->h - 0;
+
+	ymin *= pObj->w;
+	ymax *= pObj->w;
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += pObj->w)
+	{
+		pFB = &pBuf->pFB[xmin + y];
+		pFB_end = &pBuf->pFB[xmax8 + y];
+		while(pFB < pFB_end)
+		{
+			__gfx_block_set_8(pFB, color);
+			pFB += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pBuf->pFB[x + y] = color;
+		}
+	}
+
+}
+
+void GFX_box_draw(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color)
+{
+	UINT32 xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
+	UINT32 *pFB, *pFB_end;
+	buf_t *pBuf;
+
+	xmin = posx;
+	xmax = pBox->w + posx - 0;
+	if (xmax >= pObj->w)
+		xmax = pObj->w - 0;
+
+	ymin = posy;
+	ymax = pBox->h + posy - 0;
+	if (ymax >= pObj->h)
+		ymax = pObj->h - 0;
+
+	nx = xmax - xmin;
+	ny = ymax - ymin;
+
+//	fprintf(stderr, "GFX_box_draw(): x=%d, y=%d, nx=%d, ny=%d\n", posx, posy, nx, ny);
+
+	pBuf = pObj->pBuf->pNext;
+
+	// Lopp over all frame buffers, except current back buffer
+	while(pBuf != pObj->pBuf)
+	{
+		GFX_DirtyRectUpdate(pObj, pBuf, xmin, xmax-1, ymin, ymax-1);
+		pBuf = pBuf->pNext;
+
+	}
+	pBuf = pObj->pBuf;
+
+	ymin *= pObj->w;
+	ymax *= pObj->w;
+
+#ifdef USE_HW_BLITTER
+
+	pFB = (UINT32*)&pBuf->pFB[xmin + ymin];
+
+	g_pRegs->pVGA_dst = (UINT32)pFB;
+	g_pRegs->pVGA_dst_dimx = (UINT32)pObj->w;
+	g_pRegs->pVGA_nx = (UINT32)nx;
+	g_pRegs->pVGA_ny = (UINT32)ny;
+	g_pRegs->pVGA_color = color;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);
+
+
+#else
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += pObj->w)
+	{
+		pFB = &pBuf->pFB[xmin + y];
+		pFB_end = &pBuf->pFB[xmax8 + y];
+		while(pFB < pFB_end)
+		{
+			__gfx_block_set_8(pFB, color);
+			pFB += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pBuf->pFB[x + y] = color;
+		}
+	}
+
+#endif
+}
+
+void GFX_box_draw_bg(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color)
+{
+	UINT32 xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
+	UINT32 *pFB, *pFB_end;
+
+	xmin = posx;
+	xmax = pBox->w + posx - 0;
+	if (xmax >= pObj->w)
+		xmax = pObj->w - 0;
+
+	ymin = posy;
+	ymax = pBox->h + posy - 0;
+	if (ymax >= pObj->h)
+		ymax = pObj->h - 0;
+
+	nx = xmax - xmin;
+	ny = ymax - ymin;
+
+	ymin *= pObj->w;
+	ymax *= pObj->w;
+
+
+#ifdef USE_HW_BLITTER
+
+	pFB = (UINT32*)&pObj->pBG[xmin + ymin];
+
+	g_pRegs->pVGA_dst = (UINT32)pFB;
+	g_pRegs->pVGA_dst_dimx = (UINT32)pObj->w;
+	g_pRegs->pVGA_nx = (UINT32)nx;
+	g_pRegs->pVGA_ny = (UINT32)ny;
+	g_pRegs->pVGA_color = color;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);
+
+#else
+
+	xmax8 = xmax - ((xmax-xmin)%8);
+	for (y=ymin; y < ymax; y += pObj->w)
+	{
+		pFB = &pObj->pBG[xmin + y];
+		pFB_end = &pObj->pBG[xmax8 + y];
+		while(pFB < pFB_end)
+		{
+			__gfx_block_set_8(pFB, color);
+			pFB += 8;
+		}
+		// Do remainder
+		for (x=xmax8; x < xmax; x++)
+		{
+			pObj->pBG[x + y] = color;
+		}
+	}
+#endif
+
+}
+
+void GFX_box_blit_bg(gfx_t *pObj, box_t *pBox, UINT32 pos_x_src, UINT32 pos_y_src, UINT32 pos_x_dst, UINT32 pos_y_dst)
+{
+
+	UINT32 x_src, y_src, xmax8_src;
+	UINT32 x_dst, y_dst, xmax8_dst;
+	UINT32 xmin_src, xmax_src, ymin_src, ymax_src;
+	UINT32 xmin_dst, xmax_dst, ymin_dst, ymax_dst;
+	UINT32 *pDst, *pDst_end;
+	UINT32 *pSrc, *pSrc_end;
+	buf_t *pBuf;
+	pBuf = pObj->pBuf->pNext;
+
+	xmin_src = pos_x_src;
+	xmax_src = pBox->w + pos_x_src - 0;
+	if (xmax_src >= pObj->w)
+		xmax_src = pObj->w - 0;
+
+	ymin_src = pos_y_src;
+	ymax_src = pBox->h + pos_y_src - 0;
+	if (ymax_src >= pObj->h)
+		ymax_src = pObj->h - 0;
+
+	xmin_dst = pos_x_dst;
+	xmax_dst = pBox->w + pos_x_dst - 0;
+	if (xmax_dst >= pObj->w)
+		xmax_dst = pObj->w - 0;
+
+	ymin_dst = pos_y_dst;
+	ymax_dst = pBox->h + pos_y_dst - 0;
+	if (ymax_dst >= pObj->h)
+		ymax_dst = pObj->h - 0;
+
+	// Lopp over all frame buffers, except current back buffer
+	while(pBuf != pObj->pBuf)
+	{
+		GFX_DirtyRectUpdate(pObj, pBuf, xmin_dst, xmax_dst, ymin_dst, ymax_dst);
+		pBuf = pBuf->pNext;
+
+	}
+	pBuf = pObj->pBuf;
+
+#ifdef USE_HW_BLITTER
+	pSrc = (UINT32*)&pObj->pBG[xmin_src + ymin_src*pObj->w];
+	pDst = (UINT32*)&pBuf->pFB[xmin_dst + ymin_dst*pObj->w];
+
+	g_pRegs->pVGA_src0 = (UINT32)pSrc;
+	g_pRegs->pVGA_src0_dimx = pObj->w;
+	g_pRegs->pVGA_dst = (UINT32)pDst;
+	g_pRegs->pVGA_dst_dimx = pObj->w;
+	g_pRegs->pVGA_nx = pBox->w;
+	g_pRegs->pVGA_ny = pBox->h;
+
+	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);
+
+	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
+	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;
+
+#else
+
+	ymin_src *= pObj->w;
+	ymax_src *= pObj->w;
+	ymin_dst *= pObj->w;
+	ymax_dst *= pObj->w;
+
+	y_dst = ymin_dst;
+	xmax8_src = xmax_src - ((xmax_src-xmin_src)%8);
+	xmax8_dst = xmax_dst - ((xmax_dst-xmin_dst)%8);
+	for (y_src=ymin_src; y_src < ymax_src; y_src += pBuf->w)
+	{
+		pSrc = &pObj->pBG[xmin_src + y_src];
+		pSrc_end = &pObj->pBG[xmax8_src + y_src];
+		pDst = &pBuf->pFB[xmin_dst + y_dst];
+		pDst_end = &pBuf->pFB[xmax8_dst + y_dst];
+		while(pDst < pDst_end)
+		{
+			__gfx_block_copy_8(pDst, pSrc);
+			pDst += 8;
+			pSrc += 8;
+		}
+		// Do remainder
+		for (x_src=xmax8_src; x_src < xmax_src; x_src++)
+		{
+			*(pDst++) = *(pSrc++);
+		}
+		y_dst += pBuf->w;
+
+	}
+
+#endif
+}
@@ -0,0 +1,93 @@
+#ifndef GFX_H
+#define GFX_H
+
+#include "libsys.h"
+
+#define	GFX_NUM_FRAME_BUFFERS	2
+#define	GFX_FRAME_NOSYNC		0
+#define	GFX_FRAME_SYNC			1
+
+// -------------------------------------------------------------
+// Types
+// -------------------------------------------------------------
+typedef struct _sbox_t
+{
+	UINT32 w, h;
+} box_t;
+
+typedef struct _srect_t
+{
+	UINT32 xmin, xmax;
+	UINT32 ymin, ymax;
+} rect_t;
+
+typedef struct _svga_hw_t
+{
+	UINT32 volatile pVGA_ctrl;			// 0xA0030000	// R/W
+	UINT32 volatile pVGA_ascii;			// 0xA0030004	// R/W
+	UINT32 volatile pVGA_posx;			// 0xA0030008	// R/W
+	UINT32 volatile pVGA_posy;			// 0xA003000C	// R/W
+	UINT32 volatile pVGA_cgcol;			// 0xA0030010	// R/W
+	UINT32 volatile pVGA_res;			// 0xA0030014	// RO
+	UINT32 volatile pVGA_front;			// 0xA0030018	// R/W
+	UINT32 volatile pVGA_back;			// 0xA003001C	// R/W
+	UINT32 volatile pVGA_src0;			// 0xA0030020	// R/W
+	UINT32 volatile pVGA_src0_dimx;		// 0xA0030024	// R/W
+	UINT32 volatile pVGA_gap0;			// 0xA0030028	// R/W
+	UINT32 volatile pVGA_gap1;			// 0xA003002C	// R/W
+	UINT32 volatile pVGA_src1;			// 0xA0030030	// R/W
+	UINT32 volatile pVGA_src1_dimx;		// 0xA0030034	// R/W
+	UINT32 volatile pVGA_gap2;			// 0xA0030038	// R/W
+	UINT32 volatile pVGA_gap3;			// 0xA003003C	// R/W
+	UINT32 volatile pVGA_dst;			// 0xA0030040	// R/W
+	UINT32 volatile pVGA_dst_dimx;		// 0xA0030044	// R/W
+	UINT32 volatile pVGA_gap4;			// 0xA0030048	// R/W
+	UINT32 volatile pVGA_gap5;			// 0xA003004C	// R/W
+	UINT32 volatile pVGA_nx;			// 0xA0030050	// R/W
+	UINT32 volatile pVGA_ny;			// 0xA0030054	// R/W
+	UINT32 volatile pVGA_color;			// 0xA0030058	// R/W
+
+} vga_hw_t;
+
+typedef struct _sbuf_t
+{
+	UINT32 *pFB;
+	struct _sbuf_t *pNext;
+	struct _sbuf_t *pLast;
+	UINT32 is_dirty;
+	rect_t dirty;
+	UINT32 w, h;
+} buf_t;
+
+typedef struct _sgfx_t
+{
+	UINT32 w, h;
+	buf_t *pBuf;
+	UINT32 *pBG;
+	rect_t restore;
+
+} gfx_t;
+
+// -------------------------------------------------------------
+// Functions
+// -------------------------------------------------------------
+void __gfx_block_set_8(UINT32* pDst, UINT32 value);
+void __gfx_block_copy_8(UINT32* pDst, UINT32* pSrc);
+void _GFX_copy32(UINT32* pDst, UINT32* pSrc, UINT32 ndwords);
+void _GFX_copy32_hw(UINT32* pDst, UINT32* pSrc, UINT32 ndwords, UINT32 wait_finish);
+void _GFX_memset32(UINT32* pDst, UINT32 value, UINT32 ndwords);
+void _GFX_memset32_hw(UINT32* pDst, UINT32 value, UINT32 ndwords, UINT32 wait_finish);
+void _GFX_BLIT_const(UINT32* pDst, UINT32 xdim_dst, UINT32 xmin, UINT32 ymin, UINT32 xmax, UINT32 ymax, UINT32 value);
+void _GFX_BLIT_const_hw(UINT32* pDst, UINT32 xdim_dst, UINT32 xmin, UINT32 ymin, UINT32 xmax, UINT32 ymax, UINT32 value, UINT32 wait_finish);
+void GFX_init(gfx_t *pObj);
+void GFX_frame(gfx_t *pObj, UINT32 sync_mode);
+void GFX_show(gfx_t *pObj);
+void GFX_copy_front2back(gfx_t *pObj);
+void GFX_get_FB_front(gfx_t *pObj, UINT32 **ppFB);
+void box_create(box_t *pObj, UINT32 w, UINT32 h);
+void GFX_restore(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy);
+void GFX_box_draw(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color);
+void GFX_box_draw_bg(gfx_t *pObj, box_t *pBox, UINT32 posx, UINT32 posy, UINT32 color);
+void GFX_box_blit_bg(gfx_t *pObj, box_t *pBox, UINT32 pos_x_src, UINT32 pos_y_src, UINT32 pos_x_dst, UINT32 pos_y_dst);
+
+#endif // GFX_H
@@ -0,0 +1,268 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "libsys.h"
+#include "mips_ps2.h"
+
+ps2_if_t ps2_if[2] =
+{
+	{ps2_type_unknown, (UINT32*)SYS_PS2_0_STAT, (UINT32*)SYS_PS2_0_DATA},
+	{ps2_type_unknown, (UINT32*)SYS_PS2_1_STAT, (UINT32*)SYS_PS2_1_DATA}
+};
+
+UINT32 PS2_get_num_if(void)
+{
+	return (sizeof(ps2_if)/sizeof(ps2_if_t));
+}
+
+ps2_if_t *PS2_get_if(UINT32 port)
+{
+	if (port < PS2_get_num_if())
+		return &ps2_if[port];
+
+	return NULL;
+}
+
+void PS2_byte_write(UINT32 port, UINT8 byte)
+{
+	ps2_if_t *pIF;
+
+	pIF = &ps2_if[port];
+
+	while(!(*pIF->pCTRL & SYS_PS2_BIT_TX_EMPTY));
+
+	*pIF->pDATA = (UINT32)byte;
+}
+
+UINT32 PS2_byte_read(UINT32 port, UINT32 timeout_ms)
+{
+	ps2_if_t *pIF;
+
+	pIF = &ps2_if[port];
+
+	while(!(*pIF->pCTRL & SYS_PS2_BIT_RX_AVAIL))
+	{
+		timeout_ms--;
+		if (!timeout_ms)
+			return PS2_ERR_TIMEOUT;
+
+		sleep(1);
+	}
+	return (UINT32)*pIF->pDATA;
+}
+
+UINT32 PS2_cmd(UINT32 port, UINT8 cmd, UINT32 ack_timeout_ms)
+{
+	UINT32 result;
+
+	PS2_byte_write(port, cmd);
+	result = PS2_byte_read(port, ack_timeout_ms);
+	if (0xFA != result)
+	{
+		printf ("PS2_cmd(%d): no ACK received. Result = %08X\n", port, result);
+		return PS2_ERR_PS2_PREFIX | result;
+	}
+	return 0;
+}
+
+void PS2_flush(UINT32 port)
+{
+	while (!IS_ERROR(PS2_byte_read(port, 10)));
+}
+
+UINT32 PS2_init(UINT32 port)
+{
+	int i;
+	ps2_if_t *pIF;
+	UINT8 id[2];
+
+	pIF = &ps2_if[port];
+	pIF->type = ps2_type_unknown;
+
+	UINT32 result;
+
+	// Disable RX-interrupt
+	*pIF->pCTRL &= ~SYS_PS2_BIT_RX_INTEN;
+
+	printf ("PS2_init(%d) - Initialize... ", port);
+
+	// Flush buffer
+	PS2_flush(port);
+
+	// send RESET
+	result = PS2_cmd(port, 0xFF, 1000);
+	if (IS_ERROR(result))
+	{
+		printf ("Reset error %08X\n", result);
+		return result;
+	}
+
+	// Read BAT
+	result = PS2_byte_read(port, 1000);
+	if (0xAA != result)
+	{
+		printf ("BAT error %08X\n", result);
+		return result;
+	}
+
+	printf ("Success\n");
+
+	while(1)
+	{
+		// Read ID
+		result = PS2_byte_read(port, 10);
+		if (IS_ERROR(result))
+		{
+			// No ID => keyboard
+			printf ("PS2_init(%d) - Keyboard detected\n", port);
+
+			// send Get Device-ID
+			result = PS2_cmd(port, 0xF2, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("Get Device-ID error %08X\n", result);
+				break;
+			}
+			id[0] = PS2_byte_read(port, 1000);
+			id[1] = PS2_byte_read(port, 1000);
+			printf ("PS2_init(%d) - Device-ID: %02X%02X\n", port, id[0], id[1]);
+
+			// Get Scan code set (phase 1)
+			result = PS2_cmd(port, 0xF0, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("1. Get Scan code error %08X\n", result);
+				break;
+			}
+			// Get Scan code set (phase 2)
+			result = PS2_cmd(port, 0x00, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("2. Get Scan code error %08X\n", result);
+				break;
+			}
+			result = PS2_byte_read(port, 1000);
+			printf ("PS2_init(%d) - Current scan code set = %d\n", port, result);
+
+			// if Scan code SET != 2
+			if (result != 2)
+			{
+				printf ("PS2_init(%d) - Set scan code set = 2\n", port);
+				// Set Scan code set (phase 1)
+				result = PS2_cmd(port, 0xF0, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("1. Set Scan code error %08X\n", result);
+					break;
+				}
+				// Set Scan code set (phase 2)
+				result = PS2_cmd(port, 0x02, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("2. Set Scan code error %08X\n", result);
+					break;
+				}
+			}
+
+			// Test LEDs
+			for (i=0; i < 4; i++)
+			{
+				result = PS2_cmd(port, 0xED, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("1. LED error %08X\n", result);
+					break;
+				}
+				result = PS2_cmd(port, 0x02, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("2. LED error %08X\n", result);
+					break;
+				}
+				sleep(100);
+				result = PS2_cmd(port, 0xED, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("1. LED error %08X\n", result);
+					break;
+				}
+				result = PS2_cmd(port, 0x04, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("2. LED error %08X\n", result);
+					break;
+				}
+				sleep(100);
+				result = PS2_cmd(port, 0xED, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("1. LED error %08X\n", result);
+					break;
+				}
+				result = PS2_cmd(port, 0x01, 1000);
+				if (IS_ERROR(result))
+				{
+					printf ("2. LED error %08X\n", result);
+					break;
+				}
+				sleep(100);
+			}
+			result = PS2_cmd(port, 0xED, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("1. LED error %08X\n", result);
+				break;
+			}
+			result = PS2_cmd(port, 0x00, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("2. LED error %08X\n", result);
+				break;
+			}
+
+			pIF->type = ps2_type_keyb;
+			result = pIF->type;
+			break;
+		}
+
+		// We got a byte => maybe mouse
+		if (result == 0)
+		{
+			// We got zero => yes, it's a mouse
+			printf ("PS2_init(%d) - Mouse device detected\n", port);
+
+			// send Get Device-ID
+			result = PS2_cmd(port, 0xF2, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("Get Device-ID error %08X\n", result);
+				break;
+			}
+			id[0] = PS2_byte_read(port, 1000);
+			printf ("PS2_init(%d) - Device-ID: %02X\n", port, id[0]);
+			printf ("PS2_init(%d) - Enable stream mode...", port);
+			result = PS2_cmd(port, 0xF4, 1000);
+			if (IS_ERROR(result))
+			{
+				printf ("error %08X\n", result);
+				break;
+			}
+			printf ("Success\n");
+			pIF->type = ps2_type_mouse;
+			result = pIF->type;
+			break;
+		}
+	}
+
+	// Flush buffer
+	PS2_flush(port);
+
+	if (IS_ERROR(result))
+		return result;
+
+	// Enable RX-interrupt
+	*pIF->pCTRL |= SYS_PS2_BIT_RX_INTEN;
+
+	return result;
+
+}
@@ -0,0 +1,43 @@
+#ifndef PS2_H
+#define PS2_H
+
+#include "libsys.h"
+
+// --------------------------------------------------
+// Types
+// --------------------------------------------------
+enum
+{
+	ps2_type_unknown = 0,
+	ps2_type_keyb,
+	ps2_type_mouse
+};
+
+typedef struct _sps2_if_t
+{
+	UINT32 type;
+	volatile UINT32 *pCTRL;
+	volatile UINT32 *pDATA;
+} ps2_if_t;
+
+// --------------------------------------------------
+// Constants
+// --------------------------------------------------
+#define	PS2_MODULE_PREFIX	0x1000000
+#define	PS2_SUCCESS			LSYS_SUCCESS
+#define	PS2_ERROR			(LSYS_ERR_BASE | PS2_MODULE_PREFIX)
+#define	PS2_ERR_TIMEOUT		(PS2_ERROR | 0x0000001)
+#define	PS2_ERR_PS2_PREFIX	(PS2_ERROR | 0x0010000)
+
+// --------------------------------------------------
+// Functions
+// --------------------------------------------------
+UINT32 PS2_get_num_if(void);
+ps2_if_t *PS2_get_if(UINT32 port);
+UINT32 PS2_init(UINT32 port);
+void PS2_byte_write(UINT32 port, UINT8 byte);
+UINT32 PS2_byte_read(UINT32 port, UINT32 timeout_ms);
+UINT32 PS2_cmd(UINT32 port, UINT8 cmd, UINT32 ack_timeout_ms);
+void PS2_flush(UINT32 port);
+
+#endif // PS2_H
@@ -0,0 +1,81 @@
+/*
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 1985 MIPS Computer Systems, Inc.
+ * Copyright (C) 1994, 95, 99, 2003 by Ralf Baechle
+ * Copyright (C) 1990 - 1992, 1999 Silicon Graphics, Inc.
+ */
+#ifndef _ASM_REGDEF_H
+#define _ASM_REGDEF_H
+
+/*
+ * Symbolic register names for 32 bit ABI
+ */
+#define zero			$0      /* wired zero */
+#define AT				$1      /* assembler temp  - uppercase because of ".set at" */
+#define v0				$2      /* return value */
+#define v1				$3
+#define a0				$4      /* argument registers */
+#define a1				$5
+#define a2				$6
+#define a3				$7
+#define t0				$8      /* caller saved */
+#define t1				$9
+#define t2				$10
+#define t3				$11
+#define t4				$12
+#define t5				$13
+#define t6				$14
+#define t7				$15
+#define s0				$16     /* callee saved */
+#define s1				$17
+#define s2				$18
+#define s3				$19
+#define s4				$20
+#define s5				$21
+#define s6				$22
+#define s7				$23
+#define t8				$24     /* caller saved */
+#define t9				$25
+#define jp				$25     /* PIC jump register */
+#define k0				$26     /* kernel scratch */
+#define k1				$27
+#define gp				$28     /* global pointer */
+#define sp				$29     /* stack pointer */
+#define fp				$30     /* frame pointer */
+#define s8				$30		/* same like fp! */
+#define ra				$31     /* return address */
+
+/* CP0 registers */
+#define	CP0_INDEX		$0		/* R3000 available */
+#define	CP0_RANDOM		$1		/* R3000 available */
+#define	CP0_ENTRYLO		$2		/* R3000 available */
+#define	CP0_CONTEXT		$4		/* R3000 available */
+#define	CP0_BADDR		$8		/* R3000 available */
+#define	CP0_ENTRYHI		$10		/* R3000 available */
+#define	CP0_SR			$12		/* R3000 available */
+#define	CP0_CR			$13		/* R3000 available */
+#define	CP0_EPC			$14		/* R3000 available */
+#define	CP0_PRID		$15		/* R3000 available */
+
+/* Status register masks */
+#define	SR_MASK_IEC		0x00000001
+#define	SR_MASK_KUC		0x00000002
+#define	SR_MASK_IEP		0x00000004
+#define	SR_MASK_KUP		0x00000008
+#define	SR_MASK_IEO		0x00000010
+#define	SR_MASK_KUO		0x00000020
+#define	SR_MASK_IM		0x0000FF00
+#define	SR_MASK_DS		0x00FF0000
+#define	SR_MASK_RE		0x01000000
+#define	SR_MASK_CU		0xF0000000
+
+/* Cause register masks */
+#define	CR_MASK_EXC		0x0000007C
+#define	CR_MASK_IP		0x0000FF00
+#define	CR_MASK_CE		0x40000000
+#define	CR_MASK_BD		0x80000000
+
+#endif /* _ASM_REGDEF_H */
@@ -0,0 +1,87 @@
+		.file	"startup.S"
+	
+		.section .rodata
+		.data
+argv0:		.asciz		"\"This-MIPS-program\""
+argv:		.word		argv0	
+
+		.text
+		.section .start, "ax"
+		.extern _init
+		.extern libsys_init
+		.extern _exc_vect_start
+		.extern _xcpt_handler
+		.align	2
+		.globl _start
+_start:
+		.set	noreorder
+		j	init
+		nop		
+		.set	reorder
+
+		.text
+		.set	noreorder
+init:
+		# Set stack pointer 
+		la $sp, stack_ptr
+		# Set global pointer 
+		la $gp, _gp
+
+		# zero bss
+		la $8, __bss_start
+		la $9, _end
+$zeroise:		
+		sw $0, 0($8)
+		bne $8, $9, $zeroise
+		addiu	$8, 4
+
+		# Install exception vector jump code
+		la $t1, _exc_vect_copy_start
+		la $t2, _exc_vect_copy_end
+		la $t0, _exc_vect_start
+
+$install:	lw	$v0, 0($t1)
+		addiu	$t1, 4
+		sw	$v0, 0($t0)
+		bne	$t1, $t2, $install
+		addiu	$t0, 4
+		
+		# Call _init
+		la $k0, _init
+		jalr $k0
+		nop
+		
+		# Call libsys_init
+		la $k0, libsys_init
+		jalr $k0
+		nop
+
+		# Set kernel-mode and enable interrupts
+		mfc0	$a0, $12
+		li	$a1, 3
+		or	$a0, $a1
+		mtc0	$a0, $12
+		
+		# Set environment
+		li	$a0, 1
+		la	$a1, argv
+
+		# Call main
+		la $k0, main
+		jalr $k0
+		nop
+
+		# Terminate after main returns
+_terminate:	
+		nop
+		la $t0, exit
+		jalr $t0
+		move $a0, $v0
+
+_exc_vect_copy_start:
+		la $k0, _xcpt_handler
+		jr $k0
+		nop
+_exc_vect_copy_end:
+		
+		.set	reorder
@@ -0,0 +1,86 @@
+#include <stdio.h>
+#include "libsys.h"
+#include "xcpt.h"
+
+/* 10 digits + 1 sign + 1 trailing nul */
+static char itoa_buf[12];
+
+char *itoa(int i)
+{
+	char *pos = itoa_buf + sizeof(itoa_buf) - 1;
+	unsigned int u;
+	int negative = 0;
+
+	if (i < 0)
+	{
+		negative = 1;
+		u = ((unsigned int)(-(1+i))) + 1;
+	}
+	else
+	{
+		u = i;
+	}
+
+	*pos = 0;
+
+	do
+	{
+		*--pos = '0' + (u % 10);
+		u /= 10;
+	} while (u);
+
+	if (negative)
+	{
+		*--pos = '-';
+	}
+
+	return pos;
+}
+
+int syscalls_handler(struct xcptcontext * xcp)
+{
+	int syscall_code;
+	char *pStr, buf[33];
+
+	syscall_code = xcp->regs[2];
+
+	switch(syscall_code)
+	{
+		case 1:		// print integer
+		sputs(itoa(xcp->regs[4]));
+		break;
+
+		case 4:		// print string
+		pStr = (char*)xcp->regs[4];
+		sputs(pStr);
+		break;
+
+		case 5:		// read integer
+		pStr = fgets(buf, sizeof(buf), stdin);
+		xcp->regs[2] = atoi(pStr);
+		break;
+
+		case 8:		// read string
+		pStr = (char*)xcp->regs[4];
+		fgets(pStr, xcp->regs[5], stdin);
+		break;
+
+		case 10:		// exit
+		_exit(0);
+		break;
+
+		default:
+		sputs("\nUnknown syscall (");print_word(syscall_code);sputs(") at ");print_word(xcp->epc);sputs("\n");
+		break;
+	}
+
+	xcp->epc += 4;	// set return address to instruction after syscall
+
+	return 0;
+}
+
+void syscalls_init(void)
+{
+	xcpt_register(EXC_SYSCALL, syscalls_handler);
+}
+
@@ -0,0 +1,146 @@
+#include <sys/times.h>
+#include <sys/time.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <stdlib.h>
+#include "libsys.h"
+#include "xcpt.h"
+#include "irq.h"
+
+static fp_xcpt_t g_xcpt_handler[MAX_NUM_XCPT] = {NULL};
+
+char *_xcpt_code_str[MAX_NUM_XCPT] =
+{
+	"Interrupt", "Mod", "TLBL", "TLBS", "AdEL", "AdES", "IBE", "DBE",
+	"Syscall", "Break", "RI", "CpU", "Ov", "Tr", "NCD/VCEI", "MC/FPE",
+	"Res(16)", "Res(17)", "Res(18)", "Res(19)", "Res(20)", "Res(21)", "Res(22)", "WATCH",
+	"Res(24)", "Res(25)", "Res(26)", "Res(27)", "Res(28)", "Res(29)", "Res(30)", "VCED"
+ };
+
+
+void xcpt_register(int xcpt_num, fp_xcpt_t fp)
+{
+	if ((xcpt_num >= EXC_INT) && (xcpt_num <= EXC_VCED))
+		g_xcpt_handler[xcpt_num] = fp;
+
+}
+
+void xcpt_registers_print(struct xcptcontext * xcp)
+{
+	PRINT_REG(" Status : ", xcp->sr);
+	PRINT_REG("  Cause : ", xcp->cr);
+	PRINT_REG("    EPC : ", xcp->epc);
+	PRINT_REG("BadAddr : ", xcp->baddr);
+	sputs("\n");
+	PRINT_REG("   MDLO : ", xcp->mdlo);
+	PRINT_REG("   MDHI : ", xcp->mdhi);
+
+	sputs("\n");
+
+	sputs("Registers:\n");
+	PRINT_REG(" 0 (ze) : ", xcp->regs[0]);
+	PRINT_REG(" 1 (at) : ", xcp->regs[1]);
+	PRINT_REG(" 2 (v0) : ", xcp->regs[2]);
+	PRINT_REG(" 3 (v1) : ", xcp->regs[3]);
+	sputs("\n");
+	PRINT_REG(" 4 (a0) : ", xcp->regs[4]);
+	PRINT_REG(" 5 (a1) : ", xcp->regs[5]);
+	PRINT_REG(" 6 (a2) : ", xcp->regs[6]);
+	PRINT_REG(" 7 (a3) : ", xcp->regs[7]);
+	sputs("\n");
+	PRINT_REG(" 8 (t0) : ", xcp->regs[8]);
+	PRINT_REG(" 9 (t1) : ", xcp->regs[9]);
+	PRINT_REG("10 (t2) : ", xcp->regs[10]);
+	PRINT_REG("11 (t3) : ", xcp->regs[11]);
+	sputs("\n");
+	PRINT_REG("12 (t4) : ", xcp->regs[12]);
+	PRINT_REG("13 (t5) : ", xcp->regs[13]);
+	PRINT_REG("14 (t6) : ", xcp->regs[14]);
+	PRINT_REG("15 (t7) : ", xcp->regs[15]);
+	sputs("\n");
+	PRINT_REG("16 (s0) : ", xcp->regs[16]);
+	PRINT_REG("17 (s1) : ", xcp->regs[17]);
+	PRINT_REG("18 (s2) : ", xcp->regs[18]);
+	PRINT_REG("19 (s3) : ", xcp->regs[19]);
+	sputs("\n");
+	PRINT_REG("20 (s4) : ", xcp->regs[20]);
+	PRINT_REG("21 (s5) : ", xcp->regs[21]);
+	PRINT_REG("22 (s6) : ", xcp->regs[22]);
+	PRINT_REG("23 (s7) : ", xcp->regs[23]);
+	sputs("\n");
+	PRINT_REG("24 (t8) : ", xcp->regs[24]);
+	PRINT_REG("25 (t9) : ", xcp->regs[25]);
+	PRINT_REG("26 (k0) : ", xcp->regs[26]);
+	PRINT_REG("27 (k1) : ", xcp->regs[27]);
+	sputs("\n");
+	PRINT_REG("28 (gp) : ", xcp->regs[28]);
+	PRINT_REG("29 (sp) : ", xcp->regs[29]);
+	PRINT_REG("30 (fp) : ", xcp->regs[30]);
+	PRINT_REG("31 (ra) : ", xcp->regs[31]);
+	sputs("\n");
+}
+
+int xcpt_get_type(struct xcptcontext * xcp)
+{
+	return (xcp->cr >> 2) & 0x1F;
+}
+
+void xcpt_exctype_print(struct xcptcontext * xcp)
+{
+	int exc_code;
+
+	exc_code = xcpt_get_type(xcp);
+	sputs(_xcpt_code_str[exc_code]);
+
+}
+
+int _xcpt_dispatch(struct xcptcontext * xcp)
+{
+	int result;
+	int xcpt_code;
+
+	xcpt_code =  (xcp->cr >> 2) & 0x1F;
+
+	result = XCPT_ERR_NOTHANDLED;
+	if (g_xcpt_handler[xcpt_code])
+	{
+		result = (g_xcpt_handler[xcpt_code])(xcp);
+	}
+	return result;
+}
+
+int _xcpt_deliver(struct xcptcontext * _xcp)
+{
+
+	int exc_code;
+	volatile int *pInstr;
+
+	/* This function gets called by the low-level exception handler. */
+	if (_xcpt_dispatch(_xcp) == XCPT_ERR_NOTHANDLED)
+	{
+		sputs("\n");
+		xcpt_registers_print(_xcp);
+		sputs("\n");
+
+		exc_code = (_xcp->cr >> 2) & 0x1F;
+
+		if (_xcp->cr & 0x80000000)
+		{
+			pInstr = (int*)(_xcp->epc + 4);
+		}
+		else
+		{
+			pInstr = (int*)(_xcp->epc);
+		}
+		sputs("Unhandled exception <");xcpt_exctype_print(_xcp); sputs("> at PC : ");print_word((int)pInstr);sputs("\n");
+
+		sputs("Instruction at exception address : ");
+		print_word(*pInstr);
+		sputs("\n");
+
+		sputs("Terminate.\n");
+		_exit(exc_code);
+	}
+
+	return 0;
+}
@@ -0,0 +1,58 @@
+#ifndef XCPT_H
+#define XCPT_H
+
+#define MAX_NUM_XCPT			32
+#define	XCPT_ERR_NOTHANDLED		0x80000000
+
+typedef enum
+{
+	EXC_INT = 0, EXC_MOD = 1, EXC_TLBL = 2, EXC_TLBS = 3, EXC_ADEL = 4, EXC_ADES = 5,
+	EXC_IBE = 6, EXC_DBE = 7, EXC_SYSCALL = 8, EXC_BP = 9, EXC_RI = 10, EXC_CPU = 11,
+	EXC_OV = 12, EXC_TRAP = 13, EXC_VCEI = 14, EXC_FPE = 15, EXC_C2E = 16, EXC_WATCH = 23, EXC_VCED = 31
+
+} EXCEPTION_CODE;
+
+typedef unsigned int reg_t;
+
+typedef struct xcptcontext
+{
+	/* This is the exception context frame that is passed to the exception
+	handlers. It gets filled in by the low-level exception handler in
+	"machine.S". An assembler version of this structure can be found at the
+	bottom of "machine.H".*/
+	reg_t sr;					/* Status Register */
+	reg_t cr;					/* Cause Register */
+	reg_t epc;					/* PC at time of exception. */
+	reg_t baddr;
+	reg_t regs[32];				/* Copy of all general purpose registers */
+	reg_t mdlo;					/* HI/LO registers (used for memory management) */
+	reg_t mdhi;
+	reg_t count;				/* Timer registers */
+	reg_t compare;
+	struct xcptcontext * prev;	/* To link exceptions. (unused for now) */
+	unsigned xclass;			/* Priority class of this exception. (unused for now). */
+
+} EXCEPTION_CONTEXT;
+
+typedef int (*fp_xcpt_t)(struct xcptcontext * xcp);
+
+#define PRINT_REG(tag_str, reg)			\
+			sputs(tag_str);				\
+			print_word(reg);			\
+			sputs("   ");
+
+
+#define PRINT_REG_CHG(tag_str, reg, chg_tag)			\
+			sputs(tag_str);				\
+			print_word(reg);			\
+			sputs(chg_tag);				\
+			sputs("  ");
+
+
+// Public functions
+void xcpt_register(int xcpt_num, fp_xcpt_t fp);
+int xcpt_get_type(struct xcptcontext * xcp);
+void xcpt_registers_print(struct xcptcontext * xcp);
+void xcpt_exctype_print(struct xcptcontext * xcp);
+
+#endif // XCPT_H
@@ -0,0 +1,67 @@
+#ifndef XCPT_ASM_H
+#define XCPT_ASM_H
+
+/* LEAF - declare leaf routine */
+#define	LEAF(symbol)						\
+			.globl	symbol;					\
+			.align	2;						\
+			.type	symbol, @function;		\
+			.ent	symbol, 0;				\
+symbol:		.frame	sp, 0, ra
+
+/* END - mark end of function */
+#define	END(function)						\
+			.end	function;				\
+			.size	function, .-function
+
+/* Exception stack size */
+#define	XCP_SIZE		32768
+
+/* save location for registers */
+#define	XCP_SR			0
+#define	XCP_CR			4
+#define	XCP_EPC			8
+#define	XCP_BADDR		12
+#define	XCP_ZERO		16
+#define	XCP_AT			20
+#define	XCP_V0			24
+#define	XCP_V1			28
+#define	XCP_A0			32
+#define	XCP_A1			36
+#define	XCP_A2			40
+#define	XCP_A3			44
+#define	XCP_T0			48
+#define	XCP_T1			52
+#define	XCP_T2			56
+#define	XCP_T3			60
+#define	XCP_T4			64
+#define	XCP_T5			68
+#define	XCP_T6			72
+#define	XCP_T7			76
+#define	XCP_S0			80
+#define	XCP_S1			84
+#define	XCP_S2			88
+#define	XCP_S3			92
+#define	XCP_S4			96
+#define	XCP_S5			100
+#define	XCP_S6			104
+#define	XCP_S7			108
+#define	XCP_T8			112
+#define	XCP_T9			116
+#define	XCP_JP			116
+#define	XCP_K0			120
+#define	XCP_K1			124
+#define	XCP_GP			128
+#define	XCP_SP			132
+#define	XCP_S8			136
+#define	XCP_FP			136
+#define	XCP_RA			140
+#define	XCP_MDLO		144
+#define	XCP_MDHI		148
+#define	XCP_COUNT		152
+#define	XCP_COMPARE		156
+#define	XCP_PREV		160
+#define	XCP_CLASS		164
+
+
+#endif /* XCPT_ASM_H */