From 7c6f274a369061078eadb9f576002f7cf9ab37a5 Mon Sep 17 00:00:00 2001
From: Stefan Roese <sr@denx.de>
Date: Thu, 20 Aug 2020 07:21:56 +0200
Subject: [PATCH] mips: octeon: lowlevel_init.S: Add NMI handling code for SMP
 Linux booting

This patch adds the necessary lowlevel init code, to enable SMP Linux
booting. This code will be used with the platform specific Octeon Linux
boot command "bootoctlinux", which starts a configurable number of cores
into Linux.

Additionally some erratas and lowlevel register initializations are
copied from the original Cavium / Marvell U-Boot source code, enabling
booting into the Linux kernel.

Signed-off-by: Stefan Roese <sr@denx.de>
---
 arch/mips/mach-octeon/lowlevel_init.S | 76 +++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)

diff --git a/arch/mips/mach-octeon/lowlevel_init.S b/arch/mips/mach-octeon/lowlevel_init.S
index fa87cb4e34..56d1d2261e 100644
--- a/arch/mips/mach-octeon/lowlevel_init.S
+++ b/arch/mips/mach-octeon/lowlevel_init.S
@@ -10,10 +10,36 @@
 #include <asm/mipsregs.h>
 #include <asm/addrspace.h>
 #include <asm/asm.h>
+#include <mach/octeon-model.h>
+
+#define COP0_CVMCTL_REG		$9,7	/* Cavium control */
+#define COP0_CVMMEMCTL_REG	$11,7	/* Cavium memory control */
+#define COP0_PROC_ID_REG	$15,0
 
 	.set noreorder
 
 LEAF(lowlevel_init)
+
+	/* Set LMEMSZ in CVMMEMCTL register */
+	dmfc0	a0, COP0_CVMMEMCTL_REG
+	dins	a0, zero, 0, 9
+	mfc0	a4, COP0_PROC_ID_REG
+	li	a5, OCTEON_CN63XX_PASS1_0 /* Octeon cn63xx pass1 chip id */
+	bgt	a5, a4, 2f
+	 ori	 a0, 0x104	/* setup 4 lines of scratch */
+	ori	a6, a5, 8	/* Octeon cn63xx pass2 chip id */
+	bge	a4, a6, 2f
+	 nop
+	li	a6, 4
+	ins	a0, a6, 11, 4	/* Set WBTHRESH=4 as per Core-14752 errata */
+2:
+	dmtc0	a0, COP0_CVMMEMCTL_REG
+
+	/* Set REPUN bit in CVMCTL register */
+	dmfc0	a0, COP0_CVMCTL_REG
+	ori	a0, 1<<14	/* enable fixup of unaligned mem access */
+	dmtc0	a0, COP0_CVMCTL_REG
+
 	jr	ra
 	 nop
 	END(lowlevel_init)
@@ -67,3 +93,53 @@ __dummy:
 	 nop
 
 	END(mips_mach_early_init)
+
+LEAF(nmi_bootvector)
+
+	/*
+	 * From Marvell original bootvector setup
+	 */
+	mfc0	k0, CP0_STATUS
+	/* Enable 64-bit addressing, set ERL (should already be set) */
+	ori	k0, 0x84
+	mtc0	k0, CP0_STATUS
+	/* Core-14345, clear L1 Dcache virtual tags if the core hit an NMI */
+	cache	17, 0($0)
+
+	/*
+	 * Needed for Linux kernel booting, otherwise it hangs while
+	 * zero'ing all of CVMSEG
+	 */
+	dmfc0	a0, COP0_CVMMEMCTL_REG
+	dins	a0, zero, 0, 9
+	ori	a0, 0x104	/* setup 4 lines of scratch */
+	dmtc0	a0, COP0_CVMMEMCTL_REG
+
+	/*
+	 * Load parameters and entry point
+	 */
+	PTR_LA	t9, nmi_handler_para
+	sync
+
+	ld	s0, 0x00(t9)
+	ld	a0, 0x08(t9)
+	ld	a1, 0x10(t9)
+	ld	a2, 0x18(t9)
+	ld	a3, 0x20(t9)
+
+	/* Finally jump to entry point (start kernel etc) */
+	j	s0
+	 nop
+
+	END(nmi_bootvector)
+
+	/*
+	 * Add here some space for the NMI parameters (entry point and args)
+	 */
+	.globl nmi_handler_para
+nmi_handler_para:
+	.dword	0	// entry-point
+	.dword	0	// arg0
+	.dword	0	// arg1
+	.dword	0	// arg2
+	.dword	0	// arg3
-- 
2.39.5