ARM: shmobile: rcar-gen2: Make sure CNTVOFF is initialized on CA7/15

On Cortex-A7, the arch timer CNTVOFF register is uninitialized.
Ideally it should be initialized by the boot loader, but it isn't.

For the boot CPU, CNTVOFF is initialized by Linux since commit
9ce3fa6816c2fb59 ("ARM: shmobile: rcar-gen2: Add CA7 arch_timer
initialization for r8a7794").
For secondary CPU cores, no such initialization is done.

Hence when enabling SMP on r8a7794, the kernel log is spammed with:

    WARNING: Underflow in clocksource 'arch_sys_counter' observed, time update ignored.
	     Please report this, consider using a different clocksource, if possible.
	     Your kernel is probably still fine.

As Marc Zyngier pointed out that Cortex-A15 and Cortex-A7 are similar with
respect to CNTVOFF, we have been very lucky this just worked on R-Car
Gen2 SoCs with Cortex-A15 cores.

To fix this:
  - Move the existing inline asm code to initialize CNTVOFF to an
    assembler source file (adding comments and replacing hardcoded
    constants by definitions in the process), so it can be reused,
  - Perform the initialization of CNTVOFF on the boot CPU (Cortex-A15 or
    Cortex-A7) on all R-Car Gen2 and RZ/G1 parts,
  - Wrap the standard secondary_startup() routine inside a routine which
    initializes CNTVOFF.

Based on patches by Hisashi Nakamura in the BSP.

Signed-off-by: Geert Uytterhoeven <geert+renesas@glider.be>
Tested-by: Fabrizio Castro <fabrizio.castro@bp.renesas.com>
Signed-off-by: Simon Horman <horms+renesas@verge.net.au>

1 files changed, 2 insertions, 18 deletions

diff --git a/arch/arm/mach-shmobile/setup-rcar-gen2.c b/arch/arm/mach-shmobile/setup-rcar-gen2.c
index 7ab1690fab82..5561dbed7a33 100644
--- a/arch/arm/mach-shmobile/setup-rcar-gen2.c
+++ b/arch/arm/mach-shmobile/setup-rcar-gen2.c
@@ -70,28 +70,12 @@ void __init rcar_gen2_timer_init(void)
 	void __iomem *base;
 	u32 freq;
 
+	shmobile_init_cntvoff();
+
 	if (of_machine_is_compatible("renesas,r8a7745") ||
 	    of_machine_is_compatible("renesas,r8a7792") ||
 	    of_machine_is_compatible("renesas,r8a7794")) {
 		freq = 260000000 / 8;	/* ZS / 8 */
-		/* CNTVOFF has to be initialized either from non-secure
-		 * Hypervisor mode or secure Monitor mode with SCR.NS==1.
-		 * If TrustZone is enabled then it should be handled by the
-		 * secure code.
-		 */
-		asm volatile(
-		"	cps	0x16\n"
-		"	mrc	p15, 0, r1, c1, c1, 0\n"
-		"	orr	r0, r1, #1\n"
-		"	mcr	p15, 0, r0, c1, c1, 0\n"
-		"	isb\n"
-		"	mov	r0, #0\n"
-		"	mcrr	p15, 4, r0, r0, c14\n"
-		"	isb\n"
-		"	mcr	p15, 0, r1, c1, c1, 0\n"
-		"	isb\n"
-		"	cps	0x13\n"
-			: : : "r0", "r1");
 	} else {
 		/* At Linux boot time the r8a7790 arch timer comes up
 		 * with the counter disabled. Moreover, it may also report