Merge pull request #3 from riscv/unleashed_working_12_21

Unleashed working 12 21

10 files changed, 110 insertions, 43 deletions

diff --git a/include/sbi/sbi_platform.h b/include/sbi/sbi_platform.h
index 6de52f63..00fadde9 100644
--- a/include/sbi/sbi_platform.h
+++ b/include/sbi/sbi_platform.h
@@ -33,6 +33,7 @@ struct sbi_platform {
 	u64 features;
 	u32 hart_count;
 	u32 hart_stack_size;
+	u64 disabled_hart_mask;
 	int (*cold_early_init)(void);
 	int (*cold_final_init)(void);
 	int (*warm_early_init)(u32 target_hart);
@@ -83,6 +84,13 @@ static inline const char *sbi_platform_name(struct sbi_platform *plat)
 	return NULL;
 }
 
+static inline bool sbi_platform_hart_disabled(struct sbi_platform *plat, u32 hartid)
+{
+	if (plat && (plat->disabled_hart_mask & (1 << hartid)))
+		return 1;
+	else
+		return 0;
+}
 static inline u32 sbi_platform_hart_count(struct sbi_platform *plat)
 {
 	if (plat)
diff --git a/lib/sbi_hart.c b/lib/sbi_hart.c
index be114d69..1e5465f5 100644
--- a/lib/sbi_hart.c
+++ b/lib/sbi_hart.c
@@ -211,8 +211,6 @@ int sbi_hart_init(struct sbi_scratch *scratch, u32 hartid)
 
 void __attribute__((noreturn)) sbi_hart_hang(void)
 {
-	sbi_printf("\nHART%u Hang !!\n\n", sbi_current_hartid());
-
 	while (1)
 		wfi();
 	__builtin_unreachable();
diff --git a/lib/sbi_init.c b/lib/sbi_init.c
index da78a7ef..68113b49 100644
--- a/lib/sbi_init.c
+++ b/lib/sbi_init.c
@@ -86,7 +86,6 @@ static void __attribute__((noreturn)) init_coldboot(struct sbi_scratch *scratch,
 	sbi_printf("OpenSBI v%d.%d (%s %s)\n",
 		   OPENSBI_MAJOR, OPENSBI_MINOR,
 		   __DATE__, __TIME__);
-	sbi_printf("Running on Hart %u\n", hartid);
 
 	sbi_printf("%s\n", logo);
 
@@ -95,6 +94,7 @@ static void __attribute__((noreturn)) init_coldboot(struct sbi_scratch *scratch,
 	sbi_printf("Platform HART Features : RV%d%s\n", misa_xlen(), str);
 	sbi_printf("Platform Max HARTs     : %d\n",
 		   sbi_platform_hart_count(plat));
+	sbi_printf("Current Hart           : %u\n", hartid);
 	/* Firmware details */
 	sbi_printf("Firmware Base          : 0x%lx\n", scratch->fw_start);
 	sbi_printf("Firmware Size          : %d KB\n",
@@ -106,11 +106,9 @@ static void __attribute__((noreturn)) init_coldboot(struct sbi_scratch *scratch,
 
 	sbi_hart_pmp_dump(scratch);
 
-	sbi_hart_mark_available(hartid);
-
 	if (!sbi_platform_has_hart_hotplug(plat))
 		sbi_hart_wake_coldboot_harts(scratch, hartid);
-
+	sbi_hart_mark_available(hartid);
 	sbi_hart_switch_mode(hartid, scratch->next_arg1,
 			     scratch->next_addr, scratch->next_mode);
 }
@@ -124,6 +122,9 @@ static void __attribute__((noreturn)) init_warmboot(struct sbi_scratch *scratch,
 	if (!sbi_platform_has_hart_hotplug(plat))
 		sbi_hart_wait_for_coldboot(scratch, hartid);
 
+	if (sbi_platform_hart_disabled(plat, hartid))
+		sbi_hart_hang();
+
 	rc = sbi_system_warm_early_init(scratch, hartid);
 	if (rc)
 		sbi_hart_hang();
@@ -164,7 +165,11 @@ void __attribute__((noreturn)) sbi_init(struct sbi_scratch *scratch)
 {
 	bool coldboot = FALSE;
 	u32 hartid = sbi_current_hartid();
+	struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
+	if (sbi_platform_hart_disabled(plat, hartid))
+		sbi_hart_hang();
+		
 	if (atomic_add_return(&coldboot_lottery, 1) == 1)
 		coldboot = TRUE;
 
diff --git a/lib/sbi_ipi.c b/lib/sbi_ipi.c
index f3e68de1..d21ea4a2 100644
--- a/lib/sbi_ipi.c
+++ b/lib/sbi_ipi.c
@@ -28,7 +28,7 @@ int sbi_ipi_send_many(struct sbi_scratch *scratch,
 
 	/* send IPIs to everyone */
 	for (i = 0, m = mask; m; i++, m >>= 1) {
-		if ((m & 1) && (i != hartid)) {
+		if ((m & 1) && (i != hartid) && !sbi_platform_hart_disabled(plat, hartid)) {
 			oth = sbi_hart_id_to_scratch(scratch, i);
 			oth->ipi_type = event;
 			mb();
diff --git a/platform/common/include/plat/irqchip/plic.h b/platform/common/include/plat/irqchip/plic.h
index 7c062aa5..d3094acc 100644
--- a/platform/common/include/plat/irqchip/plic.h
+++ b/platform/common/include/plat/irqchip/plic.h
@@ -12,9 +12,10 @@
 
 #include <sbi/sbi_types.h>
 
-int plic_fdt_fixup(void *fdt, const char *compat);
+void plic_fdt_fixup(void *fdt, const char *compat, u32 cntx_id);
 
-int plic_warm_irqchip_init(u32 target_hart);
+int plic_warm_irqchip_init(u32 target_hart,
+			   int m_cntx_id, int s_cntx_id);
 
 int plic_cold_irqchip_init(unsigned long base,
 			   u32 num_sources, u32 hart_count);
diff --git a/platform/common/irqchip/plic.c b/platform/common/irqchip/plic.c
index 404d1b7e..61a6bd01 100644
--- a/platform/common/irqchip/plic.c
+++ b/platform/common/irqchip/plic.c
@@ -27,28 +27,20 @@ static void plic_set_priority(u32 source, u32 val)
 	writel(val, plic_base);
 }
 
-static void plic_set_m_thresh(u32 hartid, u32 val)
+static void plic_set_thresh(u32 cntxid, u32 val)
 {
-	volatile void *plic_m_thresh = plic_base +
+	volatile void *plic_thresh = plic_base +
 				PLIC_CONTEXT_BASE +
-				PLIC_CONTEXT_STRIDE * (2 * hartid);
-	writel(val, plic_m_thresh);
+				PLIC_CONTEXT_STRIDE * cntxid;
+	writel(val, plic_thresh);
 }
 
-static void plic_set_s_thresh(u32 hartid, u32 val)
+static void plic_set_ie(u32 cntxid, u32 word_index, u32 val)
 {
-	volatile void *plic_s_thresh = plic_base +
-				PLIC_CONTEXT_BASE +
-				PLIC_CONTEXT_STRIDE * (2 * hartid + 1);
-	writel(val, plic_s_thresh);
-}
-
-static void plic_set_s_ie(u32 hartid, u32 word_index, u32 val)
-{
-	volatile void *plic_s_ie = plic_base +
+	volatile void *plic_ie = plic_base +
 				PLIC_ENABLE_BASE +
-				PLIC_ENABLE_STRIDE * (2 * hartid + 1);
-	writel(val, plic_s_ie + word_index * 4);
+				PLIC_ENABLE_STRIDE * cntxid;
+	writel(val, plic_ie + word_index * 4);
 }
 
 static void plic_fdt_fixup_prop(const struct fdt_node *node,
@@ -57,6 +49,7 @@ static void plic_fdt_fixup_prop(const struct fdt_node *node,
 {
 	u32 *cells;
 	u32 i, cells_count;
+	u32 *cntx_id = priv;
 
 	if (!prop)
 		return;
@@ -70,33 +63,42 @@ static void plic_fdt_fixup_prop(const struct fdt_node *node,
 		return;
 
 	for (i = 0; i < cells_count; i++) {
-		if (i % 4 == 1)
+		if (((i % 2) == 1) && ((i / 2) == *cntx_id))
 			cells[i] = fdt_rev32(0xffffffff);
 	}
 }
 
-int plic_fdt_fixup(void *fdt, const char *compat)
+void plic_fdt_fixup(void *fdt, const char *compat, u32 cntx_id)
 {
-	fdt_compat_node_prop(fdt, compat, plic_fdt_fixup_prop, NULL);
-	return 0;
+	fdt_compat_node_prop(fdt, compat, plic_fdt_fixup_prop, &cntx_id);
 }
 
-int plic_warm_irqchip_init(u32 target_hart)
+int plic_warm_irqchip_init(u32 target_hart,
+			   int m_cntx_id, int s_cntx_id)
 {
 	size_t i, ie_words = plic_num_sources / 32 + 1;
 
 	if (plic_hart_count <= target_hart)
 		return -1;
+	
+	if (m_cntx_id > -1) {
+		for (i = 0; i < ie_words; i++)
+			plic_set_ie(m_cntx_id, i, 0);
+	}
 
 	/* By default, enable all IRQs for S-mode of target HART */
-	for (i = 0; i < ie_words; i++)
-		plic_set_s_ie(target_hart, i, -1);
+	if (s_cntx_id > -1) {
+		for (i = 0; i < ie_words; i++)
+			plic_set_ie(s_cntx_id, i, 0);
+	}
 
 	/* By default, enable M-mode threshold */
-	plic_set_m_thresh(target_hart, 1);
+	if (m_cntx_id > -1)
+		plic_set_thresh(m_cntx_id, 1);
 
 	/* By default, disable S-mode threshold */
-	plic_set_s_thresh(target_hart, 0);
+	if (s_cntx_id > -1)
+		plic_set_thresh(s_cntx_id, 0);
 
 	return 0;
 }
diff --git a/platform/kendryte/k210/platform.c b/platform/kendryte/k210/platform.c
index 03c2c2d7..d3765d0e 100644
--- a/platform/kendryte/k210/platform.c
+++ b/platform/kendryte/k210/platform.c
@@ -16,9 +16,11 @@
 #include "platform.h"
 #include "uarths.h"
 
+#define K210_UART_BAUDRATE		115200
+
 int k210_console_init(void)
 {
-	uarths_init(115200, UARTHS_STOP_1);
+	uarths_init(K210_UART_BAUDRATE, UARTHS_STOP_1);
 
 	return 0;
 }
@@ -41,7 +43,9 @@ static int k210_cold_irqchip_init(void)
 
 static int k210_warm_irqchip_init(u32 core_id)
 {
-	return plic_warm_irqchip_init(core_id);
+	return plic_warm_irqchip_init(core_id,
+				      (2 * core_id),
+				      (2 * core_id + 1));
 }
 
 static int k210_cold_ipi_init(void)
@@ -77,6 +81,7 @@ struct sbi_platform platform = {
 
 	.hart_count = PLAT_HART_COUNT,
 	.hart_stack_size = PLAT_HART_STACK_SIZE,
+	.disabled_hart_mask = 0,
 
 	.console_init = k210_console_init,
 	.console_putc = k210_console_putc,
diff --git a/platform/qemu/sifive_u/platform.c b/platform/qemu/sifive_u/platform.c
index d9168255..f01df1ef 100644
--- a/platform/qemu/sifive_u/platform.c
+++ b/platform/qemu/sifive_u/platform.c
@@ -28,7 +28,13 @@
 
 static int sifive_u_cold_final_init(void)
 {
-	return plic_fdt_fixup(sbi_scratch_thishart_arg1_ptr(), "riscv,plic0");
+	u32 i;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (i = 0; i < PLAT_HART_COUNT; i++)
+		plic_fdt_fixup(fdt, "riscv,plic0", 2 * i);
+
+	return 0;
 }
 
 static u32 sifive_u_pmp_region_count(u32 target_hart)
@@ -68,6 +74,13 @@ static int sifive_u_cold_irqchip_init(void)
 				      PLAT_HART_COUNT);
 }
 
+static int sifive_u_warm_irqchip_init(u32 target_hart)
+{
+	return plic_warm_irqchip_init(target_hart,
+				      (2 * target_hart),
+				      (2 * target_hart + 1));
+}
+
 static int sifive_u_cold_ipi_init(void)
 {
 	return clint_cold_ipi_init(SIFIVE_U_CLINT_ADDR,
@@ -91,6 +104,7 @@ struct sbi_platform platform = {
 	.features = SBI_PLATFORM_DEFAULT_FEATURES,
 	.hart_count = PLAT_HART_COUNT,
 	.hart_stack_size = PLAT_HART_STACK_SIZE,
+	.disabled_hart_mask = 0,
 	.pmp_region_count = sifive_u_pmp_region_count,
 	.pmp_region_info = sifive_u_pmp_region_info,
 	.cold_final_init = sifive_u_cold_final_init,
@@ -98,7 +112,7 @@ struct sbi_platform platform = {
 	.console_getc = sifive_uart_getc,
 	.console_init = sifive_u_console_init,
 	.cold_irqchip_init = sifive_u_cold_irqchip_init,
-	.warm_irqchip_init = plic_warm_irqchip_init,
+	.warm_irqchip_init = sifive_u_warm_irqchip_init,
 	.ipi_inject = clint_ipi_inject,
 	.ipi_sync = clint_ipi_sync,
 	.ipi_clear = clint_ipi_clear,
diff --git a/platform/qemu/virt/platform.c b/platform/qemu/virt/platform.c
index fe91fc8f..9e006744 100644
--- a/platform/qemu/virt/platform.c
+++ b/platform/qemu/virt/platform.c
@@ -23,10 +23,18 @@
 #define VIRT_PLIC_NUM_PRIORITIES	7
 
 #define VIRT_UART16550_ADDR		0x10000000
+#define VIRT_UART_BAUDRATE		115200
+#define VIRT_UART_SHIFTREG_ADDR		1843200
 
 static int virt_cold_final_init(void)
 {
-	return plic_fdt_fixup(sbi_scratch_thishart_arg1_ptr(), "riscv,plic0");
+	u32 i;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (i = 0; i < PLAT_HART_COUNT; i++)
+		plic_fdt_fixup(fdt, "riscv,plic0", 2 * i);
+
+	return 0;
 }
 
 static u32 virt_pmp_region_count(u32 target_hart)
@@ -56,7 +64,8 @@ static int virt_pmp_region_info(u32 target_hart, u32 index,
 static int virt_console_init(void)
 {
 	return uart8250_init(VIRT_UART16550_ADDR,
-			     1843200, 115200, 0, 1);
+			     VIRT_UART_SHIFTREG_ADDR,
+			     VIRT_UART_BAUDRATE, 0, 1);
 }
 
 static int virt_cold_irqchip_init(void)
@@ -66,6 +75,13 @@ static int virt_cold_irqchip_init(void)
 				      PLAT_HART_COUNT);
 }
 
+static int virt_warm_irqchip_init(u32 target_hart)
+{
+	return plic_warm_irqchip_init(target_hart,
+				      (2 * target_hart),
+				      (2 * target_hart + 1));
+}
+
 static int virt_cold_ipi_init(void)
 {
 	return clint_cold_ipi_init(VIRT_CLINT_ADDR,
@@ -89,6 +105,7 @@ struct sbi_platform platform = {
 	.features = SBI_PLATFORM_DEFAULT_FEATURES,
 	.hart_count = PLAT_HART_COUNT,
 	.hart_stack_size = PLAT_HART_STACK_SIZE,
+	.disabled_hart_mask = 0,
 	.pmp_region_count = virt_pmp_region_count,
 	.pmp_region_info = virt_pmp_region_info,
 	.cold_final_init = virt_cold_final_init,
@@ -96,7 +113,7 @@ struct sbi_platform platform = {
 	.console_getc = uart8250_getc,
 	.console_init = virt_console_init,
 	.cold_irqchip_init = virt_cold_irqchip_init,
-	.warm_irqchip_init = plic_warm_irqchip_init,
+	.warm_irqchip_init = virt_warm_irqchip_init,
 	.ipi_inject = clint_ipi_inject,
 	.ipi_sync = clint_ipi_sync,
 	.ipi_clear = clint_ipi_clear,
diff --git a/platform/sifive/hifive_u540/platform.c b/platform/sifive/hifive_u540/platform.c
index 338cc2cc..646ce273 100644
--- a/platform/sifive/hifive_u540/platform.c
+++ b/platform/sifive/hifive_u540/platform.c
@@ -27,6 +27,8 @@
 #define SIFIVE_U_UART1_ADDR			0x10011000
 #define SIFIVE_UART_BAUDRATE			115200
 
+#define SIFIVE_U_HARITD_ENABLED			1
+
 /* PRCI clock related macros */
 //TODO: Do we need a separate driver for this ?
 #define SIFIVE_PRCI_BASE_ADDR			0x10000000
@@ -35,7 +37,14 @@
 
 static int sifive_u_cold_final_init(void)
 {
-	return plic_fdt_fixup(sbi_scratch_thishart_arg1_ptr(), "riscv,plic0");
+	u32 i;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	plic_fdt_fixup(fdt, "riscv,plic0", 0);
+	for (i = 1; i < PLAT_HART_COUNT; i++)
+		plic_fdt_fixup(fdt, "riscv,plic0", 2 * i - 1);
+
+	return 0;
 }
 
 static u32 sifive_u_pmp_region_count(u32 target_hart)
@@ -85,6 +94,13 @@ static int sifive_u_cold_irqchip_init(void)
 				      PLAT_HART_COUNT);
 }
 
+static int sifive_u_warm_irqchip_init(u32 target_hart)
+{
+	return plic_warm_irqchip_init(target_hart,
+			(target_hart) ? (2 * target_hart - 1) : 0,
+			(target_hart) ? (2 * target_hart) : -1);
+}
+
 static int sifive_u_cold_ipi_init(void)
 {
 	return clint_cold_ipi_init(SIFIVE_U_CLINT_ADDR,
@@ -108,6 +124,7 @@ struct sbi_platform platform = {
 	.features = SBI_PLATFORM_DEFAULT_FEATURES,
 	.hart_count = PLAT_HART_COUNT,
 	.hart_stack_size = PLAT_HART_STACK_SIZE,
+	.disabled_hart_mask = ~(1 << SIFIVE_U_HARITD_ENABLED),
 	.pmp_region_count = sifive_u_pmp_region_count,
 	.pmp_region_info = sifive_u_pmp_region_info,
 	.cold_final_init = sifive_u_cold_final_init,
@@ -115,7 +132,7 @@ struct sbi_platform platform = {
 	.console_getc = sifive_uart_getc,
 	.console_init = sifive_u_console_init,
 	.cold_irqchip_init = sifive_u_cold_irqchip_init,
-	.warm_irqchip_init = plic_warm_irqchip_init,
+	.warm_irqchip_init = sifive_u_warm_irqchip_init,
 	.ipi_inject = clint_ipi_inject,
 	.ipi_sync = clint_ipi_sync,
 	.ipi_clear = clint_ipi_clear,