Add CPU execution monitor

Signed-off-by: Joachim Strömbergson <joachim@assured.se>

hw/application_fpga/core/tk1/rtl/tk1.v

@@ -20,6 +20,12 @@ module tk1(
 	   input wire           cpu_trap,
 	   output wire          fw_app_mode,
 
+	   input wire  [31 : 0] cpu_addr,
+	   input wire           cpu_instr,
+	   input wire           cpu_valid,
+	   output wire          force_jump,
+	   output wire [31 : 0] jump_instr,
+
            output wire          led_r,
            output wire          led_g,
            output wire          led_b,
@@ -70,6 +76,11 @@ module tk1(
   localparam ADDR_UDI_FIRST  = 8'h30;
   localparam ADDR_UDI_LAST   = 8'h31;
 
+  localparam ADDR_CPU_MON_CTRL  = 8'h60;
+  localparam ADDR_CPU_MON_FIRST = 8'h61;
+  localparam ADDR_CPU_MON_LAST  = 8'h62;
+  localparam ADDR_CPU_MON_INSTR = 8'h63;
+
   localparam TK1_NAME0       = 32'h746B3120; // "tk1 "
   localparam TK1_NAME1       = 32'h6d6b6466; // "mkdf"
   localparam TK1_VERSION     = 32'h00000004;
@@ -112,6 +123,15 @@ module tk1(
   reg [2 : 0]  cpu_trap_led_new;
   reg          cpu_trap_led_we;
 
+  reg          cpu_mon_en_reg;
+  reg          cpu_mon_en_we;
+  reg [31 : 0] cpu_mon_first_reg;
+  reg          cpu_mon_first_we;
+  reg [31 : 0] cpu_mon_last_reg;
+  reg          cpu_mon_last_we;
+  reg [31 : 0] cpu_mon_instr_reg;
+  reg          cpu_mon_instr_we;
+
 
   //----------------------------------------------------------------
   // Wires.
@@ -119,6 +139,7 @@ module tk1(
   /* verilator lint_off UNOPTFLAT */
   reg [31 : 0] tmp_read_data;
   reg          tmp_ready;
+  reg          tmp_force_jump;
   /* verilator lint_on UNOPTFLAT */
 
   reg [2 : 0]  muxed_led;
@@ -132,6 +153,9 @@ module tk1(
 
   assign fw_app_mode = switch_app_reg;
 
+  assign force_jump = tmp_force_jump;
+  assign jump_instr = cpu_mon_instr_reg;
+
   assign gpio3 = gpio3_reg;
   assign gpio4 = gpio4_reg;
 
@@ -165,7 +189,7 @@ module tk1(
     begin : reg_update
       if (!reset_n) begin
 	switch_app_reg    <= 1'h0;
-        led_reg          <= 3'h0;
+        led_reg           <= 3'h6;
         gpio1_reg         <= 2'h0;
         gpio2_reg         <= 2'h0;
         gpio3_reg         <= 1'h0;
@@ -183,6 +207,10 @@ module tk1(
 	cdi_mem[7]        <= 32'h0;
 	cpu_trap_ctr_reg  <= 24'h0;
 	cpu_trap_led_reg  <= 3'h0;
+        cpu_mon_en_reg    <= 1'h0;
+	cpu_mon_first_reg <= 32'h0;
+	cpu_mon_last_reg  <= 32'h0;
+	cpu_mon_instr_reg <= 32'h0;
       end
 
       else begin
@@ -229,6 +257,22 @@ module tk1(
 	if (cpu_trap_led_we) begin
 	  cpu_trap_led_reg <= cpu_trap_led_new;
 	end
+
+	if (cpu_mon_en_we) begin
+	  cpu_mon_en_reg <= write_data[0];
+	end
+
+	if (cpu_mon_first_we) begin
+	  cpu_mon_first_reg <= write_data;
+	end
+
+	if (cpu_mon_last_we) begin
+	  cpu_mon_last_reg <= write_data;
+	end
+
+	if (cpu_mon_instr_we) begin
+	  cpu_mon_instr_reg <= write_data;
+	end
       end
     end // reg_update
 
@@ -256,6 +300,24 @@ module tk1(
     end
 
 
+  //----------------------------------------------------------------
+  // cpu_monitor
+  //----------------------------------------------------------------
+  always @*
+    begin : cpu_monitor
+      tmp_force_jump = 1'h0;
+
+      if (cpu_mon_en_reg) begin
+	if (cpu_valid && cpu_instr) begin
+	  if ((cpu_addr >= cpu_mon_first_reg) &&
+	      (cpu_addr <= cpu_mon_last_reg)) begin
+	    tmp_force_jump = 1'h1;
+	  end
+	end
+      end
+    end
+
+
   //----------------------------------------------------------------
   // api
   //----------------------------------------------------------------
@@ -270,6 +332,11 @@ module tk1(
       blake2s_addr_we  = 1'h0;
       cdi_mem_we       = 1'h0;
       cdi_mem_we       = 1'h0;
+      cpu_mon_en_we    = 1'h0;
+      cpu_mon_first_we = 1'h0;
+      cpu_mon_last_we  = 1'h0;
+      cpu_mon_instr_we = 1'h0;
+      cpu_mon_en_we    = 1'h0;
       tmp_read_data    = 32'h0;
       tmp_ready        = 1'h0;
 
@@ -312,6 +379,22 @@ module tk1(
 	      cdi_mem_we = 1'h1;
 	    end
 	  end
+
+	  if (address == ADDR_CPU_MON_CTRL) begin
+	    cpu_mon_en_we = 1'h1;
+	  end
+
+	  if (address == ADDR_CPU_MON_FIRST) begin
+	    cpu_mon_first_we = 1'h1;
+	  end
+
+	  if (address == ADDR_CPU_MON_LAST) begin
+	    cpu_mon_last_we = 1'h1;
+	  end
+
+	  if (address == ADDR_CPU_MON_INSTR) begin
+	    cpu_mon_instr_we = 1'h1;
+	  end
 	end
 
         else begin

hw/application_fpga/rtl/application_fpga.v

@@ -70,6 +70,7 @@ module application_fpga(
 
   wire          cpu_trap;
   wire          cpu_valid;
+  wire          cpu_instr;
   wire [03 : 0] cpu_wstrb;
   /* verilator lint_off UNUSED */
   wire [31 : 0] cpu_addr;
@@ -150,6 +151,8 @@ module application_fpga(
   wire [31 : 0] tk1_read_data;
   wire          tk1_ready;
   wire          fw_app_mode;
+  wire          force_jump;
+  wire [31 : 0] jump_instr;
 
 
   //----------------------------------------------------------------
@@ -185,7 +188,7 @@ module application_fpga(
                    .eoi(),
                    .trace_valid(),
                    .trace_data(),
-                   .mem_instr(),
+                   .mem_instr(cpu_instr),
                    .mem_la_read(),
                    .mem_la_write(),
                    .mem_la_addr(),
@@ -204,6 +207,9 @@ module application_fpga(
 
 
   rom rom_inst(
+	       .force_jump(force_jump),
+	       .jump_instr(jump_instr),
+
                .cs(rom_cs),
                .address(rom_address),
                .read_data(rom_read_data),
@@ -314,6 +320,12 @@ module application_fpga(
 	       .cpu_trap(cpu_trap),
 	       .fw_app_mode(fw_app_mode),
 
+	       .cpu_addr(cpu_addr),
+	       .cpu_instr(cpu_instr),
+	       .cpu_valid(cpu_valid),
+	       .force_jump(force_jump),
+	       .jump_instr(jump_instr),
+
                .led_r(led_r),
                .led_g(led_g),
                .led_b(led_b),

hw/application_fpga/rtl/rom.v

@@ -15,6 +15,9 @@
 `default_nettype none
 
 module rom(
+	   input wire           force_jump,
+	   input wire [31 : 0]  jump_instr,
+
 	   input wire           cs,
   /* verilator lint_off UNUSED */
 	   input wire  [11 : 0] address,
@@ -60,7 +63,12 @@ module rom(
     begin : rom_logic
 
       /* verilator lint_off WIDTH */
+      if (force_jump) begin
+	rom_rdata = jump_instr;
+      end
+      else begin
 	rom_rdata = memory[address];
+      end
       /* verilator lint_on WIDTH */
       rom_ready = cs;
     end