From 773d25e3ef709fbb7ec5ac3299d7a9776185a865 Mon Sep 17 00:00:00 2001
From: Fan Rong <cinifr@gmail.com>
Date: Mon, 23 Sep 2013 22:29:56 +0800
Subject: [PATCH] Add setting CNTVOFF and CNTFRQ for all smp cpus

Signed-off-by: Fan Rong <cinifr@gmail.com>
---
 arch/arm/cpu/armv7/sunxi/Makefile         |   2 +
 arch/arm/cpu/armv7/sunxi/monitor.S        |  97 ++++++++
 arch/arm/cpu/armv7/sunxi/secondary_init.S |   2 +-
 arch/arm/cpu/armv7/sunxi/smp.c            |   9 +-
 arch/arm/cpu/armv7/sunxi/vir.c            | 398 ++++++++++++++++++++++++++++++
 board/sunxi/board.c                       |   3 +
 include/configs/sun7i.h                   |   3 +
 7 files changed, 510 insertions(+), 4 deletions(-)
 mode change 100644 => 100755 arch/arm/cpu/armv7/sunxi/Makefile
 create mode 100755 arch/arm/cpu/armv7/sunxi/monitor.S
 mode change 100644 => 100755 arch/arm/cpu/armv7/sunxi/secondary_init.S
 mode change 100644 => 100755 arch/arm/cpu/armv7/sunxi/smp.c
 create mode 100755 arch/arm/cpu/armv7/sunxi/vir.c
 mode change 100644 => 100755 include/configs/sun7i.h

diff --git a/arch/arm/cpu/armv7/sunxi/Makefile b/arch/arm/cpu/armv7/sunxi/Makefile
old mode 100644
new mode 100755
index 115181f..1ff5c21
--- a/arch/arm/cpu/armv7/sunxi/Makefile
+++ b/arch/arm/cpu/armv7/sunxi/Makefile
@@ -36,6 +36,8 @@ COBJS	+= board.o
 COBJS	+= clock.o
 COBJS	+= pinmux.o
 COBJS	+= watchdog.o
+COBJS	+= vir.o
+COBJS	+= monitor.o
 ifdef DEBUG
 COBJS	+= early_print.o
 endif
diff --git a/arch/arm/cpu/armv7/sunxi/monitor.S b/arch/arm/cpu/armv7/sunxi/monitor.S
new file mode 100755
index 0000000..8d52ad9
--- /dev/null
+++ b/arch/arm/cpu/armv7/sunxi/monitor.S
@@ -0,0 +1,97 @@
+/*
+ *
+ * Copyright (C) 2013 Fan Rong <cinifr@gmail.com>.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ */
+
+
+
+	.text
+	.pushsection    .hyp.idmap.text,"ax"
+	.align 5
+	.arch_extension virt
+
+
+
+/******************************************************************************
+cini
+******************************************************************************/
+	.align 5
+__vir_init_start:
+	.globl __vir_init_start
+
+__vir_init_monitor_vector:
+	.globl __vir_init_monitor_vector
+	
+	@ Monitor-mode exception vector
+	.word 0	/* reset */
+	.word 0	/* undef */
+	b	mon_smc
+	.word 0 /* pabt */
+	.word 0 /* dabt */
+	b	hyp_hvc
+	.word 0 /* irq */
+	.word 0 /* fiq */
+	.align
+mon_smc:
+	mov	sp,r1	
+	push	{lr}
+	mov	lr,r0	
+	blx	lr
+	pop	{lr}	
+	movs	pc, lr
+	
+	@push	{lr}
+	@mov	lr, r0
+
+	@blx	lr
+	@pop	{lr}
+	movs	pc, lr
+	
+	.align 5
+__vir_init_hyp_vector:
+	.globl __vir_init_hyp_vector
+	
+	@ Monitor-mode exception vector
+	.word 0x1234	/* reset */
+	.word 0x5678	/* undef */
+	.word 0 /* pabt */
+	b	hyp_hyt
+	.word 0 /* dabt */
+	b	hyp_hyt
+	.word 0 /* irq */
+	.word 0 /* fiq */
+	.align
+
+
+hyp_hvc:
+	eret
+	b	hyp_hvc
+
+
+hyp_hyt:
+	mov	sp,r1
+	mov	lr,r0	
+	blx	lr
+	eret
+
+	
+	push	{lr}
+	mov	lr,r0	
+	blx	lr
+	pop	{lr}
+	@b	hyp_hyt
+
+
+
+	.ltorg
+
+	.globl __vir_hyp_init_end
+__vir_hyp_init_end:
+
+__vir_init_end:
+	.globl __vir_init_end
+
+	.popsection
diff --git a/arch/arm/cpu/armv7/sunxi/secondary_init.S b/arch/arm/cpu/armv7/sunxi/secondary_init.S
old mode 100644
new mode 100755
index 034acde..af66355
--- a/arch/arm/cpu/armv7/sunxi/secondary_init.S
+++ b/arch/arm/cpu/armv7/sunxi/secondary_init.S
@@ -37,7 +37,7 @@ ENTRY(secondary_init)
 	 * Setup a secondary stack, each core gets 128 bytes.
 	 */
 	ldr	sp, =secondary_stack
-	mov	r0, #0x80
+	mov	r0, #0x1000
 	add	sp, sp, r0, lsl r5
 
 	/*
diff --git a/arch/arm/cpu/armv7/sunxi/smp.c b/arch/arm/cpu/armv7/sunxi/smp.c
old mode 100644
new mode 100755
index 59717a2..fd200f5
--- a/arch/arm/cpu/armv7/sunxi/smp.c
+++ b/arch/arm/cpu/armv7/sunxi/smp.c
@@ -32,7 +32,6 @@
 #else
 #error unsupported SoC
 #endif
-
 static void secondary_pen(void)
 {
 	struct sunxi_cpucfg *cpucfg = (struct sunxi_cpucfg *)SUNXI_CPUCFG_BASE;
@@ -50,10 +49,14 @@ static void secondary_pen(void)
 	};
 }
 
-u32 secondary_stack[32*(NUM_CORES-1)];
+u32 secondary_stack[0x400*(NUM_CORES-1)];
 
 void secondary_start(void)
 {
+	/* CNTFRQ == 24 MHz */
+	asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r"(24000000));
+
+	vir_init();
 	secondary_pen();
 }
 
@@ -62,7 +65,7 @@ void startup_secondaries(void)
 {
 	int i;
 	struct sunxi_cpucfg *cpucfg = (struct sunxi_cpucfg *)SUNXI_CPUCFG_BASE;
-
+	printf("startup_secondaries\n");
 	writel((u32)secondary_init, &cpucfg->boot_addr);
 
 	for (i = 1; i < NUM_CORES; i++) {
diff --git a/arch/arm/cpu/armv7/sunxi/vir.c b/arch/arm/cpu/armv7/sunxi/vir.c
new file mode 100755
index 0000000..02309e0
--- /dev/null
+++ b/arch/arm/cpu/armv7/sunxi/vir.c
@@ -0,0 +1,398 @@
+/*
+ * (C) Copyright 2013 Fan Rong <cinifr@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+
+
+#include <common.h>
+
+
+volatile unsigned int dist_addr; 
+volatile unsigned int cpu_addr; 
+
+
+
+/* Hyp System Control Register (HSCTLR) bits */
+#define HSCTLR_TE	(1 << 30)
+#define HSCTLR_EE	(1 << 25)
+#define HSCTLR_FI	(1 << 21)
+#define HSCTLR_WXN	(1 << 19)
+#define HSCTLR_I	(1 << 12)
+#define HSCTLR_C	(1 << 2)
+#define HSCTLR_A	(1 << 1)
+#define HSCTLR_M	1
+#define HSCTLR_MASK	(HSCTLR_M | HSCTLR_A | HSCTLR_C | HSCTLR_I | \
+			 HSCTLR_WXN | HSCTLR_FI | HSCTLR_EE | HSCTLR_TE)
+
+
+
+extern char __vir_init_monitor_vector[];
+
+
+static char smc_stack[2048];
+
+int i;
+
+#define VIR_INFO(fmt, ...)   \
+printf("[%s] " fmt,__FUNCTION__,## __VA_ARGS__)
+
+#define VIR_INFO(fmt, ...) 
+
+
+#define FUN_SPE(regname,...)	\
+void write_##regname(unsigned int tmp)	\
+{	\
+	asm	\
+	(	\
+		"mcr "# __VA_ARGS__	\
+		:	\
+		:"r"(tmp)	\
+		:"memory"	\
+	);	\
+	VIR_INFO("0x%x\r\n",tmp);	\	
+}	\
+unsigned int read_##regname(void)	\
+{	\
+	unsigned int tmp;	\
+	asm \
+	(	\
+		"mrc "# __VA_ARGS__ \
+		:"=r"(tmp)	\
+		:	\
+		:"memory"	\
+	);	\
+	VIR_INFO("0x%x\r\n",tmp);	\	
+	return tmp;	\
+}
+
+
+
+
+
+#define FUN_SPE__NO_PRINT(regname,...)	\
+void write_##regname(int tmp)	\
+{	\
+	asm	\
+	(	\
+		"mcr "# __VA_ARGS__	\
+		:	\
+		:"r"(tmp)	\
+		:"memory"	\
+	);	\
+}	\
+unsigned int read_##regname(void)	\
+{	\
+	unsigned int tmp;	\
+	asm \
+	(	\
+		"mrc "# __VA_ARGS__ \
+		:"=r"(tmp)	\
+		:	\
+		:"memory"	\
+	);	\
+	return tmp;	\
+}
+
+
+FUN_SPE(sctlr,p15,0,%0,c1,c0,0)
+FUN_SPE(vbar,p15,0,%0,c12,c0,0)
+FUN_SPE(mvbar,p15,0,%0,c12,c0,1)
+FUN_SPE(hvbar,p15,4,%0,c12,c0,0)
+FUN_SPE(scr,p15,0,%0,c1,c1,0)
+FUN_SPE(hcr,p15,4,%0,c1,c1,0)
+FUN_SPE(hdcr,p15,4,%0,c1,c1,1)
+
+FUN_SPE(hsctlr,p15,4,%0,c1,c0,0)
+FUN_SPE(hcptr,p15,4,%0,c1,c1,2)
+FUN_SPE(hstr,p15,4,%0,c1,c1,3)
+
+FUN_SPE(vpidr,p15,4,%0,c0,c0,0)
+FUN_SPE(vmpidr,p15,4,%0,c0,c0,5)
+
+FUN_SPE(mpidr,p15,0,%0,c0,c0,5)
+
+
+FUN_SPE(ttbr0,p15,0,%0,c2,c0,0)
+FUN_SPE(ttbr1,p15,0,%0,c2,c0,1)
+FUN_SPE(ttbcr,p15,0,%0,c2,c0,2)
+FUN_SPE(par,p15,0,%0,c7, c4, 0)
+
+FUN_SPE(prrr,p15,0,%0,c10,c2,0)
+FUN_SPE(nmrr,p15,0,%0,c10,c2,1)
+
+FUN_SPE(fcseidr,p15,0,%0,c13,c0,0)
+FUN_SPE(contextidr,p15,0,%0,c13,c0,1)
+FUN_SPE(tpidrurw,p15,0,%0,c13,c0,2)
+FUN_SPE(tpidruro,p15,0,%0,c13,c0,3)
+FUN_SPE(tpidrprw,p15,0,%0,c13,c0,4)
+
+
+FUN_SPE(csselr,p15,2,%0,c0,c0,0)
+FUN_SPE(dacr,p15,0,%0,c3,c0,0)
+
+FUN_SPE(dfsr,p15,0,%0,c5,c0,0)
+FUN_SPE(ifsr,p15,0,%0,c5,c0,1)
+
+
+FUN_SPE(dfar,p15,0,%0,c6,c0,0)
+FUN_SPE(ifar,p15,0,%0,c6,c0,2)
+
+FUN_SPE(nsacr,p15,0,%0,c1,c1,2)
+
+
+FUN_SPE__NO_PRINT(dccisw,p15,0,%0,c7,c14,2)
+
+FUN_SPE(tlbiall,p15,0,%0,c8,c7,0)
+
+FUN_SPE(cpacr,p15,0,%0,c1,c0,2)
+
+
+
+
+
+
+int write_cpsr(int tmp)
+{
+	asm
+	(
+		"msr	cpsr,%0"
+		:
+		:"r"(tmp)
+		:"memory"
+	);
+	VIR_INFO("VIR: write_cpsr 0x%x\r\n",tmp);		
+	return tmp;
+}
+
+int read_cpsr(void)
+{
+	int tmp;
+	asm
+	(
+		"mrs	%0,cpsr"
+		:"=r"(tmp)
+		:
+		:"memory"
+	);
+	VIR_INFO("VIR: read_cpsr 0x%x\r\n",tmp);		
+	return tmp;
+}
+
+
+int write_spsr(int tmp)
+{
+	asm
+	(
+		"msr	spsr,%0"
+		:
+		:"r"(tmp)
+		:"memory"
+	);
+	
+	VIR_INFO("VIR: write_spsr 0x%x\r\n",tmp);
+	return tmp;
+}
+
+int read_spsr(void)
+{
+	int tmp;
+	asm
+	(
+		"mrs	%0,spsr"
+		:"=r"(tmp)
+		:
+		:"memory"
+	);
+	
+	VIR_INFO("VIR: read_spsr 0x%x\r\n",tmp);
+	return tmp;
+}
+
+
+
+int write_cntvoff(int tmp1,int tmp2)
+{
+
+	asm
+	(
+		"mcrr	p15, 4, %0,%1,c14"
+		:
+		:"r"(tmp1),"r"(tmp2)		
+		:"memory"
+	);
+	
+	VIR_INFO("0x%x 0x%x\r\n",tmp1,tmp2);
+	return 0;
+}
+
+int read_cntvoff(int *tmp1,int *tmp2)
+{
+	int tmp;
+	asm
+	(
+		"mrrc	p15, 4, %0,%1,c14"
+		:"=r"(*tmp1),"=r"(*tmp2)		
+		:
+		:"memory"
+	);
+	
+	VIR_INFO("0x%x 0x%x\r\n",*tmp1,*tmp2);
+	return 0;
+}
+
+
+
+
+int write_httbr(int tmp1,int tmp2)
+{
+
+	asm
+	(
+		"mcrr	p15, 4, %0,%1,c2"
+		:
+		:"r"(tmp1),"r"(tmp2)		
+		:"memory"
+	);
+	
+	VIR_INFO("0x%x 0x%x\r\n",tmp1,tmp2);
+	return 0;
+}
+
+int read_httbr(int *tmp1,int *tmp2)
+{
+	int tmp;
+	asm
+	(
+		"mrrc	p15, 4, %0,%1,c2"
+		:"=r"(*tmp1),"=r"(*tmp2)		
+		:
+		:"memory"
+	);
+	
+	VIR_INFO("0x%x 0x%x\r\n",*tmp1,*tmp2);
+	return 0;
+}
+
+
+
+int read_sp(void)
+{
+	int tmp;
+	asm
+	(
+		"mov	%0,sp"
+		:"=r"(tmp)
+		:
+		:"memory"
+	);
+	VIR_INFO("0x%x\r\n",tmp);	
+	return tmp;
+}
+
+
+
+int call_smc(void *fn, ...)
+{
+	asm
+	(
+		"mov r0,%0"
+		:
+		:"r"(fn)
+		:"memory"
+
+	);
+	asm
+	(
+		".arch_extension sec\n\t"	
+		"mov r7,#0\n\t"
+		"mcr p15, 0, r7, c7, c5, 6\n\t"
+		"dsb\n\t"
+		"dmb\n\t"
+		"smc #0\n\t"	
+	);
+	read_cpsr();
+	return 0;
+}
+
+
+int call_hvc(void *fn, ...)
+{
+	asm
+	(	
+		".arch_extension virt \n\t"
+		"isb	\n\t"
+		"hvc #0"
+	);
+	VIR_INFO("VIR: call_hvc \r\n");
+	return 0;
+}
+
+#define VIR_INFO(fmt, ...) 
+
+void smc_handler(void)
+{
+
+	unsigned int tmp1,tmp2;
+	unsigned int tmp,ttbr0,ttbr1,ttbcr,par,vbar,prrr,nmrr;
+	unsigned int dfsr,ifsr,dfar,ifar,csselr,dacr,fcseidr,contextidr,tpidrurw,tpidruro,tpidrprw;
+	
+	unsigned int scr;
+	unsigned int pgd_low,pgd_high;
+
+
+	scr = read_scr();
+
+	write_scr(scr|1);	//set to non secure bit
+	/* now cpu in monitor with NS=1*/
+
+	read_cntvoff(&tmp1,&tmp2);
+
+	tmp1 = 0;
+	tmp2 = 0;
+	
+	write_cntvoff(tmp1,tmp2);
+	
+	read_cntvoff(&tmp1,&tmp2);
+
+	write_scr(scr);
+	
+	read_scr();
+	return 0;
+
+
+	
+}
+
+
+int vir_init()
+{
+	int result;
+	unsigned int cpsr,sctlr;	
+	unsigned int contextidr;
+	int i=0;
+	//printf("vir_init mpidr 0x%x\n",read_mpidr());
+	read_ttbr0();
+	read_ttbr1();
+	cpsr = read_cpsr();
+	sctlr = read_sctlr();	
+	write_cpsr(cpsr |(0x80)); //disable irq
+	write_sctlr(sctlr&(~0x1004));//disable cache
+	write_nsacr(0xffc00);
+
+
+	read_mvbar();
+	write_mvbar(__vir_init_monitor_vector);
+	read_mvbar();
+
+	call_smc(smc_handler,smc_stack+2048);	
+	
+	write_cpsr(cpsr);   //restore cpsr	
+	write_sctlr(sctlr);//restore sctlr	
+	return 0;
+}	
+
+
+
+
diff --git a/board/sunxi/board.c b/board/sunxi/board.c
index 85f9708..fbda71c 100755
--- a/board/sunxi/board.c
+++ b/board/sunxi/board.c
@@ -45,6 +45,9 @@ int board_init(void)
 {
 	int id_pfr1;
 
+	vir_init();
+	startup_secondaries();
+
 	gd->bd->bi_boot_params = (PHYS_SDRAM_1 + 0x100);
 
 	asm volatile("mrc p15, 0, %0, c0, c1, 1" : "=r"(id_pfr1));
diff --git a/include/configs/sun7i.h b/include/configs/sun7i.h
old mode 100644
new mode 100755
index a6ede2a..fcb36bf
--- a/include/configs/sun7i.h
+++ b/include/configs/sun7i.h
@@ -34,6 +34,9 @@
 #define CONFIG_SYS_PROMPT		"sun7i# "
 #define CONFIG_MACH_TYPE		4283
 
+
+#define CONFIG_SYS_SECONDARY_ON   1
+
 #if defined(CONFIG_SYS_SECONDARY_ON)
 #define CONFIG_BOARD_POSTCLK_INIT 1
 #endif
-- 
1.8.1.2

