From 873e412648f655de73c5312a3ef18a1640e12853 Mon Sep 17 00:00:00 2001
From: lukileczo <lukasz.leczkowski@phoenix-rtos.com>
Date: Thu, 4 May 2023 17:35:40 +0200
Subject: [PATCH] sparcv8leon3: implement plo copy to SRAM

Now kernel can be loaded to on-chip RAM.

JIRA: RTOS-450
---
 hal/sparcv8leon3/_init.S       | 113 +++++++++++++++++++++++++++++++--
 hal/sparcv8leon3/_traps.S      |   6 +-
 hal/sparcv8leon3/gr716/gr716.c |   2 +-
 ld/sparcv8leon3-gr716.ldt      |  12 ++--
 4 files changed, 119 insertions(+), 14 deletions(-)

diff --git a/hal/sparcv8leon3/_init.S b/hal/sparcv8leon3/_init.S
index e2fff82b..853c19c3 100644
--- a/hal/sparcv8leon3/_init.S
+++ b/hal/sparcv8leon3/_init.S
@@ -18,11 +18,118 @@
 #include "config.h"
 #include "../cpu.h"
 
+.extern _plo_load_addr
+.extern _plo_size
+
 .section ".init", "ax"
 .align 4
 .global _init
 .type _init, #function
 _init:
+	/* get current PC */
+	call . + 8
+	nop
+	mov %o7, %g5
+	/* check where we're loaded */
+	set _plo_load_addr, %g2
+	cmp %g5, %g2
+	bge stage1
+
+	/* Enable CGU for SRAM */
+	set 0x80006000, %g1 /* %g1 = cgu_unlock */
+	add %g1, 0x8, %g3   /* %g3 = cgu_core_reset */
+	add %g1, 0x4, %g4   /* %g4 = cgu_clk_en */
+	ld [%g1], %g2
+	or %g2, 0x10, %g2
+	st %g2, [%g1]
+
+	ld [%g3], %g2
+	or %g2, 0x10, %g2
+	st %g2, [%g3]
+
+	ld [%g4], %g2
+	or %g2, 0x10, %g2
+	st %g2, [%g4]
+
+	ld [%g4], %g2
+	andn %g2, 0x10, %g2
+	st %g2, [%g4]
+
+	ld [%g3], %g2
+	andn %g2, 0x10, %g2
+	st %g2, [%g3]
+
+	ld [%g4], %g2
+	or %g2, 0x10, %g2
+	st %g2, [%g4]
+
+	ld [%g1], %g2
+	andn %g2, 0x10, %g2
+	st %g2, [%g1]
+
+	/* Initialize SRAM pins */
+	sethi %hi(0x8030C000), %g1
+	sethi %hi(0xFE1FFFFF), %g3
+	or %g3, %lo(0xFE1FFFFF), %g3
+	st %g3, [%g1]
+
+	sethi %hi(0x8030D000), %g1
+	sethi %hi(0x1E0000), %g3
+	or %g3, 7, %g3
+	st %g3, [%g1]
+
+	sethi %hi(0x8000D000), %g1
+	sethi %hi(0x22222000), %g2
+	or %g2, 0x222, %g4
+	st %g4, [%g1]
+
+	/* 0x8000D004 */
+	add %g1, 0x4, %g1
+	st %g4, [%g1]
+
+	/* 0x8000D008 */
+	add %g1, 0x4, %g1
+	set 0x22222, %g3
+	st %g3, [%g1]
+
+	/* 0x8000D00C */
+	add %g1, 0x4, %g1
+	or %g2, %lo(0x22222220), %g3
+	st %g3, [%g1]
+
+	/* 0x8000D010 */
+	add %g1, 0x4, %g1
+	mov 0x222, %g3
+	st %g3, [%g1]
+
+	/* 0x8000D018 */
+	add %g1, 0x8, %g1
+	set 0x22220, %g3
+	st %g3, [%g1]
+
+	/* Set SRAM bank size and enable bus ready signalling */
+	set 0x80000004, %g1
+	set ((0xF << 9) | (1 << 7)), %g2
+	st %g2, [%g1]
+
+	/* Copy plo to SRAM */
+	set _plo_size, %g1
+	/* src = %g5 - 0x1000 */
+	set 0x1000, %g3
+	sub %g5, %g3, %g2       /* src */
+	set _plo_load_addr, %g3 /* dst */
+	add %g3, %g1, %g4       /* end */
+
+copy:
+	ld [%g2], %g5
+	add %g2, 4, %g2
+	st %g5, [%g3]
+	add %g3, 4, %g3
+	cmp %g3, %g4
+	bl copy
+	nop
+
+stage1:
 	/* Set up trap table */
 	sethi %hi(_trap_table), %g1
 	wr %g1, %tbr
@@ -39,9 +146,7 @@ _init:
 	mov %psr, %g1
 	or %g1, (PSR_ET | PSR_S), %g1
 	wr %g1, %psr
-	nop
-	nop
-	nop
 
-	call _startc
+	sethi %hi(_startc), %g1
+	jmpl %g1 + %lo(_startc), %g0
 	mov %g0, %g1
diff --git a/hal/sparcv8leon3/_traps.S b/hal/sparcv8leon3/_traps.S
index 5e6ba986..23fe559e 100644
--- a/hal/sparcv8leon3/_traps.S
+++ b/hal/sparcv8leon3/_traps.S
@@ -43,9 +43,9 @@
 
 #define RESET \
 	mov	%g0, %g4; \
-	sethi %hi(_init), %g4; \
-	jmp %g4 + %lo(_init); \
-	nop;
+	nop; \
+	call _init; \
+	nop; \
 
 
 /* When trap occurs, traps are disabled: ET <- 0
diff --git a/hal/sparcv8leon3/gr716/gr716.c b/hal/sparcv8leon3/gr716/gr716.c
index 52902fc3..d8c55dd7 100644
--- a/hal/sparcv8leon3/gr716/gr716.c
+++ b/hal/sparcv8leon3/gr716/gr716.c
@@ -259,5 +259,5 @@ void _gr716_init(void)
 	gr716_common.mctrl0_base = MCTRL0_BASE;
 
 	_gr716_pllSetDefault();
-	_gr716_sram_init();
+	// _gr716_sram_init();
 }
diff --git a/ld/sparcv8leon3-gr716.ldt b/ld/sparcv8leon3-gr716.ldt
index b1610b26..59b64c86 100644
--- a/ld/sparcv8leon3-gr716.ldt
+++ b/ld/sparcv8leon3-gr716.ldt
@@ -20,12 +20,12 @@ MEMORY
 	m_sram          : ORIGIN = 0x40000000, LENGTH = 2M
 }
 
-REGION_ALIAS("PLO_IMAGE", m_iocram)
-REGION_ALIAS("RODATA", m_dram)
-REGION_ALIAS("DATA", m_dram)
-REGION_ALIAS("BSS", m_dram)
-REGION_ALIAS("HEAP", m_dram)
-REGION_ALIAS("STACK", m_dram)
+REGION_ALIAS("PLO_IMAGE", m_sram)
+REGION_ALIAS("RODATA", m_sram)
+REGION_ALIAS("DATA", m_sram)
+REGION_ALIAS("BSS", m_sram)
+REGION_ALIAS("HEAP", m_sram)
+REGION_ALIAS("STACK", m_sram)
 
 #include "common/plo-sparc.lds"