From 205353fa06cc5dbfe1949de013ba905bb151c702 Mon Sep 17 00:00:00 2001
From: Guo Ren <ren_guo@c-sky.com>
Date: Fri, 12 Apr 2019 19:08:34 +0800
Subject: csky: Support vmlinux bootup with MMU off

Modify SETUP_MMU macro to fit on both MMU-on or MMU-off enviornment
and vmlinux could bootup from MMU off enviornment for some cases.

Unify the style of _start and _start_smp_secondary in head.S to make
head.S looks more concise and easy to understand.

Signed-off-by: Guo Ren <ren_guo@c-sky.com>
Cc: Arnd Bergmann <arnd@arndb.de>
---
 arch/csky/abiv2/inc/abi/entry.h | 79 ++++++++++++++++++++++++++++++++---------
 1 file changed, 62 insertions(+), 17 deletions(-)

(limited to 'arch/csky/abiv2')

diff --git a/arch/csky/abiv2/inc/abi/entry.h b/arch/csky/abiv2/inc/abi/entry.h
index c0a76c43cded..9897a16b45e5 100644
--- a/arch/csky/abiv2/inc/abi/entry.h
+++ b/arch/csky/abiv2/inc/abi/entry.h
@@ -162,31 +162,76 @@
 	mtcr	\rx, cr<8, 15>
 .endm
 
-.macro SETUP_MMU rx
-	/* Check MMU on | off */
-	mfcr	\rx, cr18
-	btsti	\rx, 0
+.macro SETUP_MMU
+	/* Init psr and enable ee */
+	lrw	r6, DEFAULT_PSR_VALUE
+	mtcr    r6, psr
+	psrset  ee
+
+	/* Invalid I/Dcache BTB BHT */
+	movi	r6, 7
+	lsli	r6, 16
+	addi	r6, (1<<4) | 3
+	mtcr	r6, cr17
+
+	/* Invalid all TLB */
+	bgeni   r6, 26
+	mtcr	r6, cr<8, 15> /* Set MCIR */
+
+	/* Check MMU on/off */
+	mfcr	r6, cr18
+	btsti	r6, 0
 	bt	1f
-	grs	\rx, 1f
+
+	/* MMU off: setup mapping tlb entry */
+	movi	r6, 0
+	mtcr	r6, cr<6, 15> /* Set MPR with 4K page size */
+
+	grs	r6, 1f /* Get current pa by PC */
+	bmaski  r7, (PAGE_SHIFT + 1) /* r7 = 0x1fff */
+	andn    r6, r7
+	mtcr	r6, cr<4, 15> /* Set MEH */
+
+	mov	r8, r6
+	movi    r7, 0x00000006
+	or      r8, r7
+	mtcr	r8, cr<2, 15> /* Set MEL0 */
+	movi    r7, 0x00001006
+	or      r8, r7
+	mtcr	r8, cr<3, 15> /* Set MEL1 */
+
+	bgeni   r8, 28
+	mtcr	r8, cr<8, 15> /* Set MCIR to write TLB */
+
 	br	2f
 1:
 	/*
-	 * cr<30, 15> format:
+	 * MMU on: use origin MSA value from bootloader
+	 *
+	 * cr<30/31, 15> MSA register format:
 	 * 31 - 29 | 28 - 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0
 	 *   BA     Reserved  SH  WA  B   SO SEC  C   D   V
 	 */
-	mfcr	\rx, cr<30, 15>
+	mfcr	r6, cr<30, 15> /* Get MSA0 */
 2:
-	lsri	\rx, 28
-	lsli	\rx, 28
-	addi	\rx, 0x1ce
-	mtcr	\rx, cr<30, 15>
-
-	lsri	\rx, 28
-	addi	\rx, 2
-	lsli	\rx, 28
-	addi	\rx, 0x1ce
-	mtcr	\rx, cr<31, 15>
+	lsri	r6, 28
+	lsli	r6, 28
+	addi	r6, 0x1ce
+	mtcr	r6, cr<30, 15> /* Set MSA0 */
+
+	lsri	r6, 28
+	addi	r6, 2
+	lsli	r6, 28
+	addi	r6, 0x1ce
+	mtcr	r6, cr<31, 15> /* Set MSA1 */
+
+	/* enable MMU */
+	mfcr    r6, cr18
+	bseti	r6, 0
+	mtcr    r6, cr18
+
+	jmpi	3f /* jump to va */
+3:
 .endm
 
 .macro ANDI_R3 rx, imm
-- 
cgit