target/linux/adm5120-2.6/image/lzma-loader/src/head.S

   1 /* Copyright 2007 Gabor Juhos <juhosg@freemail.hu>      */
   2 /* keep original values of the a0,a1,a2,a3 registers    */
   3 /* modifed to support user defined entry point address  */
   4 /* Copyright 2005 Oleg I. Vdovikin (oleg@cs.msu.su)     */
   5 /* cache manipulation adapted from Broadcom code        */
   6 /* idea taken from original bunzip2 decompressor code   */
   7 /* Copyright 2004 Manuel Novoa III (mjn3@codepoet.org)  */
   8 /* Licensed under the linux kernel's version of the GPL.*/
   9
  10 #include <asm/asm.h>
  11 #include <asm/regdef.h>
  12
  13 #define KSEG0           0x80000000
  14
  15 #define C0_CONFIG       $16
  16 #define C0_TAGLO        $28
  17 #define C0_TAGHI        $29
  18
  19 #define CONF1_DA_SHIFT  7                       /* D$ associativity */
  20 #define CONF1_DA_MASK   0x00000380
  21 #define CONF1_DA_BASE   1
  22 #define CONF1_DL_SHIFT  10                      /* D$ line size */
  23 #define CONF1_DL_MASK   0x00001c00
  24 #define CONF1_DL_BASE   2
  25 #define CONF1_DS_SHIFT  13                      /* D$ sets/way */
  26 #define CONF1_DS_MASK   0x0000e000
  27 #define CONF1_DS_BASE   64
  28 #define CONF1_IA_SHIFT  16                      /* I$ associativity */
  29 #define CONF1_IA_MASK   0x00070000
  30 #define CONF1_IA_BASE   1
  31 #define CONF1_IL_SHIFT  19                      /* I$ line size */
  32 #define CONF1_IL_MASK   0x00380000
  33 #define CONF1_IL_BASE   2
  34 #define CONF1_IS_SHIFT  22                      /* Instruction cache sets/way */
  35 #define CONF1_IS_MASK   0x01c00000
  36 #define CONF1_IS_BASE   64
  37
  38 #define Index_Invalidate_I      0x00
  39 #define Index_Writeback_Inv_D   0x01
  40
  41         .text
  42
  43 #if (BZ_STARTUP_ORG)
  44         .set    noreorder
  45
  46         b       startup
  47         nop
  48
  49         .org    BZ_STARTUP_ORG
  50 #endif
  51
  52 LEAF(startup)
  53         .set noreorder
  54
  55         move    t1, ra                  # save return address
  56         la      t0, __reloc_label       # get linked address of label
  57         bal     __reloc_label           # branch and link to label to
  58         nop                             # get actual address
  59 __reloc_label:
  60         subu    t0, ra, t0              # get reloc_delta
  61         move    ra, t1                  # restore return address
  62
  63         beqz    t0, __reloc_end         # if delta is 0 we are in the right place
  64         nop
  65
  66         /* Copy our code to the right place */
  67         la      t1, _code_start         # get linked address of _code_start
  68         la      t2, _code_end           # get linked address of _code_end
  69         addu    t0, t0, t1              # calculate actual address of _code_start
  70
  71 __reloc_copy:
  72         lw      t3, 0(t0)
  73         sw      t3, 0(t1)
  74         add     t1, 4
  75         blt     t1, t2, __reloc_copy
  76         add     t0, 4
  77
  78 __reloc_end:
  79
  80         /* At this point we need to invalidate dcache and */
  81         /* icache before jumping to new code */
  82
  83 1:      /* Get cache sizes */
  84         .set    mips32
  85         mfc0    s0,C0_CONFIG,1
  86         .set    mips0
  87
  88         li      s1,CONF1_DL_MASK
  89         and     s1,s0
  90         beq     s1,zero,nodc
  91         nop
  92
  93         srl     s1,CONF1_DL_SHIFT
  94         li      t0,CONF1_DL_BASE
  95         sll     s1,t0,s1                /* s1 has D$ cache line size */
  96
  97         li      s2,CONF1_DA_MASK
  98         and     s2,s0
  99         srl     s2,CONF1_DA_SHIFT
 100         addiu   s2,CONF1_DA_BASE        /* s2 now has D$ associativity */
 101
 102         li      t0,CONF1_DS_MASK
 103         and     t0,s0
 104         srl     t0,CONF1_DS_SHIFT
 105         li      s3,CONF1_DS_BASE
 106         sll     s3,s3,t0                /* s3 has D$ sets per way */
 107
 108         multu   s2,s3                   /* sets/way * associativity */
 109         mflo    t0                      /* total cache lines */
 110
 111         multu   s1,t0                   /* D$ linesize * lines */
 112         mflo    s2                      /* s2 is now D$ size in bytes */
 113
 114         /* Initilize the D$: */
 115         mtc0    zero,C0_TAGLO
 116         mtc0    zero,C0_TAGHI
 117
 118         li      t0,KSEG0                /* Just an address for the first $ line */
 119         addu    t1,t0,s2                /*  + size of cache == end */
 120
 121         .set    mips3
 122 1:      cache   Index_Writeback_Inv_D,0(t0)
 123         .set    mips0
 124         bne     t0,t1,1b
 125         addu    t0,s1
 126
 127 nodc:
 128         /* Now we get to do it all again for the I$ */
 129
 130         move    s3,zero                 /* just in case there is no icache */
 131         move    s4,zero
 132
 133         li      t0,CONF1_IL_MASK
 134         and     t0,s0
 135         beq     t0,zero,noic
 136         nop
 137
 138         srl     t0,CONF1_IL_SHIFT
 139         li      s3,CONF1_IL_BASE
 140         sll     s3,t0                   /* s3 has I$ cache line size */
 141
 142         li      t0,CONF1_IA_MASK
 143         and     t0,s0
 144         srl     t0,CONF1_IA_SHIFT
 145         addiu   s4,t0,CONF1_IA_BASE     /* s4 now has I$ associativity */
 146
 147         li      t0,CONF1_IS_MASK
 148         and     t0,s0
 149         srl     t0,CONF1_IS_SHIFT
 150         li      s5,CONF1_IS_BASE
 151         sll     s5,t0                   /* s5 has I$ sets per way */
 152
 153         multu   s4,s5                   /* sets/way * associativity */
 154         mflo    t0                      /* s4 is now total cache lines */
 155
 156         multu   s3,t0                   /* I$ linesize * lines */
 157         mflo    s4                      /* s4 is cache size in bytes */
 158
 159         /* Initilize the I$: */
 160         mtc0    zero,C0_TAGLO
 161         mtc0    zero,C0_TAGHI
 162
 163         li      t0,KSEG0                /* Just an address for the first $ line */
 164         addu    t1,t0,s4                /*  + size of cache == end */
 165
 166         .set    mips3
 167 1:      cache   Index_Invalidate_I,0(t0)
 168         .set    mips0
 169         bne     t0,t1,1b
 170         addu    t0,s3
 171
 172 noic:
 173         /* Setup new "C" stack */
 174         la      sp, _stack
 175
 176         addiu   sp, -32                 /* reserve stack for parameters */
 177 #if 0
 178         sw      a0, 0(sp)
 179         sw      a1, 4(sp)
 180         sw      a2, 8(sp)
 181         sw      a3, 12(sp)
 182 #endif
 183         sw      s3, 16(sp)              /* icache line size */
 184         sw      s4, 20(sp)              /* icache size */
 185         sw      s1, 24(sp)              /* dcache line size */
 186         sw      s2, 28(sp)              /* dcache size */
 187
 188         /* jump to the decompressor routine */
 189         la      t0, decompress_entry
 190         jr      t0
 191         nop
 192
 193         .set reorder
 194 END(startup)