1 /*
2  * Copyright 2014-2015 Freescale Semiconductor, Inc.
3  *
4  * SPDX-License-Identifier:	GPL-2.0+
5  */
6 
7 #include <common.h>
8 #include <asm/io.h>
9 #include <asm/system.h>
10 #include <asm/arch/mp.h>
11 #include <asm/arch/soc.h>
12 
13 DECLARE_GLOBAL_DATA_PTR;
14 
15 void *get_spin_tbl_addr(void)
16 {
17 	return &__spin_table;
18 }
19 
20 phys_addr_t determine_mp_bootpg(void)
21 {
22 	return (phys_addr_t)&secondary_boot_code;
23 }
24 
25 int fsl_layerscape_wake_seconday_cores(void)
26 {
27 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
28 #ifdef CONFIG_FSL_LSCH3
29 	struct ccsr_reset __iomem *rst = (void *)(CONFIG_SYS_FSL_RST_ADDR);
30 #elif defined(CONFIG_FSL_LSCH2)
31 	struct ccsr_scfg __iomem *scfg = (void *)(CONFIG_SYS_FSL_SCFG_ADDR);
32 #endif
33 	u32 cores, cpu_up_mask = 1;
34 	int i, timeout = 10;
35 	u64 *table = get_spin_tbl_addr();
36 
37 #ifdef COUNTER_FREQUENCY_REAL
38 	/* update for secondary cores */
39 	__real_cntfrq = COUNTER_FREQUENCY_REAL;
40 	flush_dcache_range((unsigned long)&__real_cntfrq,
41 			   (unsigned long)&__real_cntfrq + 8);
42 #endif
43 
44 	cores = cpu_mask();
45 	/* Clear spin table so that secondary processors
46 	 * observe the correct value after waking up from wfe.
47 	 */
48 	memset(table, 0, CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE);
49 	flush_dcache_range((unsigned long)table,
50 			   (unsigned long)table +
51 			   (CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE));
52 
53 	printf("Waking secondary cores to start from %lx\n", gd->relocaddr);
54 
55 #ifdef CONFIG_FSL_LSCH3
56 	gur_out32(&gur->bootlocptrh, (u32)(gd->relocaddr >> 32));
57 	gur_out32(&gur->bootlocptrl, (u32)gd->relocaddr);
58 	gur_out32(&gur->scratchrw[6], 1);
59 	asm volatile("dsb st" : : : "memory");
60 	rst->brrl = cores;
61 	asm volatile("dsb st" : : : "memory");
62 #elif defined(CONFIG_FSL_LSCH2)
63 	scfg_out32(&scfg->scratchrw[0], (u32)(gd->relocaddr >> 32));
64 	scfg_out32(&scfg->scratchrw[1], (u32)gd->relocaddr);
65 	asm volatile("dsb st" : : : "memory");
66 	gur_out32(&gur->brrl, cores);
67 	asm volatile("dsb st" : : : "memory");
68 
69 	/* Bootup online cores */
70 	scfg_out32(&scfg->corebcr, cores);
71 #endif
72 	/* This is needed as a precautionary measure.
73 	 * If some code before this has accidentally  released the secondary
74 	 * cores then the pre-bootloader code will trap them in a "wfe" unless
75 	 * the scratchrw[6] is set. In this case we need a sev here to get these
76 	 * cores moving again.
77 	 */
78 	asm volatile("sev");
79 
80 	while (timeout--) {
81 		flush_dcache_range((unsigned long)table, (unsigned long)table +
82 				   CONFIG_MAX_CPUS * 64);
83 		for (i = 1; i < CONFIG_MAX_CPUS; i++) {
84 			if (table[i * WORDS_PER_SPIN_TABLE_ENTRY +
85 					SPIN_TABLE_ELEM_STATUS_IDX])
86 				cpu_up_mask |= 1 << i;
87 		}
88 		if (hweight32(cpu_up_mask) == hweight32(cores))
89 			break;
90 		udelay(10);
91 	}
92 	if (timeout <= 0) {
93 		printf("Not all cores (0x%x) are up (0x%x)\n",
94 		       cores, cpu_up_mask);
95 		return 1;
96 	}
97 	printf("All (%d) cores are up.\n", hweight32(cores));
98 
99 	return 0;
100 }
101 
102 int is_core_valid(unsigned int core)
103 {
104 	return !!((1 << core) & cpu_mask());
105 }
106 
107 int is_core_online(u64 cpu_id)
108 {
109 	u64 *table;
110 	int pos = id_to_core(cpu_id);
111 	table = (u64 *)get_spin_tbl_addr() + pos * WORDS_PER_SPIN_TABLE_ENTRY;
112 	return table[SPIN_TABLE_ELEM_STATUS_IDX] == 1;
113 }
114 
115 int cpu_reset(int nr)
116 {
117 	puts("Feature is not implemented.\n");
118 
119 	return 0;
120 }
121 
122 int cpu_disable(int nr)
123 {
124 	puts("Feature is not implemented.\n");
125 
126 	return 0;
127 }
128 
129 int core_to_pos(int nr)
130 {
131 	u32 cores = cpu_mask();
132 	int i, count = 0;
133 
134 	if (nr == 0) {
135 		return 0;
136 	} else if (nr >= hweight32(cores)) {
137 		puts("Not a valid core number.\n");
138 		return -1;
139 	}
140 
141 	for (i = 1; i < 32; i++) {
142 		if (is_core_valid(i)) {
143 			count++;
144 			if (count == nr)
145 				break;
146 		}
147 	}
148 
149 	return count;
150 }
151 
152 int cpu_status(int nr)
153 {
154 	u64 *table;
155 	int pos;
156 
157 	if (nr == 0) {
158 		table = (u64 *)get_spin_tbl_addr();
159 		printf("table base @ 0x%p\n", table);
160 	} else {
161 		pos = core_to_pos(nr);
162 		if (pos < 0)
163 			return -1;
164 		table = (u64 *)get_spin_tbl_addr() + pos *
165 			WORDS_PER_SPIN_TABLE_ENTRY;
166 		printf("table @ 0x%p\n", table);
167 		printf("   addr - 0x%016llx\n",
168 		       table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX]);
169 		printf("   status   - 0x%016llx\n",
170 		       table[SPIN_TABLE_ELEM_STATUS_IDX]);
171 		printf("   lpid  - 0x%016llx\n",
172 		       table[SPIN_TABLE_ELEM_LPID_IDX]);
173 	}
174 
175 	return 0;
176 }
177 
178 int cpu_release(int nr, int argc, char * const argv[])
179 {
180 	u64 boot_addr;
181 	u64 *table = (u64 *)get_spin_tbl_addr();
182 	int pos;
183 
184 	pos = core_to_pos(nr);
185 	if (pos <= 0)
186 		return -1;
187 
188 	table += pos * WORDS_PER_SPIN_TABLE_ENTRY;
189 	boot_addr = simple_strtoull(argv[0], NULL, 16);
190 	table[SPIN_TABLE_ELEM_ENTRY_ADDR_IDX] = boot_addr;
191 	flush_dcache_range((unsigned long)table,
192 			   (unsigned long)table + SPIN_TABLE_ELEM_SIZE);
193 	asm volatile("dsb st");
194 	smp_kick_all_cpus();	/* only those with entry addr set will run */
195 	/*
196 	 * When the first release command runs, all cores are set to go. Those
197 	 * without a valid entry address will be trapped by "wfe". "sev" kicks
198 	 * them off to check the address again. When set, they continue to run.
199 	 */
200 	asm volatile("sev");
201 
202 	return 0;
203 }
204