Index: trunk/modules/vci_cc_vcache_wrapper_v4/caba/source/src/vci_cc_vcache_wrapper_v4.cpp
===================================================================
--- trunk/modules/vci_cc_vcache_wrapper_v4/caba/source/src/vci_cc_vcache_wrapper_v4.cpp	(revision 203)
+++ trunk/modules/vci_cc_vcache_wrapper_v4/caba/source/src/vci_cc_vcache_wrapper_v4.cpp	(revision 204)
@@ -32,6 +32,4 @@
 #define DEBUG_ICACHE		1
 #define DEBUG_CLEANUP		0
-#define DEBUG_INVAL_ITLB	1
-#define DEBUG_INVAL_DTLB	1
 
 namespace soclib { 
@@ -86,5 +84,5 @@
         "DCACHE_XTN_DC_INVAL_VA",
         "DCACHE_XTN_DC_INVAL_PA",
-        "DCACHE_XTN_DC_INVAL_WAIT",
+        "DCACHE_XTN_DC_INVAL_END",
         "DCACHE_XTN_DC_INVAL_GO",
         "DCACHE_XTN_DT_INVAL",
@@ -106,5 +104,6 @@
         "DCACHE_CC_INVAL",
         "DCACHE_CC_UPDT",
-        "DCACHE_CC_WAIT",
+
+        "DCACHE_INVAL_TLB_SCAN",
     };
 const char *cmd_fsm_state_str[] = {
@@ -142,12 +141,4 @@
         "TGT_RSP_DCACHE",
     };	
-const char *inval_itlb_fsm_state_str[] = {
-        "INVAL_ITLB_IDLE",        
-        "INVAL_ITLB_SCAN", 
-    };
-const char *inval_dtlb_fsm_state_str[] = {
-        "INVAL_DTLB_IDLE",        
-        "INVAL_DTLB_SCAN", 
-    };
 }
 
@@ -280,4 +271,5 @@
       r_dcache_p1_tlb_set("r_dcache_p1_tlb_set"),
       r_dcache_p1_tlb_nline("r_dcache_p1_tlb_nline"),
+      r_dcache_p1_tlb_big("r_dcache_p1_tlb_big"),
 
       r_dcache_p2_vaddr("r_dcache_p2_vaddr"),
@@ -289,5 +281,5 @@
       r_dcache_p2_pte_set("r_dcache_p2_pte_set"),
       r_dcache_p2_pte_word("r_dcache_p2_pte_word"),
-      r_dcache_p2_pte_flags("r_dcache_p2_pte_flags"),
+      r_dcache_p2_pte("r_dcache_p2_pte"),
 
       r_dcache_vci_paddr("r_dcache_vci_paddr"),
@@ -333,4 +325,5 @@
       r_dcache_dtlb_inval_req("r_dcache_dtlb_inval_req"),
       r_dcache_tlb_inval_line("r_dcache_tlb_inval_line"),
+      r_dcache_tlb_inval_count("r_dcache_tlb_inval_count"),
 
       r_dcache_xtn_req("r_dcache_xtn_req"),
@@ -373,10 +366,4 @@
       r_tgt_dcache_rsp("r_tgt_dcache_rsp"),
 
-      r_inval_itlb_fsm("r_inval_itlb_fsm"),          
-      r_inval_itlb_count("r_inval_itlb_count"),          
-
-      r_inval_dtlb_fsm("r_inval_dtlb_fsm"),          
-      r_inval_dtlb_count("r_inval_dtlb_count"),          
-
       r_iss(this->name(), proc_id),
       r_wbuf("wbuf", wbuf_nwords, wbuf_nlines, dcache_words ),
@@ -394,4 +381,10 @@
     assert( (icache_words == dcache_words) and
              "icache_words and dcache_words parameters must be equal");
+
+    assert( (itlb_sets == dtlb_sets) and
+             "itlb_sets and dtlb_sets parameters must be etqual");
+
+    assert( (itlb_ways == dtlb_ways) and
+             "itlb_ways and dtlb_ways parameters must be etqual");
 
     r_mmu_params = (uint32_log2(m_dtlb_ways)   << 29)   | (uint32_log2(m_dtlb_sets)   << 25) |
@@ -489,8 +482,6 @@
               << " | " << cmd_fsm_state_str[r_vci_cmd_fsm.read()]
               << " | " << rsp_fsm_state_str[r_vci_rsp_fsm.read()]
-              << " | " << tgt_fsm_state_str[r_tgt_fsm.read()] << std::endl;
-    std::cout << "  " << cleanup_fsm_state_str[r_cleanup_fsm.read()]
-              << " | " << inval_itlb_fsm_state_str[r_inval_itlb_fsm] 
-              << " | " << inval_dtlb_fsm_state_str[r_inval_dtlb_fsm];
+              << " | " << tgt_fsm_state_str[r_tgt_fsm.read()] 
+              << " | " << cleanup_fsm_state_str[r_cleanup_fsm.read()];
     if (r_dcache_p0_valid.read() ) std::cout << " | P1_WRITE";
     if (r_dcache_p1_valid.read() ) std::cout << " | P2_WRITE";
@@ -702,6 +693,4 @@
         r_vci_rsp_fsm     = RSP_IDLE;
         r_tgt_fsm         = TGT_IDLE;
-        r_inval_itlb_fsm  = INVAL_ITLB_IDLE;          
-        r_inval_dtlb_fsm  = INVAL_DTLB_IDLE;          
         r_cleanup_fsm     = CLEANUP_DATA_IDLE;
 
@@ -743,5 +732,5 @@
 	r_dcache_ll_valid          = false;
 
-        // No request from DCACHE FSM to INVAL TLB FSMs
+        // No TLB inval requestis
         r_dcache_itlb_inval_req    = false;
         r_dcache_dtlb_inval_req    = false;
@@ -770,6 +759,4 @@
         m_debug_icache_fsm	  = false;
         m_debug_cleanup_fsm  	  = false;
-        m_debug_inval_itlb_fsm    = false;
-        m_debug_inval_dtlb_fsm    = false;
 
 /*
@@ -906,6 +893,4 @@
     m_debug_icache_fsm     = (m_cpt_total_cycles > m_debug_start_cycle) and m_debug_ok;
     m_debug_dcache_fsm     = (m_cpt_total_cycles > m_debug_start_cycle) and m_debug_ok;
-    m_debug_inval_itlb_fsm = (m_cpt_total_cycles > m_debug_start_cycle) and m_debug_ok;
-    m_debug_inval_dtlb_fsm = (m_cpt_total_cycles > m_debug_start_cycle) and m_debug_ok;
 
     /////////////////////////////////////////////////////////////////////
@@ -1911,68 +1896,4 @@
     m_irsp_instruction  = irsp.instruction;
     m_irsp_error        = irsp.error;
-
-    ////////////////////////////////////////////////////////////////////////////////////
-    //      INVAL ITLB FSM 
-    // This FSM works in parallel with the ICACHE FSM.
-    // When the r_dcache_itlb_inval_req flip-flop is activated by the DCACHE FSM
-    // it scans sequencially all entries in the ITLB, and invalidates the
-    // entries matching the evicted line.
-    // It signals the completion of invalidation by reseting r_dcache_itlb_inval_req.
-    ////////////////////////////////////////////////////////////////////////////////////
-
-    switch(r_inval_itlb_fsm) 
-    {
-    /////////////////////
-    case INVAL_ITLB_IDLE:
-    {
-        if ( r_dcache_itlb_inval_req.read() )
-        {
-            r_itlb.reset_bypass(r_dcache_tlb_inval_line.read());
-            r_inval_itlb_count = 0;
-            r_inval_itlb_fsm   = INVAL_ITLB_SCAN;   
-
-#if DEBUG_INVAL_ITLB
-if ( m_debug_inval_itlb_fsm )
-{
-    std::cout << "  <PROC.INVAL_ITLB_IDLE> Invalidate request for line " 
-              << std::hex << r_dcache_tlb_inval_line.read() << std::endl;
-    r_itlb.print();
-}
-#endif
-        }   
-        break;
-    }
-    /////////////////////
-    case INVAL_ITLB_SCAN:
-    {
-        paddr_t	line = r_dcache_tlb_inval_line.read(); 		// nline
-        size_t	way  = r_inval_itlb_count.read()/m_itlb_sets;	// way
-        size_t  set  = r_inval_itlb_count.read()%m_itlb_sets;	// set
-
-        bool ok = r_itlb.inval( line,
-                                way,
-                                set );
-
-#if DEBUG_INVAL_ITLB
-if ( m_debug_inval_itlb_fsm )
-{
-    std::cout << "  <PROC.INVAL_ITLB_SCAN>" << std::hex
-              << " line = " << line << std::dec
-              << " / set = " << set
-              << " / way = " << way;
-    if ( ok ) std::cout << " / HIT" << std::endl;
-    else      std::cout << " / MISS" << std::endl;
-}
-#endif
-
-        r_inval_itlb_count = r_inval_itlb_count.read() + 1;
-        if ( r_inval_itlb_count.read() == (m_itlb_sets*m_itlb_ways - 1) ) 
-        {
-            r_inval_itlb_fsm        = INVAL_ITLB_IDLE;
-            r_dcache_itlb_inval_req = false;
-        }
-        break;
-    }        
-    } // end switch r_inval_itlb_fsm
 
     ////////////////////////////////////////////////////////////////////////////////////
@@ -2067,42 +1988,28 @@
         // Three actions are executed in this P2 stage:
         // - If r_dcache_p1_updt_cache is true, we update the local copy in dcache.
-        // - If the modified cache line has copies in itlb (resp. dtlb), and if the
-        //   INVAL_ITLB (resp. INVAL_DTLB) FSM is idle, we launch the TLB invalidate
-        //   operation. This operation is NOT blocking for the processor: the DCACHE FSM 
-        //   doesn't wait the TLB invalidate completion to handle processor requests.
-        //   If the INVAL_ITLB (resp INVAL_DTLB) FSM is already processintg a previous
-        //   invalidation request, the DCACHE FSM is frosen until completion of the 
-        //   previous  TLB invalidate operation.
+        // - If the modified cache line has copies in TLBs, we launch a TLB invalidate
+        //   operation, that is blocking for the processor, because we switch to
+        //   DCACHE_INVAL_TLB 
         // - If the PTE dirty bit must be updated, we start a "long write", that is 
         //   blocking for the processor, because we switch to the DCACHE_WRITE_SET_DIRTY
-        //   state, and the number of cycles can be large...
 
         bool long_write_set_dirty = false;
-        bool tlb_inval_frozen	  = false;
+        bool tlb_inval_required	  = false;
 
         if ( r_dcache_p1_valid.read() )		// P2 stage activated
         {
             bool     cache_updt = r_dcache_p1_updt_cache.read();
-            size_t   cache_way  = r_dcache_p1_cache_way.read();
-            size_t   cache_set  = r_dcache_p1_cache_set.read();
-            size_t   cache_word = r_dcache_p1_cache_word.read();
+            size_t   way        = r_dcache_p1_cache_way.read();
+            size_t   set        = r_dcache_p1_cache_set.read();
+            size_t   word       = r_dcache_p1_cache_word.read();
             uint32_t wdata      = r_dcache_p1_wdata.read();
             vci_be_t be         = r_dcache_p1_be.read();
 
-            // The PTE address is used when the PTE dirty bit must be set.
-            // It is the concatenation of the nline value (from dtlb)
-            // and the word index (from virtual address)
-            paddr_t  pte_paddr = (paddr_t)(r_dcache_p1_tlb_nline.read()*(m_dcache_words<<2)) 
-                                 | (paddr_t)(r_dcache_p1_vaddr.read()%(m_dcache_words<<2));
-           
-            // The line index is used when a TLB inval is required
-            paddr_t  inval_line = r_dcache_p1_paddr.read()>>(uint32_log2(m_dcache_words<<2)); 
-
-            // checking dcache update
+            // update dcache if required
             if ( cache_updt )	
             {
-                r_dcache.write( cache_way,
-                                cache_set,
-                                cache_word,
+                r_dcache.write( way,
+                                set,
+                                word,
                                 wdata,
                                 be );
@@ -2110,35 +2017,48 @@
 m_cpt_dcache_data_write++; 
 #endif
-
-                // Checking copies in TLBs
-                bool itlb_inval = ( (r_mmu_mode.read() & INS_TLB_MASK) and 
-                                    r_dcache_in_itlb[cache_way*m_dcache_sets+cache_set] );
-                bool dtlb_inval = ( (r_mmu_mode.read() & DATA_TLB_MASK) and 
-                                    r_dcache_in_dtlb[cache_way*m_dcache_sets+cache_set] );
-
-                if ( (dtlb_inval and r_dcache_dtlb_inval_req.read() ) or
-                     (itlb_inval and r_dcache_itlb_inval_req.read() ) )	// at least one FSM not idle
+                // cache update can require itlb & dtlb inval
+                bool	inval_itlb = false;
+                bool	inval_dtlb = false;
+   
+                if ( (r_mmu_mode.read() & DATA_TLB_MASK) and r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
                 {
-                    tlb_inval_frozen = true;
+	            r_dcache_in_dtlb[way*m_dcache_sets+set] = false;
+                    inval_dtlb = true;
                 }
-                else							// requested FSM idle
+                if ( (r_mmu_mode.read() & INS_TLB_MASK) and r_dcache_in_itlb[m_dcache_sets*way+set] )
+	        {	
+	            r_dcache_in_itlb[way*m_dcache_sets+set] = false;
+                    inval_itlb = true;
+                }
+                if ( inval_itlb or inval_dtlb ) 
                 {
-                    r_dcache_tlb_inval_line = inval_line;
-                    r_dcache_itlb_inval_req = itlb_inval;
-                    r_dcache_dtlb_inval_req = dtlb_inval;
-                    r_dcache_in_itlb[cache_way*m_dcache_sets+cache_set] = false;
-                    r_dcache_in_dtlb[cache_way*m_dcache_sets+cache_set] = false;
+                    tlb_inval_required       = true;
+	            r_dcache_itlb_inval_req  = inval_itlb;
+	            r_dcache_dtlb_inval_req  = inval_dtlb;
+	            r_dcache_tlb_inval_count = 0;
+	            r_dcache_tlb_inval_line  = r_dcache_p1_paddr.read()>>(uint32_log2(m_dcache_words<<2)); 
                 }
             } // end dcache update
 
-            // checking dirty bit update 
+            // checking if dirty bit update is required
             if ( r_dcache_p1_set_dirty.read() )  
             {
-                long_write_set_dirty = true;
+                long_write_set_dirty  = true;
                 r_dcache_p2_vaddr     = r_dcache_p1_vaddr.read();
                 r_dcache_p2_set_dirty = r_dcache_p1_set_dirty.read();
                 r_dcache_p2_tlb_way   = r_dcache_p1_tlb_way.read();	
                 r_dcache_p2_tlb_set   = r_dcache_p1_tlb_set.read();	
-                r_dcache_p2_pte_paddr = pte_paddr;
+                // The PTE physical address is the concatenation of the nline value (from dtlb),
+                // with the word index (obtained from the proper bits of the virtual address)
+                if ( r_dcache_p1_tlb_big.read() )	// PTE1
+                {
+                    r_dcache_p2_pte_paddr = (paddr_t)(r_dcache_p1_tlb_nline.read()*(m_dcache_words<<2)) |
+                                            (paddr_t)((r_dcache_p1_vaddr.read()>>19) & 0x3c);
+                }
+                else					// PTE2
+                {
+                    r_dcache_p2_pte_paddr = (paddr_t)(r_dcache_p1_tlb_nline.read()*(m_dcache_words<<2)) |
+                                            (paddr_t)((r_dcache_p1_vaddr.read()>>10) & 0x3c);
+                }
             }
 
@@ -2148,12 +2068,12 @@
     if ( cache_updt ) 
         std::cout << "  <PROC.DCACHE_IDLE> P2 stage: cache update" << std::dec
-                  << " / way = " << cache_way 
-                  << " / set = " << cache_set 
-                  << " / word = " << cache_word << std::hex
+                  << " / way = " << way 
+                  << " / set = " << set 
+                  << " / word = " << word << std::hex
                   << " / wdata = " << wdata
                   << " / be = " << be << std::endl;
     if ( long_write_set_dirty ) 
         std::cout << "  <PROC.DCACHE_IDLE> P2 stage: dirty bit update required"
-                  << " / pte_paddr = " << std::hex << pte_paddr << std::endl;
+                  << " / pte_paddr = " << std::hex << r_dcache_p2_pte_paddr.read() << std::endl;
 }
 #endif
@@ -2172,5 +2092,5 @@
         // if there is local copy in dcache, or if the PTE dirty bit must be set.
 
-        if ( r_dcache_p0_valid.read() and not tlb_inval_frozen )  // P1 stage activated
+        if ( r_dcache_p0_valid.read() and not tlb_inval_required )  // P1 stage activated
         {
             // write not cacheable, and previous non cacheable write registered
@@ -2234,4 +2154,5 @@
                 r_dcache_p1_tlb_set     = r_dcache_p0_tlb_set;
                 r_dcache_p1_tlb_nline   = r_dcache_p0_tlb_nline;
+                r_dcache_p1_tlb_big     = r_dcache_p0_tlb_big;
                 r_dcache_p1_cache_way   = cache_way;
                 r_dcache_p1_cache_set   = cache_set;
@@ -2249,8 +2170,8 @@
 
         /////////////////////////////////////////////////////////////////////////////
-        // handling P0 write pipe-line stage
+        // handling P0 pipe-line stage
         // This stage is controlling the DCACHE FSM state register:
-        // - the FSM is frozen if a TLB invalidate operation must be delayed,
-        // - the long write requests have the highest priority,
+        // - the TLB invalidate requests have the highest priority,
+        // - then the long write requests, 
         // - then the external coherence requests,
         // - then the itlb miss requests,
@@ -2263,8 +2184,9 @@
         bool p0_valid = false;	// default value
 
-        // TLB inval delayed
-        if ( tlb_inval_frozen )
-        {
-            break;
+        // TLB inval required
+        if ( tlb_inval_required )
+        {
+            r_dcache_fsm_save = DCACHE_IDLE;
+            r_dcache_fsm      = DCACHE_INVAL_TLB_SCAN;
         }
 
@@ -2278,6 +2200,6 @@
         else if ( r_tgt_dcache_req.read() )   
         {
-            r_dcache_fsm = DCACHE_CC_CHECK;
             r_dcache_fsm_save = DCACHE_IDLE;
+            r_dcache_fsm      = DCACHE_CC_CHECK;
         }        
 
@@ -2747,7 +2669,7 @@
     } 
     /////////////////////
-    case DCACHE_TLB_MISS: // This is the entry point for the sub-fsm handling tlb miss.
+    case DCACHE_TLB_MISS: // This is the entry point for the sub-fsm handling all tlb miss.
                           // - Input arguments are r_dcache_tlb_vaddr & r_dcache_tlb_ins 
-                          // - It try to find the missing TLB entry in dcache,
+                          // - The sub-fsm access the dcache to find the missing TLB entry,
                           //   and activates the cache miss procedure in case of miss.
                           // - It bypass the first level page table access if possible.
@@ -2778,5 +2700,5 @@
             r_dcache_fsm       = DCACHE_TLB_PTE1_GET;
         }
-        else                  // Try to read directly the PTE2 in dcache
+        else                  // Try to read the PTE2 in dcache
         {
             paddr = (paddr_t)ptba << PAGE_K_NBITS |
@@ -2818,5 +2740,5 @@
 m_cpt_dcache_dir_read++;
 #endif
-        if ( hit )	// request hit in dcache 
+        if ( hit )	//  hit in dcache 
         {
             if ( not (entry & PTE_V_MASK) )	// unmapped
@@ -2841,6 +2763,7 @@
 if ( m_debug_dcache_fsm )
 {
-    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache, but unmapped:"
-              << std::dec << " way = " << way
+    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache, but unmapped"
+              << std::hex << " / paddr = " << r_dcache_tlb_paddr.read()
+              << std::dec << " / way = " << way
               << std::dec << " / set = " << set
               << std::dec << " / word = " << word
@@ -2872,6 +2795,7 @@
 if ( m_debug_dcache_fsm )
 {
-    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache "
-              << std::dec << " way = " << way
+    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache"
+              << std::hex << " / paddr = " << r_dcache_tlb_paddr.read()
+              << std::dec << " / way = " << way
               << std::dec << " / set = " << set
               << std::dec << " / word = " << word
@@ -2882,8 +2806,6 @@
             else			//  PTE1 :  update the TLB
             {
-                if ( r_dcache_tlb_ins.read() ) 
-                    r_dcache_in_itlb[m_icache_sets*way+set] = true;
-                else                           
-                    r_dcache_in_dtlb[m_dcache_sets*way+set] = true;
+                if ( r_dcache_tlb_ins.read() ) r_dcache_in_itlb[m_icache_sets*way+set] = true;
+                else                           r_dcache_in_dtlb[m_dcache_sets*way+set] = true;
                 r_dcache_tlb_pte_flags  = entry;
                 r_dcache_tlb_cache_way  = way;
@@ -2895,6 +2817,7 @@
 if ( m_debug_dcache_fsm )
 {
-    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache:"
-              << std::dec << " way = " << way
+    std::cout << "  <PROC.DCACHE_TLB_PTE1_GET> HIT in dcache"
+              << std::hex << " / paddr = " << r_dcache_tlb_paddr.read()
+              << std::dec << " / way = " << way
               << std::dec << " / set = " << set
               << std::dec << " / word = " << word
@@ -3263,4 +3186,10 @@
                                         // request a SC transaction to CMD FSM 
     {
+#if DEBUG_DCACHE
+if ( m_debug_dcache_fsm )
+{
+    std::cout << "  <PROC.DCACHE_TLB_SC_UPDT> Update dcache: (L/R) bit" << std::endl;
+}
+#endif
         r_dcache.write(r_dcache_tlb_cache_way.read(),
                        r_dcache_tlb_cache_set.read(),
@@ -3276,9 +3205,10 @@
     }
     ////////////////////////
-    case DCACHE_TLB_SC_WAIT:		    // wait response to SC transaction from RSP FSM
-                                        // we consume the response, and exit the sub-fsm.
-                                        // we don't analyse the response, because
-                                        // we don't care if the L/R bit update is not done
-                                        // we must take the coherence requests because
+    case DCACHE_TLB_SC_WAIT:		// Waiting a response to SC transaction.
+                                        // We consume the response in rsp FIFO, 
+                                        // and exit the sub-fsm, but we don't
+                                        // analyse the response, because we don't
+                                        // care if the L/R bit update is not done.
+                                        // We must take the coherence requests because
                                         // there is a risk of dead-lock
 
@@ -3292,19 +3222,32 @@
         }
 
-        if ( not r_dcache_vci_sc_req.read() ) 	// response available
-        {
-            if ( r_vci_rsp_data_error.read() )   	r_vci_rsp_data_error = false;
-            else if ( r_vci_rsp_fifo_dcache.rok() ) 	vci_rsp_fifo_dcache_get = true;
-            else
-            {
-                assert( false and "rsp_fifo should not be empty in DCACHE_TLB_SC_WAIT state" );
-            }
-            r_dcache_fsm     = DCACHE_TLB_RETURN;
+        if ( r_vci_rsp_data_error.read() ) 	// bus error
+        {
+            std::cout << "BUS ERROR in DCACHE_TLB_SC_WAIT state" << std::endl;
+            std::cout << "This should not happen in this state" << std::endl;
+            exit(0);
+        }
+	else if ( r_vci_rsp_fifo_dcache.rok() ) // response available
+	{
+#if DEBUG_DCACHE
+if ( m_debug_dcache_fsm )
+{
+    std::cout << "  <PROC.DCACHE_TLB_SC_WAIT> SC response received" << std::endl;
+}
+#endif
+            vci_rsp_fifo_dcache_get = true;     
+            r_dcache_fsm            = DCACHE_TLB_RETURN;
         }
         break;
     }
     ///////////////////////
-    case DCACHE_TLB_RETURN:		// return to caller state depending on the tlb miss type
-    {
+    case DCACHE_TLB_RETURN:		// return to caller depending on tlb miss type
+    {
+#if DEBUG_DCACHE
+if ( m_debug_dcache_fsm )
+{
+    std::cout << "  <PROC.DCACHE_TLB_RETURN> TLB MISS completed" << std::endl;
+}
+#endif
         if ( r_dcache_tlb_ins.read() ) r_icache_tlb_miss_req = false;
         r_dcache_fsm = DCACHE_IDLE;
@@ -3493,12 +3436,13 @@
     case DCACHE_XTN_DC_INVAL_GO:  // In this state, we invalidate the cache line & cleanup
         			  // Blocked if previous cleanup not completed
-             
+                                  // Test if itlb or dtlb inval is required    
     {
 	if ( not r_dcache_cleanup_req.read() )
 	{
             paddr_t	nline;
-            size_t	way       = r_dcache_xtn_way.read();
-            size_t	set       = r_dcache_xtn_set.read();
-            bool	inval_tlb = false;
+            size_t	way        = r_dcache_xtn_way.read();
+            size_t	set        = r_dcache_xtn_set.read();
+            bool	inval_itlb = false;
+            bool	inval_dtlb = false;
    
             r_icache.inval( way,
@@ -3511,25 +3455,25 @@
 	    
             // possible itlb & dtlb invalidate requests 
-	    r_dcache_tlb_inval_line = nline;
-
-            if ( (r_mmu_mode.read() & DATA_TLB_MASK) and 
-                 r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
-            {
-	        r_dcache_dtlb_inval_req = true;
+
+            if ( (r_mmu_mode.read() & DATA_TLB_MASK) and r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
+            {
 	        r_dcache_in_dtlb[way*m_dcache_sets+set] = false;
-                inval_tlb = true;
-            }
-            if ( (r_mmu_mode.read() & INS_TLB_MASK) and 
-                 r_dcache_in_itlb[m_dcache_sets*way+set] )
+                inval_dtlb = true;
+            }
+            if ( (r_mmu_mode.read() & INS_TLB_MASK) and r_dcache_in_itlb[m_dcache_sets*way+set] )
 	    {	
-	        r_dcache_itlb_inval_req = true;
 	        r_dcache_in_itlb[way*m_dcache_sets+set] = false;
-                inval_tlb = true;
+                inval_itlb = true;
             }
 
             // no valid response until itlb & dtlb invalidated
-            if (inval_tlb ) 
-            {
-                r_dcache_fsm = DCACHE_XTN_DC_INVAL_WAIT;
+            if ( inval_itlb or inval_dtlb ) 
+            {
+	        r_dcache_itlb_inval_req  = inval_itlb;
+	        r_dcache_dtlb_inval_req  = inval_dtlb;
+	        r_dcache_tlb_inval_line  = nline;
+	        r_dcache_tlb_inval_count = 0;
+                r_dcache_fsm_save        = DCACHE_XTN_DC_INVAL_END;
+                r_dcache_fsm             = DCACHE_INVAL_TLB_SCAN;
 	    }
             else
@@ -3542,11 +3486,8 @@
     }
     //////////////////////////////
-    case DCACHE_XTN_DC_INVAL_WAIT:  	// waiting completion of itlb and dtlb invalidate
-    {
-        if ( not (r_dcache_itlb_inval_req.read() or r_dcache_dtlb_inval_req.read()) ) 
-        {
-            r_dcache_fsm = DCACHE_IDLE;
-            drsp.valid = true;
-        } 
+    case DCACHE_XTN_DC_INVAL_END:  	// waiting completion of itlb and dtlb invalidate
+    {
+        r_dcache_fsm = DCACHE_IDLE;
+        drsp.valid = true;
         break;
     }
@@ -3836,4 +3777,5 @@
     ////////////////////////////
     case DCACHE_WRITE_TLB_DIRTY:	// set PTE dirty bit in dtlb
+					// and get PTE in dcache
     {
         // set dirty bit in dtlb
@@ -3842,10 +3784,10 @@
 
         // get PTE in dcache
-        uint32_t pte_flags = 0;
+        uint32_t pte = 0;
         size_t   way;
         size_t   set;
         size_t   word;
         bool     hit = r_dcache.read( r_dcache_p2_pte_paddr.read(),
-                                      &pte_flags,
+                                      &pte,
                                       &way,
                                       &set,
@@ -3859,8 +3801,13 @@
 if ( m_debug_dcache_fsm )
 {
-    std::cout << "  <PROC.DCACHE_WRITE_TLB_DIRTY> Set PTE dirty bit in dtlb:"
-              << " paddr = " << r_dcache_p2_pte_paddr.read() 
+    std::cout << "  <PROC.DCACHE_WRITE_TLB_DIRTY> Set dirty bit in dtlb:" << std::dec
               << " / tlb_way = " << r_dcache_p2_tlb_way.read()
               << " / tlb_set = " << r_dcache_p2_tlb_set.read() << std::endl;
+    r_dtlb.print();
+    std::cout << "                                Get PTE in dcache:" << std::hex
+              << " paddr = " << r_dcache_p2_pte_paddr.read() 
+              << " / PTE = " << pte << std::dec
+              << " / dcache_way = " << way
+              << " / dcache_set = " << set << std::endl;
 }
 #endif
@@ -3870,5 +3817,5 @@
         r_dcache_p2_pte_set   = set;			// register pte set in dcache;
         r_dcache_p2_pte_word  = word;			// register pte word in dcache;
-        r_dcache_p2_pte_flags = pte_flags;		// register pte value
+        r_dcache_p2_pte       = pte;			// register pte value
         r_dcache_fsm          = DCACHE_WRITE_CACHE_DIRTY;
         break;
@@ -3882,5 +3829,5 @@
                         r_dcache_p2_pte_set.read(),
                         r_dcache_p2_pte_word.read(),
-                        r_dcache_p2_pte_flags.read() | PTE_D_MASK,
+                        r_dcache_p2_pte.read() | PTE_D_MASK,
                         0xF );
 
@@ -3892,14 +3839,18 @@
 if ( m_debug_dcache_fsm )
 {
-    std::cout << "  <PROC.DCACHE_WRITE_CACHE_DIRTY> Set PTE dirty bit in dcache:"
+    std::cout << "  <PROC.DCACHE_WRITE_CACHE_DIRTY> Set PTE dirty bit in dcache"
               << " / way = " << r_dcache_p2_pte_way.read()
               << " / set = " << r_dcache_p2_pte_set.read() 
               << " / word = " << r_dcache_p2_pte_word.read() << std::endl;
+    std::cout << "                                  Request SC transaction"
+              << " / address = " << "bloup"
+              << " / old = " << r_dcache_p2_pte.read()
+              << " / new = " << (r_dcache_p2_pte.read() | PTE_D_MASK) << std::endl;
 }
 #endif
         // request sc transaction to CMD_FSM
         r_dcache_vci_sc_req = true;
-        r_dcache_vci_sc_old = r_dcache_p2_pte_flags.read();
-        r_dcache_vci_sc_new = r_dcache_p2_pte_flags.read() | PTE_D_MASK;
+        r_dcache_vci_sc_old = r_dcache_p2_pte.read();
+        r_dcache_vci_sc_new = r_dcache_p2_pte.read() | PTE_D_MASK;
         r_dcache_fsm        = DCACHE_WRITE_SC_WAIT;    
         break;
@@ -4065,7 +4016,8 @@
     {
         paddr_t nline;
-        size_t	way       = r_dcache_cc_way.read();
-        size_t	set       = r_dcache_cc_set.read();
-        bool    inval_tlb = false;
+        size_t	way        = r_dcache_cc_way.read();
+        size_t	set        = r_dcache_cc_set.read();
+        bool    inval_itlb = false;
+        bool    inval_dtlb = false;
 
         r_dcache.inval( way, 
@@ -4074,25 +4026,25 @@
 	    
         // possible itlb & dtlb invalidate requests 
-        r_dcache_tlb_inval_line = nline;
-
-        if ( (r_mmu_mode.read() & DATA_TLB_MASK) and 
-             r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
-        {
-            r_dcache_dtlb_inval_req = true;
+
+        if ( (r_mmu_mode.read() & DATA_TLB_MASK) and r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
+        {
+            r_dtlb.reset_bypass(nline);
             r_dcache_in_dtlb[way*m_dcache_sets+set] = false;
-            inval_tlb = true;
-        }
-        if ( (r_mmu_mode.read() & INS_TLB_MASK) and 
-            r_dcache_in_itlb[m_dcache_sets*way+set] )
+            inval_dtlb = true;
+        }
+        if ( (r_mmu_mode.read() & INS_TLB_MASK) and r_dcache_in_itlb[m_dcache_sets*way+set] )
         {	
-            r_dcache_itlb_inval_req = true;
+            r_itlb.reset_bypass(nline);
             r_dcache_in_itlb[way*m_dcache_sets+set] = false;
-            inval_tlb = true;
-        }
-
-        // no valid response until itlb & dtlb invalidated
-        if (inval_tlb ) 
-        {
-            r_dcache_fsm = DCACHE_CC_WAIT;
+            inval_itlb = true;
+        }
+
+        if ( inval_itlb or inval_dtlb ) // no valid response until itlb / dtlb invalidated
+        {
+            r_dcache_itlb_inval_req  = inval_itlb;
+            r_dcache_dtlb_inval_req  = inval_dtlb;
+            r_dcache_tlb_inval_line  = nline;
+            r_dcache_tlb_inval_count = 0;
+            r_dcache_fsm             = DCACHE_INVAL_TLB_SCAN;
         }
         else
@@ -4108,5 +4060,8 @@
     std::cout << "  <PROC.DCACHE_CC_INVAL> Invalidate cache line :" << std::dec
 	      << " way = " << way
-	      << " / set = " << set << std::endl;
+	      << " / set = " << set;
+    if ( inval_itlb ) std::cout << " / itlb inval required";
+    if ( inval_dtlb ) std::cout << " / dtlb inval required";
+    std::cout << std::endl;
 }
 #endif
@@ -4118,7 +4073,10 @@
                                 // and test possible copies in TLBs
     {
-        size_t	word  = r_dcache_cc_word.read();
-        size_t	way   = r_dcache_cc_way.read();
-        size_t	set   = r_dcache_cc_set.read();
+        size_t	 word       = r_dcache_cc_word.read();
+        size_t	 way        = r_dcache_cc_way.read();
+        size_t	 set        = r_dcache_cc_set.read();
+        bool     inval_itlb = false;
+        bool     inval_dtlb = false;
+        paddr_t  nline	    = r_tgt_paddr.read() >> (uint32_log2(m_dcache_words)+2);
 
         r_dcache.write( way,
@@ -4134,15 +4092,26 @@
         if ( word == r_tgt_word_max.read() )	// last word
         {
-            // invalidate copies in TLBs
-            if ( (r_mmu_mode.read() & DATA_TLB_MASK) and 
-                 ( r_dcache_in_itlb[way*m_dcache_sets+set] or 
-                   r_dcache_in_dtlb[m_dcache_sets*way+set] ) )
-            {
-                r_dcache_tlb_inval_line = r_tgt_paddr.read() >> (uint32_log2(m_dcache_words)+2);
-                r_dcache_itlb_inval_req = r_dcache_in_itlb[m_dcache_sets*way+set];
+            // possible itlb & dtlb invalidate requests 
+
+            if ( (r_mmu_mode.read() & DATA_TLB_MASK) and r_dcache_in_dtlb[way*m_dcache_sets+set] ) 
+            {
+                r_dtlb.reset_bypass(nline);
+                r_dcache_in_dtlb[way*m_dcache_sets+set] = false;
+                inval_dtlb = true;
+            }
+            if ( (r_mmu_mode.read() & INS_TLB_MASK) and r_dcache_in_itlb[way*m_dcache_sets+set] ) 
+            {
+                r_itlb.reset_bypass(nline);
                 r_dcache_in_itlb[way*m_dcache_sets+set] = false;
-                r_dcache_dtlb_inval_req = r_dcache_in_dtlb[m_dcache_sets*way+set]; 
-                r_dcache_in_dtlb[way*m_dcache_sets+set] = false;
-      	        r_dcache_fsm     = DCACHE_CC_WAIT;
+                inval_itlb = true;
+            }
+
+            if ( inval_itlb or inval_dtlb ) // no valid response until itlb / dtlb invalidated
+            {
+                r_dcache_itlb_inval_req  = inval_itlb;
+                r_dcache_dtlb_inval_req  = inval_dtlb;
+                r_dcache_tlb_inval_line  = nline;
+                r_dcache_tlb_inval_count = 0;
+                r_dcache_fsm             = DCACHE_INVAL_TLB_SCAN;
             }
             else
@@ -4161,5 +4130,8 @@
 	      << " / set = " << set 
               << " / word = " << word
-              << " / value = " << std::hex << r_tgt_buf[word] << std::endl;
+              << " / value = " << std::hex << r_tgt_buf[word];
+    if ( inval_itlb ) std::cout << " / itlb inval required";
+    if ( inval_dtlb ) std::cout << " / dtlb inval required";
+    std::cout << std::endl;
 }
 #endif
@@ -4167,13 +4139,69 @@
         break;
     }
-    ////////////////////
-    case DCACHE_CC_WAIT:     	// wait completion of TLB invalidate
-    {
-        if ( not r_dcache_itlb_inval_req.read() and not r_dcache_dtlb_inval_req.read() )
-        {
-            r_tgt_dcache_rsp = true;
-            r_tgt_dcache_req = false;
-            r_dcache_fsm     = r_dcache_fsm_save.read();
-        }
+    ///////////////////////////
+    case DCACHE_INVAL_TLB_SCAN:     	// scan sequencially all TLB entries for both ITLB & DTLB
+                                        // It makes the assumption that (m_itlb_sets == m_dtlb_sets)
+                                        // and (m_itlb_ways == m_dtlb_ways)
+                                        // Caution : we enter this state when a DCACHE line is modified,
+                                        // and there is a copy in itlb or dtlb.
+                                        // It can be caused by a coherence transaction, a XTN inval
+                                        // or a WRITE hit.  Input arguments are:
+                                        // - r_dcache_tlb_inval_line
+                                        // - r_dcache_tlb_inval_count
+                                        // - r_dcache_itlb_inval_req
+                                        // - r_dcache_dtlb_inval_req
+                                        // - r_dcache_fsm_save
+    {
+        paddr_t	line = r_dcache_tlb_inval_line.read(); 		        // nline
+        size_t	way  = r_dcache_tlb_inval_count.read()/m_itlb_sets;	// way
+        size_t  set  = r_dcache_tlb_inval_count.read()%m_itlb_sets;	// set
+
+        if ( r_dcache_itlb_inval_req.read() )
+        {
+            bool ok = r_itlb.inval( line,
+                                    way,
+                                    set );
+#if DEBUG_DCACHE
+if ( m_debug_dcache_fsm and ok )
+{
+    std::cout << "  <PROC.DCACHE_INVAL_TLB_SCAN> Invalidate ITLB entry:" << std::hex 
+              << " line = " << line << std::dec
+              << " / set = " << set 
+              << " / way = " << way << std::endl;
+    r_itlb.print();
+}
+#endif
+        }
+
+        if ( r_dcache_dtlb_inval_req.read() )
+        {
+            bool ok = r_dtlb.inval( line,
+                                    way,
+                                    set );
+#if DEBUG_DCACHE
+if ( m_debug_dcache_fsm and ok )
+{
+    std::cout << "  <PROC.DCACHE_INVAL_TLB_SCAN> Invalidate DTLB entry:" << std::hex 
+              << " line = " << line << std::dec
+              << " / set = " << set 
+              << " / way = " << way << std::endl;
+    r_dtlb.print();
+}
+#endif
+        }
+
+        // return to the calling state when TLB inval completed
+        if ( r_dcache_tlb_inval_count.read() == (m_dtlb_sets*m_dtlb_ways-1) )
+        {
+            if ( r_dcache_fsm_save.read() != DCACHE_XTN_DC_INVAL_END ) // not an XTN inval
+            {
+                r_tgt_dcache_rsp    = true;
+                r_tgt_dcache_req    = false;
+            }
+            r_dcache_dtlb_inval_req = false;
+            r_dcache_itlb_inval_req = false;
+            r_dcache_fsm            = r_dcache_fsm_save.read();
+        }
+        r_dcache_tlb_inval_count = r_dcache_tlb_inval_count.read() + 1;
     }   
     } // end switch r_dcache_fsm
@@ -4194,68 +4222,4 @@
     ///////////////// wbuf update //////////////////////////////////////////////////////
     r_wbuf.update();
-
-    ////////////////////////////////////////////////////////////////////////////////////
-    //      INVAL DTLB FSM 
-    // This FSM works in parallel with the DCACHE FSM.
-    // When the r_dcache_dtlb_inval_req flip-flop is activated by the DCACHE FSM
-    // it scans sequencially all entries in the DTLB, and invalidates the
-    // entries matching the evicted line.
-    // It signals the completion of invalidation by reseting r_dcache_itlb_inval_req.
-    ////////////////////////////////////////////////////////////////////////////////////
-
-    switch(r_inval_dtlb_fsm) 
-    {
-    /////////////////////
-    case INVAL_DTLB_IDLE:
-    {
-        if ( r_dcache_dtlb_inval_req.read() ) 
-        {
-            r_dtlb.reset_bypass(r_dcache_tlb_inval_line.read());
-            r_inval_dtlb_count = 0;
-            r_inval_dtlb_fsm   = INVAL_DTLB_SCAN;    
-
-#if DEBUG_INVAL_DTLB
-if ( m_debug_inval_dtlb_fsm )
-{
-    std::cout << "  <PROC.INVAL_DTLB_IDLE> Invalidate request for line " 
-              << std::hex << r_dcache_tlb_inval_line.read() << std::endl;
-    r_dtlb.print();
-}
-#endif
-        }   
-        break;
-    }
-    /////////////////////
-    case INVAL_DTLB_SCAN:
-    {
-        paddr_t	line = r_dcache_tlb_inval_line.read(); 		// nline
-        size_t	way  = r_inval_dtlb_count.read()/m_itlb_sets;	// way
-        size_t  set  = r_inval_dtlb_count.read()%m_itlb_sets;	// set
-
-        bool ok = r_dtlb.inval( line,
-                                way,
-                                set );
-
-#if DEBUG_INVAL_DTLB
-if ( m_debug_inval_dtlb_fsm )
-{
-    std::cout << "  <PROC.INVAL_DTLB_SCAN>" << std::hex
-              << " line = " << line << std::dec
-              << " / set = " << set
-              << " / way = " << way;
-    if ( ok ) std::cout << " / HIT" << std::endl;
-    else      std::cout << " / MISS" << std::endl;
-}
-#endif
-                   
-        r_inval_dtlb_count = r_inval_dtlb_count.read() + 1;
-        if ( r_inval_dtlb_count.read() == (m_dtlb_sets*m_dtlb_ways - 1) )
-        {
-            r_inval_dtlb_fsm        = INVAL_DTLB_IDLE;
-            r_dcache_dtlb_inval_req = false;
-        }
-        break;
-    }
-    } // end switch r_inval_dtlb_fsm
 
     /////////// test processor frozen /////////////////////////////////////////////
@@ -4385,5 +4349,5 @@
             {
                 r_vci_cmd_fsm       = CMD_DATA_SC;
-                r_dcache_vci_sc_req     = false;
+                r_dcache_vci_sc_req = false;
                 r_vci_cmd_cpt       = 0;
 //                m_cpt_sc_transaction++;
