Add board specific code, autodetect the kernel, fixes #1707, thanks Gabor Juhos

git-svn-id: svn://svn.openwrt.org/openwrt/trunk@7398 3c298f89-4303-0410-b956-a3cf2f4a3e73

8 files changed, 445 insertions, 149 deletions

diff --git a/target/linux/adm5120-2.6/image/lzma-loader/Makefile b/target/linux/adm5120-2.6/image/lzma-loader/Makefile
index 175a1845c..34a1a469e 100644
--- a/target/linux/adm5120-2.6/image/lzma-loader/Makefile
+++ b/target/linux/adm5120-2.6/image/lzma-loader/Makefile
@@ -8,31 +8,49 @@
 
 include $(TOPDIR)/rules.mk
 
-LOADER := loader
-BZ_STARTUP_ORG := 0
+LOADER		:= adm5120
+LOADER_NAME	:= loader-$(LOADER)
+LOADER_DATA 	:=
+
+LOADER_BIN	:= $(KDIR)/$(LOADER_NAME).bin
+LOADER_GZ	:= $(KDIR)/$(LOADER_NAME).gz
+LOADER_ELF	:= $(KDIR)/$(LOADER_NAME).elf
+
+LZMA_STARTUP_ORG:= 0
+LZMA_TEXT_START	:= 0x80300000
 
 PKG_NAME := lzma-loader
-PKG_BUILD_DIR := $(BUILD_DIR)/$(PKG_NAME)
+PKG_BUILD_DIR := $(KDIR)/$(PKG_NAME)
+
+.PHONY : loader-compile
 
 $(PKG_BUILD_DIR)/.prepared:
 	mkdir $(PKG_BUILD_DIR)
 	$(CP) ./src/* $(PKG_BUILD_DIR)/
 	touch $@
 
-$(PKG_BUILD_DIR)/$(LOADER).gz: $(PKG_BUILD_DIR)/.prepared
-	$(MAKE) -C $(PKG_BUILD_DIR) CC="$(TARGET_CC)" \
-		LD="$(TARGET_CROSS)ld" CROSS_COMPILE="$(TARGET_CROSS)" \
-		LOADER=$(LOADER) BZ_STARTUP_ORG=$(BZ_STARTUP_ORG)
+loader-compile: $(PKG_BUILD_DIR)/.prepared
+	$(MAKE) -C $(PKG_BUILD_DIR) CROSS_COMPILE="$(TARGET_CROSS)" \
+		LZMA_STARTUP_ORG=$(LZMA_STARTUP_ORG) \
+		LZMA_TEXT_START=$(LZMA_TEXT_START) \
+		LOADER_DATA=$(LOADER_DATA) \
+		clean all
+
+$(LOADER_GZ): $(PKG_BUILD_DIR)/loader.bin
+	gzip -nc9 $< > $@
+
+$(LOADER_ELF) : $(PKG_BUILD_DIR)/loader.elf
+	$(CP) $< $@
+
+$(LOADER_BIN) : $(PKG_BUILD_DIR)/loader.bin
+	$(CP) $< $@
 
 download:
 prepare: $(PKG_BUILD_DIR)/.prepared
-compile: $(PKG_BUILD_DIR)/$(LOADER).gz
-install:
+compile: loader-compile $(LOADER_BIN) $(LOADER_GZ) $(LOADER_ELF)
 
-ifneq ($(TARGET),)
-install: compile
-	$(CP) $(PKG_BUILD_DIR)/$(LOADER).gz $(PKG_BUILD_DIR)/$(LOADER).elf $(PKG_BUILD_DIR)/$(LOADER).bin $(TARGET)/
-endif
+install:
 
 clean:
 	rm -rf $(PKG_BUILD_DIR)
+	rm -f $(KDIR)/loader-*.gz $(KDIR)/loader-*.elf $(KDIR)/loader-*.bin
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/Makefile b/target/linux/adm5120-2.6/image/lzma-loader/src/Makefile
index e94be307d..0f4ffa63a 100644
--- a/target/linux/adm5120-2.6/image/lzma-loader/src/Makefile
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/Makefile
@@ -22,51 +22,65 @@
 #
 
 LOADADDR	:= 0x80001000
-BZ_TEXT_START	:= 0x80300000
-BZ_STARTUP_ORG	:= 0
-LOADER		:= loader
+LZMA_TEXT_START	:= 0x80500000
+LZMA_STARTUP_ORG:= 0
+LOADER_DATA	:=
 
-OBJCOPY		:= $(CROSS_COMPILE)objcopy -O binary -R .reginfo -R .note -R .comment -R .mdebug -S
+CC		:= $(CROSS_COMPILE)gcc
+LD		:= $(CROSS_COMPILE)ld
+OBJCOPY		:= $(CROSS_COMPILE)objcopy
+OBJDUMP		:= $(CROSS_COMPILE)objdump
+
+BIN_FLAGS	:= -O binary -R .reginfo -R .note -R .comment -R .mdebug -S
 
 CFLAGS		= -D__KERNEL__ -Wall -Wstrict-prototypes -Wno-trigraphs -Os \
 		  -fno-strict-aliasing -fno-common -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic \
 		  -ffunction-sections -pipe -mlong-calls -fno-common \
 		  -mabi=32 -march=mips32 -Wa,-32 -Wa,-march=mips32 -Wa,-mips32 -Wa,--trap
-CFLAGS		+= -DLOADADDR=$(LOADADDR) -D_LZMA_IN_CB
+CFLAGS		+= -DLOADADDR=$(LOADADDR)
 
-ASFLAGS		= $(CFLAGS) -D__ASSEMBLY__ -DBZ_STARTUP_ORG=$(BZ_STARTUP_ORG)
+ASFLAGS		= $(CFLAGS) -D__ASSEMBLY__ -DLZMA_STARTUP_ORG=$(LZMA_STARTUP_ORG)
 
 LDFLAGS		= -static --gc-sections -no-warn-mismatch
-LDFLAGS		+= -e startup -Ttext $(BZ_TEXT_START) -T loader.lds.in
+LDFLAGS		+= -e startup -T loader.lds -Ttext $(LZMA_TEXT_START)
+
+O_FORMAT 	= $(shell $(OBJDUMP) -i | head -2 | grep elf32)
 
-OBJECTS		:= $(LOADER)-head.o decompress.o LzmaDecode.o
+OBJECTS		:= head.o decompress.o board.o LzmaDecode.o
 
-all: $(LOADER).gz $(LOADER).elf
+ifneq ($(strip $(LOADER_DATA)),)
+OBJECTS		+= data.o
+CFLAGS		+= -DLZMA_WRAPPER=1
+else
+CFLAGS		+= -D_LZMA_IN_CB
+endif
+
+all: loader.bin
 
 # Don't build dependencies, this may die if $(CC) isn't gcc
 dep:
 
 install:
 
-decompress.o:
-	$(CC) $(CFLAGS) -c decompress.c -o $@
-
-$(LOADER)-head.o:
-	$(CC) $(ASFLAGS) -c head.S -o $@
+%.o : %.c
+	$(CC) $(CFLAGS) -c -o $@ $<
 
-$(LOADER).gz: $(LOADER).bin
-	gzip -nc9 $< > $@
+%.o : %.S
+	$(CC) $(ASFLAGS) -c -o $@ $<
 
-$(LOADER).elf: $(LOADER).o
-	cp $< $@
+data.o: $(LOADER_DATA)
+	$(LD) -r -b binary --oformat $(O_FORMAT) -T lzma-data.lds -o $@ $<
 
-$(LOADER).bin: $(LOADER).o
-	$(OBJCOPY) -O binary $< $@
+loader.bin: loader.elf
+	$(OBJCOPY) $(BIN_FLAGS) $< $@
 
-$(LOADER).o: $(OBJECTS)
+loader.elf: $(OBJECTS)
 	$(LD) $(LDFLAGS) -o $@ $(OBJECTS)
 
 mrproper: clean
 
 clean:
 	rm -f *.gz *.elf *.bin *.o
+
+
+
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/board.c b/target/linux/adm5120-2.6/image/lzma-loader/src/board.c
new file mode 100644
index 000000000..616e7562a
--- /dev/null
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/board.c
@@ -0,0 +1,184 @@
+/*
+ * ADM5120 specific board support for LZMA decompressor 
+ *
+ * Copyright (C) 2007 OpenWrt.org
+ * Copyright (C) 2007 Gabor Juhos <juhosg@freemail.hu>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <stddef.h>
+
+#define READREG(r)	*(volatile unsigned int *)(r)
+#define WRITEREG(r,v)	*(volatile unsigned int *)(r) = v
+
+/* 
+ * INTC definitions
+ */
+#define INTC_BASE	0xB2200000
+
+/* INTC registers */
+#define INTC_REG_IRQ_DISABLE	0x0C
+
+/* 
+ * UART definitions
+ */
+#define UART_BASE	0xB2600000
+/* UART registers */
+#define UART_REG_DATA	0x00	/* Data register */
+#define UART_REG_ECR	0x04	/* Error Clear register */
+#define UART_REG_LCRH	0x08	/* Line Control High register */
+#define UART_REG_LCRM	0x0C	/* Line Control Middle register */
+#define UART_REG_LCRL	0x10	/* Line Control Low register */
+#define UART_REG_CTRL	0x14	/* Control register */
+#define UART_REG_FLAG   0x18	/* Flag register */
+
+/* Control register bits */
+#define UART_CTRL_EN	( 1 << 0 )	/* UART enable */
+
+/* Line Control High register bits */
+#define UART_LCRH_FEN	( 1 << 4 )	/* FIFO enable */
+
+/* Flag register bits */
+#define UART_FLAG_CTS	( 1 << 0 )
+#define UART_FLAG_DSR	( 1 << 1 )
+#define UART_FLAG_DCD	( 1 << 2 )
+#define UART_FLAG_BUSY	( 1 << 3 )
+#define UART_FLAG_RXFE	( 1 << 4 )	/* RX FIFO empty */
+#define UART_FLAG_TXFF	( 1 << 5 )	/* TX FIFO full */
+#define UART_FLAG_RXFF	( 1 << 6 )	/* RX FIFO full */
+#define UART_FLAG_TXFE	( 1 << 7 )	/* TX FIFO empty */
+
+/* 
+ * SWITCH definitions
+ */
+#define SWITCH_BASE	0xB2000000
+
+#define SWITCH_REG_CPUP_CONF	0x0024
+#define SWITCH_REG_PORT_CONF0	0x0028
+
+#define SWITCH_REG_GPIO_CONF0	0x00B8
+#define SWITCH_REG_GPIO_CONF2	0x00BC
+
+#define SWITCH_REG_PORT0_LED	0x0100
+#define SWITCH_REG_PORT1_LED	0x0104
+#define SWITCH_REG_PORT2_LED	0x0108
+#define SWITCH_REG_PORT3_LED	0x010C
+#define SWITCH_REG_PORT4_LED	0x0110
+
+#define SWITCH_PORTS_HW		0x3F		/* Hardware Ports */
+
+/* CPUP_CONF register bits */
+#define CPUP_CONF_DCPUP		( 1 << 0 )	/* Disable CPU port */
+
+/* PORT_CONF0 register bits */
+#define PORT_CONF0_DP_SHIFT	0	/* disable port shift*/
+
+
+/*
+ * UART routines
+ */
+
+#define UART_READ(r)	READREG(UART_BASE+(r))
+#define UART_WRITE(r,v)	WRITEREG(UART_BASE+(r),(v))
+
+static void uart_init(void)
+{
+	unsigned int t;
+	
+	/* disable uart */
+	UART_WRITE(UART_REG_CTRL, 0);
+
+	/* keep current baud rate */
+	t = UART_READ(UART_REG_LCRM);
+	UART_WRITE(UART_REG_LCRM, t);
+	t = UART_READ(UART_REG_LCRL);
+	UART_WRITE(UART_REG_LCRL, t);
+	
+	/* keep data, stop, and parity bits, but disable FIFO */
+	t = UART_READ(UART_REG_LCRH);
+	t &= ~(UART_LCRH_FEN);
+	UART_WRITE(UART_REG_LCRH, t );
+
+	/* clear error bits */
+	UART_WRITE(UART_REG_ECR, 0xFF);
+
+	/* enable uart, and disable interrupts */
+	UART_WRITE(UART_REG_CTRL, UART_CTRL_EN);
+}
+
+static void uart_putc(int ch)
+{
+	while ((UART_READ(UART_REG_FLAG) & UART_FLAG_TXFE) == 0);
+
+	UART_WRITE(UART_REG_DATA, ch);
+
+	while ((UART_READ(UART_REG_FLAG) & UART_FLAG_TXFF) != 0);
+}
+
+/*
+ * INTC routines
+ */
+
+#define INTC_READ(r)	READREG(INTC_BASE+(r))
+#define INTC_WRITE(r,v)	WRITEREG(INTC_BASE+(r),v)
+
+static void intc_init(void)
+{
+	INTC_WRITE(INTC_REG_IRQ_DISABLE, 0xFFFFFFFF);
+}
+
+/*
+ * SWITCH routines
+ */
+
+#define SWITCH_READ(r)		READREG(SWITCH_BASE+(r))
+#define SWITCH_WRITE(r,v)	WRITEREG(SWITCH_BASE+(r),v)
+
+static void switch_init(void)
+{
+	/* disable PHYS ports */
+	SWITCH_WRITE(SWITCH_REG_PORT_CONF0, 
+	    (SWITCH_PORTS_HW << PORT_CONF0_DP_SHIFT));
+
+	/* disable CPU port */
+	SWITCH_WRITE(SWITCH_REG_CPUP_CONF, CPUP_CONF_DCPUP);
+
+	/* disable GPIO lines */
+	SWITCH_WRITE(SWITCH_REG_GPIO_CONF0, 0);
+	SWITCH_WRITE(SWITCH_REG_GPIO_CONF2, 0);
+	
+	/* disable LED lines */
+	SWITCH_WRITE(SWITCH_REG_PORT0_LED, 0);
+	SWITCH_WRITE(SWITCH_REG_PORT1_LED, 0);
+	SWITCH_WRITE(SWITCH_REG_PORT2_LED, 0);
+	SWITCH_WRITE(SWITCH_REG_PORT3_LED, 0);
+	SWITCH_WRITE(SWITCH_REG_PORT4_LED, 0);
+}
+
+/*
+ *  routines needed by decompress.c
+ */
+void board_putc(int ch)
+{
+	uart_putc(ch);
+}
+
+void board_init(void)
+{
+	intc_init();
+	switch_init();
+	uart_init();
+}
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/decompress.c b/target/linux/adm5120-2.6/image/lzma-loader/src/decompress.c
index ec63ccb65..f867e20dc 100644
--- a/target/linux/adm5120-2.6/image/lzma-loader/src/decompress.c
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/decompress.c
@@ -1,7 +1,8 @@
 /*
- * LZMA compressed kernel decompressor for bcm947xx boards
+ * LZMA compressed kernel decompressor for ADM5120 boards
  *
  * Copyright (C) 2005 by Oleg I. Vdovikin <oleg@cs.msu.su>
+ * Copyright (C) 2007 OpenWrt.org
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -33,11 +34,18 @@
  * 24-Mar-2007 Gabor Juhos
  *   pass original values of the a0,a1,a2,a3 registers to the kernel
  *
+ * 19-May-2007 Gabor Juhos
+ *   endiannes related cleanups
+ *   add support for decompressing an embedded kernel 
+ *
  */
 
+#include <stddef.h>
+
 #include "LzmaDecode.h"
 
-#define BCM4710_FLASH		0x1fc00000	/* Flash */
+#define ADM5120_FLASH_START	0x1fc00000	/* Flash start */
+#define ADM5120_FLASH_END	0x1fe00000	/* Flash end */
 
 #define KSEG0			0x80000000
 #define KSEG1			0xa0000000
@@ -54,7 +62,7 @@
 		"cache %1, (%0);\n"		\
 		".set mips0;\n"			\
 		".set reorder\n"		\
-		:						\
+		:				\
 		: "r" (base),			\
 		  "i" (op));
 
@@ -81,6 +89,7 @@ static __inline__ void blast_dcache(unsigned long size, unsigned long lsize)
 }
 
 #define TRX_MAGIC       0x30524448      /* "HDR0" */
+#define TRX_ALIGN	0x1000
 
 struct trx_header {
 	unsigned int magic;		/* "HDR0" */
@@ -92,26 +101,24 @@ struct trx_header {
 
 /* beyound the image end, size not known in advance */
 extern unsigned char workspace[];
+#if LZMA_WRAPPER
+extern unsigned char _lzma_data_start[];
+extern unsigned char _lzma_data_end[];
+#endif
+
+extern void board_init(void);
+extern void board_putc(int ch);
 
-unsigned int offset;
 unsigned char *data;
+unsigned long datalen;
 
 typedef void (*kernel_entry)(unsigned long reg_a0, unsigned long reg_a1,
 	unsigned long reg_a2, unsigned long reg_a3);
 
-/* flash access should be aligned, so wrapper is used */
-/* read byte from the flash, all accesses are 32-bit aligned */
 static int read_byte(void *object, unsigned char **buffer, UInt32 *bufferSize)
 {
-	static unsigned int val;
-
-	if (((unsigned int)offset % 4) == 0) {
-		val = *(unsigned int *)data;
-		data += 4;
-	}
-
 	*bufferSize = 1;
-	*buffer = ((unsigned char *)&val) + (offset++ & 3);
+	*buffer = data++;
 
 	return LZMA_RESULT_OK;
 }
@@ -121,10 +128,90 @@ static __inline__ unsigned char get_byte(void)
 	unsigned char *buffer;
 	UInt32 fake;
 
-	return read_byte(0, &buffer, &fake), *buffer;
+	read_byte(0, &buffer, &fake);
+	return *buffer;
+}
+
+static __inline__ unsigned int read_le32(void *buf)
+{
+	unsigned char *p;
+
+	p = buf;
+	return ((unsigned int)p[0] + ((unsigned int)p[1] << 8) +
+		((unsigned int)p[2] << 16) +((unsigned int)p[3] << 24));
+}
+
+static void print_char(char ch)
+{
+	if (ch == '\n')
+		board_putc('\r');
+    	board_putc(ch);
+}
+
+static void print_str(char * str)
+{
+	while ( *str != 0 )
+    		print_char(*str++);
+}
+
+static void print_hex(int val)
+{
+	int i;
+	int tmp;
+
+	print_str("0x");
+	for ( i=0 ; i<8 ; i++ ) {
+    		tmp = (val >> ((7-i) * 4 )) & 0xf;
+    		tmp = tmp < 10 ? (tmp + '0') : (tmp + 'A' - 10);
+    		board_putc(tmp);
+	}
+}
+
+static unsigned char *find_kernel(void)
+{
+	struct trx_header *hdr;
+	unsigned char *ret;
+
+    	print_str("Looking for TRX header... ");
+	/* look for trx header, 32-bit data access */
+	hdr = NULL;
+	for (ret = ((unsigned char *) KSEG1ADDR(ADM5120_FLASH_START));
+		ret < ((unsigned char *)KSEG1ADDR(ADM5120_FLASH_END));
+		ret += TRX_ALIGN) {
+		
+		if (read_le32(ret) == TRX_MAGIC) {
+			hdr = (struct trx_header *)ret;
+			break;
+		}
+	}
+
+	if (hdr == NULL) {
+		print_str("not found!\n");
+		return NULL;	
+	}
+
+	print_str("found at ");
+	print_hex((unsigned int)ret);
+	print_str(", kernel in partition ");
+	
+	/* compressed kernel is in the partition 0 or 1 */
+	if ((read_le32(&hdr->offsets[1]) == 0) ||
+		(read_le32(&hdr->offsets[1]) > 65536)) {
+		ret += read_le32(&hdr->offsets[0]);
+		print_str("0\n");
+	} else {
+		ret += read_le32(&hdr->offsets[1]);
+		print_str("1\n");
+	}
+		
+	return ret;
 }
 
-int uart_write_str(char * str);
+static void halt(void)
+{
+	print_str("\nSystem halted!\n");
+	for(;;);
+}
 
 /* should be the first function */
 void decompress_entry(unsigned long reg_a0, unsigned long reg_a1,
@@ -137,23 +224,27 @@ void decompress_entry(unsigned long reg_a0, unsigned long reg_a1,
 	unsigned int lp; /* literal pos state bits */
 	unsigned int pb; /* pos state bits */
 	unsigned int osize; /* uncompressed size */
-
+	int res;
+#if !(LZMA_WRAPPER)
 	ILzmaInCallback callback;
-	callback.Read = read_byte;
+#endif
 
-    uart_write_str("decompress kernel ... ");
+	board_init();
 
-	/* look for trx header, 32-bit data access */
-	for (data = ((unsigned char *) KSEG1ADDR(BCM4710_FLASH));
-		((struct trx_header *)data)->magic != TRX_MAGIC; data += 65536);
+	print_str("\n\nLZMA loader for ADM5120, Copyright (C) 2007 OpenWrt.org\n\n");
 
-	/* compressed kernel is in the partition 0 or 1 */
-	if (((struct trx_header *)data)->offsets[1] > 65536)
-		data += ((struct trx_header *)data)->offsets[0];
-	else
-		data += ((struct trx_header *)data)->offsets[1];
+#if LZMA_WRAPPER
+	data = _lzma_data_start;
+	datalen = _lzma_data_end - _lzma_data_start;
+#else
+	data = find_kernel();
+	if (data == NULL) {
+		/* no compressed kernel found, halting */
+		halt();
+	}
 
-	offset = 0;
+	datalen = ((unsigned char *) KSEG1ADDR(ADM5120_FLASH_END))-data;
+#endif
 
 	/* lzma args */
 	i = get_byte();
@@ -174,68 +265,33 @@ void decompress_entry(unsigned long reg_a0, unsigned long reg_a1,
 	for (i = 0; i < 4; i++)
 		get_byte();
 
+	print_str("decompressing kernel... ");
+
 	/* decompress kernel */
-	if (LzmaDecode(workspace, ~0, lc, lp, pb, &callback,
-		(unsigned char*)LOADADDR, osize, &i) == LZMA_RESULT_OK)
-	{
-		blast_dcache(dcache_size, dcache_lsize);
-		blast_icache(icache_size, icache_lsize);
-
-		/* Jump to load address */
-        uart_write_str("ok\r\n");
-		((kernel_entry) LOADADDR)(reg_a0, reg_a1, reg_a2, reg_a3);
+#if LZMA_WRAPPER
+	res = LzmaDecode(workspace, ~0, lc, lp, pb, data, datalen,
+		(unsigned char*)LOADADDR, osize, &i);
+#else
+	callback.Read = read_byte;
+	res = LzmaDecode(workspace, ~0, lc, lp, pb, &callback,
+		(unsigned char*)LOADADDR, osize, &i);
+#endif
+	if (res != LZMA_RESULT_OK) {
+		print_str("failed, LzmaDecode error: ");
+		print_hex(res);
+		print_str("\n");
+		halt();
 	}
-    uart_write_str("failed\r\n");
-    while (1 );
-}
 
-/*  *********************************************************************
-    *
-    *  ADM5120 UART driver			File: dev_adm_uart.c
-    *
-    *  This is a console device driver for an ADM5120 UART
-    *
-    *********************************************************************
-    *
-    *  Copyright 2006
-    *  Compex Systems. All rights reserved.
-    *
-    ********************************************************************* */
-
-#define READCSR(r)      *(volatile UInt32 *)(0xB2600000+(r))
-#define WRITECSR(r,v)   *(volatile UInt32 *)(0xB2600000+(r)) = v
-
-#define UART_DR_REG         0x00
-#define UART_FR_REG         0x18
-#define UART_TX_FIFO_FULL   0x20
-
-int uart_write(int val)
-{
-	WRITECSR(UART_DR_REG, val);
-    while ( (READCSR(UART_FR_REG) & UART_TX_FIFO_FULL) );
-    return 0;
-}
+        print_str("done!\n");
 
-int uart_write_str(char * str)
-{
-    while ( *str != 0 ) {
-        uart_write ( *str++ );
-    }
-    return 0;
-}
+	blast_dcache(dcache_size, dcache_lsize);
+	blast_icache(icache_size, icache_lsize);
 
-int uart_write_hex(int val)
-{
-    int i;
-    int tmp;
-
-    uart_write_str("0x");
-    for ( i=0 ; i<8 ; i++ ) {
-        tmp = (val >> ((7-i) * 4 )) & 0xf;
-        tmp = tmp < 10 ? (tmp + '0') : (tmp + 'A' - 10);
-        uart_write(tmp);
-    }
-    uart_write_str("\r\n");
-    return 0;
+	print_str("launching kernel...\n\n");
+
+	/* Jump to load address */
+	((kernel_entry) LOADADDR)(reg_a0, reg_a1, reg_a2, reg_a3);
 }
 
+
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/head.S b/target/linux/adm5120-2.6/image/lzma-loader/src/head.S
index 3bfab9ff1..ee8b3200c 100644
--- a/target/linux/adm5120-2.6/image/lzma-loader/src/head.S
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/head.S
@@ -12,7 +12,11 @@
 
 #define KSEG0		0x80000000
 
+#define C0_STATUS	$12
+#define C0_CAUSE	$13
 #define C0_CONFIG	$16
+#define C0_WATCHLO	$18
+#define C0_WATCHHI	$19
 #define C0_TAGLO	$28
 #define C0_TAGHI	$29
 
@@ -40,17 +44,28 @@
 
 	.text
 
-#if (BZ_STARTUP_ORG)
+#if (LZMA_STARTUP_ORG)
 	.set	noreorder
 
 	b	startup
 	nop
 
-	.org	BZ_STARTUP_ORG
+	.org	LZMA_STARTUP_ORG
 #endif
 
 LEAF(startup)
 	.set noreorder
+	.set mips32
+	
+	mtc0	zero, C0_WATCHLO	# clear watch registers
+	mtc0	zero, C0_WATCHHI
+
+	mtc0	zero, C0_CAUSE		# clear before writing status register
+
+	mfc0	t0, C0_STATUS		# get status register
+	li	t1, ~(0xFF01)
+	and	t0, t1			# mask interrupts
+	mtc0	t0, C0_STATUS		# set up status register
 
 	move	t1, ra			# save return address
 	la	t0, __reloc_label	# get linked address of label
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds b/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds
new file mode 100644
index 000000000..f9358257c
--- /dev/null
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds
@@ -0,0 +1,27 @@
+OUTPUT_ARCH(mips)
+SECTIONS {
+	.text : {
+		_code_start = .;
+		*(.text)
+		*(.text.*)
+		*(.rodata)
+		*(.rodata.*)
+	}
+
+	.data : {
+		*(.data)
+		*(.data.*)
+	}
+	_code_end = .;
+
+	.bss : {
+		*(.bss)
+		*(.bss.*)
+	}
+
+	. = ALIGN(16);
+	. = . + 8192;
+	_stack = .;
+
+	workspace = .;
+}
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds.in b/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds.in
index 8c1bb9bd6..e69de29bb 100644
--- a/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds.in
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/loader.lds.in
@@ -1,27 +0,0 @@
-OUTPUT_ARCH(mips)
-SECTIONS {
-	.text : {
-		_code_start = .;
-		*(.text)
-		*(.text.*)
-		*(.rodata)
-		*(.rodata.*)
-		_code_end = .;
-	}
-
-	.data : {
-		*(.data)
-		*(.data.*)
-	}
-
-	.bss : {
-		*(.bss)
-		*(.bss.*)
-	}
-
-	. = ALIGN(16);
-	. = . + 8192;
-	_stack = .;
-
-	workspace = .;
-}
diff --git a/target/linux/adm5120-2.6/image/lzma-loader/src/lzma-data.lds b/target/linux/adm5120-2.6/image/lzma-loader/src/lzma-data.lds
new file mode 100644
index 000000000..b6984d742
--- /dev/null
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/lzma-data.lds
@@ -0,0 +1,9 @@
+OUTPUT_ARCH(mips)
+SECTIONS {
+	.rodata : {
+		. = ALIGN(16);
+		_lzma_data_start = .;
+		*(.data)
+		_lzma_data_end = .;
+	}
+}