`default_nettype none

module slave_dispatcher
  (
   input wire 	      CLK,
   input wire 	      RST,

   input wire 	      S0_ACK_O,
   output wire [17:0] S0_ADR_I,
   output wire [15:0] S0_DAT_I,
   input wire [15:0]  S0_DAT_O,
   output wire 	      S0_STB_I,
   output wire 	      S0_WE_I,
   input wire 	      S0_STALL_O,

   input wire 	      S1_ACK_O,
   output wire [17:0] S1_ADR_I,
   output wire [15:0] S1_DAT_I,
   input wire [15:0]  S1_DAT_O,
   output wire 	      S1_STB_I,
   output wire 	      S1_WE_I,
   input wire 	      S1_STALL_O,

   input wire 	      S2_ACK_O,
   output wire [16:0] S2_ADR_I,
   output wire [15:0] S2_DAT_I,
   input wire [15:0]  S2_DAT_O,
   output wire 	      S2_STB_I,
   output wire 	      S2_WE_I,
   input wire 	      S2_STALL_O,

   input wire 	      S3_ACK_O,
   output wire [16:0] S3_ADR_I,
   output wire [15:0] S3_DAT_I,
   input wire [15:0]  S3_DAT_O,
   output wire 	      S3_STB_I,
   output wire 	      S3_WE_I,
   input wire 	      S3_STALL_O,

   input wire 	      S4_ACK_O,
   output wire [16:0] S4_ADR_I,
   output wire [15:0] S4_DAT_I,
   input wire [15:0]  S4_DAT_O,
   output wire 	      S4_STB_I,
   output wire 	      S4_WE_I,
   input wire 	      S4_STALL_O,

   input wire 	      S5_ACK_O,
   output wire [16:0] S5_ADR_I,
   output wire [15:0] S5_DAT_I,
   input wire [15:0]  S5_DAT_O,
   output wire 	      S5_STB_I,
   output wire 	      S5_WE_I,
   input wire 	      S5_STALL_O,

   output wire 	      S_COMBINED_ACK_O,
   input wire [19:0]  S_COMBINED_ADR_I,
   input wire [15:0]  S_COMBINED_DAT_I,
   output wire [15:0] S_COMBINED_DAT_O,
   input wire 	      S_COMBINED_STB_I,
   input wire 	      S_COMBINED_WE_I,
   output wire 	      S_COMBINED_STALL_O
   );

   wire [0:5] 	      acks;
   wire [0:5] 	      stalls;
   wire [15:0] 	      datas [0:5];
   assign acks = {S0_ACK_O, S1_ACK_O, S2_ACK_O, S3_ACK_O, S4_ACK_O, S5_ACK_O};
   assign stalls = {S0_STALL_O, S1_STALL_O, S2_STALL_O,
		    S3_STALL_O, S4_STALL_O, S5_STALL_O};
   assign datas[0] = S0_DAT_O;
   assign datas[1] = S1_DAT_O;
   assign datas[2] = S2_DAT_O;
   assign datas[3] = S3_DAT_O;
   assign datas[4] = S4_DAT_O;
   assign datas[5] = S5_DAT_O;

   reg [1:0] 	      commands_awaiting;
   reg [2:0] 	      slave_last_accessed;

   wire 	      working;
   wire [2:0] 	      slave_accessed;
   wire [2:0] 	      slave_requested;
   wire 	      slave_switch;
   wire [1:0] 	      commands_awaiting_next_tick;
   assign working = commands_awaiting || S_COMBINED_STB_I;
   assign slave_requested = S_COMBINED_ADR_I[19] ?
			    /* one of 4 smaller slaves */
			    S_COMBINED_ADR_I[18:17] + 2 :
			    /* one of 2 bigger slaves */
			    S_COMBINED_ADR_I[19:18];
   assign slave_accessed = commands_awaiting ? slave_last_accessed :
			   slave_requested;
   assign S_COMBINED_ACK_O = acks[slave_accessed] && working;
   assign S_COMBINED_DAT_O = datas[slave_accessed];
   assign slave_switch = slave_accessed != slave_requested;
   assign S_COMBINED_STALL_O = stalls[slave_accessed] || slave_switch ||
			       (commands_awaiting == 3 && !S_COMBINED_ACK_O);
   assign commands_awaiting_next_tick
     = commands_awaiting - S_COMBINED_ACK_O +
       (S_COMBINED_STB_I && !S_COMBINED_STALL_O);

`ifdef SIMULATION
   /* anything could be latched here, this is just to avoid undefined values */
   initial begin
      slave_last_accessed <= 0;
      commands_awaiting <= 2; /* It is supposed to be driven low by RST */
   end
`endif

   always @ (posedge CLK) begin
      slave_last_accessed <= slave_accessed;

      if (RST)
	commands_awaiting <= 0;
      else
	commands_awaiting <= commands_awaiting_next_tick;
   end

   assign S0_ADR_I = S_COMBINED_ADR_I[17:0];
   assign S1_ADR_I = S_COMBINED_ADR_I[17:0];
   assign S2_ADR_I = S_COMBINED_ADR_I[16:0];
   assign S3_ADR_I = S_COMBINED_ADR_I[16:0];
   assign S4_ADR_I = S_COMBINED_ADR_I[16:0];
   assign S5_ADR_I = S_COMBINED_ADR_I[16:0];

   assign S0_DAT_I = S_COMBINED_DAT_I;
   assign S1_DAT_I = S_COMBINED_DAT_I;
   assign S2_DAT_I = S_COMBINED_DAT_I;
   assign S3_DAT_I = S_COMBINED_DAT_I;
   assign S4_DAT_I = S_COMBINED_DAT_I;
   assign S5_DAT_I = S_COMBINED_DAT_I;

   wire pass_strobe;
   assign pass_strobe = S_COMBINED_STB_I && !slave_switch &&
			(commands_awaiting != 3 || S_COMBINED_ACK_O);

   assign S0_STB_I = slave_accessed == 0 && pass_strobe;
   assign S1_STB_I = slave_accessed == 1 && pass_strobe;
   assign S2_STB_I = slave_accessed == 2 && pass_strobe;
   assign S3_STB_I = slave_accessed == 3 && pass_strobe;
   assign S4_STB_I = slave_accessed == 4 && pass_strobe;
   assign S5_STB_I = slave_accessed == 5 && pass_strobe;

   assign S0_WE_I = S_COMBINED_WE_I;
   assign S1_WE_I = S_COMBINED_WE_I;
   assign S2_WE_I = S_COMBINED_WE_I;
   assign S3_WE_I = S_COMBINED_WE_I;
   assign S4_WE_I = S_COMBINED_WE_I;
   assign S5_WE_I = S_COMBINED_WE_I;
endmodule // slave_dispatcher