[U-Boot] [Patch v1 2/4] armv8/fsl-lsch3: Release secondary cores from boot hold off with Boot Page
York Sun
yorksun at freescale.com
Fri Jun 27 18:54:08 CEST 2014
Secondary cores need to be released from holdoff by boot release
registers. With GPP bootrom, they can boot from main memory
directly. Individual spin table is used for each core. If a single
release address is needed, defining macro CONFIG_FSL_SMP_RELEASE_ALL
will use the CPU_RELEASE_ADDR. Spin table and the boot page is reserved
in device tree so OS won't overwrite.
Signed-off-by: York Sun <yorksun at freescale.com>
Signed-off-by: Arnab Basu <arnab.basu at freescale.com>
---
This set depends on this bundle http://patchwork.ozlabs.org/bundle/yorksun/armv8_fsl-lsch3/
arch/arm/cpu/armv8/fsl-lsch3/Makefile | 2 +
arch/arm/cpu/armv8/fsl-lsch3/cpu.c | 13 ++
arch/arm/cpu/armv8/fsl-lsch3/cpu.h | 1 +
arch/arm/cpu/armv8/fsl-lsch3/fdt.c | 56 +++++++
arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S | 119 +++++++++++---
arch/arm/cpu/armv8/fsl-lsch3/mp.c | 171 +++++++++++++++++++++
arch/arm/cpu/armv8/fsl-lsch3/mp.h | 36 +++++
arch/arm/cpu/armv8/transition.S | 63 +-------
arch/arm/include/asm/arch-fsl-lsch3/config.h | 3 +-
arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h | 35 +++++
arch/arm/include/asm/macro.h | 81 ++++++++++
arch/arm/lib/gic_64.S | 10 +-
common/board_f.c | 2 +-
13 files changed, 502 insertions(+), 90 deletions(-)
create mode 100644 arch/arm/cpu/armv8/fsl-lsch3/fdt.c
create mode 100644 arch/arm/cpu/armv8/fsl-lsch3/mp.c
create mode 100644 arch/arm/cpu/armv8/fsl-lsch3/mp.h
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/Makefile b/arch/arm/cpu/armv8/fsl-lsch3/Makefile
index 9249537..f920eeb 100644
--- a/arch/arm/cpu/armv8/fsl-lsch3/Makefile
+++ b/arch/arm/cpu/armv8/fsl-lsch3/Makefile
@@ -7,3 +7,5 @@
obj-y += cpu.o
obj-y += lowlevel.o
obj-y += speed.o
+obj-$(CONFIG_MP) += mp.o
+obj-$(CONFIG_OF_LIBFDT) += fdt.o
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/cpu.c b/arch/arm/cpu/armv8/fsl-lsch3/cpu.c
index c129d03..47b947f 100644
--- a/arch/arm/cpu/armv8/fsl-lsch3/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-lsch3/cpu.c
@@ -11,6 +11,7 @@
#include <asm/io.h>
#include <asm/arch-fsl-lsch3/immap_lsch3.h>
#include "cpu.h"
+#include "mp.h"
#include "speed.h"
#include <fsl_mc.h>
@@ -434,3 +435,15 @@ int cpu_eth_init(bd_t *bis)
#endif
return error;
}
+
+
+int arch_early_init_r(void)
+{
+ int rv;
+ rv = fsl_lsch3_wake_seconday_cores();
+
+ if (rv)
+ printf("Did not wake secondary cores\n");
+
+ return 0;
+}
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/cpu.h b/arch/arm/cpu/armv8/fsl-lsch3/cpu.h
index 28544d7..2e3312b 100644
--- a/arch/arm/cpu/armv8/fsl-lsch3/cpu.h
+++ b/arch/arm/cpu/armv8/fsl-lsch3/cpu.h
@@ -5,3 +5,4 @@
*/
int fsl_qoriq_core_to_cluster(unsigned int core);
+u32 cpu_mask(void);
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/fdt.c b/arch/arm/cpu/armv8/fsl-lsch3/fdt.c
new file mode 100644
index 0000000..cd34e16
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-lsch3/fdt.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <libfdt.h>
+#include <fdt_support.h>
+#include "mp.h"
+
+#ifdef CONFIG_MP
+void ft_fixup_cpu(void *blob)
+{
+ int off;
+ __maybe_unused u64 spin_tbl_addr = (u64)get_spin_tbl_addr();
+ u64 *reg;
+ u64 val;
+
+ off = fdt_node_offset_by_prop_value(blob, -1, "device_type", "cpu", 4);
+ while (off != -FDT_ERR_NOTFOUND) {
+ reg = (u64 *)fdt_getprop(blob, off, "reg", 0);
+ if (reg) {
+ val = spin_tbl_addr;
+#ifndef CONFIG_FSL_SMP_RELEASE_ALL
+ val += id_to_core(fdt64_to_cpu(*reg)) * SIZE_BOOT_ENTRY;
+#endif
+ val = cpu_to_fdt64(val);
+ fdt_setprop_string(blob, off, "enable-method",
+ "spin-table");
+ fdt_setprop(blob, off, "cpu-release-addr",
+ &val, sizeof(val));
+ } else {
+ puts("cpu NULL\n");
+ }
+ off = fdt_node_offset_by_prop_value(blob, off, "device_type",
+ "cpu", 4);
+ }
+ /*
+ * Boot page and spin table can be reserved here if not done staticlly
+ * in device tree.
+ *
+ * fdt_add_mem_rsv(blob, bootpg,
+ * *((u64 *)&(__secondary_boot_page_size)));
+ * If defined CONFIG_FSL_SMP_RELEASE_ALL, the release address should
+ * also be reserved.
+ */
+}
+#endif
+
+void ft_cpu_setup(void *blob, bd_t *bd)
+{
+#ifdef CONFIG_MP
+ ft_fixup_cpu(blob);
+#endif
+}
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S b/arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S
index b4720ae..162d3d6 100644
--- a/arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S
+++ b/arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S
@@ -8,7 +8,9 @@
#include <config.h>
#include <linux/linkage.h>
+#include <asm/gic.h>
#include <asm/macro.h>
+#include "mp.h"
ENTRY(lowlevel_init)
mov x29, lr /* Save LR */
@@ -37,26 +39,12 @@ ENTRY(lowlevel_init)
branch_if_master x0, x1, 1f
- /*
- * Slave should wait for master clearing spin table.
- * This sync prevent salves observing incorrect
- * value of spin table and jumping to wrong place.
- */
-#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
-#ifdef CONFIG_GICV2
- ldr x0, =GICC_BASE
-#endif
- bl gic_wait_for_interrupt
-#endif
-
- /*
- * All processors will enter EL2 and optionally EL1.
- */
- bl armv8_switch_to_el2
-#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
- bl armv8_switch_to_el1
-#endif
- b 2f
+ ldr x0, =(SECONDARY_CPU_BOOT_PAGE)
+ ldr x1, =secondary_boot_func
+ ldr x2, =secondary_boot_page
+ sub x1, x1, x2
+ add x0, x0, x1
+ blr x0
1:
/* Set Non Secure access for all devices protected via TZPC */
@@ -119,3 +107,94 @@ ENTRY(lowlevel_init)
mov lr, x29 /* Restore LR */
ret
ENDPROC(lowlevel_init)
+
+ /* Keep literals not used by the secondary boot page outside it */
+ .ltorg
+
+ .align 4
+ .global secondary_boot_page
+secondary_boot_page:
+ .global __spin_table
+__spin_table:
+ .space CONFIG_MAX_CPUS*ENTRY_SIZE
+
+ .align 4
+ /* Secondary Boot Page starts here */
+ENTRY(secondary_boot_func)
+ /*
+ * PIR calculation from MPIDR_EL1
+ * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
+ * MPIDR[7:2] = AFF0_RES
+ * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
+ * MPIDR[23:16] = AFF2_CLUSTERID
+ * MPIDR[24] = MT
+ * MPIDR[29:25] =RES
+ * MPIDR[30] = U
+ * MPIDR[31] = ME
+ * MPIDR[39:32] = AFF3
+ * We only use AFF0_CPUID and AFF1_CLUSTERID for now
+ * until AFF2_CLUSTERID and AFF3 have non-zero values.
+ */
+ mrs x0, mpidr_el1
+ ubfm x1, x0, #8, #15
+ ubfm x2, x0, #0, #1
+ orr x10, x2, x1, lsl #2 /* x10 has PIR */
+ ubfm x9, x0, #0, #15 /* w9 has 16-bit original PIR */
+ lsl x1, x10, #6 /* spin table is padded to 64 byte each core */
+ ldr x0, =(SECONDARY_CPU_BOOT_PAGE)
+ ldr x3, =__spin_table
+ ldr x4, =secondary_boot_page
+ sub x3, x3, x4
+ add x0, x0, x3
+ add x11, x1, x0
+
+ str x9, [x11, #16] /* ENTRY_PIR */
+ mov x4, #1
+ str x4, [x11] /* ENTRY_ADDR */
+ dsb sy
+ isb
+#if defined(CONFIG_GICV3)
+ gic_wait_for_interrupt_m x0
+#endif
+
+ bl secondary_switch_to_el2
+#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+ secondary_switch_to_el1
+#endif
+
+slave_cpu:
+ wfe
+#ifdef CONFIG_FSL_SMP_RELEASE_ALL
+ ldr x1, =CPU_RELEASE_ADDR
+ ldr x0, [x1]
+#else
+ ldr x0, [x11]
+ tbnz x0, #0, slave_cpu
+#endif
+ cbz x0, slave_cpu
+ br x0 /* branch to the given address */
+ENDPROC(secondary_boot_func)
+
+ENTRY(secondary_switch_to_el2)
+ switch_el x0, 1f, 0f, 0f
+0: ret
+1: armv8_switch_to_el2_m x0
+ENDPROC(secondary_switch_to_el2)
+
+ENTRY(secondary_switch_to_el1)
+ switch_el x0, 0f, 1f, 0f
+0: ret
+1: armv8_switch_to_el1_m x0, x1
+ENDPROC(secondary_switch_to_el1)
+
+ /* Ensure that the literals used by the secondary boot page are
+ * assembled within it
+ */
+ .ltorg
+
+ .align 4
+ .globl __secondary_boot_page_size
+ .type __secondary_boot_page_size, %object
+ /* Secondary Boot Page ends here */
+__secondary_boot_page_size:
+ .quad .-secondary_boot_page
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/mp.c b/arch/arm/cpu/armv8/fsl-lsch3/mp.c
new file mode 100644
index 0000000..1cd36ab
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-lsch3/mp.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/io.h>
+#include <asm/arch-fsl-lsch3/immap_lsch3.h>
+#include "mp.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+void *get_spin_tbl_addr(void)
+{
+ void *ptr = (void *)SECONDARY_CPU_BOOT_PAGE;
+
+ /*
+ * Spin table is at the beginning of secondary boot page. It is
+ * copied to SECONDARY_CPU_BOOT_PAGE.
+ */
+ ptr += (u64)&__spin_table - (u64)&secondary_boot_page;
+
+ return ptr;
+}
+
+phys_addr_t determine_mp_bootpg(void)
+{
+ return (phys_addr_t)SECONDARY_CPU_BOOT_PAGE;
+}
+
+int fsl_lsch3_wake_seconday_cores(void)
+{
+ struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+ struct ccsr_reset __iomem *rst = (void *)(CONFIG_SYS_FSL_RST_ADDR);
+ void *boot_loc = (void *)SECONDARY_CPU_BOOT_PAGE;
+ size_t *boot_page_size = &(__secondary_boot_page_size);
+ u32 cores, cpu_up_mask = 1;
+ int i, timeout = 10;
+ u64 *table = get_spin_tbl_addr();
+
+ cores = cpu_mask();
+ memcpy(boot_loc, &secondary_boot_page, *boot_page_size);
+ /* Clear spin table so that secondary processors
+ * observe the correct value after waking up from wfe.
+ */
+ memset(table, 0, CONFIG_MAX_CPUS*ENTRY_SIZE);
+ flush_dcache_range((unsigned long)boot_loc,
+ (unsigned long)boot_loc + *boot_page_size);
+
+ printf("Waking secondary cores to start from %lx\n", gd->relocaddr);
+ out_le32(&gur->bootlocptrh, (u32)(gd->relocaddr >> 32));
+ out_le32(&gur->bootlocptrl, (u32)gd->relocaddr);
+ out_le32(&gur->scratchrw[6], 1);
+ asm volatile("dsb st" : : : "memory");
+ rst->brrl = cores;
+ asm volatile("dsb st" : : : "memory");
+
+ /* fixme: this is only needed for the simulator because secnodary cores
+ * start to run without waiting for boot release register, then enter
+ * "wfe" before the scratch register is set above.
+ */
+ asm volatile("sev");
+
+ while (timeout--) {
+ flush_dcache_range((unsigned long)table, (unsigned long)table +
+ CONFIG_MAX_CPUS * 64);
+ for (i = 1; i < CONFIG_MAX_CPUS; i++) {
+ if (table[i * NUM_BOOT_ENTRY + BOOT_ENTRY_ADDR])
+ cpu_up_mask |= 1 << i;
+ }
+ if (hweight32(cpu_up_mask) == hweight32(cores))
+ break;
+ udelay(10);
+ }
+ if (timeout <= 0) {
+ printf("Not all cores (0x%x) are up (0x%x)\n",
+ cores, cpu_up_mask);
+ return 1;
+ }
+ printf("All (%d) cores are up.\n", hweight32(cores));
+
+ return 0;
+}
+
+int is_core_valid(unsigned int core)
+{
+ return !!((1 << core) & cpu_mask());
+}
+
+int cpu_reset(int nr)
+{
+ puts("Feature is not implemented.\n");
+
+ return 0;
+}
+
+int cpu_disable(int nr)
+{
+ puts("Feature is not implemented.\n");
+
+ return 0;
+}
+
+int core_to_pos(int nr)
+{
+ u32 cores = cpu_mask();
+ int i, count = 0;
+
+ if (nr == 0) {
+ return 0;
+ } else if (nr >= hweight32(cores)) {
+ puts("Not a valid core number.\n");
+ return -1;
+ }
+
+ for (i = 1; i < 32; i++) {
+ if (is_core_valid(i)) {
+ count++;
+ if (count == nr)
+ break;
+ }
+ }
+
+ return count;
+}
+
+int cpu_status(int nr)
+{
+ u64 *table;
+ int pos;
+
+ if (nr == 0) {
+ table = (u64 *)get_spin_tbl_addr();
+ printf("table base @ 0x%p\n", table);
+ } else {
+ pos = core_to_pos(nr);
+ if (pos < 0)
+ return -1;
+ table = (u64 *)get_spin_tbl_addr() + pos * NUM_BOOT_ENTRY;
+ printf("table @ 0x%p\n", table);
+ printf(" addr - 0x%016llx\n", table[BOOT_ENTRY_ADDR]);
+ printf(" r3 - 0x%016llx\n", table[BOOT_ENTRY_R3]);
+ printf(" pir - 0x%016llx\n", table[BOOT_ENTRY_PIR]);
+ }
+
+ return 0;
+}
+
+int cpu_release(int nr, int argc, char * const argv[])
+{
+ u64 boot_addr;
+ u64 *table = (u64 *)get_spin_tbl_addr();
+#ifndef CONFIG_FSL_SMP_RELEASE_ALL
+ int pos;
+
+ pos = core_to_pos(nr);
+ if (pos <= 0)
+ return -1;
+
+ table += pos * NUM_BOOT_ENTRY;
+#endif
+ boot_addr = simple_strtoull(argv[0], NULL, 16);
+ table[BOOT_ENTRY_ADDR] = boot_addr;
+ asm volatile("dsb st");
+ smp_kick_all_cpus(); /* only those with entry addr set will run */
+
+ return 0;
+}
diff --git a/arch/arm/cpu/armv8/fsl-lsch3/mp.h b/arch/arm/cpu/armv8/fsl-lsch3/mp.h
new file mode 100644
index 0000000..2153b41
--- /dev/null
+++ b/arch/arm/cpu/armv8/fsl-lsch3/mp.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright 2014, Freescale Semiconductor
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#ifndef _FSL_CH3_MP_H
+#define _FSL_CH3_MP_H
+
+/*
+* spin table is defined as
+* struct {
+* uint64_t entry_addr;
+* uint64_t r3;
+* uint64_t pir;
+* };
+* we pad this struct to 64 bytes so each entry is in its own cacheline
+*/
+#define ENTRY_SIZE 64
+#define BOOT_ENTRY_ADDR 0
+#define BOOT_ENTRY_R3 1
+#define BOOT_ENTRY_PIR 2
+#define NUM_BOOT_ENTRY 8 /* pad to 64 bytes */
+#define SIZE_BOOT_ENTRY (NUM_BOOT_ENTRY * sizeof(u64))
+
+#define id_to_core(x) ((x & 3) | (x >> 8))
+#ifndef __ASSEMBLY__
+extern u64 __spin_table[];
+extern u64 *secondary_boot_page;
+extern size_t __secondary_boot_page_size;
+int fsl_lsch3_wake_seconday_cores(void);
+void *get_spin_tbl_addr(void);
+phys_addr_t determine_mp_bootpg(void);
+void secondary_boot_func(void);
+#endif
+#endif /* _FSL_CH3_MP_H */
diff --git a/arch/arm/cpu/armv8/transition.S b/arch/arm/cpu/armv8/transition.S
index e0a5946..ade1cde 100644
--- a/arch/arm/cpu/armv8/transition.S
+++ b/arch/arm/cpu/armv8/transition.S
@@ -14,70 +14,11 @@
ENTRY(armv8_switch_to_el2)
switch_el x0, 1f, 0f, 0f
0: ret
-1:
- mov x0, #0x5b1 /* Non-secure EL0/EL1 | HVC | 64bit EL2 */
- msr scr_el3, x0
- msr cptr_el3, xzr /* Disable coprocessor traps to EL3 */
- mov x0, #0x33ff
- msr cptr_el2, x0 /* Disable coprocessor traps to EL2 */
-
- /* Initialize SCTLR_EL2 */
- msr sctlr_el2, xzr
-
- /* Return to the EL2_SP2 mode from EL3 */
- mov x0, sp
- msr sp_el2, x0 /* Migrate SP */
- mrs x0, vbar_el3
- msr vbar_el2, x0 /* Migrate VBAR */
- mov x0, #0x3c9
- msr spsr_el3, x0 /* EL2_SP2 | D | A | I | F */
- msr elr_el3, lr
- eret
+1: armv8_switch_to_el2_m x0
ENDPROC(armv8_switch_to_el2)
ENTRY(armv8_switch_to_el1)
switch_el x0, 0f, 1f, 0f
0: ret
-1:
- /* Initialize Generic Timers */
- mrs x0, cnthctl_el2
- orr x0, x0, #0x3 /* Enable EL1 access to timers */
- msr cnthctl_el2, x0
- msr cntvoff_el2, x0
- mrs x0, cntkctl_el1
- orr x0, x0, #0x3 /* Enable EL0 access to timers */
- msr cntkctl_el1, x0
-
- /* Initilize MPID/MPIDR registers */
- mrs x0, midr_el1
- mrs x1, mpidr_el1
- msr vpidr_el2, x0
- msr vmpidr_el2, x1
-
- /* Disable coprocessor traps */
- mov x0, #0x33ff
- msr cptr_el2, x0 /* Disable coprocessor traps to EL2 */
- msr hstr_el2, xzr /* Disable coprocessor traps to EL2 */
- mov x0, #3 << 20
- msr cpacr_el1, x0 /* Enable FP/SIMD at EL1 */
-
- /* Initialize HCR_EL2 */
- mov x0, #(1 << 31) /* 64bit EL1 */
- orr x0, x0, #(1 << 29) /* Disable HVC */
- msr hcr_el2, x0
-
- /* SCTLR_EL1 initialization */
- mov x0, #0x0800
- movk x0, #0x30d0, lsl #16
- msr sctlr_el1, x0
-
- /* Return to the EL1_SP1 mode from EL2 */
- mov x0, sp
- msr sp_el1, x0 /* Migrate SP */
- mrs x0, vbar_el2
- msr vbar_el1, x0 /* Migrate VBAR */
- mov x0, #0x3c5
- msr spsr_el2, x0 /* EL1_SP1 | D | A | I | F */
- msr elr_el2, lr
- eret
+1: armv8_switch_to_el1_m x0, x1
ENDPROC(armv8_switch_to_el1)
diff --git a/arch/arm/include/asm/arch-fsl-lsch3/config.h b/arch/arm/include/asm/arch-fsl-lsch3/config.h
index c81ab41..932a21d 100644
--- a/arch/arm/include/asm/arch-fsl-lsch3/config.h
+++ b/arch/arm/include/asm/arch-fsl-lsch3/config.h
@@ -8,7 +8,7 @@
#define _ASM_ARMV8_FSL_LSCH3_CONFIG_
#include <fsl_ddrc_version.h>
-
+#define CONFIG_MP
#define CONFIG_SYS_FSL_OCRAM_BASE 0x18000000 /* initial RAM */
/* Link Definitions */
#define CONFIG_SYS_INIT_SP_ADDR (CONFIG_SYS_FSL_OCRAM_BASE + 0xfff0)
@@ -18,6 +18,7 @@
#define CONFIG_SYS_FSL_DDR2_ADDR (CONFIG_SYS_IMMR + 0x00090000)
#define CONFIG_SYS_FSL_GUTS_ADDR (CONFIG_SYS_IMMR + 0x00E00000)
#define CONFIG_SYS_FSL_PMU_ADDR (CONFIG_SYS_IMMR + 0x00E30000)
+#define CONFIG_SYS_FSL_RST_ADDR (CONFIG_SYS_IMMR + 0x00E60000)
#define CONFIG_SYS_FSL_CH3_CLK_GRPA_ADDR (CONFIG_SYS_IMMR + 0x00300000)
#define CONFIG_SYS_FSL_CH3_CLK_GRPB_ADDR (CONFIG_SYS_IMMR + 0x00310000)
#define CONFIG_SYS_FSL_CH3_CLK_CTRL_ADDR (CONFIG_SYS_IMMR + 0x00370000)
diff --git a/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h b/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
index 18e66bd..ee1d651 100644
--- a/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
+++ b/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
@@ -113,4 +113,39 @@ struct ccsr_clk_ctrl {
u8 res_04[0x20-0x04];
} clkcncsr[8];
};
+
+struct ccsr_reset {
+ u32 rstcr; /* 0x000 */
+ u32 rstcrsp; /* 0x004 */
+ u8 res_008[0x10-0x08]; /* 0x008 */
+ u32 rstrqmr1; /* 0x010 */
+ u32 rstrqmr2; /* 0x014 */
+ u32 rstrqsr1; /* 0x018 */
+ u32 rstrqsr2; /* 0x01c */
+ u32 rstrqwdtmrl; /* 0x020 */
+ u32 rstrqwdtmru; /* 0x024 */
+ u8 res_028[0x30-0x28]; /* 0x028 */
+ u32 rstrqwdtsrl; /* 0x030 */
+ u32 rstrqwdtsru; /* 0x034 */
+ u8 res_038[0x60-0x38]; /* 0x038 */
+ u32 brrl; /* 0x060 */
+ u32 brru; /* 0x064 */
+ u8 res_068[0x80-0x68]; /* 0x068 */
+ u32 pirset; /* 0x080 */
+ u32 pirclr; /* 0x084 */
+ u8 res_088[0x90-0x88]; /* 0x088 */
+ u32 brcorenbr; /* 0x090 */
+ u8 res_094[0x100-0x94]; /* 0x094 */
+ u32 rcw_reqr; /* 0x100 */
+ u32 rcw_completion; /* 0x104 */
+ u8 res_108[0x110-0x108]; /* 0x108 */
+ u32 pbi_reqr; /* 0x110 */
+ u32 pbi_completion; /* 0x114 */
+ u8 res_118[0xa00-0x118]; /* 0x118 */
+ u32 qmbm_warmrst; /* 0xa00 */
+ u32 soc_warmrst; /* 0xa04 */
+ u8 res_a08[0xbf8-0xa08]; /* 0xa08 */
+ u32 ip_rev1; /* 0xbf8 */
+ u32 ip_rev2; /* 0xbfc */
+};
#endif /* __ARCH_FSL_LSCH3_IMMAP_H */
diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index f77e4b8..16ba76e 100644
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -105,6 +105,87 @@ lr .req x30
cbz \xreg1, \master_label
.endm
+.macro armv8_switch_to_el2_m, xreg1
+ mov \xreg1, #0x5b1 /* Non-secure EL0/EL1 | HVC | 64bit EL2 */
+ msr scr_el3, \xreg1
+ msr cptr_el3, xzr /* Disable coprocessor traps to EL3 */
+ mov \xreg1, #0x33ff
+ msr cptr_el2, \xreg1 /* Disable coprocessor traps to EL2 */
+
+ /* Initialize SCTLR_EL2 */
+ msr sctlr_el2, xzr
+
+ /* Return to the EL2_SP2 mode from EL3 */
+ mov \xreg1, sp
+ msr sp_el2, \xreg1 /* Migrate SP */
+ mrs \xreg1, vbar_el3
+ msr vbar_el2, \xreg1 /* Migrate VBAR */
+ mov x0, #0x3c9
+ msr spsr_el3, \xreg1 /* EL2_SP2 | D | A | I | F */
+ msr elr_el3, lr
+ eret
+.endm
+
+.macro armv8_switch_to_el1_m, xreg1, xreg2
+ /* Initialize Generic Timers */
+ mrs \xreg1, cnthctl_el2
+ orr \xreg1, \xreg1, #0x3 /* Enable EL1 access to timers */
+ msr cnthctl_el2, \xreg1
+ msr cntvoff_el2, \xreg1
+ mrs \xreg1, cntkctl_el1
+ orr \xreg1, \xreg1, #0x3 /* Enable EL0 access to timers */
+ msr cntkctl_el1, \xreg1
+
+ /* Initilize MPID/MPIDR registers */
+ mrs \xreg1, midr_el1
+ mrs \xreg2, mpidr_el1
+ msr vpidr_el2, \xreg1
+ msr vmpidr_el2, \xreg2
+
+ /* Disable coprocessor traps */
+ mov \xreg1, #0x33ff
+ msr cptr_el2, \xreg1 /* Disable coprocessor traps to EL2 */
+ msr hstr_el2, xzr /* Disable coprocessor traps to EL2 */
+ mov \xreg1, #3 << 20
+ msr cpacr_el1, \xreg1 /* Enable FP/SIMD at EL1 */
+
+ /* Initialize HCR_EL2 */
+ mov \xreg1, #(1 << 31) /* 64bit EL1 */
+ orr \xreg1, \xreg1, #(1 << 29) /* Disable HVC */
+ msr hcr_el2, \xreg1
+
+ /* SCTLR_EL1 initialization */
+ mov \xreg1, #0x0800
+ movk \xreg1, #0x30d0, lsl #16
+ msr sctlr_el1, \xreg1
+
+ /* Return to the EL1_SP1 mode from EL2 */
+ mov \xreg1, sp
+ msr sp_el1, \xreg1 /* Migrate SP */
+ mrs \xreg1, vbar_el2
+ msr vbar_el1, \xreg1 /* Migrate VBAR */
+ mov \xreg1, #0x3c5
+ msr spsr_el2, \xreg1 /* EL1_SP1 | D | A | I | F */
+ msr elr_el2, lr
+ eret
+.endm
+
+#if defined(CONFIG_GICV3)
+.macro gic_wait_for_interrupt_m xreg1
+0 : wfi
+ mrs \xreg1, ICC_IAR1_EL1
+ msr ICC_EOIR1_EL1, \xreg1
+ cbnz \xreg1, 0b
+.endm
+#elif defined(CONFIG_GICV2)
+.macro gic_wait_for_interrupt_m xreg1, wreg2
+0 : wfi
+ ldr \wreg2, [\xreg1, GICC_AIAR]
+ str \wreg2, [\xreg1, GICC_AEOIR]
+ cbnz \wreg2, 0b
+.endm
+#endif
+
#endif /* CONFIG_ARM64 */
#endif /* __ASSEMBLY__ */
diff --git a/arch/arm/lib/gic_64.S b/arch/arm/lib/gic_64.S
index d56396e..a3e18f7 100644
--- a/arch/arm/lib/gic_64.S
+++ b/arch/arm/lib/gic_64.S
@@ -10,8 +10,8 @@
#include <asm-offsets.h>
#include <config.h>
#include <linux/linkage.h>
-#include <asm/macro.h>
#include <asm/gic.h>
+#include <asm/macro.h>
/*************************************************************************
@@ -181,14 +181,10 @@ ENDPROC(gic_kick_secondary_cpus)
*
*************************************************************************/
ENTRY(gic_wait_for_interrupt)
-0: wfi
#if defined(CONFIG_GICV3)
- mrs x9, ICC_IAR1_EL1
- msr ICC_EOIR1_EL1, x9
+ gic_wait_for_interrupt_m x9
#elif defined(CONFIG_GICV2)
- ldr w9, [x0, GICC_AIAR]
- str w9, [x0, GICC_AEOIR]
+ gic_wait_for_interrupt_m x0, w9
#endif
- cbnz w9, 0b
ret
ENDPROC(gic_wait_for_interrupt)
diff --git a/common/board_f.c b/common/board_f.c
index 4ea4cb2..3b6df18 100644
--- a/common/board_f.c
+++ b/common/board_f.c
@@ -41,7 +41,7 @@
#include <watchdog.h>
#include <asm/errno.h>
#include <asm/io.h>
-#ifdef CONFIG_MP
+#if defined(CONFIG_MP) && (defined(CONFIG_MPC86xx) || defined(CONFIG_E500))
#include <asm/mp.h>
#endif
#include <asm/sections.h>
--
1.7.9.5
More information about the U-Boot
mailing list