git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_A9_Zynq_ZC702/RTOSDemo_bsp/ps7_cortexa9_0/libsrc/standalone_v6_6/src/boot.S

   1 /******************************************************************************
   2 *
   3 * Copyright (C) 2010 - 2016 Xilinx, Inc.  All rights reserved.
   4 *
   5 * Permission is hereby granted, free of charge, to any person obtaining a copy
   6 * of this software and associated documentation files (the "Software"), to deal
   7 * in the Software without restriction, including without limitation the rights
   8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9 * copies of the Software, and to permit persons to whom the Software is
  10 * furnished to do so, subject to the following conditions:
  11 *
  12 * The above copyright notice and this permission notice shall be included in
  13 * all copies or substantial portions of the Software.
  14 *
  15 * Use of the Software is limited solely to applications:
  16 * (a) running on a Xilinx device, or
  17 * (b) that interact with a Xilinx device through a bus or interconnect.
  18 *
  19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  22 * XILINX  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  23 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
  24 * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  25 * SOFTWARE.
  26 *
  27 * Except as contained in this notice, the name of the Xilinx shall not be used
  28 * in advertising or otherwise to promote the sale, use or other dealings in
  29 * this Software without prior written authorization from Xilinx.
  30 *
  31 ******************************************************************************/
  32 /*****************************************************************************/
  33 /**
  34 * @file boot.S
  35 *
  36 * @addtogroup a9_boot_code Cortex A9 Processor Boot Code
  37 * @{
  38 * <h2> boot.S </h2>
  39 * The boot code performs minimum configuration which is required for an
  40 * application to run starting from processor's reset state. Below is a
  41 * sequence illustrating what all configuration is performed before control
  42 * reaches to main function.
  43 *
  44 * 1. Program vector table base for exception handling
  45 * 2. Invalidate instruction cache, data cache and TLBs
  46 * 3. Program stack pointer for various modes (IRQ, FIQ, supervisor, undefine,
  47 *    abort, system)
  48 * 4. Configure MMU with short descriptor translation table format and program
  49 *    base address of translation table
  50 * 5. Enable data cache, instruction cache and MMU
  51 * 6. Enable Floating point unit
  52 * 7. Transfer control to _start which clears BSS sections, initializes
  53 *    global timer and runs global constructor before jumping to main
  54 *    application
  55 *
  56 * <pre>
  57 * MODIFICATION HISTORY:
  58 *
  59 * Ver   Who     Date     Changes
  60 * ----- ------- -------- ---------------------------------------------------
  61 * 1.00a ecm/sdm 10/20/09 Initial version
  62 * 3.06a sgd     05/15/12 Updated L2CC Auxiliary and Tag RAM Latency control
  63 *                        register settings.
  64 * 3.06a asa     06/17/12 Modified the TTBR settings and L2 Cache auxiliary
  65 *                        register settings.
  66 * 3.07a asa     07/16/12 Modified the L2 Cache controller settings to improve
  67 *                        performance. Changed the property of the ".boot"
  68 *                        section.
  69 * 3.07a sgd     08/21/12 Modified the L2 Cache controller and cp15 Aux Control
  70 *               Register settings
  71 * 3.09a sgd     02/06/13 Updated SLCR l2c Ram Control register to a
  72 *               value of 0x00020202. Fix for CR 697094 (SI#687034).
  73 * 3.10a srt     04/18/13 Implemented ARM Erratas. Please refer to file
  74 *                        'xil_errata.h' for errata description
  75 * 4.2   pkp     06/19/14 Enabled asynchronous abort exception
  76 * 5.0   pkp     16/15/14 Modified initialization code to enable scu after
  77 *                        MMU is enabled
  78 * 5.1   pkp     05/13/15 Changed the initialization order so to first invalidate
  79 *                        caches and TLB, enable MMU and caches, then enable SMP
  80 *                        bit in ACTLR. L2Cache invalidation and enabling of L2Cache
  81 *                        is done later.
  82 * 5.4   asa     12/6/15  Added code to initialize SPSR for all relevant modes.
  83 * 6.0   mus     08/04/16 Added code to detect zynq-7000 base silicon configuration and
  84 *                        attempt to enable dual core behavior on single cpu zynq-7000s
  85 *                        devices is prevented from corrupting system behavior.
  86 * 6.0   mus     08/24/16 Check CPU core before putting cpu1 to reset for single core
  87 *                        zynq-7000s devices
  88 *
  89 * </pre>
  90 *
  91 * @note
  92 *
  93 * None.
  94 *
  95 ******************************************************************************/
  96
  97 #include "xparameters.h"
  98 #include "xil_errata.h"
  99
 100 .globl MMUTable
 101 .global _prestart
 102 .global _boot
 103 .global __stack
 104 .global __irq_stack
 105 .global __supervisor_stack
 106 .global __abort_stack
 107 .global __fiq_stack
 108 .global __undef_stack
 109 .global _vector_table
 110
 111 .set PSS_L2CC_BASE_ADDR, 0xF8F02000
 112 .set PSS_SLCR_BASE_ADDR, 0xF8000000
 113
 114 .set RESERVED,          0x0fffff00
 115 .set TblBase ,          MMUTable
 116 .set LRemap,            0xFE00000F              /* set the base address of the peripheral block as not shared */
 117 .set L2CCWay,           (PSS_L2CC_BASE_ADDR + 0x077C)   /*(PSS_L2CC_BASE_ADDR + PSS_L2CC_CACHE_INVLD_WAY_OFFSET)*/
 118 .set L2CCSync,          (PSS_L2CC_BASE_ADDR + 0x0730)   /*(PSS_L2CC_BASE_ADDR + PSS_L2CC_CACHE_SYNC_OFFSET)*/
 119 .set L2CCCrtl,          (PSS_L2CC_BASE_ADDR + 0x0100)   /*(PSS_L2CC_BASE_ADDR + PSS_L2CC_CNTRL_OFFSET)*/
 120 .set L2CCAuxCrtl,       (PSS_L2CC_BASE_ADDR + 0x0104)   /*(PSS_L2CC_BASE_ADDR + XPSS_L2CC_AUX_CNTRL_OFFSET)*/
 121 .set L2CCTAGLatReg,     (PSS_L2CC_BASE_ADDR + 0x0108)   /*(PSS_L2CC_BASE_ADDR + XPSS_L2CC_TAG_RAM_CNTRL_OFFSET)*/
 122 .set L2CCDataLatReg,    (PSS_L2CC_BASE_ADDR + 0x010C)   /*(PSS_L2CC_BASE_ADDR + XPSS_L2CC_DATA_RAM_CNTRL_OFFSET)*/
 123 .set L2CCIntClear,      (PSS_L2CC_BASE_ADDR + 0x0220)   /*(PSS_L2CC_BASE_ADDR + XPSS_L2CC_IAR_OFFSET)*/
 124 .set L2CCIntRaw,        (PSS_L2CC_BASE_ADDR + 0x021C)   /*(PSS_L2CC_BASE_ADDR + XPSS_L2CC_ISR_OFFSET)*/
 125
 126 .set SLCRlockReg,           (PSS_SLCR_BASE_ADDR + 0x04) /*(PSS_SLCR_BASE_ADDR + XPSS_SLCR_LOCK_OFFSET)*/
 127 .set SLCRUnlockReg,     (PSS_SLCR_BASE_ADDR + 0x08)     /*(PSS_SLCR_BASE_ADDR + XPSS_SLCR_UNLOCK_OFFSET)*/
 128 .set SLCRL2cRamReg,     (PSS_SLCR_BASE_ADDR + 0xA1C) /*(PSS_SLCR_BASE_ADDR + XPSS_SLCR_L2C_RAM_OFFSET)*/
 129 .set SLCRCPURSTReg,     (0xF8000000 + 0x244)           /*(XPS_SYS_CTRL_BASEADDR + A9_CPU_RST_CTRL_OFFSET)*/
 130 .set EFUSEStaus,        (0xF800D000 + 0x10)            /*(XPS_EFUSE_BASEADDR + EFUSE_STATUS_OFFSET)*/
 131
 132 /* workaround for simulation not working when L1 D and I caches,MMU and  L2 cache enabled - DT568997 */
 133 .if SIM_MODE == 1
 134 .set CRValMmuCac,       0b00000000000000        /* Disable IDC, and MMU */
 135 .else
 136 .set CRValMmuCac,       0b01000000000101        /* Enable IDC, and MMU */
 137 .endif
 138
 139 .set CRValHiVectorAddr, 0b10000000000000        /* Set the Vector address to high, 0xFFFF0000 */
 140
 141 .set L2CCAuxControl,    0x72360000              /* Enable all prefetching, Cache replacement policy, Parity enable,
 142                                         Event monitor bus enable and Way Size (64 KB) */
 143 .set L2CCControl,       0x01                    /* Enable L2CC */
 144 .set L2CCTAGLatency,    0x0111                  /* latency for TAG RAM */
 145 .set L2CCDataLatency,   0x0121                  /* latency for DATA RAM */
 146
 147 .set SLCRlockKey,               0x767B                  /* SLCR lock key */
 148 .set SLCRUnlockKey,             0xDF0D                  /* SLCR unlock key */
 149 .set SLCRL2cRamConfig,      0x00020202      /* SLCR L2C ram configuration */
 150
 151 /* Stack Pointer locations for boot code */
 152 .set Undef_stack,       __undef_stack
 153 .set FIQ_stack,         __fiq_stack
 154 .set Abort_stack,       __abort_stack
 155 .set SPV_stack,         __supervisor_stack
 156 .set IRQ_stack,         __irq_stack
 157 .set SYS_stack,         __stack
 158
 159 .set vector_base,       _vector_table
 160
 161 .set FPEXC_EN,          0x40000000              /* FPU enable bit, (1 << 30) */
 162
 163 .section .boot,"ax"
 164
 165
 166 /* this initializes the various processor modes */
 167
 168 _prestart:
 169 _boot:
 170
 171 #if XPAR_CPU_ID==0
 172         /* only allow cpu0 through */
 173         mrc     p15,0,r1,c0,c0,5
 174         and     r1, r1, #0xf
 175         cmp     r1, #0
 176         beq     CheckEFUSE
 177         EndlessLoop0:
 178                 wfe
 179         b       EndlessLoop0
 180
 181 CheckEFUSE:
 182         ldr r0,=EFUSEStaus
 183         ldr r1,[r0]                             /* Read eFuse setting */
 184         ands r1,r1,#0x80                        /* Check whether device is having single core */
 185         beq OKToRun
 186
 187  /* single core device, reset cpu1 */
 188         ldr     r0,=SLCRUnlockReg               /* Load SLCR base address base + unlock register */
 189         ldr     r1,=SLCRUnlockKey               /* set unlock key */
 190         str     r1, [r0]                        /* Unlock SLCR */
 191
 192         ldr r0,=SLCRCPURSTReg
 193         ldr r1,[r0]                             /* Read CPU Software Reset Control register */
 194         orr r1,r1,#0x22
 195         str r1,[r0]                             /* Reset CPU1 */
 196
 197         ldr     r0,=SLCRlockReg                 /* Load SLCR base address base + lock register */
 198         ldr     r1,=SLCRlockKey                 /* set lock key */
 199         str     r1, [r0]                        /* lock SLCR */
 200
 201 #elif XPAR_CPU_ID==1
 202         /* only allow cpu1 through */
 203        mrc      p15,0,r1,c0,c0,5
 204        and      r1, r1, #0xf
 205        cmp      r1, #1
 206        beq      CheckEFUSE1
 207        b        EndlessLoop1
 208
 209 CheckEFUSE1:
 210         ldr r0,=EFUSEStaus
 211         ldr r1,[r0]                             /* Read eFuse setting */
 212         ands r1,r1,#0x80                        /* Check whether device is having single core */
 213         beq OKToRun
 214         EndlessLoop1:
 215                 wfe
 216         b       EndlessLoop1
 217 #endif
 218
 219 OKToRun:
 220         mrc     p15, 0, r0, c0, c0, 0           /* Get the revision */
 221         and     r5, r0, #0x00f00000
 222         and     r6, r0, #0x0000000f
 223         orr     r6, r6, r5, lsr #20-4
 224
 225 #ifdef CONFIG_ARM_ERRATA_742230
 226         cmp     r6, #0x22                       /* only present up to r2p2 */
 227         mrcle   p15, 0, r10, c15, c0, 1         /* read diagnostic register */
 228         orrle   r10, r10, #1 << 4               /* set bit #4 */
 229         mcrle   p15, 0, r10, c15, c0, 1         /* write diagnostic register */
 230 #endif
 231
 232 #ifdef CONFIG_ARM_ERRATA_743622
 233         teq     r5, #0x00200000                 /* only present in r2p* */
 234         mrceq   p15, 0, r10, c15, c0, 1         /* read diagnostic register */
 235         orreq   r10, r10, #1 << 6               /* set bit #6 */
 236         mcreq   p15, 0, r10, c15, c0, 1         /* write diagnostic register */
 237 #endif
 238
 239         /* set VBAR to the _vector_table address in linker script */
 240         ldr     r0, =vector_base
 241         mcr     p15, 0, r0, c12, c0, 0
 242
 243         /*invalidate scu*/
 244         ldr     r7, =0xf8f0000c
 245         ldr     r6, =0xffff
 246         str     r6, [r7]
 247
 248         /* Invalidate caches and TLBs */
 249         mov     r0,#0                           /* r0 = 0  */
 250         mcr     p15, 0, r0, c8, c7, 0           /* invalidate TLBs */
 251         mcr     p15, 0, r0, c7, c5, 0           /* invalidate icache */
 252         mcr     p15, 0, r0, c7, c5, 6           /* Invalidate branch predictor array */
 253         bl      invalidate_dcache               /* invalidate dcache */
 254
 255         /* Disable MMU, if enabled */
 256         mrc     p15, 0, r0, c1, c0, 0           /* read CP15 register 1 */
 257         bic     r0, r0, #0x1                    /* clear bit 0 */
 258         mcr     p15, 0, r0, c1, c0, 0           /* write value back */
 259
 260 #ifdef SHAREABLE_DDR
 261         /* Mark the entire DDR memory as shareable */
 262         ldr     r3, =0x3ff                      /* 1024 entries to cover 1G DDR */
 263         ldr     r0, =TblBase                    /* MMU Table address in memory */
 264         ldr     r2, =0x15de6                    /* S=b1 TEX=b101 AP=b11, Domain=b1111, C=b0, B=b1 */
 265 shareable_loop:
 266         str     r2, [r0]                        /* write the entry to MMU table */
 267         add     r0, r0, #0x4                    /* next entry in the table */
 268         add     r2, r2, #0x100000               /* next section */
 269         subs    r3, r3, #1
 270         bge     shareable_loop                  /* loop till 1G is covered */
 271 #endif
 272
 273         mrs     r0, cpsr                        /* get the current PSR */
 274         mvn     r1, #0x1f                       /* set up the irq stack pointer */
 275         and     r2, r1, r0
 276         orr     r2, r2, #0x12                   /* IRQ mode */
 277         msr     cpsr, r2
 278         ldr     r13,=IRQ_stack                  /* IRQ stack pointer */
 279         bic r2, r2, #(0x1 << 9)                  /* Set EE bit to little-endian */
 280         msr spsr_fsxc,r2
 281
 282         mrs     r0, cpsr                        /* get the current PSR */
 283         mvn     r1, #0x1f                       /* set up the supervisor stack pointer */
 284         and     r2, r1, r0
 285         orr     r2, r2, #0x13                   /* supervisor mode */
 286         msr     cpsr, r2
 287         ldr     r13,=SPV_stack                  /* Supervisor stack pointer */
 288         bic r2, r2, #(0x1 << 9)                 /* Set EE bit to little-endian */
 289         msr spsr_fsxc,r2
 290
 291         mrs     r0, cpsr                        /* get the current PSR */
 292         mvn     r1, #0x1f                       /* set up the Abort  stack pointer */
 293         and     r2, r1, r0
 294         orr     r2, r2, #0x17                   /* Abort mode */
 295         msr     cpsr, r2
 296         ldr     r13,=Abort_stack                /* Abort stack pointer */
 297         bic r2, r2, #(0x1 << 9)                 /* Set EE bit to little-endian */
 298         msr spsr_fsxc,r2
 299
 300         mrs     r0, cpsr                        /* get the current PSR */
 301         mvn     r1, #0x1f                       /* set up the FIQ stack pointer */
 302         and     r2, r1, r0
 303         orr     r2, r2, #0x11                   /* FIQ mode */
 304         msr     cpsr, r2
 305         ldr     r13,=FIQ_stack                  /* FIQ stack pointer */
 306         bic r2, r2, #(0x1 << 9)                 /* Set EE bit to little-endian */
 307         msr spsr_fsxc,r2
 308
 309         mrs     r0, cpsr                        /* get the current PSR */
 310         mvn     r1, #0x1f                       /* set up the Undefine stack pointer */
 311         and     r2, r1, r0
 312         orr     r2, r2, #0x1b                   /* Undefine mode */
 313         msr     cpsr, r2
 314         ldr     r13,=Undef_stack                /* Undefine stack pointer */
 315         bic r2, r2, #(0x1 << 9)                 /* Set EE bit to little-endian */
 316         msr spsr_fsxc,r2
 317
 318         mrs     r0, cpsr                        /* get the current PSR */
 319         mvn     r1, #0x1f                       /* set up the system stack pointer */
 320         and     r2, r1, r0
 321         orr     r2, r2, #0x1F                   /* SYS mode */
 322         msr     cpsr, r2
 323         ldr     r13,=SYS_stack                  /* SYS stack pointer */
 324
 325         /*set scu enable bit in scu*/
 326         ldr     r7, =0xf8f00000
 327         ldr     r0, [r7]
 328         orr     r0, r0, #0x1
 329         str     r0, [r7]
 330
 331         /* enable MMU and cache */
 332
 333         ldr     r0,=TblBase                     /* Load MMU translation table base */
 334         orr     r0, r0, #0x5B                   /* Outer-cacheable, WB */
 335         mcr     15, 0, r0, c2, c0, 0            /* TTB0 */
 336
 337         mvn     r0,#0                           /* Load MMU domains -- all ones=manager */
 338         mcr     p15,0,r0,c3,c0,0
 339
 340         /* Enable mmu, icahce and dcache */
 341         ldr     r0,=CRValMmuCac
 342         mcr     p15,0,r0,c1,c0,0                /* Enable cache and MMU */
 343         dsb                                     /* dsb  allow the MMU to start up */
 344         isb                                     /* isb  flush prefetch buffer */
 345
 346         /* Write to ACTLR */
 347         mrc     p15, 0, r0, c1, c0, 1           /* Read ACTLR*/
 348         orr     r0, r0, #(0x01 << 6)            /* set SMP bit */
 349         orr     r0, r0, #(0x01 )                /* Cache/TLB maintenance broadcast */
 350         mcr     p15, 0, r0, c1, c0, 1           /* Write ACTLR*/
 351
 352 /* Invalidate L2 Cache and enable L2 Cache*/
 353 /* For AMP, assume running on CPU1. Don't initialize L2 Cache (up to Linux) */
 354 #if USE_AMP!=1
 355         ldr     r0,=L2CCCrtl                    /* Load L2CC base address base + control register */
 356         mov     r1, #0                          /* force the disable bit */
 357         str     r1, [r0]                        /* disable the L2 Caches */
 358
 359         ldr     r0,=L2CCAuxCrtl                 /* Load L2CC base address base + Aux control register */
 360         ldr     r1,[r0]                         /* read the register */
 361         ldr     r2,=L2CCAuxControl              /* set the default bits */
 362         orr     r1,r1,r2
 363         str     r1, [r0]                        /* store the Aux Control Register */
 364
 365         ldr     r0,=L2CCTAGLatReg               /* Load L2CC base address base + TAG Latency address */
 366         ldr     r1,=L2CCTAGLatency              /* set the latencies for the TAG*/
 367         str     r1, [r0]                        /* store the TAG Latency register Register */
 368
 369         ldr     r0,=L2CCDataLatReg              /* Load L2CC base address base + Data Latency address */
 370         ldr     r1,=L2CCDataLatency             /* set the latencies for the Data*/
 371         str     r1, [r0]                        /* store the Data Latency register Register */
 372
 373         ldr     r0,=L2CCWay                     /* Load L2CC base address base + way register*/
 374         ldr     r2, =0xFFFF
 375         str     r2, [r0]                        /* force invalidate */
 376
 377         ldr     r0,=L2CCSync                    /* need to poll 0x730, PSS_L2CC_CACHE_SYNC_OFFSET */
 378                                                 /* Load L2CC base address base + sync register*/
 379         /* poll for completion */
 380 Sync:   ldr     r1, [r0]
 381         cmp     r1, #0
 382         bne     Sync
 383
 384         ldr     r0,=L2CCIntRaw                  /* clear pending interrupts */
 385         ldr     r1,[r0]
 386         ldr     r0,=L2CCIntClear
 387         str     r1,[r0]
 388
 389         ldr     r0,=SLCRUnlockReg               /* Load SLCR base address base + unlock register */
 390         ldr     r1,=SLCRUnlockKey               /* set unlock key */
 391         str     r1, [r0]                        /* Unlock SLCR */
 392
 393         ldr     r0,=SLCRL2cRamReg               /* Load SLCR base address base + l2c Ram Control register */
 394         ldr     r1,=SLCRL2cRamConfig            /* set the configuration value */
 395         str     r1, [r0]                        /* store the L2c Ram Control Register */
 396
 397         ldr     r0,=SLCRlockReg                 /* Load SLCR base address base + lock register */
 398         ldr     r1,=SLCRlockKey                 /* set lock key */
 399         str     r1, [r0]                        /* lock SLCR */
 400
 401         ldr     r0,=L2CCCrtl                    /* Load L2CC base address base + control register */
 402         ldr     r1,[r0]                         /* read the register */
 403         mov     r2, #L2CCControl                /* set the enable bit */
 404         orr     r1,r1,r2
 405         str     r1, [r0]                        /* enable the L2 Caches */
 406 #endif
 407
 408         mov     r0, r0
 409         mrc     p15, 0, r1, c1, c0, 2           /* read cp access control register (CACR) into r1 */
 410         orr     r1, r1, #(0xf << 20)            /* enable full access for p10 & p11 */
 411         mcr     p15, 0, r1, c1, c0, 2           /* write back into CACR */
 412
 413         /* enable vfp */
 414         fmrx    r1, FPEXC                       /* read the exception register */
 415         orr     r1,r1, #FPEXC_EN                /* set VFP enable bit, leave the others in orig state */
 416         fmxr    FPEXC, r1                       /* write back the exception register */
 417
 418         mrc     p15,0,r0,c1,c0,0                /* flow prediction enable */
 419         orr     r0, r0, #(0x01 << 11)           /* #0x8000 */
 420         mcr     p15,0,r0,c1,c0,0
 421
 422         mrc     p15,0,r0,c1,c0,1                /* read Auxiliary Control Register */
 423         orr     r0, r0, #(0x1 << 2)             /* enable Dside prefetch */
 424         orr     r0, r0, #(0x1 << 1)             /* enable L2 Prefetch hint */
 425         mcr     p15,0,r0,c1,c0,1                /* write Auxiliary Control Register */
 426
 427         mrs     r0, cpsr                        /* get the current PSR */
 428         bic     r0, r0, #0x100                  /* enable asynchronous abort exception */
 429         msr     cpsr_xsf, r0
 430
 431
 432         b       _start                          /* jump to C startup code */
 433         and     r0, r0, r0                      /* no op */
 434
 435 .Ldone: b       .Ldone                          /* Paranoia: we should never get here */
 436
 437
 438 /*
 439  *************************************************************************
 440  *
 441  * invalidate_dcache - invalidate the entire d-cache by set/way
 442  *
 443  * Note: for Cortex-A9, there is no cp instruction for invalidating
 444  * the whole D-cache. Need to invalidate each line.
 445  *
 446  *************************************************************************
 447  */
 448 invalidate_dcache:
 449         mrc     p15, 1, r0, c0, c0, 1           /* read CLIDR */
 450         ands    r3, r0, #0x7000000
 451         mov     r3, r3, lsr #23                 /* cache level value (naturally aligned) */
 452         beq     finished
 453         mov     r10, #0                         /* start with level 0 */
 454 loop1:
 455         add     r2, r10, r10, lsr #1            /* work out 3xcachelevel */
 456         mov     r1, r0, lsr r2                  /* bottom 3 bits are the Cache type for this level */
 457         and     r1, r1, #7                      /* get those 3 bits alone */
 458         cmp     r1, #2
 459         blt     skip                            /* no cache or only instruction cache at this level */
 460         mcr     p15, 2, r10, c0, c0, 0          /* write the Cache Size selection register */
 461         isb                                     /* isb to sync the change to the CacheSizeID reg */
 462         mrc     p15, 1, r1, c0, c0, 0           /* reads current Cache Size ID register */
 463         and     r2, r1, #7                      /* extract the line length field */
 464         add     r2, r2, #4                      /* add 4 for the line length offset (log2 16 bytes) */
 465         ldr     r4, =0x3ff
 466         ands    r4, r4, r1, lsr #3              /* r4 is the max number on the way size (right aligned) */
 467         clz     r5, r4                          /* r5 is the bit position of the way size increment */
 468         ldr     r7, =0x7fff
 469         ands    r7, r7, r1, lsr #13             /* r7 is the max number of the index size (right aligned) */
 470 loop2:
 471         mov     r9, r4                          /* r9 working copy of the max way size (right aligned) */
 472 loop3:
 473         orr     r11, r10, r9, lsl r5            /* factor in the way number and cache number into r11 */
 474         orr     r11, r11, r7, lsl r2            /* factor in the index number */
 475         mcr     p15, 0, r11, c7, c6, 2          /* invalidate by set/way */
 476         subs    r9, r9, #1                      /* decrement the way number */
 477         bge     loop3
 478         subs    r7, r7, #1                      /* decrement the index */
 479         bge     loop2
 480 skip:
 481         add     r10, r10, #2                    /* increment the cache number */
 482         cmp     r3, r10
 483         bgt     loop1
 484
 485 finished:
 486         mov     r10, #0                         /* swith back to cache level 0 */
 487         mcr     p15, 2, r10, c0, c0, 0          /* select current cache level in cssr */
 488         dsb
 489         isb
 490
 491         bx      lr
 492
 493 .end
 494 /**
 495 * @} End of "addtogroup a9_boot_code".
 496 */