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