1 files changed, 56 insertions, 12 deletions

diff --git a/models/slave.v b/models/slave.v
index f062a04..ed12a73 100644
--- a/models/slave.v
+++ b/models/slave.v
@@ -12,7 +12,7 @@ module memory_slave_model
   #(
     parameter SLAVE_NR = -1,
     parameter WORD_SIZE = 2, /* in bytes */
-    parameter GRANULARITY = 2, /* in bytes */ /* we'll use that soon */
+    parameter SEL_LINES = 1,
     parameter ADR_BITS = 18,
     /* Changing the following 3 allows us to make it function as ROM */
     parameter WRITABLE = 1,
@@ -26,21 +26,23 @@ module memory_slave_model
     input wire [ADR_BITS - 1 : 0]     ADR_I,
     input wire [8*WORD_SIZE - 1 : 0]  DAT_I,
     output wire [8*WORD_SIZE - 1 : 0] DAT_O,
-    /* input wire [GRANULARITY - 1 : 0] SEL_I, */ /* we'll use that soon */
+    input wire [SEL_LINES - 1 : 0]    SEL_I,
     input wire 			      RST_I,
     input wire 			      STB_I,
     input wire 			      WE_I,
     output wire 		      STALL_O
     );
 
+   parameter GRANULARITY = 8 * WORD_SIZE / SEL_LINES; /* in bits */
+
    /*
     * A simple memory slave should be most useful for testing purposes;
     * WARNING! The 'memory' variable might be referenced from outside the module
     * by testbench code - be careful when changing or removing it
     */
-   reg [8*WORD_SIZE - 1 : 0] 	      memory [2**ADR_BITS - 1 : 0];
+   reg [GRANULARITY - 1 : 0] 	      memory [2**ADR_BITS - 1 : 0];
 
-    /* For random stall times and acknowledge times */
+   /* For random stall times and acknowledge times */
    integer 			      seed;
 
    parameter MAX_STALL_TIME = 7;
@@ -53,6 +55,7 @@ module memory_slave_model
    reg 				      WE_I_pipeline [15:0];
    reg [ADR_BITS - 1 : 0] 	      ADR_I_pipeline [15:0];
    reg [8*WORD_SIZE - 1 : 0] 	      DAT_I_pipeline [15:0];
+   reg [SEL_LINES - 1 : 0] 	      SEL_I_pipeline [15:0];
 
    reg [3:0] 			      pipeline_oldest_element;
    reg [4:0] 			      pipeline_elements_count;
@@ -65,16 +68,19 @@ module memory_slave_model
    reg 				      WE_I_to_process;
    reg [ADR_BITS - 1 : 0] 	      ADR_I_to_process;
    reg [8*WORD_SIZE - 1 : 0] 	      DAT_I_to_process;
+   reg [SEL_LINES - 1 : 0] 	      SEL_I_to_process;
 
    always @* begin
       if (pipeline_elements_count) begin
 	 WE_I_to_process = WE_I_pipeline[pipeline_oldest_element];
 	 ADR_I_to_process = ADR_I_pipeline[pipeline_oldest_element];
 	 DAT_I_to_process = DAT_I_pipeline[pipeline_oldest_element];
+	 SEL_I_to_process = SEL_I_pipeline[pipeline_oldest_element];
       end else begin
 	 WE_I_to_process = WE_I;
 	 ADR_I_to_process = ADR_I;
 	 DAT_I_to_process = DAT_I;
+	 SEL_I_to_process = SEL_I;
       end
    end // always @ *
 
@@ -94,11 +100,27 @@ module memory_slave_model
 				 command_available_for_acknowledging;
 
 
+   integer                   i; /* loop counter */
+   reg [8*WORD_SIZE - 1 : 0] masked_out_data;
+
+   always @ (SEL_I_to_process or WE_I_to_process or ADR_I_to_process) begin
+      for (i = SEL_LINES - 1; i >= 0; i--) begin
+	 if (SEL_LINES > 1) begin
+	    masked_out_data[8*WORD_SIZE - 1 : GRANULARITY]
+	      = masked_out_data[8*WORD_SIZE - GRANULARITY - 1 : 0];
+	 end
+
+	 masked_out_data[GRANULARITY - 1 : 0]
+	   = SEL_I_to_process[i] ?
+	     memory[ADR_I_to_process + i] : {GRANULARITY{1'bx}};
+      end
+   end // always @ (SEL_I_to_process or WE_I_to_process or ADR_I_to_process)
+
    /* Finally, drive the outputs */
 
    /* only drive data outputs for read commands */
    assign DAT_O = (command_acknowledged && !WE_I_to_process) ?
-		  memory[ADR_I_to_process] : {(8 * WORD_SIZE - 1){1'bx}};
+		  masked_out_data : {(8 * WORD_SIZE - 1){1'bx}};
 
    assign STALL_O = !can_accept;
 
@@ -115,6 +137,9 @@ module memory_slave_model
 	$readmemb(INITIAL_CONTENTS_FILE, memory, 0, WORDS_TO_INITIALIZE - 1);
    end
 
+   integer                   k; /* loop counter */
+   reg [8*WORD_SIZE - 1 : 0] masked_in_data;
+
    always @ (posedge CLK_I) begin
       if (RST_I) begin
 	 pipeline_oldest_element <= 0;
@@ -125,6 +150,7 @@ module memory_slave_model
 	    WE_I_pipeline[pipeline_index_to_insert] <= WE_I;
 	    ADR_I_pipeline[pipeline_index_to_insert] <= ADR_I;
 	    DAT_I_pipeline[pipeline_index_to_insert] <= DAT_I;
+	    SEL_I_pipeline[pipeline_index_to_insert] <= SEL_I;
 	 end else begin
 	    if (stall_time_left)
 	      stall_time_left <= stall_time_left - 1;
@@ -135,18 +161,36 @@ module memory_slave_model
 	    pipeline_oldest_element <= pipeline_oldest_element + 1;
 
 	    if (WE_I_to_process) begin /* Write command */
+	       masked_in_data = DAT_I_to_process;
+
+	       for (k = 0; k < SEL_LINES; k++) begin
+		  if (SEL_I_to_process[k]) begin
+		     if (WRITABLE) begin
+			memory[ADR_I_to_process + k]
+			  <= masked_in_data[GRANULARITY - 1 : 0];
+		     end
+		  end else begin
+		     masked_in_data[GRANULARITY - 1 : 0] = {GRANULARITY{1'bx}};
+		  end
+
+		  if (SEL_LINES > 1) begin
+		     masked_in_data
+		       = {masked_in_data[GRANULARITY - 1 : 0],
+			  masked_in_data[8*WORD_SIZE - 1 : GRANULARITY]};
+		  end
+	       end // for (k = 0; k < SEL_LINES; k++)
+
 	       if (WRITABLE) begin
-		  memory[ADR_I_to_process] <= DAT_I_to_process;
-		  `DBG(("Slave %0d: write of h%x at h%x", SLAVE_NR,
-			DAT_I_to_process, ADR_I_to_process));
+		  `DBG(("Slave %0d: write of h%x (%b) at h%x", SLAVE_NR,
+			masked_in_data, SEL_I_to_process, ADR_I_to_process));
 	       end else begin
-		  `DBG(({"Slave %0d: error: write of h%x at h%x ",
+		  `DBG(({"Slave %0d: error: write of h%x (%b) at h%x ",
 			 "(read-only) memory"}, SLAVE_NR,
-			DAT_I_to_process, ADR_I_to_process));
+			DAT_I_to_process, SEL_I_to_process, ADR_I_to_process));
 	       end
 	    end else begin /* Read command */
-	       `DBG(("Slave %0d: read of h%x at h%x", SLAVE_NR,
-		     DAT_O, ADR_I_to_process));
+	       `DBG(("Slave %0d: read of h%x (%b) at h%x", SLAVE_NR,
+		     DAT_O, SEL_I_to_process, ADR_I_to_process));
 	    end
 	 end else if (command_available_for_acknowledging) begin
 	    acknowledge_time_left <= acknowledge_time_left - 1;