5 files changed, 75 insertions, 18 deletions

diff --git a/rtl/core/decode/decode.sv b/rtl/core/decode/decode.sv
index d14841b..16b53d4 100644
--- a/rtl/core/decode/decode.sv
+++ b/rtl/core/decode/decode.sv
@@ -13,12 +13,12 @@ module core_decode
 	                   branch,
 	output ptr         branch_offset,
 	output snd_decode  snd_ctrl,
-	output data_decode data_ctrl
+	output data_decode data_ctrl,
+	output ldst_decode ldst_ctrl
 );
 
 	//TODO
 	logic restore_spsr;
-	ldst_decode ldst_ctrl;
 
 	logic cond_undefined;
 
@@ -77,10 +77,16 @@ module core_decode
 	);
 
 	ldst_decode ldst_misc;
+	logic ldst_misc_off_is_reg;
+	reg_num ldst_misc_off_reg;
+	logic[7:0] ldst_misc_off_imm;
 
 	core_decode_ldst_misc group_ldst_misc
 	(
 		.decode(ldst_misc),
+		.off_imm(ldst_misc_off_imm),
+		.off_reg(ldst_misc_off_reg),
+		.off_is_reg(ldst_misc_off_is_reg),
 		.*
 	);
 
@@ -94,6 +100,15 @@ module core_decode
 		.*
 	);
 
+	ldst_decode ldst_addr;
+	data_decode data_ldst;
+
+	core_decode_ldst_addr ldst2data
+	(
+		.ldst(ldst_addr),
+		.alu(data_ldst)
+	);
+
 	always_comb begin
 		undefined = cond_undefined;
 
@@ -102,10 +117,14 @@ module core_decode
 		update_flags = 0;
 		data_ctrl = {($bits(data_ctrl)){1'bx}};
 
-		snd_ctrl = {($bits(snd_ctrl)){1'bx}};
-		snd_ctrl.shl = 0;
+		snd_ctrl = {$bits(snd_ctrl){1'bx}};
+		snd_ctrl.shl = 1;
 		snd_ctrl.shr = 0;
 		snd_ctrl.ror = 0;
+		snd_ctrl.is_imm = 1;
+		snd_ctrl.shift_imm = {$bits(snd_ctrl.shift_imm){1'b0}};
+		snd_ctrl.shift_by_reg = 0;
+
 		snd_is_imm = 1'bx;
 		snd_ror_if_imm = 1'bx;
 		snd_shift_by_reg_if_reg = 1'bx;
@@ -148,7 +167,7 @@ module core_decode
 
 				snd_ctrl = snd;
 				ldst_ctrl = ldst_single;
-				ldst_ctrl.enable = 1;
+				ldst_addr = ldst_single;
 
 				undefined = undefined | snd_undefined;
 			end
@@ -157,15 +176,22 @@ module core_decode
 				priority casez(insn `FIELD_OP)
 					`INSN_LDRB, `INSN_LDRSB, `INSN_LDRSH, `INSN_STRH: begin
 						ldst_ctrl = ldst_misc;
-						ldst_ctrl.enable = 1;
+						ldst_addr = ldst_misc;
+
+						snd_ctrl.r = ldst_misc_off_reg;
+						snd_ctrl.imm = {4'b0, ldst_misc_off_imm};
+						snd_ctrl.is_imm = !ldst_misc_off_is_reg;
 					end
 
-					default: undefined = 1;
+					default:
+						undefined = 1;
 				endcase
 
 			`GROUP_LDST_MULT: begin
 				ldst_ctrl = ldst_multiple;
-				ldst_ctrl.enable = 1;
+				ldst_addr = ldst_multiple;
+				snd_ctrl.imm = 12'd4;
+
 				restore_spsr = ldst_mult_restore_spsr;
 			end
 
@@ -178,6 +204,16 @@ module core_decode
 			default: undefined = 1;
 		endcase
 
+		unique casez(insn `FIELD_OP)
+			`GROUP_LDST_SINGLE, `GROUP_LDST_MISC, `GROUP_LDST_MULT: begin
+				ldst_ctrl.enable = 1;
+				data_ctrl = data_ldst;
+				writeback = ldst_ctrl.writeback || ldst_ctrl.load;
+			end
+
+			default: ;
+		endcase
+
 		if(undefined) begin
 			branch = 1'bx;
 			writeback = 1'bx;
diff --git a/rtl/core/decode/ldst/addr.sv b/rtl/core/decode/ldst/addr.sv
new file mode 100644
index 0000000..4a61231
--- /dev/null
+++ b/rtl/core/decode/ldst/addr.sv
@@ -0,0 +1,14 @@
+`include "core/uarch.sv"
+
+module core_decode_ldst_addr
+(
+	input  ldst_decode ldst,
+
+	output data_decode alu
+);
+
+	assign alu.op = ldst.increment ? `ALU_ADD : `ALU_SUB;
+	assign alu.rn = ldst.rn;
+	assign alu.rd = ldst.rd;
+
+endmodule
diff --git a/rtl/core/decode/ldst/misc.sv b/rtl/core/decode/ldst/misc.sv
index de2428f..f2fe258 100644
--- a/rtl/core/decode/ldst/misc.sv
+++ b/rtl/core/decode/ldst/misc.sv
@@ -5,7 +5,10 @@ module core_decode_ldst_misc
 (
 	input  word        insn,
 
-	output ldst_decode decode
+	output ldst_decode decode,
+	output logic       off_is_reg,
+	output logic[7:0]  off_imm,
+	output reg_num     off_reg
 );
 
 	logic p, w;
@@ -17,10 +20,14 @@ module core_decode_ldst_misc
 	assign decode.increment = insn `FIELD_LDST_MISC_U;
 	assign decode.writeback = !p || w;
 	assign decode.sign_extend = insn `FIELD_LDST_MISC_S;
-	assign decode.pre_indexed = p && w;
+	assign decode.pre_indexed = p;
 	assign decode.unprivileged = 0;
 	assign decode.user_regs = 0;
-	assign decode.reg_list = 16'b0;
+	assign decode.regs = 16'b0;
+
+	assign off_imm = {insn `FIELD_LDST_MISC_IMM_HI, insn `FIELD_LDST_MISC_IMM_LO};
+	assign off_reg = insn `FIELD_LDST_MISC_RM;
+	assign off_is_reg = insn `FIELD_LDST_MISC_REG;
 
 	assign p = insn `FIELD_LDST_MISC_P;
 	assign w = insn `FIELD_LDST_MISC_W;
diff --git a/rtl/core/decode/ldst/multiple.sv b/rtl/core/decode/ldst/multiple.sv
index 24e8e5b..d286a67 100644
--- a/rtl/core/decode/ldst/multiple.sv
+++ b/rtl/core/decode/ldst/multiple.sv
@@ -10,7 +10,7 @@ module core_decode_ldst_multiple
 );
 
 	logic s, l;
-	logic[15:0] reg_list;
+	reg_list regs;
 
 	assign decode.rn = insn `FIELD_LDST_MULT_RN;
 	assign decode.size = LDST_WORD;
@@ -20,13 +20,13 @@ module core_decode_ldst_multiple
 	assign decode.sign_extend = 0;
 	assign decode.pre_indexed = insn `FIELD_LDST_MULT_P;
 	assign decode.unprivileged = 0;
-	assign decode.user_regs = s && (!l || !reg_list[`R15]);
-	assign decode.reg_list = reg_list;
+	assign decode.user_regs = s && !(l && regs[`R15]);
+	assign decode.regs = regs;
 
 	assign s = insn `FIELD_LDST_MULT_S;
 	assign l = insn `FIELD_LDST_LD;
 
-	assign reg_list = insn `FIELD_LDST_MULT_LIST;
-	assign restore_spsr = s && l && reg_list[`R15];
+	assign regs = insn `FIELD_LDST_MULT_LIST;
+	assign restore_spsr = s && l && regs[`R15];
 
 endmodule
diff --git a/rtl/core/decode/ldst/single.sv b/rtl/core/decode/ldst/single.sv
index 5bcb638..0665178 100644
--- a/rtl/core/decode/ldst/single.sv
+++ b/rtl/core/decode/ldst/single.sv
@@ -18,10 +18,10 @@ module core_decode_ldst_single
 	assign decode.increment = insn `FIELD_LDST_SINGLE_U;
 	assign decode.writeback = !p || w;
 	assign decode.sign_extend = 0;
-	assign decode.pre_indexed = p && w;
+	assign decode.pre_indexed = p;
 	assign decode.unprivileged = !p && w;
 	assign decode.user_regs = 0;
-	assign decode.reg_list = 16'b0;
+	assign decode.regs = 16'b0;
 
 	assign p = insn `FIELD_LDST_SINGLE_P;
 	assign w = insn `FIELD_LDST_SINGLE_W;