From 426adf9f6025fe70470476166db63ea7c0c1514b Mon Sep 17 00:00:00 2001
From: Damien Le Moal <damien.lemoal@wdc.com>
Date: Fri, 21 Dec 2018 16:44:53 +0900
Subject: [PATCH] Cleanup and rename sbi_hart_boot_next()

Cleanup sbi_hart_boot_nexti() code, adding messages for clarity and
rename the function to sbi_hart_switch_mode() to reflect what the
function actually does.

Signed-off-by: Damien Le Moal <damien.lemoal@wdc.com>
---
 include/sbi/sbi_hart.h |  8 ++++----
 lib/sbi_hart.c         | 33 ++++++++++++++++++++++++---------
 lib/sbi_init.c         |  8 ++++----
 3 files changed, 32 insertions(+), 17 deletions(-)

diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h
index 90161f0b..e369779d 100644
--- a/include/sbi/sbi_hart.h
+++ b/include/sbi/sbi_hart.h
@@ -20,10 +20,10 @@ void sbi_hart_pmp_dump(struct sbi_scratch *scratch);
 
 void __attribute__((noreturn)) sbi_hart_hang(void);
 
-void __attribute__((noreturn)) sbi_hart_boot_next(unsigned long arg0,
-						  unsigned long arg1,
-						  unsigned long next_addr,
-						  unsigned long next_mode);
+void __attribute__((noreturn)) sbi_hart_switch_mode(unsigned long arg0,
+						    unsigned long arg1,
+						    unsigned long next_addr,
+						    unsigned long next_mode);
 
 void sbi_hart_mark_available(u32 hartid);
 
diff --git a/lib/sbi_hart.c b/lib/sbi_hart.c
index 12c5c55b..32dcfe41 100644
--- a/lib/sbi_hart.c
+++ b/lib/sbi_hart.c
@@ -205,37 +205,52 @@ void __attribute__((noreturn)) sbi_hart_hang(void)
 	__builtin_unreachable();
 }
 
-void __attribute__((noreturn)) sbi_hart_boot_next(unsigned long arg0,
-					     unsigned long arg1,
-					     unsigned long next_addr,
-					     unsigned long next_mode)
+void __attribute__((noreturn)) sbi_hart_switch_mode(unsigned long arg0,
+						    unsigned long arg1,
+						    unsigned long next_addr,
+						    unsigned long next_mode)
 {
 	unsigned long val;
+	char mode = 'M';
 
-	if (next_mode != PRV_S && next_mode != PRV_M && next_mode != PRV_U)
-		sbi_hart_hang();
-	if (next_mode == PRV_S && !misa_extension('S'))
-		sbi_hart_hang();
-	if (next_mode == PRV_U && !misa_extension('U'))
+	switch (next_mode) {
+	case PRV_M:
+		break;
+	case PRV_S:
+		if (!misa_extension('S'))
+			sbi_hart_hang();
+		break;
+	case PRV_U:
+		if (!misa_extension('U'))
+			sbi_hart_hang();
+		break;
+	default:
+		sbi_printf("\nTrying to switch to unsupported mode\n");
 		sbi_hart_hang();
+	}
 
 	val = csr_read(mstatus);
 	val = INSERT_FIELD(val, MSTATUS_MPP, next_mode);
 	val = INSERT_FIELD(val, MSTATUS_MPIE, 0);
+
 	csr_write(mstatus, val);
 	csr_write(mepc, next_addr);
 
 	if (next_mode == PRV_S) {
+		mode = 'S';
 		csr_write(stvec, next_addr);
 		csr_write(sscratch, 0);
 		csr_write(sie, 0);
 		csr_write(satp, 0);
 	} else if (next_mode == PRV_U) {
+		mode = 'U';
 		csr_write(utvec, next_addr);
 		csr_write(uscratch, 0);
 		csr_write(uie, 0);
 	}
 
+	sbi_printf("\nSwitching to %c-mode...\n\n", mode);
+
 	register unsigned long a0 asm ("a0") = arg0;
 	register unsigned long a1 asm ("a1") = arg1;
 	__asm__ __volatile__ ("mret" : : "r" (a0), "r" (a1));
diff --git a/lib/sbi_init.c b/lib/sbi_init.c
index 72f2c19b..f8451d18 100644
--- a/lib/sbi_init.c
+++ b/lib/sbi_init.c
@@ -111,8 +111,8 @@ static void __attribute__((noreturn)) init_coldboot(struct sbi_scratch *scratch,
 	if (!sbi_platform_has_hart_hotplug(plat))
 		sbi_hart_wake_coldboot_harts(scratch, hartid);
 
-	sbi_hart_boot_next(hartid, scratch->next_arg1,
-			   scratch->next_addr, scratch->next_mode);
+	sbi_hart_switch_mode(hartid, scratch->next_arg1,
+			     scratch->next_addr, scratch->next_mode);
 }
 
 static void __attribute__((noreturn)) init_warmboot(struct sbi_scratch *scratch,
@@ -154,8 +154,8 @@ static void __attribute__((noreturn)) init_warmboot(struct sbi_scratch *scratch,
 		/* TODO: To be implemented in-future. */
 		sbi_hart_hang();
 	else
-		sbi_hart_boot_next(hartid, scratch->next_arg1,
-				   scratch->next_addr, scratch->next_mode);
+		sbi_hart_switch_mode(hartid, scratch->next_arg1,
+				     scratch->next_addr, scratch->next_mode);
 }
 
 static atomic_t coldboot_lottery = ATOMIC_INITIALIZER(0);