git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_A53_64-bit_UltraScale_MPSoC/RTOSDemo_A53_bsp/psu_cortexa53_0/libsrc/standalone_v6_1/src/boot.S

   1 /******************************************************************************
   2 *
   3 * Copyright (C) 2014 - 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 * This file contains the initial startup code for the Cortex A53 processor
  37 * It checks the current exception level and if it matches with selected
  38 * exception level, then only it initializes the required system registers and
  39 * executes the code further, otherwise it will loop busy loop around error.
  40 * If the selected exception level is EL3, execution of the processor has to
  41 * be in EL3. If the selected exception level is EL1 non-secure, execution of
  42 * the processor can be EL2 or EL1.
  43 *
  44 *
  45 * <pre>
  46 * MODIFICATION HISTORY:
  47 *
  48 * Ver   Who     Date     Changes
  49 * ----- ------- -------- ---------------------------------------------------
  50 * 5.00  pkp     05/21/14 Initial version
  51 * 6.00  pkp     07/25/16 Program the counter frequency
  52 *
  53 * @note
  54 *
  55 * None.
  56 *
  57 ******************************************************************************/
  58
  59 #include "xparameters.h"
  60 #include "bspconfig.h"
  61
  62 .globl MMUTableL0
  63 .globl MMUTableL1
  64 .globl MMUTableL2
  65 .global _prestart
  66 .global _boot
  67
  68 .global __el3_stack
  69 .global __el2_stack
  70 .global __el1_stack
  71 .global __el0_stack
  72 .global _vector_table
  73
  74 .set EL3_stack,         __el3_stack
  75 .set EL2_stack,         __el2_stack
  76 .set EL1_stack,         __el1_stack
  77 .set EL0_stack,         __el0_stack
  78
  79 .set TT_S1_FAULT,       0x0
  80 .set TT_S1_TABLE,       0x3
  81
  82 .set L0Table,   MMUTableL0
  83 .set L1Table,   MMUTableL1
  84 .set L2Table,   MMUTableL2
  85 .set vector_base,       _vector_table
  86 .set rvbar_base,        0xFD5C0040
  87
  88 .set counterfreq,       XPAR_CPU_CORTEXA53_0_TIMESTAMP_CLK_FREQ
  89 .set MODE_EL1, 0x5
  90 .set DAIF_BIT,  0x1C0
  91
  92 .section .boot,"ax"
  93
  94
  95 /* this initializes the various processor modes */
  96
  97 _prestart:
  98 _boot:
  99         mov      x0, #0
 100         mov      x1, #0
 101         mov      x2, #0
 102         mov      x3, #0
 103         mov      x4, #0
 104         mov      x5, #0
 105         mov      x6, #0
 106         mov      x7, #0
 107         mov      x8, #0
 108         mov      x9, #0
 109         mov      x10, #0
 110         mov      x11, #0
 111         mov      x12, #0
 112         mov      x13, #0
 113         mov      x14, #0
 114         mov      x15, #0
 115         mov      x16, #0
 116         mov      x17, #0
 117         mov      x18, #0
 118         mov      x19, #0
 119         mov      x20, #0
 120         mov      x21, #0
 121         mov      x22, #0
 122         mov      x23, #0
 123         mov      x24, #0
 124         mov      x25, #0
 125         mov      x26, #0
 126         mov      x27, #0
 127         mov      x28, #0
 128         mov      x29, #0
 129         mov      x30, #0
 130 #if 0 //dont put other a53 cpus in wfi
 131    //Which core am I
 132    // ----------------
 133         mrs      x0, MPIDR_EL1
 134         and      x0, x0, #0xFF                        //Mask off to leave Aff0
 135         cbz      x0, OKToRun                          //If core 0, run the primary init code
 136 EndlessLoop0:
 137         wfi
 138         b        EndlessLoop0
 139 #endif
 140 OKToRun:
 141
 142         /*Set vector table base address*/
 143         ldr     x1, =vector_base
 144         msr     VBAR_EL3,x1
 145
 146         /* Set reset vector address */
 147         /* Get the cpu ID */
 148         mrs  x0, MPIDR_EL1
 149         and  x0, x0, #0xFF
 150         mov  w0, w0
 151         ldr      w2, =rvbar_base
 152         /* calculate the rvbar base address for particular CPU core */
 153         mov      w3, #0x8
 154         mul      w0, w0, w3
 155         add      w2, w2, w0
 156         /* store vector base address to RVBAR */
 157         str  x1, [x2]
 158
 159         /*Define stack pointer for current exception level*/
 160         ldr      x2,=EL3_stack
 161         mov      sp,x2
 162
 163         /* Disable trapping of CPTR_EL3 accesses or use of Adv.SIMD/FPU*/
 164         mov      x0, #0                 // Clear all trap bits
 165         msr      CPTR_EL3, x0
 166
 167         /* Configure SCR_EL3 */
 168         mov      w1, #0                 //; Initial value of register is unknown
 169         orr      w1, w1, #(1 << 11)     //; Set ST bit (Secure EL1 can access CNTPS_TVAL_EL1, CNTPS_CTL_EL1 & CNTPS_CVAL_EL1)
 170         orr      w1, w1, #(1 << 10)     //; Set RW bit (EL1 is AArch64, as this is the Secure world)
 171         orr      w1, w1, #(1 << 3)      //; Set EA bit (SError routed to EL3)
 172         orr      w1, w1, #(1 << 2)      //; Set FIQ bit (FIQs routed to EL3)
 173         orr      w1, w1, #(1 << 1)      //; Set IRQ bit (IRQs routed to EL3)
 174         msr      SCR_EL3, x1
 175
 176         /*Enable ECC protection*/
 177         mrs     x0, S3_1_C11_C0_2       // register L2CTLR_EL1
 178         orr     x0, x0, #(1<<22)
 179         msr     S3_1_C11_C0_2, x0
 180         /*configure cpu auxiliary control register EL1 */
 181         ldr     x0,=0x80CA000           // L1 Data prefetch control - 5, Enable device split throttle, 2 independent data prefetch streams
 182         msr     S3_1_C15_C2_0, x0       //CPUACTLR_EL1
 183
 184         /* program the counter frequency */
 185         ldr     x0,=counterfreq
 186         msr     CNTFRQ_EL0, x0
 187
 188         /*Enable hardware coherency between cores*/
 189         mrs      x0, S3_1_c15_c2_1      //Read EL1 CPU Extended Control Register
 190         orr      x0, x0, #(1 << 6)      //Set the SMPEN bit
 191         msr      S3_1_c15_c2_1, x0      //Write EL1 CPU Extended Control Register
 192         isb
 193
 194         tlbi    ALLE3
 195         ic      IALLU                   //; Invalidate I cache to PoU
 196         bl      invalidate_dcaches
 197         dsb      sy
 198         isb
 199
 200         ldr      x1, =L0Table           //; Get address of level 0 for TTBR0_EL3
 201         msr      TTBR0_EL3, x1          //; Set TTBR0_EL3
 202
 203         /**********************************************
 204         * Set up memory attributes
 205         * This equates to:
 206         * 0 = b01000100 = Normal, Inner/Outer Non-Cacheable
 207         * 1 = b11111111 = Normal, Inner/Outer WB/WA/RA
 208         * 2 = b00000000 = Device-nGnRnE
 209         * 3 = b00000100 = Device-nGnRE
 210         * 4 = b10111011 = Normal, Inner/Outer WT/WA/RA
 211         **********************************************/
 212         ldr      x1, =0x000000BB0400FF44
 213         msr      MAIR_EL3, x1
 214
 215         /**********************************************
 216          * Set up TCR_EL3
 217          * Physical Address Size PS =  010 -> 40bits 1TB
 218          * Granual Size TG0 = 00 -> 4KB
 219          * size offset of the memory region T0SZ = 24 -> (region size 2^(64-24) = 2^40)
 220          ***************************************************/
 221         ldr     x1,=0x80823518
 222         msr     TCR_EL3, x1
 223         isb
 224
 225         /* Enable SError Exception for asynchronous abort */
 226         mrs     x1,DAIF
 227         bic     x1,x1,#(0x1<<8)
 228         msr     DAIF,x1
 229
 230         /* Configure SCTLR_EL3 */
 231         mov      x1, #0                //Most of the SCTLR_EL3 bits are unknown at reset
 232         orr      x1, x1, #(1 << 12)     //Enable I cache
 233         orr      x1, x1, #(1 << 3)      //Enable SP alignment check
 234         orr      x1, x1, #(1 << 2)      //Enable caches
 235         orr      x1, x1, #(1 << 0)      //Enable MMU
 236         msr      SCTLR_EL3, x1
 237         dsb      sy
 238         isb
 239
 240         b        _startup               //jump to start
 241
 242 loop:   b       loop
 243
 244 invalidate_dcaches:
 245
 246         dmb     ISH
 247         mrs     x0, CLIDR_EL1          //; x0 = CLIDR
 248         ubfx    w2, w0, #24, #3        //; w2 = CLIDR.LoC
 249         cmp     w2, #0                 //; LoC is 0?
 250         b.eq    invalidateCaches_end   //; No cleaning required and enable MMU
 251         mov     w1, #0                 //; w1 = level iterator
 252
 253 invalidateCaches_flush_level:
 254         add     w3, w1, w1, lsl #1     //; w3 = w1 * 3 (right-shift for cache type)
 255         lsr     w3, w0, w3             //; w3 = w0 >> w3
 256         ubfx    w3, w3, #0, #3         //; w3 = cache type of this level
 257         cmp     w3, #2                 //; No cache at this level?
 258         b.lt    invalidateCaches_next_level
 259
 260         lsl     w4, w1, #1
 261         msr     CSSELR_EL1, x4         //; Select current cache level in CSSELR
 262         isb                            //; ISB required to reflect new CSIDR
 263         mrs     x4, CCSIDR_EL1         //; w4 = CSIDR
 264
 265         ubfx    w3, w4, #0, #3
 266         add     w3, w3, #2             //; w3 = log2(line size)
 267         ubfx    w5, w4, #13, #15
 268         ubfx    w4, w4, #3, #10        //; w4 = Way number
 269         clz     w6, w4                 //; w6 = 32 - log2(number of ways)
 270
 271 invalidateCaches_flush_set:
 272         mov     w8, w4                 //; w8 = Way number
 273 invalidateCaches_flush_way:
 274         lsl     w7, w1, #1             //; Fill level field
 275         lsl     w9, w5, w3
 276         orr     w7, w7, w9             //; Fill index field
 277         lsl     w9, w8, w6
 278         orr     w7, w7, w9             //; Fill way field
 279         dc      CISW, x7               //; Invalidate by set/way to point of coherency
 280         subs    w8, w8, #1             //; Decrement way
 281         b.ge    invalidateCaches_flush_way
 282         subs    w5, w5, #1             //; Descrement set
 283         b.ge    invalidateCaches_flush_set
 284
 285 invalidateCaches_next_level:
 286         add     w1, w1, #1             //; Next level
 287         cmp     w2, w1
 288         b.gt    invalidateCaches_flush_level
 289
 290 invalidateCaches_end:
 291         ret
 292
 293 .end