arch/arm/cpu/armv8/fsl-layerscape/spintable.S

   1 /* SPDX-License-Identifier: GPL-2.0+ */
   2 /*
   3  * (C) Copyright 2014-2015 Freescale Semiconductor
   4  * Copyright 2019 NXP
   5  */
   6
   7 #include <config.h>
   8 #include <linux/linkage.h>
   9 #include <asm/macro.h>
  10 #include <asm/system.h>
  11 #include <asm/arch/mp.h>
  12
  13 .align 3
  14 .global secondary_boot_addr
  15 secondary_boot_addr:
  16         .quad __secondary_boot_func
  17
  18 .global secondary_boot_code_start
  19 secondary_boot_code_start:
  20         .quad __secondary_boot_code_start
  21
  22 .global secondary_boot_code_size
  23 secondary_boot_code_size:
  24         .quad __secondary_boot_code_end - __secondary_boot_code_start
  25
  26         /* Using 64 bit alignment since the spin table is accessed as data */
  27         .align 3
  28         /* Secondary Boot Code starts here */
  29 __secondary_boot_code_start:
  30 __spin_table:
  31         .space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE
  32
  33         .align 2
  34 __secondary_boot_func:
  35         /*
  36          * MPIDR_EL1 Fields:
  37          * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
  38          * MPIDR[7:2] = AFF0_RES
  39          * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
  40          * MPIDR[23:16] = AFF2_CLUSTERID
  41          * MPIDR[24] = MT
  42          * MPIDR[29:25] = RES0
  43          * MPIDR[30] = U
  44          * MPIDR[31] = ME
  45          * MPIDR[39:32] = AFF3
  46          *
  47          * Linear Processor ID (LPID) calculation from MPIDR_EL1:
  48          * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
  49          * until AFF2_CLUSTERID and AFF3 have non-zero values)
  50          *
  51          * LPID = MPIDR[15:8] | MPIDR[1:0]
  52          */
  53         mrs     x0, mpidr_el1
  54         ubfm    x1, x0, #8, #15
  55         ubfm    x2, x0, #0, #1
  56         orr     x10, x2, x1, lsl #2     /* x10 has LPID */
  57         ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
  58         /*
  59          * offset of the spin table element for this core from start of spin
  60          * table (each elem is padded to 64 bytes)
  61          */
  62         lsl     x1, x10, #6
  63         adr     x0, __spin_table
  64         /* physical address of this cpus spin table element */
  65         add     x11, x1, x0
  66
  67         adr     x0, __real_cntfrq
  68         ldr     x0, [x0]
  69         msr     cntfrq_el0, x0  /* set with real frequency */
  70         str     x9, [x11, #16]  /* LPID */
  71         mov     x4, #1
  72         str     x4, [x11, #8]   /* STATUS */
  73         dsb     sy
  74
  75 1:
  76         wfe
  77         ldr     x4, [x11]
  78         cbz     x4, 1b
  79         mrs     x1, sctlr_el2
  80         tbz     x1, #25, 2f
  81         rev     x4, x4                  /* BE to LE conversion */
  82 2:
  83         ldr     x6, =ES_TO_AARCH64
  84 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
  85         adr     x5, 3f
  86         switch_el x7, 0f, _dead_loop, _dead_loop
  87 0:      armv8_switch_to_el2_m x5, x6, x7
  88 #endif
  89 3:
  90         ldr     x7, [x11, #24]  /* ARCH_COMP */
  91         cbz     x7, 4f
  92         ldr     x6, =ES_TO_AARCH32
  93 4:
  94 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
  95         switch_el x7, _dead_loop, 0f, _dead_loop
  96 0:      armv8_switch_to_el1_m x4, x6, x7
  97 #else
  98         switch_el x7, 0f, _dead_loop, _dead_loop
  99 0:      armv8_switch_to_el2_m x4, x6, x7
 100 #endif
 101
 102 _dead_loop:
 103         wfe
 104         b _dead_loop
 105
 106         /* Ensure that the literals used by the secondary boot code are
 107          * assembled within it (this is required so that we can protect
 108          * this area with a single memreserve region
 109          */
 110         .ltorg
 111
 112         /* 64 bit alignment for elements accessed as data */
 113         .align 3
 114         .global __real_cntfrq
 115 __real_cntfrq:
 116         .quad COUNTER_FREQUENCY
 117         /* Secondary Boot Code ends here */
 118 __secondary_boot_code_end: