diff --git a/lib/CPUs/MIPS/bsp/examples/Makefile b/lib/CPUs/MIPS/bsp/examples/Makefile
index 163c4b4..8fe45e5 100644
--- a/lib/CPUs/MIPS/bsp/examples/Makefile
+++ b/lib/CPUs/MIPS/bsp/examples/Makefile
@@ -1,7 +1,6 @@
-CFLAGS=-msoft-float -O2 -march=r3000 -I.
-LFLAGS=-M -T link.ld
-LIB_DIRS=-L . -L $(MIPS_TOOLS_PREFIX)/mipsel-elf/lib -L $(MIPS_TOOLS_PREFIX)/lib/gcc/mipsel-elf/$(MIPS_GCC_VER)
-LIBS=-lc -lm -lgcc
+CFLAGS=-msoft-float -O2 -march=r3000 -I. -Wl,-M
+LIBS=-lc -lm
+
 
 AS=mipsel-elf-as
 AR=mipsel-elf-ar
@@ -15,122 +14,107 @@ PROG = hello testbench test_irq dhry queens stanford paranoia rmd160_test Bessel
 
 all: $(PROG)
 
-libsys: startup.S init.S kernel.S libsys.c xcpt.c irq.c
+libsys: startup.S kernel.S libsys.c xcpt.c irq.c
 	$(CC) $(CFLAGS) -G 0 -c startup.S -o startup.o
 	$(CC) $(CFLAGS) -G 0 -c kernel.S -o kernel.o
-	$(CC) $(CFLAGS) -G 0 -c init.S -o init.o
 	$(CC) $(CFLAGS) -G 0 -c libsys.c -o libsys.o
 	$(CC) $(CFLAGS) -G 0 -c xcpt.c -o xcpt.o
 	$(CC) $(CFLAGS) -G 0 -c irq.c -o irq.o
-	$(AR) -r libsys.a init.o libsys.o xcpt.o irq.o
+	$(AR) -r libsys.a libsys.o xcpt.o irq.o
 	
-hello: hello.c 
-	$(CC) $(CFLAGS) -c hello.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) hello.o -o $@.elf $(LIBS) >$@.map
+hello: hello.c
+	$(CC) $(CFLAGS) -o $@.elf -Os -g hello.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 testbench: testbench.c 
-	$(CC) $(CFLAGS) -c testbench.c paranoia.c -DNOSIGNAL -DBATCHMODE -DNOMAIN
-	$(LD) $(LFLAGS) $(LIB_DIRS) paranoia.o testbench.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DNOSIGNAL -DBATCHMODE -DNOMAIN -Os -g testbench.c paranoia.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 test_irq: test_irq.c 
-	$(CC) $(CFLAGS) -c test_irq.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) test_irq.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf test_irq.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 dhry: dhry_1.c dhry_2.c dhry.h
-	$(CC) $(CFLAGS) -DHZ=1000 -c dhry_1.c dhry_2.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) dhry_1.o dhry_2.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DHZ=1000 dhry_1.c dhry_2.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 queens: queens.c 
-	$(CC) $(CFLAGS) -DUNIX_Old -DHZ=1000 -c queens.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) queens.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DUNIX_Old -DHZ=1000 queens.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 stanford: stanford.c
-	$(CC) $(CFLAGS) -DHZ=1000 -c stanford.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) stanford.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DHZ=1000 stanford.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 paranoia: paranoia.c
-	$(CC) $(CFLAGS) -DNOSIGNAL -DBATCHMODE -g -c paranoia.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) paranoia.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DNOSIGNAL -DBATCHMODE -g paranoia.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 rmd160_test: rmd160_test.c rmd160.c rmd160.h
-	$(CC) $(CFLAGS) -DNOSIGNAL -DBATCHMODE -g -c rmd160.c rmd160_test.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) rmd160.o rmd160_test.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -DNOSIGNAL -DBATCHMODE -g rmd160.c rmd160_test.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 Bessel: Bessel.c 
-	$(CC) $(CFLAGS) -g -c Bessel.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) Bessel.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -g Bessel.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 whet: whet.c 
-	$(CC) $(CFLAGS) -c whet.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) whet.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf whet.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 phrasen: phrasen.c 
-	$(CC) $(CFLAGS) -g -c phrasen.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) phrasen.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -g phrasen.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 dttl: dttl.c random.c
-	$(CC) $(CFLAGS) -g -c dttl.c random.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) random.o dttl.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -g dttl.c random.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 richards_benchmark: richards_benchmark.c 
-	$(CC) $(CFLAGS) -c richards_benchmark.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) richards_benchmark.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf richards_benchmark.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
 	$(FLASHGEN) $@.bin
 
 test_exception: test_exception.c 
-	$(CC) $(CFLAGS) -O1 -c test_exception.c
-	$(LD) $(LFLAGS) $(LIB_DIRS) test_exception.o -o $@.elf $(LIBS) >$@.map
+	$(CC) $(CFLAGS) -o $@.elf -O1 test_exception.c $(LIBS) >$@.map
 	$(OBJDUMP) -d $@.elf > $@.dis
 	$(OBJCOPY) $@.elf -O binary $@.bin
 	$(OBJCOPY) -O srec $@.elf $@.srec
diff --git a/lib/CPUs/MIPS/bsp/examples/bootloader/Makefile b/lib/CPUs/MIPS/bsp/examples/bootloader/Makefile
index 5400352..7bc8833 100644
--- a/lib/CPUs/MIPS/bsp/examples/bootloader/Makefile
+++ b/lib/CPUs/MIPS/bsp/examples/bootloader/Makefile
@@ -1,5 +1,3 @@
-ROM_WORDADDR_WIDTH=11
-
 CFLAGS=-Os -I. -I../ -msoft-float -march=r3000
 LFLAGS=-M -T bootloader.ld
 LIB_DIRS=-L $(MIPS_TOOLS_PREFIX)/mipsel-elf/lib -L $(MIPS_TOOLS_PREFIX)/lib/gcc/mipsel-elf/$(MIPS_GCC_VER)
diff --git a/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader.ld b/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader.ld
index 8ca0c24..2c3fabf 100644
--- a/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader.ld
+++ b/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader.ld
@@ -16,7 +16,7 @@ sys_timer_sec = 0xA000000C;
 
 SECTIONS
 {
-   .stext ORIGIN(rom) :
+   .init ORIGIN(rom) :
    {
       start = ALIGN(4);
       entry = ALIGN(4);
diff --git a/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader_with_flash.c b/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader_with_flash.c
index 07c36ed..94e4bee 100644
--- a/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader_with_flash.c
+++ b/lib/CPUs/MIPS/bsp/examples/bootloader/bootloader_with_flash.c
@@ -9,12 +9,17 @@ typedef struct _ssrec_t
 	UINT8 *data;
 
 } srec_t;
+typedef struct _sflash_alloc_tbl_t
+{
+	UINT32 images[16];
+
+} flash_alloc_tbl_t;
 
 typedef struct _sflash_img_hdr_t
 {
 	UINT8 magic[4];
-	UINT32 target_addr;
-	UINT32 img_offset;
+	UINT32 target_base;
+	UINT32 img_base;
 	UINT32 img_size;
 	UINT32 hdr_next;
 	UINT8 img_name[128];
@@ -207,7 +212,8 @@ void PrintCPUinfo(void)
 
 #define	MAX_IMG_SIZE		(1024*1024)	// bytes
 #define SDRAM_BASE			0x40000000
-#define	FLASH_OFFSET		0x00000000
+#define FLASHIO_OFFSET		0x00000000
+#define FLASH_BASE			0x00000000
 
 int main(int argc, char *argv[])
 {
@@ -222,10 +228,11 @@ int main(int argc, char *argv[])
 
 	volatile UINT32 *pReg = (UINT32*)sys_led_port;
 	volatile UINT32 *pBtn = (UINT32*)sys_gpio0;
+	volatile UINT32 *pDip = (UINT32*)sys_gpio1;
 	volatile UINT8 *pMem;
 	volatile UINT32 *pROM32;
 	volatile UINT32 *pMem32;
-	volatile UINT32 *pFlash = (UINT32*)sys_flash_io;
+	volatile UINT32 *pFlashIO = (UINT32*)sys_flash_io;
 	volatile UINT32 *pUSB = (UINT32*)sys_usb_data;
 
 	volatile UINT8 *ram8 = (UINT8*)SDRAM_BASE;
@@ -237,20 +244,20 @@ int main(int argc, char *argv[])
 
 	// ----------------------------------------------------------
 	ram32 = (UINT32*)(SDRAM_BASE);
-	pFlash = (UINT32*)(sys_flash_io + FLASH_OFFSET);
+	pFlashIO = (UINT32*)(sys_flash_io + FLASHIO_OFFSET);
 
 	if (!*pBtn)
 	{
 		sputs("\n\n");
-		sputs("Booting from flash...");
+		sputs("Booting application from flash...");
 
-		pImg_hdr = (flash_img_hdr_t*)pFlash;
-		ram32 = (UINT32*)pImg_hdr->target_addr;
-		pFlash = (UINT32*)(sys_flash_io + pImg_hdr->img_offset);
+		pImg_hdr = (flash_img_hdr_t*)pFlashIO;
+		ram32 = (UINT32*)pImg_hdr->target_base;
+		pFlashIO = (UINT32*)(sys_flash_io + pImg_hdr->img_base);
 		img_size = pImg_hdr->img_size;
 
 		for (i=0; i < img_size/4; i++)
-			ram32[i] = pFlash[i];
+			ram32[i] = pFlashIO[i];
 
 		sputs("done\n\n");
 
@@ -259,6 +266,17 @@ int main(int argc, char *argv[])
 	else
 	{
 
+		sputs("\n\n");
+
+		if (*pDip)
+		{
+			sputs("Booting stage 2 from flash.\n");
+			Jump_to((void*)FLASH_BASE);
+
+		}
+
+		sputs("Reading HEX-Record from UART..");
+
 		if (flash_find(&flash, sys_flash_io) < 0)
 		{
 			sputs("Cannot find flash device. Exit now!\n\n");
@@ -269,8 +287,6 @@ int main(int argc, char *argv[])
 		for (i=0; i < MAX_IMG_SIZE/4; i++)
 			ram32[i] = 0;
 
-		sputs("\n\n");
-		sputs("Booting from UART..");
 
 		while(1)
 		{
@@ -299,13 +315,14 @@ int main(int argc, char *argv[])
 						img_hdr.magic[1] = 'F';
 						img_hdr.magic[2] = 'I';
 						img_hdr.magic[3] = '1';
-						img_hdr.target_addr = srec.addr;
+						img_hdr.target_base = srec.addr;
 						img_hdr.img_size = haddr - srec.addr;
-						img_hdr.img_offset = FLASH_OFFSET + sizeof(flash_img_hdr_t);
-						img_hdr.hdr_next = FLASH_OFFSET;
+						img_hdr.img_base = FLASHIO_OFFSET + sizeof(flash_img_hdr_t);
+						img_hdr.hdr_next = flash_get_nextblock(&flash, img_hdr.img_base + img_hdr.img_size);
+						sputs("Next free flash offset: ");print_word(img_hdr.hdr_next); sputs("\n");
 
 						sputs("Flash erase...");
-						result = flash_erase(&flash, FLASH_OFFSET, img_hdr.img_size + sizeof(flash_img_hdr_t));
+						result = flash_erase(&flash, FLASHIO_OFFSET, img_hdr.img_size + sizeof(flash_img_hdr_t));
 						if (result < 0)
 						{
 							sputs("failed!\n");
@@ -314,13 +331,13 @@ int main(int argc, char *argv[])
 						sputs("done\n");
 
 						sputs("Flash write...");
-						result = flash_program(&flash, FLASH_OFFSET, (UINT8*)&img_hdr, sizeof(flash_img_hdr_t));
+						result = flash_program(&flash, FLASHIO_OFFSET, (UINT8*)&img_hdr, sizeof(flash_img_hdr_t));
 						if (result < 0)
 						{
 							sputs("failed!\n");
 							break;
 						}
-						result = flash_program(&flash, img_hdr.img_offset, (UINT8*)ram32, img_hdr.img_size);
+						result = flash_program(&flash, img_hdr.img_base, (UINT8*)ram32, img_hdr.img_size);
 						if (result < 0)
 						{
 							sputs("failed!\n");
@@ -329,7 +346,7 @@ int main(int argc, char *argv[])
 						sputs("done\n");
 
 						sputs("Flash verify...");
-						result = flash_verify(&flash, img_hdr.img_offset, (UINT8*)ram32, img_hdr.img_size);
+						result = flash_verify(&flash, img_hdr.img_base, (UINT8*)ram32, img_hdr.img_size);
 						if (result < 0)
 						{
 							sputs("failed!\n");
diff --git a/lib/CPUs/MIPS/bsp/examples/bootloader/init_boot.S b/lib/CPUs/MIPS/bsp/examples/bootloader/init_boot.S
index 3464877..c575209 100644
--- a/lib/CPUs/MIPS/bsp/examples/bootloader/init_boot.S
+++ b/lib/CPUs/MIPS/bsp/examples/bootloader/init_boot.S
@@ -1,6 +1,7 @@
 		.file	"init.S"
 	
 		.text
+		.section .init,"ax"
 		.align	2
 		.globl _init
 		.extern end
@@ -12,197 +13,7 @@ _init:
 		la	$26, sys_uart_baud
 		addiu	$27, $0, baudrate
 		sb	$27, 0($26)
-
-		# set stack pointer 
-		la $sp, stack_ptr
-		j	$jmain
 		
-		# TEST
-		li	$26, +5
-		beqz	$26, _terminate	
-		li	$26, -5
-		beqz	$26, _terminate	
-		li	$26, 0
-		beqz	$26, $_L11
-		nop
-		j	_terminate
-		nop
-$_L11:		
-		li	$26, +5
-		li	$27, +6
-		beq	$26, $27, _terminate	
-		li	$26, -5
-		beq	$26, $27, _terminate	
-		li	$26, 0
-		beq	$26, $27, _terminate	
-		li	$26, +6
-		beq	$26, $27, $_L12
-		nop
-		j	_terminate
-		nop
-$_L12:			
-		li	$26, -5
-		bgez	$26, _terminate	
-		li	$26, +5
-		bgez	$26, $_L13
-		nop
-		j	_terminate
-$_L13:		li	$26, 0
-		bgez	$26, $_L14
-		nop
-		j	_terminate
-$_L14:		
-		li	$26, -5
-		bgtz	$26, _terminate	
-		li	$26, 0
-		bgtz	$26, _terminate	
-		li	$26, +5
-		bgtz	$26, $_L15
-		nop
-		j	_terminate
-		
-$_L15:			
-		li	$26, +5
-		blez	$26, _terminate	
-		li	$26, 0
-		blez	$26, $_L16
-		nop
-		j	_terminate
-$_L16:		li	$26, -5
-		blez	$26, $_L17
-		nop
-		j	_terminate
-			
-$_L17:			
-		li	$26, +5
-		bltz	$26, _terminate	
-		li	$26, 0
-		bltz	$26, _terminate	
-		li	$26, -5
-		bltz	$26, $_L18
-		nop
-		j	_terminate
-$_L18:			
-		li	$26, +5
-		li	$27, +6
-		bne	$26, $27, $_L19
-		nop
-		j	_terminate
-$_L19:		li	$26, -5
-		bne	$26, $27, $_L20
-		nop
-		j	_terminate
-$_L20:		li	$26, -6
-		bne	$26, $27, $_L21
-		nop
-		j	_terminate
-$_L21:		li	$26, 0
-		bne	$26, $27, $_L22
-		nop
-		j	_terminate
-$_L22:		li	$26, +6
-		bne	$26, $27, _terminate
-		li	$26, +5
-		bnez	$26, $_L23
-		nop
-		j	_terminate
-$_L23:		li	$26, -5
-		bnez	$26, $_L24
-		nop
-		j	_terminate
-$_L24:		li	$26, 0
-		bnez	$26, _terminate
+		jr	$ra
 
-$_L25:
-		li	$27, +5
-		sltiu	$26,$27,+6
-		sltiu	$26,$27,-6
-		li	$27, -5
-		sltiu	$26,$27,+6
-		sltiu	$26,$27,-6
-		li	$27, +6
-		sltiu	$26,$27,+5
-		sltiu	$26,$27,-5
-		li	$27, -6
-		sltiu	$26,$27,+5
-		sltiu	$26,$27,-5
-
-		li	$27, +5
-		slti	$26,$27,+6
-		slti	$26,$27,-6
-		li	$27, -5
-		slti	$26,$27,+6
-		slti	$26,$27,-6
-		li	$27, +6
-		slti	$26,$27,+5
-		slti	$26,$27,-5
-		li	$27, -6
-		slti	$26,$27,+5
-		slti	$26,$27,-5
-
-		
-		li	$26, +5
-		li	$27, +6
-		sltu	$26,$26,$27
-		li	$26, +5
-		li	$27, -6
-		sltu	$26,$26,$27
-		li	$26, -5
-		li	$27, +6
-		sltu	$26,$26,$27
-		li	$26, -5
-		li	$27, -6
-		sltu	$26,$26,$27
-
-		li	$26, +6
-		li	$27, +5
-		sltu	$26,$26,$27
-		li	$26, +6
-		li	$27, -5
-		sltu	$26,$26,$27
-		li	$26, -6
-		li	$27, +5
-		sltu	$26,$26,$27
-		li	$26, -6
-		li	$27, -5
-		sltu	$26,$26,$27
-
-		li	$26, +5
-		li	$27, +6
-		slt	$26,$26,$27
-		li	$26, +5
-		li	$27, -6
-		slt	$26,$26,$27
-		li	$26, -5
-		li	$27, +6
-		slt	$26,$26,$27
-		li	$26, -5
-		li	$27, -6
-		slt	$26,$26,$27
-
-		li	$26, +6
-		li	$27, +5
-		slt	$26,$26,$27
-		li	$26, +6
-		li	$27, -5
-		slt	$26,$26,$27
-		li	$26, -6
-		li	$27, +5
-		slt	$26,$26,$27
-		li	$26, -6
-		li	$27, -5
-		slt	$26,$26,$27
-
-		# TEST
-		
-		
-		# jump main
-$jmain:		la $26, main
-		jalr $26
-
-_terminate:
-		nop
-		j		_terminate
-		nop
-		
 		.set	reorder
diff --git a/lib/CPUs/MIPS/bsp/examples/bootloader/startup_boot.S b/lib/CPUs/MIPS/bsp/examples/bootloader/startup_boot.S
index fa88df4..ccbb7e5 100644
--- a/lib/CPUs/MIPS/bsp/examples/bootloader/startup_boot.S
+++ b/lib/CPUs/MIPS/bsp/examples/bootloader/startup_boot.S
@@ -1,12 +1,16 @@
 		.file	"startup.S"
 	
-		.section .stext,"ax"
+		.text
+		.section .init,"ax"
 		.extern _init
 		.align	2
 	
 _start:
 		.set	noreorder
 
+		# set stack pointer 
+		la	$sp, stack_ptr
+
 		# Set Kernel mode
 		li		$26, 0x1040000C
 		mtc0	$26, $12
@@ -19,7 +23,15 @@ _start:
 
 		# jump init
 		la $26, _init
-		jr $26
+		jalr $26
+		nop
+
+		la $26, main
+		jalr $26
+		nop
+_terminate:
+		nop
+		j		_terminate
 		nop
 	
 		.set	reorder
diff --git a/lib/CPUs/MIPS/bsp/examples/cfiflash.c b/lib/CPUs/MIPS/bsp/examples/cfiflash.c
index e022561..483ef85 100644
--- a/lib/CPUs/MIPS/bsp/examples/cfiflash.c
+++ b/lib/CPUs/MIPS/bsp/examples/cfiflash.c
@@ -20,6 +20,17 @@ UINT32 cfi_get_status(flash_t *pObj)
 
 }
 
+UINT32 flash_get_nextblock(flash_t *pObj, UINT32 offset)
+{
+	UINT32 blocknum;
+
+	blocknum = offset/pObj->info.blocksize;
+	if (offset%pObj->info.blocksize)
+		blocknum++;
+
+	return blocknum*pObj->info.blocksize;
+}
+
 void cfi_init(flash_t *pObj, UINT32 base_addr)
 {
 	int i;
diff --git a/lib/CPUs/MIPS/bsp/examples/cfiflash.h b/lib/CPUs/MIPS/bsp/examples/cfiflash.h
index 1c5a079..95e5ea2 100644
--- a/lib/CPUs/MIPS/bsp/examples/cfiflash.h
+++ b/lib/CPUs/MIPS/bsp/examples/cfiflash.h
@@ -36,6 +36,7 @@ UINT32 cfi_block_erase(flash_t *pObj, UINT32 word_index);
 UINT32 cfi_program_single(flash_t *pObj, UINT32 word_index, UINT32 word);
 UINT32 cfi_program_multi(flash_t *pObj, UINT32 word_index, UINT32 *pWords, UINT32 num_words);
 
+UINT32 flash_get_nextblock(flash_t *pObj, UINT32 offset);
 UINT32 flash_find(flash_t *pObj, UINT32 base_addr);
 UINT32 flash_erase(flash_t *pObj, UINT32 offset, UINT32 size);
 UINT32 flash_program(flash_t *pObj, UINT32 offset, UINT8 *pData, UINT32 size);
diff --git a/lib/CPUs/MIPS/bsp/examples/libsys.c b/lib/CPUs/MIPS/bsp/examples/libsys.c
index 209e79a..eda27e8 100644
--- a/lib/CPUs/MIPS/bsp/examples/libsys.c
+++ b/lib/CPUs/MIPS/bsp/examples/libsys.c
@@ -397,18 +397,26 @@ void print_word(int word)
 // ---------------------------------------------------------------------------
 void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
 {
-    int i, j, cnt_hex, cnt_asc;
+	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(i);
+        print_word(base + i);
         sputs(": ");
-        for (j=0; j < nbpr; j++)
+        for (j=0; j < num_bytes_per_row; j++)
         {
             if (cnt_hex)
             {
@@ -425,7 +433,7 @@ void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
         }
 
         sputs("   ");
-        for (j=0; j < nbpr; j++)
+        for (j=0; j < num_bytes_per_row; j++)
         {
             if (cnt_asc)
             {
@@ -443,7 +451,7 @@ void PrintBuffer8(UINT8 *pBuf, int nbpr, int len)
             }
         }
         sputs("\n");
-        i += nbpr;
+        i += num_bytes_per_row;
 
     } while (cnt_hex);
 }
diff --git a/lib/CPUs/MIPS/bsp/examples/libsys.h b/lib/CPUs/MIPS/bsp/examples/libsys.h
index c155cc5..dced397 100644
--- a/lib/CPUs/MIPS/bsp/examples/libsys.h
+++ b/lib/CPUs/MIPS/bsp/examples/libsys.h
@@ -76,6 +76,7 @@ void _exit (int exitcode);
 void sleep(unsigned ms);
 
 void PrintBuffer8(UINT8 *pBuf, int nbpr, int len);
+void memdump(UINT8 *pBuf, int print_offset_only, int num_bytes_per_row, int len);
 
 #endif // LIBSYS_H
 
diff --git a/lib/CPUs/MIPS/bsp/examples/link.ld b/lib/CPUs/MIPS/bsp/examples/link.ld
index 5d45f15..6173aa5 100644
--- a/lib/CPUs/MIPS/bsp/examples/link.ld
+++ b/lib/CPUs/MIPS/bsp/examples/link.ld
@@ -3,7 +3,9 @@ OUTPUT_FORMAT("elf32-littlemips", "elf32-bigmips",
 	      "elf32-littlemips")
 OUTPUT_ARCH(mips)
 ENTRY(_start)
+SEARCH_DIR(".");
 SEARCH_DIR("/usr/local/mipsel-elf/lib");
+SEARCH_DIR("/usr/local/lib/gcc/mipsel-elf/4.3.3");
 
 stack_ptr = 0x7FFFEFF0;
 baudrate = 0x0D;
@@ -15,78 +17,24 @@ sys_timer_usec = 0xA000008;
 sys_timer_sec = 0xA000000C;
 
 STARTUP(startup.o)
-INPUT(kernel.o)
-INPUT(libsys.a)
 
 SECTIONS
 {
   /* Read-only sections, merged into text segment: */
   PROVIDE (__executable_start = 0x40000000); . = 0x40000000;
-   .etext __executable_start : 
+   .start __executable_start : 
    {
       start = ALIGN(2);
       entry = ALIGN(2);
       _entry = ALIGN(2);
       __entry = ALIGN(2);
-     *(.etext)
+     *(.start)
    }
-   
+   /* Kernel text with low-level exception handler */
    .ktext __executable_start + 0x180 :
    {
      *(.ktext)
    }
-
-  .interp         : { *(.interp) }
-  .reginfo        : { *(.reginfo) }
-  .note.gnu.build-id : { *(.note.gnu.build-id) }
-  .dynamic        : { *(.dynamic) }
-  .hash           : { *(.hash) }
-  .gnu.hash       : { *(.gnu.hash) }
-  .dynsym         : { *(.dynsym) }
-  .dynstr         : { *(.dynstr) }
-  .gnu.version    : { *(.gnu.version) }
-  .gnu.version_d  : { *(.gnu.version_d) }
-  .gnu.version_r  : { *(.gnu.version_r) }
-  .rel.dyn        :
-    {
-      *(.rel.init)
-      *(.rel.text .rel.text.* .rel.gnu.linkonce.t.*)
-      *(.rel.fini)
-      *(.rel.rodata .rel.rodata.* .rel.gnu.linkonce.r.*)
-      *(.rel.data.rel.ro* .rel.gnu.linkonce.d.rel.ro.*)
-      *(.rel.data .rel.data.* .rel.gnu.linkonce.d.*)
-      *(.rel.tdata .rel.tdata.* .rel.gnu.linkonce.td.*)
-      *(.rel.tbss .rel.tbss.* .rel.gnu.linkonce.tb.*)
-      *(.rel.ctors)
-      *(.rel.dtors)
-      *(.rel.got)
-      *(.rel.dyn)
-      *(.rel.sdata .rel.sdata.* .rel.gnu.linkonce.s.*)
-      *(.rel.sbss .rel.sbss.* .rel.gnu.linkonce.sb.*)
-      *(.rel.sdata2 .rel.sdata2.* .rel.gnu.linkonce.s2.*)
-      *(.rel.sbss2 .rel.sbss2.* .rel.gnu.linkonce.sb2.*)
-      *(.rel.bss .rel.bss.* .rel.gnu.linkonce.b.*)
-    }
-  .rela.dyn       :
-    {
-      *(.rela.init)
-      *(.rela.text .rela.text.* .rela.gnu.linkonce.t.*)
-      *(.rela.fini)
-      *(.rela.rodata .rela.rodata.* .rela.gnu.linkonce.r.*)
-      *(.rela.data .rela.data.* .rela.gnu.linkonce.d.*)
-      *(.rela.tdata .rela.tdata.* .rela.gnu.linkonce.td.*)
-      *(.rela.tbss .rela.tbss.* .rela.gnu.linkonce.tb.*)
-      *(.rela.ctors)
-      *(.rela.dtors)
-      *(.rela.got)
-      *(.rela.sdata .rela.sdata.* .rela.gnu.linkonce.s.*)
-      *(.rela.sbss .rela.sbss.* .rela.gnu.linkonce.sb.*)
-      *(.rela.sdata2 .rela.sdata2.* .rela.gnu.linkonce.s2.*)
-      *(.rela.sbss2 .rela.sbss2.* .rela.gnu.linkonce.sb2.*)
-      *(.rela.bss .rela.bss.* .rela.gnu.linkonce.b.*)
-    }
-  .rel.plt        : { *(.rel.plt) }
-  .rela.plt       : { *(.rela.plt) }
   .init           :
   {
     KEEP (*(.init))
@@ -99,7 +47,7 @@ SECTIONS
     /* .gnu.warning sections are handled specially by elf32.em.  */
     *(.gnu.warning)
     *(.mips16.fn.*) *(.mips16.call.*)
-  }
+  } =0
   .fini           :
   {
     KEEP (*(.fini))
diff --git a/lib/CPUs/MIPS/bsp/examples/startup.S b/lib/CPUs/MIPS/bsp/examples/startup.S
index c7283a5..1c50f25 100644
--- a/lib/CPUs/MIPS/bsp/examples/startup.S
+++ b/lib/CPUs/MIPS/bsp/examples/startup.S
@@ -1,15 +1,50 @@
 		.file	"startup.S"
 	
-		.section .etext,"ax"
+		.section .rodata
+		.data
+argv0:		.asciz		"\"This-MIPS-program\""
+argv:		.word		argv0	
+
+		.text
+		.section .start, "ax"
 		.extern _init
 		.align	2
 		.globl _start
 _start:
 		.set	noreorder
 
-		# jump init
-		la $8, _init
-		jr $8
+		# 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
+
+		# Call _init
+		la $k0, _init
+		jalr $k0
 		nop
 		
+		# 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
+		
 		.set	reorder
diff --git a/lib/CPUs/MIPS/bsp/examples/test_exception.c b/lib/CPUs/MIPS/bsp/examples/test_exception.c
index 32d491f..400c676 100644
--- a/lib/CPUs/MIPS/bsp/examples/test_exception.c
+++ b/lib/CPUs/MIPS/bsp/examples/test_exception.c
@@ -103,7 +103,7 @@ void _exc_ibe(void)
 	__asm
 	(
 		".set	noreorder\n"
-		" li	$t1, 0xAc563440\n"
+		" li	$t1, 0x10000000\n"
 		" jr    $t1\n"
 		"nop\n"
 		".set	reorder\n"
@@ -117,7 +117,7 @@ void _exc_dbe(void)
 	__asm
 	(
 		".set	noreorder\n"
-		" li	$t1, 0xAc563440\n"
+		" li	$t1, 0x10000000\n"
 		" lw	$t0, 0($t1)\n"
 		"nop\n"
 		" jr    $t0\n"
@@ -143,8 +143,6 @@ void handler1(void)
 int main(void)
 {
 	int buf[256];
-	volatile int *pLED = (int*)sys_led_port;
-	volatile int *pFlash = (int*)sys_flash_io;
 	int i, sel;
 
 	interrupt_register(0, handler0);
@@ -152,9 +150,6 @@ int main(void)
 	interrupt_register(1, handler1);
 	interrupt_enable(1);
 
-	for (i=0; i < 8192; i++)
-		*pLED = *pFlash;
-
 	while(1)
 	{
 		printf("Welche exception möchten Sie testen?\n");
diff --git a/lib/CPUs/MIPS/bsp/examples/testbench.c b/lib/CPUs/MIPS/bsp/examples/testbench.c
index 3a64e37..3bee759 100644
--- a/lib/CPUs/MIPS/bsp/examples/testbench.c
+++ b/lib/CPUs/MIPS/bsp/examples/testbench.c
@@ -304,11 +304,11 @@ int pi_calc()
     return 0;
 }
 
-#define	TEST_SIZE			(128*1024)	// Bytes
+#define	TEST_SIZE			(16*1024*1024)	// Bytes
 #define	SMALL_TEST_SIZE		(8192)			// Bytes
 int main (void)
 {
-	int result, i, j, cnt;
+	int result, i, j, cnt, size;
 	volatile UINT32 *pUART_baud = (UINT32*)sys_uart_baud;
 	volatile UINT32 *pReg = (UINT32*)sys_led_port;
 	volatile UINT32 *pReg_usec = (UINT32*)sys_timer_usec;
@@ -365,6 +365,9 @@ int main (void)
 		printf("passed (%.2f MByte/s)\n", (double)TEST_SIZE/(1000*(end-start)));
 */
 	// ----------------------------------------------------------
+	printf("Dump Flash\n");
+	PrintBuffer8((UINT8*)0, 16, 256);
+
 	// Memtest BEGIN
 	printf("SDRAM Memory Test\n");
 	printf("Test size %d kByte\n\n", TEST_SIZE/1024);
@@ -523,6 +526,37 @@ int main (void)
 	free(pSrc32);
 	free(pDst32);
 
+	printf("Boot code read \n");
+	size = 0x2000;
+	pSrc32 = (UINT32*)0xBFC00000;
+	pDst32 = (UINT32*)calloc(size,sizeof(UINT32));
+	printf("Copying %d kBytes...", size/1024);
+	start = clock();
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	memcpy(pDst32, pSrc32, size);
+	end = clock();
+	printf("done (%.2f MByte/s)\n", (double)10*size/(1000*(end-start)));
+	memdump((UINT8*)pSrc32, 0, 16, size);
+	printf("Verify Boot code...");
+	for (i=size/4-1; i >= 0; i--)
+		if (pDst32[i] != pSrc32[i])
+			break;
+	i++;
+	if (i)
+		printf("failed\r\n");
+	else
+		printf("passed\r\n");
+
+	free(pDst32);
+
 	// Memtest END
 	// ----------------------------------------------------------
 	sputs("\r\n");
diff --git a/lib/CPUs/MIPS/bsp/examples/xcpt.c b/lib/CPUs/MIPS/bsp/examples/xcpt.c
index 804c364..5b71693 100644
--- a/lib/CPUs/MIPS/bsp/examples/xcpt.c
+++ b/lib/CPUs/MIPS/bsp/examples/xcpt.c
@@ -113,8 +113,6 @@ int _xcpt_deliver(struct xcptcontext * _xcp)
 		sputs("Unhandled exception <");
 		sputs(_xcpt_code_str[exc_code]);
 		sputs("> at PC : ");
-		print_word(_xcp->epc);
-		sputs("\n");
 
 		if (_xcp->cr & 0x80000000)
 		{
@@ -124,6 +122,8 @@ int _xcpt_deliver(struct xcptcontext * _xcp)
 		{
 			pInstr = (int*)(_xcp->epc);
 		}
+		print_word((int)pInstr);
+		sputs("\n");
 
 		sputs("Instruction at exception address : ");
 		print_word(*pInstr);