Board: Fix for am64x diagnostic test failure on mpu core
[processor-sdk/pdk.git] / packages / ti / board / diag / common / am64x / diag_entry.asm
1 /*\r
2  * Copyright (c) 2017-2018, Texas Instruments Incorporated\r
3  * All rights reserved.\r
4  *\r
5  * Redistribution and use in source and binary forms, with or without\r
6  * modification, are permitted provided that the following conditions\r
7  * are met:\r
8  *\r
9  * *  Redistributions of source code must retain the above copyright\r
10  *    notice, this list of conditions and the following disclaimer.\r
11  *\r
12  * *  Redistributions in binary form must reproduce the above copyright\r
13  *    notice, this list of conditions and the following disclaimer in the\r
14  *    documentation and/or other materials provided with the distribution.\r
15  *\r
16  * *  Neither the name of Texas Instruments Incorporated nor the names of\r
17  *    its contributors may be used to endorse or promote products derived\r
18  *    from this software without specific prior written permission.\r
19  *\r
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"\r
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,\r
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\r
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR\r
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,\r
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,\r
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;\r
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,\r
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR\r
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,\r
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
31  */\r
32 #include <ti/csl/arch/a53/src/startup/aarch64/boot_defs.h>\r
33 \r
34 //************************** Global symbols ************************************\r
35         .global Entry\r
36         .global _stack\r
37         .global _bss_start\r
38         .global _bss_end\r
39         .global start_boot\r
40         .global main\r
41         .global ti_csl_arm_gicv3_vectors\r
42 \r
43 .macro GICD_WRITE_LOOP x, y, offset\r
44     str     w1, [x0, #\offset]\r
45     .if \y-\x\r
46     GICD_WRITE_LOOP "(\x+1)", \y, "(\offset+4)"\r
47     .endif\r
48 .endm\r
49 \r
50 .macro INIT_GICD_IGROUPR\r
51     ldr     x0, =CSL_GIC_BASE_ADDR\r
52     mvn     w1, wzr\r
53     GICD_WRITE_LOOP 0, 31, 0x0080\r
54 .endm\r
55 \r
56 .macro INIT_GICD_IGRPMODR\r
57     ldr     x0, =CSL_GIC_BASE_ADDR\r
58     mov     w1, wzr\r
59     GICD_WRITE_LOOP 0, 31, 0x0D00\r
60 .endm\r
61 \r
62 \r
63     .section .text.csl_a53_startup, "ax"\r
64     .func Entry\r
65 Entry:\r
66     mov     x21, sp   /* Store the SP in x21 */\r
67     mov     x22, x30  /* Store the LR in x22 */\r
68     mrs     x0, currentel\r
69     cmp     x0, #0xC\r
70     bne     2f\r
71     ldr     x0, =ti_csl_arm_v8a_Core_disableCaches\r
72     blr     x0\r
73 \r
74     /*\r
75      * ----------------------\r
76      * Switch from EL3 to EL2\r
77      * ----------------------\r
78      */\r
79     mov     x0, #0x0531\r
80     msr     scr_el3, x0\r
81     msr     cptr_el3, xzr       /* Disable all trap bits */\r
82     mov     x0, #0x33ff\r
83     msr     cptr_el2, x0        /* Disable all trap bits */\r
84     ldr     x0, =CSL_GIC_BASE_ADDR\r
85     mov     w1, #0x37\r
86     str     w1, [x0]            /* Enable GIC */\r
87     mov     x0, #0xf\r
88     msr     s3_6_c12_c12_5, x0     /* Enable GIC system register interface\r
89                                    and disable IRQ/FIQ bypass */\r
90     isb\r
91     mov     x0, #0x1\r
92     msr     s3_6_c12_c12_7, x0 /* Enable Non-secure group 1 interrupts */\r
93     isb\r
94     INIT_GICD_IGROUPR\r
95     INIT_GICD_IGRPMODR\r
96     ldr     x0, =CSL_GICR_BASE_ADDR\r
97     ldr     x1, =ti_csl_arm_v8a_Core_getGicxAddr\r
98     blr     x1\r
99     ldr     w1, [x0, #0x14]\r
100     mov     w2, #0x2\r
101     bic     w1, w1, w2\r
102     str     w1, [x0, #0x14]     /* wakeup GICR */\r
103 1:  ldr     w1, [x0, #0x14]\r
104     and     w1, w1, #0x4\r
105     cbnz    w1, 1b\r
106     ldr     x0, =CSL_GICS_BASE_ADDR\r
107     ldr     x1, =ti_csl_arm_v8a_Core_getGicxAddr\r
108     blr     x1\r
109     mvn     x1, xzr\r
110     str     w1, [x0, #0x80]     /* Configure all SGIs & PPIs as Group 1 ints */\r
111     str     wzr, [x0, #0xD00]   /* Clear GICR_IGRPMODR0 */\r
112     mov     x0, #0xff\r
113     msr     icc_pmr_el1, x0     /* Set priority mask */\r
114     mov     x0, #0x0830\r
115     movk    x0, #0x30c5, lsl #16\r
116     msr     sctlr_el2, x0       /* Initialize sctlr_el2 to reset values */\r
117     mrs     x0, actlr_el3\r
118     orr     x0, x0, #0x2\r
119     msr     actlr_el3, x0       /* Enable CPUECTLR_EL1 access from EL2 */\r
120     /* TODO setup vbar */\r
121     ldr     x0, =CSL_GTC_CNTCR_ADDR\r
122     ldr     w1, [x0]\r
123     orr     w1, w1, #0x1\r
124     str     w1, [x0]            /* Enable system counter */\r
125     isb\r
126     bl      switch_to_el2\r
127 \r
128 2:\r
129     mrs     x0, currentel       /* Read again as currentEL may have changed */\r
130     cmp     x0, #0x8\r
131     bne     3f\r
132 \r
133     /*\r
134      * ----------------------\r
135      * Switch from EL2 to EL1\r
136      * ----------------------\r
137      */\r
138     mrs     x0, cnthctl_el2\r
139     orr     x0, x0, #0x3        /* Enable EL1/EL0 access to timers */\r
140     msr     cnthctl_el2, x0\r
141     msr     cntvoff_el2, xzr\r
142     mov     x0, #0x33ff\r
143     msr     cptr_el2, x0        /* Disable all trap bits */\r
144     msr     hstr_el2, xzr       /* Disable all coprocessor trap bits */\r
145     mov     x0, #0x0002         /* 64bit EL1, Disable hypervisor call (HVC)\r
146                                    instruction, Set/Way Invalidation Override */\r
147     movk    x0, #0xA000, lsl #16\r
148     msr     hcr_el2, x0\r
149     mov     x0, #0x0838\r
150     movk    x0, #0x30d0, lsl #16\r
151     msr     sctlr_el1, x0\r
152     mrs     x0, actlr_el2\r
153     orr     x0, x0, #2\r
154     msr     actlr_el2, x0\r
155     isb\r
156     mov     x0, #0xF\r
157     msr     icc_sre_el2, x0     /* Enable GIC system register interface\r
158                                    and disable IRQ/FIQ bypass */\r
159     bl      switch_to_el1\r
160 3:\r
161     ldr     x0, =gnu_targets_arm_rtsv8A_startupAsm\r
162     br      x0\r
163 \r
164 switch_to_el1:\r
165     mov     x0, #0x3c5\r
166     msr     spsr_el2, x0\r
167     msr     elr_el2, x30\r
168     eret\r
169 \r
170 switch_to_el2:\r
171     mov     x0, #0x3c9\r
172     msr     spsr_el3, x0\r
173     msr     elr_el3, x30\r
174     eret\r
175 \r
176     .endfunc\r
177 \r
178 /*\r
179  *  ======== Core_getGicxBaseAddr ========\r
180  *\r
181  *  Ptr Core_getGicxAddr(Ptr gicxBaseAddr)\r
182  */\r
183     .section .text.csl_startup, "ax"\r
184     .func ti_csl_arm_v8a_Core_getGicxAddr\r
185 \r
186 ti_csl_arm_v8a_Core_getGicxAddr:\r
187     ldr     x1, =CSL_GICX_OFFSET\r
188     mrs     x2, mpidr_el1\r
189     ubfx    x3, x2, #8, #8      /* x3 = Cluster Id */\r
190     and     x2, x2, #0xFF       /* x2 = Core Id */\r
191     sub     x3, x3, #0\r
192     mrs     x4, s3_1_c11_c0_2   /* Read L2CTLR_EL1 */\r
193     ubfx    x4, x4, #24, #2     /* x4 = Number of cores per cluster */\r
194     lsl     x3, x3, x4          /* x3 = clusterIdx * numCoresPerCluster */\r
195     add     x2, x2, x3\r
196     madd    x0, x1, x2, x0\r
197     ret\r
198     .endfunc\r
199 \r
200 \r
201     .section .text.csl_a53_startup, "ax"\r
202     .func gnu_targets_arm_rtsv8A_startupAsm\r
203 gnu_targets_arm_rtsv8A_startupAsm:\r
204     /*\r
205      * ---------------------\r
206      * Boot code starts here\r
207      * ---------------------\r
208      */\r
209 \r
210     mov     x0, #0x3C0\r
211     msr     daif, x0            /* Mask all interrupts */\r
212 \r
213 #if defined(__ARM_FP)\r
214     mov     x0, #(3 << 20)\r
215     msr     cpacr_el1, x0       /* Enable FP/SIMD at EL1 */\r
216 #endif\r
217 \r
218     isb                         /* Synchronize processor context */\r
219 \r
220     /*\r
221      * ------------------------\r
222      * Initialize stack pointer\r
223      * ------------------------\r
224      */\r
225     msr     spsel, #1           /* Use SP_ELx for ELx */\r
226     ldr     x0, =__TI_STACK_BASE\r
227     ldr     x1, =__TI_STACK_SIZE\r
228     add     x0, x0, x1\r
229     and     x0, x0, #(~0xf)     /* 16-byte align SP */\r
230     mov     sp, x0              /* Initialize SP */\r
231 \r
232     /* Setup the vector table for all levels */\r
233     ldr     x0, =ti_csl_arm_gicv3_vectors\r
234     msr     vbar_el1, x0         /* Set vector table base address */\r
235 \r
236     stp     x21, x22, [sp, #-16]!  /* save SP and LR to stack */\r
237 \r
238     /* do more initialization in C, go to main() */\r
239     bl      start_boot\r
240     mov     x20, x0\r
241     ldp     x0, x1, [sp], #16  /* Load SP and LR to stack */\r
242     mov     sp, x0\r
243     mov     x30, x1\r
244     ret\r
245     .endfunc\r
246 \r
247 /*\r
248  * ======== Core_disableCaches ========\r
249  */\r
250         .section .text.ti_csl_arm_v8a_Core_disableCaches\r
251         .func ti_csl_arm_v8a_Core_disableCaches\r
252 \r
253 ti_csl_arm_v8a_Core_disableCaches:\r
254     mrs     x0, clidr_el1\r
255     and     w3, w0, #0x07000000     /* Get 2 x Level of Coherence */\r
256     lsr     w3, w3, #23\r
257     cbz     w3, 5f\r
258     mov     w10, #0                 /* w10 = 2 x cache level */\r
259     mov     w8, #1                  /* w8 = Constant 0b1 */\r
260 1:\r
261     add     w2, w10, w10, lsr #1    /* Caclulate 3x cache level */\r
262     lsr     w1, w0, w2              /* Extract cache type for this level */\r
263     and     w1, w1, #0x7\r
264     cmp     w1, #2\r
265     blt     4f                      /* No data or unified cache */\r
266     msr     csselr_el1, x10         /* Select this cache level */\r
267     isb                             /* Synchronize change of csselr */\r
268     mrs     x1, ccsidr_el1          /* Read ccsidr */\r
269     and     w2, w1, #7              /* w2 = log2(linelen)-4 */\r
270     add     w2, w2, #4              /* w2 = log2(linelen) */\r
271     ubfx    w4, w1, #3, #10         /* w4 = max way number, right aligned */\r
272     clz     w5, w4                  /* w5 = 32-log2(ways), bit position of\r
273                                        way in dc operand */\r
274     lsl     w9, w4, w5              /* w9 = max way number, aligned to\r
275                                        position in DC operand */\r
276     lsl     w16, w8, w5             /* w16 = amount to decrement way number\r
277                                        per iteration */\r
278 2:\r
279     ubfx    w7, w1, #13, #15        /* w7 = max set number, right aligned */\r
280     lsl     w7, w7, w2              /* w7 = max set number, aligned to\r
281                                        position in DC operand */\r
282     lsl     w17, w8, w2             /* w17 = amount to decrement set number\r
283                                            per iteration */\r
284 3:\r
285     orr     w11, w10, w9            /* w11 = combine way num & cache ...*/\r
286     orr     w11, w11, w7            /* ... num and set num for DC operand */\r
287     dc      cisw, x11               /* Do data cache clean and invalidate\r
288                                        by set and way */\r
289     subs    w7, w7, w17             /* Decrement set number */\r
290     bge     3b\r
291     subs    x9, x9, x16             /* Decrement way number */\r
292     bge     2b\r
293 4:\r
294     add     w10, w10, #2            /* Increment 2 x cache level */\r
295     cmp     w3, w10\r
296     dsb     sy                      /* Ensure completion of previous cache\r
297                                        maintenance operation */\r
298     bgt     1b\r
299 5:\r
300     mrs     x0, sctlr_el3            /* read SCTLR_EL3 */\r
301     bic     x0, x0, #0x0004          /* clear C bit */\r
302     msr     sctlr_el3, x0            /* DCache disabled */\r
303 \r
304     mrs     x0, sctlr_el3            /* read SCTLR_EL3 */\r
305     bic     x0, x0, #0x1000          /* clear I bit */\r
306     msr     sctlr_el3, x0            /* ICache disabled */\r
307     ic      iallu                    /* invalidate all ICache */\r
308     dsb     sy\r
309     isb\r
310 \r
311     ret\r
312     .endfunc\r
313 \r
314     .end\r