1/* 2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230) 3 * 4 * SPDX-License-Identifier: GPL-2.0-only 5 */ 6 7#include <autoconf.h> 8#include <elfloader/gen_config.h> 9#include <elfloader_common.h> 10 11#include <devices_gen.h> 12#include <drivers/common.h> 13#include <drivers/smp.h> 14 15#include <printf.h> 16#include <types.h> 17#include <scu.h> 18#include <abort.h> 19#include <armv/machine.h> 20 21#define REG(base, offs) ((volatile uint32_t *)(((uintptr_t)base) + (offs))) 22 23#define CPU_JUMP_PTR 0xFFFFFFF0 24 25/* 26 * A9_CPU_RST_CTRL Register definitions. 27 * See TRM B.28 System Level Control Registers (slcr) 28 */ 29#define A9_CPU_RST_CTRL 0x44 30#define PERI_RST_BIT 8 31#define A9_CLKSTOPx_BIT(x) (4 + (x)) 32#define A9_RSTx_BIT(x) (0 + (x)) 33 34UNUSED static void *get_scu_base(void) 35{ 36 void *scu = NULL; 37#if CONFIG_ARCH_AARCH32 38 asm("mrc p15, 4, %0, c15, c0, 0" : "=r"(scu)); 39#else 40 abort(); 41#endif 42 return scu; 43} 44 45 46static int smp_zynq7000_cpu_on(UNUSED struct elfloader_device *dev, 47 UNUSED struct elfloader_cpu *cpu, UNUSED void *entry, UNUSED void *stack) 48{ 49#if CONFIG_MAX_NUM_NODES > 1 50 volatile void *mmio = dev->region_bases[0]; 51 volatile word_t *jump_ptr = (volatile word_t *)CPU_JUMP_PTR; 52 secondary_data.entry = entry; 53 secondary_data.stack = stack; 54 /* stop core - see TRM 3.7 Application Processing Unit (APU) Reset */ 55 *REG(mmio, A9_CPU_RST_CTRL) |= BIT(A9_RSTx_BIT(cpu->cpu_id)); 56 dsb(); 57 *REG(mmio, A9_CPU_RST_CTRL) |= BIT(A9_CLKSTOPx_BIT(cpu->cpu_id)); 58 dsb(); 59 /* set where core should jump to when it's woken up */ 60 *jump_ptr = (word_t)secondary_startup; 61 /* start core */ 62 *REG(mmio, A9_CPU_RST_CTRL) &= ~BIT(A9_RSTx_BIT(cpu->cpu_id)); 63 dsb(); 64 *REG(mmio, A9_CPU_RST_CTRL) &= ~BIT(A9_CLKSTOPx_BIT(cpu->cpu_id)); 65 dsb(); 66 /* the other core is in WFE, we need to wake it */ 67 asm volatile("sev"); 68 69 return 0; 70#else 71 return -1; 72#endif 73} 74 75static int smp_zynq7000_init(UNUSED struct elfloader_device *dev, 76 UNUSED void *match_data) 77{ 78#if CONFIG_MAX_NUM_NODES > 1 79 void *scu = get_scu_base(); 80 scu_enable(scu); 81 smp_register_handler(dev); 82#endif 83 return 0; 84} 85 86 87static const struct dtb_match_table smp_zynq7000_matches[] = { 88 { .compatible = "xlnx,zynq-reset" }, 89 { .compatible = NULL /* sentinel */ }, 90}; 91 92static const struct elfloader_smp_ops smp_zynq7000_ops = { 93 .enable_method = NULL, 94 .cpu_on = &smp_zynq7000_cpu_on, 95}; 96 97static const struct elfloader_driver smp_zynq7000 = { 98 .match_table = smp_zynq7000_matches, 99 .type = DRIVER_SMP, 100 .init = &smp_zynq7000_init, 101 .ops = &smp_zynq7000_ops, 102}; 103 104ELFLOADER_DRIVER(smp_zynq7000); 105