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