`timescale 1 ns / 1 ps

`ifndef VERILATOR
module testbench #(
    parameter VERBOSE = 0
);
  reg  clk = 1;
  reg  resetn = 1;
  wire trap;

  always #5 clk = ~clk;

  initial begin
    repeat (100) @(posedge clk);
    resetn <= 0;
  end

  initial begin
    if ($test$plusargs("vcd")) begin
      $dumpfile("testbench.vcd");
      $dumpvars(0, testbench);
    end
    repeat (1000000) @(posedge clk);
    $display("TIMEOUT");
    $finish;
  end

  wire trace_valid;
  wire [35:0] trace_data;
  integer trace_file;

  initial begin
    if ($test$plusargs("trace")) begin
      trace_file = $fopen("testbench.trace", "w");
      repeat (10) @(posedge clk);
      while (!trap) begin
        @(posedge clk);
        if (trace_valid) $fwrite(trace_file, "%x\n", trace_data);
      end
      $fclose(trace_file);
      $display("Finished writing testbench.trace.");
    end
  end

  picorv32_wrapper #(
      .VERBOSE(VERBOSE)
  ) top (
      .wb_clk(clk),
      .wb_rst(resetn),
      .trap(trap),
      .trace_valid(trace_valid),
      .trace_data(trace_data)
  );
endmodule
`endif

module picorv32_wrapper #(
    parameter VERBOSE = 0
) (
    input wb_clk,
    input wb_rst,
    output trap,
    output trace_valid,
    output [35:0] trace_data,
    input testcase
);
  wire exit;
  reg [31:0] irq = 0;
  wire mem_instr;

  reg [15:0] count_cycle = 0;
  always @(posedge wb_clk) count_cycle <= !wb_rst ? count_cycle + 1 : 0;

  always @* begin
    irq = 0;
    irq[4] = &count_cycle[12:0];
    irq[5] = &count_cycle[15:0];
  end

  wire [31:0] wb_m2s_adr;
  wire [31:0] wb_m2s_dat;
  wire [3:0] wb_m2s_sel;
  wire wb_m2s_we;
  wire wb_m2s_cyc;
  wire wb_m2s_stb;
  wire [31:0] wb_s2m_dat;
  wire wb_s2m_ack;

  picorv32_wb #() uut (
      .trap(trap),
      .exit(exit),
      .irq(irq),
      .trace_valid(trace_valid),
      .trace_data(trace_data),
      .mem_instr(mem_instr),

      .wb_clk_i(wb_clk),
      .wb_rst_i(wb_rst),
  );

  reg [1023:0] firmware_file;
  initial begin
    if (!testcase) firmware_file = "firmware/firmware.hex";
    else firmware_file = "dhrystone/dhry.hex";
    // if (!$value$plusargs("firmware=%s", firmware_file)) 
    // firmware_file = "firmware/firmware.hex";
    $readmemh(firmware_file, uut.memory);
  end

  integer cycle_counter;
  always @(posedge wb_clk) begin
    cycle_counter <= !wb_rst ? cycle_counter + 1 : 0;
    if (!wb_rst && trap) begin
`ifndef VERILATOR
      repeat (10) @(posedge wb_clk);
`endif
      $display("TRAP after %1d clock cycles", cycle_counter);
      if (exit) begin
        $display("ALL TESTS PASSED.");
        $finish;
      end else begin
        $display("ERROR!");
        if ($test$plusargs("noerror")) $finish;
        $stop;
      end
    end
  end
endmodule



/***************************************************************
 * picorv32_wb
 ***************************************************************/

module picorv32_wb #(
) (
    output trap,
    output reg exit,

    // Wishbone interfaces
    input wb_rst_i,
    input wb_clk_i,

    output reg [31:0] wbm_adr_o,
    output reg [31:0] wbm_dat_o,
    input [31:0] wbm_dat_i,
    output reg wbm_we_o,
    output reg [3:0] wbm_sel_o,
    output reg wbm_stb_o,
    input wbm_ack_i,
    output reg wbm_cyc_o,

    // Pico Co-Processor Interface (PCPI)
    output        pcpi_valid,
    output [31:0] pcpi_insn,
    output [31:0] pcpi_rs1,
    output [31:0] pcpi_rs2,
    input         pcpi_wr,
    input  [31:0] pcpi_rd,
    input         pcpi_wait,
    input         pcpi_ready,

    // IRQ interface
    input  [31:0] irq,
    output [31:0] eoi,

    // Trace Interface
    output        trace_valid,
    output [35:0] trace_data,

    output mem_instr
);
  wire        mem_valid;
  wire [31:0] mem_addr;
  wire [31:0] mem_wdata;
  wire [ 3:0] mem_wstrb;
  reg         mem_ready;
  reg  [31:0] mem_rdata;

  wire        mem_la_read;
  wire        mem_la_write;
  wire [31:0] mem_la_addr;
  wire [31:0] mem_la_wdata;
  wire [ 3:0] mem_la_wstrb;


  wire        clk;
  wire        resetn;
  initial exit = 0;

  assign clk = wb_clk_i;
  assign resetn = ~wb_rst_i;

  picorv32 #(
      .ENABLE_TRACE(1),
      .MASKED_IRQ(32'h0000_0000),
      .LATCHED_IRQ(32'hffff_ffff),
      .PROGADDR_RESET(32'h0001_0000),
      .PROGADDR_IRQ(32'h0001_0010),
      .STACKADDR(32'h0001_0000)
  ) picorv32_core (
      .clk   (clk),
      .resetn(resetn),
      .trap  (trap),


      .mem_valid   (mem_valid),
      .mem_instr   (mem_instr),
      .mem_ready   (mem_ready),
      .mem_addr    (mem_addr),
      .mem_wdata   (mem_wdata),
      .mem_wstrb   (mem_wstrb),
      .mem_rdata   (mem_rdata),
      .mem_la_read (mem_la_read),
      .mem_la_write(mem_la_write),
      .mem_la_addr (mem_la_addr),
      .mem_la_wdata(mem_la_wdata),
      .mem_la_wstrb(mem_la_wstrb),
      .irq         (irq),
      .eoi         (eoi),

      .trace_valid(trace_valid),
      .trace_data (trace_data)
  );


  reg [7:0] memory[0:256*1024-1];

  assign mem_ready = 1;

  always @(posedge clk) begin
    mem_rdata[7:0]   <= mem_la_read ? memory[mem_la_addr+0] : 'bx;
    mem_rdata[15:8]  <= mem_la_read ? memory[mem_la_addr+1] : 'bx;
    mem_rdata[23:16] <= mem_la_read ? memory[mem_la_addr+2] : 'bx;
    mem_rdata[31:24] <= mem_la_read ? memory[mem_la_addr+3] : 'bx;
    if (mem_la_write) begin
      case (mem_la_addr)
        32'h1000_0000: begin
`ifndef TIMING
          $write("%c", mem_la_wdata);
          $fflush();
`endif
        end
        32'h2000_0000: begin
          if (mem_la_wdata[31:0] == 123456789) exit = 1;
        end
        default: begin
          if (mem_la_wstrb[0]) memory[mem_la_addr+0] <= mem_la_wdata[7:0];
          if (mem_la_wstrb[1]) memory[mem_la_addr+1] <= mem_la_wdata[15:8];
          if (mem_la_wstrb[2]) memory[mem_la_addr+2] <= mem_la_wdata[23:16];
          if (mem_la_wstrb[3]) memory[mem_la_addr+3] <= mem_la_wdata[31:24];
        end
      endcase
    end
  end
endmodule