library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

library work;
use work.common.all;
use work.wishbone_types.all;

library unisim;
use unisim.vcomponents.all;

entity dmi_dtm_tb is
end dmi_dtm_tb;

architecture behave of dmi_dtm_tb is
    signal clk           : std_ulogic;
    signal rst           : std_ulogic;
    constant clk_period  : time := 10 ns;
    constant jclk_period : time := 30 ns;

    -- DMI debug bus signals
    signal dmi_addr : std_ulogic_vector(7 downto 0);
    signal dmi_din  : std_ulogic_vector(63 downto 0);
    signal dmi_dout : std_ulogic_vector(63 downto 0);
    signal dmi_req  : std_ulogic;
    signal dmi_wr   : std_ulogic;
    signal dmi_ack  : std_ulogic;

    -- Global JTAG signals (used by BSCANE2 inside dmi_dtm
    alias j : glob_jtag_t is glob_jtag;

    -- Wishbone interfaces
    signal wishbone_ram_in : wishbone_slave_out;
    signal wishbone_ram_out : wishbone_master_out;

begin
    dtm: entity work.dmi_dtm
        generic map(
            ABITS => 8,
            DBITS => 64
            )
        port map(
            sys_clk   => clk,
            sys_reset => rst,
            dmi_addr  => dmi_addr,
            dmi_din   => dmi_din,
            dmi_dout  => dmi_dout,
            dmi_req   => dmi_req,
            dmi_wr    => dmi_wr,
            dmi_ack   => dmi_ack
            );

    simple_ram_0: entity work.wishbone_bram_wrapper
        generic map(RAM_INIT_FILE => "main_ram.bin",
                    MEMORY_SIZE => 524288)
        port map(clk => clk, rst => rst,
                 wishbone_in => wishbone_ram_out,
                 wishbone_out => wishbone_ram_in);

    wishbone_debug_0: entity work.wishbone_debug_master
        port map(clk => clk, rst => rst,
                 dmi_addr => dmi_addr(1 downto 0),
                 dmi_dout => dmi_din,
                 dmi_din => dmi_dout,
                 dmi_wr => dmi_wr,
                 dmi_ack => dmi_ack,
                 dmi_req => dmi_req,
                 wb_in => wishbone_ram_in,
                 wb_out => wishbone_ram_out);

    -- system clock
    sys_clk: process
    begin
        clk <= '1';
        wait for clk_period / 2;
        clk <= '0';
        wait for clk_period / 2;
    end process sys_clk;

    -- system sim: just reset and wait
    sys_sim: process
    begin
        rst <= '1';
        wait for clk_period;
        rst <= '0';
        wait;
    end process;

    -- jtag sim process
    sim_jtag: process
        procedure clock(count: in INTEGER) is
        begin
            for i in 1 to count loop
                j.tck <= '0';
                wait for jclk_period/2;
                j.tck <= '1';
                wait for jclk_period/2;
            end loop;
        end procedure clock;

        procedure shift_out(val: in std_ulogic_vector) is
        begin
            for i in 0 to val'length-1 loop
                j.tdi <= val(i);
                clock(1);
            end loop;
        end procedure shift_out;

        procedure shift_in(val: out std_ulogic_vector) is
        begin
            for i in val'length-1 downto 0 loop
                val := j.tdo & val(val'length-1 downto 1);
                clock(1);
            end loop;
        end procedure shift_in;

        procedure send_command(
            addr : in std_ulogic_vector(7 downto 0);
            data : in std_ulogic_vector(63 downto 0);
            op   : in std_ulogic_vector(1 downto 0)) is
        begin
            j.capture <= '1';
            clock(1);
            j.capture <= '0';
            clock(1);
            j.shift <= '1';
            shift_out(op);
            shift_out(data);
            shift_out(addr);
            j.shift <= '0';
            j.update <= '1';
            clock(1);
            j.update <= '0';
            clock(1);
        end procedure send_command;

        procedure read_resp(
            op   : out std_ulogic_vector(1 downto 0);
            data : out std_ulogic_vector(63 downto 0)) is

            variable addr : std_ulogic_vector(7 downto 0);
        begin
            j.capture <= '1';
            clock(1);
            j.capture <= '0';        
            clock(1);
            j.shift <= '1';
            shift_in(op);
            shift_in(data);
            shift_in(addr);
            j.shift <= '0';
            j.update <= '1';
            clock(1);
            j.update <= '0';
            clock(1);
        end procedure read_resp;        

        procedure dmi_write(addr : in std_ulogic_vector(7 downto 0);
                            data : in std_ulogic_vector(63 downto 0)) is
            variable resp_op   : std_ulogic_vector(1 downto 0);
            variable resp_data : std_ulogic_vector(63 downto 0);
            variable timeout   : integer;
        begin
            send_command(addr, data, "10");
            loop
                read_resp(resp_op, resp_data);
                case resp_op is
                when "00" =>
                    return;
                when "11" =>
                    timeout := timeout + 1;
                    assert timeout < 0
                        report "dmi_write timed out !" severity error;
                when others =>
                    assert 0 > 1 report "dmi_write got odd status: " &
                        to_hstring(resp_op) severity error;
                end case;
            end loop;
        end procedure dmi_write;
        

        procedure dmi_read(addr : in std_ulogic_vector(7 downto 0);
                           data : out std_ulogic_vector(63 downto 0)) is
            variable resp_op   : std_ulogic_vector(1 downto 0);
            variable timeout   : integer;
        begin
            send_command(addr, (others => '0'), "01");
            loop
                read_resp(resp_op, data);
                case resp_op is
                when "00" =>
                    return;
                when "11" =>
                    timeout := timeout + 1;
                    assert timeout < 0
                        report "dmi_read timed out !" severity error;
                when others =>
                    assert 0 > 1 report "dmi_read got odd status: " &
                        to_hstring(resp_op) severity error;
                end case;
            end loop;
        end procedure dmi_read;

        variable data : std_ulogic_vector(63 downto 0);
    begin
        -- init & reset
        j.reset <= '1';
        j.sel <= "0000";
        j.capture <= '0';
        j.update <= '0';
        j.shift <= '0';
        j.tdi <= '0';
        j.tms <= '0';
        j.runtest <= '0';
        clock(5);
        j.reset <= '0';
        clock(5);

        -- select chain 2
        j.sel <= "0010";
        clock(1);

        -- send command
        dmi_read(x"00", data);
        report "Read addr reg:" & to_hstring(data);
        report "Writing addr reg to all 1's";
        dmi_write(x"00", (others => '1'));
        dmi_read(x"00", data);
        report "Read addr reg:" & to_hstring(data);

        report "Writing ctrl reg to all 1's";
        dmi_write(x"02", (others => '1'));
        dmi_read(x"02", data);
        report "Read ctrl reg:" & to_hstring(data);

        report "Read memory at 0...\n";
        dmi_write(x"00", x"0000000000000000");
        dmi_write(x"02", x"00000000000007ff");
        dmi_read(x"01", data);
        report "00:" & to_hstring(data);
        dmi_read(x"01", data);
        report "08:" & to_hstring(data);
        dmi_read(x"01", data);
        report "10:" & to_hstring(data);
        dmi_read(x"01", data);
        report "18:" & to_hstring(data);
        clock(10);
        std.env.finish;
    end process;
end behave;