1/* 2 * Copyright Linux Kernel team 3 * SPDX-License-Identifier: GPL-2.0-only 4 * 5 * The code in here is loosely derived from the Linux kernel 6 */ 7 8#include <autoconf.h> 9#include <elfloader/gen_config.h> 10#include <elfloader_common.h> 11 12#include <devices_gen.h> 13#include <drivers/common.h> 14#include <drivers/smp.h> 15 16#include <printf.h> 17#include <armv/machine.h> 18#include <scu.h> 19#include <abort.h> 20 21#define REG(base, offs) ((volatile word_t *)(((uintptr_t)base) + (offs))) 22 23#define SRC_SCR 0x000 24#define SRC_GPR1 0x020 25#define BP_SRC_SCR_WARM_RESET_ENABLE 0 26#define BP_SRC_SCR_CORE1_RST 14 27#define BP_SRC_SCR_CORE1_ENABLE 22 28 29 30UNUSED static void *get_scu_base(void) 31{ 32 void *scu = NULL; 33#if CONFIG_ARCH_AARCH32 34 asm("mrc p15, 4, %0, c15, c0, 0" : "=r"(scu)); 35#else 36 abort(); 37#endif 38 return scu; 39} 40 41UNUSED static void src_init(volatile void *mmio) 42{ 43 uint32_t val; 44 val = *REG(mmio, SRC_SCR); 45 val &= ~(1 << BP_SRC_SCR_WARM_RESET_ENABLE); 46 *REG(mmio, SRC_SCR) = val; 47} 48 49static int smp_imx6_cpu_on(UNUSED struct elfloader_device *dev, 50 UNUSED struct elfloader_cpu *cpu, UNUSED void *entry, UNUSED void *stack) 51{ 52#if CONFIG_MAX_NUM_NODES > 1 53 volatile void *mmio = dev->region_bases[0]; 54 secondary_data.entry = entry; 55 secondary_data.stack = stack; 56 *REG(mmio, SRC_GPR1 + (cpu->cpu_id * 8)) = (word_t)secondary_startup; 57 dsb(); 58 59 if (cpu->cpu_id == 0) { 60 /* there is no core0_enable bit in the SCR register */ 61 printf("error: cannot power on CPU 0!\n"); 62 return -1; 63 } 64 65 uint32_t mask = 1 << (BP_SRC_SCR_CORE1_ENABLE + (cpu->cpu_id - 1)); 66 *REG(mmio, SRC_SCR) |= mask; 67 return 0; 68#else 69 return -1; 70#endif 71} 72 73static int smp_imx6_init(UNUSED struct elfloader_device *dev, 74 UNUSED void *match_data) 75{ 76#if CONFIG_MAX_NUM_NODES > 1 77 void *scu = get_scu_base(); 78 scu_enable(scu); 79 src_init(dev->region_bases[0]); 80 smp_register_handler(dev); 81#endif 82 return 0; 83} 84 85 86static const struct dtb_match_table smp_imx6_matches[] = { 87 { .compatible = "fsl,imx6q-src" }, 88 { .compatible = NULL /* sentinel */ }, 89}; 90 91static const struct elfloader_smp_ops smp_imx6_ops = { 92 .enable_method = NULL, 93 .cpu_on = &smp_imx6_cpu_on, 94}; 95 96static const struct elfloader_driver smp_imx6 = { 97 .match_table = smp_imx6_matches, 98 .type = DRIVER_SMP, 99 .init = &smp_imx6_init, 100 .ops = &smp_imx6_ops, 101}; 102 103ELFLOADER_DRIVER(smp_imx6); 104