1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 * Copyright: Linux Kernel team
4 * SPDX-License-Identifier: GPL-2.0-only
5 *
6 * The code in here is derived from the Linux kernel
7 */
8
9#include <autoconf.h>
10#include <elfloader/gen_config.h>
11
12#include <printf.h>
13#include <armv/machine.h>
14#include <scu.h>
15#include <abort.h>
16
17#if CONFIG_MAX_NUM_NODES > 1
18VISIBLE volatile word_t smp_aps_index = 1;
19
20/* System Reset Controller base address */
21#define SRC_BASE 0x30390000
22#define GPC_BASE 0x303a0000
23
24#define SRC_SCR             0x000
25#define SRC_GPR1            0x020
26#define BP_SRC_SCR_WARM_RESET_ENABLE    0
27#define BP_SRC_SCR_CORE1_RST        14
28#define BP_SRC_SCR_CORE1_ENABLE     22
29
30#define GPC_CPU_PGC_SW_PUP_REQ              0xf0
31#define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7  0x2
32#define GPC_PGC_C1                          0x840
33#define BP_SRC_A7RCR1_A7_CORE1_ENABLE       1
34
35#define SRC_A7RCR1  0x008
36#define SRC_GPR1_V2 0x074
37
38#define REG(base,offset) (*(volatile unsigned int*)(((void *)(base))+(offset)))
39
40void imx_non_boot(void);
41
42static void src_init(void)
43{
44    unsigned int val;
45    val = REG(SRC_BASE, SRC_SCR);
46    val &= ~(1 << BP_SRC_SCR_WARM_RESET_ENABLE);
47    REG(SRC_BASE, SRC_SCR) = val;
48}
49
50static void gpc_core1_up(void)
51{
52    unsigned int val = REG(GPC_BASE, GPC_CPU_PGC_SW_PUP_REQ);
53
54    REG(GPC_BASE, GPC_PGC_C1) = 1;
55
56    val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
57
58    REG(GPC_BASE, GPC_CPU_PGC_SW_PUP_REQ) = val;
59
60    while ((REG(GPC_BASE, GPC_CPU_PGC_SW_PUP_REQ) & BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7) != 0);
61
62    REG(GPC_BASE, GPC_PGC_C1) = 0;
63}
64
65static void src_enable_cpu(int cpu)
66{
67    unsigned int mask, val;
68
69    gpc_core1_up();
70    mask = 1 << (BP_SRC_A7RCR1_A7_CORE1_ENABLE + cpu - 1);
71    val = REG(SRC_BASE, SRC_A7RCR1);
72    val |= mask;
73    REG(SRC_BASE, SRC_A7RCR1) = val;
74}
75
76static void src_set_cpu_jump(int cpu, unsigned int jump_addr)
77{
78    REG(SRC_BASE, SRC_GPR1_V2 + cpu * 8) = (unsigned int)jump_addr;
79    dsb();
80}
81
82void init_cpus(void)
83{
84    unsigned int i, num;
85
86    src_init();
87
88    /* get core count from L2CTLR */
89    asm volatile("mrc p15, 1, %0, c9, c0, 2": "=r"(num));
90    num = ((num >> 24) & 0x3) + 1;
91
92    if (num > CONFIG_MAX_NUM_NODES) {
93        num = CONFIG_MAX_NUM_NODES;
94    } else if (num < CONFIG_MAX_NUM_NODES) {
95        printf("Error: Unsupported number of CPUs! This platform has %u CPUs, while static configuration provided is %u CPUs\n",
96               num, CONFIG_MAX_NUM_NODES);
97        abort();
98    }
99
100    printf("Bringing up %d other cpus\n", num - 1);
101    for (i = 1; i < num; i++) {
102        src_set_cpu_jump(i, (unsigned int)imx_non_boot);
103        src_enable_cpu(i);
104    }
105}
106#endif /* CONFIG_MAX_NUM_NODES */
107