Linux preempt-rt

Check our new training course

Real-Time Linux with PREEMPT_RT

Check our new training course
with Creative Commons CC-BY-SA
lecture and lab materials

Bootlin logo

Elixir Cross Referencer

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (c) 2018 Chen-Yu Tsai
 *
 * Chen-Yu Tsai <wens@csie.org>
 *
 * arch/arm/mach-sunxi/mc_smp.c
 *
 * Based on Allwinner code, arch/arm/mach-exynos/mcpm-exynos.c, and
 * arch/arm/mach-hisi/platmcpm.c
 * Cluster cache enable trampoline code adapted from MCPM framework
 */

#include <linux/arm-cci.h>
#include <linux/cpu_pm.h>
#include <linux/delay.h>
#include <linux/io.h>
#include <linux/iopoll.h>
#include <linux/irqchip/arm-gic.h>
#include <linux/of.h>
#include <linux/of_address.h>
#include <linux/of_device.h>
#include <linux/smp.h>

#include <asm/cacheflush.h>
#include <asm/cp15.h>
#include <asm/cputype.h>
#include <asm/idmap.h>
#include <asm/smp_plat.h>
#include <asm/suspend.h>

#define SUNXI_CPUS_PER_CLUSTER		4
#define SUNXI_NR_CLUSTERS		2

#define POLL_USEC	100
#define TIMEOUT_USEC	100000

#define CPUCFG_CX_CTRL_REG0(c)		(0x10 * (c))
#define CPUCFG_CX_CTRL_REG0_L1_RST_DISABLE(n)	BIT(n)
#define CPUCFG_CX_CTRL_REG0_L1_RST_DISABLE_ALL	0xf
#define CPUCFG_CX_CTRL_REG0_L2_RST_DISABLE_A7	BIT(4)
#define CPUCFG_CX_CTRL_REG0_L2_RST_DISABLE_A15	BIT(0)
#define CPUCFG_CX_CTRL_REG1(c)		(0x10 * (c) + 0x4)
#define CPUCFG_CX_CTRL_REG1_ACINACTM	BIT(0)
#define CPUCFG_CX_STATUS(c)		(0x30 + 0x4 * (c))
#define CPUCFG_CX_STATUS_STANDBYWFI(n)	BIT(16 + (n))
#define CPUCFG_CX_STATUS_STANDBYWFIL2	BIT(0)
#define CPUCFG_CX_RST_CTRL(c)		(0x80 + 0x4 * (c))
#define CPUCFG_CX_RST_CTRL_DBG_SOC_RST	BIT(24)
#define CPUCFG_CX_RST_CTRL_ETM_RST(n)	BIT(20 + (n))
#define CPUCFG_CX_RST_CTRL_ETM_RST_ALL	(0xf << 20)
#define CPUCFG_CX_RST_CTRL_DBG_RST(n)	BIT(16 + (n))
#define CPUCFG_CX_RST_CTRL_DBG_RST_ALL	(0xf << 16)
#define CPUCFG_CX_RST_CTRL_H_RST	BIT(12)
#define CPUCFG_CX_RST_CTRL_L2_RST	BIT(8)
#define CPUCFG_CX_RST_CTRL_CX_RST(n)	BIT(4 + (n))
#define CPUCFG_CX_RST_CTRL_CORE_RST(n)	BIT(n)
#define CPUCFG_CX_RST_CTRL_CORE_RST_ALL	(0xf << 0)

#define PRCM_CPU_PO_RST_CTRL(c)		(0x4 + 0x4 * (c))
#define PRCM_CPU_PO_RST_CTRL_CORE(n)	BIT(n)
#define PRCM_CPU_PO_RST_CTRL_CORE_ALL	0xf
#define PRCM_PWROFF_GATING_REG(c)	(0x100 + 0x4 * (c))
/* The power off register for clusters are different from a80 and a83t */
#define PRCM_PWROFF_GATING_REG_CLUSTER_SUN8I	BIT(0)
#define PRCM_PWROFF_GATING_REG_CLUSTER_SUN9I	BIT(4)
#define PRCM_PWROFF_GATING_REG_CORE(n)	BIT(n)
#define PRCM_PWR_SWITCH_REG(c, cpu)	(0x140 + 0x10 * (c) + 0x4 * (cpu))
#define PRCM_CPU_SOFT_ENTRY_REG		0x164

/* R_CPUCFG registers, specific to sun8i-a83t */
#define R_CPUCFG_CLUSTER_PO_RST_CTRL(c)	(0x30 + (c) * 0x4)
#define R_CPUCFG_CLUSTER_PO_RST_CTRL_CORE(n)	BIT(n)
#define R_CPUCFG_CPU_SOFT_ENTRY_REG		0x01a4

#define CPU0_SUPPORT_HOTPLUG_MAGIC0	0xFA50392F
#define CPU0_SUPPORT_HOTPLUG_MAGIC1	0x790DCA3A

static void __iomem *cpucfg_base;
static void __iomem *prcm_base;
static void __iomem *sram_b_smp_base;
static void __iomem *r_cpucfg_base;

extern void sunxi_mc_smp_secondary_startup(void);
extern void sunxi_mc_smp_resume(void);
static bool is_a83t;

static bool sunxi_core_is_cortex_a15(unsigned int core, unsigned int cluster)
{
	struct device_node *node;
	int cpu = cluster * SUNXI_CPUS_PER_CLUSTER + core;
	bool is_compatible;

	node = of_cpu_device_node_get(cpu);

	/* In case of_cpu_device_node_get fails */
	if (!node)
		node = of_get_cpu_node(cpu, NULL);

	if (!node) {
		/*
		 * There's no point in returning an error, since we
		 * would be mid way in a core or cluster power sequence.
		 */
		pr_err("%s: Couldn't get CPU cluster %u core %u device node\n",
		       __func__, cluster, core);

		return false;
	}

	is_compatible = of_device_is_compatible(node, "arm,cortex-a15");
	of_node_put(node);
	return is_compatible;
}

static int sunxi_cpu_power_switch_set(unsigned int cpu, unsigned int cluster,
				      bool enable)
{
	u32 reg;

	/* control sequence from Allwinner A80 user manual v1.2 PRCM section */
	reg = readl(prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
	if (enable) {
		if (reg == 0x00) {
			pr_debug("power clamp for cluster %u cpu %u already open\n",
				 cluster, cpu);
			return 0;
		}

		writel(0xff, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
		writel(0xfe, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
		writel(0xf8, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
		writel(0xf0, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
		writel(0x00, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
	} else {
		writel(0xff, prcm_base + PRCM_PWR_SWITCH_REG(cluster, cpu));
		udelay(10);
	}

	return 0;
}

static void sunxi_cpu0_hotplug_support_set(bool enable)
{
	if (enable) {
		writel(CPU0_SUPPORT_HOTPLUG_MAGIC0, sram_b_smp_base);
		writel(CPU0_SUPPORT_HOTPLUG_MAGIC1, sram_b_smp_base + 0x4);
	} else {
		writel(0x0, sram_b_smp_base);
		writel(0x0, sram_b_smp_base + 0x4);
	}
}

static int sunxi_cpu_powerup(unsigned int cpu, unsigned int cluster)
{
	u32 reg;

	pr_debug("%s: cluster %u cpu %u\n", __func__, cluster, cpu);
	if (cpu >= SUNXI_CPUS_PER_CLUSTER || cluster >= SUNXI_NR_CLUSTERS)
		return -EINVAL;

	/* Set hotplug support magic flags for cpu0 */
	if (cluster == 0 && cpu == 0)
		sunxi_cpu0_hotplug_support_set(true);

	/* assert processor power-on reset */
	reg = readl(prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));
	reg &= ~PRCM_CPU_PO_RST_CTRL_CORE(cpu);
	writel(reg, prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));

	if (is_a83t) {
		/* assert cpu power-on reset */
		reg  = readl(r_cpucfg_base +
			     R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		reg &= ~(R_CPUCFG_CLUSTER_PO_RST_CTRL_CORE(cpu));
		writel(reg, r_cpucfg_base +
		       R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		udelay(10);
	}

	/* Cortex-A7: hold L1 reset disable signal low */
	if (!sunxi_core_is_cortex_a15(cpu, cluster)) {
		reg = readl(cpucfg_base + CPUCFG_CX_CTRL_REG0(cluster));
		reg &= ~CPUCFG_CX_CTRL_REG0_L1_RST_DISABLE(cpu);
		writel(reg, cpucfg_base + CPUCFG_CX_CTRL_REG0(cluster));
	}

	/* assert processor related resets */
	reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
	reg &= ~CPUCFG_CX_RST_CTRL_DBG_RST(cpu);

	/*
	 * Allwinner code also asserts resets for NEON on A15. According
	 * to ARM manuals, asserting power-on reset is sufficient.
	 */
	if (!sunxi_core_is_cortex_a15(cpu, cluster))
		reg &= ~CPUCFG_CX_RST_CTRL_ETM_RST(cpu);

	writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));

	/* open power switch */
	sunxi_cpu_power_switch_set(cpu, cluster, true);

	/* Handle A83T bit swap */
	if (is_a83t) {
		if (cpu == 0)
			cpu = 4;
	}

	/* clear processor power gate */
	reg = readl(prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	reg &= ~PRCM_PWROFF_GATING_REG_CORE(cpu);
	writel(reg, prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	udelay(20);

	/* Handle A83T bit swap */
	if (is_a83t) {
		if (cpu == 4)
			cpu = 0;
	}

	/* de-assert processor power-on reset */
	reg = readl(prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));
	reg |= PRCM_CPU_PO_RST_CTRL_CORE(cpu);
	writel(reg, prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));

	if (is_a83t) {
		reg  = readl(r_cpucfg_base +
			     R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		reg |= R_CPUCFG_CLUSTER_PO_RST_CTRL_CORE(cpu);
		writel(reg, r_cpucfg_base +
		       R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		udelay(10);
	}

	/* de-assert all processor resets */
	reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
	reg |= CPUCFG_CX_RST_CTRL_DBG_RST(cpu);
	reg |= CPUCFG_CX_RST_CTRL_CORE_RST(cpu);
	if (!sunxi_core_is_cortex_a15(cpu, cluster))
		reg |= CPUCFG_CX_RST_CTRL_ETM_RST(cpu);
	else
		reg |= CPUCFG_CX_RST_CTRL_CX_RST(cpu); /* NEON */
	writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));

	return 0;
}

static int sunxi_cluster_powerup(unsigned int cluster)
{
	u32 reg;

	pr_debug("%s: cluster %u\n", __func__, cluster);
	if (cluster >= SUNXI_NR_CLUSTERS)
		return -EINVAL;

	/* For A83T, assert cluster cores resets */
	if (is_a83t) {
		reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
		reg &= ~CPUCFG_CX_RST_CTRL_CORE_RST_ALL;   /* Core Reset    */
		writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
		udelay(10);
	}

	/* assert ACINACTM */
	reg = readl(cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));
	reg |= CPUCFG_CX_CTRL_REG1_ACINACTM;
	writel(reg, cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));

	/* assert cluster processor power-on resets */
	reg = readl(prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));
	reg &= ~PRCM_CPU_PO_RST_CTRL_CORE_ALL;
	writel(reg, prcm_base + PRCM_CPU_PO_RST_CTRL(cluster));

	/* assert cluster cores resets */
	if (is_a83t) {
		reg  = readl(r_cpucfg_base +
			     R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		reg &= ~CPUCFG_CX_RST_CTRL_CORE_RST_ALL;
		writel(reg, r_cpucfg_base +
		       R_CPUCFG_CLUSTER_PO_RST_CTRL(cluster));
		udelay(10);
	}

	/* assert cluster resets */
	reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
	reg &= ~CPUCFG_CX_RST_CTRL_DBG_SOC_RST;
	reg &= ~CPUCFG_CX_RST_CTRL_DBG_RST_ALL;
	reg &= ~CPUCFG_CX_RST_CTRL_H_RST;
	reg &= ~CPUCFG_CX_RST_CTRL_L2_RST;

	/*
	 * Allwinner code also asserts resets for NEON on A15. According
	 * to ARM manuals, asserting power-on reset is sufficient.
	 */
	if (!sunxi_core_is_cortex_a15(0, cluster))
		reg &= ~CPUCFG_CX_RST_CTRL_ETM_RST_ALL;

	writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));

	/* hold L1/L2 reset disable signals low */
	reg = readl(cpucfg_base + CPUCFG_CX_CTRL_REG0(cluster));
	if (sunxi_core_is_cortex_a15(0, cluster)) {
		/* Cortex-A15: hold L2RSTDISABLE low */
		reg &= ~CPUCFG_CX_CTRL_REG0_L2_RST_DISABLE_A15;
	} else {
		/* Cortex-A7: hold L1RSTDISABLE and L2RSTDISABLE low */
		reg &= ~CPUCFG_CX_CTRL_REG0_L1_RST_DISABLE_ALL;
		reg &= ~CPUCFG_CX_CTRL_REG0_L2_RST_DISABLE_A7;
	}
	writel(reg, cpucfg_base + CPUCFG_CX_CTRL_REG0(cluster));

	/* clear cluster power gate */
	reg = readl(prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	if (is_a83t)
		reg &= ~PRCM_PWROFF_GATING_REG_CLUSTER_SUN8I;
	else
		reg &= ~PRCM_PWROFF_GATING_REG_CLUSTER_SUN9I;
	writel(reg, prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	udelay(20);

	/* de-assert cluster resets */
	reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
	reg |= CPUCFG_CX_RST_CTRL_DBG_SOC_RST;
	reg |= CPUCFG_CX_RST_CTRL_H_RST;
	reg |= CPUCFG_CX_RST_CTRL_L2_RST;
	writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));

	/* de-assert ACINACTM */
	reg = readl(cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));
	reg &= ~CPUCFG_CX_CTRL_REG1_ACINACTM;
	writel(reg, cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));

	return 0;
}

/*
 * This bit is shared between the initial nocache_trampoline call to
 * enable CCI-400 and proper cluster cache disable before power down.
 */
static void sunxi_cluster_cache_disable_without_axi(void)
{
	if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A15) {
		/*
		 * On the Cortex-A15 we need to disable
		 * L2 prefetching before flushing the cache.
		 */
		asm volatile(
		"mcr	p15, 1, %0, c15, c0, 3\n"
		"isb\n"
		"dsb"
		: : "r" (0x400));
	}

	/* Flush all cache levels for this cluster. */
	v7_exit_coherency_flush(all);

	/*
	 * Disable cluster-level coherency by masking
	 * incoming snoops and DVM messages:
	 */
	cci_disable_port_by_cpu(read_cpuid_mpidr());
}

static int sunxi_mc_smp_cpu_table[SUNXI_NR_CLUSTERS][SUNXI_CPUS_PER_CLUSTER];
int sunxi_mc_smp_first_comer;

static DEFINE_SPINLOCK(boot_lock);

static bool sunxi_mc_smp_cluster_is_down(unsigned int cluster)
{
	int i;

	for (i = 0; i < SUNXI_CPUS_PER_CLUSTER; i++)
		if (sunxi_mc_smp_cpu_table[cluster][i])
			return false;
	return true;
}

static void sunxi_mc_smp_secondary_init(unsigned int cpu)
{
	/* Clear hotplug support magic flags for cpu0 */
	if (cpu == 0)
		sunxi_cpu0_hotplug_support_set(false);
}

static int sunxi_mc_smp_boot_secondary(unsigned int l_cpu, struct task_struct *idle)
{
	unsigned int mpidr, cpu, cluster;

	mpidr = cpu_logical_map(l_cpu);
	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);

	if (!cpucfg_base)
		return -ENODEV;
	if (cluster >= SUNXI_NR_CLUSTERS || cpu >= SUNXI_CPUS_PER_CLUSTER)
		return -EINVAL;

	spin_lock_irq(&boot_lock);

	if (sunxi_mc_smp_cpu_table[cluster][cpu])
		goto out;

	if (sunxi_mc_smp_cluster_is_down(cluster)) {
		sunxi_mc_smp_first_comer = true;
		sunxi_cluster_powerup(cluster);
	} else {
		sunxi_mc_smp_first_comer = false;
	}

	/* This is read by incoming CPUs with their cache and MMU disabled */
	sync_cache_w(&sunxi_mc_smp_first_comer);
	sunxi_cpu_powerup(cpu, cluster);

out:
	sunxi_mc_smp_cpu_table[cluster][cpu]++;
	spin_unlock_irq(&boot_lock);

	return 0;
}

#ifdef CONFIG_HOTPLUG_CPU
static void sunxi_cluster_cache_disable(void)
{
	unsigned int cluster = MPIDR_AFFINITY_LEVEL(read_cpuid_mpidr(), 1);
	u32 reg;

	pr_debug("%s: cluster %u\n", __func__, cluster);

	sunxi_cluster_cache_disable_without_axi();

	/* last man standing, assert ACINACTM */
	reg = readl(cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));
	reg |= CPUCFG_CX_CTRL_REG1_ACINACTM;
	writel(reg, cpucfg_base + CPUCFG_CX_CTRL_REG1(cluster));
}

static void sunxi_mc_smp_cpu_die(unsigned int l_cpu)
{
	unsigned int mpidr, cpu, cluster;
	bool last_man;

	mpidr = cpu_logical_map(l_cpu);
	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
	pr_debug("%s: cluster %u cpu %u\n", __func__, cluster, cpu);

	spin_lock(&boot_lock);
	sunxi_mc_smp_cpu_table[cluster][cpu]--;
	if (sunxi_mc_smp_cpu_table[cluster][cpu] == 1) {
		/* A power_up request went ahead of us. */
		pr_debug("%s: aborting due to a power up request\n",
			 __func__);
		spin_unlock(&boot_lock);
		return;
	} else if (sunxi_mc_smp_cpu_table[cluster][cpu] > 1) {
		pr_err("Cluster %d CPU%d boots multiple times\n",
		       cluster, cpu);
		BUG();
	}

	last_man = sunxi_mc_smp_cluster_is_down(cluster);
	spin_unlock(&boot_lock);

	gic_cpu_if_down(0);
	if (last_man)
		sunxi_cluster_cache_disable();
	else
		v7_exit_coherency_flush(louis);

	for (;;)
		wfi();
}

static int sunxi_cpu_powerdown(unsigned int cpu, unsigned int cluster)
{
	u32 reg;

	pr_debug("%s: cluster %u cpu %u\n", __func__, cluster, cpu);
	if (cpu >= SUNXI_CPUS_PER_CLUSTER || cluster >= SUNXI_NR_CLUSTERS)
		return -EINVAL;

	/* gate processor power */
	reg = readl(prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	reg |= PRCM_PWROFF_GATING_REG_CORE(cpu);
	writel(reg, prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	udelay(20);

	/* close power switch */
	sunxi_cpu_power_switch_set(cpu, cluster, false);

	return 0;
}

static int sunxi_cluster_powerdown(unsigned int cluster)
{
	u32 reg;

	pr_debug("%s: cluster %u\n", __func__, cluster);
	if (cluster >= SUNXI_NR_CLUSTERS)
		return -EINVAL;

	/* assert cluster resets or system will hang */
	pr_debug("%s: assert cluster reset\n", __func__);
	reg = readl(cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));
	reg &= ~CPUCFG_CX_RST_CTRL_DBG_SOC_RST;
	reg &= ~CPUCFG_CX_RST_CTRL_H_RST;
	reg &= ~CPUCFG_CX_RST_CTRL_L2_RST;
	writel(reg, cpucfg_base + CPUCFG_CX_RST_CTRL(cluster));

	/* gate cluster power */
	pr_debug("%s: gate cluster power\n", __func__);
	reg = readl(prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	if (is_a83t)
		reg |= PRCM_PWROFF_GATING_REG_CLUSTER_SUN8I;
	else
		reg |= PRCM_PWROFF_GATING_REG_CLUSTER_SUN9I;
	writel(reg, prcm_base + PRCM_PWROFF_GATING_REG(cluster));
	udelay(20);

	return 0;
}

static int sunxi_mc_smp_cpu_kill(unsigned int l_cpu)
{
	unsigned int mpidr, cpu, cluster;
	unsigned int tries, count;
	int ret = 0;
	u32 reg;

	mpidr = cpu_logical_map(l_cpu);
	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);

	/* This should never happen */
	if (WARN_ON(cluster >= SUNXI_NR_CLUSTERS ||
		    cpu >= SUNXI_CPUS_PER_CLUSTER))
		return 0;

	/* wait for CPU core to die and enter WFI */
	count = TIMEOUT_USEC / POLL_USEC;
	spin_lock_irq(&boot_lock);
	for (tries = 0; tries < count; tries++) {
		spin_unlock_irq(&boot_lock);
		usleep_range(POLL_USEC / 2, POLL_USEC);
		spin_lock_irq(&boot_lock);

		/*
		 * If the user turns off a bunch of cores at the same
		 * time, the kernel might call cpu_kill before some of
		 * them are ready. This is because boot_lock serializes
		 * both cpu_die and cpu_kill callbacks. Either one could
		 * run first. We should wait for cpu_die to complete.
		 */
		if (sunxi_mc_smp_cpu_table[cluster][cpu])
			continue;

		reg = readl(cpucfg_base + CPUCFG_CX_STATUS(cluster));
		if (reg & CPUCFG_CX_STATUS_STANDBYWFI(cpu))
			break;
	}

	if (tries >= count) {
		ret = ETIMEDOUT;
		goto out;
	}

	/* power down CPU core */
	sunxi_cpu_powerdown(cpu, cluster);

	if (!sunxi_mc_smp_cluster_is_down(cluster))
		goto out;

	/* wait for cluster L2 WFI */
	ret = readl_poll_timeout(cpucfg_base + CPUCFG_CX_STATUS(cluster), reg,
				 reg & CPUCFG_CX_STATUS_STANDBYWFIL2,
				 POLL_USEC, TIMEOUT_USEC);
	if (ret) {
		/*
		 * Ignore timeout on the cluster. Leaving the cluster on
		 * will not affect system execution, just use a bit more
		 * power. But returning an error here will only confuse
		 * the user as the CPU has already been shutdown.
		 */
		ret = 0;
		goto out;
	}

	/* Power down cluster */
	sunxi_cluster_powerdown(cluster);

out:
	spin_unlock_irq(&boot_lock);
	pr_debug("%s: cluster %u cpu %u powerdown: %d\n",
		 __func__, cluster, cpu, ret);
	return !ret;
}

static bool sunxi_mc_smp_cpu_can_disable(unsigned int cpu)
{
	/* CPU0 hotplug not handled for sun8i-a83t */
	if (is_a83t)
		if (cpu == 0)
			return false;
	return true;
}
#endif

static const struct smp_operations sunxi_mc_smp_smp_ops __initconst = {
	.smp_secondary_init	= sunxi_mc_smp_secondary_init,
	.smp_boot_secondary	= sunxi_mc_smp_boot_secondary,
#ifdef CONFIG_HOTPLUG_CPU
	.cpu_die		= sunxi_mc_smp_cpu_die,
	.cpu_kill		= sunxi_mc_smp_cpu_kill,
	.cpu_can_disable	= sunxi_mc_smp_cpu_can_disable,
#endif
};

static bool __init sunxi_mc_smp_cpu_table_init(void)
{
	unsigned int mpidr, cpu, cluster;

	mpidr = read_cpuid_mpidr();
	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);

	if (cluster >= SUNXI_NR_CLUSTERS || cpu >= SUNXI_CPUS_PER_CLUSTER) {
		pr_err("%s: boot CPU is out of bounds!\n", __func__);
		return false;
	}
	sunxi_mc_smp_cpu_table[cluster][cpu] = 1;
	return true;
}

/*
 * Adapted from arch/arm/common/mc_smp_entry.c
 *
 * We need the trampoline code to enable CCI-400 on the first cluster
 */
typedef typeof(cpu_reset) phys_reset_t;

static int __init nocache_trampoline(unsigned long __unused)
{
	phys_reset_t phys_reset;

	setup_mm_for_reboot();
	sunxi_cluster_cache_disable_without_axi();

	phys_reset = (phys_reset_t)(unsigned long)__pa_symbol(cpu_reset);
	phys_reset(__pa_symbol(sunxi_mc_smp_resume), false);
	BUG();
}

static int __init sunxi_mc_smp_loopback(void)
{
	int ret;

	/*
	 * We're going to soft-restart the current CPU through the
	 * low-level MCPM code by leveraging the suspend/resume
	 * infrastructure. Let's play it safe by using cpu_pm_enter()
	 * in case the CPU init code path resets the VFP or similar.
	 */
	sunxi_mc_smp_first_comer = true;
	local_irq_disable();
	local_fiq_disable();
	ret = cpu_pm_enter();
	if (!ret) {
		ret = cpu_suspend(0, nocache_trampoline);
		cpu_pm_exit();
	}
	local_fiq_enable();
	local_irq_enable();
	sunxi_mc_smp_first_comer = false;

	return ret;
}

/*
 * This holds any device nodes that we requested resources for,
 * so that we may easily release resources in the error path.
 */
struct sunxi_mc_smp_nodes {
	struct device_node *prcm_node;
	struct device_node *cpucfg_node;
	struct device_node *sram_node;
	struct device_node *r_cpucfg_node;
};

/* This structure holds SoC-specific bits tied to an enable-method string. */
struct sunxi_mc_smp_data {
	const char *enable_method;
	int (*get_smp_nodes)(struct sunxi_mc_smp_nodes *nodes);
	bool is_a83t;
};

static void __init sunxi_mc_smp_put_nodes(struct sunxi_mc_smp_nodes *nodes)
{
	of_node_put(nodes->prcm_node);
	of_node_put(nodes->cpucfg_node);
	of_node_put(nodes->sram_node);
	of_node_put(nodes->r_cpucfg_node);
	memset(nodes, 0, sizeof(*nodes));
}

static int __init sun9i_a80_get_smp_nodes(struct sunxi_mc_smp_nodes *nodes)
{
	nodes->prcm_node = of_find_compatible_node(NULL, NULL,
						   "allwinner,sun9i-a80-prcm");
	if (!nodes->prcm_node) {
		pr_err("%s: PRCM not available\n", __func__);
		return -ENODEV;
	}

	nodes->cpucfg_node = of_find_compatible_node(NULL, NULL,
						     "allwinner,sun9i-a80-cpucfg");
	if (!nodes->cpucfg_node) {
		pr_err("%s: CPUCFG not available\n", __func__);
		return -ENODEV;
	}

	nodes->sram_node = of_find_compatible_node(NULL, NULL,
						   "allwinner,sun9i-a80-smp-sram");
	if (!nodes->sram_node) {
		pr_err("%s: Secure SRAM not available\n", __func__);
		return -ENODEV;
	}

	return 0;
}

static int __init sun8i_a83t_get_smp_nodes(struct sunxi_mc_smp_nodes *nodes)
{
	nodes->prcm_node = of_find_compatible_node(NULL, NULL,
						   "allwinner,sun8i-a83t-r-ccu");
	if (!nodes->prcm_node) {
		pr_err("%s: PRCM not available\n", __func__);
		return -ENODEV;
	}

	nodes->cpucfg_node = of_find_compatible_node(NULL, NULL,
						     "allwinner,sun8i-a83t-cpucfg");
	if (!nodes->cpucfg_node) {
		pr_err("%s: CPUCFG not available\n", __func__);
		return -ENODEV;
	}

	nodes->r_cpucfg_node = of_find_compatible_node(NULL, NULL,
						       "allwinner,sun8i-a83t-r-cpucfg");
	if (!nodes->r_cpucfg_node) {
		pr_err("%s: RCPUCFG not available\n", __func__);
		return -ENODEV;
	}

	return 0;
}

static const struct sunxi_mc_smp_data sunxi_mc_smp_data[] __initconst = {
	{
		.enable_method	= "allwinner,sun9i-a80-smp",
		.get_smp_nodes	= sun9i_a80_get_smp_nodes,
	},
	{
		.enable_method	= "allwinner,sun8i-a83t-smp",
		.get_smp_nodes	= sun8i_a83t_get_smp_nodes,
		.is_a83t	= true,
	},
};

static int __init sunxi_mc_smp_init(void)
{
	struct sunxi_mc_smp_nodes nodes = { 0 };
	struct device_node *node;
	struct resource res;
	void __iomem *addr;
	int i, ret;

	/*
	 * Don't bother checking the "cpus" node, as an enable-method
	 * property in that node is undocumented.
	 */
	node = of_cpu_device_node_get(0);
	if (!node)
		return -ENODEV;

	/*
	 * We can't actually use the enable-method magic in the kernel.
	 * Our loopback / trampoline code uses the CPU suspend framework,
	 * which requires the identity mapping be available. It would not
	 * yet be available if we used the .init_cpus or .prepare_cpus
	 * callbacks in smp_operations, which we would use if we were to
	 * use CPU_METHOD_OF_DECLARE
	 */
	for (i = 0; i < ARRAY_SIZE(sunxi_mc_smp_data); i++) {
		ret = of_property_match_string(node, "enable-method",
					       sunxi_mc_smp_data[i].enable_method);
		if (!ret)
			break;
	}

	is_a83t = sunxi_mc_smp_data[i].is_a83t;

	of_node_put(node);
	if (ret)
		return -ENODEV;

	if (!sunxi_mc_smp_cpu_table_init())
		return -EINVAL;

	if (!cci_probed()) {
		pr_err("%s: CCI-400 not available\n", __func__);
		return -ENODEV;
	}

	/* Get needed device tree nodes */
	ret = sunxi_mc_smp_data[i].get_smp_nodes(&nodes);
	if (ret)
		goto err_put_nodes;

	/*
	 * Unfortunately we can not request the I/O region for the PRCM.
	 * It is shared with the PRCM clock.
	 */
	prcm_base = of_iomap(nodes.prcm_node, 0);
	if (!prcm_base) {
		pr_err("%s: failed to map PRCM registers\n", __func__);
		ret = -ENOMEM;
		goto err_put_nodes;
	}

	cpucfg_base = of_io_request_and_map(nodes.cpucfg_node, 0,
					    "sunxi-mc-smp");
	if (IS_ERR(cpucfg_base)) {
		ret = PTR_ERR(cpucfg_base);
		pr_err("%s: failed to map CPUCFG registers: %d\n",
		       __func__, ret);
		goto err_unmap_prcm;
	}

	if (is_a83t) {
		r_cpucfg_base = of_io_request_and_map(nodes.r_cpucfg_node,
						      0, "sunxi-mc-smp");
		if (IS_ERR(r_cpucfg_base)) {
			ret = PTR_ERR(r_cpucfg_base);
			pr_err("%s: failed to map R-CPUCFG registers\n",
			       __func__);
			goto err_unmap_release_cpucfg;
		}
	} else {
		sram_b_smp_base = of_io_request_and_map(nodes.sram_node, 0,
							"sunxi-mc-smp");
		if (IS_ERR(sram_b_smp_base)) {
			ret = PTR_ERR(sram_b_smp_base);
			pr_err("%s: failed to map secure SRAM\n", __func__);
			goto err_unmap_release_cpucfg;
		}
	}

	/* Configure CCI-400 for boot cluster */
	ret = sunxi_mc_smp_loopback();
	if (ret) {
		pr_err("%s: failed to configure boot cluster: %d\n",
		       __func__, ret);
		goto err_unmap_release_sram_rcpucfg;
	}

	/* We don't need the device nodes anymore */
	sunxi_mc_smp_put_nodes(&nodes);

	/* Set the hardware entry point address */
	if (is_a83t)
		addr = r_cpucfg_base + R_CPUCFG_CPU_SOFT_ENTRY_REG;
	else
		addr = prcm_base + PRCM_CPU_SOFT_ENTRY_REG;
	writel(__pa_symbol(sunxi_mc_smp_secondary_startup), addr);

	/* Actually enable multi cluster SMP */
	smp_set_ops(&sunxi_mc_smp_smp_ops);

	pr_info("sunxi multi cluster SMP support installed\n");

	return 0;

err_unmap_release_sram_rcpucfg:
	if (is_a83t) {
		iounmap(r_cpucfg_base);
		of_address_to_resource(nodes.r_cpucfg_node, 0, &res);
	} else {
		iounmap(sram_b_smp_base);
		of_address_to_resource(nodes.sram_node, 0, &res);
	}
	release_mem_region(res.start, resource_size(&res));
err_unmap_release_cpucfg:
	iounmap(cpucfg_base);
	of_address_to_resource(nodes.cpucfg_node, 0, &res);
	release_mem_region(res.start, resource_size(&res));
err_unmap_prcm:
	iounmap(prcm_base);
err_put_nodes:
	sunxi_mc_smp_put_nodes(&nodes);
	return ret;
}

early_initcall(sunxi_mc_smp_init);