diff options
Diffstat (limited to 'arch/arm/cpu/armv8/fsl-lsch3')
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/Makefile | 2 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/cpu.c | 13 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/cpu.h | 1 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/fdt.c | 58 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/lowlevel.S | 125 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/mp.c | 168 | ||||
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-lsch3/mp.h | 36 |
7 files changed, 383 insertions, 20 deletions
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..e392eb9 --- /dev/null +++ b/arch/arm/cpu/armv8/fsl-lsch3/fdt.c @@ -0,0 +1,58 @@ +/* + * 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(); + fdt32_t *reg; + int addr_cells; + u64 val; + size_t *boot_code_size = &(__secondary_boot_code_size); + + off = fdt_path_offset(blob, "/cpus"); + if (off < 0) { + puts("couldn't find /cpus node\n"); + return; + } + of_bus_default_count_cells(blob, off, &addr_cells, NULL); + + off = fdt_node_offset_by_prop_value(blob, -1, "device_type", "cpu", 4); + while (off != -FDT_ERR_NOTFOUND) { + reg = (fdt32_t *)fdt_getprop(blob, off, "reg", 0); + if (reg) { + val = spin_tbl_addr; + val += id_to_core(of_read_number(reg, addr_cells)) + * SPIN_TABLE_ELEM_SIZE; + 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("Warning: found cpu node without reg property\n"); + } + off = fdt_node_offset_by_prop_value(blob, off, "device_type", + "cpu", 4); + } + + fdt_add_mem_rsv(blob, (uintptr_t)&secondary_boot_code, + *boot_code_size); +} +#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 ad32b6c..2a88aab 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 */ @@ -35,31 +37,114 @@ ENTRY(lowlevel_init) #endif #endif - branch_if_master x0, x1, 1f + branch_if_master x0, x1, 2f + ldr x0, =secondary_boot_func + blr x0 +2: + mov lr, x29 /* Restore LR */ + ret +ENDPROC(lowlevel_init) + + /* Keep literals not used by the secondary boot code outside it */ + .ltorg + + /* Using 64 bit alignment since the spin table is accessed as data */ + .align 4 + .global secondary_boot_code + /* Secondary Boot Code starts here */ +secondary_boot_code: + .global __spin_table +__spin_table: + .space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE + + .align 2 +ENTRY(secondary_boot_func) /* - * Slave should wait for master clearing spin table. - * This sync prevent salves observing incorrect - * value of spin table and jumping to wrong place. + * MPIDR_EL1 Fields: + * 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] = RES0 + * MPIDR[30] = U + * MPIDR[31] = ME + * MPIDR[39:32] = AFF3 + * + * Linear Processor ID (LPID) calculation from MPIDR_EL1: + * (We only use AFF0_CPUID and AFF1_CLUSTERID for now + * until AFF2_CLUSTERID and AFF3 have non-zero values) + * + * LPID = MPIDR[15:8] | MPIDR[1:0] */ -#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3) -#ifdef CONFIG_GICV2 - ldr x0, =GICC_BASE -#endif - bl gic_wait_for_interrupt -#endif - + mrs x0, mpidr_el1 + ubfm x1, x0, #8, #15 + ubfm x2, x0, #0, #1 + orr x10, x2, x1, lsl #2 /* x10 has LPID */ + ubfm x9, x0, #0, #15 /* x9 contains MPIDR[15:0] */ /* - * All processors will enter EL2 and optionally EL1. + * offset of the spin table element for this core from start of spin + * table (each elem is padded to 64 bytes) */ - bl armv8_switch_to_el2 + lsl x1, x10, #6 + ldr x0, =__spin_table + /* physical address of this cpus spin table element */ + add x11, x1, x0 + + str x9, [x11, #16] /* LPID */ + mov x4, #1 + str x4, [x11, #8] /* STATUS */ + dsb sy +#if defined(CONFIG_GICV3) + gic_wait_for_interrupt_m x0 +#elif defined(CONFIG_GICV2) + ldr x0, =GICC_BASE + gic_wait_for_interrupt_m x0, w1 +#endif + + bl secondary_switch_to_el2 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1 - bl armv8_switch_to_el1 + bl secondary_switch_to_el1 #endif - b 2f -1: -2: - mov lr, x29 /* Restore LR */ - ret -ENDPROC(lowlevel_init) +slave_cpu: + wfe + ldr x0, [x11] + cbz x0, slave_cpu +#ifndef CONFIG_ARMV8_SWITCH_TO_EL1 + mrs x1, sctlr_el2 +#else + mrs x1, sctlr_el1 +#endif + tbz x1, #25, cpu_is_le + rev x0, x0 /* BE to LE conversion */ +cpu_is_le: + 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 code are + * assembled within it (this is required so that we can protect + * this area with a single memreserve region + */ + .ltorg + + /* 64 bit alignment for elements accessed as data */ + .align 4 + .globl __secondary_boot_code_size + .type __secondary_boot_code_size, %object + /* Secondary Boot Code ends here */ +__secondary_boot_code_size: + .quad .-secondary_boot_code 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..94998bf --- /dev/null +++ b/arch/arm/cpu/armv8/fsl-lsch3/mp.c @@ -0,0 +1,168 @@ +/* + * 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) +{ + return &__spin_table; +} + +phys_addr_t determine_mp_bootpg(void) +{ + return (phys_addr_t)&secondary_boot_code; +} + +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); + u32 cores, cpu_up_mask = 1; + int i, timeout = 10; + u64 *table = get_spin_tbl_addr(); + + cores = cpu_mask(); + /* Clear spin table so that secondary processors + * observe the correct value after waking up from wfe. + */ + memset(table, 0, CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE); + flush_dcache_range((unsigned long)table, + (unsigned long)table + + (CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_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"); + + /* This is needed as a precautionary measure. + * If some code before this has accidentally released the secondary + * cores then the pre-bootloader code will trap them in a "wfe" unless + * the scratchrw[6] is set. In this case we need a sev here to get these + * cores moving again. + */ + 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 * WORDS_PER_SPIN_TABLE_ENTRY + + SPIN_TABLE_ELEM_STATUS_IDX]) + 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 * + WORDS_PER_SPIN_TABLE_ENTRY; + printf("table @ 0x%p\n", table); + printf(" addr - 0x%016llx\n", + table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX]); + printf(" status - 0x%016llx\n", + table[SPIN_TABLE_ELEM_STATUS_IDX]); + printf(" lpid - 0x%016llx\n", + table[SPIN_TABLE_ELEM_LPID_IDX]); + } + + return 0; +} + +int cpu_release(int nr, int argc, char * const argv[]) +{ + u64 boot_addr; + u64 *table = (u64 *)get_spin_tbl_addr(); + int pos; + + pos = core_to_pos(nr); + if (pos <= 0) + return -1; + + table += pos * WORDS_PER_SPIN_TABLE_ENTRY; + boot_addr = simple_strtoull(argv[0], NULL, 16); + table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX] = boot_addr; + flush_dcache_range((unsigned long)table, + (unsigned long)table + SPIN_TABLE_ELEM_SIZE); + 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..06ac0bc --- /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 + +/* +* Each spin table element is defined as +* struct { +* uint64_t entry_addr; +* uint64_t status; +* uint64_t lpid; +* }; +* we pad this struct to 64 bytes so each entry is in its own cacheline +* the actual spin table is an array of these structures +*/ +#define SPIN_TABLE_ELEM_ENTRY_ADDR_IDX 0 +#define SPIN_TABLE_ELEM_STATUS_IDX 1 +#define SPIN_TABLE_ELEM_LPID_IDX 2 +#define WORDS_PER_SPIN_TABLE_ENTRY 8 /* pad to 64 bytes */ +#define SPIN_TABLE_ELEM_SIZE 64 + +#define id_to_core(x) ((x & 3) | (x >> 6)) +#ifndef __ASSEMBLY__ +extern u64 __spin_table[]; +extern u64 *secondary_boot_code; +extern size_t __secondary_boot_code_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 */ |