Implement branching in fetch stage

3 files changed, 41 insertions, 18 deletions

diff --git a/rtl/core/arm810.sv b/rtl/core/arm810.sv
index b344836..5ba1a11 100644
--- a/rtl/core/arm810.sv
+++ b/rtl/core/arm810.sv
@@ -18,7 +18,8 @@ module arm810
 
 	core_fetch #(.PREFETCH_ORDER(2)) fetch
 	(
-		.flush(explicit_branch | wr_pc),
+		.branch(explicit_branch | wr_pc),
+		.flush(0), //TODO
 		.target(wr_pc ? wr_value[29:0] : branch_target),
 		.addr(bus_addr),
 		.fetched(bus_ready),
diff --git a/rtl/core/fetch/fetch.sv b/rtl/core/fetch/fetch.sv
index e8f4832..a57c679 100644
--- a/rtl/core/fetch/fetch.sv
+++ b/rtl/core/fetch/fetch.sv
@@ -5,6 +5,7 @@ module core_fetch
 (
 	input  logic clk,
 	             stall,
+	             branch,
 	             flush,
 	             fetched,
 	input  word  fetch_data,
@@ -16,19 +17,39 @@ module core_fetch
 	             addr
 );
 
-	ptr next_pc;
+	ptr next_pc, head;
+	logic fetched_valid, do_flush, discard;
+
+	assign do_flush = branch | flush;
+	assign fetched_valid = fetched & ~discard;
 
 	core_prefetch #(.ORDER(PREFETCH_ORDER)) prefetch
 	(
+		.flush(do_flush),
+		.fetched(fetched_valid),
 		.*
 	);
 
-	always_ff @(posedge clk)
-		if(flush)
-			addr <= next_pc;
-		else if(fetched)
+	always_comb
+		if(branch)
+			head = target;
+		else if(flush)
+			head = next_pc;
+		else
+			head = {30{1'bx}};
+
+	always_ff @(posedge clk) begin
+		if(do_flush)
+			addr <= head;
+		else if(fetched_valid)
 			addr <= addr + 1;
 
-	initial addr = 0;
+		discard <= discard ? ~fetched : do_flush & fetch;
+	end
+
+	initial begin
+		addr = 0;
+		discard = 0;
+	end
 
 endmodule
diff --git a/rtl/core/fetch/prefetch.sv b/rtl/core/fetch/prefetch.sv
index 4a51e46..486ec96 100644
--- a/rtl/core/fetch/prefetch.sv
+++ b/rtl/core/fetch/prefetch.sv
@@ -3,16 +3,17 @@
 module core_prefetch
 #(parameter ORDER=2)
 (
-	input  logic       clk,
-	                   stall,
-	                   flush,
-	                   fetched,
-	input  logic[31:0] fetch_data,
+	input  logic clk,
+	             stall,
+	             flush,
+	             fetched,
+	input  word  fetch_data,
+	input  ptr   head,
 
-	output logic[31:0] insn,
-	output logic[29:0] insn_pc,
-	                   next_pc,
-	output logic       fetch
+	output word  insn,
+	output ptr   insn_pc,
+	             next_pc,
+	output logic fetch
 );
 
 	localparam SIZE = (1 << ORDER) - 1;
@@ -20,7 +21,7 @@ module core_prefetch
 	logic[31:0] prefetch[SIZE];
 	logic[ORDER - 1:0] valid;
 
-	assign insn = ~flush ? prefetch[0] : `NOP;
+	assign insn = flush ? `NOP : prefetch[0];
 	assign next_pc = ~stall & |valid ? insn_pc + 1 : insn_pc;
 
 	always_comb
@@ -30,7 +31,7 @@ module core_prefetch
 			fetch = ~&valid;
 
 	always_ff @(posedge clk) begin
-		insn_pc <= next_pc;
+		insn_pc <= flush ? head : next_pc;
 
 		if(~flush & fetched & (valid == SIZE - 1))
 			prefetch[SIZE - 1] <= fetch_data;