Merged features/inband-usb r5224:6306 into trunk. This is work-in-progress on inband signaling for the USRP1.

git-svn-id: http://gnuradio.org/svn/gnuradio/trunk@6307 221aa14e-8319-0410-a670-987f0aec2ac5

13 files changed, 1629 insertions, 649 deletions

diff --git a/inband_lib/chan_fifo_reader.v b/inband_lib/chan_fifo_reader.v
index 2b3178da7..9392bf151 100755
--- a/inband_lib/chan_fifo_reader.v
+++ b/inband_lib/chan_fifo_reader.v
@@ -1,197 +1,219 @@
 module chan_fifo_reader 
-  ( input       reset,
-    input       tx_clock,
-    input       tx_strobe,
-    input       [31:0]adc_clock,
-    input       [3:0] samples_format,
-    input       [15:0] fifodata,
-    input       pkt_waiting,
-    output  reg rdreq,
-    output  reg skip,
-    output  reg [15:0]tx_q,
-    output  reg [15:0]tx_i,
-    output  reg overrun,
-    output  reg underrun) ;
-    
+  ( reset, tx_clock, tx_strobe, adc_time, samples_format,
+    fifodata, pkt_waiting, rdreq, skip, tx_q, tx_i,
+    underrun, tx_empty, debug, rssi, threshhold) ;
+
+    input   wire                     reset ;
+    input   wire                     tx_clock ;
+    input   wire                     tx_strobe ; //signal to output tx_i and tx_q
+    input   wire              [31:0] adc_time ; //current time
+    input   wire               [3:0] samples_format ;// not useful at this point
+    input   wire              [31:0] fifodata ; //the data input
+    input   wire                     pkt_waiting ; //signal the next packet is ready
+    output  reg                      rdreq ; //actually an ack to the current fifodata
+    output  reg                      skip ; //finish reading current packet
+    output  reg               [15:0] tx_q ; //top 16 bit output of fifodata
+    output  reg               [15:0] tx_i ; //bottom 16 bit output of fifodata
+    output  reg                      underrun ; 
+    output  reg                      tx_empty ; //cause 0 to be the output
+    input 	wire			  [31:0] rssi;
+    input	wire			  [31:0] threshhold;
+
+	output wire [14:0] debug;
+	assign debug = {reader_state, trash, skip, timestamp[4:0], adc_time[4:0]};
     // Should not be needed if adc clock rate < tx clock rate
-    `define JITTER        5
+    // Used only to debug
+    `define JITTER                   5
     
     //Samples format
     // 16 bits interleaved complex samples
-    `define QI16         4'b0
+    `define QI16                     4'b0
     
     // States
-   `define IDLE          4'd0
-   `define READ          4'd1
-   `define HEADER1       4'd2
-   `define HEADER2       4'd3
-   `define TIMESTAMP1    4'd4
-   `define TIMESTAMP2    4'd5
-   `define WAIT          4'd6
-   `define WAITSTROBE    4'd7
-   `define SENDWAIT      4'd8
-   `define SEND          4'd9
-   `define FEED          4'd10
-   `define DISCARD       4'd11
+    parameter IDLE           =     3'd0;    
+	parameter HEADER         =     3'd1;
+    parameter TIMESTAMP      =     3'd2;
+    parameter WAIT           =     3'd3;
+    parameter WAITSTROBE     =     3'd4;
+    parameter SEND           =     3'd5;
 
-   // State registers
-   reg[3:0] reader_state;
-   reg[3:0] reader_next_state;
+    // Header format
+    `define PAYLOAD                  8:2
+    `define ENDOFBURST               27
+    `define STARTOFBURST            28
+    `define RSSI_FLAG				 15
+	
+
+    /* State registers */
+    reg                        [2:0] reader_state;
+  
+    reg                        [6:0] payload_len;
+    reg                        [6:0] read_len;
+    reg                       [31:0] timestamp;
+    reg                              burst;
+	reg								 trash;
+	reg								 rssi_flag;
    
-   //Variables
-   reg[8:0] payload_len;
-   reg[8:0] read_len;
-   reg[31:0] timestamp;
-   reg burst;
-   reg qsample;
-   always @(posedge tx_clock)
-   begin
-       if (reset) 
+    always @(posedge tx_clock)
+    begin
+        if (reset) 
           begin
-             reader_state <= `IDLE;
-             reader_next_state <= `IDLE;
-             rdreq <= 0;
-             skip <= 0;
-             overrun <= 0;
-             underrun <= 0;
-             burst <= 0;
-             qsample <= 1;
-          end
+            reader_state <= IDLE;
+            rdreq <= 0;
+            skip <= 0;
+            underrun <= 0;
+            burst <= 0;
+            tx_empty <= 1;
+            tx_q <= 0;
+            tx_i <= 0;
+			trash <= 0;
+			rssi_flag <= 0;
+         end
        else 
-		 begin
-           reader_state = reader_next_state;
+		   begin
            case (reader_state)
-               `IDLE:
-                  begin
-                     if (pkt_waiting == 1)
-                       begin
-                          reader_next_state <= `READ;
-                          rdreq <= 1;
-                          underrun <= 0;
-                       end
-                     else if (burst == 1)
+               IDLE:
+               begin
+				/*
+				 * reset all the variables and wait for a tx_strobe
+				 * it is assumed that the ram connected to this fifo_reader 
+				 * is a short hand fifo meaning that the header to the next packet
+				 * is already available to this fifo_reader when pkt_waiting is on
+				 */
+                   skip <=0;
+                   if (pkt_waiting == 1)
+                     begin
+                        reader_state <= HEADER;
+                        rdreq <= 1;
+                        underrun <= 0;
+                     end
+                   else if (burst == 1)
                         underrun <= 1;
-                  end
-
-				// Just wait for the fifo data to arrive
-               `READ: 
-                  begin
-                     reader_next_state <= `HEADER1;
-                  end
-				
-				// First part of the header
-               `HEADER1:
-                  begin
-                     reader_next_state <= `HEADER2;
-                     
-                     //Check Start burst flag
-                     if (fifodata[3] == 1)
-                        burst <= 1;
                         
-                     if (fifodata[4] == 1)
-                        burst <= 0;
-                  end
+                   if (tx_strobe == 1)
+                       tx_empty <= 1 ;
+               end
 
-				// Read payload length
-               `HEADER2:
-                  begin
-                     payload_len <= (fifodata & 16'h1FF);
-                     read_len <= 9'd0;
-                     reader_next_state <= `TIMESTAMP1;
-                  end
+				   /* Process header */
+               HEADER:
+               begin
+                   if (tx_strobe == 1)
+                       tx_empty <= 1 ;
+                   
+                   rssi_flag <= fifodata[`RSSI_FLAG]&fifodata[`STARTOFBURST];
+                   //Check Start/End burst flag
+                   if  (fifodata[`STARTOFBURST] == 1 
+                       && fifodata[`ENDOFBURST] == 1)
+                       burst <= 0;
+                   else if (fifodata[`STARTOFBURST] == 1)
+                       burst <= 1;
+                   else if (fifodata[`ENDOFBURST] == 1)
+                       burst <= 0;
 
-               `TIMESTAMP1: 
-                  begin
-                     timestamp <= {fifodata, 16'b0};
-                     rdreq <= 0;
-                     reader_next_state <= `TIMESTAMP2;
-                  end
-				
-               `TIMESTAMP2:
-                  begin
-                     timestamp <= timestamp + fifodata;
-                     reader_next_state <= `WAIT;
-                  end
+					if (trash == 1 && fifodata[`STARTOFBURST] == 0)
+					begin
+						skip <= 1;
+						reader_state <= IDLE;
+						rdreq <= 0;
+					end 
+                    else
+					begin   
+                   		payload_len <= fifodata[`PAYLOAD] ;
+                   		read_len <= 0;
+                        rdreq <= 1;
+						reader_state <= TIMESTAMP;
+					end
+               end
+
+               TIMESTAMP: 
+               begin
+                   timestamp <= fifodata;
+                   reader_state <= WAIT;
+                   if (tx_strobe == 1)
+                       tx_empty <= 1 ;
+                   rdreq <= 0;
+               end
 				
-				// Decide if we wait, send or discard samples
-               `WAIT: 
-                  begin
-                   // Wait a little bit more
-                     if (timestamp > adc_clock + `JITTER)
-                        reader_next_state <= `WAIT;
+				   // Decide if we wait, send or discard samples
+               WAIT: 
+               begin
+                   if (tx_strobe == 1)
+                       tx_empty <= 1 ;
+                          
                    // Let's send it
-                   else if ((timestamp < adc_clock + `JITTER 
-                           && timestamp > adc_clock)
-                           || timestamp == 32'hFFFFFFFF)
-                      begin
-                         reader_next_state <= `WAITSTROBE;
-                      end
+                   if ((timestamp <= adc_time + `JITTER 
+                             && timestamp > adc_time)
+                             || timestamp == 32'hFFFFFFFF)
+					begin
+						if (rssi <= threshhold || rssi_flag == 0)
+						  begin
+						    trash <= 0;
+                            reader_state <= WAITSTROBE; 
+                          end
+						else
+						    reader_state <= WAIT;
+					end
+                   // Wait a little bit more
+                   else if (timestamp > adc_time + `JITTER)
+                       reader_state <= WAIT; 
                    // Outdated
-                   else if (timestamp < adc_clock)
-                      begin
-                         reader_next_state <= `DISCARD;
-                         skip <= 1;
+                   else if (timestamp < adc_time)
+                     begin
+						trash <= 1;
+                        reader_state <= IDLE;
+                        skip <= 1;
                      end
-                 end
+               end
                  
-            // Wait for the transmit chain to be ready
-               `WAITSTROBE:
-                  begin
-                      // If end of payload...
-                     if (read_len == payload_len)
+               // Wait for the transmit chain to be ready
+               WAITSTROBE:
+               begin
+                   // If end of payload...
+                   if (read_len == payload_len)
+                     begin
+                       reader_state <= IDLE;
+                       skip <= 1;
+                       if (tx_strobe == 1)
+                           tx_empty <= 1 ;
+                     end  
+                   else if (tx_strobe == 1)
+                     begin
+                       reader_state <= SEND;
+                       rdreq <= 1;
+                     end
+               end
+               
+				   // Send the samples to the tx_chain
+               SEND:
+               begin
+                   reader_state <= WAITSTROBE; 
+                   read_len <= read_len + 7'd1;
+                   tx_empty <= 0;
+                   rdreq <= 0;
+                   
+                   case(samples_format)
+                       `QI16:
                         begin
-                           reader_next_state <= `DISCARD;
-                           skip <= (payload_len < 508);
+                            tx_i <= fifodata[15:0];
+                            tx_q <= fifodata[31:16];
                         end
-                          
-                      if (tx_strobe == 1)
-                         reader_next_state <= `SENDWAIT;
-                  end
-               
-               `SENDWAIT:
-                  begin
-                     rdreq <= 1;
-                     reader_next_state <= `SEND; 
-                  end
-               
-				// Send the samples to the tx_chain
-               `SEND:
-                  begin
-                     reader_next_state <= `WAITSTROBE; 
-                     rdreq <= 0;
-                     read_len <= read_len + 2;
-                     case(samples_format)
-                        `QI16:
-                           begin
-                              tx_q <= qsample ? fifodata : 16'bZ;
-                              tx_i <= ~qsample ? fifodata : 16'bZ;
-                              qsample <= ~ qsample;
-                           end  
+                        
+                        // Assume 16 bits complex samples by default
                         default:
-                           begin
-                               // Assume 16 bits complex samples by default
-                              $display ("Error unknown samples format");
-                              tx_q <= qsample ? fifodata : 16'bZ;
-                              tx_i <= ~qsample ? fifodata : 16'bZ;
-                              qsample <= ~ qsample;
-                           end 
-                     endcase
-                  end
-
-               `DISCARD:
-                  begin
-                     skip <= 0;
-                     reader_next_state <= `IDLE;
-                  end
+                        begin
+                            tx_i <= fifodata[15:0];
+                            tx_q <= fifodata[31:16];
+                        end 
+                   endcase
+               end
                
                default:
-                  begin
-                     $display ("Error unknown state");
-                     reader_state <= `IDLE;
-                     reader_next_state <= `IDLE;
-                  end
+               begin
+					//error handling
+                   reader_state <= IDLE;
+               end
            endcase
        end
    end
  
-endmodule
\ No newline at end of file
+endmodule
diff --git a/inband_lib/channel_demux.v b/inband_lib/channel_demux.v
new file mode 100644
index 000000000..d46be9397
--- /dev/null
+++ b/inband_lib/channel_demux.v
@@ -0,0 +1,78 @@
+module channel_demux
+ #(parameter NUM_CHAN = 2, parameter CHAN_WIDTH = 2) (     //usb Side
+			input [31:0]usbdata_final,
+			input WR_final, 
+			
+			// TX Side
+			input reset,
+			input txclk,
+			output reg [CHAN_WIDTH:0] WR_channel,
+			output reg [31:0] ram_data,
+			output reg [CHAN_WIDTH:0] WR_done_channel );
+/* Parse header and forward to ram */
+	reg [2:0]reader_state;
+	reg [4:0]channel ;
+	reg [6:0]read_length ;
+	
+	 // States
+    parameter IDLE		=	3'd0;
+    parameter HEADER	=	3'd1;
+    parameter WAIT		=	3'd2;
+    parameter FORWARD	=	3'd3;
+	
+	`define CHANNEL 20:16
+	`define PKT_SIZE 127
+	wire [4:0] true_channel;
+	assign true_channel = (usbdata_final[`CHANNEL] == 5'h1f) ?
+							NUM_CHAN : (usbdata_final[`CHANNEL]);
+	
+	always @(posedge txclk)
+	begin
+	    if (reset)
+	      begin
+	       reader_state <= IDLE;
+	       WR_channel <= 0;
+	       WR_done_channel <= 0;
+	      end
+	      else
+	        case (reader_state)
+	        IDLE: begin
+	            if (WR_final)
+	                reader_state <= HEADER; 
+	            end
+	       
+            // Store channel and forware header
+	        HEADER: begin
+	            channel <= true_channel;
+	            WR_channel[true_channel] <= 1;
+	            ram_data <= usbdata_final;
+				read_length <= 7'd0 ;
+				
+                reader_state <= WAIT;
+	        end
+	       
+	        WAIT: begin
+	           WR_channel[channel] <= 0;
+	
+			   if (read_length == `PKT_SIZE)
+	               reader_state <= IDLE;
+	           else if (WR_final)
+	               reader_state <= FORWARD;
+	        end
+	       
+	        FORWARD: begin
+	           WR_channel[channel] <= 1;
+	           ram_data <= usbdata_final;
+	           read_length <= read_length + 7'd1;
+	           
+	           reader_state <= WAIT;
+	        end
+	
+			default:
+               begin
+					//error handling
+                   reader_state <= IDLE;
+               end
+	       endcase
+	   end
+endmodule
diff --git a/inband_lib/channel_ram.v b/inband_lib/channel_ram.v
new file mode 100755
index 000000000..40e0efc01
--- /dev/null
+++ b/inband_lib/channel_ram.v
@@ -0,0 +1,114 @@
+module channel_ram 
+	( // System
+	input txclk,
+	input reset,
+	
+	// USB side
+	input [31:0] datain, 
+	input WR, 
+	input WR_done,
+	output have_space,
+
+	// Reader side
+	output [31:0] dataout,
+	input RD,
+	input RD_done,
+	output packet_waiting);
+	
+	reg [6:0] wr_addr, rd_addr;
+	reg [1:0] which_ram_wr, which_ram_rd;
+	reg [2:0] nb_packets;
+	
+	reg [31:0] ram0 [0:127];
+	reg [31:0] ram1 [0:127];
+	reg [31:0] ram2 [0:127];
+	reg [31:0] ram3 [0:127];
+	
+	reg [31:0] dataout0;
+	reg [31:0] dataout1;
+	reg [31:0] dataout2;
+	reg [31:0] dataout3;
+	
+	wire wr_done_int;
+	wire rd_done_int;
+   	wire [6:0] rd_addr_final;
+	wire [1:0] which_ram_rd_final;
+	
+	// USB side
+	always @(posedge txclk)
+		if(WR & (which_ram_wr == 2'd0)) ram0[wr_addr] <= datain;
+			
+	always @(posedge txclk)
+		if(WR & (which_ram_wr == 2'd1)) ram1[wr_addr] <= datain;
+
+	always @(posedge txclk)
+		if(WR & (which_ram_wr == 2'd2)) ram2[wr_addr] <= datain;
+
+	always @(posedge txclk)
+		if(WR & (which_ram_wr == 2'd3)) ram3[wr_addr] <= datain;
+
+   assign wr_done_int = ((WR && (wr_addr == 7'd127)) || WR_done);
+   
+	always @(posedge txclk)
+		if(reset)
+			wr_addr <= 0;
+		else if (WR_done)
+			wr_addr <= 0;
+		else if (WR) 
+			wr_addr <= wr_addr + 7'd1;
+		
+	always @(posedge txclk)
+		if(reset)
+			which_ram_wr <= 0;
+		else if (wr_done_int) 
+			which_ram_wr <= which_ram_wr + 2'd1;
+	
+	assign have_space = (nb_packets < 3'd3);
+		
+	// Reader side
+	// short hand fifo
+	// rd_addr_final is what rd_addr is going to be next clock cycle
+	// which_ram_rd_final is what which_ram_rd is going to be next clock cycle
+	always @(posedge txclk)  dataout0 <= ram0[rd_addr_final];
+	always @(posedge txclk)  dataout1 <= ram1[rd_addr_final];
+	always @(posedge txclk)  dataout2 <= ram2[rd_addr_final];
+	always @(posedge txclk)  dataout3 <= ram3[rd_addr_final];
+	
+	assign dataout = (which_ram_rd_final[1]) ? 
+						(which_ram_rd_final[0] ? dataout3 : dataout2) :
+						(which_ram_rd_final[0] ? dataout1 : dataout0);
+
+	//RD_done is the only way to signal the end of one packet
+	assign rd_done_int = RD_done;   
+
+	always @(posedge txclk)
+		if (reset)
+			rd_addr <= 0;
+		else if (RD_done)
+			rd_addr <= 0;
+		else if (RD) rd_addr <= rd_addr + 7'd1;
+			
+	assign rd_addr_final = (reset|RD_done) ? (6'd0) : 
+	                        ((RD)?(rd_addr+7'd1):rd_addr); 
+	always @(posedge txclk)
+	   if (reset)
+			which_ram_rd <= 0;
+		else if (rd_done_int)
+			which_ram_rd <= which_ram_rd + 2'd1;
+
+	assign which_ram_rd_final = (reset) ? (2'd0):
+	                       ((rd_done_int) ? (which_ram_rd + 2'd1) : which_ram_rd);
+	                        
+	//packet_waiting is set to zero if rd_done_int is high
+	//because there is no guarantee that nb_packets will be pos.
+	assign packet_waiting = (nb_packets != 0) & (~rd_done_int);
+	
+	always @(posedge txclk)
+		if (reset)
+			nb_packets <= 0;
+		else if (wr_done_int & ~rd_done_int)
+			nb_packets <= nb_packets + 3'd1;
+		else if (rd_done_int & ~wr_done_int)
+			nb_packets <= nb_packets - 3'd1;
+			
+endmodule
\ No newline at end of file
diff --git a/inband_lib/cmd_reader.v b/inband_lib/cmd_reader.v
new file mode 100755
index 000000000..7604321e4
--- /dev/null
+++ b/inband_lib/cmd_reader.v
@@ -0,0 +1,292 @@
+module cmd_reader(
+		//System
+		input reset,
+		input txclk,
+		input [31:0] adc_time,
+		//FX2 Side
+		output reg skip,
+		output reg rdreq,
+		input [31:0] fifodata,
+		input pkt_waiting,
+		//Rx side
+		input rx_WR_enabled,
+		output reg [15:0] rx_databus,
+		output reg rx_WR,
+		output reg rx_WR_done,
+		//register io
+		input wire [31:0] reg_data_out,
+		output reg [31:0] reg_data_in,
+		output reg [6:0] reg_addr,
+		output reg [1:0] reg_io_enable,
+		output wire [14:0] debug		
+	);
+	
+	// States
+    parameter IDLE				=	4'd0;
+	parameter HEADER			=  	4'd1;
+	parameter TIMESTAMP			=	4'd2;
+    parameter WAIT          	=   4'd3;
+	parameter TEST				=	4'd4;
+	parameter SEND				=	4'd5;
+	parameter PING				=	4'd6;
+	parameter WRITE_REG			=	4'd7;
+	parameter WRITE_REG_MASKED	= 	4'd8;
+	parameter READ_REG			=   4'd9;
+	parameter DELAY				=	4'd14;		
+
+	`define OP_PING_FIXED				8'd0
+	`define OP_PING_FIXED_REPLY			8'd1
+	`define OP_WRITE_REG				8'd2
+	`define OP_WRITE_REG_MASKED			8'd3
+	`define OP_READ_REG					8'd4
+	`define OP_READ_REG_REPLY			8'd5
+	`define OP_DELAY					8'd12
+	
+	reg [6:0] 	payload;
+	reg [6:0] 	payload_read;
+	reg [3:0] 	state;
+	reg [15:0]  high;
+	reg [15:0]	low;
+	reg			pending;
+	reg [31:0]  value0;
+	reg [31:0]	value1;
+	reg [31:0]	value2;
+	reg [1:0]   lines_in;
+	reg [1:0]	lines_out;
+	reg [1:0] 	lines_out_total;
+	
+	`define JITTER                      5
+	`define OP_CODE						31:24
+	`define PAYLOAD   					8:2
+	
+	wire [7:0] ops;
+	assign ops = value0[`OP_CODE];
+	assign debug = {state[3:0], lines_out[1:0], pending, rx_WR, rx_WR_enabled, value0[2:0], ops[2:0]};
+	
+	always @(posedge txclk)
+		if (reset)
+		  begin
+			pending <= 0;
+		    state <= IDLE;
+			skip <= 0;
+			rdreq <= 0;
+			rx_WR <= 0;
+			reg_io_enable <= 0;
+			reg_data_in <= 0;
+			reg_addr <= 0;
+		  end
+		else case (state)
+			IDLE : begin
+				payload_read <= 0;
+				skip <= 0;
+				lines_in <= 0;
+				if (pkt_waiting)
+				  begin
+					state <= HEADER;
+					rdreq <= 1;
+				  end
+			end
+			
+			HEADER : begin
+				payload <= fifodata[`PAYLOAD];
+				state <= TIMESTAMP;
+			end
+			
+			TIMESTAMP : begin
+				value0 <= fifodata;
+				state <= WAIT;
+				rdreq <= 0;
+			end
+			
+			WAIT : begin
+  					// Let's send it
+                   if ((value0 <= adc_time + `JITTER 
+                             && value0 > adc_time)
+                             || value0 == 32'hFFFFFFFF)
+                       state <= TEST;
+                   // Wait a little bit more
+                   else if (value0 > adc_time + `JITTER)
+                       state <= WAIT; 
+                   // Outdated
+                   else if (value0 < adc_time)
+                     begin
+                        state <= IDLE;
+                        skip <= 1;
+                     end
+			end
+			
+			TEST : begin
+				reg_io_enable <= 0;
+				rx_WR <= 0;
+				rx_WR_done <= 1;
+				if (payload_read == payload)
+					begin
+						skip <= 1;
+						state <= IDLE;
+						rdreq <= 0;
+					end
+				else
+					begin
+						value0 <= fifodata;
+						lines_in <= 2'd1;
+						rdreq <= 1;
+						payload_read <= payload_read + 7'd1;
+						lines_out <= 0;
+						case (fifodata[`OP_CODE])
+							`OP_PING_FIXED: begin
+								state <= PING;
+							end
+							`OP_WRITE_REG: begin
+								state <= WRITE_REG;
+								pending <= 1;
+							end
+							`OP_WRITE_REG_MASKED: begin
+								state <= WRITE_REG_MASKED;
+								pending <= 1;
+							end
+							`OP_READ_REG: begin
+								state <= READ_REG;
+							end
+							`OP_DELAY: begin
+								state <= DELAY;
+							end
+							default: begin
+							//error, skip this packet
+								skip <= 1;
+								state <= IDLE;
+							end
+						endcase
+					end
+			end
+			
+			SEND: begin
+				rdreq <= 0;
+				rx_WR_done <= 0;
+				if (pending)
+					begin
+						rx_WR <= 1;
+						rx_databus <= high;
+						pending <= 0;
+						if (lines_out == lines_out_total)
+							state <= TEST;
+						else case (ops)
+							`OP_READ_REG: begin
+								state <= READ_REG;
+							end
+							default: begin
+								state <= TEST;
+							end
+						endcase
+					end
+				else
+					begin
+						if (rx_WR_enabled)
+						begin
+							rx_WR <= 1;
+							rx_databus <= low;
+							pending <= 1;
+							lines_out <= lines_out + 2'd1;
+						end
+						else
+							rx_WR <= 0;
+					end
+			end
+			
+			PING: begin
+				rx_WR <= 0;
+				rdreq <= 0;
+				rx_WR_done <= 0;
+				lines_out_total <= 2'd1;
+				pending <= 0;
+				state <= SEND;
+				high <= {`OP_PING_FIXED_REPLY, 8'd2};
+				low <= value0[15:0];	
+			end
+			
+			READ_REG: begin
+				rx_WR <= 0;
+				rx_WR_done <= 0;
+				rdreq <= 0;
+				lines_out_total <= 2'd2;
+				pending <= 0;
+				state <= SEND;
+				if (lines_out == 0)
+					begin
+						high <= {`OP_READ_REG_REPLY, 8'd6};
+						low <= value0[15:0];
+						reg_io_enable <= 2'd3;
+						reg_addr <= value0[6:0];
+					end
+				else
+					begin		
+						high <= reg_data_out[31:16];
+						low <= reg_data_out[15:0];
+					end
+			end
+			
+			WRITE_REG: begin
+				rx_WR <= 0;
+				if (pending)
+					pending <= 0;
+				else
+					begin
+						if (lines_in == 2'd1)
+						begin
+							payload_read <= payload_read + 7'd1;
+							lines_in <= lines_in + 2'd1;
+							value1 <= fifodata;
+							rdreq <= 0;
+						end
+						else
+						begin
+							reg_io_enable <= 2'd2;
+							reg_data_in <= value1;
+							reg_addr <= value0[6:0];
+							state <= TEST;
+						end
+					end
+			end
+			
+			WRITE_REG_MASKED: begin
+				rx_WR <= 0;
+				if (pending)
+					pending <= 0;
+				else
+					begin
+						if (lines_in == 2'd1)
+						begin
+							rdreq <= 1;
+							payload_read <= payload_read + 7'd1;
+							lines_in <= lines_in + 2'd1;
+							value1 <= fifodata;
+						end
+						else if (lines_in == 2'd2)
+						begin
+							rdreq <= 0;
+							payload_read <= payload_read + 7'd1;
+							lines_in <= lines_in + 2'd1;
+							value2 <= fifodata;
+						end
+						else
+						begin
+							reg_io_enable <= 2'd2;
+							reg_data_in <= (value1 & value2);
+							reg_addr <= value0[6:0];
+							state <= TEST;
+						end
+					end
+			end
+			
+			DELAY : begin
+				rdreq <= 0;
+				value1 <= value1 + 32'd1;
+				if (value0[15:0] == value1[15:0])
+					state <= TEST;
+			end
+			
+			default : begin
+				//error state handling
+				state <= IDLE;
+			end
+		endcase
+endmodule
\ No newline at end of file
diff --git a/inband_lib/data_packet_fifo.v b/inband_lib/data_packet_fifo.v
index 5b37b14ea..a9bcbdae7 100755
--- a/inband_lib/data_packet_fifo.v
+++ b/inband_lib/data_packet_fifo.v
@@ -1,128 +1,118 @@
 module data_packet_fifo 
   ( input       reset,
     input       clock,
-    input       [15:0]ram_data_in,
+    input       [31:0]ram_data_in,
     input       write_enable,
     output  reg have_space,
-    output  reg [15:0]ram_data_out,
+    output  reg [31:0]ram_data_out,
     output  reg pkt_waiting,
+	output	reg	isfull,
+	output	reg [1:0]usb_ram_packet_out,
+	output	reg [1:0]usb_ram_packet_in,
     input       read_enable,
     input       pkt_complete,
     input       skip_packet) ;
 
     /* Some parameters for usage later on */
-    parameter DATA_WIDTH = 16 ;
+    parameter DATA_WIDTH = 32 ;
+    parameter PKT_DEPTH = 128 ;
     parameter NUM_PACKETS = 4 ;
 
     /* Create the RAM here */
-    reg [DATA_WIDTH-1:0] usb_ram [256*NUM_PACKETS-1:0] ;
+    reg [DATA_WIDTH-1:0] usb_ram [PKT_DEPTH*NUM_PACKETS-1:0] ;
 
     /* Create the address signals */
-    reg [7:0] usb_ram_offset_out ;
-    reg [1:0] usb_ram_packet_out ;
-    reg [7:0] usb_ram_offset_in ;
-    reg [1:0] usb_ram_packet_in ;
+    reg [6:0] usb_ram_offset_out ;
+    //reg [1:0] usb_ram_packet_out ;
+    reg [6:0] usb_ram_offset_in ;
+    //reg [1:0] usb_ram_packet_in ;
 
-    wire [7-2+NUM_PACKETS:0] usb_ram_aout ;
-    wire [7-2+NUM_PACKETS:0] usb_ram_ain ;
-    reg isfull;
+    wire [6-2+NUM_PACKETS:0] usb_ram_aout ;
+    wire [6-2+NUM_PACKETS:0] usb_ram_ain ;
+    //reg isfull;
 
     assign usb_ram_aout = {usb_ram_packet_out, usb_ram_offset_out} ;
     assign usb_ram_ain = {usb_ram_packet_in, usb_ram_offset_in} ;
     
     // Check if there is one full packet to process
-    always @(usb_ram_ain, usb_ram_aout)
+    always @(usb_ram_ain, usb_ram_aout, isfull)
     begin
-        if (reset)
-            pkt_waiting <= 0;
-        else if (usb_ram_ain >= usb_ram_aout)
-            pkt_waiting <= usb_ram_ain - usb_ram_aout >= 256;
+        if (usb_ram_ain == usb_ram_aout)
+            pkt_waiting <= isfull ;
+        else if (usb_ram_ain > usb_ram_aout)
+            pkt_waiting <= (usb_ram_ain - usb_ram_aout) >= PKT_DEPTH;
         else
-            pkt_waiting <= (usb_ram_ain + 10'b1111111111 - usb_ram_aout) >= 256;
+            pkt_waiting <= (usb_ram_ain + 10'b1000000000 - usb_ram_aout) >= PKT_DEPTH;
     end
-
+ 
     // Check if there is room
-    always @(usb_ram_ain, usb_ram_aout)
+    always @(usb_ram_ain, usb_ram_aout, isfull)
     begin
-        if (reset)
-            have_space <= 1;
-        else if (usb_ram_ain == usb_ram_aout)
+        if (usb_ram_ain == usb_ram_aout)
             have_space <= ~isfull;   
         else if (usb_ram_ain > usb_ram_aout)
-            have_space <= (usb_ram_ain - usb_ram_aout) <= 256 * (NUM_PACKETS - 1);
+            have_space <= ((usb_ram_ain - usb_ram_aout) <= PKT_DEPTH * (NUM_PACKETS - 1))? 1'b1 : 1'b0;
         else
-            have_space <= (usb_ram_aout - usb_ram_ain) >= 256;
+            have_space <= (usb_ram_aout - usb_ram_ain) >= PKT_DEPTH;
     end
 
-    /* RAM Write Address process */
-    always @(posedge clock)
-    begin
-        if( reset )
-          begin
-            usb_ram_offset_in <= 0 ;
-            usb_ram_packet_in <= 0 ;
-          end
-        else
-            if( pkt_complete )
-              begin
-                usb_ram_packet_in <= usb_ram_packet_in + 1;  
-                usb_ram_offset_in <= 0;
-              end
-            else if( write_enable ) 
-              begin
-                if (usb_ram_offset_in == 8'b11111111)
-                  begin
-                    usb_ram_offset_in <= 0;
-                    usb_ram_packet_in <= usb_ram_packet_in + 1;    
-                  end
-                else
-                    usb_ram_offset_in <= usb_ram_offset_in + 1 ;
-                    if (usb_ram_ain + 1 == usb_ram_aout)
-                       isfull <= 1;
-              end
-    end
 
-    /* RAM Writing process */
+
+    /* RAM Writing/Reading process */
     always @(posedge clock)
     begin
         if( write_enable ) 
           begin
             usb_ram[usb_ram_ain] <= ram_data_in ;
           end
+		ram_data_out <= usb_ram[usb_ram_aout] ;
     end
 
-    /* RAM Read Address process */
+    /* RAM Write/Read Address process */
     always @(posedge clock)
     begin
         if( reset ) 
           begin
             usb_ram_packet_out <= 0 ;
             usb_ram_offset_out <= 0 ;
+			usb_ram_offset_in <= 0 ;
+            usb_ram_packet_in <= 0 ;
             isfull <= 0;
           end
         else
+		  begin
             if( skip_packet )
               begin
                 usb_ram_packet_out <= usb_ram_packet_out + 1 ;
                 usb_ram_offset_out <= 0 ;
+                isfull <= 0;
               end
-            else if(read_enable) begin
-                if( usb_ram_offset_out == 8'b11111111 )
+            else if(read_enable) 
+			  begin
+                if( usb_ram_offset_out == 7'b1111111 )
                   begin
+                    isfull <= 0 ;
                     usb_ram_offset_out <= 0 ;
                     usb_ram_packet_out <= usb_ram_packet_out + 1 ;
                   end
                 else
                     usb_ram_offset_out <= usb_ram_offset_out + 1 ;  
-            end 
-            if (usb_ram_ain == usb_ram_aout)
-               isfull <= 0;                    
-    end
-
-    /* RAM Reading Process */
-    always @(posedge clock)
-    begin
-        ram_data_out <= usb_ram[usb_ram_aout] ;
+              end
+			if( pkt_complete )
+              begin
+                usb_ram_packet_in <= usb_ram_packet_in + 1 ;
+                usb_ram_offset_in <= 0 ;
+                if ((usb_ram_packet_in + 2'b1) == usb_ram_packet_out)
+                    isfull <= 1 ;
+              end
+            else if( write_enable ) 
+              begin
+                if (usb_ram_offset_in == 7'b1111111)
+                    usb_ram_offset_in <= 7'b1111111 ;    
+                else
+                    usb_ram_offset_in <= usb_ram_offset_in + 1 ;
+              end
+		  end
     end
 
 endmodule
diff --git a/inband_lib/packet_builder.v b/inband_lib/packet_builder.v
new file mode 100755
index 000000000..205293479
--- /dev/null
+++ b/inband_lib/packet_builder.v
@@ -0,0 +1,132 @@
+module packet_builder #(parameter NUM_CHAN = 1)(
+    // System
+    input rxclk,
+    input reset,
+	 input [31:0] adctime,
+	 input [3:0] channels,
+    // ADC side
+    input [15:0]chan_fifodata,
+    input [NUM_CHAN:0]chan_empty,
+    input [9:0]chan_usedw,
+    output reg [3:0]rd_select,
+    output reg chan_rdreq,
+    // FX2 side
+    output reg WR,
+    output reg [15:0]fifodata,
+    input have_space, 
+	input wire [31:0]rssi_0, input wire [31:0]rssi_1, input wire [31:0]rssi_2,
+	input wire [31:0]rssi_3, output wire [7:0] debugbus);
+    
+    
+    // States
+    `define IDLE                     3'd0
+    `define HEADER1                  3'd1
+	`define HEADER2					 3'd2
+    `define TIMESTAMP                3'd3
+ 	`define FORWARD					 3'd4
+	
+    `define MAXPAYLOAD 504
+    
+    `define PAYLOAD_LEN 8:0
+    `define TAG 12:9
+    `define MBZ 15:13
+    
+    `define CHAN 4:0
+    `define RSSI 10:5
+    `define BURST 12:11
+    `define DROPPED 13
+    `define UNDERRUN 14
+    `define OVERRUN 15
+    
+    reg [2:0] state;
+    reg [8:0] read_length;
+    reg [8:0] payload_len;
+    reg tstamp_complete;
+    reg [3:0] check_next;
+	wire [8:0] chan_used;
+    wire [31:0] true_rssi;
+
+	assign debugbus = {state, chan_empty[0], chan_empty[1], check_next[0],
+						have_space, rd_select[0]};
+	assign chan_used = chan_usedw[8:0];
+	assign true_rssi = (rd_select[1]) ? ((rd_select[0]) ? rssi_3:rssi_2) :
+							((rd_select[0]) ? rssi_1:rssi_0);	
+    always @(posedge rxclk)
+    begin
+        if (reset)
+          begin
+            WR <= 0;
+            rd_select <= 0;
+            chan_rdreq <= 0;
+            tstamp_complete <= 0;
+            check_next <= 0;
+            state <= `IDLE;
+          end
+        else case (state)
+            `IDLE: begin
+				if (have_space)
+				  begin
+					if(~chan_empty[check_next])
+				      begin
+                		state <= #1 `HEADER1;
+						rd_select <= #1 check_next;
+				  	  end
+					check_next <= #1 (check_next == channels ? 4'd0 : check_next + 4'd1);
+				  end	
+            end
+            
+            `HEADER1: begin
+                fifodata[`PAYLOAD_LEN] <= #1 (chan_used > 9'd252
+                                           ? 9'd252 : chan_used << 1);
+                payload_len <= #1 (chan_used > 9'd252
+                                ? 9'd252 : chan_used << 1);
+                fifodata[`TAG] <= #1 0;
+                fifodata[`MBZ] <= #1 0;
+                WR <= #1 1;
+                
+                state <= #1 `HEADER2;
+                read_length <= #1 0;
+            end
+            
+            `HEADER2: begin
+                fifodata[`CHAN] <= #1 (check_next == 4'd0 ? 5'h1f : {1'd0, check_next - 4'd1});
+                fifodata[`RSSI] <= #1 true_rssi[5:0];
+                fifodata[`BURST] <= #1 0;
+                fifodata[`DROPPED] <= #1 0;
+                fifodata[`UNDERRUN] <= #1 0;
+                fifodata[`OVERRUN] <= #1 0;
+                
+                state <= #1 `TIMESTAMP;
+            end
+            
+            `TIMESTAMP: begin
+                fifodata <= #1 (tstamp_complete ? adctime[31:16] : adctime[15:0]);
+                tstamp_complete <= #1 ~tstamp_complete;
+                
+                if (~tstamp_complete)
+                    chan_rdreq <= #1 1;
+                
+                state <= #1 (tstamp_complete ? `FORWARD : `TIMESTAMP);
+            end
+            
+            `FORWARD: begin
+                read_length <= #1 read_length + 9'd2;
+                fifodata <= #1 (read_length >= payload_len ? 16'hDEAD : chan_fifodata);
+                
+                if (read_length >= `MAXPAYLOAD)
+                  begin
+                    WR <= #1 0;
+                    state <= #1 `IDLE;
+                  end
+                else if (read_length == payload_len - 4)
+                    chan_rdreq <= #1 0;
+            end
+            
+            default: begin
+				//handling error state
+                state <= `IDLE;
+            end
+            endcase
+    end
+endmodule
+
diff --git a/inband_lib/register_io.v b/inband_lib/register_io.v
new file mode 100755
index 000000000..63a26549c
--- /dev/null
+++ b/inband_lib/register_io.v
@@ -0,0 +1,60 @@
+module register_io
+	(input clk, input reset, input wire [1:0] enable, input wire [6:0] addr, 
+	 input wire [31:0] datain, output reg [31:0] dataout, output wire [15:0] debugbus,
+	 input wire [31:0] rssi_0, input wire [31:0] rssi_1,
+	 input wire [31:0] rssi_2, input wire [31:0] rssi_3, output wire [31:0] threshhold);
+	 
+	reg strobe;
+	wire [31:0] out[7:0];
+	assign debugbus = {clk, enable, addr[2:0], datain[4:0], dataout[4:0]};
+	assign threshhold = out[1];
+	
+	always @(*)
+        if (reset | ~enable[1])
+           begin
+             strobe <= 0;
+		     dataout <= 0;
+		   end
+		else
+		   begin
+	         if (enable[0])
+	           begin
+	             //read
+                 if (addr == 7'd9)
+                 	dataout <= rssi_0;
+                 else if (addr == 7'd10)
+                 	dataout <= rssi_1;
+                 else if (addr == 7'd11)
+                 	dataout <= rssi_2;
+                 else if (addr == 7'd12)
+                 	dataout <= rssi_3;
+                 else
+	             	dataout <= out[addr[2:0]];
+	             strobe <= 0;
+               end
+             else
+               begin
+                 //write
+	             dataout <= dataout;
+                 strobe <= 1;
+               end
+          end
+
+	//register declarations
+    setting_reg #(0) setting_reg0(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[0]));
+    setting_reg #(1) setting_reg1(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[1]));
+    setting_reg #(2) setting_reg2(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[2]));
+    setting_reg #(3) setting_reg3(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[3]));
+    setting_reg #(4) setting_reg4(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[4]));
+    setting_reg #(5) setting_reg5(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[5]));
+    setting_reg #(6) setting_reg6(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[6]));
+    setting_reg #(7) setting_reg7(.clock(clk),.reset(reset),
+    .strobe(strobe),.addr(addr),.in(datain),.out(out[7]));
+endmodule	
\ No newline at end of file
diff --git a/inband_lib/rx_buffer_inband.v b/inband_lib/rx_buffer_inband.v
new file mode 100755
index 000000000..c23ce09b7
--- /dev/null
+++ b/inband_lib/rx_buffer_inband.v
@@ -0,0 +1,175 @@
+//`include "../../firmware/include/fpga_regs_common.v"

+//`include "../../firmware/include/fpga_regs_standard.v"

+module rx_buffer_inband

+  ( input usbclk,

+    input bus_reset,

+    input reset,  // DSP side reset (used here), do not reset registers

+    input reset_regs, //Only reset registers

+    output [15:0] usbdata,

+    input RD,

+    output wire have_pkt_rdy,

+    output reg rx_overrun,

+    input wire [3:0] channels,

+    input wire [15:0] ch_0,

+    input wire [15:0] ch_1,

+    input wire [15:0] ch_2,

+    input wire [15:0] ch_3,

+    input wire [15:0] ch_4,

+    input wire [15:0] ch_5,

+    input wire [15:0] ch_6,

+    input wire [15:0] ch_7,

+    input rxclk,

+    input rxstrobe,

+    input clear_status,

+    input [6:0] serial_addr, 

+    input [31:0] serial_data, 

+    input serial_strobe,

+    output wire [15:0] debugbus,

+	

+	//Connection with tx_inband

+	input rx_WR,

+	input [15:0] rx_databus,

+	input rx_WR_done,

+	output reg rx_WR_enabled,

+	//signal strength

+	input wire [31:0] rssi_0, input wire [31:0] rssi_1,

+	input wire [31:0] rssi_2, input wire [31:0] rssi_3

+    );

+    

+    parameter NUM_CHAN = 1;

+    genvar i ;

+    

+    // FX2 Bug Fix

+    reg [8:0] read_count;

+    always @(negedge usbclk)

+        if(bus_reset)

+            read_count <= #1 9'd0;

+        else if(RD & ~read_count[8])

+            read_count <= #1 read_count + 9'd1;

+        else

+            read_count <= #1 RD ? read_count : 9'b0;

+       

+	// Time counter

+	reg [31:0] adctime;

+	always @(posedge rxclk)

+		if (reset)

+			adctime <= 0;

+		else if (rxstrobe)

+			adctime <= adctime + 1;

+     

+    // USB side fifo

+    wire [11:0] rdusedw;

+    wire [11:0] wrusedw;

+    wire [15:0] fifodata;

+    wire WR;

+    wire have_space;

+

+    fifo_4kx16_dc	rx_usb_fifo (

+	     .aclr ( reset ),

+	     .data ( fifodata ),

+	     .rdclk ( ~usbclk ),

+	     .rdreq ( RD & ~read_count[8] ),

+	     .wrclk ( rxclk ),

+	     .wrreq ( WR ),

+	     .q ( usbdata ),

+	     .rdempty (  ),

+	     .rdusedw ( rdusedw ),

+	     .wrfull (  ),

+	     .wrusedw ( wrusedw ) );

+    

+     assign have_pkt_rdy = (rdusedw >= 12'd256);

+	 assign have_space = (wrusedw < 12'd760);

+	 

+	 // Rx side fifos

+	 wire chan_rdreq;

+	 wire [15:0] chan_fifodata;

+	 wire [9:0] chan_usedw;

+	 wire [NUM_CHAN:0] chan_empty;

+	 wire [3:0] rd_select;

+	 wire [NUM_CHAN:0] rx_full;

+	 

+	 packet_builder #(NUM_CHAN) rx_pkt_builer (

+	     .rxclk ( rxclk ),

+	     .reset ( reset ),

+		  .adctime ( adctime ),

+		  .channels ( 4'd1 ), 

+	     .chan_rdreq ( chan_rdreq ),

+	     .chan_fifodata ( chan_fifodata ),

+	     .chan_empty ( chan_empty ),

+	     .rd_select ( rd_select ),

+	     .chan_usedw ( chan_usedw ),

+	     .WR ( WR ),

+	     .fifodata ( fifodata ),

+	     .have_space ( have_space ),

+		 .rssi_0(rssi_0), .rssi_1(rssi_1),

+		.rssi_2(rssi_2),.rssi_3(rssi_3), .debugbus(debug));

+	 

+	 // Detect overrun

+	 always @(posedge rxclk)

+        if(reset)

+            rx_overrun <= 1'b0;

+        else if(rx_full[0])

+            rx_overrun <= 1'b1;

+        else if(clear_status)

+            rx_overrun <= 1'b0;

+

+	reg [6:0] test;

+	always @(posedge rxclk)

+		if (reset)

+			test <= 0;

+		else

+			test <= test + 7'd1;

+		

+	 // TODO write this genericly

+	 wire [15:0]ch[NUM_CHAN:0];

+	 assign ch[0] = ch_0;

+	 

+	 wire cmd_empty;

+	 always @(posedge rxclk)

+        if(reset)

+            rx_WR_enabled <= 1;

+		else if(cmd_empty)

+            rx_WR_enabled <= 1;

+        else if(rx_WR_done)

+            rx_WR_enabled <= 0;

+

+	wire [15:0] dataout [0:NUM_CHAN];

+	wire [9:0]  usedw	[0:NUM_CHAN];

+	

+	 generate for (i = 0 ; i < NUM_CHAN; i = i + 1)

+     begin : generate_channel_fifos

+		wire rdreq;

+

+		assign rdreq = (rd_select == i) & chan_rdreq;

+		assign chan_empty[i] = usedw[i] < 10'd126;

+		

+        fifo_2kx16	rx_chan_fifo (

+	         .aclr ( reset ),

+	         .clock ( rxclk ),

+	         .data ( ch[i] ),

+	         .rdreq ( rdreq ),

+			 .wrreq ( ~rx_full[i] & rxstrobe),

+	         .empty (  ),

+	         .full ( rx_full[i] ),

+	         .q ( dataout[i]),

+             .usedw ( usedw[i] )

+		);

+     end

+     endgenerate

+	wire [7:0] debug;

+	 fifo_2kx16 rx_cmd_fifo (

+	         .aclr ( reset ),

+	         .clock ( rxclk ),

+	         .data ( rx_databus ),

+	         .rdreq ( (rd_select == NUM_CHAN) & chan_rdreq ),

+			 .wrreq ( rx_WR & rx_WR_enabled),

+	         .empty ( cmd_empty),

+	         .full ( rx_full[NUM_CHAN] ),

+	         .q ( dataout[NUM_CHAN]),

+             .usedw ( usedw[NUM_CHAN] )

+	);	

+  	assign chan_empty[NUM_CHAN] = cmd_empty | rx_WR_enabled;

+	assign chan_fifodata 	= dataout[rd_select];

+	assign chan_usedw	  	= usedw[rd_select];

+    assign debugbus = {wrusedw, have_space, RD, read_count[8], rxclk};

+endmodule

diff --git a/inband_lib/tx_buffer_inband.v b/inband_lib/tx_buffer_inband.v
index 56c07807a..af7ed394a 100755
--- a/inband_lib/tx_buffer_inband.v
+++ b/inband_lib/tx_buffer_inband.v
@@ -1,183 +1,227 @@
 module tx_buffer_inband
-  ( input usbclk,
-    input bus_reset,  // Used here for the 257-Hack to fix the FX2 bug
-    input reset,  // standard DSP-side reset
-    input [15:0] usbdata,
-    input wire WR,
-    output wire have_space,
-    output reg tx_underrun,
-    input wire [3:0] channels,
-    output [15:0] tx_i_0,
-    output [15:0] tx_q_0,
-    output [15:0] tx_i_1,
-    output [15:0] tx_q_1,
-    //NOT USED
-    output reg [15:0] tx_i_2,
-    output reg [15:0] tx_q_2,
-    output reg [15:0] tx_i_3,
-    output reg [15:0] tx_q_3,
-    input txclk,
-    input txstrobe,
-    input clear_status,
-    output wire tx_empty,
-    output [11:0] debugbus
-    );
+  ( usbclk, bus_reset, reset, usbdata, WR, have_space, 
+    tx_underrun, channels, tx_i_0, tx_q_0, tx_i_1, tx_q_1,
+    tx_i_2, tx_q_2, tx_i_3, tx_q_3, txclk, txstrobe,
+    clear_status, tx_empty, debugbus, 
+	rx_databus, rx_WR, rx_WR_done, rx_WR_enabled, reg_io_enable,
+	reg_data_in, reg_data_out, reg_addr, rssi_0, rssi_1, rssi_2, 
+    rssi_3, threshhold
+   );
 
-   wire [15:0] tx_data_bus;
+	//CHAN_WIDTH is the width of the channel
+	//NUM_CHAN is the number of data channel (index from 0 to NUM_CHAN-1)
+	//index NUM_CHAN is reserved for command
+	
+	parameter CHAN_WIDTH = 		2 ;
+    parameter NUM_CHAN	 =      2 ;
+	/* Debug paramters */
+    parameter STROBE_RATE_0 =   8'd1 ;
+    parameter STROBE_RATE_1 =   8'd2 ;
+    
+    input   wire                usbclk ;
+    input   wire                bus_reset ; // Used here for the 257-Hack to fix the FX2 bug
+    input   wire                reset ; // standard DSP-side reset
+    input   wire         [15:0] usbdata ;
+    input   wire                WR ;
+    input   wire                txclk ;
+    input   wire                txstrobe ;
+	input 	wire				rx_WR_enabled;
+    /* Not used yet */
+    input   wire          [3:0] channels ;
+    input   wire                clear_status ;
+    /*register io*/
+    input   wire          [31:0]reg_data_out;
+    // rssi
+    input	wire		  [31:0]rssi_0;
+    input	wire		  [31:0]rssi_1;
+    input	wire		  [31:0]rssi_2;
+    input	wire		  [31:0]rssi_3;
+    input	wire		  [31:0]threshhold;
 
-   wire WR_chan_0;
-   wire chan_0_done;
-   wire OR0;
-   wire UR0;
-   
-   wire WR_chan_1;
-   wire chan_1_done;
-   wire OR1;
-   wire UR1;
-   
-   // NOT USED yet
-   wire WR_cmd;
-   wire cmd_done;
-   
-   //EXTERNAL REGISTER
-   //TODO: increment it
-   reg [31:0] time_counter;
-   reg [7:0] txstrobe_rate_0;
-   reg [7:0] txstrobe_rate_1;
-   
-   
-   //Usb block
-   wire [15:0] tupf_fifodata;
-   wire tupf_pkt_waiting;
-   wire tupf_rdreq;
-   wire tupf_skip;
-   wire tupf_have_space;
+    output  wire                have_space ;
+    output  wire                tx_underrun ;
+    output  wire                tx_empty ;
+    output  wire         [15:0] tx_i_0 ;
+    output  wire         [15:0] tx_q_0 ;
+    output  wire         [15:0] tx_i_1 ;
+    output  wire         [15:0] tx_q_1 ;
+    output  wire         [15:0] debugbus ;
+    /* Not used yet */
+    output  wire         [15:0] tx_i_2 ;
+    output  wire         [15:0] tx_q_2 ;
+    output  wire         [15:0] tx_i_3 ;
+    output  wire         [15:0] tx_q_3 ;
+
+	output	wire		 [15:0] rx_databus ;
+	output	wire		 		rx_WR;
+	output 	wire				rx_WR_done;
+    /* reg_io */
+    output  wire         [31:0] reg_data_in;
+    output  wire         [6:0]  reg_addr;
+    output  wire         [1:0]  reg_io_enable;
+
+    /* To generate channel readers */
+    genvar i ;
+    
+    /* These will eventually be external register */
+    reg                  [31:0] adc_time ;
+    wire                  [7:0] txstrobe_rate [CHAN_WIDTH-1:0] ;
+    wire				 [31:0] rssi [3:0];
+    assign rssi[0] = rssi_0;
+    assign rssi[1] = rssi_1;
+    assign rssi[2] = rssi_2;
+    assign rssi[3] = rssi_3;
    
-   usb_packet_fifo2 tx_usb_packet_fifo 
-     (  .reset         (reset),
-        .usb_clock     (usbclk), 
-        .fpga_clock    (txclk),
-        .write_data    (usbdata),
-        .write_enable  (WR),
-        .read_data     (tupf_fifodata),
-        .pkt_waiting   (tupf_pkt_waiting),
-        .read_enable   (tupf_rdreq), 
-        .skip_packet   (tupf_skip),
-        .have_space    (tupf_have_space),
-        .tx_empty      (tx_empty)
-       );
+	always @(posedge txclk)
+		if (reset)
+			adc_time <= 0;
+		else if (txstrobe)
+			adc_time <= adc_time + 1;
+
+
+    /* Connections between tx_usb_fifo_reader and
+       cnannel/command processing blocks */
+    wire                 [31:0] tx_data_bus ;
+    wire           [CHAN_WIDTH:0] chan_WR ;
+    wire           [CHAN_WIDTH:0] chan_done ;
+    
+    /* Connections between data block and the
+       FX2/TX chains */
+    wire           [CHAN_WIDTH:0] chan_underrun ;
+    wire           [CHAN_WIDTH:0] chan_txempty ;
    
-	usb_fifo_reader tx_usb_packet_reader (
-		.reset(reset),
-		.tx_clock(txclk),
-		.tx_data_bus(tx_data_bus),
-      .WR_chan_0(WR_chan_0),
-      .WR_chan_1(WR_chan_1),
-      .WR_cmd(WR_cmd),
-      .chan_0_done(chan_0_done),
-      .chan_1_done(chan_1_done),
-      .cmd_done(cmd_done),
-      .rdreq(tupf_rdreq),
-      .skip(tupf_skip),
-      .pkt_waiting(tupf_pkt_waiting),
-      .fifodata(tupf_fifodata)
+    /* Conections between tx_data_packet_fifo and
+       its reader + strobe generator */
+    wire                 [31:0] chan_fifodata [CHAN_WIDTH:0] ;
+    wire                        chan_pkt_waiting [CHAN_WIDTH:0] ;
+    wire                        chan_rdreq [CHAN_WIDTH:0] ;
+    wire                        chan_skip [CHAN_WIDTH:0] ;
+    wire           [CHAN_WIDTH:0] chan_have_space ;
+    wire                        chan_txstrobe [CHAN_WIDTH-1:0] ;
+
+	wire				[14:0]  debug;
+    
+    /* Outputs to transmit chains */
+    wire                 [15:0] tx_i [CHAN_WIDTH-1:0] ;
+    wire                 [15:0] tx_q [CHAN_WIDTH-1:0] ;
+    
+	/* TODO: Figure out how to write this genericly */
+    assign have_space = chan_have_space[0] & chan_have_space[1];
+    assign tx_empty = chan_txempty[0] & chan_txempty[1] ;
+    assign tx_underrun = chan_underrun[0] | chan_underrun[1] ;
+    assign tx_i_0 = chan_txempty[0] ? 16'b0 : tx_i[0] ;
+    assign tx_q_0 = chan_txempty[0] ? 16'b0 : tx_q[0] ;
+    assign tx_i_1 = chan_txempty[1] ? 16'b0 : tx_i[1] ;
+    assign tx_q_1 = chan_txempty[1] ? 16'b0 : tx_q[1] ;
+        
+    /* Debug statement */
+    assign txstrobe_rate[0] = STROBE_RATE_0 ;
+    assign txstrobe_rate[1] = STROBE_RATE_1 ;
+	assign tx_q_2 = 16'b0 ;
+	assign tx_i_2 = 16'b0 ;
+	assign tx_q_3 = 16'b0 ;
+	assign tx_i_3 = 16'b0 ;
+	assign tx_i_3 = 16'b0 ;
+	
+	assign debugbus = {debug, txclk};
+
+	wire [31:0] usbdata_final;
+	wire		WR_final;
+
+	tx_packer tx_usb_packer
+	(
+				.bus_reset			(bus_reset),
+				.usbclk				(usbclk),
+				.WR_fx2				(WR),
+				.usbdata			(usbdata),
+				.reset				(reset),
+				.txclk				(txclk),
+				.usbdata_final 		(usbdata_final),
+				.WR_final			(WR_final)
+	);
+	
+	channel_demux channel_demuxer
+	(
+				.usbdata_final		(usbdata_final),
+				.WR_final			(WR_final),
+				.reset				(reset),
+				.txclk				(txclk),
+                .WR_channel         (chan_WR),
+                .WR_done_channel    (chan_done),
+                .ram_data           (tx_data_bus)				
 	);
+	
+    generate for (i = 0 ; i < NUM_CHAN; i = i + 1)
+    begin : generate_channel_readers
+        channel_ram tx_data_packet_fifo 
+            (      .reset               (reset),
+                   .txclk               (txclk), 
+                   .datain              (tx_data_bus),
+                   .WR                  (chan_WR[i]),
+                   .WR_done             (chan_done[i]),
+                   .have_space          (chan_have_space[i]),
+                   .dataout             (chan_fifodata[i]),
+                   .packet_waiting      (chan_pkt_waiting[i]),
+                   .RD                  (chan_rdreq[i]),
+                   .RD_done             (chan_skip[i])
+             );
 
+        chan_fifo_reader tx_chan_reader 
+           (       .reset               (reset),
+                   .tx_clock            (txclk),
+                   .tx_strobe           (txstrobe),
+                   .adc_time            (adc_time),
+                   .samples_format      (4'b0),
+                   .tx_q                (tx_q[i]),
+                   .tx_i                (tx_i[i]),
+                   .underrun            (chan_underrun[i]),
+                   .skip             	(chan_skip[i]),
+                   .rdreq               (chan_rdreq[i]),
+                   .fifodata            (chan_fifodata[i]),
+                   .pkt_waiting         (chan_pkt_waiting[i]),
+                   .tx_empty            (chan_txempty[i]),
+                   .rssi				(rssi[i]),
+                   .threshhold			(threshhold)
+            );	    
+        
+    end
+    endgenerate
 
-   //Channel 0 block
-   wire [15:0] tdpf_fifodata_0;
-   wire tdpf_pkt_waiting_0;
-   wire tdpf_rdreq_0;
-   wire tdpf_skip_0;
-   wire tdpf_have_space_0;
-   wire txstrobe_chan_0;
 
-   data_packet_fifo tx_data_packet_fifo_0 
-     (  .reset(reset),
-        .clock(txclk), 
-        .ram_data_in(tx_data_bus),
-        .write_enable(WR_chan_0),
-        .ram_data_out(tdpf_fifodata_0),
-        .pkt_waiting(tdpf_pkt_waiting_0),
-        .read_enable(tdpf_rdreq_0),
-        .pkt_complete(chan_0_done), 
-        .skip_packet(tdpf_skip_0),
-        .have_space(tdpf_have_space_0)
-       );
-   
-   strobe_gen strobe_gen_0
-    (   .clock(txclk),
-        .reset(reset),
-        .enable(1'b1),
-        .rate(txstrobe_rate_0),
-        .strobe_in(txstrobe),
-        .strobe(txstrobe_chan_0) 
-       );
-   
-   chan_fifo_reader tx_chan_0_reader (
-      .reset(reset),
-      .tx_clock(txclk),
-      .tx_strobe(txstrobe),
-      //.tx_strobe(txstrobe_chan_0),
-      .adc_clock(time_counter),
-      .samples_format(4'b0),
-      .tx_q(tx_q_0),
-      .tx_i(tx_i_0),
-      .overrun(OR0),
-      .underrun(UR0),
-      .skip(tdpf_skip_0),
-      .rdreq(tdpf_rdreq_0),
-      .fifodata(tdpf_fifodata_0),
-      .pkt_waiting(tdpf_pkt_waiting_0)
-   );	
-   
-   
-   //Channel 1 block
-   wire [15:0] tdpf_fifodata_1;
-   wire tdpf_pkt_waiting_1;
-   wire tdpf_rdreq_1;
-   wire tdpf_skip_1;
-   wire tdpf_have_space_1;
-   wire txstrobe_chan_1;
-   
-   data_packet_fifo tx_data_packet_fifo_1 
-     (  .reset(reset),
-        .clock(txclk), 
-        .ram_data_in(tx_data_bus),
-        .write_enable(WR_chan_1),
-        .ram_data_out(tdpf_fifodata_1),
-        .pkt_waiting(tdpf_pkt_waiting_1),
-        .read_enable(tdpf_rdreq_1),
-        .pkt_complete(chan_1_done), 
-        .skip_packet(tdpf_skip_1),
-        .have_space(tdpf_have_space_1)
-       );
-   
-   strobe_gen strobe_gen_1
-    (   .clock(txclk),
-        .reset(reset),
-        .enable(1'b1),
-        .rate(txstrobe_rate_1),
-        .strobe_in(txstrobe),
-        .strobe(txstrobe_chan_1) 
-       );
-   
-   chan_fifo_reader tx_chan_1_reader (
-      .reset(reset),
-      .tx_clock(txclk),
-      .tx_strobe(txstrobe),
-      //.tx_strobe(txstrobe_chan_1),
-      .adc_clock(time_counter),
-      .samples_format(4'b0),
-      .tx_q(tx_q_1),
-      .tx_i(tx_i_1),
-      .overrun(OR1),
-      .underrun(UR1),
-      .skip(tdpf_skip_1),
-      .rdreq(tdpf_rdreq_1),
-      .fifodata(tdpf_fifodata_1),
-      .pkt_waiting(tdpf_pkt_waiting_1)
-   );
+	channel_ram tx_cmd_packet_fifo 
+            (      .reset               (reset),
+                   .txclk               (txclk), 
+                   .datain              (tx_data_bus),
+                   .WR                  (chan_WR[NUM_CHAN]),
+                   .WR_done             (chan_done[NUM_CHAN]),
+                   .have_space          (chan_have_space[NUM_CHAN]),
+                   .dataout             (chan_fifodata[NUM_CHAN]),
+                   .packet_waiting      (chan_pkt_waiting[NUM_CHAN]),
+                   .RD                  (chan_rdreq[NUM_CHAN]),
+                   .RD_done             (chan_skip[NUM_CHAN])
+             );
+
+
+	cmd_reader tx_cmd_reader
+		(		.reset					(reset),
+				.txclk					(txclk),
+				.adc_time				(adc_time),
+				.skip					(chan_skip[NUM_CHAN]),
+				.rdreq					(chan_rdreq[NUM_CHAN]),
+				.fifodata				(chan_fifodata[NUM_CHAN]),
+				.pkt_waiting			(chan_pkt_waiting[NUM_CHAN]),
+				.rx_databus				(rx_databus),
+				.rx_WR					(rx_WR),
+				.rx_WR_done				(rx_WR_done),
+				.rx_WR_enabled			(rx_WR_enabled),
+				.reg_data_in			(reg_data_in),
+				.reg_data_out			(reg_data_out),
+				.reg_addr				(reg_addr),
+				.reg_io_enable			(reg_io_enable),
+				.debug					(debug)
+		);
+				
+		
    
 endmodule // tx_buffer
 
diff --git a/inband_lib/tx_packer.v b/inband_lib/tx_packer.v
new file mode 100644
index 000000000..2f19b21f3
--- /dev/null
+++ b/inband_lib/tx_packer.v
@@ -0,0 +1,119 @@
+module tx_packer
+   (     //FX2 Side
+			input bus_reset, 
+			input usbclk, 
+			input WR_fx2, 
+			input [15:0]usbdata,
+			
+			// TX Side
+			input reset,
+			input txclk,
+			output reg [31:0] usbdata_final,
+			output reg WR_final);
+
+	reg [8:0] write_count;
+
+	/* Fix FX2 bug */
+	always @(posedge usbclk)
+	begin
+    	if(bus_reset)        // Use bus reset because this is on usbclk
+       		write_count <= #1 0;
+    	else if(WR_fx2 & ~write_count[8])
+    		write_count <= #1 write_count + 9'd1;
+    	else
+    		write_count <= #1 WR_fx2 ? write_count : 9'b0;
+	end
+	
+	reg WR_fx2_fixed;
+	reg [15:0]usbdata_fixed;
+	
+	always @(posedge usbclk) 
+	begin
+	   WR_fx2_fixed <= WR_fx2 & ~write_count[8];
+	   usbdata_fixed <= usbdata;
+	end
+
+	/* Used to convert 16 bits bus_data to the 32 bits wide fifo */
+    reg                             word_complete ;
+    reg     [15:0]         			usbdata_delayed ;
+    reg                             writing ;
+	wire	[31:0]					usbdata_packed ;    
+	wire							WR_packed ;
+   
+    always @(posedge usbclk)
+    begin
+        if (bus_reset)
+          begin
+            word_complete <= 0 ;
+            writing <= 0 ;
+          end
+        else if (WR_fx2_fixed)
+          begin
+            writing <= 1 ;
+            if (word_complete)
+                word_complete <= 0 ;
+            else
+              begin
+                usbdata_delayed <= usbdata_fixed ;
+                word_complete <= 1 ;
+              end
+          end
+        else
+            writing <= 0 ;
+	end
+    
+	assign usbdata_packed = {usbdata_fixed, usbdata_delayed} ;
+    assign WR_packed = word_complete & writing ;
+
+	/* Make sure data are sync with usbclk */
+ 	reg [31:0]usbdata_usbclk;
+	reg WR_usbclk; 
+    
+    always @(posedge usbclk)
+    begin
+    	if (WR_packed)
+    		usbdata_usbclk <= usbdata_packed;
+        WR_usbclk <= WR_packed;
+    end
+
+	/* Cross clock boundaries */
+	reg [31:0] usbdata_tx ;
+	reg WR_tx;
+    reg WR_1;
+    reg WR_2;
+  
+	always @(posedge txclk) usbdata_tx <= usbdata_usbclk;
+
+    always @(posedge txclk) 
+    	if (reset)
+    		WR_1 <= 0;
+    	else
+       		WR_1 <= WR_usbclk;
+
+    always @(posedge txclk) 
+    	if (reset)
+       		WR_2 <= 0;
+    	else
+      		WR_2 <= WR_1;
+
+	always @(posedge txclk)
+	begin
+		if (reset)
+			WR_tx <= 0;
+		else
+		   WR_tx <= WR_1 & ~WR_2;
+	end
+	
+	always @(posedge txclk)
+	begin
+	   if (reset)
+	      WR_final <= 0;
+	   else
+	   begin
+	      WR_final <= WR_tx; 
+	      if (WR_tx)
+	         usbdata_final <= usbdata_tx;
+	   end
+	end
+
+endmodule
diff --git a/inband_lib/usb_fifo_reader.v b/inband_lib/usb_fifo_reader.v
index 170c70fd4..d002d90ff 100755
--- a/inband_lib/usb_fifo_reader.v
+++ b/inband_lib/usb_fifo_reader.v
@@ -1,134 +1,24 @@
-module usb_fifo_reader (tx_clock, fifodata, pkt_waiting, reset,
-      rdreq, skip, done_chan, WR_chan, tx_data_bus);
+module usb_fifo_reader (
+      input usbclk,
+      input bus_reset, 
+      input RD,
+      output rdreq,
+      );
       
-    /* Module parameters */
-    parameter                       NUM_CHAN      =   2 ;
-    parameter                       WIDTH         =   32 ;
+    // FX2 Bug Fix
+    reg [8:0] read_count;
+    always @(negedge usbclk)
+        if(bus_reset)
+            read_count <= #1 9'd0;
+        else if(RD & ~read_count[8])
+            read_count <= #1 read_count + 9'd1;
+        else
+            read_count <= #1 RD ? read_count : 9'b0;
+            
+    assign rdreq = RD & ~read_count[8];
     
-    input   wire                    tx_clock ;
-    input   wire                    reset ;
-    input   wire       [WIDTH-1:0]  fifodata ;
-    input   wire                    pkt_waiting ;
-    output  reg                     rdreq ;
-    output  reg                     skip ;
-    output  reg        [NUM_CHAN:0] done_chan ;
-    output  reg        [NUM_CHAN:0] WR_chan ;
-    output  reg        [WIDTH-1:0]  tx_data_bus ;
-     
-   
-   
-    /* States definition */
-    `define IDLE                      3'd0
-    `define WAIT                      3'd1
-    `define READ_HEADER               3'd2
-    `define FORWARD_DATA              3'd3
-    `define SKIP_REST                 3'd4
-   
-    /* Channel Ids */
-    `define TXCHAN0                   5'h0
-    `define TXCHAN1                   5'h1
-    `define TXCMD                     5'h1F
-   
-    /* Local registers */
-    reg                      [2:0]    reader_state ;
-    reg                      [2:0]    reader_next_state ;
-    reg                      [4:0]    channel ;
-    reg                      [8:0]    pkt_length ;
-    reg                      [8:0]    read_length ;
     
-    /* State Machine */
-    always @(posedge tx_clock)
-    begin
-        if (reset) 
-		  begin
-		      reader_state <= `IDLE ;
-            reader_next_state <= `IDLE ;
-            rdreq <= 0 ;
-            skip <= 0 ;
-            WR_chan <= {NUM_CHAN+1{1'b0}} ;
-            done_chan <= {NUM_CHAN+1{1'b0}} ;
-          end
-        else 
-		  begin
-            reader_state = reader_next_state ;
-            
-            case(reader_state)
-            `IDLE: 
-				begin
-				    reader_next_state <= pkt_waiting ? `WAIT : `IDLE ;
-                rdreq <= pkt_waiting ;
-            end
-     
-            /* Wait for the fifo's data to show up */
-            `WAIT:
-            begin
-			       reader_next_state <= `READ_HEADER ;
-            end
-               
-            `READ_HEADER: 
-			   begin
-                reader_next_state <= `FORWARD_DATA ;
-                  
-                /* Read header fields */
-                channel <= (fifodata & 32'h1F0000) ;
-                pkt_length <= (fifodata & 16'h1FF) + 4 ;
-                read_length <= 9'd0 ;
-                  
-                /* Forward data */
-                case (channel)
-                    `TXCHAN0: WR_chan[0] <= 1 ;
-                    `TXCHAN1: WR_chan[1] <= 1 ;
-                    `TXCMD:   WR_chan[2] <= 1 ;
-                    default:  WR_chan <= 1 ;
-                endcase
-                tx_data_bus <= fifodata ;
-            end
-               
-            `FORWARD_DATA:
-			   begin
-                read_length <= read_length + 4 ;
-                  
-                // If end of payload...
-                if (read_length == pkt_length)
-				    begin
-                    reader_next_state <= `SKIP_REST ;
-                    /* If the packet is 512 bytes, don't skip */
-                    skip <= pkt_length < 506 ;
-                     
-                    /* Data pushing done */
-                    WR_chan <= {NUM_CHAN+1{1'b0}} ;
-                    
-                    /* Notify next block */
-                    case (channel)
-                       `TXCHAN0: done_chan[0] <= 1 ;
-                       `TXCHAN1: done_chan[1] <= 1 ;
-                       `TXCMD:   done_chan[2] <= 1 ;
-                       default:  done_chan[0] <= 1 ;
-                    endcase
-                end
-                else if (read_length == pkt_length - 4)
-                    rdreq <= 0 ;
-                    
-                /* Forward data */
-                tx_data_bus <= fifodata ;
-            end
-               
-            `SKIP_REST: 
-			   begin
-			       reader_next_state <= pkt_waiting ? `READ_HEADER : `IDLE ;
-                done_chan <= {NUM_CHAN+1{1'b0}} ;
-                rdreq <= pkt_waiting ;
-                skip <= 0 ;
-            end
-                
-            default: 
-			   begin
-                reader_state <= `IDLE;
-                reader_next_state <= `IDLE;
-            end
-            endcase
-        end
-    end  
+ 
 endmodule
        
    
diff --git a/inband_lib/usb_fifo_writer.v b/inband_lib/usb_fifo_writer.v
new file mode 100755
index 000000000..abe1dd567
--- /dev/null
+++ b/inband_lib/usb_fifo_writer.v
@@ -0,0 +1,183 @@
+
+module usb_fifo_writer
+   #(parameter BUS_WIDTH = 16,
+     parameter NUM_CHAN = 2,
+     parameter FIFO_WIDTH = 32)
+   (     //FX2 Side
+			input bus_reset, 
+			input usbclk, 
+			input WR_fx2, 
+			input [15:0]usbdata,
+			
+			// TX Side
+			input reset,
+			input txclk,
+			output reg [NUM_CHAN:0] WR_channel,
+			output reg [FIFO_WIDTH-1:0] ram_data,
+			output reg [NUM_CHAN:0] WR_done_channel );
+   
+
+	reg [8:0] write_count;
+
+	/* Fix FX2 bug */
+	always @(posedge usbclk)
+    	if(bus_reset)        // Use bus reset because this is on usbclk
+       		write_count <= #1 0;
+    	else if(WR_fx2 & ~write_count[8])
+    		write_count <= #1 write_count + 9'd1;
+    	else
+    		write_count <= #1 WR_fx2 ? write_count : 9'b0;
+
+	reg WR_fx2_fixed;
+	reg [15:0]usbdata_fixed;
+	
+	always @(posedge usbclk) 
+	begin
+	   WR_fx2_fixed <= WR_fx2 & ~write_count[8];
+	   usbdata_fixed <= usbdata;
+	end
+
+	/* Used to convert 16 bits bus_data to the 32 bits wide fifo */
+    reg                             word_complete ;
+    reg     [BUS_WIDTH-1:0]         usbdata_delayed ;
+    reg                             writing ;
+	wire	[FIFO_WIDTH-1:0]		usbdata_packed ;    
+	wire							WR_packed ;
+   
+    always @(posedge usbclk)
+    begin
+        if (bus_reset)
+          begin
+            word_complete <= 0 ;
+            writing <= 0 ;
+          end
+        else if (WR_fx2_fixed)
+          begin
+            writing <= 1 ;
+            if (word_complete)
+                word_complete <= 0 ;
+            else
+              begin
+                usbdata_delayed <= usbdata_fixed ;
+                word_complete <= 1 ;
+              end
+          end
+        else
+            writing <= 0 ;
+	end
+    
+	assign usbdata_packed = {usbdata_fixed, usbdata_delayed} ;
+    assign WR_packed = word_complete & writing ;
+
+	/* Make sure data are sync with usbclk */
+ 	reg [31:0]usbdata_usbclk;
+	reg WR_usbclk; 
+    
+    always @(posedge usbclk)
+    begin
+    	if (WR_packed)
+    		usbdata_usbclk <= usbdata_packed;
+        WR_usbclk <= WR_packed;
+    end
+
+	/* Cross clock boundaries */
+	reg [FIFO_WIDTH-1:0] usbdata_tx ;
+	reg WR_tx;
+    reg WR_1;
+    reg WR_2;
+	reg [31:0] usbdata_final;
+	reg WR_final;
+   
+	always @(posedge txclk) usbdata_tx <= usbdata_usbclk;
+
+    always @(posedge txclk) 
+    	if (reset)
+    		WR_1 <= 0;
+    	else
+       		WR_1 <= WR_usbclk;
+
+    always @(posedge txclk) 
+    	if (reset)
+       		WR_2 <= 0;
+    	else
+      		WR_2 <= WR_1;
+
+	always @(posedge txclk)
+	begin
+		if (reset)
+			WR_tx <= 0;
+		else
+		   WR_tx <= WR_1 & ~WR_2;
+	end
+	
+	always @(posedge txclk)
+	begin
+	   if (reset)
+	      WR_final <= 0;
+	   else
+	   begin
+	      WR_final <= WR_tx; 
+	      if (WR_tx)
+	         usbdata_final <= usbdata_tx;
+	   end
+	end
+	     
+	/* Parse header and forward to ram */
+	reg [3:0]reader_state;
+	reg [4:0]channel ;
+	reg [9:0]read_length ;
+	
+	parameter IDLE = 4'd0;
+	parameter HEADER = 4'd1;
+	parameter WAIT = 4'd2;
+	parameter FORWARD = 4'd3;
+	
+	`define CHANNEL 20:16
+	`define PKT_SIZE 512
+	
+	always @(posedge txclk)
+	begin
+	    if (reset)
+	      begin
+	       reader_state <= 0;
+	       WR_channel <= 0;
+	       WR_done_channel <= 0;
+	      end
+	      else
+	        case (reader_state)
+	        IDLE: begin
+	            if (WR_final)
+	                reader_state <= HEADER; 
+	            end
+	       
+            // Store channel and forware header
+	        HEADER: begin
+	            channel <= (usbdata_final[`CHANNEL] == 5'h1f ? NUM_CHAN : usbdata_final[`CHANNEL]) ;
+	            WR_channel[(usbdata_final[`CHANNEL] == 5'h1f ? NUM_CHAN : usbdata_final[`CHANNEL])] <= 1;
+				//channel <= usbdata_final[`CHANNEL] ;
+	            //WR_channel[usbdata_final[`CHANNEL]] <= 1;
+	            ram_data <= usbdata_final;
+				read_length <= 10'd4 ;
+				
+                reader_state <= WAIT;
+	        end
+	       
+	        WAIT: begin
+	           WR_channel[channel] <= 0;
+	
+			   if (read_length == `PKT_SIZE)
+	               reader_state <= IDLE;
+	           else if (WR_final)
+	               reader_state <= FORWARD;
+	        end
+	       
+	        FORWARD: begin
+	           WR_channel[channel] <= 1;
+	           ram_data <= usbdata_final;
+	           read_length <= read_length + 10'd4;
+	           
+	           reader_state <= WAIT;
+	        end
+	       endcase
+	end
+endmodule  
\ No newline at end of file
diff --git a/inband_lib/usb_packet_fifo2.v b/inband_lib/usb_packet_fifo2.v
deleted file mode 100755
index d815e4e37..000000000
--- a/inband_lib/usb_packet_fifo2.v
+++ /dev/null
@@ -1,119 +0,0 @@
-`default_nettype none
-
-module usb_packet_fifo2(reset, usb_clock, fpga_clock, write_enable, write_data, 
-        read_enable, skip_packet, read_data, have_space, pkt_waiting, tx_empty) ;
-    
-    /* Module parameters */
-    parameter                       LOG2_N          =   2 ;
-    parameter                       BUS_WIDTH       =   16 ;
-    parameter                       FIFO_WIDTH      =   32 ;
-
-    input   wire                    reset;
-    input   wire                    usb_clock ;
-    input   wire                    fpga_clock ;
-    input   wire                    write_enable ;
-    input   wire    [BUS_WIDTH-1:0] write_data ;
-    input   wire                    read_enable ;
-    input   wire                    skip_packet ;
-    output  wire   [FIFO_WIDTH-1:0] read_data ;
-    output  wire                    have_space ;
-    output  wire                    pkt_waiting ;
-    output  wire                    tx_empty;
-
-    
-    /* Variable for generate statement */
-    genvar i ;
-    
-    /* Local wires for FIFO connections */
-    wire                      [2**LOG2_N-1:0]     fifo_resets ;
-    reg                       [2**LOG2_N-1:0]     fifo_we ;
-    wire                      [2**LOG2_N-1:0]     fifo_re ;
-    reg                       [FIFO_WIDTH-1:0]    fifo_wdata[2**LOG2_N-1:0] ;
-    wire                      [FIFO_WIDTH-1:0]    fifo_rdata[2**LOG2_N-1:0] ;
-    wire                      [2**LOG2_N-1:0]     fifo_rempty ;
-    wire                      [2**LOG2_N-1:0]     fifo_rfull ;
-    wire                      [2**LOG2_N-1:0]     fifo_wempty ;
-    wire                      [2**LOG2_N-1:0]     fifo_wfull ;
-    
-    /* FIFO Select for read and write ports */
-    reg     [LOG2_N-1:0]            fifo_rselect ;
-    reg     [LOG2_N-1:0]            fifo_wselect ;
-    
-    /* Used to convert 16 bits usbdata to the 32 bits wide fifo */
-    reg                             word_complete ;
-    reg     [BUS_WIDTH-1:0]         write_data_delayed ;
-    
-    /* Assign have_space to empty flag of currently selected write FIFO */
-    assign have_space = fifo_wempty[fifo_wselect] ;
-    
-    /* Assign pkt_waiting to full flag of currently selected read FIFO */
-    assign pkt_waiting = fifo_rfull[fifo_rselect] ;
-    
-    /* Assign the read_data to the output of the currently selected FIFO */
-    assign read_data = fifo_rdata[fifo_rselect] ;
-    
-    /* Figure out if we're all empty */
-    assign tx_empty = !(~fifo_rempty) ;
-    
-    /* Increment fifo_rselect here */
-    always @(posedge fpga_clock)
-    begin
-        if (reset)
-            fifo_rselect <= {2**LOG2_N{1'b0}} ;
-        
-        if (fifo_rempty[fifo_rselect])
-            fifo_rselect <= fifo_rselect + 1 ;
-            
-        if (skip_packet)
-            fifo_rselect <= fifo_rselect + 1 ;
-    end
-    
-    /* Increment fifo_wselect and pack data into 32 bits block  */
-    always @(posedge usb_clock, reset)
-    begin
-        if (reset)
-          begin
-            fifo_wselect <= {2**LOG2_N{1'b0}} ;
-            word_complete <= 0;
-          end
-            
-        if (fifo_wfull[fifo_wselect])
-            fifo_wselect <= fifo_wselect + 1 ;
-            
-        if (write_enable)
-          begin
-            word_complete <= ~word_complete ;
-            
-            if (word_complete)
-                fifo_wdata[fifo_wselect] <= {write_data_delayed, write_data} ;
-            else
-                write_data_delayed <= write_data ;
-              
-            /* Avoid to continue to write in the previous fifo when we have 
-               just swichted to the next one */ 
-            fifo_we[fifo_wselect-1] <= 0 ;
-            
-            fifo_we[fifo_wselect] <= write_enable & word_complete ;
-          end
-    end
-    
-    /* Generate all the single packet FIFOs */
-    generate
-        for( i = 0 ; i < 2**LOG2_N ; i = i + 1 )
-        begin : generate_single_packet_fifos
-            assign fifo_re[i] = (fifo_rselect == i) ? read_enable : 1'b0 ;
-            assign fifo_resets[i] = (fifo_rselect == i) ? skip_packet : 1'b0 ;
-            fifo_512 single_packet_fifo(.wrclk  ( usb_clock      ),
-                                        .rdclk  ( fpga_clock     ),
-                                        .aclr   ( fifo_resets[i] ), 
-                                        .wrreq  ( fifo_we[i]     ),
-                                        .data   ( fifo_wdata[i]  ),
-                                        .rdreq  ( fifo_re[i]     ),
-                                        .q      ( fifo_rdata[i]  ),
-                                        .rdfull ( fifo_rfull[i]  ),
-                                        .rdempty( fifo_rempty[i] ),
-                                        .wrfull ( fifo_wfull[i]  ),
-                                        .wrempty( fifo_wempty[i] ) ) ;
-        end
-    endgenerate
-endmodule 
\ No newline at end of file