Subversion Repositories xgate

////////////////////////////////////////////////////////////////////////////////

//

//  WISHBONE revB.2 compliant Xgate Coprocessor - Test Bench

//

//  Author: Bob Hayes

//          rehayes@opencores.org

//

//  Downloaded from: http://www.opencores.org/projects/xgate.....

//

////////////////////////////////////////////////////////////////////////////////

// Copyright (c) 2009, Robert Hayes

//

// This source file is free software: you can redistribute it and/or modify

// it under the terms of the GNU Lesser General Public License as published

// by the Free Software Foundation, either version 3 of the License, or

// (at your option) any later version.

//

// Supplemental terms.

//     * Redistributions of source code must retain the above copyright

//       notice, this list of conditions and the following disclaimer.

//     * Neither the name of the <organization> nor the

//       names of its contributors may be used to endorse or promote products

//       derived from this software without specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY Robert Hayes ''AS IS'' AND ANY

// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

// DISCLAIMED. IN NO EVENT SHALL Robert Hayes BE LIABLE FOR ANY

// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//

// You should have received a copy of the GNU General Public License

// along with this program.  If not, see <http://www.gnu.org/licenses/>.

////////////////////////////////////////////////////////////////////////////////

// 45678901234567890123456789012345678901234567890123456789012345678901234567890

`include "timescale.v"

module tst_bench_top();

  parameter MAX_CHANNEL = 127;    // Max XGATE Interrupt Channel Number

  parameter STOP_ON_ERROR = 1'b0;

  parameter MAX_VECTOR = 9000;

  parameter L_BYTE = 2'b01;

  parameter H_BYTE = 2'b10;

  parameter WORD   = 2'b11;

  // Name Address Locations

  parameter XGATE_BASE     = 24'h1000;

  parameter XGATE_XGMCTL   = XGATE_BASE + 6'h00;

  parameter XGATE_XGCHID   = XGATE_BASE + 6'h02;

  parameter XGATE_XGISPHI  = XGATE_BASE + 6'h04;

  parameter XGATE_XGISPLO  = XGATE_BASE + 6'h06;

  parameter XGATE_XGVBR    = XGATE_BASE + 6'h08;

  parameter XGATE_XGIF_7   = XGATE_BASE + 6'h0a;

  parameter XGATE_XGIF_6   = XGATE_BASE + 6'h0c;

  parameter XGATE_XGIF_5   = XGATE_BASE + 6'h0e;

  parameter XGATE_XGIF_4   = XGATE_BASE + 6'h10;

  parameter XGATE_XGIF_3   = XGATE_BASE + 6'h12;

  parameter XGATE_XGIF_2   = XGATE_BASE + 6'h14;

  parameter XGATE_XGIF_1   = XGATE_BASE + 6'h16;

  parameter XGATE_XGIF_0   = XGATE_BASE + 6'h18;

  parameter XGATE_XGSWT    = XGATE_BASE + 6'h1a;

  parameter XGATE_XGSEM    = XGATE_BASE + 6'h1c;

  parameter XGATE_RES1     = XGATE_BASE + 6'h1e;

  parameter XGATE_XGCCR    = XGATE_BASE + 6'h20;

  parameter XGATE_XGPC     = XGATE_BASE + 6'h22;

  parameter XGATE_RES2     = XGATE_BASE + 6'h24;

  parameter XGATE_XGR1     = XGATE_BASE + 6'h26;

  parameter XGATE_XGR2     = XGATE_BASE + 6'h28;

  parameter XGATE_XGR3     = XGATE_BASE + 6'h2a;

  parameter XGATE_XGR4     = XGATE_BASE + 6'h2c;

  parameter XGATE_XGR5     = XGATE_BASE + 6'h2e;

  parameter XGATE_XGR6     = XGATE_BASE + 6'h30;

  parameter XGATE_XGR7     = XGATE_BASE + 6'h32;

  // Define bits in XGATE Control Register

  parameter XGMCTL_XGEM     = 16'h8000;

  parameter XGMCTL_XGFRZM   = 16'h4000;

  parameter XGMCTL_XGDBGM   = 15'h2000;

  parameter XGMCTL_XGSSM    = 15'h1000;

  parameter XGMCTL_XGFACTM  = 15'h0800;

  parameter XGMCTL_XGBRKIEM = 15'h0400;

  parameter XGMCTL_XGSWEIFM = 15'h0200;

  parameter XGMCTL_XGIEM    = 15'h0100;

  parameter XGMCTL_XGE      = 16'h0080;

  parameter XGMCTL_XGFRZ    = 16'h0040;

  parameter XGMCTL_XGDBG    = 15'h0020;

  parameter XGMCTL_XGSS     = 15'h0010;

  parameter XGMCTL_XGFACT   = 15'h0008;

  parameter XGMCTL_XGBRKIE  = 15'h0004;

  parameter XGMCTL_XGSWEIF  = 15'h0002;

  parameter XGMCTL_XGIE     = 15'h0001;

  parameter CHECK_POINT     = 16'h8000;

  parameter CHANNEL_ACK     = CHECK_POINT + 2;

  parameter CHANNEL_ERR     = CHECK_POINT + 4;

  parameter TB_SEMPHORE     = CHECK_POINT + 10;

  parameter CHANNEL_XGIRQ_0 = CHECK_POINT + 16;

  parameter CHANNEL_XGIRQ_1 = CHECK_POINT + 18;

  parameter CHANNEL_XGIRQ_2 = CHECK_POINT + 20;

  parameter CHANNEL_XGIRQ_3 = CHECK_POINT + 22;

  parameter CHANNEL_XGIRQ_4 = CHECK_POINT + 24;

  parameter CHANNEL_XGIRQ_5 = CHECK_POINT + 26;

  parameter CHANNEL_XGIRQ_6 = CHECK_POINT + 28;

  parameter CHANNEL_XGIRQ_7 = CHECK_POINT + 30;

  parameter SYS_RAM_BASE = 24'h00_0000;

  parameter RAM_WAIT_STATES = 1; // Number between 0 and 15

  parameter SYS_READ_DELAY  = 10;

  parameter XGATE_ACCESS_DELAY = SYS_READ_DELAY + RAM_WAIT_STATES;

  parameter XGATE_SS_DELAY = XGATE_ACCESS_DELAY + RAM_WAIT_STATES;

  parameter IRQ_BASE       = XGATE_BASE + 64;

  parameter IRQ_BYPS_0     = IRQ_BASE + 0;

  parameter IRQ_BYPS_1     = IRQ_BASE + 2;

  parameter IRQ_BYPS_2     = IRQ_BASE + 4;

  parameter IRQ_BYPS_3     = IRQ_BASE + 6;

  parameter IRQ_BYPS_4     = IRQ_BASE + 8;

  parameter IRQ_BYPS_5     = IRQ_BASE + 10;

  parameter IRQ_BYPS_6     = IRQ_BASE + 12;

  parameter IRQ_BYPS_7     = IRQ_BASE + 14;

  //

  // wires && regs

  //

  reg         mstr_test_clk;

  reg  [19:0] vector;

  reg  [15:0] error_count;

  reg  [ 7:0] test_num;

  reg  [15:0] q, qq;

  reg         rstn;

  reg         sync_reset;

  reg         por_reset_b;

  reg         scantestmode;

  reg  [MAX_CHANNEL:1] channel_req;  // XGATE Interrupt inputs

  wire [MAX_CHANNEL:1] xgif;         // XGATE Interrupt outputs

  wire         [  7:0] xgswt;         // XGATE Software Trigger outputs

  wire                 xg_sw_irq;    // Xgate Software Error interrupt

  wire [15:0] wbm_dat_o;  // WISHBONE Master Mode data output from XGATE

  wire [15:0] wbm_dat_i;  // WISHBONE Master Mode data input to XGATE

  wire [15:0] wbm_adr_o;  // WISHBONE Master Mode address output from XGATE

  wire [ 1:0] wbm_sel_o;

  reg         mem_wait_state_enable;

  wire [15:0] tb_ram_out;

  wire [15:0] tb_slave_dout; // WISHBONE data bus output from testbench slave module

  wire        error_pulse;   // Error detected output pulse from the testbench slave module

  wire        tb_slave_ack;  // WISHBONE ack from testbench slave module

  wire        ack_pulse;     // Thread ack output pulse from testbench slave module

  wire        wbm_cyc_o;

  wire        wbm_stb_o;

  wire        wbm_we_o;

  wire        wbs_err_o;

  // Registers used to mirror internal registers

  reg  [15:0] data_xgmctl;

  reg  [15:0] data_xgchid;

  reg  [15:0] data_xgvbr;

  reg  [15:0] data_xgswt;

  reg  [15:0] data_xgsem;

  wire        sys_cyc;

  wire        sys_stb;

  wire        sys_we;

  wire [ 1:0] sys_sel;

  wire [23:0] sys_adr;

  wire [15:0] sys_dout;

  wire [15:0] sys_din;

  wire        host_ack;

  wire [15:0] host_dout;

  wire        host_cyc;

  wire        host_stb;

  wire        host_we;

  wire [ 1:0] host_sel;

  wire [23:0] host_adr;

  wire [15:0] host_din;

  wire        xgate_ack;

  wire [15:0] xgate_dout;

  wire        xgate_cyc;

  wire        xgate_stb;

  wire        xgate_we;

  wire [ 1:0] xgate_sel;

  wire [15:0] xgate_adr;

  wire [15:0] xgate_din;

  wire        xgate_s_stb;

  wire        xgate_s_ack;

  wire [15:0] xgate_s_dout;

  wire        slv2_stb;

  wire        ram_sel;

  wire [15:0] ram_dout;

  // initial values and testbench setup

  initial

    begin

      mstr_test_clk = 0;

      vector = 0;

      test_num = 0;

      por_reset_b = 0;

      scantestmode = 0;

      error_count = 0;

      mem_wait_state_enable = 0;

      // channel_req = 0;

      `ifdef WAVES

           $shm_open("waves");

           $shm_probe("AS",tst_bench_top,"AS");

           $display("\nINFO: Signal dump enabled ...\n\n");

      `endif

      `ifdef WAVES_V

           $dumpfile ("xgate_wave_dump.lxt");

           $dumpvars (0, tst_bench_top);

           $dumpon;

           $display("\nINFO: VCD Signal dump enabled ...\n\n");

      `endif

    end

  // generate clock

  always #20 mstr_test_clk = ~mstr_test_clk;

  // Keep a count of how many clocks we've simulated

  always @(posedge mstr_test_clk)

    begin

      vector <= vector + 1;

      if (vector > MAX_VECTOR)

        begin

          error_count <= error_count + 1;

          $display("\n ------ !!!!! Simulation Timeout at vector=%d\n -------", vector);

          wrap_up;

        end

    end

  // Add up errors that come from WISHBONE read compares

  always @host.cmp_error_detect

    begin

      error_count <= error_count + 1;

    end

  always @(posedge error_pulse) //channel_ack_wrt

    begin

      #1;

      error_count = error_count + 1;

      if (STOP_ON_ERROR == 1'b1)

        wrap_up;

    end

  wire [ 6:0] current_active_channel = xgate.risc.xgchid;

  always @(posedge ack_pulse) //channel_ack_wrt

    clear_channel(current_active_channel);

  // Testbench RAM for Xgate program storage and Load/Store instruction tests

  ram p_ram

  (

    // Outputs

    .ram_out( ram_dout ),

    // inputs

    .address( sys_adr[15:0] ),

    .ram_in( sys_dout ),

    .we( sys_we ),

    .ce( ram_sel ),

    .stb( mstr_test_clk ),

    .sel( sys_sel )

  );

  // hookup wishbone master model

  wb_master_model #(.dwidth(16), .awidth(24))

    host(

    // Outputs

    .cyc( host_cyc ),

    .stb( host_stb ),

    .we( host_we ),

    .sel( host_sel ),

    .adr( host_adr ),

    .dout( host_dout ),

    // inputs

    .din( sys_din ),

    .clk( mstr_test_clk ),

    .ack( host_ack ),

    .rst( rstn ),

    .err( 1'b0 ),

    .rty( 1'b0 )

  );

  bus_arbitration  #(.dwidth(16),

                     .awidth(24),

                     .ram_base(0),

                     .ram_size(17'h10000),

                     .slv1_base(XGATE_BASE),

                     .slv1_size(128),

                     .slv2_base(CHECK_POINT),

                     .slv2_size(32),

                     .ram_wait_states(RAM_WAIT_STATES)

)

    arb(

    // System bus I/O

    .sys_cyc( sys_cyc ),

    .sys_stb( sys_stb ),

    .sys_we( sys_we ),

    .sys_sel( sys_sel ),

    .sys_adr( sys_adr ),

    .sys_dout( sys_dout ),

    .sys_din( sys_din ),

    // Host bus I/O

    .host_ack( host_ack ),

    .host_dout( host_din ),

    .host_cyc( host_cyc ),

    .host_stb( host_stb ),

    .host_we( host_we ),

    .host_sel( host_sel ),

    .host_adr( host_adr ),

    .host_din( host_dout ),

    // Alternate Bus Master #1 Bus I/O

    .alt1_ack( xgate_ack ),

    .alt1_cyc( wbm_cyc_o ),

    .alt1_stb( wbm_stb_o ),

    .alt1_we( wbm_we_o ),

    .alt1_sel( wbm_sel_o ),

    .alt1_adr( {8'h00, wbm_adr_o} ),

    .alt1_din( wbm_dat_o ),

    // RAM

    .ram_sel( ram_sel ),

    .ram_dout( ram_dout ),

    // Slave #1 Bus I/O

    .slv1_stb( xgate_s_stb ),

    .slv1_ack( xgate_s_ack ),

    .slv1_din( xgate_s_dout ),

    // Slave #2 Bus I/O

    .slv2_stb( slv2_stb ),

    .slv2_ack( tb_slave_ack ),

    .slv2_din( tb_slave_dout ),

    // Miscellaneous

    .host_clk( mstr_test_clk ),

    .risc_clk( mstr_test_clk ),

    .rst( rstn ),  // No Connect

    .err( 1'b0 ),  // No Connect

    .rty( 1'b0 )   // No Connect

  );

  // hookup XGATE core - Parameters take all default values

  xgate_top  #(.SINGLE_CYCLE(1'b0),

               .WB_RD_DEFAULT(1'b0),

               .MAX_CHANNEL(MAX_CHANNEL))    // Max XGATE Interrupt Channel Number

          xgate(

          // Wishbone slave interface

          .wbs_clk_i( mstr_test_clk ),

          .wbs_rst_i( 1'b0 ),         // sync_reset

          .arst_i( rstn ),            // async resetn

          .wbs_adr_i( sys_adr[6:1] ),

          .wbs_dat_i( sys_dout ),

          .wbs_dat_o( xgate_s_dout ),

          .wbs_we_i( sys_we ),

          .wbs_stb_i( xgate_s_stb ),

          .wbs_cyc_i( sys_cyc ),

          .wbs_sel_i( sys_sel ),

          .wbs_ack_o( xgate_s_ack ),

          .wbs_err_o( wbs_err_o ),

          // Wishbone master Signals

          .wbm_dat_o( wbm_dat_o ),

          .wbm_we_o( wbm_we_o ),

          .wbm_stb_o( wbm_stb_o ),

          .wbm_cyc_o( wbm_cyc_o ),

          .wbm_sel_o( wbm_sel_o ),

          .wbm_adr_o( wbm_adr_o ),

          .wbm_dat_i( sys_din ),

          .wbm_ack_i( xgate_ack ),

          .xgif( xgif ),             // XGATE Interrupt Flag output

          .xg_sw_irq( xg_sw_irq ),   // XGATE Software Error Interrupt Flag output

          .xgswt( xgswt ),

          .risc_clk( mstr_test_clk ),

          .chan_req_i( {channel_req[MAX_CHANNEL:40], xgswt, channel_req[31:1]} ),

          .debug_mode_i( 1'b0 ),

          .secure_mode_i( 1'b0 ),

          .scantestmode( scantestmode )

  );

  tb_slave #(.DWIDTH(16),

             .SINGLE_CYCLE(1'b1),

             .MAX_CHANNEL(MAX_CHANNEL))

          tb_slave_regs(

          // wishbone interface

          .wb_clk_i( mstr_test_clk ),

          .wb_rst_i( 1'b0 ),

          .arst_i( rstn ),

          .wb_adr_i( sys_adr[4:1] ),

          .wb_dat_i( sys_dout ),

          .wb_dat_o( tb_slave_dout),

          .wb_we_i( sys_we ),

          .wb_stb_i( slv2_stb ),

          .wb_cyc_i( sys_cyc ),

          .wb_sel_i( sys_sel ),

          .wb_ack_o( tb_slave_ack ),

          .ack_pulse( ack_pulse ),

          .error_pulse( error_pulse ),

          .brk_pt(  ),

          .x_address( wbm_adr_o ),

          .xgif( xgif ),

          .vector( vector )

  );

////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////

// Main Test Program

initial

  begin

    $display("\nstatus at time: %t Testbench started", $time);

    // reset system

    rstn = 1'b1; // negate reset

    channel_req = 1; //

    repeat(1) @(posedge mstr_test_clk);

    sync_reset = 1'b1;  // Make the sync reset 1 clock cycle long

    #2;          // move the async reset away from the clock edge

    rstn = 1'b0; // assert async reset

    #5;          // Keep the async reset pulse with less than a clock cycle

    rstn = 1'b1; // negate async reset

    por_reset_b = 1'b1;

    channel_req = 0; //

    repeat(1) @(posedge mstr_test_clk);

    sync_reset = 1'b0;

    channel_req = 0; //

    $display("\nstatus at time: %t done reset", $time);

    test_inst_set;

    test_debug_mode;

    test_debug_bit;

    test_chid_debug;

    reg_test_16;

    reg_irq;

    // host_ram;

    // End testing

    wrap_up;

  end

////////////////////////////////////////////////////////////////////////////////

// Test CHID Debug mode operation

task test_chid_debug;

  begin

    test_num = test_num + 1;

    $display("\nTEST #%d Starts at vector=%d, test_chid_debug", test_num, vector);

    $readmemh("../../../bench/verilog/debug_test.v", p_ram.ram_8);

    // Enable interrupts to RISC

    host.wb_write(0, IRQ_BYPS_0,  16'h0000, WORD);

    data_xgmctl = XGMCTL_XGBRKIEM | XGMCTL_XGBRKIE;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Enable interrupt on BRK instruction

    $display("BRK Software Error Interrupt enabled at vector=%d", vector);

    activate_thread_sw(3);

    wait_debug_set;   // Debug Status bit is set by BRK instruction

    host.wb_cmp(0, XGATE_XGPC,      16'h20c6, WORD);      // See Program code (BRK).

    host.wb_cmp(0, XGATE_XGR3,      16'h0001, WORD);      // See Program code.R3 = 1

    host.wb_cmp(0, XGATE_XGCHID,   16'h0003, WORD);       // Check for Correct CHID

    $display("Debug entry detected at vector=%d", vector);

    channel_req[5] = 1'b1; //

    repeat(7) @(posedge mstr_test_clk);

    host.wb_cmp(0, XGATE_XGCHID,   16'h0003, WORD);       // Check for Correct CHID

    host.wb_write(0, XGATE_XGCHID, 16'h000f, H_BYTE);     // Check byte select lines

    repeat(4) @(posedge mstr_test_clk);

    host.wb_cmp(0, XGATE_XGCHID,   16'h0003, WORD);       // Verify CHID is unchanged

    host.wb_write(0, XGATE_XGCHID, 16'h000f, L_BYTE);     // Change CHID

    host.wb_cmp(0, XGATE_XGCHID,   16'h000f, WORD);       // Check for Correct CHID

    host.wb_write(0, XGATE_XGCHID, 16'h0000, WORD);       // Change CHID to 00, RISC should go to IDLE state

    repeat(1) @(posedge mstr_test_clk);

    host.wb_write(0, XGATE_XGCHID, 16'h0004, WORD);       // Change CHID

    repeat(8) @(posedge mstr_test_clk);

    $display("Channel ID changed at vector=%d", vector);

    data_xgmctl = XGMCTL_XGDBGM;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Clear Debug Mode Control Bit

    wait_debug_set;   // Debug Status bit is set by BRK instruction

    host.wb_cmp(0, XGATE_XGCHID,   16'h0004, WORD);       // Check for Correct CHID

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Clear Debug Mode Control Bit (Excape from Break State and run)

    wait_debug_set;   // Debug Status bit is set by BRK instruction

    host.wb_cmp(0, XGATE_XGCHID,   16'h0005, WORD);       // Check for Correct CHID

    activate_channel(6);

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Clear Debug Mode Control Bit (Excape from Break State and run)

    wait_debug_set;   // Debug Status bit is set by BRK instruction

    host.wb_cmp(0, XGATE_XGCHID,   16'h0006, WORD);       // Check for Correct CHID

    host.wb_cmp(0, XGATE_XGPC,      16'h211c, WORD);      // See Program code (BRK)

    data_xgmctl = XGMCTL_XGSSM | XGMCTL_XGSS;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Do a Single Step

    repeat(XGATE_SS_DELAY) @(posedge mstr_test_clk);

    host.wb_cmp(0, XGATE_XGPC,      16'h211e, WORD);      // See Program code (BRA)

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Do a Single Step

    repeat(XGATE_SS_DELAY) @(posedge mstr_test_clk);

    host.wb_cmp(0, XGATE_XGPC,      16'h2122, WORD);      // See Program code ()

    repeat(20) @(posedge mstr_test_clk);

    data_xgmctl = XGMCTL_XGDBGM;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Clear Debug Mode Control Bit

    repeat(50) @(posedge mstr_test_clk);

    p_ram.dump_ram(0);

    read_ram_cmp(16'h0000, 16'h7b55);

    read_ram_cmp(16'h0004, 16'h7faa);

    read_ram_cmp(16'h0006, 16'h6f55);

    read_ram_cmp(16'h0008, 16'h00c3);

    read_ram_cmp(16'h000a, 16'h5f66);

    read_ram_cmp(16'h000c, 16'h0003);

    read_ram_cmp(16'h0022, 16'hccxx);

    read_ram_cmp(16'h0026, 16'hxx99);

    read_ram_cmp(16'h0032, 16'h1fcc);

    read_ram_cmp(16'h0038, 16'h2f99);

    read_ram_cmp(16'h0042, 16'h33xx);

    read_ram_cmp(16'h0046, 16'hxx55);

    read_ram_cmp(16'h0052, 16'hxx66);

    read_ram_cmp(16'h0058, 16'h99xx);

    read_ram_cmp(16'h0062, 16'h1faa);

    read_ram_cmp(16'h0068, 16'h2fcc);

  end

endtask

////////////////////////////////////////////////////////////////////////////////

// Test Debug bit operation

task test_debug_bit;

  begin

    test_num = test_num + 1;

    $display("\nTEST #%d Starts at vector=%d, test_debug_bit", test_num, vector);

    $readmemh("../../../bench/verilog/debug_test.v", p_ram.ram_8);

    // Enable interrupts to RISC

    host.wb_write(0, IRQ_BYPS_0,  16'h0000, WORD);

    data_xgmctl = XGMCTL_XGBRKIEM | XGMCTL_XGBRKIE;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Enable interrupt on BRK instruction

    activate_thread_sw(2);

    // Approxmatly 12 instructions need to be done before activating Debug Mode

    repeat(12 + RAM_WAIT_STATES*12) @(posedge mstr_test_clk);

    data_xgmctl = XGMCTL_XGDBGM | XGMCTL_XGDBG;

    host.wb_write(0, XGATE_XGMCTL, data_xgmctl, WORD);    // Set Debug Mode Control Bit

    repeat(5) @(posedge mstr_test_clk);

    $display("DEBUG bit set at vector=%d", vector);

    host.wb_read(1, XGATE_XGR3, q, WORD);

    data_xgmctl = XGMCTL_XGSSM | XGMCTL_XGSS;

    qq = q;