Commit 80f4866b authored by Andrey Filippov's avatar Andrey Filippov
Browse files

continue on ahci_fis_transmit.v

parent f81326a9
Loading
Loading
Loading
Loading
+37 −1
Original line number Diff line number Diff line
@@ -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);
@@ -279,11 +290,20 @@ 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 #(
+1 −1
Original line number Diff line number Diff line
@@ -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;
+31 −3
Original line number Diff line number Diff line
@@ -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;
@@ -173,6 +185,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;
        else if (dma_in_start)      dma_in <= 1;
@@ -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

+145 −29
Original line number Diff line number Diff line
@@ -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,9 +165,21 @@ 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 ch_ctba =  ch_ctba_r[31:7];

    assign reg_re_w = fetch_chead || chead_bsy_re;
    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
@@ -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;
       
/*
    reg                [3:0] fetch_chead_r;
    reg                [3:0] fetch_chead_stb_r;
    reg                      chead_bsy; // busy reading command header
       // 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;
        
       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