arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S

   1 /*
   2  * (C) Copyright 2014 Freescale Semiconductor
   3  *
   4  * SPDX-License-Identifier:     GPL-2.0+
   5  *
   6  * Extracted from armv8/start.S
   7  */
   8
   9 #include <config.h>
  10 #include <linux/linkage.h>
  11 #include <asm/gic.h>
  12 #include <asm/macro.h>
  13 #include "mp.h"
  14
  15 ENTRY(lowlevel_init)
  16         mov     x29, lr                 /* Save LR */
  17
  18         /* Set the SMMU page size in the sACR register */
  19         ldr     x1, =SMMU_BASE
  20         ldr     w0, [x1, #0x10]
  21         orr     w0, w0, #1 << 16  /* set sACR.pagesize to indicate 64K page */
  22         str     w0, [x1, #0x10]
  23
  24         /* Initialize GIC Secure Bank Status */
  25 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
  26         branch_if_slave x0, 1f
  27         ldr     x0, =GICD_BASE
  28         bl      gic_init_secure
  29 1:
  30 #ifdef CONFIG_GICV3
  31         ldr     x0, =GICR_BASE
  32         bl      gic_init_secure_percpu
  33 #elif defined(CONFIG_GICV2)
  34         ldr     x0, =GICD_BASE
  35         ldr     x1, =GICC_BASE
  36         bl      gic_init_secure_percpu
  37 #endif
  38 #endif
  39
  40         branch_if_master x0, x1, 2f
  41
  42         ldr     x0, =secondary_boot_func
  43         blr     x0
  44 2:
  45         mov     lr, x29                 /* Restore LR */
  46         ret
  47 ENDPROC(lowlevel_init)
  48
  49         /* Keep literals not used by the secondary boot code outside it */
  50         .ltorg
  51
  52         /* Using 64 bit alignment since the spin table is accessed as data */
  53         .align 4
  54         .global secondary_boot_code
  55         /* Secondary Boot Code starts here */
  56 secondary_boot_code:
  57         .global __spin_table
  58 __spin_table:
  59         .space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE
  60
  61         .align 2
  62 ENTRY(secondary_boot_func)
  63         /*
  64          * MPIDR_EL1 Fields:
  65          * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
  66          * MPIDR[7:2] = AFF0_RES
  67          * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
  68          * MPIDR[23:16] = AFF2_CLUSTERID
  69          * MPIDR[24] = MT
  70          * MPIDR[29:25] = RES0
  71          * MPIDR[30] = U
  72          * MPIDR[31] = ME
  73          * MPIDR[39:32] = AFF3
  74          *
  75          * Linear Processor ID (LPID) calculation from MPIDR_EL1:
  76          * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
  77          * until AFF2_CLUSTERID and AFF3 have non-zero values)
  78          *
  79          * LPID = MPIDR[15:8] | MPIDR[1:0]
  80          */
  81         mrs     x0, mpidr_el1
  82         ubfm    x1, x0, #8, #15
  83         ubfm    x2, x0, #0, #1
  84         orr     x10, x2, x1, lsl #2     /* x10 has LPID */
  85         ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
  86         /*
  87          * offset of the spin table element for this core from start of spin
  88          * table (each elem is padded to 64 bytes)
  89          */
  90         lsl     x1, x10, #6
  91         ldr     x0, =__spin_table
  92         /* physical address of this cpus spin table element */
  93         add     x11, x1, x0
  94
  95         str     x9, [x11, #16]  /* LPID */
  96         mov     x4, #1
  97         str     x4, [x11, #8]   /* STATUS */
  98         dsb     sy
  99 #if defined(CONFIG_GICV3)
 100         gic_wait_for_interrupt_m x0
 101 #elif defined(CONFIG_GICV2)
 102         ldr     x0, =GICC_BASE
 103         gic_wait_for_interrupt_m x0, w1
 104 #endif
 105
 106         bl secondary_switch_to_el2
 107 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
 108         bl secondary_switch_to_el1
 109 #endif
 110
 111 slave_cpu:
 112         wfe
 113         ldr     x0, [x11]
 114         cbz     x0, slave_cpu
 115 #ifndef CONFIG_ARMV8_SWITCH_TO_EL1
 116         mrs     x1, sctlr_el2
 117 #else
 118         mrs     x1, sctlr_el1
 119 #endif
 120         tbz     x1, #25, cpu_is_le
 121         rev     x0, x0                  /* BE to LE conversion */
 122 cpu_is_le:
 123         br      x0                      /* branch to the given address */
 124 ENDPROC(secondary_boot_func)
 125
 126 ENTRY(secondary_switch_to_el2)
 127         switch_el x0, 1f, 0f, 0f
 128 0:      ret
 129 1:      armv8_switch_to_el2_m x0
 130 ENDPROC(secondary_switch_to_el2)
 131
 132 ENTRY(secondary_switch_to_el1)
 133         switch_el x0, 0f, 1f, 0f
 134 0:      ret
 135 1:      armv8_switch_to_el1_m x0, x1
 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 4
 146         .globl __secondary_boot_code_size
 147         .type __secondary_boot_code_size, %object
 148         /* Secondary Boot Code ends here */
 149 __secondary_boot_code_size:
 150         .quad .-secondary_boot_code