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 ENTRY(__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 slave_cpu:
  76         wfe
  77         ldr     x0, [x11]
  78         cbz     x0, slave_cpu
  79 #ifndef CONFIG_ARMV8_SWITCH_TO_EL1
  80         mrs     x1, sctlr_el2
  81 #else
  82         mrs     x1, sctlr_el1
  83 #endif
  84         tbz     x1, #25, cpu_is_le
  85         rev     x0, x0                  /* BE to LE conversion */
  86 cpu_is_le:
  87         ldr     x5, [x11, #24]
  88         cbz     x5, 1f
  89
  90 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
  91         adr     x4, secondary_switch_to_el1
  92         ldr     x5, =ES_TO_AARCH64
  93 #else
  94         ldr     x4, [x11]
  95         ldr     x5, =ES_TO_AARCH32
  96 #endif
  97         bl      secondary_switch_to_el2
  98
  99 1:
 100 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
 101         adr     x4, secondary_switch_to_el1
 102 #else
 103         ldr     x4, [x11]
 104 #endif
 105         ldr     x5, =ES_TO_AARCH64
 106         bl      secondary_switch_to_el2
 107
 108 ENDPROC(__secondary_boot_func)
 109
 110 ENTRY(secondary_switch_to_el2)
 111         switch_el x6, 1f, 0f, 0f
 112 0:      ret
 113 1:      armv8_switch_to_el2_m x4, x5, x6
 114 ENDPROC(secondary_switch_to_el2)
 115
 116 ENTRY(secondary_switch_to_el1)
 117         mrs     x0, mpidr_el1
 118         ubfm    x1, x0, #8, #15
 119         ubfm    x2, x0, #0, #1
 120         orr     x10, x2, x1, lsl #2     /* x10 has LPID */
 121
 122         lsl     x1, x10, #6
 123         adr     x0, __spin_table
 124         /* physical address of this cpus spin table element */
 125         add     x11, x1, x0
 126
 127         ldr     x4, [x11]
 128
 129         ldr     x5, [x11, #24]
 130         cbz     x5, 2f
 131
 132         ldr     x5, =ES_TO_AARCH32
 133         bl      switch_to_el1
 134
 135 2:      ldr     x5, =ES_TO_AARCH64
 136
 137 switch_to_el1:
 138         switch_el x6, 0f, 1f, 0f
 139 0:      ret
 140 1:      armv8_switch_to_el1_m x4, x5, x6
 141 ENDPROC(secondary_switch_to_el1)
 142
 143         /* Ensure that the literals used by the secondary boot code are
 144          * assembled within it (this is required so that we can protect
 145          * this area with a single memreserve region
 146          */
 147         .ltorg
 148
 149         /* 64 bit alignment for elements accessed as data */
 150         .align 3
 151         .global __real_cntfrq
 152 __real_cntfrq:
 153         .quad COUNTER_FREQUENCY
 154         /* Secondary Boot Code ends here */
 155 __secondary_boot_code_end: