`include "rsp_regr.h"

reg [31:0] fp;
initial fp = $fopen("misc_tests.log");

reg dump_true;
initial
  begin
    dump_true = 0;
    if ($test$plusargs("dumpvars"))
        begin
          $display("Enabling dumpvars");
          dump_true = 1;
	  $dumpvars;
        end
  end


reg AllDiags;
initial
    begin
       AllDiags=0;
       if ($test$plusargs("AllDiags"))
           AllDiags=1;
    end

reg [15:0] sweep_start;
initial
 begin
   sweep_start = 0;
   if ($getnum$plusarg("sweep_start=", sweep_start) == 1)
         $display("sweep start value is set to %d",sweep_start);
 end



reg rcp_int;
initial rcp_int = 0;
always @(reality.rcp_0.int_pad) rcp_int <= !reality.rcp_0.int_pad;
//always @(reality.rcp_0.rsp_0.sp_interrupt) rcp_int <= reality.rcp_0.rsp_0.sp_interrupt;

/************************************************************
 * TimeOut counters
 ***********************************************************/
reg [23:0] cycle_count;
reg [23:0] cycle_count_save;
reg TimeOut;
reg TimerOn;

initial
begin
  TimeOut = 0;
  TimerOn = 0;
end

always @(posedge `CLK)
   if (TimeOut)
     cycle_count <= cycle_count;
   else
   if (!TimerOn)
         cycle_count <= 0;
   else
     cycle_count <= cycle_count + 1;

always @(cycle_count) TimeOut <= (cycle_count === `TIMEOUT_CNT);


/****************************************************
 * Tasks 
 ***************************************************/
task clean_up;
  begin
   `tr.test_on = 0;
    `tr.su_trError = 0;
    `tr.vs_trError = 0;
    `tr.vv_trError = 0;
    `tr.dm_trError = 0;
    `tr.su_trError_eot = 0;
    `tr.vu_trError_eot = 0;
    `tr.dm_trError_eot = 0;
    TimeOut = 0; 
    TimerOn = 0; 
    @(posedge `CLK);
  end
endtask


task load_mem_from_rdram;
   input [31:0] mem_addr;
   input [31:0] rdram_addr;
   input [11:0] length;

   integer i;
   reg [31:0] status;

   begin
      $display("initializing rsp_mem");
      reality.r4200b_0.sp_dma_read(mem_addr,rdram_addr,'h0,'h0,length); // in to dmem

      status = 32'hff;
      while (status[3:2]!==0)
       begin
         repeat (200) @(posedge `CLK);
         reality.r4200b_0.read_word('h404_0010, 3);
         status = reality.r4200b_0.data[0];
         //$display("waitng for DMA to complete status=%h", status);
       end
   end
endtask

task dump_rdram;
input [15:0] start_addr;
input [15:0] oct_length;
input rdram_1;
reg [71:0] temp;
reg [63:0] data;
integer i;
reg failed;
begin
  failed = 0;
  `ifdef RDRAM_1_PRESENT
  `else
    failed = rdram_1;
    if (rdram_1) $display("SORRY NO RDRAM_1 present");
  `endif


  if (!failed)
    for (i=0; i<oct_length; i= i+1)
      begin
       if (rdram_1)
	 `ifdef RDRAM_1_PRESENT
            temp[71:0] = reality.rdram_1.rdram_near_model_0.core.core[i];
	`endif
       else
         temp[71:0] = reality.rdram_0.rdram_near_model_0.core.core[i];

       data = {temp[7:0], temp[16:9],temp[25:18],temp[34:27],
               temp[43:36], temp[52:45],temp[61:54],temp[70:63]};
       $display("rdram_%0d, Oct_Address = %h, Data = %h,  core_data = %h",rdram_1,i,data,temp[71:0]);
      end
end
endtask

task run_test_1;
reg [15:0] SWEEP_CNT;
integer  i;
reg EOT;
begin
  EOT = 0;

  $readmemh("tests/test_1.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_1.rdram_DMEM",`rdram_0.core.core);
  $readmemh("tests/ctrace/test_1.trSU",`tr.su_mem);
  $readmemh("tests/ctrace/test_1.trVU",`tr.vu_mem);
  $readmemh("tests/ctrace/test_1.trDM",`tr.dm_mem);

  reality.r4200b_0.write_word('h470_0010, 3, 32'h0); // Disable refresh

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Imem 

  //$display (" AFTER DMA ");
  //dump_rdram('h0,'h400);
 
  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  $display("clearing halt,break");
  reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break

  if (dump_true) $dumpon;
  SWEEP_CNT = sweep_start;

  while (!EOT)
    begin
   	cycle_count = 0; wait (`Halt==0 || TimeOut || `tr.trError);
   	cycle_count = 0; wait ((`Break==1 && `Halt==1) || TimeOut || `tr.trError); //break instruction
        if (SWEEP_CNT>0) `tr.test_on = 0;


	if (SWEEP_CNT==125 || `tr.trError ||  `tr.trError_eot || TimeOut ) EOT = 1;

	if (!EOT) 
         begin
   	   reality.r4200b_0.write_word(32'h0404_0000, 3, 'h1f00); //master
   	   reality.r4200b_0.write_word(32'h0404_0004, 3, 'h1000);  //slave
   
           `tr.test_on = 1;
	   $display ($time," Starting Sweep # %d",SWEEP_CNT);
   	
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
   	
   	   cycle_count = 0; wait (`Halt==0 || TimeOut);
   	
   	   for (i=0; i<SWEEP_CNT; i=i+1) @(posedge `CLK); 
	   SWEEP_CNT = SWEEP_CNT + 1;
   	
           reality.r4200b_0.write_word(32'h0404_0008, 3, 'h4); //lenght and trigger
   	   reality.r4200b_0.p_valid <= 1'b0;
   	   @(posedge `CLK);
	  end
    end

    repeat (10) @(posedge `CLK); 

    if (TimeOut)
       $fwrite(fp,"test_1 \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (`tr.trError|| `tr.trError_eot)
       $fwrite(fp,"test_1 \t FAILED... \t **TRACE_ERROR** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_1 \t Passed    \t                 \t Time=%0d \n",$time);

    clean_up;

end
endtask

task run_test_2;
reg [15:0] SWEEP_CNT;
integer  i;
reg EOT;
begin
  EOT = 0;

  $readmemh("tests/test_2.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_2.rdram_DMEM",`rdram_0.core.core);
  $readmemh("tests/ctrace/test_2.trSU",`tr.su_mem);
  $readmemh("tests/ctrace/test_2.trVU",`tr.vu_mem);
  $readmemh("tests/ctrace/test_2.trDM",`tr.dm_mem);

  reality.r4200b_0.write_word('h470_0010, 3, 32'h0); // Disable refresh

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Dmem 
 
  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  $display("clearing halt,break");
  reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break

  if (dump_true) $dumpon;
  SWEEP_CNT = sweep_start;

  while (!EOT)
    begin
   	cycle_count = 0; wait (`Halt==0 || TimeOut || `tr.trError);
   	cycle_count = 0; wait ((`Break==1 && `Halt==1) || TimeOut || `tr.trError); //break instruction
        if (SWEEP_CNT>0) `tr.test_on = 0;


	if (SWEEP_CNT==125 || `tr.trError ||  `tr.trError_eot || TimeOut ) EOT = 1;

	if (!EOT) 
         begin
   	   reality.r4200b_0.write_word(32'h0404_0000, 3, 'h1f00); //master
   	   reality.r4200b_0.write_word(32'h0404_0004, 3, 'h1000);  //slave
   
           `tr.test_on = 1;
	   $display ($time," Starting Sweep # %d",SWEEP_CNT);
   	
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
   	
   	   cycle_count = 0; wait (`Halt==0 || TimeOut);
   	
   	   for (i=0; i<SWEEP_CNT; i=i+1) @(posedge `CLK); 
	   SWEEP_CNT = SWEEP_CNT + 1;
   	
           reality.r4200b_0.write_word(32'h0404_0008, 3, 'h30); //lenght and trigger
   	   reality.r4200b_0.p_valid <= 1'b0;
   	   @(posedge `CLK);
	  end
    end

    repeat (10) @(posedge `CLK); 

    if (TimeOut)
       $fwrite(fp,"test_2 \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (`tr.trError|| `tr.trError_eot)
       $fwrite(fp,"test_2 \t FAILED... \t **TRACE_ERROR** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_2 \t Passed    \t                 \t Time=%0d \n",$time);

    clean_up;
end
endtask


task run_test_3;
reg [15:0] SWEEP_CNT;
integer  i;
reg EOT;
begin
  EOT = 0;

  $readmemh("tests/test_3.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_3.rdram_DMEM",`rdram_0.core.core);
  $readmemh("tests/ctrace/test_3.trSU",`tr.su_mem);
  $readmemh("tests/ctrace/test_3.trVU",`tr.vu_mem);
  $readmemh("tests/ctrace/test_3.trDM",`tr.dm_mem);

  reality.r4200b_0.write_word('h470_0010, 3, 32'h0); // Disable refresh

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Dmem 
 
  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  $display("clearing halt,break");
  reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break

  if (dump_true) $dumpon;
  SWEEP_CNT = sweep_start;

  while (!EOT)
    begin
   	cycle_count = 0; wait (`Halt==0 || TimeOut || `tr.trError);
   	cycle_count = 0; wait ((`Break==1 && `Halt==1) || TimeOut || `tr.trError); //break instruction
        if (SWEEP_CNT>0) `tr.test_on = 0;

	if (SWEEP_CNT==115 || `tr.trError || `tr.trError_eot || TimeOut ) EOT = 1;
        repeat (10) @(posedge `CLK); 

	if (!EOT) 
         begin
           `tr.test_on = 1;
	   $display ($time," Starting Sweep # %d",SWEEP_CNT);

   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
   	
   	   cycle_count = 0; wait (`Halt==0 || TimeOut);
   	
   	   for (i=0; i<(SWEEP_CNT+20); i=i+1) @(posedge `CLK); 
	   SWEEP_CNT = SWEEP_CNT + 1;
   	
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h2); //set halt
   	   cycle_count = 0; wait (`Halt==1 || TimeOut);
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h1); //clear halt
	  end
    end

    repeat (10) @(posedge `CLK); 

    if (TimeOut)
       $fwrite(fp,"test_3 \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (`tr.trError || `tr.trError_eot)
       $fwrite(fp,"test_3 \t FAILED... \t **TRACE_ERROR** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_3 \t Passed    \t                 \t Time=%0d \n",$time);

    clean_up;
end
endtask

task run_test_intr;
integer  i;
reg Error;
begin
  Error = 0;

  $readmemh("tests/test_intr.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_intr.rdram_DMEM",`rdram_0.core.core);

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Imem 
 
  if (dump_true) $dumpon;
  $display("set interrupt mask");
  reality.r4200b_0.write_word('h430_000c, 3, 32'h2); //set interrupt mask
  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  // Int 0 -> 1
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (rcp_int) 
      begin
       Error = 1'b1;  
       $display($time," Error: Interrupt signal should be cleared");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
     wait (`Halt===0 || TimeOut || Error);
    end

    begin 
     wait (rcp_int || TimeOut || Error); 
     wait (`Halt===1 || TimeOut || Error);
    end
  join

  $display($time," Completed Int 0->1 transition");

  // Int 1 -> 0
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (!rcp_int)
      begin
       Error = 1'b1; 
       $display($time," Error: Interrupt signal should be set");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
     wait (`Halt===0 || TimeOut || Error);
    end

    begin 
     wait (!rcp_int || TimeOut || Error); 
     wait (`Halt===1 || TimeOut || Error);
    end
  join

  $display($time," Completed Int 1->0 transition");

  // Int 0 -> 1
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (rcp_int)
      begin
       Error = 1'b1; 
       $display($time," Error: Interrupt signal should be cleared");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
     wait (`Halt===0 || TimeOut || Error);
    end

    begin 
     wait (rcp_int || TimeOut || Error); 
     wait (`Halt===1 || TimeOut || Error);
    end
  join

  $display($time," Completed Int 0->1 transition");

  // Int 1 -> 0
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (!rcp_int)
      begin
       Error = 1'b1; 
       $display($time," Error: Interrupt signal should be set");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
     wait (`Halt===0 || TimeOut || Error);
    end

    begin 
     wait (!rcp_int || TimeOut || Error); 
     wait (`Halt===1 || TimeOut || Error);
    end
  join

  $display($time," Completed Int 1->0 transition");

   /***********************************************/

  // Int 0 -> 1
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (rcp_int) 
      begin
       Error = 1'b1; 
       $display($time," Error: Interrupt signal should be cleared");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h10); //set Interrupt
     wait (rcp_int || TimeOut || Error); 
    end
  join
  $display($time," Completed (MBUS asserted) Int 0->1 transition");

  // Int 1 -> 0
  for (i = 0; i<10 && !TimeOut && !Error; i = i+1)
   begin
     if (!rcp_int) 
      begin
       Error = 1'b1; 
       $display($time," Error: Interrupt signal should be set");
      end
     @(posedge `CLK);
   end
  fork
    begin
     cycle_count = 0;
     if (!(TimeOut || Error)) reality.r4200b_0.write_word('h404_0010, 3, 32'h8); //clear Interrupt
     wait (!rcp_int || TimeOut || Error); 
    end
  join
  $display($time," Completed (MBUS asserted) Int 1->0 transition");

  $display("clear interrupt mask");
  reality.r4200b_0.write_word('h430_000c, 3, 32'h1); //clear interrupt mask

  repeat (10) @(posedge `CLK);

    if (TimeOut)
       $fwrite(fp,"test_intr \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (Error)
       $fwrite(fp,"test_intr \t FAILED... \t **Functional Problem ** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_intr \t Passed    \t                 \t Time=%0d \n",$time);

    clean_up;
  if (dump_true) $dumpoff;
end
endtask

task change_and_run_pc;
input [11:0] new_pc;
input run;
output Error;
  begin
    Error = 0;
    if (`Halt ) $display($time," Halt  = 1"); 
    if (`Break) $display($time," Break = 1"); 
    $display("writing PC = %h",new_pc);
    reality.r4200b_0.write_word('h408_0000, 3, new_pc); 
    $display("reading PC");
    reality.r4200b_0.read_word('h408_0000, 3);
    $display("read PC = %h",reality.r4200b_0.data[0]);
    if (reality.r4200b_0.data[0] !== new_pc) 
       begin
        Error = 1;
        $display($time," PC mismatch. Expected=%h, Actual=%h",new_pc,reality.r4200b_0.data[0]); 
       end
    else
    if (run)
       begin
         $display("clearing halt,break"); 
         reality.r4200b_0.write_word('h404_0010, 3, 32'h5); // clear halt and break
	 wait (`Halt===0);
       end
  end
endtask 

task check_and_run_pc;
input [11:0] exp_pc;
input run;
output Error;
  begin
    Error = 0;
    if (`Halt ) $display($time," Halt  = 1"); 
    if (`Break) $display($time," Break = 1"); 
    $display("reading PC");
    reality.r4200b_0.read_word('h408_0000, 3);
    $display("read PC = %h",reality.r4200b_0.data[0]);
    if (reality.r4200b_0.data[0] !== exp_pc) 
       begin
        Error = 1;
        $display($time," PC mismatch. Expected=%h, Actual=%h",exp_pc,reality.r4200b_0.data[0]); 
       end
    else
    if (run)
       begin
         $display("clearing halt,break"); 
         reality.r4200b_0.write_word('h404_0010, 3, 32'h5); // clear halt and break
	 wait (`Halt===0);
       end
  end
endtask 

task run_test_pc;
integer  i;
reg Error;
begin
  Error = 0;

  $readmemh("tests/test_pc.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_pc.rdram_DMEM",`rdram_0.core.core);
  $readmemh("tests/ctrace/test_pc.trSU",`tr.su_mem);
  $readmemh("tests/ctrace/test_pc.trVU",`tr.vu_mem);
  $readmemh("tests/ctrace/test_pc.trDM",`tr.dm_mem);

  TimerOn = 1;
  if (dump_true) $dumpon;

  reality.r4200b_0.sp_dma_read(32'h1000,32'h1000,'h0,'h0,12'h1ff); // in to dmem
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Imem 

  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
 
  if (dump_true) $dumpon;
  `tr.test_on = 1;

  if (!(TimeOut || Error || `tr.trError)) change_and_run_pc('h0, 1'b1, Error);  // Start
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError)) check_and_run_pc('h88, 1'b1, Error);  //continue
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h114, 1'b0, Error); 
  if (!(TimeOut || Error || `tr.trError)) change_and_run_pc('h168, 1'b1, Error); //jump to L1
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h17c, 1'b0, Error);
  if (!(TimeOut || Error || `tr.trError)) change_and_run_pc('h198, 1'b1, Error); //jump to L2
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h1a4, 1'b0, Error);
  if (!(TimeOut || Error || `tr.trError)) change_and_run_pc('h1c8, 1'b1, Error); //jump to L4
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h1e4, 1'b1, Error);
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h1c8, 1'b0, Error);
  if (!(TimeOut || Error || `tr.trError)) change_and_run_pc('h20c, 1'b1, Error); //jump to L6
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 

  if (!(TimeOut || Error || `tr.trError))  check_and_run_pc('h224, 1'b0, Error); //End
  wait ((`Break==1 && `Halt==1) || TimeOut || Error || `tr.trError); 


  repeat (10) @(posedge `CLK);
  `tr.test_on = 0;
  repeat (2) @(posedge `CLK);

  if (TimeOut)
     $fwrite(fp,"test_pc \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
  else
  if (`tr.trError || `tr.trError_eot)
     $fwrite(fp,"test_pc \t FAILED... \t **Trace Error** \t Time=%0d \n",$time);
  else
  if (Error)
     $fwrite(fp,"test_pc \t FAILED... \t **Functional Problem ** \t Time=%0d \n",$time);
  else
     $fwrite(fp,"test_pc \t Passed    \t                 \t Time=%0d \n",$time);

  if (dump_true) $dumpoff;

    clean_up;

end
endtask

task run_test_sem_1;
reg [15:0] SWEEP_CNT;
integer  i;
reg EOT, Error;
reg CPU_GOT_IT, RSP_GOT_IT;
begin
  EOT = 0;
  Error = 0;

  $readmemh("tests/test_sem_1.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_sem_1.rdram_DMEM",`rdram_0.core.core);

  reality.r4200b_0.write_word('h470_0010, 3, 32'h0); // Disable refresh

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Imem 

  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  $display("clearing semaphore");
  reality.r4200b_0.write_word('h0404_001c, 3, 32'h0); //clear semaphore

  $display("clearing halt,break");
  reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break


  if (dump_true) $dumpon;
  SWEEP_CNT = sweep_start;

  CPU_GOT_IT = 0; 
  RSP_GOT_IT = 0; 

  while (!EOT)
    begin
   	cycle_count = 0; wait (`Halt==0 || TimeOut || Error);
        if (SWEEP_CNT>0) 
            begin
   	      cycle_count = 0; wait ((`Break==1 && `Halt==1) || TimeOut || Error); //break instruction

	      RSP_GOT_IT = `r1 & 1'b1;
	      CPU_GOT_IT = reality.r4200b_0.data[0] & 1'b1;
	       
              if (RSP_GOT_IT==='hx || RSP_GOT_IT==='hz) 
		begin
		  Error = 1;
		  $display($time," ERROR: RSP lock is undefined (x,z)"); 
		end
              if (CPU_GOT_IT==='hx || CPU_GOT_IT==='hz) 
		begin
		  Error = 1;
		  $display($time," ERROR: CPU lock is undefined (x,z)"); 
		end
		 
	      if (!(CPU_GOT_IT^RSP_GOT_IT) && !Error) 
		begin
		  Error = 1;
		  if (CPU_GOT_IT===1)
		     $display($time,"ERROR:  Both contenders failed to get lock"); 
		  else
		     $display($time,"ERROR:  Both contenders granted lock"); 
		end
	      else
		begin
		  if (CPU_GOT_IT===0)
		     $display($time," CPU succeeded in getting lock"); 
		  else
		     $display($time," RSP succeeded in getting lock"); 
		end

  		$display("releasing semaphore lock");
  		reality.r4200b_0.write_word('h0404_001c, 3, 32'h0); //release lock

            end

	if (SWEEP_CNT==20 || Error || TimeOut ) EOT = 1;

	if (!EOT) 
         begin
	   $display ($time," Starting Sweep # %d",SWEEP_CNT);
   	
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
   	   cycle_count = 0; wait (`Halt==0 || TimeOut || Error);
   	
   	   for (i=0; i<SWEEP_CNT; i=i+1) @(posedge `CLK); 
           reality.r4200b_0.read_word('h0404_001c, 3); // read semaphore
	   SWEEP_CNT = SWEEP_CNT + 1;
	  end
    end

    repeat (10) @(posedge `CLK); 

    if (TimeOut)
       $fwrite(fp,"test_sem_1 \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (Error)
       $fwrite(fp,"test_sem_1 \t FAILED... \t **Functional** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_sem_1 \t Passed    \t                 \t Time=%0d \n",$time);
    clean_up;

end
endtask

task run_test_bugfix_3_1;
reg [15:0] SWEEP_CNT;
integer  i;
reg EOT;
begin
  EOT = 0;

  $readmemh("tests/test_bugfix_3_1.rdram_IMEM",`rdram_0.core.core);
  $readmemh("tests/test_bugfix_3_1.rdram_DMEM",`rdram_0.core.core);
  $readmemh("tests/ctrace/test_bugfix_3_1.trSU",`tr.su_mem);
  $readmemh("tests/ctrace/test_bugfix_3_1.trVU",`tr.vu_mem);
  $readmemh("tests/ctrace/test_bugfix_3_1.trDM",`tr.dm_mem);

  reality.r4200b_0.write_word('h470_0010, 3, 32'h0); // Disable refresh

  TimerOn = 1;
  if (dump_true) $dumpoff;
  load_mem_from_rdram(32'h0, 32'h0,12'hfff); //init  Dmem 
  load_mem_from_rdram(32'h1000, 32'h1000,12'hfff); //init  Imem 

  //$display (" AFTER DMA ");
  //dump_rdram('h0,'h400);
 
  $display("clearing PC");
  reality.r4200b_0.write_word('h408_0000, 3, 32'h0); //clear pc

  $display("clearing halt,break");
  reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break

  if (dump_true) $dumpon;
  SWEEP_CNT = sweep_start;

  while (!EOT)
    begin
   	cycle_count = 0; wait (`Halt==0 || TimeOut || `tr.trError);
   	cycle_count = 0; wait ((`Break==1 && `Halt==1) || TimeOut || `tr.trError); //break instruction
        if (SWEEP_CNT>0) `tr.test_on = 0;
	
	@(posedge `CLK);

	if (SWEEP_CNT==10 || `tr.trError ||  `tr.trError_eot || TimeOut ) EOT = 1;

	if (!EOT) 
         begin
           `tr.test_on = 1;
	   $display ($time," Starting Sweep # %d",SWEEP_CNT);
   	
   	   reality.r4200b_0.write_word('h404_0010, 3, 32'h5); //clear halt,break
   	
   	   cycle_count = 0; wait (`Halt==0 || TimeOut);
   	
   	   for (i=0; i<(SWEEP_CNT+13); i=i+1) @(posedge `CLK); 
	   SWEEP_CNT = SWEEP_CNT + 1;
   
	   bugfix_3_1_dma_sequence;

	  end
    end

    repeat (10) @(posedge `CLK); 

    if (TimeOut)
       $fwrite(fp,"test_bugfix_3_1 \t FAILED... \t **TIME_OUT   ** \t Time=%0d \n",$time);
    else
    if (`tr.trError|| `tr.trError_eot)
       $fwrite(fp,"test_bugfix_3_1 \t FAILED... \t **TRACE_ERROR** \t Time=%0d \n",$time);
    else
       $fwrite(fp,"test_bugfix_3_1 \t Passed    \t                 \t Time=%0d \n",$time);

    clean_up;

end
endtask

task bugfix_3_1_dma_sequence;
begin
  @(negedge `CLK);
  force `rsp_path.dma_imem_select = 1;
  force `rsp_path.dma_rd_to_dm = 1;
  force `rsp_path.mem_write_data = 'h0;

  @(negedge `CLK);
  release `rsp_path.dma_imem_select;
  release `rsp_path.dma_rd_to_dm;
  force `rsp_path.dma_address = 9'h1f0;

  @(negedge `CLK);
  release `rsp_path.dma_address;

  @(negedge `CLK);
  force `rsp_path.dma_imem_select = 0;
  force `rsp_path.dma_dm_to_rd = 1;
   
  @(negedge `CLK);
  force `rsp_path.dma_imem_select = 1;
  force `rsp_path.dma_rd_to_dm = 1;
  release `rsp_path.dma_dm_to_rd;
  force `rsp_path.dma_address = 9'h1f0;

  @(negedge `CLK);
  @(negedge `CLK);
  @(negedge `CLK);
  release `rsp_path.dma_imem_select;
  release `rsp_path.dma_rd_to_dm;
  release `rsp_path.dma_dm_to_rd;
  release `rsp_path.mem_write_data;

  @(negedge `CLK);
  release `rsp_path.dma_address;

  @(negedge `CLK);
  @(negedge `CLK);

end
endtask