platform: implement SMP controller

2 files changed, 131 insertions, 0 deletions

diff --git a/rtl/smp/pe.sv b/rtl/smp/pe.sv
new file mode 100644
index 0000000..f50ed2f
--- /dev/null
+++ b/rtl/smp/pe.sv
@@ -0,0 +1,47 @@
+module mp_pe
+#(parameter IS_BSP=0)
+(
+	input  logic	  clk,
+	                  rst_n,
+	
+	input  logic      write,
+	input  logic[7:0] writedata,
+	output logic[7:0] readdata,
+
+	input logic       cpu_halted,
+	                  breakpoint,
+
+	output logic      halt,
+	                  step
+);
+
+	struct packed
+	{
+		logic step, halt, run;
+	} req;
+
+	struct packed
+	{
+		logic breakpoint, cpu_halted;
+	} status;
+
+	assign req = writedata[$bits(req) - 1:0];
+	assign readdata = {{(8 - $bits(status)){1'b0}}, status};
+
+	always @(posedge clk or negedge rst_n)
+		if (!rst_n) begin
+			halt <= IS_BSP ? 0 : 1; // Boot es single-core
+			step <= 0;
+			status <= {($bits(status)){1'b0}};
+		end else begin
+			status.breakpoint <= breakpoint;
+			status.cpu_halted <= cpu_halted;
+
+			//Se hace halt hasta el siguiente ciclo después de que se 
+			//solicita el breakpoint
+			step <= !breakpoint || (req.step && write);
+			halt <= (halt || breakpoint || (req.halt && write))
+			     && !((req.run || req.step) && write);
+		end
+
+endmodule
diff --git a/rtl/smp/smp_ctrl.sv b/rtl/smp/smp_ctrl.sv
new file mode 100644
index 0000000..b6123ad
--- /dev/null
+++ b/rtl/smp/smp_ctrl.sv
@@ -0,0 +1,84 @@
+module smp_ctrl
+(
+	input  logic	   clk,
+	                   rst_n,
+	
+	input  logic       avl_read,
+	                   avl_write,
+	input  logic[31:0] avl_writedata,
+	output logic[31:0] avl_readdata,
+
+	input logic        cpu_halted_0,
+	                   cpu_halted_1,
+	                   cpu_halted_2,
+	                   cpu_halted_3,
+	input logic        breakpoint_0,
+	                   breakpoint_1,
+	                   breakpoint_2,
+	                   breakpoint_3,	
+
+	output logic       halt_0,
+	                   halt_1,
+	                   halt_2,
+	                   halt_3,
+	                   step_0,
+	                   step_1,
+	                   step_2,
+	                   step_3
+);
+
+	logic write;
+	logic[7:0] readdata_3, readdata_2, readdata_1, readdata_0,
+	           writedata_3, writedata_2, writedata_1, writedata_0;
+
+	assign avl_readdata = {readdata_3, readdata_2, readdata_1, readdata_0};
+	assign {writedata_3, writedata_2, writedata_1, writedata_0} = avl_writedata;
+
+	// No hay addresses
+	assign write = avl_write;
+
+	mp_pe #(.IS_BSP(1)) pe_0
+	(
+		.step(step_0),
+		.halt(halt_0),
+		.cpu_halted(cpu_halted_0),
+		.breakpoint(breakpoint_0),
+		.readdata(readdata_0),
+		.writedata(writedata_0),
+		.*
+	);
+
+	mp_pe pe_1
+	(
+		.step(step_1),
+		.halt(halt_1),
+		.cpu_halted(cpu_halted_1),
+		.breakpoint(breakpoint_1),
+		.readdata(readdata_1),
+		.writedata(writedata_1),
+		.*
+	);
+
+	mp_pe pe_2
+	(
+		.step(step_2),
+		.halt(halt_2),
+		.cpu_halted(cpu_halted_2),
+		.breakpoint(breakpoint_2),
+		.readdata(readdata_2),
+		.writedata(writedata_2),
+		.*
+	);
+
+	mp_pe pe_3
+	(
+		.step(step_3),
+		.halt(halt_3),
+		.cpu_halted(cpu_halted_3),
+		.breakpoint(breakpoint_3),
+		.readdata(readdata_3),
+		.writedata(writedata_3),
+		.*
+	);
+
+endmodule