[adm5120] restore support for Infineon's EASY 5120 boards

[openwrt.git] / target / linux / adm5120-2.6 / image / lzma-loader / src / head.S

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 9bfbd53..ee8b320 100644 (file)
--- a/target/linux/adm5120-2.6/image/lzma-loader/src/head.S
+++ b/target/linux/adm5120-2.6/image/lzma-loader/src/head.S
@@ -1,3 +1,6 @@
+/* Copyright 2007 Gabor Juhos <juhosg@freemail.hu>     */
+/* keep original values of the a0,a1,a2,a3 registers   */
+/* modifed to support user defined entry point address */
 /* Copyright 2005 Oleg I. Vdovikin (oleg@cs.msu.su)    */
 /* cache manipulation adapted from Broadcom code       */
 /* idea taken from original bunzip2 decompressor code  */
@@ -9,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
 
@@ -36,22 +43,55 @@
 #define Index_Writeback_Inv_D   0x01
 
        .text
-       LEAF(startup)
+
+#if (LZMA_STARTUP_ORG)
+       .set    noreorder
+
+       b       startup
+       nop
+
+       .org    LZMA_STARTUP_ORG
+#endif
+
+LEAF(startup)
        .set noreorder
+       .set mips32
        
-       /* Copy decompressor code to the right place */
-       li      t2, BZ_TEXT_START
-       add     a0, t2, 0
-       la      a1, code_start
-       la      a2, code_stop
-$L1:
-       lw      t0, 0(a1)
-       sw      t0, 0(a0)
-       add     a1, 4
-       add     a0, 4
-       blt     a1, a2, $L1
+       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
+       bal     __reloc_label           # branch and link to label to
+       nop                             # get actual address
+__reloc_label:
+       subu    t0, ra, t0              # get reloc_delta
+       move    ra, t1                  # restore return address
+
+       beqz    t0, __reloc_end         # if delta is 0 we are in the right place
        nop
-       
+
+       /* Copy our code to the right place */
+       la      t1, _code_start         # get linked address of _code_start
+       la      t2, _code_end           # get linked address of _code_end
+       addu    t0, t0, t1              # calculate actual address of _code_start
+
+__reloc_copy:
+       lw      t3, 0(t0)
+       sw      t3, 0(t1)
+       add     t1, 4
+       blt     t1, t2, __reloc_copy
+       add     t0, 4
+
+__reloc_end:
+
        /* At this point we need to invalidate dcache and */
        /* icache before jumping to new code */
 
@@ -98,10 +138,10 @@ $L1:
        .set    mips0
        bne     t0,t1,1b
        addu    t0,s1
-       
+
 nodc:
        /* Now we get to do it all again for the I$ */
-       
+
        move    s3,zero                 /* just in case there is no icache */
        move    s4,zero
 
@@ -145,11 +185,25 @@ nodc:
        addu    t0,s3
 
 noic:
-       move    a0,s3                   /* icache line size */
-       move    a1,s4                   /* icache size */
-       move    a2,s1                   /* dcache line size */
-       jal     t2
-       move    a3,s2                   /* dcache size */
-       
+       /* Setup new "C" stack */
+       la      sp, _stack
+
+       addiu   sp, -32                 /* reserve stack for parameters */
+#if 0
+       sw      a0, 0(sp)
+       sw      a1, 4(sp)
+       sw      a2, 8(sp)
+       sw      a3, 12(sp)
+#endif
+       sw      s3, 16(sp)              /* icache line size */
+       sw      s4, 20(sp)              /* icache size */
+       sw      s1, 24(sp)              /* dcache line size */
+       sw      s2, 28(sp)              /* dcache size */
+
+       /* jump to the decompressor routine */
+       la      t0, decompress_entry
+       jr      t0
+       nop
+
        .set reorder
-       END(startup)
+END(startup)