Index: trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/include/vci_cc_vcache_wrapper2_v1.h
===================================================================
--- trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/include/vci_cc_vcache_wrapper2_v1.h	(revision 108)
+++ trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/include/vci_cc_vcache_wrapper2_v1.h	(revision 119)
@@ -68,20 +68,22 @@
         ICACHE_TLB1_READ,           // 02
         ICACHE_TLB1_WRITE,          // 03
-        ICACHE_TLB1_UPDT,           // 04
-        ICACHE_TLB2_READ,           // 05
-        ICACHE_TLB2_WRITE,          // 06
-        ICACHE_TLB2_UPDT,           // 07
-        ICACHE_SW_FLUSH,            // 08
-        ICACHE_TLB_FLUSH,           // 09
-        ICACHE_CACHE_FLUSH,         // 0a
-        ICACHE_TLB_INVAL,           // 0b
-        ICACHE_CACHE_INVAL,         // 0c
-        ICACHE_CACHE_INVAL_PA,      // 0d
-        ICACHE_MISS_WAIT,           // 0e
-        ICACHE_UNC_WAIT,            // 0f
-        ICACHE_MISS_UPDT,           // 10
-        ICACHE_ERROR,               // 11
-        ICACHE_CC_INVAL,            // 12
-        ICACHE_TLB_CC_INVAL,        // 13
+        ICACHE_TLB1_UPDT_SEL,       // 04
+        ICACHE_TLB1_UPDT,           // 05
+        ICACHE_TLB2_READ,           // 06
+        ICACHE_TLB2_WRITE,          // 07
+        ICACHE_TLB2_UPDT_SEL,       // 08
+        ICACHE_TLB2_UPDT,           // 09
+        ICACHE_SW_FLUSH,            // 0a
+        ICACHE_TLB_FLUSH,           // 0b
+        ICACHE_CACHE_FLUSH,         // 0c
+        ICACHE_TLB_INVAL,           // 0d
+        ICACHE_CACHE_INVAL,         // 0e
+        ICACHE_CACHE_INVAL_PA,      // 0f
+        ICACHE_MISS_WAIT,           // 10
+        ICACHE_UNC_WAIT,            // 11
+        ICACHE_MISS_UPDT,           // 12
+        ICACHE_ERROR,               // 13
+        ICACHE_CC_INVAL,            // 14
+        ICACHE_TLB_CC_INVAL,        // 15
     };
 
@@ -94,40 +96,42 @@
         DCACHE_TLB1_READ,           // 05
         DCACHE_TLB1_READ_UPDT,      // 06
-        DCACHE_TLB1_UPDT,           // 07
-        DCACHE_DTLB2_READ_CACHE,    // 08
-        DCACHE_TLB2_LL_WAIT,        // 09
-        DCACHE_TLB2_SC_WAIT,        // 0a
-        DCACHE_TLB2_READ,           // 0b
-        DCACHE_TLB2_READ_UPDT,      // 0c
-        DCACHE_TLB2_UPDT,           // 0d
-        DCACHE_CTXT_SWITCH,         // 0e
-        DCACHE_ICACHE_FLUSH,        // 0f
-        DCACHE_DCACHE_FLUSH,        // 10
-        DCACHE_ITLB_INVAL,          // 11
-        DCACHE_DTLB_INVAL,          // 12
-        DCACHE_ICACHE_INVAL,        // 13
-        DCACHE_DCACHE_INVAL,        // 14
-	    DCACHE_ICACHE_INVAL_PA,     // 15
-	    DCACHE_DCACHE_INVAL_PA,     // 16
-        DCACHE_DCACHE_SYNC,         // 17
-        DCACHE_LL_DIRTY_WAIT,       // 18
-        DCACHE_SC_DIRTY_WAIT,       // 19
-        DCACHE_WRITE_UPDT,          // 1a
-        DCACHE_WRITE_DIRTY,         // 1b
-        DCACHE_WRITE_REQ,           // 1c
-        DCACHE_MISS_WAIT,           // 1d
-        DCACHE_MISS_UPDT,           // 1e
-        DCACHE_UNC_WAIT,            // 1f
-        DCACHE_ERROR,               // 20
-        DCACHE_ITLB_READ,           // 21
-        DCACHE_ITLB_UPDT,           // 22
-        DCACHE_ITLB_LL_WAIT,        // 23
-        DCACHE_ITLB_SC_WAIT,        // 24
-        DCACHE_CC_CHECK,            // 25
-        DCACHE_CC_INVAL,            // 26
-        DCACHE_CC_UPDT,             // 27
-        DCACHE_CC_NOP,              // 28
-        DCACHE_TLB_CC_INVAL,        // 29
-        DCACHE_ITLB_CLEANUP,        // 2a
+        DCACHE_TLB1_UPDT_SEL,       // 07
+        DCACHE_TLB1_UPDT,           // 08
+        DCACHE_DTLB2_READ_CACHE,    // 09
+        DCACHE_TLB2_LL_WAIT,        // 0a
+        DCACHE_TLB2_SC_WAIT,        // 0b
+        DCACHE_TLB2_READ,           // 0c
+        DCACHE_TLB2_READ_UPDT,      // 0d
+        DCACHE_TLB2_UPDT_SEL,       // 0e
+        DCACHE_TLB2_UPDT,           // 0f
+        DCACHE_CTXT_SWITCH,         // 10
+        DCACHE_ICACHE_FLUSH,        // 11
+        DCACHE_DCACHE_FLUSH,        // 12
+        DCACHE_ITLB_INVAL,          // 13
+        DCACHE_DTLB_INVAL,          // 14
+        DCACHE_ICACHE_INVAL,        // 15
+        DCACHE_DCACHE_INVAL,        // 16
+	    DCACHE_ICACHE_INVAL_PA,     // 17
+	    DCACHE_DCACHE_INVAL_PA,     // 18
+        DCACHE_DCACHE_SYNC,         // 19
+        DCACHE_LL_DIRTY_WAIT,       // 1a
+        DCACHE_SC_DIRTY_WAIT,       // 1b
+        DCACHE_WRITE_UPDT,          // 1c
+        DCACHE_WRITE_DIRTY,         // 1d
+        DCACHE_WRITE_REQ,           // 1e
+        DCACHE_MISS_WAIT,           // 1f
+        DCACHE_MISS_UPDT,           // 20
+        DCACHE_UNC_WAIT,            // 21
+        DCACHE_ERROR,               // 22
+        DCACHE_ITLB_READ,           // 23
+        DCACHE_ITLB_UPDT,           // 24
+        DCACHE_ITLB_LL_WAIT,        // 25
+        DCACHE_ITLB_SC_WAIT,        // 26
+        DCACHE_CC_CHECK,            // 27
+        DCACHE_CC_INVAL,            // 28
+        DCACHE_CC_UPDT,             // 29
+        DCACHE_CC_NOP,              // 2a
+        DCACHE_TLB_CC_INVAL,        // 2b
+        DCACHE_ITLB_CLEANUP,        // 2c
     };
 
Index: trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/src/vci_cc_vcache_wrapper2_v1.cpp
===================================================================
--- trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/src/vci_cc_vcache_wrapper2_v1.cpp	(revision 108)
+++ trunk/modules/vci_cc_vcache_wrapper2_v1/caba/source/src/vci_cc_vcache_wrapper2_v1.cpp	(revision 119)
@@ -40,8 +40,10 @@
         "ICACHE_BIS",       
         "ICACHE_TLB1_READ",  
-        "ICACHE_TLB1_WRITE",  
+        "ICACHE_TLB1_WRITE",
+	"ICACHE_TLB1_UPDT_SEL",  
         "ICACHE_TLB1_UPDT",  
         "ICACHE_TLB2_READ",  
-        "ICACHE_TLB2_WRITE",  
+        "ICACHE_TLB2_WRITE",
+	"ICACHE_TLB2_UPDT_SEL",  
         "ICACHE_TLB2_UPDT",  
         "ICACHE_SW_FLUSH", 
@@ -65,5 +67,6 @@
 	"DCACHE_TLB1_SC_WAIT",    
         "DCACHE_TLB1_READ",
-        "DCACHE_TLB1_READ_UPDT",  
+        "DCACHE_TLB1_READ_UPDT",
+	"DCACHE_TLB1_UPDT_SEL",  
         "DCACHE_TLB1_UPDT", 
         "DCACHE_DTLB2_READ_CACHE", 
@@ -72,4 +75,5 @@
         "DCACHE_TLB2_READ",
         "DCACHE_TLB2_READ_UPDT",  
+	"DCACHE_TLB2_UPDT_SEL",
         "DCACHE_TLB2_UPDT",   
         "DCACHE_CTXT_SWITCH",   
@@ -1117,6 +1121,4 @@
                 if ( !icache_hit_c ) 
                 {
-                    m_cpt_ins_miss++;
-                    m_cost_ins_miss_frz++;
                     if ( icache_cached ) 
                     {
@@ -1125,4 +1127,6 @@
 			r_icache_vaddr_req = ireq.addr;
                         r_icache_fsm = ICACHE_MISS_WAIT;
+                    	m_cpt_ins_miss++;
+                    	m_cost_ins_miss_frz++;
                     } 
                     else 
@@ -1150,8 +1154,9 @@
     case ICACHE_BIS: 
     {
+	if ( ireq.valid ) m_cost_ins_miss_frz++;
+
         // external cache invalidate request
         if ( r_tgt_icache_req )
         {
-	    if ( ireq.valid ) m_cost_ins_miss_frz++;
             r_icache_fsm = ICACHE_CC_INVAL;
             r_icache_fsm_save = r_icache_fsm;
@@ -1162,5 +1167,4 @@
         if ( r_dcache_itlb_inval_req )
         {
-	    if ( ireq.valid ) m_cost_ins_miss_frz++;
 	    r_itlb_inval_req = true;
             r_icache_fsm = ICACHE_TLB_CC_INVAL;
@@ -1172,5 +1176,4 @@
         if ( r_icache_inval_tlb_rsp )
         {
-	    if ( ireq.valid ) m_cost_ins_miss_frz++;
             r_icache_inval_tlb_rsp = false;
             r_icache_fsm = ICACHE_IDLE;
@@ -1284,5 +1287,5 @@
             		    {
                                 r_icache_pte_update = r_dcache_rsp_itlb_miss;
-                                r_icache_fsm        = ICACHE_TLB1_UPDT;
+                                r_icache_fsm        = ICACHE_TLB1_UPDT_SEL;
             		    }
             		    else
@@ -1300,5 +1303,5 @@
             		    {
                                 r_icache_pte_update = r_dcache_rsp_itlb_miss;
-                                r_icache_fsm        = ICACHE_TLB1_UPDT;
+                                r_icache_fsm        = ICACHE_TLB1_UPDT_SEL;
             		    }
             		    else
@@ -1377,5 +1380,5 @@
                 else  
                 {
-                    r_icache_fsm = ICACHE_TLB1_UPDT;  
+                    r_icache_fsm = ICACHE_TLB1_UPDT_SEL;  
                 }
             } 
@@ -1405,7 +1408,8 @@
     }
     //////////////////////
-    case ICACHE_TLB1_UPDT:
+    case ICACHE_TLB1_UPDT_SEL:
     {
         if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
+        m_cost_ins_tlb_update_acc_frz++;
 
         // external cache invalidate request
@@ -1426,14 +1430,4 @@
         }
 
-        // TLB update and invalidate different PTE
-        if ( !r_dcache_itlb_cleanup_req && !r_icache_inval_tlb_rsp )  
-        {
-            paddr_t victim_index = 0;
-            r_dcache_itlb_cleanup_req = icache_tlb.update(r_icache_pte_update,r_icache_vaddr_req.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index);
-            r_dcache_itlb_cleanup_line = victim_index;
-	    m_cpt_cc_cleanup_ins++;
-            r_icache_fsm = ICACHE_IDLE;
-        }
-
         // TLB update and invalidate same PTE
         if ( r_icache_inval_tlb_rsp )                                 
@@ -1441,5 +1435,33 @@
             r_icache_inval_tlb_rsp = false;
             r_icache_fsm = ICACHE_IDLE;
-        }
+	    break;
+        }
+
+	if ( r_dcache_itlb_cleanup_req ) break; 
+
+	size_t way = 0;
+	size_t set = 0;
+	paddr_t victim_index = 0;
+
+	bool cleanup = icache_tlb.select((r_icache_vaddr_req.read()>> PAGE_M_NBITS),&victim_index,&way,&set);
+	r_icache_way = way;
+	r_icache_set = set;
+	if (cleanup)
+	{
+	    r_dcache_itlb_cleanup_req = true;
+	    r_dcache_itlb_cleanup_line = victim_index;
+	    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++;
+        m_cost_ins_tlb_update_acc_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;
     }
@@ -1494,5 +1516,5 @@
             		    if ( (r_dcache_rsp_itlb_miss & 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_dcache_rsp_itlb_miss;
             		    }
@@ -1510,5 +1532,5 @@
             		    if ( (r_dcache_rsp_itlb_miss & 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_dcache_rsp_itlb_miss;
             		    }
@@ -1588,5 +1610,5 @@
                 else  
                 {
-                    r_icache_fsm = ICACHE_TLB2_UPDT;  
+                    r_icache_fsm = ICACHE_TLB2_UPDT_SEL;  
                 }
             }
@@ -1615,8 +1637,9 @@
         break;
     }
-    /////////////////////
-    case ICACHE_TLB2_UPDT: 
+    //////////////////////////
+    case ICACHE_TLB2_UPDT_SEL: 
     {
         if ( ireq.valid ) m_cost_ins_tlb_miss_frz++;
+        m_cost_ins_tlb_update_acc_frz++;
 
         // external cache invalidate request
@@ -1637,13 +1660,4 @@
         }
 
-        // TLB update and invalidate different PTE
-        if ( !r_dcache_itlb_cleanup_req && !r_icache_inval_tlb_rsp ) 
-        {
-            paddr_t victim_index = 0;
-            r_dcache_itlb_cleanup_req = icache_tlb.update(r_icache_pte_update,r_dcache_rsp_itlb_ppn,r_icache_vaddr_req.read(),(r_icache_paddr_save.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index);
-            r_dcache_itlb_cleanup_line = victim_index;
-	    m_cpt_cc_cleanup_ins++;
-            r_icache_fsm = ICACHE_IDLE;
-        }
         // TLB update and invalidate same PTE
         if ( r_icache_inval_tlb_rsp )                            
@@ -1651,6 +1665,34 @@
             r_icache_inval_tlb_rsp = false;
             r_icache_fsm = ICACHE_IDLE;
-        }
-        break;
+	    break;
+        }
+
+	if ( r_dcache_itlb_cleanup_req ) break; 
+
+	size_t way = 0;
+	size_t set = 0;
+	paddr_t victim_index = 0;
+
+	bool cleanup = icache_tlb.select((r_icache_vaddr_req.read()>> PAGE_K_NBITS),&victim_index,&way,&set);
+	r_icache_way = way;
+	r_icache_set = set;
+	if (cleanup)
+	{
+	    r_dcache_itlb_cleanup_req = true;
+	    r_dcache_itlb_cleanup_line = victim_index;
+	    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++;
+        m_cost_ins_tlb_update_acc_frz++;
+
+        icache_tlb.update(r_icache_pte_update,r_dcache_rsp_itlb_ppn,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;
     }
     /////////////////////////////
@@ -1988,5 +2030,6 @@
 
                 r_icache_cleanup_req = r_icache.update(r_icache_paddr_save.read(), buf, &victim_index);
-                r_icache_cleanup_line = victim_index;                        m_cpt_cc_cleanup_ins++;
+                r_icache_cleanup_line = victim_index;                        
+		m_cpt_cc_cleanup_ins++;
                 r_icache_fsm = ICACHE_IDLE;
 	    }
@@ -2014,7 +2057,7 @@
             m_cost_ins_miss_frz++;
         }
-        if( (( r_icache_fsm_save == ICACHE_TLB1_READ )   || ( r_icache_fsm_save == ICACHE_TLB2_READ )    ||
-             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )  || ( r_icache_fsm_save == ICACHE_TLB2_WRITE ) ||
-             ( r_icache_fsm_save == ICACHE_TLB1_UPDT )   || ( r_icache_fsm_save == ICACHE_TLB2_UPDT ))   && (ireq.valid) )
+        if( (( r_icache_fsm_save == ICACHE_TLB1_READ )     || ( r_icache_fsm_save == ICACHE_TLB2_READ )      ||
+             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )    || ( r_icache_fsm_save == ICACHE_TLB2_WRITE )     ||
+             ( r_icache_fsm_save == ICACHE_TLB1_UPDT_SEL ) || ( r_icache_fsm_save == ICACHE_TLB2_UPDT_SEL )) && (ireq.valid) )
         {
             m_cost_ins_tlb_miss_frz++;
@@ -2045,7 +2088,7 @@
             m_cost_ins_miss_frz++;
         }
-        if( (( r_icache_fsm_save == ICACHE_TLB1_READ )   || ( r_icache_fsm_save == ICACHE_TLB2_READ )  ||
-             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )  || ( r_icache_fsm_save == ICACHE_TLB2_WRITE ) ||
-             ( r_icache_fsm_save == ICACHE_TLB1_UPDT )   || ( r_icache_fsm_save == ICACHE_TLB2_UPDT ))   && (ireq.valid) )
+        if( (( r_icache_fsm_save == ICACHE_TLB1_READ )     || ( r_icache_fsm_save == ICACHE_TLB2_READ )      ||
+             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )    || ( r_icache_fsm_save == ICACHE_TLB2_WRITE )     ||
+             ( r_icache_fsm_save == ICACHE_TLB1_UPDT_SEL ) || ( r_icache_fsm_save == ICACHE_TLB2_UPDT_SEL )) && (ireq.valid) )
         {
             m_cost_ins_tlb_miss_frz++;
@@ -2054,7 +2097,7 @@
 	if ( r_itlb_inval_req ) break;
         // invalidate cache
-        if( (( r_icache_fsm_save == ICACHE_TLB1_READ ) || ( r_icache_fsm_save == ICACHE_TLB2_READ )  ||
-             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )|| ( r_icache_fsm_save == ICACHE_TLB2_WRITE ) ||
-             ( r_icache_fsm_save == ICACHE_TLB1_UPDT ) || ( r_icache_fsm_save == ICACHE_TLB2_UPDT )) && 
+        if( (( r_icache_fsm_save == ICACHE_TLB1_READ )     || ( r_icache_fsm_save == ICACHE_TLB2_READ )  ||
+             ( r_icache_fsm_save == ICACHE_TLB1_WRITE )    || ( r_icache_fsm_save == ICACHE_TLB2_WRITE ) ||
+             ( r_icache_fsm_save == ICACHE_TLB1_UPDT_SEL ) || ( r_icache_fsm_save == ICACHE_TLB2_UPDT_SEL )) && 
             (((r_icache_paddr_save.read() & ~((m_icache_words<<2)-1)) >> (uint32_log2(m_icache_words) + 2) ) == r_dcache_itlb_inval_line.read()) ) 
         {
@@ -2074,5 +2117,4 @@
     }
     } // end switch r_icache_fsm
-
 #ifdef SOCLIB_MODULE_DEBUG
     std::cout << name() << " Instruction Response: " << irsp << std::endl;
@@ -2293,4 +2335,6 @@
     	else if ( r_itlb_acc_dcache_req ) // ins tlb write access bit
     	{
+	    if ( dreq.valid ) m_cost_ins_tlb_occup_cache_frz++;
+
 	    data_t rsp_itlb_miss;
     	    bool itlb_hit_dcache = r_dcache.read(r_icache_paddr_save, &rsp_itlb_miss);
@@ -2765,68 +2809,4 @@
                         }
                         break;
-/*
-                    case iss_t::DATA_READ:
-                        m_cpt_read++;
-                        if ( dcache_hit_c ) 
-                        {
-                            r_dcache_buf_unc_valid = false;
-                            r_dcache_fsm = DCACHE_IDLE;
-                            drsp.valid = true;
-                            drsp.rdata = dcache_rdata;
-                        } 
-                        else 
-                        {
-                            if ( dcache_cached ) 
-                            {
-                                r_dcache_miss_req = true;
-                                r_dcache_fsm = DCACHE_MISS_WAIT;
-                                m_cpt_data_miss++;
-                                m_cost_data_miss_frz++;
-                            } 
-                            else 
-                            {
-                                r_dcache_unc_req = true;
-                                r_dcache_fsm = DCACHE_UNC_WAIT;
-                                m_cpt_unc_read++;
-                                m_cost_unc_read_frz++;
-                            }
-                        }
-                        break;
-                    case iss_t::DATA_LL:
-			if (r_dcache_llsc_reserved && (r_dcache_llsc_addr_save == tlb_dpaddr) && r_dcache_buf_unc_valid)
-			{
-                            r_dcache_buf_unc_valid = false;
-                            r_dcache_fsm = DCACHE_IDLE;
-                            drsp.valid = true;
-                            drsp.rdata = dcache_rdata;
-			}
-			else
-			{
-			    r_dcache_llsc_reserved = true;
-			    r_dcache_llsc_addr_save = tlb_dpaddr;
-                            r_dcache_unc_req = true;
-                            r_dcache_fsm = DCACHE_UNC_WAIT;
-			}
-			break;
-                    case iss_t::DATA_SC:
-			if (r_dcache_llsc_reserved && (r_dcache_llsc_addr_save == tlb_dpaddr))
-			{
-			    r_dcache_llsc_reserved = false;
-                            r_dcache_unc_req = true;
-                            r_dcache_fsm = DCACHE_UNC_WAIT;
-			}
-			else
-			{   
-			    if ( r_dcache_buf_unc_valid )
-			    {                         
-			        r_dcache_llsc_reserved = false;
-			        r_dcache_buf_unc_valid = false;
-                                drsp.valid = true;
-                                drsp.rdata = dcache_rdata;
-			    }
-                            r_dcache_fsm = DCACHE_IDLE;
-			}			
-			break;
-*/
                     case iss_t::DATA_WRITE:
                         m_cpt_write++;
@@ -2918,4 +2898,6 @@
     case DCACHE_BIS:
     {
+        if ( dreq.valid ) m_cost_data_miss_frz++;
+
         // external cache invalidate request
         if ( r_tgt_dcache_req )   
@@ -2923,5 +2905,4 @@
             r_dcache_fsm = DCACHE_CC_CHECK;
             r_dcache_fsm_save = r_dcache_fsm;
-            if ( dreq.valid ) m_cost_data_miss_frz++;
             break;
         }
@@ -2932,5 +2913,4 @@
             r_dcache_inval_tlb_rsp = false;
             r_dcache_fsm = DCACHE_IDLE;
-            if ( dreq.valid ) m_cost_data_miss_frz++;
             break;
         }
@@ -3228,5 +3208,4 @@
                         r_dcache_tlb_ll_dirty_req = true;
                         r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
-		        //m_cpt_dcache_data_write++; 
                         m_cpt_data_tlb_update_dirty++;
                         m_cost_data_tlb_update_dirty_frz++;
@@ -3251,5 +3230,5 @@
 		    {
                         r_dcache_pte_update = tlb_data;
-                        r_dcache_fsm = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm = DCACHE_TLB1_UPDT_SEL;
 		    }
 		    else
@@ -3267,5 +3246,5 @@
 		    {
                         r_dcache_pte_update = tlb_data;
-                        r_dcache_fsm = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm = DCACHE_TLB1_UPDT_SEL;
 		    }
 		    else
@@ -3292,5 +3271,6 @@
     {
         if ( dreq.valid ) m_cost_data_tlb_miss_frz++;
-        m_cost_data_tlb_update_acc_frz++;	    
+        m_cost_data_tlb_update_acc_frz++;
+	    
         // external cache invalidate request
         if ( r_tgt_dcache_req )   
@@ -3414,5 +3394,5 @@
 		    bool write_hit = r_dcache.write(r_dcache_tlb_paddr,r_dcache_pte_update);  
                     assert(write_hit && "Write on miss ignores data for data MMU update data access bit"); 
-	    	    r_dcache_fsm = DCACHE_TLB1_UPDT; 
+	    	    r_dcache_fsm = DCACHE_TLB1_UPDT_SEL; 
 		    m_cpt_dcache_data_write++;
 	    	}
@@ -3579,5 +3559,4 @@
                         r_dcache_tlb_ll_dirty_req = true;
                         r_dcache_fsm = DCACHE_LL_DIRTY_WAIT;
-		        //m_cpt_dcache_data_write++; 
                         m_cpt_data_tlb_update_dirty++;
                         m_cost_data_tlb_update_dirty_frz++;
@@ -3601,5 +3580,5 @@
 		    {
                         r_dcache_pte_update = rsp_dtlb_miss;
-                        r_dcache_fsm        = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB1_UPDT_SEL;
 		    }
 		    else
@@ -3617,5 +3596,5 @@
 		    {
                         r_dcache_pte_update = rsp_dtlb_miss;
-                        r_dcache_fsm        = DCACHE_TLB1_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB1_UPDT_SEL;
 		    }
 		    else
@@ -3632,9 +3611,10 @@
         break;
     }
-    //////////////////////
-    case DCACHE_TLB1_UPDT: 
+    /////////////////////////
+    case DCACHE_TLB1_UPDT_SEL: 
     {
         m_cost_data_tlb_miss_frz++;
-
+        m_cost_data_tlb_update_acc_frz++;
+	
         // external cache invalidate request
         if ( r_tgt_dcache_req )   
@@ -3647,12 +3627,17 @@
         if ( !r_dcache_inval_tlb_rsp && !r_dcache_inval_rsp )
         {
+	    size_t way = 0;
+	    size_t set = 0;
             paddr_t victim_index = 0;
-            if (dcache_tlb.update(r_dcache_pte_update,dreq.addr,(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index))
-            {
-                r_dcache.setinbit((paddr_t)victim_index << (uint32_log2(m_dcache_words)+2), r_dcache_in_dtlb, false);
-            }
-            bool set_hit = r_dcache.setinbit(r_dcache_tlb_paddr, r_dcache_in_dtlb, true);
-            assert(set_hit && "TLB1_UPDT set hit error");  
-            r_dcache_fsm = DCACHE_IDLE;
+	    bool cleanup = dcache_tlb.select((dreq.addr >> PAGE_M_NBITS),&victim_index,&way,&set);
+	    if (cleanup)
+	    {
+	        r_dcache_dtlb_cleanup_req = true;
+		r_dcache_dtlb_cleanup_line = victim_index;
+		m_cpt_cc_cleanup_data++;
+	    }
+	    r_dcache_way = way;
+	    r_dcache_set = set;
+            r_dcache_fsm = DCACHE_TLB1_UPDT;
         }
         else  
@@ -3663,4 +3648,17 @@
         }
         break;
+    }
+    //////////////////////
+    case DCACHE_TLB1_UPDT: 
+    {
+        m_cost_data_tlb_miss_frz++;
+        m_cost_data_tlb_update_acc_frz++;
+
+	if (r_dcache_dtlb_cleanup_req) r_dcache.setinbit(r_dcache_dtlb_cleanup_line.read() << (uint32_log2(m_dcache_words)+2), r_dcache_in_dtlb, false);
+        bool set_hit = r_dcache.setinbit(r_dcache_tlb_paddr, r_dcache_in_dtlb, true);
+        assert(set_hit && "TLB1_UPDT set hit error");  
+	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;
     }
     /////////////////////////////
@@ -3715,5 +3713,5 @@
                 r_dcache_pte_update = tlb_data;
 	        r_dcache_ppn_update = tlb_data_ppn;
-		r_dcache_fsm = DCACHE_TLB2_UPDT;
+		r_dcache_fsm = DCACHE_TLB2_UPDT_SEL;
 	    }
 	    else 
@@ -3726,5 +3724,5 @@
                         r_dcache_pte_update = tlb_data;
 			r_dcache_ppn_update = tlb_data_ppn;
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 		    }
 		    else
@@ -3744,5 +3742,5 @@
                         r_dcache_pte_update = tlb_data;
 			r_dcache_ppn_update = tlb_data_ppn;
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 		    }
 		    else
@@ -3893,5 +3891,5 @@
 		    bool write_hit = r_dcache.write(r_dcache_tlb_paddr,r_dcache_pte_update);  
                     assert(write_hit && "Write on miss ignores data for data MMU update data access bit"); 
-	            r_dcache_fsm = DCACHE_TLB2_UPDT; 
+	            r_dcache_fsm = DCACHE_TLB2_UPDT_SEL; 
 		    m_cpt_dcache_data_write++;
 	        }
@@ -4044,5 +4042,5 @@
                 r_dcache_pte_update = rsp_dtlb_miss;
 	        r_dcache_ppn_update = tlb_data_ppn;
-		r_dcache_fsm = DCACHE_TLB2_UPDT;
+		r_dcache_fsm = DCACHE_TLB2_UPDT_SEL;
 	    }
 	    else
@@ -4054,5 +4052,5 @@
                         r_dcache_pte_update = rsp_dtlb_miss;
 			r_dcache_ppn_update = tlb_data_ppn;
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 		    }
 		    else
@@ -4072,5 +4070,5 @@
                         r_dcache_pte_update = rsp_dtlb_miss;
 			r_dcache_ppn_update = tlb_data_ppn;
-                        r_dcache_fsm        = DCACHE_TLB2_UPDT;
+                        r_dcache_fsm        = DCACHE_TLB2_UPDT_SEL;
 		    }
 		    else
@@ -4088,8 +4086,9 @@
         break;
     }
-    //////////////////////
-    case DCACHE_TLB2_UPDT:  
+    //////////////////////////
+    case DCACHE_TLB2_UPDT_SEL:  
     {
         m_cost_data_tlb_miss_frz++;
+        m_cost_data_tlb_update_acc_frz++;
 
         // external cache invalidate request
@@ -4103,12 +4102,17 @@
         if ( !r_dcache_inval_tlb_rsp && !r_dcache_inval_rsp )
         {
+	    size_t way = 0;
+	    size_t set = 0;
             paddr_t victim_index = 0;
-            if (dcache_tlb.update(r_dcache_pte_update,r_dcache_ppn_update,dreq.addr,(r_dcache_tlb_paddr.read() >> (uint32_log2(m_dcache_words)+2)),&victim_index))
-            {
-                r_dcache.setinbit((paddr_t)victim_index << (uint32_log2(m_dcache_words)+2), r_dcache_in_dtlb, false);
-            }
-            bool set_hit = r_dcache.setinbit(r_dcache_tlb_paddr, r_dcache_in_dtlb, true);
-            assert(set_hit && "TLB2_UPDT set hit error");  
-            r_dcache_fsm = DCACHE_IDLE;
+	    bool cleanup = dcache_tlb.select((dreq.addr >> PAGE_K_NBITS),&victim_index,&way,&set);
+	    if (cleanup) 
+	    {
+	        r_dcache_dtlb_cleanup_req = true;
+		r_dcache_dtlb_cleanup_line = victim_index;
+		m_cpt_cc_cleanup_data++;
+	    }
+	    r_dcache_way = way;
+	    r_dcache_set = set;
+            r_dcache_fsm = DCACHE_TLB2_UPDT;
         }
         else  
@@ -4119,4 +4123,17 @@
         }
         break;
+    }
+    //////////////////////
+    case DCACHE_TLB2_UPDT:  
+    {
+        m_cost_data_tlb_miss_frz++;
+        m_cost_data_tlb_update_acc_frz++;
+
+	if (r_dcache_dtlb_cleanup_req) r_dcache.setinbit(r_dcache_dtlb_cleanup_line.read() << (uint32_log2(m_dcache_words)+2), r_dcache_in_dtlb, false);
+        bool set_hit = r_dcache.setinbit(r_dcache_tlb_paddr, r_dcache_in_dtlb, true);
+        assert(set_hit && "TLB2_UPDT set hit error");  
+        dcache_tlb.update(r_dcache_pte_update,r_dcache_ppn_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;
     }
     ///////////////////////
@@ -4889,5 +4906,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     || 
              ( r_dcache_fsm_save == DCACHE_TLB1_READ_UPDT )   || ( r_dcache_fsm_save == DCACHE_TLB2_READ_UPDT )   ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )) && (dreq.valid) )
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )) && (dreq.valid) )
         {
             m_cost_data_tlb_miss_frz++;
@@ -4933,5 +4950,5 @@
             if ( dcache_hit )
             {
-		if (((( r_dcache_fsm_save == DCACHE_TLB1_UPDT )     || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )     ||
+		if (((( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL ) || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL ) ||
 		      ( r_dcache_fsm_save == DCACHE_TLB1_LL_WAIT )  || ( r_dcache_fsm_save == DCACHE_TLB2_LL_WAIT )  ||
 	              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )  || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )  ||
@@ -4992,5 +5009,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     || 
              ( r_dcache_fsm_save == DCACHE_TLB1_READ_UPDT )   || ( r_dcache_fsm_save == DCACHE_TLB2_READ_UPDT )   ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )) && (dreq.valid) )
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )) && (dreq.valid) )
         {
             m_cost_data_tlb_miss_frz++;
@@ -5023,5 +5040,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     || 
              ( r_dcache_fsm_save == DCACHE_TLB1_READ_UPDT )   || ( r_dcache_fsm_save == DCACHE_TLB2_READ_UPDT )   ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )) && (dreq.valid) )
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )) && (dreq.valid) )
         {
             m_cost_data_tlb_miss_frz++;
@@ -5046,5 +5063,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     || 
              ( r_dcache_fsm_save == DCACHE_TLB1_READ_UPDT )   || ( r_dcache_fsm_save == DCACHE_TLB2_READ_UPDT )   ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )) && (dreq.valid) )
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )) && (dreq.valid) )
         {
             m_cost_data_tlb_miss_frz++;
@@ -5077,5 +5094,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     || 
              ( r_dcache_fsm_save == DCACHE_TLB1_READ_UPDT )   || ( r_dcache_fsm_save == DCACHE_TLB2_READ_UPDT )   ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )) && (dreq.valid) )
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )) && (dreq.valid) )
         {
             m_cost_data_tlb_miss_frz++;
@@ -5088,5 +5105,5 @@
              ( r_dcache_fsm_save == DCACHE_TLB1_LL_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_LL_WAIT )     ||
              ( r_dcache_fsm_save == DCACHE_TLB1_SC_WAIT )     || ( r_dcache_fsm_save == DCACHE_TLB2_SC_WAIT )     ||
-             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT )        || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT )        ||
+             ( r_dcache_fsm_save == DCACHE_TLB1_UPDT_SEL )    || ( r_dcache_fsm_save == DCACHE_TLB2_UPDT_SEL )    ||
              ( r_dcache_fsm_save == DCACHE_DTLB1_READ_CACHE ) || ( r_dcache_fsm_save == DCACHE_DTLB2_READ_CACHE ) ||
              ( r_dcache_fsm_save == DCACHE_LL_DIRTY_WAIT )    || ( r_dcache_fsm_save == DCACHE_SC_DIRTY_WAIT )    ||
@@ -6045,4 +6062,5 @@
 
     } // end switch TGT_FSM
+
 #ifdef SOCLIB_MODULE_DEBUG 
    std::cout << name() 
