support setting PC for branch and jump instructions

2 files changed, 34 insertions, 4 deletions

diff --git a/alu_stage.v b/alu_stage.v
index 3761f25..7bdbfe8 100644
--- a/alu_stage.v
+++ b/alu_stage.v
@@ -20,6 +20,10 @@ module alu_stage (
 	input [15:0] memData_in,    // read in from memory
 	output reg [15:0] memData_out,  // to write out to memory
 	
+	// setting PC
+	output setPC,
+	output [15:0] setPCValue,
+	
 	output reg [32:0] control_signals_out,
 	output reg [15:0] imm_out,
 	output reg [15:0] pc_out,
@@ -90,14 +94,29 @@ module alu_stage (
 		.carry(aluCarry)
 	);
 	
+	wire setRegZCond, setRegCCond, setSomeReg;
+	assign setRegZCond = setRegCond_in[4] | (setRegCond_in[3] == statusReg[1]);
+	assign setRegCCond = setRegCond_in[1] | (setRegCond_in[0] == statusReg[0]);
+	assign setSomeReg =
+		en &
+		setRegCond_in[5] &
+		(
+			setRegCond_in[2] ?
+				(setRegZCond & setRegCCond) :
+				(setRegZCond | setRegCCond)
+		);
+
 	assign rf_regA = aluReg1_in;
 	assign rf_regB = aluReg2_in;
-	assign rf_we = en & (regSetH_in | regSetL_in);
+	assign rf_we = setSomeReg & !aluDest_in;
 	assign rf_hb = regSetH_in;
 	assign rf_lb = regSetL_in;
 	assign rf_dataIn = aluResult;
 	assign rf_regDest = regDest_in;
 
+	assign setPC = setSomeReg & aluDest_in;
+	assign setPCValue = aluResult;
+	
 	always @(posedge clk) begin
 		if(en) begin
 			control_signals_out <= control_signals_in;
diff --git a/nqcpu.v b/nqcpu.v
index 34935c6..c27c742 100644
--- a/nqcpu.v
+++ b/nqcpu.v
@@ -31,6 +31,9 @@ module nqcpu (
 	output [15:0] dbg_r7,
 	output [9:0] dbg_state,
 	
+	output dbg_setPC,
+	output [15:0] dbg_setPCValue,
+	
 	output [1:0] dbg_statusreg
 );
 
@@ -40,7 +43,8 @@ module nqcpu (
 		pc = 16'h0;
 	end
 
-	wire fetch_en, decode_en, alu_en, incr_pc;
+	wire fetch_en, decode_en, alu_en, incr_pc, setPC;
+	wire [15:0] setPCValue;
 
 	wire fetch_ready;
 	wire [15:0] fetched_instr;
@@ -58,6 +62,9 @@ module nqcpu (
 		if(incr_pc) begin
 			pc <= pc + 16'h2;
 		end
+		else if (setPC & alu_en) begin
+			pc <= setPCValue;
+		end
 	end
 
 	wire [32:0] ctrl_from_decoder;
@@ -130,6 +137,9 @@ module nqcpu (
 		.memData_in(16'hDEAD),    // read in from memory
 		//.memData_out(??),  // to write out to memory
 		
+		.setPC(setPC),
+		.setPCValue(setPCValue),
+
 		.control_signals_out(ctrl_from_alu),
 		.imm_out(imm_from_alu),
 		.pc_out(pc_from_alu),
@@ -174,6 +184,7 @@ module nqcpu (
 		.setRegCond(debugSetRegCond)
 	);
 	
-	assign debugPC = pc_from_alu;
-
+	assign debugPC = pc;
+	assign dbg_setPC = setPC;
+	assign dbg_setPCValue = setPCValue;
 endmodule