Index: trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/include/vci_cc_vcache_wrapper_v1.h
===================================================================
--- trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/include/vci_cc_vcache_wrapper_v1.h	(revision 104)
+++ trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/include/vci_cc_vcache_wrapper_v1.h	(revision 119)
@@ -69,20 +69,22 @@
         ICACHE_TLB1_LL_WAIT,        // 03
         ICACHE_TLB1_SC_WAIT,        // 04
-        ICACHE_TLB1_UPDT,           // 05
-        ICACHE_TLB2_READ,           // 06
-        ICACHE_TLB2_LL_WAIT,        // 07
-        ICACHE_TLB2_SC_WAIT,        // 08
-        ICACHE_TLB2_UPDT,           // 09
-        ICACHE_TLB_FLUSH,           // 0a
-        ICACHE_CACHE_FLUSH,         // 0b
-        ICACHE_TLB_INVAL,           // 0c
-        ICACHE_CACHE_INVAL,         // 0d
-        ICACHE_CACHE_INVAL_PA,      // 0e
-        ICACHE_MISS_WAIT,           // 0f
-        ICACHE_UNC_WAIT,            // 10
-        ICACHE_MISS_UPDT,           // 11
-        ICACHE_ERROR,               // 12
-        ICACHE_CC_INVAL,            // 13
-        ICACHE_TLB_CC_INVAL,        // 14
+        ICACHE_TLB1_UPDT_SEL,       // 05
+        ICACHE_TLB1_UPDT,           // 06
+        ICACHE_TLB2_READ,           // 07
+        ICACHE_TLB2_LL_WAIT,        // 08
+        ICACHE_TLB2_SC_WAIT,        // 09
+        ICACHE_TLB2_UPDT_SEL,       // 0a
+        ICACHE_TLB2_UPDT,           // 0b
+        ICACHE_TLB_FLUSH,           // 0c
+        ICACHE_CACHE_FLUSH,         // 0d
+        ICACHE_TLB_INVAL,           // 0e
+        ICACHE_CACHE_INVAL,         // 0f
+        ICACHE_CACHE_INVAL_PA,      // 10
+        ICACHE_MISS_WAIT,           // 11
+        ICACHE_UNC_WAIT,            // 12
+        ICACHE_MISS_UPDT,           // 13
+        ICACHE_ERROR,               // 14
+        ICACHE_CC_INVAL,            // 15
+        ICACHE_TLB_CC_INVAL,        // 16
     };
 
@@ -93,33 +95,35 @@
         DCACHE_TLB1_LL_WAIT,        // 03
         DCACHE_TLB1_SC_WAIT,        // 04
-        DCACHE_TLB1_UPDT,           // 05
-        DCACHE_TLB2_READ,           // 06
-        DCACHE_TLB2_LL_WAIT,        // 07
-        DCACHE_TLB2_SC_WAIT,        // 08
-        DCACHE_TLB2_UPDT,           // 09
-        DCACHE_CTXT_SWITCH,         // 0a
-        DCACHE_ICACHE_FLUSH,        // 0b
-        DCACHE_DCACHE_FLUSH,        // 0c
-        DCACHE_ITLB_INVAL,          // 0d
-        DCACHE_DTLB_INVAL,          // 0e
-        DCACHE_ICACHE_INVAL,        // 0f
-        DCACHE_DCACHE_INVAL,        // 10
-        DCACHE_ICACHE_INVAL_PA,     // 0f
-        DCACHE_DCACHE_INVAL_PA,     // 10
-        DCACHE_DCACHE_SYNC,         // 11
-        DCACHE_LL_DIRTY_WAIT,       // 12
-        DCACHE_SC_DIRTY_WAIT,       // 13
-        DCACHE_WRITE_UPDT,          // 14
-        DCACHE_WRITE_DIRTY,         // 15
-        DCACHE_WRITE_REQ,           // 16
-        DCACHE_MISS_WAIT,           // 17
-        DCACHE_MISS_UPDT,           // 18
-        DCACHE_UNC_WAIT,            // 19
-        DCACHE_ERROR,               // 1a
-        DCACHE_CC_CHECK,            // 1b 
-        DCACHE_CC_INVAL,            // 1c
-        DCACHE_CC_UPDT,             // 1d
-        DCACHE_CC_NOP,              // 1e
-        DCACHE_TLB_CC_INVAL,        // 1f
+        DCACHE_TLB1_UPDT_SEL,       // 05
+        DCACHE_TLB1_UPDT,           // 06
+        DCACHE_TLB2_READ,           // 07
+        DCACHE_TLB2_LL_WAIT,        // 08
+        DCACHE_TLB2_SC_WAIT,        // 09
+        DCACHE_TLB2_UPDT_SEL,       // 0a
+        DCACHE_TLB2_UPDT,           // 0b
+        DCACHE_CTXT_SWITCH,         // 0c
+        DCACHE_ICACHE_FLUSH,        // 0d
+        DCACHE_DCACHE_FLUSH,        // 0e
+        DCACHE_ITLB_INVAL,          // 0f
+        DCACHE_DTLB_INVAL,          // 10
+        DCACHE_ICACHE_INVAL,        // 11
+        DCACHE_DCACHE_INVAL,        // 12
+        DCACHE_ICACHE_INVAL_PA,     // 13
+        DCACHE_DCACHE_INVAL_PA,     // 14
+        DCACHE_DCACHE_SYNC,         // 15
+        DCACHE_LL_DIRTY_WAIT,       // 16
+        DCACHE_SC_DIRTY_WAIT,       // 17
+        DCACHE_WRITE_UPDT,          // 18
+        DCACHE_WRITE_DIRTY,         // 19
+        DCACHE_WRITE_REQ,           // 1a
+        DCACHE_MISS_WAIT,           // 1b
+        DCACHE_MISS_UPDT,           // 1c
+        DCACHE_UNC_WAIT,            // 1d
+        DCACHE_ERROR,               // 1e
+        DCACHE_CC_CHECK,            // 1f 
+        DCACHE_CC_INVAL,            // 20
+        DCACHE_CC_UPDT,             // 21
+        DCACHE_CC_NOP,              // 22
+        DCACHE_TLB_CC_INVAL,        // 23
     };
 
@@ -304,5 +308,6 @@
     sc_signal<bool>         r_dcache_tlb_sc_acc_req;    // used for tlb entry type update
     sc_signal<bool>         r_dcache_tlb_ll_dirty_req;  // used for tlb dirty bit update 
-    sc_signal<bool>         r_dcache_tlb_sc_dirty_req;  // used for tlb dirty bit update 
+    sc_signal<bool>         r_dcache_tlb_sc_dirty_req;  // used for tlb dirty bit update
+    sc_signal<bool>         r_dcache_sc_updt_dirty;     // used for tlb dirty bit update  
     sc_signal<bool>         r_dcache_tlb_sc_fail;       // used for tlb entry sc failed
     sc_signal<bool>         r_dcache_tlb_ptba_read;     // used for tlb ptba read when write dirty bit 
Index: trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/src/vci_cc_vcache_wrapper_v1.cpp
===================================================================
--- trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/src/vci_cc_vcache_wrapper_v1.cpp	(revision 104)
+++ trunk/modules/vci_cc_vcache_wrapper_v1/caba/source/src/vci_cc_vcache_wrapper_v1.cpp	(revision 119)
@@ -41,8 +41,10 @@
         "ICACHE_TLB1_LL_WAIT",        
         "ICACHE_TLB1_SC_WAIT",     
+        "ICACHE_TLB1_UPDT_SEL",  
         "ICACHE_TLB1_UPDT",  
         "ICACHE_TLB2_READ",  
         "ICACHE_TLB2_LL_WAIT",        
         "ICACHE_TLB2_SC_WAIT",     
+        "ICACHE_TLB2_UPDT_SEL",  
         "ICACHE_TLB2_UPDT",  
         "ICACHE_TLB_FLUSH", 
@@ -64,8 +66,10 @@
         "DCACHE_TLB1_LL_WAIT",
         "DCACHE_TLB1_SC_WAIT",
+        "DCACHE_TLB1_UPDT_SEL",  
         "DCACHE_TLB1_UPDT",  
         "DCACHE_TLB2_READ",  
         "DCACHE_TLB2_LL_WAIT",
         "DCACHE_TLB2_SC_WAIT", 
+        "DCACHE_TLB2_UPDT_SEL",   
         "DCACHE_TLB2_UPDT",   
         "DCACHE_CTXT_SWITCH",   
@@ -395,4 +399,5 @@
         r_dcache_tlb_ll_dirty_req  = false;
         r_dcache_tlb_sc_dirty_req  = false;
+        r_dcache_sc_updt_dirty     = false;	
         r_dcache_tlb_sc_fail       = false;
         r_dcache_tlb_ptba_read     = false;
@@ -614,5 +619,5 @@
 
             // multi-update or multi-invalidate for data type
-            if ( ( (address & 0x3) != 0x3 ) && ( ! m_segment.contains(address)) ) 
+            if ( ( (address&0x3) != 0x3 ) && ( ! m_segment.contains(address)) ) 
             {
                 std::cout << "error in component VCI_CC_VCACHE_WRAPPER " << name() << std::endl;
@@ -628,5 +633,5 @@
 			  (paddr_t)p_vci_tgt.wdata.read() * m_dcache_words * 4; 
 
-            if ( (address & 0x3) == 0x3 ) // broadcast invalidate for data or instruction type
+            if ( (address&0x3) == 0x3 ) // broadcast invalidate for data or instruction type
             {
                 if ( ! p_vci_tgt.eop.read() ) 
@@ -793,5 +798,5 @@
     case TGT_RSP_ICACHE:
     {
-	if ( (p_vci_tgt.rspack.read() || !r_tgt_icache_rsp.read()) && !r_tgt_icache_req.read() ) 
+	    if ( (p_vci_tgt.rspack.read() || !r_tgt_icache_rsp.read()) && !r_tgt_icache_req.read() ) 
         {
             r_vci_tgt_fsm = TGT_IDLE;
@@ -803,5 +808,5 @@
     case TGT_RSP_DCACHE:
     {
-	if ( (p_vci_tgt.rspack.read() || !r_tgt_dcache_rsp.read()) && !r_tgt_dcache_req.read() )
+	    if ( (p_vci_tgt.rspack.read() || !r_tgt_dcache_rsp.read()) && !r_tgt_dcache_req.read() )
         {
             r_vci_tgt_fsm = TGT_IDLE;
@@ -1208,5 +1213,5 @@
 	                        {
                                 r_icache_pte_update = r_icache_miss_buf[0];
-                                r_icache_fsm        = ICACHE_TLB1_UPDT;
+                                r_icache_fsm        = ICACHE_TLB1_UPDT_SEL;
 	                        }
 	                        else
@@ -1224,5 +1229,5 @@
 	                        {
                                 r_icache_pte_update = r_icache_miss_buf[0];
-                                r_icache_fsm        = ICACHE_TLB1_UPDT;
+                                r_icache_fsm        = ICACHE_TLB1_UPDT_SEL;
 	                        }
 	                        else
@@ -1356,5 +1361,5 @@
 	            else 
 	            {
-	                r_icache_fsm = ICACHE_TLB1_UPDT; 
+	                r_icache_fsm = ICACHE_TLB1_UPDT_SEL; 
 	            }
             }
@@ -1362,6 +1367,6 @@
 	    break;
     }
-    //////////////////////
-    case ICACHE_TLB1_UPDT:
+    //////////////////////////
+    case ICACHE_TLB1_UPDT_SEL:
     {
         if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
@@ -1375,15 +1380,4 @@
         }
 
-        // TLB update and invalidate different PTE
-        if ( !r_icache_inval_tlb_rsp && !r_icache_cleanup_req )
-        {
-            paddr_t victim_index = 0;
-            r_icache_cleanup_req = icache_tlb.update1(r_icache_pte_update,r_icache_vaddr_req.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_icache_words)+2)),&victim_index);
-            r_icache_cleanup_line = victim_index;
-            r_icache_cleanup_type = TLB_CLEANUP; 
-            m_cpt_cc_cleanup_ins++;
-            r_icache_fsm = ICACHE_IDLE;
-        }
-        
         // TLB update and invalidate same PTE
         if ( r_icache_inval_tlb_rsp )                                 
@@ -1392,4 +1386,33 @@
             r_icache_fsm = ICACHE_IDLE;
         }
+
+        // TLB update and invalidate different PTE
+        if ( !r_icache_cleanup_req )
+        {
+            size_t way = 0;
+            size_t set = 0;
+            paddr_t victim_index = 0;
+
+            bool cleanup = icache_tlb.select1((r_icache_vaddr_req.read()>> PAGE_M_NBITS),&victim_index,&way,&set);
+            r_icache_way = way;
+            r_icache_set = set;
+            if (cleanup)
+            {
+                r_icache_cleanup_req = true;
+                r_icache_cleanup_line = victim_index;
+                r_icache_cleanup_type = TLB_CLEANUP; 
+                m_cpt_cc_cleanup_ins++;
+            }
+            r_icache_fsm = ICACHE_TLB1_UPDT;
+        }
+        break;
+    }
+    /////////////////////
+    case ICACHE_TLB1_UPDT:
+    {
+        if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
+
+	    icache_tlb.update(r_icache_pte_update,r_icache_vaddr_req.read(),r_icache_way.read(),r_icache_set.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_dcache_words)+2)));
+        r_icache_fsm = ICACHE_IDLE;
         break;
     }
@@ -1445,5 +1468,5 @@
 	                        if ( (r_icache_miss_buf[0] & PTE_L_MASK ) >> PTE_L_SHIFT ) // L bit is set
 	                        {
-                                r_icache_fsm        = ICACHE_TLB2_UPDT;
+                                r_icache_fsm        = ICACHE_TLB2_UPDT_SEL;
                                 r_icache_pte_update = r_icache_miss_buf[0];
 	                        }
@@ -1461,5 +1484,5 @@
 	                        if ( (r_icache_miss_buf[0] & PTE_R_MASK ) >> PTE_R_SHIFT ) // R bit is set
 	                        {
-                                r_icache_fsm        = ICACHE_TLB2_UPDT;
+                                r_icache_fsm        = ICACHE_TLB2_UPDT_SEL;
                                 r_icache_pte_update = r_icache_miss_buf[0];
 	                        }
@@ -1595,41 +1618,57 @@
 	            else 
 	            {
-	                r_icache_fsm = ICACHE_TLB2_UPDT; 
+	                r_icache_fsm = ICACHE_TLB2_UPDT_SEL; 
 	            }
             }
         }
 	    break;
+    }
+    //////////////////////////
+    case ICACHE_TLB2_UPDT_SEL: 
+    {
+        if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
+
+        // external cache invalidate request
+        if ( r_tgt_icache_req )
+        {
+            r_icache_fsm = ICACHE_CC_INVAL;
+            r_icache_fsm_save = r_icache_fsm;
+            break;
+        }
+
+        // TLB update and invalidate same PTE
+        if ( r_icache_inval_tlb_rsp )                            
+        {
+            r_icache_inval_tlb_rsp = false;
+            r_icache_fsm = ICACHE_IDLE;
+        }
+
+        // TLB update and invalidate different PTE
+        if ( !r_icache_cleanup_req )
+        {
+            size_t way = 0;
+            size_t set = 0;
+            paddr_t victim_index = 0;
+
+            bool cleanup = icache_tlb.select1((r_icache_vaddr_req.read()>> PAGE_K_NBITS),&victim_index,&way,&set);
+            r_icache_way = way;
+            r_icache_set = set;
+            if (cleanup)
+            {
+                r_icache_cleanup_req = true;
+                r_icache_cleanup_line = victim_index;
+                r_icache_cleanup_type = TLB_CLEANUP; 
+                m_cpt_cc_cleanup_ins++;
+            }
+            r_icache_fsm = ICACHE_TLB2_UPDT;
+        }
+
+        break;
     }
     /////////////////////
     case ICACHE_TLB2_UPDT: 
     {
-        if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
-
-        // external cache invalidate request
-        if ( r_tgt_icache_req )
-        {
-            r_icache_fsm = ICACHE_CC_INVAL;
-            r_icache_fsm_save = r_icache_fsm;
-            break;
-        }
-
-        // TLB update and invalidate different PTE
-
-        if ( !r_icache_inval_tlb_rsp && !r_icache_cleanup_req )
-        {
-            paddr_t victim_index = 0;
-            r_icache_cleanup_req = icache_tlb.update1(r_icache_pte_update,r_icache_miss_buf[1],r_icache_vaddr_req.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_icache_words)+2)),&victim_index);
-            r_icache_cleanup_line = victim_index;
-            r_icache_cleanup_type = TLB_CLEANUP; 
-            m_cpt_cc_cleanup_ins++;
-            r_icache_fsm = ICACHE_IDLE;
-        }
-
-        // TLB update and invalidate same PTE
-        if ( r_icache_inval_tlb_rsp )                            
-        {
-            r_icache_inval_tlb_rsp = false;
-            r_icache_fsm = ICACHE_IDLE;
-        }
+        icache_tlb.update(r_icache_pte_update,r_icache_miss_buf[1],r_icache_vaddr_req.read(),r_icache_way.read(),r_icache_set.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_icache_words)+2)));
+        r_icache_fsm = ICACHE_IDLE;
         break;
     }
@@ -1662,4 +1701,5 @@
                 }
                 if (clean) break;
+                set = 0;
             }
 
@@ -1711,4 +1751,5 @@
                 }
                 if (clean) break;
+                set = 0;
             }
             if (way == m_icache_ways)
@@ -2016,8 +2057,8 @@
 
         // invalidate cache
-        if( (( r_icache_fsm_save.read() == ICACHE_TLB1_READ )   || ( r_icache_fsm_save.read() == ICACHE_TLB2_READ )    ||
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_LL_WAIT )|| ( r_icache_fsm_save.read() == ICACHE_TLB2_LL_WAIT ) ||
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_SC_WAIT )|| ( r_icache_fsm_save.read() == ICACHE_TLB2_SC_WAIT ) || 
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_UPDT )   || ( r_icache_fsm_save.read() == ICACHE_TLB2_UPDT ))   && 
+        if( (( r_icache_fsm_save.read() == ICACHE_TLB1_READ )     || ( r_icache_fsm_save.read() == ICACHE_TLB2_READ )    ||
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_LL_WAIT )  || ( r_icache_fsm_save.read() == ICACHE_TLB2_LL_WAIT ) ||
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_SC_WAIT )  || ( r_icache_fsm_save.read() == ICACHE_TLB2_SC_WAIT ) || 
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_UPDT_SEL ) || ( r_icache_fsm_save.read() == ICACHE_TLB2_UPDT_SEL ))   && 
             ((r_icache_paddr_save.read() & ~((m_icache_words<<2)-1)) == r_tgt_addr.read()) ) 
         {
@@ -2032,7 +2073,7 @@
 
         if( (/*( r_icache_fsm_save == ICACHE_TLB1_READ )   || ( r_icache_fsm_save == ICACHE_TLB2_READ ) ||*/
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_LL_WAIT )|| ( r_icache_fsm_save.read() == ICACHE_TLB2_LL_WAIT ) ||
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_SC_WAIT )|| ( r_icache_fsm_save.read() == ICACHE_TLB2_SC_WAIT ) || 
-             ( r_icache_fsm_save.read() == ICACHE_TLB1_UPDT )   || ( r_icache_fsm_save.read() == ICACHE_TLB2_UPDT ))   && 
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_LL_WAIT )  || ( r_icache_fsm_save.read() == ICACHE_TLB2_LL_WAIT ) ||
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_SC_WAIT )  || ( r_icache_fsm_save.read() == ICACHE_TLB2_SC_WAIT ) || 
+             ( r_icache_fsm_save.read() == ICACHE_TLB1_UPDT_SEL ) || ( r_icache_fsm_save.read() == ICACHE_TLB2_UPDT_SEL ))   && 
             ((r_icache_paddr_save.read() & ~((m_icache_words<<2)-1)) == r_tgt_addr.read()) ) 
         {
@@ -2621,7 +2662,54 @@
                             else 
                             {
-                                r_dcache_unc_req = true;
-                                r_dcache_fsm = DCACHE_UNC_WAIT;
-                                m_cpt_unc_read++;
+                                if ( (dreq.type == iss_t::DATA_SC) && !dcache_pte_info.d && (r_mmu_mode.read() & DATA_TLB_MASK) )   // dirty bit update required
+                                {
+                                    m_cpt_data_tlb_update_dirty++;
+                                    m_cost_data_tlb_update_dirty_frz++;     
+                                    r_dcache_sc_updt_dirty = true;                       
+                                    if (dcache_tlb.getpagesize(dcache_tlb_way, dcache_tlb_set)) 
+                                    {
+                                        r_dcache_pte_update = dcache_tlb.getpte(dcache_tlb_way, dcache_tlb_set) | PTE_D_MASK;
+                                        r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
+                                        r_dcache_tlb_ll_dirty_req = true;
+                                        r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
+                                    }
+                                    else
+                                    {   
+                                        if (dcache_hit_p) 
+                                        {
+                                            r_dcache_pte_update = dcache_tlb.getpte(dcache_tlb_way, dcache_tlb_set) | PTE_D_MASK;
+                                            r_dcache_tlb_paddr = (paddr_t)r_dcache_ptba_save | (paddr_t)(((dreq.addr&PTD_ID2_MASK)>>PAGE_K_NBITS) << 3);
+                                            r_dcache_tlb_ll_dirty_req = true;
+                                            r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
+                                        }
+                                        else    // get PTBA to calculate the physical address of PTE
+                                        {
+                                            data_t ptba;
+                                            if (r_dcache.read((paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2), &ptba))
+                                            {
+                                                r_dcache_pte_update = dcache_tlb.getpte(dcache_tlb_way, dcache_tlb_set) | PTE_D_MASK;
+                				                r_dcache_tlb_paddr = (paddr_t)(ptba & ((1<<(m_paddr_nbits - PAGE_K_NBITS))-1)) << PAGE_K_NBITS | 
+                                     (paddr_t)(((dreq.addr & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
+                                            	r_dcache_tlb_ll_dirty_req = true;
+                                            	r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;                                               
+                                            }    
+                                            else
+                                            {    
+                                                r_dcache_pte_update = dcache_tlb.getpte(dcache_tlb_way, dcache_tlb_set) | PTE_D_MASK;
+                                                r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
+                                                r_dcache_tlb_read_req = true;
+                                                r_dcache_tlb_first_req = true;
+                                                r_dcache_tlb_ptba_read = true;
+                                                r_dcache_fsm = DCACHE_TLB1_READ;
+                                            }
+                                        }
+                                    }
+                                }
+                                else
+                                {
+                                    r_dcache_unc_req = true;
+                                    r_dcache_fsm = DCACHE_UNC_WAIT;
+                                    m_cpt_unc_read++;
+                                }
                                 m_cost_unc_read_frz++;
                             }
@@ -2661,9 +2749,19 @@
                                 {
                                     r_dcache_pte_update = dcache_tlb.getpte(dcache_tlb_way, dcache_tlb_set) | PTE_D_MASK;
-                                    r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
-                                    r_dcache_tlb_read_req = true;
-                                    r_dcache_tlb_first_req = true;
-                                    r_dcache_tlb_ptba_read = true;
-                                    r_dcache_fsm = DCACHE_TLB1_READ;
+			                        data_t ptba;
+            		                if (r_dcache.read((paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2), &ptba))
+			                        {
+                			            r_dcache_tlb_paddr = (paddr_t)(ptba & ((1<<(m_paddr_nbits - PAGE_K_NBITS))-1)) << PAGE_K_NBITS | (paddr_t)(((dreq.addr & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
+                                        r_dcache_tlb_ll_dirty_req = true;
+                                        r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
+			                        }
+				                    else
+				                    {
+                                        r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
+                                        r_dcache_tlb_read_req = true;
+                                        r_dcache_tlb_first_req = true;
+                                        r_dcache_tlb_ptba_read = true;
+                                        r_dcache_fsm = DCACHE_TLB1_READ;
+				                    }
                                 }
                             }
@@ -2791,10 +2889,20 @@
                         else
                         {
-                            r_dcache_pte_update = dcache_tlb.getpte(r_dcache_tlb_way_save, r_dcache_tlb_set_save) | PTE_D_MASK;
-                            r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
-                            r_dcache_tlb_read_req = true;
-                            r_dcache_tlb_first_req = true;
-                            r_dcache_tlb_ptba_read = true;
-                            r_dcache_fsm = DCACHE_TLB1_READ;
+	                        r_dcache_pte_update = dcache_tlb.getpte(r_dcache_tlb_way_save, r_dcache_tlb_set_save) | PTE_D_MASK;
+			                data_t ptba;
+            		        if (r_dcache.read((paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2), &ptba))
+			                {
+                	            r_dcache_tlb_paddr = (paddr_t)(ptba & ((1<<(m_paddr_nbits - PAGE_K_NBITS))-1)) << PAGE_K_NBITS | (paddr_t)(((dreq.addr & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
+                                r_dcache_tlb_ll_dirty_req = true;
+                                r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
+			                }
+			                else
+			                {
+                                r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
+                                r_dcache_tlb_read_req = true;
+                                r_dcache_tlb_first_req = true;
+                                r_dcache_tlb_ptba_read = true;
+                                r_dcache_fsm = DCACHE_TLB1_READ;
+			                }
                         }
                     }
@@ -2847,4 +2955,5 @@
 		        if (r_dcache_inval_tlb_rsp) r_dcache_inval_tlb_rsp = false;
 		        if (r_dcache_inval_rsp) r_dcache_inval_rsp = false;
+		        if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
             }
             else
@@ -2865,4 +2974,5 @@
 		            if (r_dcache_inval_tlb_rsp) r_dcache_inval_tlb_rsp = false;
 		            if (r_dcache_inval_rsp) r_dcache_inval_rsp = false;
+                    if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
                 }
                 else if ( r_dcache_inval_tlb_rsp )
@@ -2870,4 +2980,5 @@
                     r_dcache_inval_tlb_rsp = false;
                     r_dcache_fsm = DCACHE_IDLE;
+		            if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
                 }
 	            else if ( r_dcache_inval_rsp )
@@ -2875,4 +2986,5 @@
                 	r_dcache_inval_rsp = false;
 	                r_dcache_fsm = DCACHE_IDLE;
+		            if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
 	            }
         	    else
@@ -2915,4 +3027,5 @@
 		        if (r_dcache_inval_tlb_rsp) r_dcache_inval_tlb_rsp = false;
 		        if (r_dcache_inval_rsp) r_dcache_inval_rsp = false;
+		        if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
             }
             else
@@ -2923,4 +3036,5 @@
                     r_dcache_inval_tlb_rsp = false;
                     if (r_dcache_tlb_sc_fail) r_dcache_tlb_sc_fail = false;
+		            if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
                     r_dcache_fsm = DCACHE_IDLE;
                 }
@@ -2929,4 +3043,5 @@
                 	r_dcache_inval_rsp = false;
                     if (r_dcache_tlb_sc_fail) r_dcache_tlb_sc_fail = false;
+		            if (r_dcache_sc_updt_dirty) r_dcache_sc_updt_dirty = false;
 	                r_dcache_fsm = DCACHE_IDLE;
 	            }
@@ -2977,4 +3092,5 @@
 		        if (r_dcache_inval_tlb_rsp) r_dcache_inval_tlb_rsp = false;
 		        if (r_dcache_inval_rsp) r_dcache_inval_rsp = false;	
+                if ( r_dcache_tlb_ptba_read ) r_dcache_tlb_ptba_read = false;
 		        break;
             }
@@ -2988,4 +3104,5 @@
                 m_cpt_cc_cleanup_data++;
             	r_dcache_inval_tlb_rsp = false;
+                if ( r_dcache_tlb_ptba_read ) r_dcache_tlb_ptba_read = false;
 		        break;
             }
@@ -3047,5 +3164,5 @@
 	                {
                         r_dcache_pte_update = r_dcache_miss_buf[0];
-                        r_dcache_fsm        = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB1_UPDT_SEL;
 	                }
 	                else
@@ -3063,5 +3180,5 @@
 	                {
                         r_dcache_pte_update = r_dcache_miss_buf[0];
-                        r_dcache_fsm        = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB1_UPDT_SEL;
 	                }
 	                else
@@ -3189,9 +3306,49 @@
 	            else
 	            {
-	                r_dcache_fsm = DCACHE_TLB1_UPDT; 
+	                r_dcache_fsm = DCACHE_TLB1_UPDT_SEL; 
 	            }
             }
         }
 	    break;
+    }
+    //////////////////////
+    case DCACHE_TLB1_UPDT_SEL: 
+    {
+        if ( dreq.valid ) m_cost_data_tlb_miss_frz++;
+
+        // external cache invalidate request
+        if ( r_tgt_dcache_req )   
+        {
+            r_dcache_fsm = DCACHE_CC_CHECK;
+            r_dcache_fsm_save = r_dcache_fsm;
+            break;
+        }
+        
+        // TLB update and invalidate same PTE
+        if ( r_dcache_inval_tlb_rsp )                                 
+        {
+            r_dcache_inval_tlb_rsp = false;
+            r_dcache_fsm = DCACHE_IDLE;
+        }
+
+        // TLB update and invalidate different PTE
+        if ( !r_dcache_cleanup_req )
+        {
+            size_t way = 0;
+            size_t set = 0;
+            paddr_t victim_index = 0;
+            bool cleanup = dcache_tlb.select1((dreq.addr >> PAGE_M_NBITS),&victim_index,&way,&set);
+            r_dcache_way = way;
+            r_dcache_set = set;
+            if (cleanup)
+            {
+                r_dcache_cleanup_req = true;
+                r_dcache_cleanup_line = victim_index;
+                r_dcache_cleanup_type = TLB_CLEANUP; 
+                m_cpt_cc_cleanup_data++;
+            }
+            r_dcache_fsm = DCACHE_TLB1_UPDT;
+        }
+        break;
     }
     //////////////////////
@@ -3200,29 +3357,6 @@
         if ( dreq.valid ) m_cost_data_tlb_miss_frz++;
 
-        // external cache invalidate request
-        if ( r_tgt_dcache_req )   
-        {
-            r_dcache_fsm = DCACHE_CC_CHECK;
-            r_dcache_fsm_save = r_dcache_fsm;
-            break;
-        }
-        
-        // TLB update and invalidate different PTE
-        if ( !r_dcache_inval_tlb_rsp && !r_dcache_cleanup_req)
-        {
-            paddr_t victim_index = 0;
-            r_dcache_cleanup_req = dcache_tlb.update1(r_dcache_pte_update,dreq.addr,(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index);
-            r_dcache_cleanup_line = victim_index;
-            r_dcache_cleanup_type = TLB_CLEANUP; 
-            m_cpt_cc_cleanup_data++;
-            r_dcache_fsm = DCACHE_IDLE;
-        }
-
-        // TLB update and invalidate same PTE
-        if ( r_dcache_inval_tlb_rsp )                                 
-        {
-            r_dcache_inval_tlb_rsp = false;
-            r_dcache_fsm = DCACHE_IDLE;
-        }
+        dcache_tlb.update(r_dcache_pte_update,dreq.addr,r_dcache_way.read(),r_dcache_set.read(),(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)));
+        r_dcache_fsm = DCACHE_IDLE;
         break;
     }
@@ -3293,5 +3427,5 @@
 	        {
                 r_dcache_pte_update = r_dcache_miss_buf[0];
-	            r_dcache_fsm = DCACHE_TLB2_UPDT;
+	            r_dcache_fsm = DCACHE_TLB2_UPDT_SEL;
 	        }
             else
@@ -3302,5 +3436,5 @@
 	                {
                         r_dcache_pte_update = r_dcache_miss_buf[0];
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 	                }
 	                else
@@ -3318,5 +3452,5 @@
 	                {
                         r_dcache_pte_update = r_dcache_miss_buf[0];
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 	                }
 	                else
@@ -3444,9 +3578,49 @@
 	            else 
 	            {
-	                r_dcache_fsm = DCACHE_TLB2_UPDT; 
+	                r_dcache_fsm = DCACHE_TLB2_UPDT_SEL; 
 	            }
             }
         }
 	    break;
+    }
+    //////////////////////////
+    case DCACHE_TLB2_UPDT_SEL:  
+    {
+        if ( dreq.valid ) m_cost_data_tlb_miss_frz++;
+
+        // external cache invalidate request
+        if ( r_tgt_dcache_req )   
+        {
+            r_dcache_fsm = DCACHE_CC_CHECK;
+            r_dcache_fsm_save = r_dcache_fsm;
+            break;
+        }   
+     
+        // TLB update and invalidate same PTE
+        if ( r_dcache_inval_tlb_rsp )                                 
+        {
+            r_dcache_inval_tlb_rsp = false;
+            r_dcache_fsm = DCACHE_IDLE;
+        }
+
+        // TLB update and invalidate different PTE
+        if ( !r_dcache_cleanup_req )
+        {
+            size_t way = 0;
+            size_t set = 0;
+            paddr_t victim_index = 0;
+            bool cleanup = dcache_tlb.select1((dreq.addr >> PAGE_K_NBITS),&victim_index,&way,&set);            
+            r_dcache_way = way;
+            r_dcache_set = set;
+            if (cleanup)
+            {
+                r_dcache_cleanup_req = true;
+                r_dcache_cleanup_line = victim_index;
+                r_dcache_cleanup_type = TLB_CLEANUP; 
+                m_cpt_cc_cleanup_data++;
+            }
+            r_dcache_fsm = DCACHE_TLB2_UPDT;
+        }
+        break;
     }
     //////////////////////
@@ -3455,29 +3629,6 @@
         if ( dreq.valid ) m_cost_data_tlb_miss_frz++;
 
-        // external cache invalidate request
-        if ( r_tgt_dcache_req )   
-        {
-            r_dcache_fsm = DCACHE_CC_CHECK;
-            r_dcache_fsm_save = r_dcache_fsm;
-            break;
-        }        
-
-        // TLB update and invalidate different PTE
-        if ( !r_dcache_inval_tlb_rsp && !r_dcache_cleanup_req)
-        {
-            paddr_t victim_index = 0;
-            r_dcache_cleanup_req = dcache_tlb.update1(r_dcache_pte_update,r_dcache_miss_buf[1],dreq.addr,(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index);
-            r_dcache_cleanup_line = victim_index;
-            r_dcache_cleanup_type = TLB_CLEANUP; 
-            m_cpt_cc_cleanup_data++;
-            r_dcache_fsm = DCACHE_IDLE;
-        }
-
-        // TLB update and invalidate same PTE
-        if ( r_dcache_inval_tlb_rsp )                                 
-        {
-            r_dcache_inval_tlb_rsp = false;
-            r_dcache_fsm = DCACHE_IDLE;
-        }
+        dcache_tlb.update(r_dcache_pte_update,r_dcache_miss_buf[1],dreq.addr,r_dcache_way.read(),r_dcache_set.read(),(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)));
+        r_dcache_fsm = DCACHE_IDLE;
         break;
     }
@@ -3510,4 +3661,5 @@
                 }
                 if (clean) break;
+                set = 0;
             }
 
@@ -3579,4 +3731,5 @@
                 }
                 if (clean) break;
+                set = 0;
             }
 
@@ -3874,9 +4027,20 @@
                 {
                     r_dcache_pte_update = dcache_tlb.getpte(r_dcache_tlb_way_save, r_dcache_tlb_set_save) | PTE_D_MASK;
-                    r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
-                    r_dcache_tlb_ptba_read = true;
-                    r_dcache_tlb_read_req = true;
-                    r_dcache_tlb_first_req = true;
-                    r_dcache_fsm = DCACHE_TLB1_READ;
+		            data_t ptba;
+            	    if (r_dcache.read((paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2), &ptba))
+		            {
+                        r_dcache_tlb_paddr = (paddr_t)(ptba & ((1<<(m_paddr_nbits - PAGE_K_NBITS))-1)) << PAGE_K_NBITS | (paddr_t)(((dreq.addr & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
+                    	r_dcache_tlb_ll_dirty_req = true;
+                    	r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
+		        
+		            }
+		            else
+		            {
+                        r_dcache_tlb_paddr = (paddr_t)r_mmu_ptpr << (INDEX1_NBITS+2) | (paddr_t)((dreq.addr>>PAGE_M_NBITS)<<2);
+                        r_dcache_tlb_ptba_read = true;
+                        r_dcache_tlb_read_req = true;
+                        r_dcache_tlb_first_req = true;
+                        r_dcache_fsm = DCACHE_TLB1_READ;
+		            }
                 }
             }        
@@ -3906,11 +4070,22 @@
         {
             r_dcache_fsm = DCACHE_IDLE;
+	        if ( r_dcache_sc_updt_dirty ) r_dcache_sc_updt_dirty = false;
             r_dcache_inval_tlb_rsp = false;
 	        break;
         }
         dcache_tlb.setdirty(r_dcache_tlb_way_save, r_dcache_tlb_set_save);
-        r_dcache_fsm = DCACHE_WRITE_REQ;
-        drsp.valid = true;
-        drsp.rdata = 0;
+	    if (  r_dcache_sc_updt_dirty )
+	    {
+	        r_dcache_sc_updt_dirty = false;
+            r_dcache_unc_req = true;
+            r_dcache_fsm = DCACHE_UNC_WAIT;
+            m_cpt_unc_read++;
+	    }
+	    else
+	    {
+            r_dcache_fsm = DCACHE_WRITE_REQ;
+            drsp.valid = true;
+            drsp.rdata = 0;	
+	    }	
         break;
     }
@@ -4093,5 +4268,5 @@
              ( r_dcache_fsm_save.read() == DCACHE_TLB1_LL_WAIT )  || ( r_dcache_fsm_save.read() == DCACHE_TLB2_LL_WAIT )     ||
              ( r_dcache_fsm_save.read() == DCACHE_TLB1_SC_WAIT )  || ( r_dcache_fsm_save.read() == DCACHE_TLB2_SC_WAIT )     ||
-             ( r_dcache_fsm_save.read() == DCACHE_TLB1_UPDT )     || ( r_dcache_fsm_save.read() == DCACHE_TLB2_UPDT )        ||
+             ( r_dcache_fsm_save.read() == DCACHE_TLB1_UPDT_SEL ) || ( r_dcache_fsm_save.read() == DCACHE_TLB2_UPDT_SEL )    ||
              ( r_dcache_fsm_save.read() == DCACHE_LL_DIRTY_WAIT ) || ( r_dcache_fsm_save.read() == DCACHE_SC_DIRTY_WAIT )    ||
 	         ( r_dcache_fsm_save.read() == DCACHE_WRITE_DIRTY )) && 
@@ -4111,5 +4286,5 @@
              ( r_dcache_fsm_save.read() == DCACHE_TLB1_LL_WAIT )     || ( r_dcache_fsm_save.read() == DCACHE_TLB2_LL_WAIT )     ||
              ( r_dcache_fsm_save.read() == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save.read() == DCACHE_TLB2_SC_WAIT )     ||
-             ( r_dcache_fsm_save.read() == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save.read() == DCACHE_TLB2_UPDT )        ||
+             ( r_dcache_fsm_save.read() == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save.read() == DCACHE_TLB2_UPDT_SEL )    ||
              ( r_dcache_fsm_save.read() == DCACHE_LL_DIRTY_WAIT )    || ( r_dcache_fsm_save.read() == DCACHE_SC_DIRTY_WAIT )    ||
 	         ( r_dcache_fsm_save.read() == DCACHE_WRITE_DIRTY )) && 
@@ -5099,5 +5274,4 @@
              << "p_vci_tgt.reop:" << p_vci_tgt.reop
              << std::endl;
-
 #endif
 }
