Add Tercel PHY reset synchronization
[microwatt.git] / dcache.vhdl
index 1d9cbdaf1df7149f3182c2677ccd101615dd515d..bb93148c3b68f94f3dcdcb16894d36c422b2183e 100644 (file)
@@ -1,12 +1,6 @@
 --
 -- Set associative dcache write-through
 --
--- TODO (in no specific order):
---
--- * See list in icache.vhdl
--- * Complete load misses on the cycle when WB data comes instead of
---   at the end of line (this requires dealing with requests coming in
---   while not idle...)
 --
 library ieee;
 use ieee.std_logic_1164.all;
@@ -31,7 +25,9 @@ entity dcache is
         -- L1 DTLB number of sets
         TLB_NUM_WAYS : positive := 2;
         -- L1 DTLB log_2(page_size)
-        TLB_LG_PGSZ : positive := 12
+        TLB_LG_PGSZ : positive := 12;
+        -- Non-zero to enable log data collection
+        LOG_LENGTH : natural := 0
         );
     port (
         clk          : in std_ulogic;
@@ -46,14 +42,16 @@ entity dcache is
        stall_out    : out std_ulogic;
 
         wishbone_out : out wishbone_master_out;
-        wishbone_in  : in wishbone_slave_out
+        wishbone_in  : in wishbone_slave_out;
+
+        log_out      : out std_ulogic_vector(19 downto 0)
         );
 end entity dcache;
 
 architecture rtl of dcache is
     -- BRAM organisation: We never access more than wishbone_data_bits at
     -- a time so to save resources we make the array only that wide, and
-    -- use consecutive indices for to make a cache "line"
+    -- use consecutive indices to make a cache "line"
     --
     -- ROW_SIZE is the width in bytes of the BRAM (based on WB, so 64-bits)
     constant ROW_SIZE      : natural := wishbone_data_bits / 8;
@@ -81,6 +79,8 @@ architecture rtl of dcache is
     constant SET_SIZE_BITS : natural := LINE_OFF_BITS + INDEX_BITS;
     -- TAG_BITS is the number of bits of the tag part of the address
     constant TAG_BITS      : natural := REAL_ADDR_BITS - SET_SIZE_BITS;
+    -- TAG_WIDTH is the width in bits of each way of the tag RAM
+    constant TAG_WIDTH     : natural := TAG_BITS + 7 - ((TAG_BITS + 7) mod 8);
     -- WAY_BITS is the number of bits to select a way
     constant WAY_BITS     : natural := log2(NUM_WAYS);
 
@@ -98,6 +98,7 @@ architecture rtl of dcache is
     subtype row_t is integer range 0 to BRAM_ROWS-1;
     subtype index_t is integer range 0 to NUM_LINES-1;
     subtype way_t is integer range 0 to NUM_WAYS-1;
+    subtype row_in_line_t is unsigned(ROW_LINEBITS-1 downto 0);
 
     -- The cache data BRAM organized as described above for each way
     subtype cache_row_t is std_ulogic_vector(wishbone_data_bits-1 downto 0);
@@ -108,17 +109,19 @@ architecture rtl of dcache is
     subtype cache_tag_t is std_logic_vector(TAG_BITS-1 downto 0);
 --    type cache_tags_set_t is array(way_t) of cache_tag_t;
 --    type cache_tags_array_t is array(index_t) of cache_tags_set_t;
-    constant TAG_RAM_WIDTH : natural := TAG_BITS * NUM_WAYS;
+    constant TAG_RAM_WIDTH : natural := TAG_WIDTH * NUM_WAYS;
     subtype cache_tags_set_t is std_logic_vector(TAG_RAM_WIDTH-1 downto 0);
     type cache_tags_array_t is array(index_t) of cache_tags_set_t;
 
     -- The cache valid bits
     subtype cache_way_valids_t is std_ulogic_vector(NUM_WAYS-1 downto 0);
     type cache_valids_t is array(index_t) of cache_way_valids_t;
+    type row_per_line_valid_t is array(0 to ROW_PER_LINE - 1) of std_ulogic;
 
     -- Storage. Hopefully "cache_rows" is a BRAM, the rest is LUTs
-    signal cache_tags   : cache_tags_array_t;
-    signal cache_valids : cache_valids_t;
+    signal cache_tags    : cache_tags_array_t;
+    signal cache_tag_set : cache_tags_set_t;
+    signal cache_valids  : cache_valids_t;
 
     attribute ram_style : string;
     attribute ram_style of cache_tags : signal is "distributed";
@@ -175,18 +178,17 @@ architecture rtl of dcache is
 
     -- Type of operation on a "valid" input
     type op_t is (OP_NONE,
+                 OP_BAD,           -- NC cache hit, TLB miss, prot/RC failure
+                  OP_STCX_FAIL,     -- conditional store w/o reservation
                  OP_LOAD_HIT,      -- Cache hit on load
                  OP_LOAD_MISS,     -- Load missing cache
                  OP_LOAD_NC,       -- Non-cachable load
-                 OP_BAD,           -- BAD: Cache hit on NC load/store
-                  OP_TLB_ERR,       -- TLB miss or protection/RC failure
                  OP_STORE_HIT,     -- Store hitting cache
                  OP_STORE_MISS);   -- Store missing cache
                      
     -- Cache state machine
     type state_t is (IDLE,            -- Normal load hit processing
                     RELOAD_WAIT_ACK,  -- Cache reload wait ack
-                     FINISH_LD_MISS,   -- Extra cycle after load miss
                     STORE_WAIT_ACK,   -- Store wait ack
                     NC_LOAD_WAIT_ACK);-- Non-cachable load wait ack
 
@@ -198,58 +200,155 @@ architecture rtl of dcache is
     -- which means that the BRAM output is delayed by an extra cycle.
     --
     -- Thus, the dcache has a 2-stage internal pipeline for cache hits
-    -- with no stalls.
+    -- with no stalls.  Stores also complete in 2 cycles in most
+    -- circumstances.
+    --
+    -- A request proceeds through the pipeline as follows.
+    --
+    -- Cycle 0: Request is received from loadstore or mmu if either
+    -- d_in.valid or m_in.valid is 1 (not both).  In this cycle portions
+    -- of the address are presented to the TLB tag RAM and data RAM
+    -- and the cache tag RAM and data RAM.
+    --
+    -- Clock edge between cycle 0 and cycle 1:
+    -- Request is stored in r0 (assuming r0_full was 0).  TLB tag and
+    -- data RAMs are read, and the cache tag RAM is read.  (Cache data
+    -- comes out a cycle later due to its output register, giving the
+    -- whole of cycle 1 to read the cache data RAM.)
     --
-    -- All other operations are handled via stalling in the first stage.
+    -- Cycle 1: TLB and cache tag matching is done, the real address
+    -- (RA) for the access is calculated, and the type of operation is
+    -- determined (the OP_* values above).  This gives the TLB way for
+    -- a TLB hit, and the cache way for a hit or the way to replace
+    -- for a load miss.
     --
-    -- The second stage can thus complete a hit at the same time as the
-    -- first stage emits a stall for a complex op.
+    -- Clock edge between cycle 1 and cycle 2:
+    -- Request is stored in r1 (assuming r1.full was 0)
+    -- The state machine transitions out of IDLE state for a load miss,
+    -- a store, a dcbz, or a non-cacheable load.  r1.full is set to 1
+    -- for a load miss, dcbz or non-cacheable load but not a store.
     --
+    -- Cycle 2: Completion signals are asserted for a load hit,
+    -- a store (excluding dcbz), a TLB operation, a conditional
+    -- store which failed due to no matching reservation, or an error
+    -- (cache hit on non-cacheable operation, TLB miss, or protection
+    -- fault).
+    --
+    -- For a load miss, store, or dcbz, the state machine initiates
+    -- a wishbone cycle, which takes at least 2 cycles.  For a store,
+    -- if another store comes in with the same cache tag (therefore
+    -- in the same 4k page), it can be added on to the existing cycle,
+    -- subject to some constraints.
+    -- While r1.full = 1, no new requests can go from r0 to r1, but
+    -- requests can come in to r0 and be satisfied if they are
+    -- cacheable load hits or stores with the same cache tag.
+    --
+    -- Writing to the cache data RAM is done at the clock edge
+    -- at the end of cycle 2 for a store hit (excluding dcbz).
+    -- Stores that miss are not written to the cache data RAM
+    -- but just stored through to memory.
+    -- Dcbz is done like a cache miss, but the wishbone cycle
+    -- is a write rather than a read, and zeroes are written to
+    -- the cache data RAM.  Thus dcbz will allocate the line in
+    -- the cache as well as zeroing memory.
+    --
+    -- Since stores are written to the cache data RAM at the end of
+    -- cycle 2, and loads can come in and hit on the data just stored,
+    -- there is a two-stage bypass from store data to load data to
+    -- make sure that loads always see previously-stored data even
+    -- if it has not yet made it to the cache data RAM.
+    --
+    -- Load misses read the requested dword of the cache line first in
+    -- the memory read request and then cycle around through the other
+    -- dwords.  The load is completed on the cycle after the requested
+    -- dword comes back from memory (using a forwarding path, rather
+    -- than going via the cache data RAM).  We maintain an array of
+    -- valid bits per dword for the line being refilled so that
+    -- subsequent load requests to the same line can be completed as
+    -- soon as the necessary data comes in from memory, without
+    -- waiting for the whole line to be read.
 
     -- Stage 0 register, basically contains just the latched request
     type reg_stage_0_t is record
         req   : Loadstore1ToDcacheType;
-        tlbie : std_ulogic;
-        doall : std_ulogic;
-        tlbld : std_ulogic;
+        tlbie : std_ulogic;     -- indicates a tlbie request (from MMU)
+        doall : std_ulogic;     -- with tlbie, indicates flush whole TLB
+        tlbld : std_ulogic;     -- indicates a TLB load request (from MMU)
         mmu_req : std_ulogic;   -- indicates source of request
+        d_valid : std_ulogic;   -- indicates req.data is valid now
     end record;
 
     signal r0 : reg_stage_0_t;
-    signal r0_valid : std_ulogic;
-    
+    signal r0_full : std_ulogic;
+
+    type mem_access_request_t is record
+        op        : op_t;
+        valid     : std_ulogic;
+        dcbz      : std_ulogic;
+        real_addr : std_ulogic_vector(REAL_ADDR_BITS - 1 downto 0);
+        data      : std_ulogic_vector(63 downto 0);
+        byte_sel  : std_ulogic_vector(7 downto 0);
+        hit_way   : way_t;
+        same_tag  : std_ulogic;
+        mmu_req   : std_ulogic;
+    end record;
+
     -- First stage register, contains state for stage 1 of load hits
     -- and for the state machine used by all other operations
     --
     type reg_stage_1_t is record
-       -- Latch the complete request from ls1
-       req              : Loadstore1ToDcacheType;
-        mmu_req          : std_ulogic;
+        -- Info about the request
+        full             : std_ulogic;          -- have uncompleted request
+        mmu_req          : std_ulogic;          -- request is from MMU
+        req              : mem_access_request_t;
 
        -- Cache hit state
        hit_way          : way_t;
        hit_load_valid   : std_ulogic;
-
-       -- Data buffer for "slow" read ops (load miss and NC loads).
-       slow_data        : std_ulogic_vector(63 downto 0);
-       slow_valid       : std_ulogic;
-
-        -- Signal to complete a failed stcx.
-        stcx_fail        : std_ulogic;
+        hit_index        : index_t;
+        cache_hit        : std_ulogic;
+
+        -- TLB hit state
+        tlb_hit          : std_ulogic;
+        tlb_hit_way      : tlb_way_t;
+        tlb_hit_index    : tlb_index_t;
+
+       -- 2-stage data buffer for data forwarded from writes to reads
+       forward_data1    : std_ulogic_vector(63 downto 0);
+       forward_data2    : std_ulogic_vector(63 downto 0);
+        forward_sel1     : std_ulogic_vector(7 downto 0);
+       forward_valid1   : std_ulogic;
+        forward_way1     : way_t;
+        forward_row1     : row_t;
+        use_forward1     : std_ulogic;
+        forward_sel      : std_ulogic_vector(7 downto 0);
 
        -- Cache miss state (reload state machine)
         state            : state_t;
+        dcbz             : std_ulogic;
+        write_bram       : std_ulogic;
+        write_tag        : std_ulogic;
+        slow_valid       : std_ulogic;
         wb               : wishbone_master_out;
+        reload_tag       : cache_tag_t;
        store_way        : way_t;
        store_row        : row_t;
         store_index      : index_t;
-
-        -- Signals to complete with error
-        error_done       : std_ulogic;
+        end_row_ix       : row_in_line_t;
+        rows_valid       : row_per_line_valid_t;
+        acks_pending     : unsigned(2 downto 0);
+        inc_acks         : std_ulogic;
+        dec_acks         : std_ulogic;
+
+        -- Signals to complete (possibly with error)
+        ls_valid         : std_ulogic;
+        ls_error         : std_ulogic;
+        mmu_done         : std_ulogic;
+        mmu_error        : std_ulogic;
         cache_paradox    : std_ulogic;
 
-        -- completion signal for tlbie
-        tlbie_done       : std_ulogic;
+        -- Signal to complete a failed stcx.
+        stcx_fail        : std_ulogic;
     end record;
 
     signal r1 : reg_stage_1_t;
@@ -270,7 +369,8 @@ architecture rtl of dcache is
     signal req_tag     : cache_tag_t;
     signal req_op      : op_t;
     signal req_data    : std_ulogic_vector(63 downto 0);
-    signal req_laddr   : std_ulogic_vector(63 downto 0);
+    signal req_same_tag : std_ulogic;
+    signal req_go      : std_ulogic;
 
     signal early_req_row  : row_t;
 
@@ -278,6 +378,12 @@ architecture rtl of dcache is
     signal set_rsrv     : std_ulogic;
     signal clear_rsrv   : std_ulogic;
 
+    signal r0_valid   : std_ulogic;
+    signal r0_stall   : std_ulogic;
+
+    signal use_forward1_next : std_ulogic;
+    signal use_forward2_next : std_ulogic;
+
     -- Cache RAM interface
     type cache_ram_out_t is array(way_t) of cache_row_t;
     signal cache_out   : cache_ram_out_t;
@@ -303,6 +409,7 @@ architecture rtl of dcache is
     signal perm_attr : perm_attr_t;
     signal rc_ok : std_ulogic;
     signal perm_ok : std_ulogic;
+    signal access_ok : std_ulogic;
 
     -- TLB PLRU output interface
     type tlb_plru_out_t is array(tlb_index_t) of std_ulogic_vector(TLB_WAY_BITS-1 downto 0);
@@ -313,31 +420,35 @@ architecture rtl of dcache is
     --
 
     -- Return the cache line index (tag index) for an address
-    function get_index(addr: std_ulogic_vector(63 downto 0)) return index_t is
+    function get_index(addr: std_ulogic_vector) return index_t is
     begin
         return to_integer(unsigned(addr(SET_SIZE_BITS - 1 downto LINE_OFF_BITS)));
     end;
 
     -- Return the cache row index (data memory) for an address
-    function get_row(addr: std_ulogic_vector(63 downto 0)) return row_t is
+    function get_row(addr: std_ulogic_vector) return row_t is
     begin
         return to_integer(unsigned(addr(SET_SIZE_BITS - 1 downto ROW_OFF_BITS)));
     end;
 
+    -- Return the index of a row within a line
+    function get_row_of_line(row: row_t) return row_in_line_t is
+       variable row_v : unsigned(ROW_BITS-1 downto 0);
+    begin
+       row_v := to_unsigned(row, ROW_BITS);
+        return row_v(ROW_LINEBITS-1 downto 0);
+    end;
+
     -- Returns whether this is the last row of a line
-    function is_last_row_addr(addr: wishbone_addr_type) return boolean is
-       constant ones : std_ulogic_vector(ROW_LINEBITS-1 downto 0) := (others => '1');
+    function is_last_row_addr(addr: wishbone_addr_type; last: row_in_line_t) return boolean is
     begin
-       return addr(LINE_OFF_BITS-1 downto ROW_OFF_BITS) = ones;
+       return unsigned(addr(LINE_OFF_BITS-1 downto ROW_OFF_BITS)) = last;
     end;
 
     -- Returns whether this is the last row of a line
-    function is_last_row(row: row_t) return boolean is
-       variable row_v : std_ulogic_vector(ROW_BITS-1 downto 0);
-       constant ones  : std_ulogic_vector(ROW_LINEBITS-1 downto 0) := (others => '1');
+    function is_last_row(row: row_t; last: row_in_line_t) return boolean is
     begin
-       row_v := std_ulogic_vector(to_unsigned(row, ROW_BITS));
-       return row_v(ROW_LINEBITS-1 downto 0) = ones;
+        return get_row_of_line(row) = last;
     end;
 
     -- Return the address of the next row in the current cache line
@@ -369,7 +480,7 @@ architecture rtl of dcache is
     end;
 
     -- Get the tag value from the address
-    function get_tag(addr: std_ulogic_vector(REAL_ADDR_BITS - 1 downto 0)) return cache_tag_t is
+    function get_tag(addr: std_ulogic_vector) return cache_tag_t is
     begin
         return addr(REAL_ADDR_BITS - 1 downto SET_SIZE_BITS);
     end;
@@ -377,14 +488,7 @@ architecture rtl of dcache is
     -- Read a tag from a tag memory row
     function read_tag(way: way_t; tagset: cache_tags_set_t) return cache_tag_t is
     begin
-       return tagset((way+1) * TAG_BITS - 1 downto way * TAG_BITS);
-    end;
-
-    -- Write a tag to tag memory row
-    procedure write_tag(way: in way_t; tagset: inout cache_tags_set_t;
-                       tag: cache_tag_t) is
-    begin
-       tagset((way+1) * TAG_BITS - 1 downto way * TAG_BITS) := tag;
+       return tagset(way * TAG_WIDTH + TAG_BITS - 1 downto way * TAG_WIDTH);
     end;
 
     -- Read a TLB tag from a TLB tag memory row
@@ -435,38 +539,52 @@ begin
        report "geometry bits don't add up" severity FAILURE;
     assert (64 = wishbone_data_bits)
        report "Can't yet handle a wishbone width that isn't 64-bits" severity FAILURE;
+    assert SET_SIZE_BITS <= TLB_LG_PGSZ report "Set indexed by virtual address" severity FAILURE;
 
     -- Latch the request in r0.req as long as we're not stalling
     stage_0 : process(clk)
+        variable r : reg_stage_0_t;
     begin
         if rising_edge(clk) then
+            assert (d_in.valid and m_in.valid) = '0' report
+                "request collision loadstore vs MMU";
+            if m_in.valid = '1' then
+                r.req.valid := '1';
+                r.req.load := not (m_in.tlbie or m_in.tlbld);
+                r.req.dcbz := '0';
+                r.req.nc := '0';
+                r.req.reserve := '0';
+                r.req.virt_mode := '0';
+                r.req.priv_mode := '1';
+                r.req.addr := m_in.addr;
+                r.req.data := m_in.pte;
+                r.req.byte_sel := (others => '1');
+                r.tlbie := m_in.tlbie;
+                r.doall := m_in.doall;
+                r.tlbld := m_in.tlbld;
+                r.mmu_req := '1';
+            else
+                r.req := d_in;
+                r.req.data := (others => '0');
+                r.tlbie := '0';
+                r.doall := '0';
+                r.tlbld := '0';
+                r.mmu_req := '0';
+            end if;
+            r.d_valid := '0';
             if rst = '1' then
-                r0.req.valid <= '0';
-            elsif stall_out = '0' then
-                assert (d_in.valid and m_in.valid) = '0' report
-                    "request collision loadstore vs MMU";
-                if m_in.valid = '1' then
-                    r0.req.valid <= '1';
-                    r0.req.load <= not (m_in.tlbie or m_in.tlbld);
-                    r0.req.dcbz <= '0';
-                    r0.req.nc <= '0';
-                    r0.req.reserve <= '0';
-                    r0.req.virt_mode <= '0';
-                    r0.req.priv_mode <= '1';
-                    r0.req.addr <= m_in.addr;
-                    r0.req.data <= m_in.pte;
-                    r0.req.byte_sel <= (others => '1');
-                    r0.tlbie <= m_in.tlbie;
-                    r0.doall <= m_in.doall;
-                    r0.tlbld <= m_in.tlbld;
-                    r0.mmu_req <= '1';
-                else
-                    r0.req <= d_in;
-                    r0.tlbie <= '0';
-                    r0.doall <= '0';
-                    r0.tlbld <= '0';
-                    r0.mmu_req <= '0';
-                end if;
+                r0_full <= '0';
+            elsif (r1.full = '0' and d_in.hold = '0') or r0_full = '0' then
+                r0 <= r;
+                r0_full <= r.req.valid;
+            end if;
+            -- Sample data the cycle after a request comes in from loadstore1.
+            -- If another request has come in already then the data will get
+            -- put directly into req.data below.
+            if r0.req.valid = '1' and r.req.valid = '0' and r0.d_valid = '0' and
+                r0.mmu_req = '0' then
+                r0.req.data <= d_in.data;
+                r0.d_valid <= '1';
             end if;
         end if;
     end process;
@@ -474,9 +592,10 @@ begin
     -- we don't yet handle collisions between loadstore1 requests and MMU requests
     m_out.stall <= '0';
 
-    -- Hold off the request in r0 when stalling,
-    -- and cancel it if we get an error in a previous request.
-    r0_valid <= r0.req.valid and not stall_out and not r1.error_done;
+    -- Hold off the request in r0 when r1 has an uncompleted request
+    r0_stall <= r0_full and (r1.full or d_in.hold);
+    r0_valid <= r0_full and not r1.full and not d_in.hold;
+    stall_out <= r0_stall;
 
     -- TLB
     -- Operates in the second cycle on the request latched in r0.req.
@@ -486,20 +605,19 @@ begin
         variable addrbits : std_ulogic_vector(TLB_SET_BITS - 1 downto 0);
     begin
         if rising_edge(clk) then
-            if stall_out = '1' then
-                -- keep reading the same thing while stalled
-                index := tlb_req_index;
+            if m_in.valid = '1' then
+                addrbits := m_in.addr(TLB_LG_PGSZ + TLB_SET_BITS - 1 downto TLB_LG_PGSZ);
             else
-                if m_in.valid = '1' then
-                    addrbits := m_in.addr(TLB_LG_PGSZ + TLB_SET_BITS - 1 downto TLB_LG_PGSZ);
-                else
-                    addrbits := d_in.addr(TLB_LG_PGSZ + TLB_SET_BITS - 1 downto TLB_LG_PGSZ);
-                end if;
-                index := to_integer(unsigned(addrbits));
+                addrbits := d_in.addr(TLB_LG_PGSZ + TLB_SET_BITS - 1 downto TLB_LG_PGSZ);
+            end if;
+            index := to_integer(unsigned(addrbits));
+            -- If we have any op and the previous op isn't finished,
+            -- then keep the same output for next cycle.
+            if r0_stall = '0' then
+                tlb_valid_way <= dtlb_valids(index);
+                tlb_tag_way <= dtlb_tags(index);
+                tlb_pte_way <= dtlb_ptes(index);
             end if;
-            tlb_valid_way <= dtlb_valids(index);
-            tlb_tag_way <= dtlb_tags(index);
-            tlb_pte_way <= dtlb_ptes(index);
         end if;
     end process;
 
@@ -524,15 +642,15 @@ begin
                    lru => tlb_plru_out
                    );
 
-           process(tlb_req_index, tlb_hit, tlb_hit_way, tlb_plru_out)
+           process(all)
            begin
                -- PLRU interface
-               if tlb_hit = '1' and tlb_req_index = i then
-                   tlb_plru_acc_en <= '1';
+               if r1.tlb_hit_index = i then
+                   tlb_plru_acc_en <= r1.tlb_hit;
                else
                    tlb_plru_acc_en <= '0';
                end if;
-               tlb_plru_acc <= std_ulogic_vector(to_unsigned(tlb_hit_way, TLB_WAY_BITS));
+               tlb_plru_acc <= std_ulogic_vector(to_unsigned(r1.tlb_hit_way, TLB_WAY_BITS));
                tlb_plru_victim(i) <= tlb_plru_out;
            end process;
        end generate;
@@ -565,10 +683,12 @@ begin
         valid_ra <= tlb_hit or not r0.req.virt_mode;
         if r0.req.virt_mode = '1' then
             ra <= pte(REAL_ADDR_BITS - 1 downto TLB_LG_PGSZ) &
-                  r0.req.addr(TLB_LG_PGSZ - 1 downto 0);
+                  r0.req.addr(TLB_LG_PGSZ - 1 downto ROW_OFF_BITS) &
+                  (ROW_OFF_BITS-1 downto 0 => '0');
             perm_attr <= extract_perm_attr(pte);
         else
-            ra <= r0.req.addr(REAL_ADDR_BITS - 1 downto 0);
+            ra <= r0.req.addr(REAL_ADDR_BITS - 1 downto ROW_OFF_BITS) &
+                  (ROW_OFF_BITS-1 downto 0 => '0');
             perm_attr <= real_mode_perm_attr;
         end if;
     end process;
@@ -633,50 +753,59 @@ begin
                    lru => plru_out
                    );
 
-           process(req_index, req_op, req_hit_way, plru_out)
+           process(all)
            begin
                -- PLRU interface
-               if (req_op = OP_LOAD_HIT or
-                   req_op = OP_STORE_HIT) and req_index = i then
-                   plru_acc_en <= '1';
+               if r1.hit_index = i then
+                   plru_acc_en <= r1.cache_hit;
                else
                    plru_acc_en <= '0';
                end if;
-               plru_acc <= std_ulogic_vector(to_unsigned(req_hit_way, WAY_BITS));
+               plru_acc <= std_ulogic_vector(to_unsigned(r1.hit_way, WAY_BITS));
                plru_victim(i) <= plru_out;
            end process;
        end generate;
     end generate;
 
+    -- Cache tag RAM read port
+    cache_tag_read : process(clk)
+        variable index : index_t;
+    begin
+        if rising_edge(clk) then
+            if r0_stall = '1' then
+                index := req_index;
+            elsif m_in.valid = '1' then
+                index := get_index(m_in.addr);
+            else
+                index := get_index(d_in.addr);
+            end if;
+            cache_tag_set <= cache_tags(index);
+        end if;
+    end process;
+
     -- Cache request parsing and hit detection
     dcache_request : process(all)
-       variable is_hit  : std_ulogic;
-       variable hit_way : way_t;
-       variable op      : op_t;
-       variable opsel   : std_ulogic_vector(2 downto 0);
-        variable go      : std_ulogic;
-        variable nc      : std_ulogic;
-        variable s_hit   : std_ulogic;
-        variable s_tag   : cache_tag_t;
-        variable s_pte   : tlb_pte_t;
-        variable s_ra    : std_ulogic_vector(REAL_ADDR_BITS - 1 downto 0);
-        variable hit_set : std_ulogic_vector(TLB_NUM_WAYS - 1 downto 0);
+        variable is_hit      : std_ulogic;
+        variable hit_way     : way_t;
+        variable op          : op_t;
+        variable opsel       : std_ulogic_vector(2 downto 0);
+        variable go          : std_ulogic;
+        variable nc          : std_ulogic;
+        variable s_hit       : std_ulogic;
+        variable s_tag       : cache_tag_t;
+        variable s_pte       : tlb_pte_t;
+        variable s_ra        : std_ulogic_vector(REAL_ADDR_BITS - 1 downto 0);
+        variable hit_set     : std_ulogic_vector(TLB_NUM_WAYS - 1 downto 0);
         variable hit_way_set : hit_way_set_t;
+        variable rel_matches : std_ulogic_vector(TLB_NUM_WAYS - 1 downto 0);
+        variable rel_match   : std_ulogic;
     begin
        -- Extract line, row and tag from request
         req_index <= get_index(r0.req.addr);
         req_row <= get_row(r0.req.addr);
         req_tag <= get_tag(ra);
 
-        -- Only do anything if not being stalled by stage 1
-        go := r0_valid and not (r0.tlbie or r0.tlbld);
-
-        -- Calculate address of beginning of cache line, will be
-        -- used for cache miss processing if needed
-        --
-        req_laddr <= (63 downto REAL_ADDR_BITS => '0') &
-                     ra(REAL_ADDR_BITS - 1 downto LINE_OFF_BITS) &
-                     (LINE_OFF_BITS-1 downto 0 => '0');
+        go := r0_valid and not (r0.tlbie or r0.tlbld) and not r1.ls_error;
 
        -- Test if pending request is a hit on any way
         -- In order to make timing in virtual mode, when we are using the TLB,
@@ -684,7 +813,9 @@ begin
         -- the TLB, and then decide later which match to use.
         hit_way := 0;
         is_hit := '0';
+        rel_match := '0';
         if r0.req.virt_mode = '1' then
+            rel_matches := (others => '0');
             for j in tlb_way_t loop
                 hit_way_set(j) := 0;
                 s_hit := '0';
@@ -694,40 +825,80 @@ begin
                 s_tag := get_tag(s_ra);
                 for i in way_t loop
                     if go = '1' and cache_valids(req_index)(i) = '1' and
-                        read_tag(i, cache_tags(req_index)) = s_tag and
+                        read_tag(i, cache_tag_set) = s_tag and
                         tlb_valid_way(j) = '1' then
                         hit_way_set(j) := i;
                         s_hit := '1';
                     end if;
                 end loop;
                 hit_set(j) := s_hit;
+                if s_tag = r1.reload_tag then
+                    rel_matches(j) := '1';
+                end if;
             end loop;
             if tlb_hit = '1' then
                 is_hit := hit_set(tlb_hit_way);
                 hit_way := hit_way_set(tlb_hit_way);
+                rel_match := rel_matches(tlb_hit_way);
             end if;
         else
-            s_tag := get_tag(r0.req.addr(REAL_ADDR_BITS - 1 downto 0));
+            s_tag := get_tag(r0.req.addr);
             for i in way_t loop
                 if go = '1' and cache_valids(req_index)(i) = '1' and
-                    read_tag(i, cache_tags(req_index)) = s_tag then
+                    read_tag(i, cache_tag_set) = s_tag then
                     hit_way := i;
                     is_hit := '1';
                 end if;
             end loop;
+            if s_tag = r1.reload_tag then
+                rel_match := '1';
+            end if;
+        end if;
+        req_same_tag <= rel_match;
+
+        -- See if the request matches the line currently being reloaded
+        if r1.state = RELOAD_WAIT_ACK and req_index = r1.store_index and
+            rel_match = '1' then
+            -- For a store, consider this a hit even if the row isn't valid
+            -- since it will be by the time we perform the store.
+            -- For a load, check the appropriate row valid bit.
+            is_hit := not r0.req.load or r1.rows_valid(req_row mod ROW_PER_LINE);
+            hit_way := replace_way;
+        end if;
+
+        -- Whether to use forwarded data for a load or not
+        use_forward1_next <= '0';
+        if get_row(r1.req.real_addr) = req_row and r1.req.hit_way = hit_way then
+            -- Only need to consider r1.write_bram here, since if we are
+            -- writing refill data here, then we don't have a cache hit this
+            -- cycle on the line being refilled.  (There is the possibility
+            -- that the load following the load miss that started the refill
+            -- could be to the old contents of the victim line, since it is a
+            -- couple of cycles after the refill starts before we see the
+            -- updated cache tag.  In that case we don't use the bypass.)
+            use_forward1_next <= r1.write_bram;
+        end if;
+        use_forward2_next <= '0';
+        if r1.forward_row1 = req_row and r1.forward_way1 = hit_way then
+            use_forward2_next <= r1.forward_valid1;
         end if;
 
        -- The way that matched on a hit               
        req_hit_way <= hit_way;
 
-       -- The way to replace on a miss
-       replace_way <= to_integer(unsigned(plru_victim(req_index)));
+        -- The way to replace on a miss
+        if r1.write_tag = '1' then
+            replace_way <= to_integer(unsigned(plru_victim(r1.store_index)));
+        else
+            replace_way <= r1.store_way;
+        end if;
 
         -- work out whether we have permission for this access
         -- NB we don't yet implement AMR, thus no KUAP
         rc_ok <= perm_attr.reference and (r0.req.load or perm_attr.changed);
         perm_ok <= (r0.req.priv_mode or not perm_attr.priv) and
                    (perm_attr.wr_perm or (r0.req.load and perm_attr.rd_perm));
+        access_ok <= valid_ra and perm_ok and rc_ok;
 
        -- Combine the request and cache hit status to decide what
        -- operation needs to be done
@@ -735,7 +906,11 @@ begin
         nc := r0.req.nc or perm_attr.nocache;
         op := OP_NONE;
         if go = '1' then
-            if valid_ra = '1' and rc_ok = '1' and perm_ok = '1' then
+            if access_ok = '0' then
+                op := OP_BAD;
+            elsif cancel_store = '1' then
+                op := OP_STCX_FAIL;
+            else
                 opsel := r0.req.load & nc & is_hit;
                 case opsel is
                     when "101" => op := OP_LOAD_HIT;
@@ -748,17 +923,16 @@ begin
                     when "111" => op := OP_BAD;
                     when others => op := OP_NONE;
                 end case;
-            else
-                op := OP_TLB_ERR;
             end if;
         end if;
        req_op <= op;
+        req_go <= go;
 
         -- Version of the row number that is valid one cycle earlier
         -- in the cases where we need to read the cache data BRAM.
         -- If we're stalling then we need to keep reading the last
         -- row requested.
-        if stall_out = '0' then
+        if r0_stall = '0' then
             if m_in.valid = '1' then
                 early_req_row <= get_row(m_in.addr);
             else
@@ -772,9 +946,6 @@ begin
     -- Wire up wishbone request latch out of stage 1
     wishbone_out <= r1.wb;
 
-    -- Generate stalls from stage 1 state machine
-    stall_out <= '1' when r1.state /= IDLE else '0';
-
     -- Handle load-with-reservation and store-conditional instructions
     reservation_comb: process(all)
     begin
@@ -786,10 +957,10 @@ begin
             -- XXX or if r0.req.nc = '1'
             if r0.req.load = '1' then
                 -- load with reservation
-                set_rsrv <= '1';
+                set_rsrv <= r0.req.atomic_last;
             else
                 -- store conditional
-                clear_rsrv <= '1';
+                clear_rsrv <= r0.req.atomic_last;
                 if reservation.valid = '0' or
                     r0.req.addr(63 downto LINE_OFF_BITS) /= reservation.addr then
                     cancel_store <= '1';
@@ -801,11 +972,15 @@ begin
     reservation_reg: process(clk)
     begin
         if rising_edge(clk) then
-            if rst = '1' or clear_rsrv = '1' then
+            if rst = '1' then
                 reservation.valid <= '0';
-            elsif set_rsrv = '1' then
-                reservation.valid <= '1';
-                reservation.addr <= r0.req.addr(63 downto LINE_OFF_BITS);
+            elsif r0_valid = '1' and access_ok = '1' then
+                if clear_rsrv = '1' then
+                    reservation.valid <= '0';
+                elsif set_rsrv = '1' then
+                    reservation.valid <= '1';
+                    reservation.addr <= r0.req.addr(63 downto LINE_OFF_BITS);
+                end if;
             end if;
         end if;
     end process;
@@ -813,19 +988,36 @@ begin
     -- Return data for loads & completion control logic
     --
     writeback_control: process(all)
+        variable data_out : std_ulogic_vector(63 downto 0);
+        variable data_fwd : std_ulogic_vector(63 downto 0);
+        variable j        : integer;
     begin
+        -- Use the bypass if are reading the row that was written 1 or 2 cycles
+        -- ago, including for the slow_valid = 1 case (i.e. completing a load
+        -- miss or a non-cacheable load).
+        if r1.use_forward1 = '1' then
+            data_fwd := r1.forward_data1;
+        else
+            data_fwd := r1.forward_data2;
+        end if;
+        data_out := cache_out(r1.hit_way);
+        for i in 0 to 7 loop
+            j := i * 8;
+            if r1.forward_sel(i) = '1' then
+                data_out(j + 7 downto j) := data_fwd(j + 7 downto j);
+            end if;
+        end loop;
 
-       -- The mux on d_out.data defaults to the normal load hit case.
-       d_out.valid <= '0';
-       d_out.data <= cache_out(r1.hit_way);
-        d_out.store_done <= '0';
-        d_out.error <= '0';
-        d_out.cache_paradox <= '0';
+       d_out.valid <= r1.ls_valid;
+       d_out.data <= data_out;
+        d_out.store_done <= not r1.stcx_fail;
+        d_out.error <= r1.ls_error;
+        d_out.cache_paradox <= r1.cache_paradox;
 
         -- Outputs to MMU
-        m_out.done <= r1.tlbie_done;
-        m_out.err <= '0';
-        m_out.data <= cache_out(r1.hit_way);
+        m_out.done <= r1.mmu_done;
+        m_out.err <= r1.mmu_error;
+        m_out.data <= data_out;
 
        -- We have a valid load or store hit or we just completed a slow
        -- op such as a load miss, a NC load or a store
@@ -849,58 +1041,33 @@ begin
             -- Request came from loadstore1...
             -- Load hit case is the standard path
             if r1.hit_load_valid = '1' then
-                report "completing load hit";
-                d_out.valid <= '1';
+                report "completing load hit data=" & to_hstring(data_out);
             end if;
 
             -- error cases complete without stalling
-            if r1.error_done = '1' then
+            if r1.ls_error = '1' then
                 report "completing ld/st with error";
-                d_out.error <= '1';
-                d_out.cache_paradox <= r1.cache_paradox;
-                d_out.valid <= '1';
             end if;
 
             -- Slow ops (load miss, NC, stores)
             if r1.slow_valid = '1' then
-                -- If it's a load, enable register writeback and switch
-                -- mux accordingly
-                --
-                if r1.req.load then
-                    -- Read data comes from the slow data latch
-                    d_out.data <= r1.slow_data;
-                end if;
-                d_out.store_done <= '1';
-
-                report "completing store or load miss";
-                d_out.valid <= '1';
-            end if;
-
-            if r1.stcx_fail = '1' then
-                d_out.store_done <= '0';
-                d_out.valid <= '1';
+                report "completing store or load miss data=" & to_hstring(data_out);
             end if;
 
         else
             -- Request came from MMU
             if r1.hit_load_valid = '1' then
                 report "completing load hit to MMU, data=" & to_hstring(m_out.data);
-                m_out.done <= '1';
             end if;
 
             -- error cases complete without stalling
-            if r1.error_done = '1' then
+            if r1.mmu_error = '1' then
                 report "completing MMU ld with error";
-                m_out.err <= '1';
-                m_out.done <= '1';
             end if;
 
             -- Slow ops (i.e. load miss)
             if r1.slow_valid = '1' then
-                -- Read data comes from the slow data latch
-                m_out.data <= r1.slow_data;
                 report "completing MMU load miss, data=" & to_hstring(m_out.data);
-                m_out.done <= '1';
             end if;
         end if;
 
@@ -923,6 +1090,7 @@ begin
        signal wr_addr  : std_ulogic_vector(ROW_BITS-1 downto 0);
        signal wr_data  : std_ulogic_vector(wishbone_data_bits-1 downto 0);
        signal wr_sel   : std_ulogic_vector(ROW_SIZE-1 downto 0);
+       signal wr_sel_m : std_ulogic_vector(ROW_SIZE-1 downto 0);
        signal dout     : cache_row_t;
     begin
        way: entity work.cache_ram
@@ -936,14 +1104,11 @@ begin
                rd_en   => do_read,
                rd_addr => rd_addr,
                rd_data => dout,
-               wr_en   => do_write,
-               wr_sel  => wr_sel,
+               wr_sel  => wr_sel_m,
                wr_addr => wr_addr,
                wr_data => wr_data
                );
        process(all)
-           variable tmp_adr   : std_ulogic_vector(63 downto 0);
-           variable reloading : boolean;
        begin
            -- Cache hit reads
            do_read <= '1';
@@ -955,38 +1120,42 @@ begin
            -- Defaults to wishbone read responses (cache refill),
            --
            -- For timing, the mux on wr_data/sel/addr is not dependent on anything
-           -- other than the current state. Only the do_write signal is.
+           -- other than the current state.
            --
-           if r1.state = IDLE then
-               -- In IDLE state, the only write path is the store-hit update case
-               wr_addr  <= std_ulogic_vector(to_unsigned(req_row, ROW_BITS));
-               wr_data  <= r0.req.data;
-               wr_sel   <= r0.req.byte_sel;
+            wr_sel_m <= (others => '0');
+
+           do_write <= '0';
+            if r1.write_bram = '1' then
+                -- Write store data to BRAM.  This happens one cycle after the
+                -- store is in r0.
+                wr_data <= r1.req.data;
+                wr_sel  <= r1.req.byte_sel;
+                wr_addr <= std_ulogic_vector(to_unsigned(get_row(r1.req.real_addr), ROW_BITS));
+                if i = r1.req.hit_way then
+                    do_write <= '1';
+                end if;
            else
                -- Otherwise, we might be doing a reload or a DCBZ
-                if r1.req.dcbz = '1' then
+                if r1.dcbz = '1' then
                     wr_data <= (others => '0');
                 else
                     wr_data <= wishbone_in.dat;
                 end if;
-               wr_sel  <= (others => '1');
-               wr_addr <= std_ulogic_vector(to_unsigned(r1.store_row, ROW_BITS));
-           end if;
+                wr_addr <= std_ulogic_vector(to_unsigned(r1.store_row, ROW_BITS));
+                wr_sel <= (others => '1');
 
-           -- The two actual write cases here
-           do_write <= '0';
-           reloading := r1.state = RELOAD_WAIT_ACK;
-           if reloading and wishbone_in.ack = '1' and r1.store_way = i then
-               do_write <= '1';
-           end if;
-           if req_op = OP_STORE_HIT and req_hit_way = i and cancel_store = '0' and
-                r0.req.dcbz = '0' then
-               assert not reloading report "Store hit while in state:" &
-                   state_t'image(r1.state)
-                   severity FAILURE;
-               do_write <= '1';
+                if r1.state = RELOAD_WAIT_ACK and wishbone_in.ack = '1' and replace_way = i then
+                    do_write <= '1';
+                end if;
            end if;
-       end process;
+
+            -- Mask write selects with do_write since BRAM doesn't
+            -- have a global write-enable
+            if do_write = '1' then
+                wr_sel_m <= wr_sel;
+            end if;
+
+        end process;
     end generate;
 
     --
@@ -996,15 +1165,7 @@ begin
     dcache_fast_hit : process(clk)
     begin
         if rising_edge(clk) then
-           -- If we have a request incoming, we have to latch it as r0.req.valid
-           -- is only set for a single cycle. It's up to the control logic to
-           -- ensure we don't override an uncompleted request (for now we are
-           -- single issue on load/stores so we are fine, later, we can generate
-           -- a stall output if necessary).
-
-           if req_op /= OP_NONE and stall_out = '0' then
-               r1.req <= r0.req;
-                r1.mmu_req <= r0.mmu_req;
+            if req_op /= OP_NONE then
                report "op:" & op_t'image(req_op) &
                    " addr:" & to_hstring(r0.req.addr) &
                    " nc:" & std_ulogic'image(r0.req.nc) &
@@ -1012,36 +1173,52 @@ begin
                    " tag:" & to_hstring(req_tag) &
                    " way: " & integer'image(req_hit_way);
            end if;
+            if r0_valid = '1' then
+                r1.mmu_req <= r0.mmu_req;
+            end if;
 
-           -- Fast path for load/store hits. Set signals for the writeback controls.
+            -- Fast path for load/store hits. Set signals for the writeback controls.
+            r1.hit_way <= req_hit_way;
+            r1.hit_index <= req_index;
            if req_op = OP_LOAD_HIT then
-               r1.hit_way <= req_hit_way;
                r1.hit_load_valid <= '1';
            else
                r1.hit_load_valid <= '0';
            end if;
+            if req_op = OP_LOAD_HIT or req_op = OP_STORE_HIT then
+                r1.cache_hit <= '1';
+            else
+                r1.cache_hit <= '0';
+            end if;
 
-            if req_op = OP_TLB_ERR then
+            if req_op = OP_BAD then
                 report "Signalling ld/st error valid_ra=" & std_ulogic'image(valid_ra) &
                     " rc_ok=" & std_ulogic'image(rc_ok) & " perm_ok=" & std_ulogic'image(perm_ok);
-                r1.error_done <= '1';
-                r1.cache_paradox <= '0';
-            elsif req_op = OP_BAD then
-                report "Signalling cache paradox";
-                r1.error_done <= '1';
-                r1.cache_paradox <= '1';
+                r1.ls_error <= not r0.mmu_req;
+                r1.mmu_error <= r0.mmu_req;
+                r1.cache_paradox <= access_ok;
             else
-                r1.error_done <= '0';
+                r1.ls_error <= '0';
+                r1.mmu_error <= '0';
                 r1.cache_paradox <= '0';
             end if;
 
-            -- complete tlbies and TLB loads in the third cycle
-            r1.tlbie_done <= r0_valid and (r0.tlbie or r0.tlbld);
+            if req_op = OP_STCX_FAIL then
+                r1.stcx_fail <= '1';
+            else
+                r1.stcx_fail <= '0';
+            end if;
+
+            -- Record TLB hit information for updating TLB PLRU
+            r1.tlb_hit <= tlb_hit;
+            r1.tlb_hit_way <= tlb_hit_way;
+            r1.tlb_hit_index <= tlb_req_index;
+
        end if;
     end process;
 
     --
-    -- Every other case is handled by this state machine:
+    -- Memory accesses are handled by this state machine:
     --
     --   * Cache load miss/reload (in conjunction with "rams")
     --   * Load hits for non-cachable forms
@@ -1051,138 +1228,204 @@ begin
     -- operates at stage 1.
     --
     dcache_slow : process(clk)
-       variable tagset    : cache_tags_set_t;
        variable stbs_done : boolean;
+        variable req       : mem_access_request_t;
+        variable acks      : unsigned(2 downto 0);
     begin
         if rising_edge(clk) then
+            r1.use_forward1 <= use_forward1_next;
+            r1.forward_sel <= (others => '0');
+            if use_forward1_next = '1' then
+                r1.forward_sel <= r1.req.byte_sel;
+            elsif use_forward2_next = '1' then
+                r1.forward_sel <= r1.forward_sel1;
+            end if;
+
+            r1.forward_data2 <= r1.forward_data1;
+            if r1.write_bram = '1' then
+                r1.forward_data1 <= r1.req.data;
+                r1.forward_sel1 <= r1.req.byte_sel;
+                r1.forward_way1 <= r1.req.hit_way;
+                r1.forward_row1 <= get_row(r1.req.real_addr);
+                r1.forward_valid1 <= '1';
+            else
+                if r1.dcbz = '1' then
+                    r1.forward_data1 <= (others => '0');
+                else
+                    r1.forward_data1 <= wishbone_in.dat;
+                end if;
+                r1.forward_sel1 <= (others => '1');
+                r1.forward_way1 <= replace_way;
+                r1.forward_row1 <= r1.store_row;
+                r1.forward_valid1 <= '0';
+            end if;
+
            -- On reset, clear all valid bits to force misses
             if rst = '1' then
                for i in index_t loop
                    cache_valids(i) <= (others => '0');
                end loop;
                 r1.state <= IDLE;
+                r1.full <= '0';
                r1.slow_valid <= '0';
                 r1.wb.cyc <= '0';
                 r1.wb.stb <= '0';
+                r1.ls_valid <= '0';
+                r1.mmu_done <= '0';
 
                -- Not useful normally but helps avoiding tons of sim warnings
                r1.wb.adr <= (others => '0');
             else
                -- One cycle pulses reset
                r1.slow_valid <= '0';
-                r1.stcx_fail <= '0';
+                r1.write_bram <= '0';
+                r1.inc_acks <= '0';
+                r1.dec_acks <= '0';
+
+                r1.ls_valid <= '0';
+                -- complete tlbies and TLB loads in the third cycle
+                r1.mmu_done <= r0_valid and (r0.tlbie or r0.tlbld);
+                if req_op = OP_LOAD_HIT or req_op = OP_STCX_FAIL then
+                    if r0.mmu_req = '0' then
+                        r1.ls_valid <= '1';
+                    else
+                        r1.mmu_done <= '1';
+                    end if;
+                end if;
+
+                if r1.write_tag = '1' then
+                    -- Store new tag in selected way
+                    for i in 0 to NUM_WAYS-1 loop
+                        if i = replace_way then
+                            cache_tags(r1.store_index)((i + 1) * TAG_WIDTH - 1 downto i * TAG_WIDTH) <=
+                                (TAG_WIDTH - 1 downto TAG_BITS => '0') & r1.reload_tag;
+                        end if;
+                    end loop;
+                    r1.store_way <= replace_way;
+                    r1.write_tag <= '0';
+                end if;
+
+                -- Take request from r1.req if there is one there,
+                -- else from req_op, ra, etc.
+                if r1.full = '1' then
+                    req := r1.req;
+                else
+                    req.op := req_op;
+                    req.valid := req_go;
+                    req.mmu_req := r0.mmu_req;
+                    req.dcbz := r0.req.dcbz;
+                    req.real_addr := ra;
+                    -- Force data to 0 for dcbz
+                    if r0.req.dcbz = '1' then
+                        req.data := (others => '0');
+                    elsif r0.d_valid = '1' then
+                        req.data := r0.req.data;
+                    else
+                        req.data := d_in.data;
+                    end if;
+                    -- Select all bytes for dcbz and for cacheable loads
+                    if r0.req.dcbz = '1' or (r0.req.load = '1' and r0.req.nc = '0') then
+                        req.byte_sel := (others => '1');
+                    else
+                        req.byte_sel := r0.req.byte_sel;
+                    end if;
+                    req.hit_way := req_hit_way;
+                    req.same_tag := req_same_tag;
+
+                    -- Store the incoming request from r0, if it is a slow request
+                    -- Note that r1.full = 1 implies req_op = OP_NONE
+                    if req_op = OP_LOAD_MISS or req_op = OP_LOAD_NC or
+                        req_op = OP_STORE_MISS or req_op = OP_STORE_HIT then
+                        r1.req <= req;
+                        r1.full <= '1';
+                    end if;
+                end if;
 
                -- Main state machine
                case r1.state is
                 when IDLE =>
-                   case req_op is
+                    r1.wb.adr <= req.real_addr(r1.wb.adr'left downto 0);
+                    r1.wb.sel <= req.byte_sel;
+                    r1.wb.dat <= req.data;
+                    r1.dcbz <= req.dcbz;
+
+                    -- Keep track of our index and way for subsequent stores.
+                    r1.store_index <= get_index(req.real_addr);
+                    r1.store_row <= get_row(req.real_addr);
+                    r1.end_row_ix <= get_row_of_line(get_row(req.real_addr)) - 1;
+                    r1.reload_tag <= get_tag(req.real_addr);
+                    r1.req.same_tag <= '1';
+
+                    if req.op = OP_STORE_HIT then
+                        r1.store_way <= req.hit_way;
+                    end if;
+
+                    -- Reset per-row valid bits, ready for handling OP_LOAD_MISS
+                    for i in 0 to ROW_PER_LINE - 1 loop
+                        r1.rows_valid(i) <= '0';
+                    end loop;
+
+                    case req.op is
                     when OP_LOAD_HIT =>
                         -- stay in IDLE state
 
                     when OP_LOAD_MISS =>
                        -- Normal load cache miss, start the reload machine
                        --
-                       report "cache miss addr:" & to_hstring(r0.req.addr) &
-                           " idx:" & integer'image(req_index) &
-                           " way:" & integer'image(replace_way) &
-                           " tag:" & to_hstring(req_tag);
-
-                       -- Force misses on that way while reloading that line
-                       cache_valids(req_index)(replace_way) <= '0';
-
-                       -- Store new tag in selected way
-                       for i in 0 to NUM_WAYS-1 loop
-                           if i = replace_way then
-                               tagset := cache_tags(req_index);
-                               write_tag(i, tagset, req_tag);
-                               cache_tags(req_index) <= tagset;
-                           end if;
-                       end loop;
-
-                       -- Keep track of our index and way for subsequent stores.
-                       r1.store_index <= req_index;
-                       r1.store_way <= replace_way;
-                       r1.store_row <= get_row(req_laddr);
-
-                       -- Prep for first wishbone read. We calculate the address of
-                       -- the start of the cache line and start the WB cycle
-                       --
-                       r1.wb.adr <= req_laddr(r1.wb.adr'left downto 0);
-                       r1.wb.sel <= (others => '1');
+                       report "cache miss real addr:" & to_hstring(req.real_addr) &
+                           " idx:" & integer'image(get_index(req.real_addr)) &
+                           " tag:" & to_hstring(get_tag(req.real_addr));
+
+                       -- Start the wishbone cycle
                        r1.wb.we  <= '0';
                        r1.wb.cyc <= '1';
                        r1.wb.stb <= '1';
 
                        -- Track that we had one request sent
                        r1.state <= RELOAD_WAIT_ACK;
+                        r1.write_tag <= '1';
 
                    when OP_LOAD_NC =>
-                        r1.wb.sel <= r0.req.byte_sel;
-                        r1.wb.adr <= ra(r1.wb.adr'left downto 3) & "000";
                         r1.wb.cyc <= '1';
                         r1.wb.stb <= '1';
                        r1.wb.we <= '0';
                        r1.state <= NC_LOAD_WAIT_ACK;
 
                     when OP_STORE_HIT | OP_STORE_MISS =>
-                        if r0.req.dcbz = '0' then
-                            r1.wb.sel <= r0.req.byte_sel;
-                            r1.wb.adr <= ra(r1.wb.adr'left downto 3) & "000";
-                            r1.wb.dat <= r0.req.data;
-                            if cancel_store = '0' then
-                                r1.wb.cyc <= '1';
-                                r1.wb.stb <= '1';
-                                r1.wb.we <= '1';
-                                r1.state <= STORE_WAIT_ACK;
+                        if req.dcbz = '0' then
+                            r1.state <= STORE_WAIT_ACK;
+                            r1.acks_pending <= to_unsigned(1, 3);
+                            r1.full <= '0';
+                            r1.slow_valid <= '1';
+                            if req.mmu_req = '0' then
+                                r1.ls_valid <= '1';
                             else
-                                r1.stcx_fail <= '1';
-                                r1.state <= IDLE;
+                                r1.mmu_done <= '1';
+                            end if;
+                            if req.op = OP_STORE_HIT then
+                                r1.write_bram <= '1';
                             end if;
                         else
                             -- dcbz is handled much like a load miss except
                             -- that we are writing to memory instead of reading
-                            r1.store_index <= req_index;
-                            r1.store_row <= get_row(req_laddr);
-
-                            if req_op = OP_STORE_HIT then
-                                r1.store_way <= req_hit_way;
-                            else
-                                r1.store_way <= replace_way;
-
-                                -- Force misses on the victim way while zeroing
-                                cache_valids(req_index)(replace_way) <= '0';
-
-                                -- Store new tag in selected way
-                                for i in 0 to NUM_WAYS-1 loop
-                                    if i = replace_way then
-                                        tagset := cache_tags(req_index);
-                                        write_tag(i, tagset, req_tag);
-                                        cache_tags(req_index) <= tagset;
-                                    end if;
-                                end loop;
-                            end if;
-
-                            -- Set up for wishbone writes
-                            r1.wb.adr <= req_laddr(r1.wb.adr'left downto 0);
-                            r1.wb.sel <= (others => '1');
-                            r1.wb.we <= '1';
-                            r1.wb.dat <= (others => '0');
-                            r1.wb.cyc <= '1';
-                            r1.wb.stb <= '1';
-
-                            -- Handle the rest like a load miss
                             r1.state <= RELOAD_WAIT_ACK;
+                            if req.op = OP_STORE_MISS then
+                                r1.write_tag <= '1';
+                            end if;
                         end if;
+                        r1.wb.we <= '1';
+                        r1.wb.cyc <= '1';
+                        r1.wb.stb <= '1';
 
                    -- OP_NONE and OP_BAD do nothing
-                    -- OP_BAD was handled above already
+                    -- OP_BAD & OP_STCX_FAIL were handled above already
                    when OP_NONE =>
                     when OP_BAD =>
-                    when OP_TLB_ERR =>
+                    when OP_STCX_FAIL =>
                    end case;
 
-               when RELOAD_WAIT_ACK =>
-                   -- Requests are all sent if stb is 0
+                when RELOAD_WAIT_ACK =>
+                    -- Requests are all sent if stb is 0
                    stbs_done := r1.wb.stb = '0';
 
                    -- If we are still sending requests, was one accepted ?
@@ -1191,7 +1434,7 @@ begin
                        -- stb and set stbs_done so we can handle an eventual last
                        -- ack on the same cycle.
                        --
-                       if is_last_row_addr(r1.wb.adr) then
+                       if is_last_row_addr(r1.wb.adr, r1.end_row_ix) then
                            r1.wb.stb <= '0';
                            stbs_done := true;
                        end if;
@@ -1201,44 +1444,94 @@ begin
                    end if;
 
                    -- Incoming acks processing
+                    r1.forward_valid1 <= wishbone_in.ack;
                    if wishbone_in.ack = '1' then
-                       -- Is this the data we were looking for ? Latch it so
-                       -- we can respond later. We don't currently complete the
-                       -- pending miss request immediately, we wait for the
-                       -- whole line to be loaded. The reason is that if we
-                       -- did, we would potentially get new requests in while
-                       -- not idle, which we don't currently know how to deal
-                       -- with.
-                       --
-                       if r1.store_row = get_row(r1.req.addr) and r1.req.dcbz = '0' then
-                           r1.slow_data <= wishbone_in.dat;
+                        r1.rows_valid(r1.store_row mod ROW_PER_LINE) <= '1';
+                        -- If this is the data we were looking for, we can
+                        -- complete the request next cycle.
+                        -- Compare the whole address in case the request in
+                        -- r1.req is not the one that started this refill.
+                       if req.valid = '1' and req.same_tag = '1' and
+                            ((r1.dcbz = '1' and req.dcbz = '1') or
+                             (r1.dcbz = '0' and req.op = OP_LOAD_MISS)) and
+                            r1.store_row = get_row(req.real_addr) then
+                            r1.full <= '0';
+                            r1.slow_valid <= '1';
+                            if r1.mmu_req = '0' then
+                                r1.ls_valid <= '1';
+                            else
+                                r1.mmu_done <= '1';
+                            end if;
+                            r1.forward_sel <= (others => '1');
+                            r1.use_forward1 <= '1';
                        end if;
 
                        -- Check for completion
-                       if stbs_done and is_last_row(r1.store_row) then
+                       if stbs_done and is_last_row(r1.store_row, r1.end_row_ix) then
                            -- Complete wishbone cycle
                            r1.wb.cyc <= '0';
 
                            -- Cache line is now valid
                            cache_valids(r1.store_index)(r1.store_way) <= '1';
 
-                            -- Don't complete and go idle until next cycle, in
-                            -- case the next request is for the last dword of
-                            -- the cache line we just loaded.
-                            r1.state <= FINISH_LD_MISS;
+                            r1.state <= IDLE;
                        end if;
 
                        -- Increment store row counter
                        r1.store_row <= next_row(r1.store_row);
                    end if;
 
-                when FINISH_LD_MISS =>
-                    -- Write back the load data that we got
-                    r1.slow_valid <= '1';
-                    r1.state <= IDLE;
-                    report "completing miss !";
+                when STORE_WAIT_ACK =>
+                   stbs_done := r1.wb.stb = '0';
+                    acks := r1.acks_pending;
+                    if r1.inc_acks /= r1.dec_acks then
+                        if r1.inc_acks = '1' then
+                            acks := acks + 1;
+                        else
+                            acks := acks - 1;
+                        end if;
+                    end if;
+                    r1.acks_pending <= acks;
+                   -- Clear stb when slave accepted request
+                    if wishbone_in.stall = '0' then
+                        -- See if there is another store waiting to be done
+                        -- which is in the same real page.
+                        if req.valid = '1' then
+                            r1.wb.adr(SET_SIZE_BITS - 1 downto 0) <=
+                                req.real_addr(SET_SIZE_BITS - 1 downto 0);
+                            r1.wb.dat <= req.data;
+                            r1.wb.sel <= req.byte_sel;
+                        end if;
+                        if acks < 7 and req.same_tag = '1' and
+                            (req.op = OP_STORE_MISS or req.op = OP_STORE_HIT) then
+                            r1.wb.stb <= '1';
+                            stbs_done := false;
+                            if req.op = OP_STORE_HIT then
+                                r1.write_bram <= '1';
+                            end if;
+                            r1.full <= '0';
+                            r1.slow_valid <= '1';
+                            -- Store requests never come from the MMU
+                            r1.ls_valid <= '1';
+                            stbs_done := false;
+                            r1.inc_acks <= '1';
+                        else
+                            r1.wb.stb <= '0';
+                            stbs_done := true;
+                        end if;
+                   end if;
+
+                   -- Got ack ? See if complete.
+                   if wishbone_in.ack = '1' then
+                        if stbs_done and acks = 1 then
+                            r1.state <= IDLE;
+                            r1.wb.cyc <= '0';
+                            r1.wb.stb <= '0';
+                        end if;
+                        r1.dec_acks <= '1';
+                   end if;
 
-                when STORE_WAIT_ACK | NC_LOAD_WAIT_ACK =>
+                when NC_LOAD_WAIT_ACK =>
                    -- Clear stb when slave accepted request
                     if wishbone_in.stall = '0' then
                        r1.wb.stb <= '0';
@@ -1246,11 +1539,16 @@ begin
 
                    -- Got ack ? complete.
                    if wishbone_in.ack = '1' then
-                       if r1.state = NC_LOAD_WAIT_ACK then
-                           r1.slow_data <= wishbone_in.dat;
-                       end if;
                         r1.state <= IDLE;
+                        r1.full <= '0';
                        r1.slow_valid <= '1';
+                        if r1.mmu_req = '0' then
+                            r1.ls_valid <= '1';
+                        else
+                            r1.mmu_done <= '1';
+                        end if;
+                        r1.forward_sel <= (others => '1');
+                        r1.use_forward1 <= '1';
                        r1.wb.cyc <= '0';
                        r1.wb.stb <= '0';
                    end if;
@@ -1258,4 +1556,26 @@ begin
            end if;
        end if;
     end process;
+
+    dc_log: if LOG_LENGTH > 0 generate
+        signal log_data : std_ulogic_vector(19 downto 0);
+    begin
+        dcache_log: process(clk)
+        begin
+            if rising_edge(clk) then
+                log_data <= r1.wb.adr(5 downto 3) &
+                            wishbone_in.stall &
+                            wishbone_in.ack &
+                            r1.wb.stb & r1.wb.cyc &
+                            d_out.error &
+                            d_out.valid &
+                            std_ulogic_vector(to_unsigned(op_t'pos(req_op), 3)) &
+                            stall_out &
+                            std_ulogic_vector(to_unsigned(tlb_hit_way, 3)) &
+                            valid_ra &
+                            std_ulogic_vector(to_unsigned(state_t'pos(r1.state), 3));
+            end if;
+        end process;
+        log_out <= log_data;
+    end generate;
 end;