`timescale 1ns/1ps

module rocket_bscan_dmi_tunnel #(
  parameter integer DMI_ADDR_WIDTH = 7,
  parameter integer DMI_DATA_WIDTH = 32,
  parameter integer DMI_SCAN_WIDTH = 41
) (
  input  wire                      tck,
  input  wire                      reset,
  input  wire                      bscan_capture,
  input  wire                      bscan_sel,
  input  wire                      bscan_shift,
  input  wire                      bscan_tdi,
  input  wire                      bscan_update,
  output wire                      bscan_tdo,

  output wire                      dmi_clock,
  output wire                      dmi_reset,
  input  wire                      dmi_req_ready,
  output reg                       dmi_req_valid,
  output reg  [DMI_ADDR_WIDTH-1:0] dmi_req_addr,
  output reg  [DMI_DATA_WIDTH-1:0] dmi_req_data,
  output reg  [1:0]                dmi_req_op,
  output wire                      dmi_resp_ready,
  input  wire                      dmi_resp_valid,
  input  wire [DMI_DATA_WIDTH-1:0] dmi_resp_data,
  input  wire [1:0]                dmi_resp_resp
);
  localparam [4:0] IR_DTMCS = 5'h10;
  localparam [4:0] IR_DMI   = 5'h11;
  localparam [7:0] DMI_SCAN_WIDTH_U8 = DMI_SCAN_WIDTH;
  localparam [6:0] DMI_SCAN_WIDTH_U7 = DMI_SCAN_WIDTH;

  reg        scan_is_dr;
  reg [6:0]  scan_width;
  reg [7:0]  scan_count;
  reg [DMI_SCAN_WIDTH-1:0] scan_data;
  reg [DMI_SCAN_WIDTH-1:0] response_data;
  reg        tdo_reg;
  reg [4:0]  selected_ir;

  reg        busy_reg;
  reg        sticky_busy;
  reg        sticky_error;
  reg        downgrade_op;
  reg        last_resp_valid;
  reg [DMI_ADDR_WIDTH-1:0] last_resp_addr;
  reg [DMI_DATA_WIDTH-1:0] last_resp_data;
  reg [1:0]  last_resp_resp;

  wire busy = busy_reg | sticky_busy;
  wire nonzero_resp = sticky_error | (last_resp_valid & (last_resp_resp != 2'b00));
  wire [1:0] dmi_status = {sticky_error, sticky_error | sticky_busy};
  wire [31:0] dtmcs_response = {
    15'd0,
    1'b0,
    1'b0,
    3'd5,
    dmi_status,
    6'd7,
    4'd1
  };
  wire [DMI_SCAN_WIDTH-1:0] dmi_response =
    busy ? {{(DMI_ADDR_WIDTH){1'b0}}, {(DMI_DATA_WIDTH){1'b0}}, 2'b11} :
    last_resp_valid ? {last_resp_addr, last_resp_data, last_resp_resp} :
    {(DMI_SCAN_WIDTH){1'b0}};
  wire [DMI_SCAN_WIDTH-1:0] next_response =
    selected_ir == IR_DTMCS ? {{(DMI_SCAN_WIDTH-32){1'b0}}, dtmcs_response} :
    selected_ir == IR_DMI   ? dmi_response :
                              {(DMI_SCAN_WIDTH){1'b0}};

  assign bscan_tdo = tdo_reg;
  assign dmi_clock = tck;
  assign dmi_reset = reset;
  assign dmi_resp_ready = 1'b1;

  wire [7:0] data_index = scan_count - 8'd8;

  always @(posedge tck or posedge reset) begin
    if (reset) begin
      scan_is_dr <= 1'b0;
      scan_width <= 7'd0;
      scan_count <= 8'd0;
      scan_data <= {DMI_SCAN_WIDTH{1'b0}};
      response_data <= {DMI_SCAN_WIDTH{1'b0}};
      selected_ir <= IR_DTMCS;
      busy_reg <= 1'b0;
      sticky_busy <= 1'b0;
      sticky_error <= 1'b0;
      downgrade_op <= 1'b0;
      last_resp_valid <= 1'b0;
      last_resp_addr <= {DMI_ADDR_WIDTH{1'b0}};
      last_resp_data <= {DMI_DATA_WIDTH{1'b0}};
      last_resp_resp <= 2'b00;
      dmi_req_valid <= 1'b0;
      dmi_req_addr <= {DMI_ADDR_WIDTH{1'b0}};
      dmi_req_data <= {DMI_DATA_WIDTH{1'b0}};
      dmi_req_op <= 2'b00;
    end else begin
      if (dmi_req_valid && dmi_req_ready) begin
        dmi_req_valid <= 1'b0;
      end

      if (dmi_resp_valid) begin
        last_resp_valid <= 1'b1;
        last_resp_data <= dmi_resp_data;
        last_resp_resp <= dmi_resp_resp;
        last_resp_addr <= dmi_req_addr;
        busy_reg <= 1'b0;
      end

      if (bscan_sel && bscan_capture) begin
        scan_is_dr <= 1'b0;
        scan_width <= 7'd0;
        scan_count <= 8'd0;
        scan_data <= {DMI_SCAN_WIDTH{1'b0}};
        response_data <= next_response;

        if (selected_ir == IR_DMI) begin
          downgrade_op <= !busy & nonzero_resp;
          sticky_busy <= busy;
          sticky_error <= nonzero_resp;
          if (!busy && last_resp_valid) begin
            last_resp_valid <= 1'b0;
          end
        end
      end else if (bscan_sel && bscan_shift) begin
        if (scan_count == 8'd0) begin
          scan_is_dr <= bscan_tdi;
        end else if (scan_count <= 8'd7) begin
          scan_width[scan_count - 8'd1] <= bscan_tdi;
        end else begin
          if (data_index < DMI_SCAN_WIDTH_U8) begin
            scan_data[data_index] <= bscan_tdi;
          end
        end
        scan_count <= scan_count + 8'd1;
      end

      if (bscan_sel && bscan_update) begin
        if (!scan_is_dr) begin
          selected_ir <= scan_data[4:0];
        end else if (selected_ir == IR_DTMCS && scan_width == 7'd32) begin
          if (scan_data[16]) begin
            sticky_busy <= 1'b0;
            sticky_error <= 1'b0;
          end
        end else if (selected_ir == IR_DMI && scan_width == DMI_SCAN_WIDTH_U7) begin
          if (sticky_busy) begin
            dmi_req_valid <= dmi_req_valid;
          end else if (downgrade_op || scan_data[1:0] == 2'b00) begin
            dmi_req_addr <= {DMI_ADDR_WIDTH{1'b0}};
            dmi_req_data <= {DMI_DATA_WIDTH{1'b0}};
            dmi_req_op <= 2'b00;
          end else begin
            dmi_req_addr <= scan_data[DMI_SCAN_WIDTH-1:DMI_DATA_WIDTH+2];
            dmi_req_data <= scan_data[DMI_DATA_WIDTH+1:2];
            dmi_req_op <= scan_data[1:0];
            dmi_req_valid <= 1'b1;
            busy_reg <= 1'b1;
          end
        end
      end
    end
  end

  always @(negedge tck or posedge reset) begin
    if (reset) begin
      tdo_reg <= 1'b0;
    end else if (bscan_sel && bscan_shift) begin
      // Genesys2 Kintex-7 BSCANE2 USER chain needs two extra lead-in bits on
      // TDO. Without this, OpenOCD sees DTMCS right-shifted by two bits
      // (0x5071 becomes 0x141c) and reports an unsupported DTM version.
      if (scan_count <= 8'd7) begin
        tdo_reg <= 1'b0;
      end else if (scan_count <= (DMI_SCAN_WIDTH_U8 + 8'd7)) begin
        tdo_reg <= response_data[scan_count - 8'd8];
      end else begin
        tdo_reg <= 1'b0;
      end
    end else begin
      tdo_reg <= 1'b0;
    end
  end
endmodule