`define MEMBASE 'h400000
`define DATABASE 'h10010000
`define TEXTSIZE 'h100
`define MEMSIZE 'h200
`define MEMRANGE `MEMBASE:`MEMBASE+`MEMSIZE-1
`define A(addr) ((addr)-`DATABASE+`MEMBASE+`TEXTSIZE)
`define MEM(addr) mem[((addr)-`DATABASE+`MEMBASE+`TEXTSIZE)]

module test_proc();

   parameter read_trace = 0;
   parameter write_trace = 0;
   parameter shadow_trace = 1;
   parameter tr_size = 100;
   parameter use_trace = read_trace | shadow_trace;

   wire [7:0] exception;
   reg        reset, clk, clk2;
   wire       exc;

   system dut(exception,reset,clk);
   tb_proc p2(exc,clk2);
   
   reg [31:0] gpr_shadow [0:31];
   reg [31:0] gpr_shadow2 [0:31];
   reg [319:0] isource[dut.m1.text_base>>2:dut.m1.text_base+dut.m1.text_size>>2];
   reg [31:0]  regname [0:31];
   reg [15:0]  statename [0:7];

   integer    i, reg_old, reg_new;
   reg        go;
   real       cycle_count;
   integer    icount;
   integer changed_reg, changed_reg_count;
   reg [31:0] tr_pc_a [0:tr_size];
   reg [31:0] tr_regv_a [0:tr_size];
   reg [4:0]  tr_regno_a [0:tr_size];
   reg [31:0] tr_pc;
   reg [31:0] tr_regv;
   reg [4:0]  tr_regno;
   integer    tr_fd, tr_icount_end;
   real       ireal;
   reg [31:0] last_pc;

   always wait( go ) begin clk = 0; #9; clk = 1; #1; end

   task initmem;
      input [31:0] addr;
      input [31:0] text;
      input [319:0] source;

      reg [2:0] err;

      begin

         err = dut.m1.poke_word(addr,text);

         if( err ) begin
            $display("Error %d initializing text address 0x%h.",
                     err, addr);
            $stop;
         end

         {p2.mem[addr],p2.mem[addr+1],p2.mem[addr+2],p2.mem[addr+3]} = text;

         while( source[319:312] === 8'b0 ) source = {source[311:0]," "};
         isource[addr>>2] = source;

      end

   endtask

   task initdmem;
      input [31:0] addr;
      input [31:0] word;

      reg [31:0] daddr;
      reg [2:0] err;

      begin

         err = dut.m1.poke_word(addr,word);

         if( err ) begin
            $display("Error %d initializing data address 0x%h.",
                     err, addr);
            $stop;
         end

         daddr = `A(addr);
         {p2.mem[daddr],p2.mem[daddr+1],p2.mem[daddr+2],p2.mem[daddr+3]} = word;

      end

   endtask

   task get_trace_record;
      output [31:0] tr_pc;
      output [5:0] tr_regno;
      output [31:0] tr_regv;

      integer i;
      reg [31:0] cpy_reg [0:31];

      if( read_trace ) begin

         tr_pc    = tr_pc_a[icount];
         tr_regno = tr_regno_a[icount];
         tr_regv  = tr_regv_a[icount];

      end else if ( write_trace ) begin

         $fdisplay(tr_fd, " tr_regno_a[%d] = %d; tr_regv_a[%d] = 'h%h;",
                   icount, changed_reg,
                   icount, gpr_shadow[changed_reg]);

         $fdisplay(tr_fd," tr_pc_a[%d] = 'h%h;", icount,dut.cpu1.pc);

      end else if ( shadow_trace ) begin
         
         tr_pc    = p2.pc;
         tr_regno = 0;
         tr_regv  = 0;

         for(i=0; i<32; i=i+1) if( gpr_shadow2[i] !== p2.gpr[i] )
           begin
              tr_regno       = i;
              tr_regv        = p2.gpr[i];
              gpr_shadow2[i] = tr_regv;
           end

         p2.step;

      end

   endtask
   
   integer rno;

   task initregs;
      input [31:0] name;
      input [3:0] cnt;
      integer i;
      for(i=0; i<cnt; i=i+1) begin
         regname[rno] = name + i;
         rno = rno + 1;
      end
   endtask

   task cpi_and_stop;
      real cpi,icountr;
      begin
         cpi = icount ? cycle_count / icount : 0;
         icountr = icount;
         $display("Executed %.0f instructions, average time %.2f CPI.",
                  icountr, cpi);
         $display("End of testbench run.");

         $stop;
      end
   endtask

   initial begin

      go = 0;
      cycle_count = 0;
      icount = 0;
      reset = 1;

      begin:INITNAMES
         integer i;

         regname[0] = "zero";
         regname[1] = "at";
         rno = 2;
         initregs("v0",2);
         initregs("a0",4);
         initregs("t0",8);
         initregs("s0",8);
         initregs("t8",2);
         initregs("k0",2);
         regname[28] = "gp";
         regname[29] = "sp";
         regname[30] = "fp";
         regname[31] = "ra";

         for(i=0; i<8; i=i+1) statename[i] = "S0"+i;
         statename[1] = "IF";
         statename[2] = "ID";
         statename[3] = "EX";
         statename[4] = "ME";
         statename[5] = "WB";

      end

      if( read_trace ) begin
         $display("Not working at the moment implemented.");
         $stop;
// `include "trace.v"
      end

      if( write_trace )
        tr_fd = $fopen("trace.v");

`include `MIPS_PROG

      for(i=0; i<32; i=i+1)
        begin:B
           reg [31:0] val;
           val = i * 10;
           dut.cpu1.gpr[i] = val;
           p2.gpr[i] = val;
           gpr_shadow[i] = val;
           gpr_shadow2[i] = val;
        end

      dut.cpu1.pc = 'h400000;
      p2.pc = 'h400000;
      dut.cpu1.npc = dut.cpu1.pc + 4;
      p2.npc = dut.cpu1.pc + 4;

      icount = 0;
      go = 1;
      changed_reg = 0;
      changed_reg_count = 0;

      wait( clk == 0 );
      @( posedge clk ); @( negedge clk );
      reset = 0;
      
      while( !exception ) begin
         cycle_count = cycle_count + 1;
         if( dut.cpu1.state == 1 ) begin

            get_trace_record(tr_pc,tr_regno,tr_regv);

            last_pc = dut.cpu1.pc;
            
            if( !exception && use_trace ) begin

               if( changed_reg_count > 1 ) begin
                  $display("Too many registers, %d, changed.\n",
                           changed_reg_count);
                  cpi_and_stop;
               end

               if( tr_regno != changed_reg ) begin
                  $display("*** FAIL: Wrong register modified %d (%s) should change, %d (%s) changed. ***",
                           tr_regno, regname[tr_regno],
                           changed_reg, regname[changed_reg]);
                  cpi_and_stop;
               end

               if( tr_regv !== gpr_shadow[changed_reg] ) begin
                  $display("*** FAIL: Wrong value written.  0x%h (correct) != 0x%h ***",
                           tr_regv,gpr_shadow[changed_reg]);
                  cpi_and_stop;
               end
            end
            
            icount = icount + 1;
            changed_reg_count = 0;
            changed_reg = 0;

            if(0)$display("%s  PC 0x%h:  %-s",
                     statename[dut.cpu1.state], dut.cpu1.pc,
                     isource[dut.cpu1.pc>>2] );

            $display(" PC 0x%h:  %-s",
                     dut.cpu1.pc, isource[dut.cpu1.pc>>2] );

            if( use_trace && tr_pc !== dut.cpu1.pc ) begin
               $display("*** FAIL: PC mismatch at instruction %d, 0x%h (correct) != %h ***",
                        icount, tr_pc, dut.cpu1.pc);
               cpi_and_stop;
            end

         end
         @( negedge clk );
         for(i=0; i<32; i=i+1)
           if( gpr_shadow[i] !== dut.cpu1.gpr[i] ) begin
              changed_reg = i;
              changed_reg_count = changed_reg_count + 1;
              reg_old = gpr_shadow[i];
              reg_new = dut.cpu1.gpr[i];
              ireal = i;
              $display(" Register \$%.0f (%s): 0x%h (%d) -> 0x%h (%d)",
                       ireal, regname[i], gpr_shadow[i],
                       reg_old, dut.cpu1.gpr[i], reg_new);
              gpr_shadow[i] = dut.cpu1.gpr[i];
           end
      end

      if( write_trace ) begin
         if( icount > tr_size )
           $display("WARNING!!! Increase tr_size from %d to at least %d.\n",
                    tr_size,icount);
         $fdisplay(tr_fd," tr_icount_end = %d;",icount);
         $fclose(tr_fd);
      end

      if( read_trace ) begin
         if( icount != tr_icount_end ) begin
            $display("*** FAIL: Valid instruction unrecognized. ***");
         end else begin
            $display("PASS: Correctly executed %d instructions.",icount);
         end
      end

      if( exception ) begin
         if( dut.cpu1.ir == 'hc )
           $display("Ending normally at a syscall instruction.");
         else
           $display("*** Illegal instruction exception at address 0x%h ***",
                    last_pc);
      end

      cpi_and_stop;

   end

endmodule


module tb_proc(exc,clk);
   input clk;
   output exc;

   reg    exc;

   reg [7:0] mem [`MEMRANGE];

   reg [31:0] pc, npc, nnpc, ir;
   reg [31:0] gpr [0:31];

   reg [5:0]  opcode, funct;
   reg [4:0]  rs, rt, rd, sa;
   reg [15:0] immed;
   reg [25:0] ii;
   reg [31:0] uimm16, simm16;
   reg [31:0] branch_target;

   // Values for funct field.
   parameter  f_sll = 6'h0;
   parameter  f_srl = 6'h2;
   parameter  f_sllv = 6'h4;
   parameter  f_srlv = 6'h6;
   parameter  f_add = 6'h20;
   parameter  f_sub = 6'h22;
   parameter  f_or  = 6'h25;
   parameter  f_jr  = 6'h8;

   // Values for opcode field.
   parameter  o_j     = 6'h2;
   parameter  o_beq   = 6'h4;
   parameter  o_bne   = 6'h5;
   parameter  o_addi  = 6'h8;
   parameter  o_slti  = 6'ha;
   parameter  o_sltiu = 6'hb;
   parameter  o_andi  = 6'hc;
   parameter  o_ori   = 6'hd;
   parameter  o_lui   = 6'hf;
   parameter  o_lw    = 6'h23;
   parameter  o_lbu   = 6'h24;
   parameter  o_lb    = 6'h20;
   parameter  o_sw    = 6'h2b;
   parameter  o_sb    = 6'h28;
   parameter  o_jal   = 6'h3;

   initial begin
      exc = 0;
      pc = 0;
      npc = 4;
   end


   task step;

      begin

      ir = {mem[pc],mem[pc+1],mem[pc+2],mem[pc+3]};

      // R Format
      {opcode,rs,rt,rd,sa,funct} = ir;

      nnpc = npc + 4; // May be reassigned below.

      if( !opcode ) begin
         //
         // R-Format Instructions
         case( funct )
           f_sllv  : gpr[rd] = gpr[rt] << ( gpr[rs] & 32'h1f );
           f_srlv  : gpr[rd] = gpr[rt] >> ( gpr[rs] & 32'h1f );
           f_sll   : gpr[rd] = gpr[rt] << sa;
           f_srl   : gpr[rd] = gpr[rt] >> sa;
           f_add   : gpr[rd] = gpr[rs] + gpr[rt];
           f_or    : gpr[rd] = gpr[rs] | gpr[rt];
           f_sub   : gpr[rd] = gpr[rs] - gpr[rt];
           f_jr    : nnpc = gpr[rs];
           default : exc = 1;
         endcase

      end else begin
         //
         // I- and J-Format Instructions

         // I Format  (Also uses opcode, rs, and rt.)
         immed = ir[15:0];
         // J Format  (Also uses opcode.)
         ii = ir[25:0];

         uimm16 = { 16'b0, immed };
         simm16 = immed[15] ? { 16'hffff, immed } : uimm16;

         branch_target = npc + ( simm16 << 2 );

         case( opcode )
           o_j     : nnpc = {npc[31:28],ii,2'b0};
           o_jal: begin gpr[31] = nnpc;  nnpc = {npc[31:28],ii,2'd0};  end
           o_beq   : if( gpr[rs] == gpr[rt] ) nnpc = branch_target;
           o_bne   : if( gpr[rs] != gpr[rt] ) nnpc = branch_target;
           o_andi  : gpr[rt] = gpr[rs] & uimm16;
           o_sltiu  : gpr[rt] = gpr[rs] < simm16;
           o_slti  : begin:A
              integer a, b;
              a = gpr[rs];  b = simm16;
              gpr[rt] = a < b;
           end
           o_addi  : gpr[rt] = gpr[rs] + simm16;
           o_ori   : gpr[rt] = gpr[rs] | uimm16;
           o_lui   : gpr[rt] = { immed, 16'b0 };
           o_lbu   : gpr[rt] = { 24'b0, `MEM( gpr[rs] + simm16 ) };
           o_lb    : begin:LB
              reg [7:0] c;
              c = `MEM( gpr[rs] + simm16 );
              gpr[rt] = {c[7] ? 24'hffffff : 24'h0, c };
           end
           o_sb    : `MEM( gpr[rs] + simm16 ) = gpr[rt];
           default : exc = 1;
         endcase

      end

      gpr[0] = 0;

      pc = npc;
      npc = nnpc;

      end

   endtask


endmodule


module mips_memory_1p(dout,err,addr,size,we,din,clk);
   input [31:0] addr;
   input [1:0]  size;
   input        we;
   input [31:0] din;
   input        clk;
   output [31:0] dout;
   output [2:0]  err;

   reg [31:0]    dout;
   reg [2:0]     err;

   parameter     text_base = 'h400000;
   parameter     text_size = 'h100;
   parameter     data_base = 'h10010000;
   parameter     data_size = 'h100;

   parameter     err_none = 0;
   parameter     err_bus  = 1;   // Bad alignment.
   parameter     err_seg  = 2;   // Bad address.

   reg [31:0]    storage [0:(text_size+data_size)>>2];
   reg [31:0]    saddr, mask, rd;
   reg [4:0]     amt;

   function [31:0] addr_to_saddr;
      input [31:0] a;

      if( a >= text_base && a < text_base + text_size ) begin
         addr_to_saddr = a - text_base >> 2;
      end else if( a >= data_base && a < data_base + data_size ) begin
         addr_to_saddr = a - data_base + text_size >> 2;
      end else begin
         addr_to_saddr = -1;
      end

   endfunction
   

   reg [2:0]  access_mem_err;

   function [31:0] access_mem;
      input [31:0] addr;
      input [1:0] size;
      input we;
      input [31:0] din;

      begin

         saddr = addr_to_saddr(addr);

         if( !size ) begin

            access_mem_err = err_none;
            access_mem = 0;

         end else if( saddr == -1 ) begin

            access_mem_err = err_seg;
            access_mem = 32'bz;

         end else begin

            access_mem_err  = addr << 3 - size & 3 ? err_bus : err_none;
            amt  = 3 - addr[1:0] - { &size, size[1] } << 3;
            mask = ( ( 1 << ( 8 << (size-1) ) ) - 1 ) << amt;
            rd   = storage[ saddr ];
            access_mem = err == err_bus ? 32'bzx : ( rd & mask ) >> amt;

            if( we ) storage[ saddr ] = ( rd & ~mask ) | ( (din<<amt ) & mask );

         end

      end

   endfunction

   function [31:0] peek_word;
      input [31:0] peek_addr;

      peek_word = access_mem(peek_addr,3,0,0);

   endfunction

   function [2:0] poke_word;
      input [31:0] poke_addr;
      input [31:0] poke_data;

      reg [31:0] dummy;

      begin
         dummy = access_mem(poke_addr,3,1,poke_data);
         poke_word = access_mem_err;
      end

   endfunction

   always @( negedge clk ) begin
      dout = access_mem(addr,size,we,din);
      err = access_mem_err;
   end


endmodule