pad out packets to a minimum length

2 files changed, 76 insertions, 1 deletions

diff --git a/usrp2/control_lib/fifo_to_wb_tb.v b/usrp2/control_lib/fifo_to_wb_tb.v
index e02e60b7c..f1538e8d9 100644
--- a/usrp2/control_lib/fifo_to_wb_tb.v
+++ b/usrp2/control_lib/fifo_to_wb_tb.v
@@ -17,11 +17,14 @@ module fifo_to_wb_tb();
    wire        cmd_dst_rdy, resp_src_rdy, resp_dst_rdy;
    reg [17:0]  cmd;
    wire [17:0] resp;
+
+   wire [17:0] resp_int;
+   wire        resp_src_rdy_int, resp_dst_rdy_int;
    
    fifo_to_wb fifo_to_wb
      (.clk(clk), .reset(rst), .clear(clear),
       .data_i(cmd), .src_rdy_i(cmd_src_rdy), .dst_rdy_o(cmd_dst_rdy),
-      .data_o(resp), .src_rdy_o(resp_src_rdy), .dst_rdy_i(resp_dst_rdy),
+      .data_o(resp_int), .src_rdy_o(resp_src_rdy_int), .dst_rdy_i(resp_dst_rdy_int),
 
       .wb_adr_o(wb_adr), .wb_dat_mosi(wb_dat_mosi), .wb_dat_miso(wb_dat_miso),
       .wb_sel_o(), .wb_cyc_o(wb_cyc), .wb_stb_o(wb_stb), 
@@ -29,6 +32,12 @@ module fifo_to_wb_tb();
       .triggers());
 
    assign wb_dat_miso = {wb_adr[7:0],8'hBF};
+
+   fifo19_pad #(.LENGTH(16)) fifo19_pad
+     (.clk(clk), .reset(rst), .clear(clear),
+      .data_i(resp_int), .src_rdy_i(resp_src_rdy_int), .dst_rdy_o(resp_dst_rdy_int),
+      .data_o(resp), .src_rdy_o(resp_src_rdy), .dst_rdy_i(resp_dst_rdy));
+        
    
    // Set up monitors
    always @(posedge clk)
diff --git a/usrp2/fifo/fifo19_pad.v b/usrp2/fifo/fifo19_pad.v
new file mode 100644
index 000000000..1a40c1631
--- /dev/null
+++ b/usrp2/fifo/fifo19_pad.v
@@ -0,0 +1,66 @@
+
+// Pads a packet out to the minimum length
+//  Packets already longer than min length are unchanged
+
+
+module fifo19_pad
+  #(parameter LENGTH=16,
+    parameter PAD_VALUE=0)
+   (input clk, input reset, input clear,
+    input [18:0] data_i,
+    input src_rdy_i,
+    output dst_rdy_o,
+    output [18:0] data_o,
+    output src_rdy_o,
+    input dst_rdy_i);
+
+   reg [15:0] count;
+   reg [1:0]  pad_state;
+   localparam PAD_IDLE = 0;
+   localparam PAD_TOOSHORT = 1;
+   localparam PAD_LONGENOUGH = 2;
+   localparam PAD_PADDING = 3;
+
+   always @(posedge clk)
+     if(reset | clear)
+       pad_state <= PAD_IDLE;
+     else
+       case(pad_state)
+	 PAD_IDLE :
+	   begin
+	      count <= 1;
+	      pad_state <= PAD_TOOSHORT;
+	   end
+	 PAD_TOOSHORT :
+	   if(src_rdy_i & dst_rdy_i)
+	     begin
+		count <= count + 1;
+		if(data_i[17])
+		  pad_state <= PAD_PADDING;
+		else if(count == (LENGTH-1))
+		  pad_state <= PAD_LONGENOUGH;
+	     end
+	 PAD_PADDING :
+	   if(dst_rdy_i)
+	     begin
+		count <= count + 1;
+		if(count == LENGTH)
+		  pad_state <= PAD_IDLE;
+	     end
+	 PAD_LONGENOUGH :
+	   if(src_rdy_i & dst_rdy_i & data_i[17])
+	     pad_state <= PAD_IDLE;
+       endcase // case (pad_state)
+   
+   wire passthru = (pad_state == PAD_TOOSHORT) | (pad_state == PAD_LONGENOUGH);
+   
+   assign dst_rdy_o = passthru ? dst_rdy_i : 1'b0;
+   assign src_rdy_o = passthru ? src_rdy_i : (pad_state == PAD_PADDING);
+   
+   assign data_o[15:0] = (pad_state == PAD_PADDING) ? PAD_VALUE : data_i[15:0];
+   assign data_o[16] = (count == 1);
+   assign data_o[17] = (pad_state == PAD_LONGENOUGH) ? data_i[17] : (count == LENGTH);
+   assign data_o[18] = (pad_state == PAD_LONGENOUGH) ? data_i[18] : 1'b0;
+   
+         
+endmodule // fifo19_pad