continue on ahci_fis_transmit.v

utils/ahci_dma.v

@@ -46,6 +46,7 @@ module  ahci_dma (
     input                  [15:0] prdtl,        // number of entries in PRD table (valid at cmd_start)
     input                         dev_wr,       // write to device (valid at start)
     input                         cmd_start,     // start processing command table, reset prdbc
+    input                         prd_start,     // at or after cmd_start - enable reading PRD/data (if any)
     input                         cmd_abort,     // try to abort a command TODO: Implement
 
 // Optional control of the AXI cache mode, default will be set to 4'h3, 4'h3 at mrst
@@ -145,9 +146,16 @@ module  ahci_dma (
     reg     [31:7] ctba_r;
     reg     [15:0] prdtl_mclk;
     wire           cmd_start_hclk;
+    reg            prd_start_r;
+    wire           prd_start_hclk;
+    reg            prd_start_hclk_r; // to make sure it is with/after prd_start_hclk if in mclk they are in the same cycle
     wire           cmd_abort_hclk; // TODO: Implement as graceful as possible command abort
+    reg            prd_enabled;
+    reg      [1:0] ct_over_prd_enabled; // prd read and data r/w enabled, command table fetch done
+    
     reg     [31:4] ct_maddr; // granularity matches PRDT entry - 4xDWORD, 2xQWORD
     wire           ct_done;
+    wire           first_prd_fetch; // CT read done, prd enabled
     reg     [31:0] afi_addr; // common for afi_araddr and afi_awaddr
     wire           axi_set_raddr_ready = !(|afi_racount[2:1]) && (!axi_set_raddr_r || !afi_racount[0]); // What is the size of ra fifo - just 4? Latency?
 //    wire           axi_set_raddr_ready = !(|afi_racount) && !axi_set_raddr_r); // Most pessimistic
@@ -198,7 +206,9 @@ module  ahci_dma (
     
     reg            data_next_burst;
     
-    wire           raddr_prd_rq = (|prds_left) && (ct_done || prd_done);
+//    wire           raddr_prd_rq = (|prds_left) && (ct_done || prd_done);
+    wire           raddr_prd_rq = (|prds_left) && (first_prd_fetch || prd_done);
+    
     reg            raddr_prd_pend;
             
     wire           raddr_ct_rq = cmd_start_hclk;
@@ -222,6 +232,7 @@ module  ahci_dma (
     assign prd_irq = data_irq && prd_done;
     assign cmd_done_hclk = ((ct_busy_r==2'b10) && (prdtl_mclk == 0)) || done_flush || done_dev_rd;
     assign ct_done = (ct_busy_r == 2'b10);
+    assign first_prd_fetch = ct_over_prd_enabled == 2'b01;
     assign axi_set_raddr_w = axi_set_raddr_ready && (raddr_ct_pend || raddr_prd_pend || raddr_data_pend);    
     assign axi_set_waddr_w = axi_set_raddr_ready && raddr_data_pend;    
     assign axi_set_addr_data_w = (axi_set_raddr_ready && raddr_data_pend) || (axi_set_waddr_ready && waddr_data_pend);
@@ -278,12 +289,21 @@ module  ahci_dma (
 
         if      (mrst)                  afi_awcache <= 4'h3;
         else if (set_axi_wr_cache_mode) afi_awcache <= axi_wr_cache_mode;
+        
+        prd_start_r <= prd_start;
 
     end
        
         
         
     always @ (posedge hclk) begin
+        prd_start_hclk_r <= prd_start_hclk;
+        
+        if      (hrst || cmd_abort_hclk) prd_enabled <= 0;
+        else if (prd_start_hclk_r)       prd_enabled <= 1; // presedence over  cmd_start_hclk
+        else if (cmd_start_hclk)         prd_enabled <= 0;
+    
+    
         if (cmd_start_hclk)  ct_maddr[31:4] <= {ctba_r[31:7],3'b0};
         else if (ct_done)    ct_maddr[31:4] <= ct_maddr[31:4] + 16;
         else if (wcount_set) ct_maddr[31:4] <= ct_maddr[31:4] + 1;
@@ -364,6 +384,10 @@ module  ahci_dma (
         else if (afi_rd_ctl[0] && is_ct_addr && (&int_data_addr)) ct_busy_r[0] <= 0;
         ct_busy_r[1] <= ct_busy_r[0]; // delayed version to detect end of command
         
+        if (hrst || ct_busy_r[0])                   ct_over_prd_enabled[0] <= 0;
+        else if (prd_enabled)                       ct_over_prd_enabled[0] <= 1;
+        ct_over_prd_enabled[1] <= ct_over_prd_enabled[0];  // detecting 0->1 transition
+        
         // generate busy for PRD table entry read
         if      (hrst)        prd_rd_busy <= 0;
         else if (prd_rd_busy) prd_rd_busy <= 1;
@@ -475,6 +499,18 @@ module  ahci_dma (
         .out_pulse (cmd_abort_hclk),    // output
         .busy()                       // output
     );
+    pulse_cross_clock #(
+        .EXTRA_DLY(0)
+    ) prd_start_hclk_i (
+        .rst       (mrst),            // input
+        .src_clk   (mclk),            // input
+        .dst_clk   (hclk),            // input
+        .in_pulse  (prd_start_r),     // input
+        .out_pulse (prd_start_hclk),  // output
+        .busy()                       // output
+    );
+
+
     
     // hclk -> mclk;
     pulse_cross_clock #(

utils/ahci_dma_rd_stuff.v

@@ -45,7 +45,7 @@ module  ahci_dma_rd_stuff(
     output reg        flushed,  // flush (end of last PRD is finished - data left module)
     output reg [31:0] dout,     // output 32-bit data
     output            dout_vld, // output data valid
-    input             dout_re   // consumer reads output data (should be anded with dout_vld)
+    input             dout_re   // consumer reads output data (should be AND-ed with dout_vld)
 );
     reg  [15:0] hr; // holds 16-bit data from previous din_re if not consumed
     reg         hr_full;

utils/ahci_fis_receive.v

@@ -49,11 +49,17 @@ module  ahci_fis_receive#(
     output reg                    fis_ok,        // FIS done,  checksum OK reset by starting a new get FIS
     output reg                    fis_err,       // FIS done, checksum ERROR reset by starting a new get FIS
     output                        fis_ferr,      // FIS done, fatal error - FIS too long
+    // next commands use register address/data/we for 1 clock cycle - after next to command (commnd - t0, we - t2)
     input                         update_err_sts,// update PxTFD.STS and PxTFD.ERR from the last received regs d2h
+    input                         update_prdbc,  // update PRDBC in registers
     input                         clear_bsy_drq, // clear PxTFD.STS.BSY and PxTFD.STS.DRQ, update
     input                         set_bsy,       // set PxTFD.STS.BSY, update
     input                         set_sts_7f,    // set PxTFD.STS = 0x7f, update
     input                         set_sts_80,    // set PxTFD.STS = 0x80 (may be combined with set_sts_7f), update
+    
+    input                         decr_dwc,      // decrement DMA Xfer counter // need pulse to 'update_prdbc' to write to registers
+    input                  [11:2] decr_DXC_dw,   // decrement value (in DWORDs)
+    
     // TODO: Add writing PRDBC here?
     
     output                  [7:0] tfd_sts,       // Current PxTFD status field (updated after regFIS and SDB - certain fields)
@@ -66,7 +72,9 @@ module  ahci_fis_receive#(
     output reg                    pio_i,         // value of "I" field in received PIO Setup FIS
     output reg                    pio_d,         // value of "D" field in received PIO Setup FIS
     output reg              [7:0] pio_es,        // value of PIO E_Status
-    output reg             [31:1] xfer_cntr,     // transfer counter in words for both DMA (31 bit) and PIO (lower 15 bits)
+    // Using even word count (will be rounded up), partial DWORD (last) will be handled by PRD length if needed
+    output                 [31:2] xfer_cntr,     // transfer counter in words for both DMA (31 bit) and PIO (lower 15 bits), updated after decr_dwc
+    output reg                    xfer_cntr_zero,// valid next cycle                   
 
 // Registers interface
 // 2. HBA R/W registers, may be added external register layer
@@ -96,6 +104,7 @@ FB_OFFS =  0xc00 # Needs 0x100 bytes
 
 
 */
+localparam CLB_OFFS32 =        'h200; //  # In the second half of the register space (0x800..0xbff - 1KB)
 localparam HBA_OFFS32 =         0;
 localparam HBA_PORT0_OFFS32  = 'h40;
 localparam PXSIG_OFFS32 = HBA_OFFS32 + HBA_PORT0_OFFS32 + 'h9; 
@@ -153,10 +162,13 @@ localparam DATA_TYPE_ERR =      3;
     reg           [4:0] reg_ps;
     reg                 reg_d2h;    
     reg                 reg_sdb;    
+    reg          [31:2] xfer_cntr_r;
+    reg          [31:2] prdbc_r;
     
 
     reg          [15:0] tf_err_sts;
     reg                 update_err_sts_r;
+    reg                 update_prdbc_r;
     
     // Forward data to DMA (dev->mem) engine
     assign              dma_in_valid = dma_in_ready && (hda_data_in_type == DATA_TYPE_DMA) && data_in_ready && !too_long_err;
@@ -172,6 +184,9 @@ localparam DATA_TYPE_ERR =      3;
     
     assign tfd_sts = tf_err_sts[ 7:0];
     assign tfd_err = tf_err_sts[15:8];
+    
+    assign xfer_cntr = xfer_cntr_r[31:2];
+    
      
     always @ (posedge mclk) begin
         if (hba_rst || dma_in_stop) dma_in <= 0;
@@ -248,20 +263,23 @@ localparam DATA_TYPE_ERR =      3;
         if (reg_we_w)                         reg_data[31:8] <= hda_data_in[31:8];
         else if (update_sig[1])               reg_data[31:8] <= hda_data_in[23:0];
         else if (update_err_sts_r)            reg_data[31:8] <= {16'b0,tf_err_sts[15:8]};
+        else if (update_prdbc_r)              reg_data[31:8] <= {xfer_cntr_r[31:8]};
 
         if (reg_we_w)                         reg_data[ 7:0] <=  hda_data_in[ 7:0];
         else if (update_sig[3])               reg_data[ 7:0] <=  hda_data_in[ 7:0];
         else if (update_err_sts_r)            reg_data[ 7:0] <=  tf_err_sts [ 7:0];
+        else if (update_prdbc_r)              reg_data[ 7:0] <=  {xfer_cntr_r[ 7:2],2'b0};
         
         if (reg_d2h || update_sig[0])         tf_err_sts  <= hda_data_in[15:0];
         else if (reg_sdb)                     tf_err_sts  <= {hda_data_in[15:8], tf_err_sts[7], hda_data_in[6:4], tf_err_sts[3],hda_data_in[2:0]};
         else if (clear_bsy_drq || set_bsy)    tf_err_sts  <= tf_err_sts & {8'hff,clear_bsy_drq,3'h7,clear_bsy_drq,3'h7} | {8'h0,set_bsy,7'h0};
         else if (set_sts_7f || set_sts_80)    tf_err_sts  <= {tf_err_sts[15:8],set_sts_80,{7{set_sts_7f}}} ;
         
-        reg_we <= reg_we_w || update_sig[3] || update_err_sts_r;
+        reg_we <= reg_we_w || update_sig[3] || update_err_sts_r || update_prdbc_r;
         
         if (reg_we_w || update_sig[3])        reg_addr <=  reg_addr_r;
         else if (update_err_sts_r)            reg_addr <=  PXTFD_OFFS32;
+        else if (update_prdbc_r)              reg_addr <=  CLB_OFFS32 + 1; // location of PRDBC
 
         if (reg_d2h || reg_sdb || reg_ds[0])  fis_i <=           hda_data_in[14];
         if (reg_sdb)                          sdb_n <=           hda_data_in[15];
@@ -272,9 +290,19 @@ localparam DATA_TYPE_ERR =      3;
         if (hba_rst)                          pio_es  <=         0;
         else if (reg_ps[3])                   pio_es  <=         hda_data_in[31:24];
         
-        if (reg_ps[4] || reg_ds[5])           xfer_cntr[31:1] <= {reg_ds[5]?hda_data_in[31:16]:16'b0,hda_data_in[15:1]};
+        if (hba_rst || reg_sdb)               xfer_cntr_r[31:2] <= 0;
+        else if (reg_ps[4] || reg_ds[5])      xfer_cntr_r[31:2] <= {reg_ds[5]?hda_data_in[31:16]:16'b0, hda_data_in[15:2]} + hda_data_in[1]; // round up
+        else if (decr_dwc)                    xfer_cntr_r[31:2] <= {xfer_cntr_r[31:2]} - {20'b0, decr_DXC_dw[11:2]};
+        
+        if (hba_rst || reg_sdb || reg_ps[4] || reg_ds[5])  prdbc_r[31:2] <= 0;
+        else if (decr_dwc)                                 prdbc_r[31:2] <= {prdbc_r[31:2]} + {20'b0, decr_DXC_dw[11:2]};
+        
+        
+        
+        xfer_cntr_zero <=                     xfer_cntr_r[31:2] == 0;
         
         update_err_sts_r <= update_err_sts || clear_bsy_drq || set_bsy || set_sts_7f || set_sts_80;
+        update_prdbc_r <= update_prdbc; // same latency as update_err_sts
         
     end
 

utils/ahci_fis_transmit.v

@@ -21,14 +21,28 @@
 `timescale 1ns/1ps
 
 module  ahci_fis_transmit #(
-    parameter READ_REG_LATENCY =  2 // 0 if  reg_rdata is available with reg_re/reg_addr
+    parameter READ_REG_LATENCY =  2, // 0 if  reg_rdata is available with reg_re/reg_addr
+    parameter READ_CT_LATENCY =   2, // 0 if  reg_rdata is available with reg_re/reg_addr
+    parameter ADDRESS_BITS =     10 // number of memory address bits - now fixed. Low half - RO/RW/RWC,RW1 (2-cycle write), 2-nd just RW (single-cycle)
 
 )(
     input                         hba_rst, // @posedge mclk - sync reset
     input                         mclk, // for command/status
     
     
-    input                         fetch_chead, // fetch command header (from the register memory
+    input                         fetch_cmd,   // Enter p:FetchCmd, fetch command header (from the register memory, prefetch command FIS)
+                                               // wait for either fetch_cmd_busy == 0 or pCmdToIssue ==1 after fetch_cmd
+    output                        pCmdToIssue, // AHCI port variable
+    output                        dmaCntrZero, // DmA counter is zero (first command)
+    output reg                    fetch_cmd_busy, // does not include prefetching CT
+//    input                         fetch_ct,    // fetch command table (ch_ctba[31:7] should be valid by now)
+    input                         cfis_xmit,    // transmit command (wait for dma_ct_busy == 0)
+    input                         dx_transmit,  // send FIS header DWORD, (just 0x46), then forward DMA data
+                                                // transmit until error, 2048DWords or pDmaXferCnt 
+    input                         syncesc_recv, // These two inputs interrupt transmit
+    input                         xmit_err,     //
+    
+    
     output                 [15:0] ch_prdtl,    // Physical region descriptor table length (in entries, 0 is 0)
     output                        ch_c,        // Clear busy upon R_OK for this FIS
     output                        ch_b,        // Built-in self test command
@@ -38,37 +52,62 @@ module  ahci_fis_transmit #(
     output                        ch_a,        // ATAPI: 1 means device should send PIO setup FIS for ATAPI command
     output                  [4:0] ch_cfl,      // length of the command FIS in DW, 0 means none. 0 and 1 - illegal,
                                                // maximal is 16 (0x10)
-    output                 [31:7] ch_ctba,     // command table base address                                       
+//    output                 [31:7] ch_ctba,     // command table base address - use   reg_rdata[31:7] - outside                                    
     
 
     // register memory interface
     output reg [ADDRESS_BITS-1:0] reg_addr,      
     output                        reg_re,
-    output reg             [31:0] reg_rdata,
+    input                  [31:0] reg_rdata,
+
+
+    // ahci_fis_receive interface
+    input                  [31:2] xfer_cntr,     // transfer counter in words for both DMA (31 bit) and PIO (lower 15 bits), updated after decr_dwc
+
+
+    output                        dma_ctba_ld,   // load command table base address
+    output                        dma_start,     // start processing command table, reset prdbc (next cycle after dma_ctba_ld, bits prdtl valid)
+    output                        dma_dev_wr,    // write to device (valid at start)
+    input                         dma_ct_busy,   // dma module is busy reading command table from the system memory
+    // issue dma_prd_start same time as dma_start if prefetch enabled, otherwise with cfis_xmit
+    output                        dma_prd_start, // at or after cmd_start - enable reading PRD/data (if any) ch_prdtl should be valid
+    
+//    output                        cmd_abort,   // try to abort a command TODO: Implement
+    
+    
+    // reading out command table data from DMA module
+    output reg             [ 4:0] ct_addr,     // DWORD address
+    output                        ct_re,       //  
+    input                  [31:0] ct_data,     // 
+    
+    
+    
+    
     
     // DMA (memory -> device) interface
     input                  [31:0] dma_out,      // 32-bit data from the DMA module, HBA -> device port
     input                         dma_dav,      // at least one dword is ready to be read from DMA module
-    input                         dma_re,       // read dword from DMA module to the outpu register
+    output                        dma_re,       // read dword from DMA module to the output register
     
     // Data System memory or FIS -> device
     output reg             [31:0] todev_data,     // 32-bit data from the system memory to HBA (dma data)
     output reg             [ 1:0] todev_type,     // 0 - data, 1 - FIS head, 2 - FIS END (make FIS_Last?)
     output                        todev_valid,    // output register full
-    input                         todev_ready     // send FIFO has room for data (>= ? dwords)
+    input                         todev_ready     // send FIFO has room for data (>= 8? dwords)
     
     // Add a possiblity to flush any data to FIFO if error was detected after data went there?
 );
     localparam CLB_OFFS32 = 'h200; //  # In the second half of the register space (0x800..0xbff - 1KB)
- 
+    localparam DATA_FIS =   32'h46;
     reg                 todev_full_r;
     reg                 dma_en_r;
     wire                fis_data_valid;
     wire          [1:0] fis_data_type;
     wire         [31:0] fis_data_out;
     
-    wire                write_or_w = (dma_en_r?dma_dav:fis_data_valid) && todev_ready; // do not fill the buffer if FIFO is not ready
-    wire                fis_out_w =  !dma_en_r && fis_data_valid && todev_ready;
+    wire                write_or_w = (dma_en_r?(dma_dav && todev_ready):fis_data_valid); // do not fill the buffer if FIFO is not ready for DMA,
+                                                                                         // for fis_data_valid - longer latency
+//    wire                fis_out_w =  !dma_en_r && fis_data_valid && todev_ready;
     wire                dma_re_w =    dma_en_r && dma_dav && todev_ready;
 
     reg               [15:0] ch_prdtl_r;
@@ -79,15 +118,40 @@ module  ahci_fis_transmit #(
     reg                      ch_w_r;
     reg                      ch_a_r;
     reg                [4:0] ch_cfl_r;
-    reg               [31:7] ch_ctba_r;                                       
+    reg                [4:0] ch_cfl_out_r;
+//    reg               [31:7] ch_ctba_r;                                       
     reg [READ_REG_LATENCY:0] reg_re_r;
     wire                     reg_re_w; // combined conditions to read register memory
-    wire                     reg_stb =     reg_re_r[READ_REG_LATENCY];
+///    wire                     reg_stb =     reg_re_r[READ_REG_LATENCY];
     wire                     pre_reg_stb = reg_re_r[READ_REG_LATENCY-1];
     reg                [3:0] fetch_chead_r;
-    reg                [2:0] fetch_chead_stb_r;
+    reg                [3:0] fetch_chead_stb_r;
+    wire                     chead_done_w = fetch_chead_stb_r[2]; // done fetching command header
     reg                      chead_bsy;    // busy reading command header
     reg                      chead_bsy_re; // busy sending read command header
+    reg                      pCmdToIssue_r;
+    wire                     clearCmdToIssue; // TODO: assign - clear pCmdToIssue
+//    reg                      fetch_ct_r;
+    reg                      cfis_xmit_pend_r; //
+    reg                      cfis_xmit_start_r; 
+    reg                      cfis_xmit_busy_r; //
+    reg                      dmaCntrZero_r;  // first command 
+//    wire                     start_sync_escape_w = cfis_xmit && ch_r_r; - no, it should be instead of a ct_fetch
+// TODO: Start FIS transmit when all FIS is in FIFO or data and >half(too slow, need minimum to be able to send wait primitive) or less?
+
+    wire                     cfis_xmit_start_w = (dx_transmit || cfis_xmit_pend_r) && !dma_ct_busy && !fetch_cmd_busy; // dma_ct_busy no gaps with fetch_cmd_busy
+    wire                     cfis_xmit_end;
+    
+    wire                     ct_re_w; // next cycle will be ct_re;
+    reg  [READ_CT_LATENCY:0] ct_re_r;
+    wire                     ct_stb = ct_re_r[READ_CT_LATENCY];
+    
+    reg                      fis_dw_first;
+    wire                     fis_dw_last;
+    
+    reg               [11:2] tx_dwords_left;
+    reg                      tx_fis_pend_r; // waiting to send first DWORD of the  H2D data transfer
+    wire                     tx_dma_last_w; // sending last adat word
     
     assign todev_valid = todev_full_r;
     assign dma_re =   dma_re_w;
@@ -101,11 +165,23 @@ module  ahci_fis_transmit #(
     assign ch_w =     ch_w_r;
     assign ch_a =     ch_a_r;
     assign ch_cfl =   ch_cfl_r;
-    assign ch_ctba =  ch_ctba_r[31:7];
-
-    assign reg_re_w = fetch_chead || chead_bsy_re;
+//    assign ch_ctba =  ch_ctba_r[31:7];
 
+    assign reg_re_w = fetch_cmd || chead_bsy_re;
+    assign dma_ctba_ld = fetch_chead_stb_r[2];
+    assign dma_start =   fetch_chead_stb_r[3]; // next cycle after dma_ctba_ld 
+    assign pCmdToIssue = pCmdToIssue_r;
+    assign dmaCntrZero = dmaCntrZero_r;
+    assign ct_re =       ct_re_r[0];
+    assign fis_data_valid = ct_stb; // no wait write to output register 'todev_data', ct_re_r[0] is throttled according to FIFO room availability
+    assign ct_re_w = todev_ready && ((ch_cfl_r[4:1] != 0) || (ch_cfl_r[0] && !ct_re_r[0]));  // Later add more sources
+    assign fis_dw_last = (ch_cfl_out_r == 1);
+    assign fis_data_type = {fis_dw_last, (write_or_w && tx_fis_pend_r) | (fis_dw_first && ct_stb)};
     
+    assign fis_data_out = ({32{tx_fis_pend_r}} & DATA_FIS) | ({32{ct_stb}} & ct_data) ;
+    assign tx_dma_last_w = dma_en_r && dma_re_w && (tx_dwords_left[11:2] == 1);
+    
+
     always @ (posedge mclk) begin
         // Mutliplex between DMA and FIS output to the output routed to transmit FIFO
         // Count bypassing DMA dwords to generate FIS_last condition?
@@ -115,11 +191,11 @@ module  ahci_fis_transmit #(
         
         if (write_or_w)       todev_data <= dma_en_r? dma_out: fis_data_out;
         
-        if      (hba_rst)     todev_type <= 3; // invalid?
-        else if (write_or_w)  todev_type <= dma_en_r? 2'h0 : fis_data_type;
+        if      (hba_rst)     todev_type <= 3; // invalid? - no, now first and last word in command FIS (impossible?)
+        else if (write_or_w)  todev_type <= dma_en_r? {tx_dma_last_w, 1'b0} : fis_data_type;
         
         if (hba_rst)          fetch_chead_r <= 0;
-        else if (fetch_chead) fetch_chead_r <= 1;
+        else if (fetch_cmd) fetch_chead_r <= 1;
         else                  fetch_chead_r <= fetch_chead_r << 1;
         
         if      (hba_rst)                  fetch_chead_stb_r <= 0;
@@ -127,18 +203,18 @@ module  ahci_fis_transmit #(
         else                               fetch_chead_stb_r <= fetch_chead_stb_r << 1;
         
         if      (hba_rst)              chead_bsy <= 0;
-        else if (fetch_chead)          chead_bsy <= 1;
-        else if (fetch_chead_stb_r[2]) chead_bsy <= 0;
+        else if (fetch_cmd)            chead_bsy <= 1;
+        else if (chead_done_w)         chead_bsy <= 0;
 
         if      (hba_rst)              chead_bsy_re <= 0;
-        else if (fetch_chead)          chead_bsy_re <= 1;
+        else if (fetch_cmd)            chead_bsy_re <= 1;
         else if (fetch_chead_r[1])     chead_bsy_re <= 0; // read 3 dwords
         
         if      (hba_rst)              reg_re_r <= 0;
         else if (reg_re_w)             reg_re_r <= 1;
         else                           reg_re_r <= reg_re_r << 1;
         
-        if      (fetch_chead)          reg_addr <= CLB_OFFS32;   // there will be more conditions
+        if      (fetch_cmd)            reg_addr <= CLB_OFFS32;   // there will be more conditions
         else if (reg_re_r[0])          reg_addr <= reg_addr + 1;
         
         // save command header data to registers
@@ -153,15 +229,55 @@ module  ahci_fis_transmit #(
             ch_cfl_r <=   reg_rdata[ 4: 0];
         end
 
-        if (fetch_chead_stb_r[2]) ch_ctba_r[31:7] <= reg_rdata[31:7];
-        
+       if      (hba_rst)         pCmdToIssue_r <= 0;
+       else if (chead_done_w)    pCmdToIssue_r <= 1;
+       else if (clearCmdToIssue) pCmdToIssue_r <= 0;
+       
+       if      (hba_rst)         fetch_cmd_busy <= 0;
+       else if (fetch_cmd)       fetch_cmd_busy <= 1;
+       else if (dma_start)       fetch_cmd_busy <= 0;
+       
+       // fetch and send command fis
+       if (hba_rst || cfis_xmit_start_w) cfis_xmit_pend_r <= 0;
+       else if (cfis_xmit)               cfis_xmit_pend_r <= 1;
         
-/*
-    reg                [3:0] fetch_chead_r;
-    reg                [3:0] fetch_chead_stb_r;
-    reg                      chead_bsy; // busy reading command header
+       cfis_xmit_start_r <= !hba_rst && cfis_xmit_start_w;
+       
+       if      (hba_rst)           cfis_xmit_busy_r <= 0;
+       else if (cfis_xmit_start_r) cfis_xmit_busy_r <= 1;
+       else if (cfis_xmit_end)     cfis_xmit_busy_r <= 0;
+       
+       if      (fetch_chead_stb_r[0])            ch_cfl_r <=   reg_rdata[ 4: 0];  // Will assume that there is room for ... 
+       else if (cfis_xmit_busy_r && ct_re_r[0])  ch_cfl_r <=   ch_cfl_r - 1;
+       
+       // Counting CFIS dwords sent to TL
+       if (cfis_xmit_start_w) ch_cfl_out_r <= ch_cfl_r;
+       else if (ct_stb)       ch_cfl_out_r <=  ch_cfl_out_r - 1;
+       
+       ct_re_r <= {ct_re_r[READ_CT_LATENCY-1:0],ct_re_w};
+       
+       if      (cfis_xmit)   ct_addr <= 0;
+       else if (ch_cfl_r[0]) ct_addr <= ct_addr + 1;
+
+       // first/last dword in FIS
+       if (!cfis_xmit_busy_r) fis_dw_first <= 1;
+       else if (ct_stb)       fis_dw_first <= 0;
+       
+       // TODO: Implement ATAPI command, other FIS to send?
+       
+       // Send Data FIS TODO: abort on errors, and busy (or done) output
+       //    input                         syncesc_recv, // These two inputs interrupt transmit
+       // input                         xmit_err,     //
+       
+       if   (dx_transmit) tx_dwords_left[11:2] <= (|xfer_cntr[31:11])?10'h200:{1'b0,xfer_cntr[10:2]};
+       else if (dma_re_w) tx_dwords_left[11:2] <= tx_dwords_left[11:2] - 1;
 
-*/        
+       // send FIS header
+       if (hba_rst || write_or_w) tx_fis_pend_r <= 0;
+       else if (dx_transmit)      tx_fis_pend_r <= 1;
+       
+       if      (hba_rst ||  tx_dma_last_w)    dma_en_r  <= 0;
+       else if (tx_fis_pend_r &&  write_or_w) dma_en_r  <= 1;
         
     end