Implement sub-word memory accesses

2 files changed, 69 insertions, 5 deletions

diff --git a/rtl/core/control/ldst/ldst.sv b/rtl/core/control/ldst/ldst.sv
index 12bd6b9..02f7b4a 100644
--- a/rtl/core/control/ldst/ldst.sv
+++ b/rtl/core/control/ldst/ldst.sv
@@ -10,6 +10,7 @@ module core_control_ldst
 	                   mem_ready,
 	input  word        rd_value_b,
 	                   q_alu,
+	                   q_shifter,
 
 	input  ctrl_cycle  cycle,
 	                   next_cycle,
@@ -25,16 +26,20 @@ module core_control_ldst
 	                   ldst,
 	                   ldst_next,
 	                   ldst_writeback,
+	output logic[1:0]  ldst_shift,
+	output word        ldst_read,
 	output reg_num     popped
 );
 
-	logic pre, increment;
+	word base;
+	logic pre, increment, sign_extend;
 	reg_num popped_upper, popped_lower;
 	reg_list mem_regs, next_regs_upper, next_regs_lower;
+	ldst_size size;
 
 	assign popped = increment ? popped_lower : popped_upper;
 	assign ldst_next = !cycle.transfer || mem_ready;
-	assign mem_data_wr = rd_value_b;
+	assign mem_data_wr = q_shifter;
 
 	core_control_ldst_pop pop
 	(
@@ -46,14 +51,26 @@ module core_control_ldst
 		.pop_lower(popped_lower)
 	);
 
+	core_control_ldst_sizes sizes
+	(
+		.addr(mem_addr),
+		.read(ldst_read),
+		.shift(ldst_shift),
+		.fault(), //TODO: alignment check
+		.byteenable(), //TODO
+		.*
+	);
+
 	always_ff @(posedge clk or negedge rst_n)
 		if(!rst_n) begin
 			pre <= 0;
 			ldst <= 0;
+			size <= LDST_WORD;
 			increment <= 0;
+			sign_extend <= 0;
 			ldst_writeback <= 0;
 
-			mem_addr <= {$bits(mem_addr){1'b0}};
+			base <= {$bits(base){1'b0}};
 			mem_regs <= {$bits(mem_regs){1'b0}};
 			mem_write <= 0;
 			mem_start <= 0;
@@ -63,12 +80,13 @@ module core_control_ldst
 
 			if(next_cycle.issue) begin
 				// TODO: dec.ldst.unprivileged
-				// TODO: byte/halfword sizes
 				if(issue)
 					ldst <= dec.ctrl.ldst;
 
 				pre <= dec.ldst.pre_indexed;
+				size <= dec.ldst.size;
 				increment <= dec.ldst.increment;
+				sign_extend <= dec.ldst.sign_extend;
 				ldst_writeback <= dec.ldst.writeback;
 
 				mem_regs <= dec.ldst.regs;
@@ -80,8 +98,8 @@ module core_control_ldst
 				end
 
 				if(ldst_next) begin
+					base <= pre ? q_alu : alu_a;
 					mem_regs <= increment ? next_regs_lower : next_regs_upper;
-					mem_addr <= pre ? q_alu[31:2] : alu_a[31:2];
 				end
 
 				mem_start <= !cycle.transfer || (mem_ready && pop_valid);
diff --git a/rtl/core/control/ldst/sizes.sv b/rtl/core/control/ldst/sizes.sv
new file mode 100644
index 0000000..dff4662
--- /dev/null
+++ b/rtl/core/control/ldst/sizes.sv
@@ -0,0 +1,46 @@
+`include "core/uarch.sv"
+
+module core_control_ldst_sizes
+(
+	input  word       base,
+	                  q_shifter,
+	input  ldst_size  size,
+	input  logic      sign_extend,
+
+	output ptr        addr,
+	output word       read,
+	output logic[1:0] shift,
+	output logic[3:0] byteenable,
+	output logic      fault
+);
+
+	assign {addr, shift} = base;
+
+	always_comb
+		unique case(size)
+			LDST_BYTE: begin
+				read = {{24{q_shifter[7] && sign_extend}}, q_shifter[7:0]};
+				fault = 0;
+
+				unique case(shift)
+					2'b00: byteenable = 4'b0001;
+					2'b01: byteenable = 4'b0010;
+					2'b10: byteenable = 4'b0100;
+					2'b11: byteenable = 4'b1000;
+				endcase
+			end
+
+			LDST_HALF: begin
+				read = {{16{q_shifter[15] && sign_extend}}, q_shifter[15:0]};
+				fault = shift[0];
+				byteenable = shift[1] ? 4'b1100 : 4'b0011;
+			end
+
+			LDST_WORD: begin
+				read = q_shifter;
+				fault = shift[1] || shift[0];
+				byteenable = 4'b1111;
+			end
+		endcase
+
+endmodule