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
  19         /* Using 64 bit alignment since the spin table is accessed as data */
  20         .align 3
  21         .global secondary_boot_code
  22         /* Secondary Boot Code starts here */
  23 secondary_boot_code:
  24         .global __spin_table
  25 __spin_table:
  26         .space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE
  27
  28         .align 2
  29 ENTRY(secondary_boot_func)
  30         /*
  31          * MPIDR_EL1 Fields:
  32          * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
  33          * MPIDR[7:2] = AFF0_RES
  34          * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
  35          * MPIDR[23:16] = AFF2_CLUSTERID
  36          * MPIDR[24] = MT
  37          * MPIDR[29:25] = RES0
  38          * MPIDR[30] = U
  39          * MPIDR[31] = ME
  40          * MPIDR[39:32] = AFF3
  41          *
  42          * Linear Processor ID (LPID) calculation from MPIDR_EL1:
  43          * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
  44          * until AFF2_CLUSTERID and AFF3 have non-zero values)
  45          *
  46          * LPID = MPIDR[15:8] | MPIDR[1:0]
  47          */
  48         mrs     x0, mpidr_el1
  49         ubfm    x1, x0, #8, #15
  50         ubfm    x2, x0, #0, #1
  51         orr     x10, x2, x1, lsl #2     /* x10 has LPID */
  52         ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
  53         /*
  54          * offset of the spin table element for this core from start of spin
  55          * table (each elem is padded to 64 bytes)
  56          */
  57         lsl     x1, x10, #6
  58         adr     x0, __spin_table
  59         /* physical address of this cpus spin table element */
  60         add     x11, x1, x0
  61
  62         adr     x0, __real_cntfrq
  63         ldr     x0, [x0]
  64         msr     cntfrq_el0, x0  /* set with real frequency */
  65         str     x9, [x11, #16]  /* LPID */
  66         mov     x4, #1
  67         str     x4, [x11, #8]   /* STATUS */
  68         dsb     sy
  69
  70 slave_cpu:
  71         wfe
  72         ldr     x0, [x11]
  73         cbz     x0, slave_cpu
  74 #ifndef CONFIG_ARMV8_SWITCH_TO_EL1
  75         mrs     x1, sctlr_el2
  76 #else
  77         mrs     x1, sctlr_el1
  78 #endif
  79         tbz     x1, #25, cpu_is_le
  80         rev     x0, x0                  /* BE to LE conversion */
  81 cpu_is_le:
  82         ldr     x5, [x11, #24]
  83         cbz     x5, 1f
  84
  85 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
  86         adr     x4, secondary_switch_to_el1
  87         ldr     x5, =ES_TO_AARCH64
  88 #else
  89         ldr     x4, [x11]
  90         ldr     x5, =ES_TO_AARCH32
  91 #endif
  92         bl      secondary_switch_to_el2
  93
  94 1:
  95 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
  96         adr     x4, secondary_switch_to_el1
  97 #else
  98         ldr     x4, [x11]
  99 #endif
 100         ldr     x5, =ES_TO_AARCH64
 101         bl      secondary_switch_to_el2
 102
 103 ENDPROC(secondary_boot_func)
 104
 105 ENTRY(secondary_switch_to_el2)
 106         switch_el x6, 1f, 0f, 0f
 107 0:      ret
 108 1:      armv8_switch_to_el2_m x4, x5, x6
 109 ENDPROC(secondary_switch_to_el2)
 110
 111 ENTRY(secondary_switch_to_el1)
 112         mrs     x0, mpidr_el1
 113         ubfm    x1, x0, #8, #15
 114         ubfm    x2, x0, #0, #1
 115         orr     x10, x2, x1, lsl #2     /* x10 has LPID */
 116
 117         lsl     x1, x10, #6
 118         adr     x0, __spin_table
 119         /* physical address of this cpus spin table element */
 120         add     x11, x1, x0
 121
 122         ldr     x4, [x11]
 123
 124         ldr     x5, [x11, #24]
 125         cbz     x5, 2f
 126
 127         ldr     x5, =ES_TO_AARCH32
 128         bl      switch_to_el1
 129
 130 2:      ldr     x5, =ES_TO_AARCH64
 131
 132 switch_to_el1:
 133         switch_el x6, 0f, 1f, 0f
 134 0:      ret
 135 1:      armv8_switch_to_el1_m x4, x5, x6
 136 ENDPROC(secondary_switch_to_el1)
 137
 138         /* Ensure that the literals used by the secondary boot code are
 139          * assembled within it (this is required so that we can protect
 140          * this area with a single memreserve region
 141          */
 142         .ltorg
 143
 144         /* 64 bit alignment for elements accessed as data */
 145         .align 3
 146         .global __real_cntfrq
 147 __real_cntfrq:
 148         .quad COUNTER_FREQUENCY
 149         .globl __secondary_boot_code_size
 150         .type __secondary_boot_code_size, %object
 151         /* Secondary Boot Code ends here */
 152 __secondary_boot_code_size:
 153         .quad .-secondary_boot_code