`default_nettype none

module stack_machine
  (
   input wire 	     ACK_I,
   input wire 	     CLK_I,
   output reg [19:0] ADR_O,
   input wire [15:0] DAT_I,
   output reg [15:0] DAT_O,
   input wire 	     RST_I,
   output reg 	     STB_O,
   output reg 	     CYC_O,
   output reg 	     WE_O,
   input wire 	     STALL_I,

   /* non-wishbone */
   output wire 	     finished
   );

   reg [19:0] 	      pc;
   reg [19:0] 	      sp;

   reg [15:0] 	      instr;

   reg [31:0] 	      r0;
   reg [31:0] 	      r1;
   reg [31:0] 	      im;

   parameter STEP_START = 0;
   parameter STEP_LOADING_INSTRUCTION = 1;
   parameter STEP_PARSING_INSTRUCTION = 2;
   parameter STEP_LOAD_OR_STORE = 3;
   parameter STEP_PERFORM_OPERATION = 4;
   reg [2:0] 	      step;

   reg [15:0] 	      DAT_I_latched;
   reg [15:0] 	      instruction_latched;
   reg 		      instruction_just_received;
   wire [15:0] 	      instruction;
   assign instruction = instruction_just_received ?
			DAT_I_latched : instruction_latched;

   /* Results of instruction parsing */
   wire 	      shift_immediate; /* only if modify_immediate is true */
   assign shift_immediate = instruction[11];

   /* only if shift_immediate is true */
   wire [10:0] 	      immediate_bits_to_shift;
   assign immediate_bits_to_shift = instruction[10:0];

   /* only if shift_immediate is false */
   wire 	      set_immediate;
   assign set_immediate = instruction[10];

   /* only if (set_immediate && !shift_immediate) is true */
   wire [31:0] 	      immediate_value_to_set;
   assign immediate_value_to_set = {{22{instruction[9]}}, instruction[9:0]};

   /* only if (!set_immediate && !shift_immediate) is true */
   wire 	      add_immediate;
   assign add_immediate = instruction[9];

   /* only if (add_immediate && !set_immediate && !shift_immediate) is true */
   wire [9:0] 	      immediate_value_to_add;
   assign immediate_value_to_add = instruction[8:0];


   wire [31:0] 	      new_im;
   assign new_im = shift_immediate ? {im[20:0], immediate_bits_to_shift} :
		   set_immediate ? immediate_value_to_set :
		   add_immediate ? im + immediate_value_to_add : im;


   wire 	      shift_regs;
   assign shift_regs = instruction[12];


   wire 	      load_or_store;
   assign load_or_store = instruction[15];

   wire [19:0] 	      addr_to_use; /* only if load_or_store is true */
   /* The implementation is currently unable to make non-aligned accesses */
   assign addr_to_use = new_im / 2 + (immediate_addressing ? 0 : sp);

   wire 	      load; /* only if load_or_store is true */
   assign load = instruction[14];

   wire 	      store; /* only if load_or_store is true */
   assign store = !load;

   wire 	      immediate_addressing; /* only if load_or_store is true */
   assign immediate_addressing = instruction[13];
   wire 	      sp_addressing; /* only if load_or_store is true */
   assign sp_addressing = !immediate_addressing;

   wire 	      load_or_store_r0; /* only if load_or_store is true */
   assign load_or_store_r0 = instruction[12];

   wire 	      load_or_store_r1; /* only if load_or_store is true */
   assign load_or_store_r1 = !load_or_store_r0;


   /* only if load_or_store is false */
   wire 	      cond_jump;
   assign cond_jump = instruction[14:13] == 2'b11;

   /* only if load_or_store is false */
   wire 	      jump;
   assign jump = instruction[14:13] == 2'b10;

   /* only if load_or_store is false */
   wire 	      exchange_r1_im;
   assign exchange_r1_im = instruction[14:13] == 2'b01;

   /* only if load_or_store is false */
   wire 	      immediate_modification; 
   assign immediate_modification
     = instruction[14:13] == 0 &&
       (shift_immediate || set_immediate || add_immediate);

   wire 	      extended_instruction; /* only if load_or_store is false */
   assign extended_instruction = instruction[14:13] == 0 && ! shift_immediate &&
				 !set_immediate && !add_immediate;

   /* only if (!load_or_store && extended_instruction) is true */
   wire [8:0] 	      extended_instruction_bits;
   assign extended_instruction_bits = instruction[8:0];

   /* extended instructions */
   parameter INSTR_TEE = 9'd7;
   parameter INSTR_MUL = 9'd6;
   parameter INSTR_DIV = 9'd5;
   parameter INSTR_SUB = 9'd4;
   parameter INSTR_ADD = 9'd3;
   parameter INSTR_SET_SP = 9'd2;
   parameter INSTR_HALT = 9'd1;
   parameter INSTR_NOP = 9'd0;

   /* module for division */
   wire [31:0] 	      div_quotient;
   wire [31:0] 	      div_remainder;
   wire 	      div_done;

   div
     #(
       .WIDTH(32)
       ) div
   (
    .clock(CLK_I),
    .start(step == STEP_PARSING_INSTRUCTION),
    .dividend(r0),
    .divisor(r1),

    .quotient(div_quotient),
    .remainder(div_remainder),
    .done(div_done)
    );

   assign finished = !load_or_store && extended_instruction &&
		     extended_instruction_bits == INSTR_HALT;


   reg [15:0] 	      second_word_to_store;

   /*
    * Variables necessary to know where we are when transferring
    * a sequence of words through wishbone
    */
   reg 		      first_command_sent;
   reg 		      first_ack_received;

   /* Informs us, that DAT_I_latched should be moved to r0 or r1 */
   reg 		      load_half_of_r0;
   reg 		      load_half_of_r1;

`ifdef SIMULATION
   /*
    * RST should still be used when powering up, even in benches;
    * this is just to avoid undefined values
    */
   initial begin
      ADR_O <= 0;
      STB_O <= 0;
      CYC_O <= 0;
      WE_O <= 0;

      r0 <= 0;
      r1 <= 0;
      im <= 0;
      pc <= 0;
      sp <= 20'h40000;
      step <= STEP_START;
      instruction_latched <= 0;
      instruction_just_received <= 0;
   end
`endif

   always @ (posedge CLK_I) begin
      DAT_I_latched <= DAT_I;

      /*
       * Later part of this always block sets them to 1 under certain
       * conditions, which takes precedence over this assignment
       */
      load_half_of_r0 <= 0;
      load_half_of_r1 <= 0;
      instruction_just_received <= 0;

      if (RST_I) begin
	 STB_O <= 0;
	 CYC_O <= 0;

	 pc <= 0;
	 sp <= 20'h40000;
	 step <= STEP_START;
      end else begin
	 case (step)
	   STEP_START : begin
	      ADR_O <= pc;
	      STB_O <= 1;
	      CYC_O <= 1;
	      WE_O <= 0;
	      step <= STEP_LOADING_INSTRUCTION;
	      pc <= pc + 1;
	   end
	   STEP_LOADING_INSTRUCTION : begin
	      if (!STALL_I)
		STB_O <= 0;

	      if (ACK_I) begin
		 instruction_just_received <= 1;
		 step <= STEP_PARSING_INSTRUCTION;
		 CYC_O <= 0;
	      end
	   end // case: STEP_LOADING_INSTRUCTION
	   STEP_PARSING_INSTRUCTION : begin
	      im <= new_im;
	      instruction_latched <= instruction;

	      first_command_sent <= 0;
	      first_ack_received <= 0;

	      if (shift_regs)
		{r0, r1} <= {r1, r0}; /* magic! */

	      if (load_or_store) begin
		 CYC_O <= 1;
		 STB_O <= 1;
		 WE_O <= store;
		 ADR_O <= addr_to_use;

		 /* Even if not a store, setting these won't break anything */
		 DAT_O <= load_or_store_r0 ? r0[15:0] : r1[15:0];
		 second_word_to_store <= load_or_store_r0 ?
					 r0[31:16] : r1[31:16];

		 if (load_or_store_r0)
		   sp <= sp + (load ? 2 : -2);

		 step <= STEP_LOAD_OR_STORE;
	      end else begin // if (load_or_store)
		 if (immediate_modification) begin
		    step <= STEP_START;
		 end else if (exchange_r1_im || extended_instruction) begin
		    step <= STEP_PERFORM_OPERATION;
		 end else if (jump) begin
		    pc <= new_im / 2;
		    step <= STEP_START;
		 end else if (cond_jump) begin
		    if (r1 != 0)
		      pc <= new_im / 2;

		    step <= STEP_START;
		 end else begin
`ifdef SIMULATION
		    $display("operation not implemented yet");
`endif
		    step <= STEP_START;
		 end
	      end // else: !if(load_or_store)
	   end // case: STEP_PARSING_INSTRUCTION
	   STEP_LOAD_OR_STORE : begin
	      if (!STALL_I) begin
		 ADR_O <= ADR_O + 1;

		 DAT_O <= second_word_to_store;

		 if (!first_command_sent)
		   first_command_sent <= 1;
		 else
		   STB_O <= 0;
	      end // if (!STALL_I)

	      if (ACK_I) begin
		 if (load) begin
		    if (load_or_store_r0)
		      load_half_of_r0 <= 1;
		    else
		      load_half_of_r1 <= 1;
		 end

		 if (!first_ack_received)
		   first_ack_received <= 1;
		 else
		   step <= STEP_START;
	      end // if (ACK_I)
	   end // case: STEP_LOAD_OR_STORE
	   STEP_PERFORM_OPERATION : begin
	      step <= STEP_START; /* unless overwritten in case block below */

	      if (extended_instruction) begin
		 case (extended_instruction_bits)
		   INSTR_NOP :
		     ;
		   INSTR_HALT :
		     step <= step; /* stay in this step forever */
		   INSTR_SET_SP :
		    sp <= im / 2;
		   INSTR_ADD :
		     r0 <= r0 + r1;
		   INSTR_SUB :
		     r0 <= r0 - r1;
		   INSTR_DIV : begin
		      {r0, r1} <= {div_quotient, div_remainder};
		      if (!div_done)
			step <= step;
		   end
		   INSTR_MUL :
		     r0 <= r0 * r1;
		   INSTR_TEE :
		     r1 <= r0;
		   default : begin
`ifdef SIMULATION
		      $display("operation not implemented yet");
`endif
		   end
		 endcase // case (extended_instruction_bits)
	      end else if (exchange_r1_im) begin
		 {r1, im} <= {im, r1};
	      end else begin
`ifdef SIMULATION
		 $display("operation not implemented yet");
`endif
	      end
	   end // case: STEP_PERFORM_OPERATION
	 endcase // case (step)
      end // else: !if(RST_I)

      if (load_half_of_r0)
	r0 <= {DAT_I_latched, r0[31:16]};

      if (load_half_of_r1)
	r1 <= {DAT_I_latched, r1[31:16]};
   end // always @ (posedge CLK_I)

endmodule // stack_machine