X-Git-Url: http://git.joshuawise.com/firearm.git/blobdiff_plain/4d7253f1058e6bd63bfc720a5b881e85d99270ee..b45b7b7cecfef2b61eb74a6047c6994a100299be:/Memory.v

diff --git a/Memory.v b/Memory.v
index 4b41dca..7ca786e 100644
--- a/Memory.v
+++ b/Memory.v
@@ -36,6 +36,7 @@ module Memory(
 	input [31:0] op2,
 	input [31:0] spsr,
 	input [31:0] cpsr,
+	input cpsrup,
 	input write_reg,
 	input [3:0] write_num,
 	input [31:0] write_data,
@@ -49,10 +50,12 @@ module Memory(
 	output reg [3:0] out_write_num = 4'bxxxx,
 	output reg [31:0] out_write_data = 32'hxxxxxxxx,
 	output reg [31:0] outspsr = 32'hxxxxxxxx,
-	output reg [31:0] outcpsr = 32'hxxxxxxxx
+	output reg [31:0] outcpsr = 32'hxxxxxxxx,
+	output reg outcpsrup = 1'hx
 	);
 
 	reg [31:0] addr, raddr, prev_raddr, next_regdata, next_outcpsr;
+	reg next_outcpsrup;
 	reg [31:0] prevaddr;
 	reg [3:0] next_regsel, cur_reg, prev_reg;
 	reg next_writeback;
@@ -92,6 +95,7 @@ module Memory(
 		prev_raddr <= raddr;
 		outcpsr <= next_outcpsr;
 		outspsr <= spsr;
+		outcpsrup <= next_outcpsrup;
 		swp_state <= next_swp_state;
 		lsm_state <= next_lsm_state;
 		lsr_state <= next_lsr_state;
@@ -126,6 +130,7 @@ module Memory(
 		cp_write = 32'hxxxxxxxx;
 		offset = prev_offset;
 		next_outcpsr = lsm_state == 4'b0010 ? outcpsr : cpsr;
+		next_outcpsrup = cpsrup;
 		lsrh_rddata = 32'hxxxxxxxx;
 		lsrh_rddata_s1 = 16'hxxxx;
 		lsrh_rddata_s2 = 8'hxx;
@@ -205,7 +210,7 @@ module Memory(
 				if(insn[20]) begin
 					next_write_reg = 1'b1;
 				end
-				if(insn[21] | !insn[24] && !flush) begin
+				if(insn[21] | !insn[24]) begin
 					outstall = 1'b1;
 					if(!rw_wait)
 						next_lsrh_state = 3'b010;
@@ -225,6 +230,11 @@ module Memory(
 			end
 			default: begin end
 			endcase
+			
+			if ((lsrh_state == 3'b001) && flush) begin	/* Reject it. */
+				outstall = 1'b0;
+				next_lsrh_state = 3'b001;
+			end
 		end
 		`DECODE_LDRSTR_UNDEFINED: begin end
 		`DECODE_LDRSTR: if(!inbubble) begin
@@ -249,7 +259,7 @@ module Memory(
 				if(insn[20] /* L */) begin
 					next_write_data = align_rddata;
 				end
-				if(insn[21] /* W */ | !insn[24] /* P */ && !flush /* don't move on if we get a flush on the first time around. */) begin
+				if(insn[21] /* W */ | !insn[24] /* P */) begin
 					outstall = 1'b1;
 					if(!rw_wait)
 						next_lsr_state = 3'b010;
@@ -270,6 +280,11 @@ module Memory(
 			end
 			default: begin end
 			endcase
+			
+			if ((lsr_state == 3'b001) && flush) begin	/* Reject it. */
+				outstall = 1'b0;
+				next_lsr_state = 3'b001;
+			end
 		end
 		/* XXX ldm/stm incorrect in that stupid case where one of the listed regs is the base reg */
 		`DECODE_LDMSTM: if(!inbubble) begin
@@ -284,10 +299,8 @@ module Memory(
 				next_regs = insn[23] /* U */ ? op1[15:0] : {op1[0], op1[1], op1[2], op1[3], op1[4], op1[5], op1[6], op1[7],
 				                                            op1[8], op1[9], op1[10], op1[11], op1[12], op1[13], op1[14], op1[15]};
 				offset = 6'b0;
-				if (!flush /* Don't move on if we got a flush on the first time around. */) begin
-					outstall = 1'b1;
-					next_lsm_state = 4'b0010;
-				end
+				outstall = 1'b1;
+				next_lsm_state = 4'b0010;
 			end
 			4'b0010: begin
 				rd_req = insn[20];
@@ -365,6 +378,7 @@ module Memory(
 				cur_reg = insn[23] ? cur_reg : 4'hF - cur_reg;
 				if(cur_reg == 4'hF && insn[22]) begin
 					next_outcpsr = spsr;
+					next_outcpsrup = 1;
 				end
 
 				offset = prev_offset + 6'h4;
@@ -407,6 +421,10 @@ module Memory(
 			end
 			default: $stop;
 			endcase
+			if ((lsm_state == 4'b0001) && flush) begin	/* Reject it. */
+				outstall = 1'b0;
+				next_lsm_state = 4'b0001;
+			end
 			$display("LDMSTM: Decoded, bubble %d, insn %08x, lsm state %b -> %b, stall %d", inbubble, insn, lsm_state, next_lsm_state, outstall);
 		end
 		`DECODE_LDCSTC: if(!inbubble) begin
@@ -433,8 +451,10 @@ module Memory(
 					next_write_reg = 1'b1;
 					next_write_num = insn[15:12];
 					next_write_data = cp_read;
-				end else
+				end else begin
 					next_outcpsr = {cp_read[31:28], cpsr[27:0]};
+					next_outcpsrup = 1;
+				end
 			end
 			if (cp_busy) begin
 				outstall = 1;