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