[U-Boot] [PATCH] nds32: Enable FPU if the version of CPU supported

[Kuan-Yu Kuo]

Some version of Andes core support FPU coprocessor, if this is the case,
we should enable it at low level initialization time.

Signed-off-by: Kuan-Yu Kuo <ken.kuoky at gmail.com>
Cc: Macpaul Lin <macpaul at gmail.com>
---
 arch/nds32/cpu/n1213/ag101/lowlevel_init.S |   22 ++++++++++++++++++++++
 arch/nds32/cpu/n1213/ag102/lowlevel_init.S |   22 ++++++++++++++++++++++
 2 files changed, 44 insertions(+)

diff --git a/arch/nds32/cpu/n1213/ag101/lowlevel_init.S b/arch/nds32/cpu/n1213/ag101/lowlevel_init.S
index 55985cf..b4c2b0f 100644
--- a/arch/nds32/cpu/n1213/ag101/lowlevel_init.S
+++ b/arch/nds32/cpu/n1213/ag101/lowlevel_init.S
@@ -98,6 +98,9 @@ lowlevel_init:
 	led	0x10
 	jal	remap
 
+	led	0x1f
+	jal	enable_fpu
+
 	led	0x20
 	ret	$r10
 
@@ -234,6 +237,25 @@ relo_base:
 2:
 	ret
 
+	/*
+	 * enable_fpu:
+	 *  Some of Andes CPU version support FPU coprocessor, if so,
+	 *  we should enable it.
+	 */
+enable_fpu:
+	mfsr    $r0, $CPU_VER 		/* enable FPU if it exists */
+	srli    $r0, $r0, 3
+	andi    $r0, $r0, 1
+	beqz    $r0, 1f						/* skip if no COP */
+	mfsr    $r0, $FUCOP_EXIST
+	srli    $r0, $r0, 31
+	beqz    $r0, 1f						/* skip if no FPU */
+	mfsr    $r0, $FUCOP_CTL
+	ori     $r0, $r0, 1
+	mtsr    $r0, $FUCOP_CTL
+1:
+	ret
+
 .globl show_led
 show_led:
     li      $r8, (CONFIG_DEBUG_LED)
diff --git a/arch/nds32/cpu/n1213/ag102/lowlevel_init.S b/arch/nds32/cpu/n1213/ag102/lowlevel_init.S
index d842afa..345f316 100644
--- a/arch/nds32/cpu/n1213/ag102/lowlevel_init.S
+++ b/arch/nds32/cpu/n1213/ag102/lowlevel_init.S
@@ -114,6 +114,9 @@ lowlevel_init:
 	led	0x20
 	jal	remap
 
+	led	0x2f
+	jal	enable_fpu
+
 	led	0x30
 	ret	$r10
 
@@ -289,6 +292,25 @@ relo_base:
 2:
 	ret
 
+	/*
+	 * enable_fpu:
+	 *  Some of Andes CPU version support FPU coprocessor, if so,
+	 *  we should enable it.
+	 */
+enable_fpu:
+	mfsr    $r0, $CPU_VER 		/* enable FPU if it exists */
+	srli    $r0, $r0, 3
+	andi    $r0, $r0, 1
+	beqz    $r0, 1f						/* skip if no COP */
+	mfsr    $r0, $FUCOP_EXIST
+	srli    $r0, $r0, 31
+	beqz    $r0, 1f						/* skip if no FPU */
+	mfsr    $r0, $FUCOP_CTL
+	ori     $r0, $r0, 1
+	mtsr    $r0, $FUCOP_CTL
+1:
+	ret
+
 .globl show_led
 show_led:
     li      $r8, (CONFIG_DEBUG_LED)
-- 
1.7.9.5