rtl/smp: implement SMP dead/alive handling

12 files changed, 71 insertions, 15 deletions

diff --git a/core_hw.tcl b/core_hw.tcl
index 0b0a2da..65d0e99 100644
--- a/core_hw.tcl
+++ b/core_hw.tcl
@@ -225,6 +225,7 @@ set_interface_property smp SVD_ADDRESS_GROUP ""
 
 add_interface_port smp step step Input 1
 add_interface_port smp cpu_halt halt Input 1
+add_interface_port smp cpu_alive cpu_alive Output 1
 add_interface_port smp cpu_halted cpu_halted Output 1
 add_interface_port smp breakpoint breakpoint Output 1
 
diff --git a/demo/demo.h b/demo/demo.h
index 5ffd4cd..d8b9da6 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -27,6 +27,7 @@ void console_init(void);
 void print(const char *fmt, ...);
 void read_line(char *buf, unsigned size);
 
+int cpu_is_alive(unsigned num);
 void run_cpu(unsigned num);
 void run_cpus(unsigned mask);
 void halt_cpu(unsigned num);
diff --git a/demo/main.c b/demo/main.c
index c5b7353..d8f38d4 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -123,13 +123,27 @@ static void cmd_remote(char **tokens)
 		unknown_command(cmd);
 }
 
+static void kick_cpus(void)
+{
+	for (unsigned i = this_cpu->num + 1; i < NUM_CPUS; ++i) {
+		if (cpu_is_alive(i)) {
+			run_cpu(i);
+			return;
+		}
+
+		print("cpu%u is dead", i);
+	}
+
+	boot_done = 1;
+}
+
 static void bsp_main(void)
 {
 	for (struct cpu *cpu = all_cpus; cpu < all_cpus + NUM_CPUS; ++cpu)
 		cpu->mailbox = 0;
 
 	boot_done = 0;
-	run_cpu(1);
+	kick_cpus();
 
 	while (!boot_done);
 	print("booted %u cpus", NUM_CPUS);
@@ -165,11 +179,7 @@ static void bsp_main(void)
 
 static void ap_main(void)
 {
-	if (this_cpu->num < NUM_CPUS - 1)
-		run_cpu(this_cpu->num + 1);
-	else
-		boot_done = 1;
-
+	kick_cpus();
 	halt_cpu(this_cpu->num);
 
 	while (1) {
diff --git a/demo/smp.c b/demo/smp.c
index 4357d35..8bf220e 100644
--- a/demo/smp.c
+++ b/demo/smp.c
@@ -3,6 +3,11 @@
 #define SMP_CTRL_BASE 0x30140000
 #define SMP_CTRL      (*(volatile unsigned *)SMP_CTRL_BASE)
 
+int cpu_is_alive(unsigned num)
+{
+	return !!(SMP_CTRL & (0b100 << (num * 8)));
+}
+
 void run_cpu(unsigned num)
 {
 	run_cpus(1 << num);
diff --git a/rtl/core/core.sv b/rtl/core/core.sv
index ce51a71..cf63b2d 100644
--- a/rtl/core/core.sv
+++ b/rtl/core/core.sv
@@ -9,6 +9,7 @@ module core
 
 	input  wire       step,
 	input  wire       cpu_halt,
+	output wire       cpu_alive,
 	output wire       cpu_halted,
 	output wire       breakpoint,
 
@@ -27,6 +28,8 @@ module core
 
 	generate
 		if (ID < `CONFIG_CPUS) begin: enable
+			assign cpu_alive = 1;
+
 			ptr addr;
 			word data_wr;
 			logic start, write;
@@ -57,6 +60,7 @@ module core
 				.*
 			);
 		end else begin
+			assign cpu_alive = 0;
 			assign cpu_halted = 1;
 			assign breakpoint = 0;
 
diff --git a/rtl/smp/pe.sv b/rtl/smp/pe.sv
index 212ce5a..5c675ee 100644
--- a/rtl/smp/pe.sv
+++ b/rtl/smp/pe.sv
@@ -8,7 +8,8 @@ module smp_pe
 	input  logic[7:0] writedata,
 	output logic[7:0] readdata,
 
-	input  logic      cpu_halted,
+	input  logic      cpu_alive,
+	                  cpu_halted,
 	                  breakpoint,
 
 	output logic      halt,
@@ -22,11 +23,16 @@ module smp_pe
 
 	struct packed
 	{
-		logic breakpoint, cpu_halted;
-	} status;
+		logic alive, breakpoint, cpu_halted;
+	} status, status_out;
 
 	assign req = writedata[$bits(req) - 1:0];
-	assign readdata = {{(8 - $bits(status)){1'b0}}, status};
+	assign readdata = {{(8 - $bits(status_out)){1'b0}}, status_out};
+
+	always_comb begin
+		status_out = status;
+		status_out.alive = cpu_alive;
+	end
 
 	always @(posedge clk or negedge rst_n)
 		if (!rst_n) begin
diff --git a/rtl/smp/smp_ctrl.sv b/rtl/smp/smp_ctrl.sv
index 4d6d1a5..2bf812e 100644
--- a/rtl/smp/smp_ctrl.sv
+++ b/rtl/smp/smp_ctrl.sv
@@ -8,11 +8,15 @@ module smp_ctrl
 	input  logic[31:0] avl_writedata,
 	output logic[31:0] avl_readdata,
 
-	input logic        cpu_halted_0,
+	input logic        cpu_alive_0,
+	                   cpu_alive_1,
+	                   cpu_alive_2,
+	                   cpu_alive_3,
+	                   cpu_halted_0,
 	                   cpu_halted_1,
 	                   cpu_halted_2,
 	                   cpu_halted_3,
-	input logic        breakpoint_0,
+	                   breakpoint_0,
 	                   breakpoint_1,
 	                   breakpoint_2,
 	                   breakpoint_3,	
@@ -41,6 +45,7 @@ module smp_ctrl
 	(
 		.step(step_0),
 		.halt(halt_0),
+		.cpu_alive(cpu_alive_0),
 		.cpu_halted(cpu_halted_0),
 		.breakpoint(breakpoint_0),
 		.readdata(readdata_0),
@@ -52,6 +57,7 @@ module smp_ctrl
 	(
 		.step(step_1),
 		.halt(halt_1),
+		.cpu_alive(cpu_alive_1),
 		.cpu_halted(cpu_halted_1),
 		.breakpoint(breakpoint_1),
 		.readdata(readdata_1),
@@ -63,6 +69,7 @@ module smp_ctrl
 	(
 		.step(step_2),
 		.halt(halt_2),
+		.cpu_alive(cpu_alive_2),
 		.cpu_halted(cpu_halted_2),
 		.breakpoint(breakpoint_2),
 		.readdata(readdata_2),
@@ -74,6 +81,7 @@ module smp_ctrl
 	(
 		.step(step_3),
 		.halt(halt_3),
+		.cpu_alive(cpu_alive_3),
 		.cpu_halted(cpu_halted_3),
 		.breakpoint(breakpoint_3),
 		.readdata(readdata_3),
diff --git a/rtl/top/test_smp.sv b/rtl/top/test_smp.sv
index df417c3..e6a66e8 100644
--- a/rtl/top/test_smp.sv
+++ b/rtl/top/test_smp.sv
@@ -9,11 +9,15 @@ module test_smp
 	input  logic[31:0] avl_writedata,
 	output logic[31:0] avl_readdata,
 
-	input logic        cpu_halted_0,
+	input logic        cpu_alive_0,
+	                   cpu_alive_1,
+	                   cpu_alive_2,
+	                   cpu_alive_3,
+	                   cpu_halted_0,
 	                   cpu_halted_1,
 	                   cpu_halted_2,
 	                   cpu_halted_3,
-	input logic        breakpoint_0,
+	                   breakpoint_0,
 	                   breakpoint_1,
 	                   breakpoint_2,
 	                   breakpoint_3,	
diff --git a/smp_hw.tcl b/smp_hw.tcl
index f96039e..a853287 100644
--- a/smp_hw.tcl
+++ b/smp_hw.tcl
@@ -132,6 +132,7 @@ set_interface_property cpu_0 SVD_ADDRESS_GROUP ""
 
 add_interface_port cpu_0 halt_0 halt Output 1
 add_interface_port cpu_0 step_0 step Output 1
+add_interface_port cpu_0 cpu_alive_0 cpu_alive Input 1
 add_interface_port cpu_0 cpu_halted_0 cpu_halted Input 1
 add_interface_port cpu_0 breakpoint_0 breakpoint Input 1
 
@@ -149,6 +150,7 @@ set_interface_property cpu_1 CMSIS_SVD_VARIABLES ""
 set_interface_property cpu_1 SVD_ADDRESS_GROUP ""
 
 add_interface_port cpu_1 breakpoint_1 breakpoint Input 1
+add_interface_port cpu_1 cpu_alive_1 cpu_alive Input 1
 add_interface_port cpu_1 cpu_halted_1 cpu_halted Input 1
 add_interface_port cpu_1 halt_1 halt Output 1
 add_interface_port cpu_1 step_1 step Output 1
@@ -167,6 +169,7 @@ set_interface_property cpu_2 CMSIS_SVD_VARIABLES ""
 set_interface_property cpu_2 SVD_ADDRESS_GROUP ""
 
 add_interface_port cpu_2 breakpoint_2 breakpoint Input 1
+add_interface_port cpu_2 cpu_alive_2 cpu_alive Input 1
 add_interface_port cpu_2 cpu_halted_2 cpu_halted Input 1
 add_interface_port cpu_2 halt_2 halt Output 1
 add_interface_port cpu_2 step_2 step Output 1
@@ -185,6 +188,7 @@ set_interface_property cpu_3 CMSIS_SVD_VARIABLES ""
 set_interface_property cpu_3 SVD_ADDRESS_GROUP ""
 
 add_interface_port cpu_3 breakpoint_3 breakpoint Input 1
+add_interface_port cpu_3 cpu_alive_3 cpu_alive Input 1
 add_interface_port cpu_3 cpu_halted_3 cpu_halted Input 1
 add_interface_port cpu_3 halt_3 halt Output 1
 add_interface_port cpu_3 step_3 step Output 1
diff --git a/tb/models/smp.py b/tb/models/smp.py
index c5bb760..deb463c 100644
--- a/tb/models/smp.py
+++ b/tb/models/smp.py
@@ -26,7 +26,10 @@ class SmpPe:
         self._halted = halt_on_reset
 
     def read(self):
-        return self._bkpt << 1 | self._halted
+        # bit 2 es alive
+        return 1 << 2 \
+            | self._bkpt << 1 \
+            | self._halted
 
     def halt(self):
         self._halted = 1
diff --git a/tb/top/conspiracion/platform.sv b/tb/top/conspiracion/platform.sv
index 9c6fb27..6302d53 100644
--- a/tb/top/conspiracion/platform.sv
+++ b/tb/top/conspiracion/platform.sv
@@ -93,6 +93,7 @@ module platform
 		.step(step_0),
 		.breakpoint(breakpoint_0),
 		.cpu_halt(halt_0),
+		.cpu_alive(cpu_alive_0),
 		.cpu_halted(cpu_halted_0),
 		.avl_address(cpu_0_address),
 		.avl_read(cpu_0_read),
@@ -111,6 +112,7 @@ module platform
 		.step(step_1),
 		.breakpoint(breakpoint_1),
 		.cpu_halt(halt_1),
+		.cpu_alive(cpu_alive_1),
 		.cpu_halted(cpu_halted_1),
 		.avl_address(cpu_1_address),
 		.avl_read(cpu_1_read),
@@ -130,6 +132,7 @@ module platform
 		.step(step_2),
 		.breakpoint(breakpoint_2),
 		.cpu_halt(halt_2),
+		.cpu_alive(cpu_alive_2),
 		.cpu_halted(cpu_halted_2),
 		.avl_address(cpu_2_address),
 		.avl_read(cpu_2_read),
@@ -149,6 +152,7 @@ module platform
 		.step(step_3),
 		.breakpoint(breakpoint_3),
 		.cpu_halt(halt_3),
+		.cpu_alive(cpu_alive_3),
 		.cpu_halted(cpu_halted_3),
 		.avl_address(cpu_3_address),
 		.avl_read(cpu_3_read),
@@ -419,6 +423,7 @@ module platform
 	logic step_0, step_1, step_2, step_3,
 	      halt_0, halt_1, halt_2, halt_3,
 	      breakpoint_0, breakpoint_1, breakpoint_2, breakpoint_3,
+	      cpu_alive_0, cpu_alive_1, cpu_alive_2, cpu_alive_3,
 	      cpu_halted_0, cpu_halted_1, cpu_halted_2, cpu_halted_3;
 
 	smp_ctrl smp
diff --git a/tb/top/test_smp.py b/tb/top/test_smp.py
index a3cb61f..b7903ce 100644
--- a/tb/top/test_smp.py
+++ b/tb/top/test_smp.py
@@ -9,6 +9,11 @@ from tb.models import CorePaceModel, SmpModel
 async def bring_up(dut):
     await cocotb.start(Clock(dut.clk, 2).start())
 
+    dut.cpu_alive_0.value = 1
+    dut.cpu_alive_1.value = 1
+    dut.cpu_alive_2.value = 1
+    dut.cpu_alive_3.value = 1
+
     dut.rst_n.value = 1
     await Timer(1)
     dut.rst_n.value = 0