arm: init CNTVOFF

There is an property "arm,cpu-registers-not-fw-configured" in Linux side,
that could workaround the issue that firmare initialize CNTVOFF.

But if use that property, virtualization support will be break in linux.

Also without CNTVOFF or that property no defined, kernel could not
boot up on i.MX7D with two cores.

So we init CNTVOFF in OP-TEE to make kernel work well.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>

core/arch/arm/include/arm32.h

@@ -179,6 +179,12 @@
 /* Valid if FSR.LPAE is 0 */
 #define FSR_FS_MASK		(BIT32(10) | (BIT32(4) - 1))
 
+/* ID_PFR1 bit fields */
+#define IDPFR1_VIRT_SHIFT            12
+#define IDPFR1_VIRT_MASK             (0xF << IDPFR1_VIRT_SHIFT)
+#define IDPFR1_GENTIMER_SHIFT        16
+#define IDPFR1_GENTIMER_MASK         (0xF << IDPFR1_GENTIMER_SHIFT)
+
 #ifndef ASM
 static inline uint32_t read_mpidr(void)
 {

core/arch/arm/include/arm32_macros.S

@@ -40,6 +40,10 @@
 	mrc	p15, 0, \reg, c0, c0, 5
 	.endm
 
+	.macro read_idpfr1 reg
+	mrc	p15, 0, \reg, c0, c1, 1
+	.endm
+
 	.macro read_sctlr reg
 	mrc	p15, 0, \reg, c1, c0, 0
 	.endm
@@ -270,6 +274,10 @@
 	mrc	p15, 0, \reg, c13, c0, 4
 	.endm
 
+	.macro write_cntvoff reg0, reg1
+	mcrr  p15, 4, \reg0, \reg1, c14
+	.endm
+
 	.macro read_clidr reg
 	/* Cache Level ID Register */
 	mrc	p15, 1, \reg, c0, c0, 1

core/arch/arm/plat-imx/conf.mk

@@ -80,6 +80,7 @@ include core/arch/arm/cpu/cortex-a7.mk
 
 $(call force,CFG_SECURE_TIME_SOURCE_REE,y)
 CFG_BOOT_SECONDARY_REQUEST ?= y
+CFG_INIT_CNTVOFF ?= y
 endif
 
 ifneq (,$(filter $(PLATFORM_FLAVOR),mx6sxsabreauto))

core/arch/arm/sm/sm_a32.S

@@ -312,6 +312,38 @@ UNWIND(	.fnstart)
 	cps	#CPSR_MODE_MON
 	/* Point just beyond sm_ctx.sec */
 	sub	sp, r0, #(SM_CTX_SIZE - SM_CTX_NSEC)
+
+#ifdef CFG_INIT_CNTVOFF
+	read_scr r0
+	orr	r0, r0, #SCR_NS /* Set NS bit in SCR */
+	write_scr r0
+	isb
+
+	/*
+	 * Accessing CNTVOFF:
+	 * If the implementation includes the Virtualization Extensions
+	 * this is a RW register, accessible from Hyp mode, and
+	 * from Monitor mode when SCR.NS is set to 1.
+	 * If the implementation includes the Security Extensions
+	 * but not the Virtualization Extensions, an MCRR or MRRC to
+	 * the CNTVOFF encoding is UNPREDICTABLE if executed in Monitor
+	 * mode, regardless of the value of SCR.NS.
+	 */
+	read_idpfr1 r2
+	mov	r3, r2
+	ands    r3, r3, #IDPFR1_GENTIMER_MASK
+	beq	.no_gentimer
+	ands    r2, r2, #IDPFR1_VIRT_MASK
+	beq	.no_gentimer
+	mov	r2, #0
+	write_cntvoff r2, r2
+
+.no_gentimer:
+	bic	r0, r0, #SCR_NS /* Clr NS bit in SCR */
+	write_scr r0
+	isb
+#endif
+
 	msr	cpsr, r1
 
 	/* Set monitor vector (MVBAR) */