Index: trunk/modules/vci_io_bridge/caba/source/src/vci_io_bridge.cpp
===================================================================
--- trunk/modules/vci_io_bridge/caba/source/src/vci_io_bridge.cpp	(revision 952)
+++ trunk/modules/vci_io_bridge/caba/source/src/vci_io_bridge.cpp	(revision 984)
@@ -23,4 +23,6 @@
  *
  * SOCLIB_LGPL_HEADER_END
+ *
+ * Maintainers: Cesar Fuguet Tortolero <cesar.fuguet-tortolero@lip6.fr>
  */
 
@@ -115,5 +117,6 @@
 }
 
-#define tmpl(...)  template<typename vci_param_int,typename vci_param_ext> __VA_ARGS__ VciIoBridge<vci_param_int,vci_param_ext>
+#define tmpl(...)  template<typename vci_param_int,typename vci_param_ext> \
+    __VA_ARGS__ VciIoBridge<vci_param_int,vci_param_ext>
 
 ////////////////////////
@@ -258,4 +261,6 @@
       r_miss_wti_rsp_to_dma_rsp_rtrdid("r_miss_wti_rsp_to_dma_rsp_rtrdid"),
       r_miss_wti_rsp_to_dma_rsp_rpktid("r_miss_wti_rsp_to_dma_rsp_rpktid"),
+
+      r_miss_wti_rsp_to_tlb_done("r_miss_wti_rsp_to_tlb_done"),
 
       // TLB for IOMMU
@@ -546,4 +551,5 @@
         r_tlb_to_miss_wti_cmd_req      = false;
         r_miss_wti_rsp_to_dma_rsp_req  = false;
+        r_miss_wti_rsp_to_tlb_done     = false;
 
         // error flip_flops
@@ -610,4 +616,5 @@
     vci_pktid_t     miss_wti_cmd_fifo_pktid   = 0;
     int_data_t      miss_wti_cmd_fifo_wdata   = 0;
+    vci_plen_t      miss_wti_cmd_fifo_plen    = 0;
 
 #ifdef INSTRUMENTATION
@@ -649,22 +656,4 @@
         if ( p_vci_tgt_iox.cmdval.read() )
         {
-            if ( not r_iommu_active.read() )    // tlb not activated
-            {
-                // save paddr address
-                r_dma_cmd_paddr = p_vci_tgt_iox.address.read();
-
-                // analyse paddr for WTI/DMA routing
-                // WTI requests must be single flit (READ or WRITE)
-                if ( is_wti( p_vci_tgt_iox.address.read() ) )
-                {
-                    assert( p_vci_tgt_iox.eop.read() and
-                    "ERROR in VCI_IOB illegal VCI WTI command from IOX network");
-
-                    r_dma_cmd_fsm = DMA_CMD_WTI_IOX_REQ;
-                }
-                else
-                {
-                    r_dma_cmd_fsm = DMA_CMD_DMA_REQ;
-                }
 
 #if DEBUG_DMA_CMD
@@ -678,4 +667,24 @@
           << " / eop = " << p_vci_tgt_iox.eop.read() << std::endl;
 #endif
+
+            if ( not r_iommu_active.read() )    // tlb not activated
+            {
+                // save paddr address
+                r_dma_cmd_paddr = p_vci_tgt_iox.address.read();
+
+                // analyse paddr for WTI/DMA routing
+                // WTI requests must be single flit (READ or WRITE)
+                if ( is_wti( p_vci_tgt_iox.address.read() ) )
+                {
+                    assert( p_vci_tgt_iox.eop.read() and
+                    "ERROR in VCI_IOB illegal VCI WTI command from IOX network");
+
+                    r_dma_cmd_fsm = DMA_CMD_WTI_IOX_REQ;
+                }
+                else
+                {
+                    r_dma_cmd_fsm = DMA_CMD_DMA_REQ;
+                }
+
             }
             else if (r_tlb_fsm.read() == TLB_IDLE ||
@@ -735,6 +744,5 @@
                         if ( is_wti( iotlb_paddr ) )
                         {
-                            assert( p_vci_tgt_iox.eop.read() and
-                                   (p_vci_tgt_iox.cmd.read() == vci_param_int::CMD_WRITE) and
+                            assert( p_vci_tgt_iox.eop.read() &&
                             "ERROR in VCI_IOB illegal VCI WTI command from IOX network");
 
@@ -1080,11 +1088,11 @@
         {
             // Checking prefetch buffer
-            if( not r_tlb_buf_big_page )     // small page => PTE2
-            {
-                if( r_tlb_buf_valid &&         // Hit on prefetch buffer
+            if( r_tlb_buf_valid.read() )
+            {
+                if ( !r_tlb_buf_big_page.read() &&  // Hit on prefetch buffer and small page => PTE2
                     (r_tlb_buf_vaddr.read() ==
-                    (r_dma_cmd_to_tlb_vaddr.read()& ~PTE2_LINE_OFFSET & ~K_PAGE_OFFSET_MASK)))
-                {
-                    size_t   pte_offset = (r_dma_cmd_to_tlb_vaddr.read()& PTE2_LINE_OFFSET)>>12;
+                    (r_dma_cmd_to_tlb_vaddr.read() & ~PTE2_LINE_OFFSET & ~K_PAGE_OFFSET_MASK)))
+                {
+                    size_t   pte_offset = (r_dma_cmd_to_tlb_vaddr.read() & PTE2_LINE_OFFSET) >> 12;
                     uint32_t pte_flags  = r_tlb_buf_data[2*pte_offset];
                     uint32_t pte_ppn    = r_tlb_buf_data[2*pte_offset+1];
@@ -1122,12 +1130,10 @@
                     break;
                 }
-            }
-            else                             // big page => PTE1
-            {
-                if( r_tlb_buf_valid &&         // Hit on prefetch buffer
+
+                if( r_tlb_buf_big_page.read() &&         // Hit on prefetch buffer and big page
                     (r_tlb_buf_vaddr.read() ==
-                    (r_dma_cmd_to_tlb_vaddr.read()& ~PTE1_LINE_OFFSET & ~M_PAGE_OFFSET_MASK )))
-                {
-                    size_t   pte_offset = (r_dma_cmd_to_tlb_vaddr.read()& PTE1_LINE_OFFSET)>>21;
+                    (r_dma_cmd_to_tlb_vaddr.read() & ~PTE1_LINE_OFFSET & ~M_PAGE_OFFSET_MASK )))
+                {
+                    size_t   pte_offset = (r_dma_cmd_to_tlb_vaddr.read() & PTE1_LINE_OFFSET) >> 21;
                     uint32_t pte_flags  = r_tlb_buf_data[pte_offset];
 
@@ -1150,28 +1156,30 @@
                     }
 
+#if DEBUG_TLB_MISS
+if ( m_debug_activated )
+std::cout << name()
+          << "  <IOB TLB_PTE1_GET> Hit on prefetch buffer: PTE1" << std::hex
+          << " / paddr = " << r_tlb_paddr.read()
+          << std::hex << " / PTE1 = " << pte_flags << std::endl;
+#endif
                     // valid PTE1 : we must update the TLB
                     r_tlb_pte_flags = pte_flags;
                     r_tlb_fsm       = TLB_PTE1_SELECT;
+
+                    break;
+                }
+            }
+
+            // prefetch buffer miss
+            r_tlb_fsm = TLB_MISS;
+
 #if DEBUG_TLB_MISS
 if ( m_debug_activated )
 std::cout << name()
-          << "  <IOB TLB_PTE1_GET> Hit on prefetch buffer: PTE1" << std::hex
-          << " / paddr = " << r_tlb_paddr.read()
-          << std::hex << " / PTE1 = " << pte_flags << std::endl;
-#endif
-                    break;
-                }
-            }
-
-            // prefetch buffer miss
-            r_tlb_fsm = TLB_MISS;
-
-#if DEBUG_TLB_MISS
-if ( m_debug_activated )
-std::cout << name()
           << "  <IOB TLB_IDLE> Miss on prefetch buffer"
           << std::hex << " / vaddr = " << r_dma_cmd_to_tlb_vaddr.read() << std::endl;
 #endif
         }
+
         break;
     }
@@ -1192,17 +1200,18 @@
         if ( not bypass )     // Read PTE1/PTD1 in XRAM
         {
-
-#if DEBUG_TLB_MISS
-if ( m_debug_activated )
-std::cout << name()
-          << "  <IOB TLB_MISS> Read PTE1/PTD1 in memory" << std::endl;
-#endif
-            pte_paddr = (vci_addr_t)((r_iommu_ptpr.read()) << (INDEX1_NBITS+2)) |
-                        (vci_addr_t)((r_dma_cmd_to_tlb_vaddr.read() >> PAGE_M_NBITS) << 2);
-            r_tlb_paddr = pte_paddr;
-
+            pte_paddr =
+                ((vci_addr_t)r_iommu_ptpr.read() << (INDEX1_NBITS+2)) |
+                ((vci_addr_t)(r_dma_cmd_to_tlb_vaddr.read() >> PAGE_M_NBITS) << 2);
+            r_tlb_paddr               = pte_paddr;
             r_tlb_to_miss_wti_cmd_req = true;
             r_tlb_miss_type           = PTE1_MISS;
             r_tlb_fsm                 = TLB_WAIT;
+
+#if DEBUG_TLB_MISS
+if ( m_debug_activated )
+std::cout << name()
+          << "  <IOB TLB_MISS> Read PTE1/PTD1 in memory: PADDR = " << std::hex
+          << pte_paddr << std::dec << std::endl;
+#endif
         }
         else                  // Read PTE2 in XRAM
@@ -1215,9 +1224,8 @@
 #endif
             //&PTE2 = PTBA + IX2 * 8
-            pte_paddr = (vci_addr_t)ptba << PAGE_K_NBITS |
-                        (vci_addr_t)(r_dma_cmd_to_tlb_vaddr.read()&PTD_ID2_MASK)>>(PAGE_K_NBITS-3);
-
-            r_tlb_paddr = pte_paddr;
-
+            pte_paddr =
+                ((vci_addr_t)ptba << PAGE_K_NBITS) |
+                ((vci_addr_t)(r_dma_cmd_to_tlb_vaddr.read()&PTD_ID2_MASK)>>(PAGE_K_NBITS-3));
+            r_tlb_paddr               = pte_paddr;
             r_tlb_to_miss_wti_cmd_req = true;
             r_tlb_miss_type           = PTE2_MISS;
@@ -1238,6 +1246,6 @@
         // Hit test. Just to verify.
         // Hit must happen, since we've just finished its' miss transaction
-        bool hit = (r_tlb_buf_valid && (r_tlb_buf_tag.read()== line_number) );
-        assert(hit and "Error: No hit on prefetch buffer after Miss Transaction");
+        bool hit = (r_tlb_buf_valid && (r_tlb_buf_tag.read() == line_number) );
+        assert(hit && "Error: No hit on prefetch buffer after Miss Transaction");
 
         entry = r_tlb_buf_data[word_position];
@@ -1257,11 +1265,11 @@
 if ( m_debug_activated )
 {
-    std::cout << name()
-              << "  <IOB DMA_PTE1_GET> First level entry Unmapped"
-              << std::hex << " / paddr = " << r_tlb_paddr.read()
-              << std::hex << " / PTE = " << entry << std::endl;
+std::cout << name()
+          << "  <IOB DMA_PTE1_GET> First level entry Unmapped"
+          << std::hex << " / paddr = " << r_tlb_paddr.read()
+          << std::hex << " / PTE = " << entry << std::endl;
 }
 #endif
-                    break;
+            break;
         }
 
@@ -1275,6 +1283,7 @@
             // &PTE2 = PTBA + IX2 * 8
             // ps: PAGE_K_NBITS corresponds also to the size of a second level page table
-            r_tlb_paddr = (vci_addr_t)(entry & ((1<<(vci_param_int::N-PAGE_K_NBITS))-1)) << PAGE_K_NBITS |
-                                (vci_addr_t)(((r_dma_cmd_to_tlb_vaddr.read() & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
+            r_tlb_paddr =
+                (((vci_addr_t)entry & ((1ULL<<(vci_param_int::N-PAGE_K_NBITS))-1)) << PAGE_K_NBITS) |
+                (((vci_addr_t)(r_dma_cmd_to_tlb_vaddr.read() & PTD_ID2_MASK) >> PAGE_K_NBITS) << 3);
 
             r_tlb_to_miss_wti_cmd_req = true;
@@ -1297,6 +1306,6 @@
                         //  Should not occur if working only with small pages
         {
-            r_tlb_pte_flags   = entry;
-            r_tlb_fsm  = TLB_PTE1_SELECT;
+            r_tlb_pte_flags = entry;
+            r_tlb_fsm       = TLB_PTE1_SELECT;
 
 #if DEBUG_TLB_MISS
@@ -1316,5 +1325,5 @@
         size_t  set;
 
-        r_iotlb.select(  r_dma_cmd_to_tlb_vaddr.read(),
+        r_iotlb.select( r_dma_cmd_to_tlb_vaddr.read(),
                         true,  // PTE1
                         &way,
@@ -1333,5 +1342,5 @@
         r_tlb_way = way;
         r_tlb_set = set;
-        r_tlb_fsm     = TLB_PTE1_UPDT;
+        r_tlb_fsm = TLB_PTE1_UPDT;
         break;
     }
@@ -1340,17 +1349,14 @@
                             // not necessary to treat the L/R bit
     {
-        uint32_t  pte   = r_tlb_pte_flags.read();
-
-        r_tlb_paddr = (vci_addr_t)( ((r_tlb_pte_flags.read() & PPN1_MASK) << 21)
-                        | (r_dma_cmd_to_tlb_vaddr.read()& M_PAGE_OFFSET_MASK) );
+        uint32_t pte = r_tlb_pte_flags.read();
 
         // update TLB
         r_iotlb.write( true,        // 2M page
-                      pte,
-                      0,        // argument unused for a PTE1
-                      r_dma_cmd_to_tlb_vaddr.read(),
-                      r_tlb_way.read(),
-                      r_tlb_set.read(),
-                      0 );      //we set nline = 0
+                       pte,
+                       0,        // argument unused for a PTE1
+                       r_dma_cmd_to_tlb_vaddr.read(),
+                       r_tlb_way.read(),
+                       r_tlb_set.read(),
+                       0 );      //we set nline = 0
 
 #ifdef INSTRUMENTATION
@@ -1381,7 +1387,6 @@
         size_t word_position = (size_t)( ((r_tlb_paddr.read())&(~CACHE_LINE_MASK))>>2 );
 
-
         // Hit test. Just to verify.
-        bool hit = (r_tlb_buf_valid && (r_tlb_buf_tag.read()== line_number) );
+        bool hit = (r_tlb_buf_valid && (r_tlb_buf_tag.read() == line_number) );
         assert(hit and "Error: No hit on prefetch buffer after Miss Transaction");
         pte_flags= r_tlb_buf_data[word_position];
@@ -1394,6 +1399,6 @@
                       << "The Page Table entry ins't valid (unmapped)" << std::endl;
 
-            r_tlb_miss_error       = true;
-            r_dma_cmd_to_tlb_req         = false;
+            r_tlb_miss_error      = true;
+            r_dma_cmd_to_tlb_req  = false;
             r_tlb_fsm             = TLB_IDLE;
 
@@ -1451,9 +1456,6 @@
                                 // not necessary to treat the L/R bit
     {
-        uint32_t        pte_flags = r_tlb_pte_flags.read();
-        uint32_t        pte_ppn   = r_tlb_pte_ppn.read();
-
-        r_tlb_paddr = (vci_addr_t)( ((r_tlb_pte_ppn.read() & PPN2_MASK) << 12)
-                        | (r_dma_cmd_to_tlb_vaddr.read()& K_PAGE_OFFSET_MASK) );
+        uint32_t pte_flags = r_tlb_pte_flags.read();
+        uint32_t pte_ppn   = r_tlb_pte_ppn.read();
 
         // update TLB for a PTE2
@@ -1497,6 +1499,12 @@
 m_cost_iotlbmiss_transaction++;
 #endif
-        if ( not r_tlb_to_miss_wti_cmd_req.read() ) //  Miss transaction completed
-        {
+        if ( r_miss_wti_rsp_to_tlb_done.read() ) // Miss transaction completed
+        {
+            r_miss_wti_rsp_to_tlb_done = false;
+
+            r_tlb_buf_valid = true;
+            r_tlb_buf_vaddr = r_dma_cmd_to_tlb_vaddr.read();
+            r_tlb_buf_tag   = r_tlb_paddr.read() & CACHE_LINE_MASK;
+
             if ( r_miss_wti_rsp_error_miss.read() ) // bus error reported
             {
@@ -1508,8 +1516,10 @@
             else if(r_tlb_miss_type == PTE1_MISS)
             {
+                r_tlb_buf_big_page = true;
                 r_tlb_fsm = TLB_PTE1_GET;
             }
             else
             {
+                r_tlb_buf_big_page = false;
                 r_tlb_fsm = TLB_PTE2_GET;
             }
@@ -1638,17 +1648,59 @@
                 "ERROR in vci_io_bridge : local config access must be one flit");
 
+#if DEBUG_CONFIG_CMD
+if( m_debug_activated )
+std::cout << name()
+          << "  <IOB CONFIG_CMD_IDLE> Command on IOB configuration registers" << std::endl;
+#endif
+
                 if ( not read && (cell == IOB_IOMMU_PTPR) )       // WRITE PTPR
                 {
                     r_iommu_ptpr = (uint32_t)wdata;
+
+#if DEBUG_CONFIG_CMD
+if( m_debug_activated )
+std::cout << name()
+          << "  <IOB CONFIG_CMD_IDLE> Write IOB_IOMMU_PTPR: / wdata = " << std::hex
+          << wdata << std::dec << std::endl;
+#endif
                 }
                 else if ( read && (cell == IOB_IOMMU_PTPR) )      // READ PTPR
                 {
                     rdata = r_iommu_ptpr.read();
-                }
-                else if( not read && (cell == IOB_WTI_ENABLE))  // WRITE WTI_ENABLE
+
+#if DEBUG_CONFIG_CMD
+if( m_debug_activated )
+std::cout << name()
+          << "  <IOB CONFIG_CMD_IDLE> Read IOB_IOMMU_PTPR: / rdata = " << std::hex
+          << rdata << std::dec << std::endl;
+#endif
+                }
+                else if ( not read && (cell == IOB_IOMMU_ACTIVE) )     // WRITE ACTIVE
+                {
+                    r_iommu_active = wdata ? true : false;
+
+#if DEBUG_CONFIG_CMD
+if( m_debug_activated )
+std::cout << name()
+          << "  <IOB CONFIG_CMD_IDLE> Write IOB_IOMMU_ACTIVE: / wdata = " << std::hex
+          << wdata << std::dec << std::endl;
+#endif
+                }
+                else if ( read && (cell == IOB_IOMMU_ACTIVE) )    // READ ACTIVE
+                {
+                    rdata = r_iommu_active.read();
+
+#if DEBUG_CONFIG_CMD
+if( m_debug_activated )
+std::cout << name()
+          << "  <IOB CONFIG_CMD_IDLE> Read IOB_IOMMU_ACTIVE: / rdata = " << std::hex
+          << rdata << std::dec << std::endl;
+#endif
+                }
+                else if( not read && (cell == IOB_WTI_ENABLE))    // WRITE WTI_ENABLE
                 {
                     r_iommu_wti_enable = wdata;
                 }
-                else if( read && (cell == IOB_WTI_ENABLE))       // READ WTI ENABLE
+                else if( read && (cell == IOB_WTI_ENABLE))        // READ WTI ENABLE
                 {
                     rdata = r_iommu_wti_enable.read();
@@ -1658,5 +1710,5 @@
                     rdata = r_iommu_bvar.read();
                 }
-                else if( read && (cell == IOB_IOMMU_ETR))          // READ ETR
+                else if( read && (cell == IOB_IOMMU_ETR))         // READ ETR
                 {
                     rdata = r_iommu_etr.read();
@@ -1671,17 +1723,17 @@
                     r_config_cmd_to_tlb_vaddr = (uint32_t)wdata;
                 }
-                else if( not read && (cell == IOB_WTI_ADDR_LO)) // WRITE WTI_PADDR_LO
+                else if( not read && (cell == IOB_WTI_ADDR_LO))   // WRITE WTI_PADDR_LO
                 {
                     r_iommu_wti_addr_lo = (vci_addr_t)wdata;
                 }
-                else if( read && (cell == IOB_WTI_ADDR_LO))    // READ WTI_PADDR_LO
+                else if( read && (cell == IOB_WTI_ADDR_LO))       // READ WTI_PADDR_LO
                 {
                     rdata = r_iommu_wti_addr_lo.read();
                 }
-                else if( not read && (cell == IOB_WTI_ADDR_HI)) // WRITE WTI_PADDR_HI
+                else if( not read && (cell == IOB_WTI_ADDR_HI))   // WRITE WTI_PADDR_HI
                 {
                     r_iommu_wti_addr_hi = (vci_addr_t)wdata;
                 }
-                else if( read && (cell == IOB_WTI_ADDR_HI))    // READ WTI_PADDR_HI
+                else if( read && (cell == IOB_WTI_ADDR_HI))       // READ WTI_PADDR_HI
                 {
                     rdata = r_iommu_wti_addr_hi.read();
@@ -2118,6 +2170,8 @@
          m_miss_wti_cmd_addr_fifo.wok() )                   // put MISS READ
     {
+        r_tlb_to_miss_wti_cmd_req = false;
+
         miss_wti_cmd_fifo_put     = true;
-        miss_wti_cmd_fifo_address = r_tlb_paddr.read();
+        miss_wti_cmd_fifo_address = r_tlb_paddr.read() & CACHE_LINE_MASK;
         miss_wti_cmd_fifo_wdata   = 0;
         miss_wti_cmd_fifo_cmd     = vci_param_int::CMD_READ;
@@ -2125,4 +2179,5 @@
         miss_wti_cmd_fifo_srcid   = m_int_srcid;
         miss_wti_cmd_fifo_trdid   = 0;
+        miss_wti_cmd_fifo_plen    = m_words * vci_param_int::B;
 
 #if DEBUG_MISS_WTI_CMD
@@ -2130,5 +2185,6 @@
 std::cout << name()
           << "  <IOB MISS_WTI_CMD_WTI> push MISS TLB command into MISS_WTI FIFO"
-          << " / PADDR = " << miss_wti_cmd_fifo_address << std::endl;
+          << " / PADDR = " << std::hex << miss_wti_cmd_fifo_address << std::dec
+          << std::endl;
 #endif
 
@@ -2146,4 +2202,5 @@
         miss_wti_cmd_fifo_trdid   = r_dma_cmd_to_miss_wti_cmd_trdid.read();
         miss_wti_cmd_fifo_pktid   = r_dma_cmd_to_miss_wti_cmd_pktid.read();
+        miss_wti_cmd_fifo_plen    = vci_param_int::B;
 
 #if DEBUG_MISS_WTI_CMD
@@ -2152,5 +2209,6 @@
           << "  <IOB MISS_WTI_CMD_WTI> push WTI command into MISS_WTI FIFO"
           << " / CMD = " << miss_wti_cmd_fifo_cmd
-          << " / PADDR = " << miss_wti_cmd_fifo_address << std::endl;
+          << " / PADDR = " << std::hex << miss_wti_cmd_fifo_address << std::dec
+          << std::endl;
 #endif
 
@@ -2279,11 +2337,10 @@
                 if ( p_vci_ini_int.reop.read() )               // last flit
                 {
-                    bool eop = p_vci_ini_int.eop.read();
-                    assert(((eop == (r_miss_wti_rsp_count.read() == (m_words-1)))) and
+                    assert((r_miss_wti_rsp_count.read() == (m_words-1)) and
                     "VCI_IO_BRIDGE ERROR: invalid length for a TLB MISS response");
 
-                    r_miss_wti_rsp_count      = 0;
-                    r_miss_wti_rsp_fsm        = MISS_WTI_RSP_IDLE;
-                    r_tlb_to_miss_wti_cmd_req = false;
+                    r_miss_wti_rsp_count       = 0;
+                    r_miss_wti_rsp_fsm         = MISS_WTI_RSP_IDLE;
+                    r_miss_wti_rsp_to_tlb_done = true;
                 }
                 else                                           // not the last flit
@@ -2457,5 +2514,5 @@
     m_miss_wti_cmd_plen_fifo.update(   miss_wti_cmd_fifo_get,
                                        miss_wti_cmd_fifo_put,
-                                       4 );
+                                       miss_wti_cmd_fifo_plen );
     m_miss_wti_cmd_wrap_fifo.update(   miss_wti_cmd_fifo_get,
                                        miss_wti_cmd_fifo_put,
