Index: soft/giet_vm/giet_drivers/hba_driver.c
===================================================================
--- soft/giet_vm/giet_drivers/hba_driver.c	(revision 540)
+++ soft/giet_vm/giet_drivers/hba_driver.c	(revision 545)
@@ -15,4 +15,5 @@
 #include <hba_driver.h>
 #include <xcu_driver.h>
+#include <mmc_driver.h>
 #include <kernel_locks.h>
 #include <utils.h>
@@ -36,9 +37,9 @@
 // command list : up to 32 commands
 __attribute__((section(".kdata")))
-hba_cmd_desc_t  _hba_cmd_list[32] __attribute__((aligned(0x10)));   
+hba_cmd_desc_t  _hba_cmd_list[32] __attribute__((aligned(0x40)));   
 
 // command tables array : one command table per entry in command list
 __attribute__((section(".kdata")))
-hba_cmd_table_t _hba_cmd_table[32] __attribute__((aligned(0x1000))); 
+hba_cmd_table_t _hba_cmd_table[32] __attribute__((aligned(0x40))); 
 
 // command list write index : next slot to register a command 
@@ -90,26 +91,28 @@
 
 #if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] P[%d,%d,%d] enters _hba_access at cycle %d\n"
+_printf("\n[DEBUG HBA] _hba_access() : P[%d,%d,%d] enters at cycle %d\n"
         "  use_irq = %d / to_mem = %d / lba = %x / paddr = %l / count = %d\n",
         x , y , p , _get_proctime() , use_irq , to_mem , lba , buf_paddr, count );
 #endif
 
-    unsigned int       pxci;           // HBA_PXCI register value
-    unsigned int       ptw;            // command list write pointer
-    unsigned int       pxis;           // HBA_PXIS register value
-    hba_cmd_desc_t*    cmd_desc;       // command descriptor pointer   
-    hba_cmd_table_t*   cmd_table;      // command table pointer
+    unsigned int       pxci;              // HBA_PXCI register value
+    unsigned int       ptw;               // command list write pointer
+    unsigned int       pxis;              // HBA_PXIS register value
+    hba_cmd_desc_t*    cmd_desc;          // command descriptor pointer   
+    hba_cmd_table_t*   cmd_table;         // command table pointer
 
     // check buffer alignment
-    if( buf_paddr & 0x1FF )
-    {
-        _printf("\n[HBA ERROR] in _hba_access() : buffer not block aligned\n");
+    if( buf_paddr & 0x3F )
+    {
+        _printf("\n[HBA ERROR] in _hba_access() : buffer not 64 bytes aligned\n");
         return -1;
     }
 
-    // get pointer on the next possible entry in command list 
-    ptw = _atomic_increment( &_hba_cmd_ptw , 1 );
-
-    // poll PXCI register until pointed entry empty
+    // get one entry in Command List
+    // atomic increment on the _hba_cmd_ptw allocator
+    // only the 5 LSB bits are used to index the Command List
+    ptw = _atomic_increment( &_hba_cmd_ptw , 1 ) & 0x1F;
+
+    // blocked until allocated entry in Command List is empty
     do
     {
@@ -118,5 +121,5 @@
     } 
     while ( pxci & (1<<ptw) );
-    
+
     // compute pointers on command descriptor and command table    
     cmd_desc  = &_hba_cmd_list[ptw];
@@ -141,8 +144,34 @@
     if( to_mem ) cmd_desc->flag[0] = 0x00;
     else         cmd_desc->flag[0] = 0x40;     
-   
-    // set command in PXCI[ptw]
-    _hba_set_register( HBA_PXCI, pxci + (1<<ptw) );
-
+
+#if USE_IOB    // software L2/L3 cache coherence
+
+    // compute physical addresses
+    unsigned long long cmd_desc_paddr;    // command descriptor physical address
+    unsigned long long cmd_table_paddr;   // command table header physical address
+    unsigned int       flags;             // unused
+
+    if ( _get_mmu_mode() & 0x4 )
+    {
+        cmd_desc_paddr  = _v2p_translate( (unsigned int)cmd_desc  , &flags );
+        cmd_table_paddr = _v2p_translate( (unsigned int)cmd_table , &flags );
+    }
+    else
+    {
+        cmd_desc_paddr  = (unsigned int)cmd_desc;
+        cmd_table_paddr = (unsigned int)cmd_table;
+    }
+
+    // update external memory for command table 
+    _mmc_sync( cmd_table_paddr & (~0x3F) , sizeof(hba_cmd_table_t) );
+
+    // update external memory for command descriptor
+    _mmc_sync( cmd_desc_paddr & (~0x3F) , sizeof(hba_cmd_desc_t) );
+
+    // inval or synchronize memory buffer
+    if ( to_mem )  _mmc_inval( buf_paddr, count<<9 );
+    else           _mmc_sync( buf_paddr, count<<9 );
+
+#endif     // end software L2/L3 cache coherence
 
     /////////////////////////////////////////////////////////////////////
@@ -151,9 +180,11 @@
     if ( use_irq == 0 ) 
     {
-
-#if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] _hba_access() : P[%d,%d,%d] launch transfer"
-        " in polling mode at cycle %d\n",
-        x , y , p , _get_proctime() );
+        // start HBA transfer
+        _hba_set_register( HBA_PXCI, (1<<ptw) );
+
+#if GIET_DEBUG_IOC_DRIVER
+_printf("\n[DEBUG HBA] _hba_access() : command %d for P[%d,%d,%d]"
+        " at cycle %d / polling\n",
+        ptw , x , y , p , _get_proctime() );
 #endif
         // disable IRQs in PXIE register
@@ -163,9 +194,9 @@
         do
         {
-            pxci = _hba_get_register( HBA_PXCI ) & (1<<ptw);
-
-#if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] _hba_access() : P[%d,%d,%d] wait on HBA_STATUS ...\n",
-        x , y , p );
+            pxci = _hba_get_register( HBA_PXCI );
+
+#if GIET_DEBUG_IOC_DRIVER
+_printf("\n[DEBUG HBA] _hba_access() : P[%d,%d,%d] wait on HBA_PXCI / pxci = %x\n",
+        x , y , p , pxci );
 #endif
         }
@@ -190,7 +221,7 @@
 
 #if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] _hba_access() : P[%d,%d,%d] launch transfer"
-        " in descheduling mode at cycle %d\n",
-        x , y , p , _get_proctime() );
+_printf("\n[DEBUG HBA] _hba_access() : command %d for P[%d,%d,%d] "
+        "at cycle %d / descheduling\n",
+        ptw , x , y , p , _get_proctime() );
 #endif
         unsigned int save_sr;
@@ -209,10 +240,13 @@
         _set_task_slot( x, y, p, ltid, CTX_RUN_ID, 0 );  
 
+        // start HBA transfer
+        _hba_set_register( HBA_PXCI, (1<<ptw) );
+
         // deschedule task
         _ctx_switch();                      
 
 #if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] _hba_access() : P[%d,%d,%d] resume execution at cycle %d\n",
-        x , y , p , _get_proctime() );
+_printf("\n[DEBUG HBA] _hba_access() : task %d on P[%d,%d,%d] resume at cycle %d\n",
+        ltid , x , y , p , _get_proctime() );
 #endif
 
@@ -225,5 +259,5 @@
 
 #if GIET_DEBUG_IOC_DRIVER
-_printf("\n[HBA DEBUG] _hba_access() : P[%d,%d,%d] exit at cycle %d\n",
+_printf("\n[DEBUG HBA] _hba_access() : P[%d,%d,%d] exit at cycle %d\n",
         x , y , p , _get_proctime() );
 #endif
@@ -238,41 +272,45 @@
 unsigned int _hba_init()
 {
-    unsigned int       flags;
-    unsigned int       vaddr;
-    unsigned long long paddr;
-    unsigned int       c;      
-    unsigned int       pxclb;
-    unsigned int       pxclbu;
-
-    // command list pointers
+    unsigned int       cmd_list_vaddr;
+    unsigned int       cmd_table_vaddr;
+    unsigned long long cmd_list_paddr;
+    unsigned long long cmd_table_paddr;
+    unsigned int       flags;            // unused
+
+    // compute Command list & command table physical addresses
+    cmd_list_vaddr  = (unsigned int)(&_hba_cmd_list[0]);
+    cmd_table_vaddr = (unsigned int)(&_hba_cmd_table[0]);
+    if ( _get_mmu_mode() & 0x4 )
+    {
+        cmd_list_paddr  = _v2p_translate( cmd_list_vaddr  , &flags );
+        cmd_table_paddr = _v2p_translate( cmd_table_vaddr , &flags );
+    }
+    else
+    {
+        cmd_list_paddr  = (unsigned long long)cmd_list_vaddr;
+        cmd_table_paddr = (unsigned long long)cmd_table_vaddr;
+    }
+
+    // initialise Command List pointers
     _hba_cmd_ptw = 0;
     _hba_cmd_ptr = 0;
 
-    // Command list physical addresse
-    vaddr  = (unsigned int)(_hba_cmd_list);
-    paddr  = _v2p_translate( vaddr , &flags );
-    pxclb  = (unsigned int)paddr;
-    pxclbu = (unsigned int)(paddr>>32);
-
-    // Command tables physical addresses
+    // initialise Command Descriptors in Command List
+    unsigned int         c;      
+    unsigned long long   paddr;
     for( c=0 ; c<32 ; c++ )
     {
-        // compute command table physical address 
-        // for one entry in the command list
-        vaddr = (unsigned int)(&_hba_cmd_table[c]);
-        paddr = _v2p_translate( vaddr , &flags );
-
-        // initialise the corresponding command descriptor
-        _hba_cmd_list[c].ctba  = (unsigned int)paddr;
+        paddr = cmd_table_paddr + c * sizeof(hba_cmd_table_t);
+        _hba_cmd_list[c].ctba  = (unsigned int)(paddr);
         _hba_cmd_list[c].ctbau = (unsigned int)(paddr>>32);
     }
 
-    // set HBA registers 
-    _hba_set_register( HBA_PXCLB , pxclb  );
-    _hba_set_register( HBA_PXCLBU, pxclbu );
-    _hba_set_register( HBA_PXIE  , 0      );
-    _hba_set_register( HBA_PXIS  , 0      );
-    _hba_set_register( HBA_PXCI  , 0      );
-    _hba_set_register( HBA_PXCMD , 1      );
+    // initialise HBA registers 
+    _hba_set_register( HBA_PXCLB  , (unsigned int)(cmd_list_paddr) );
+    _hba_set_register( HBA_PXCLBU , (unsigned int)(cmd_list_paddr>>32) );
+    _hba_set_register( HBA_PXIE   , 0 );
+    _hba_set_register( HBA_PXIS   , 0 );
+    _hba_set_register( HBA_PXCI   , 0 );
+    _hba_set_register( HBA_PXCMD  , 1 );
 
     return 0;
@@ -280,5 +318,5 @@
 
 
-/////////////////////////////////////
+/////////////////////////////////////////////////////
 void _hba_isr( unsigned int irq_type,   // HWI / WTI
                unsigned int irq_id,     // index returned by ICU
@@ -288,17 +326,18 @@
     unsigned int pxci = _hba_get_register( HBA_PXCI );
 
-    // scan active commands from (_hba_cmd_ptr) to (_hba_cmd_ptw-1) 
-    unsigned int c;
-    for ( c = _hba_cmd_ptr ; 
-          c != _hba_cmd_ptw ; 
-          c = (c + 1) % 32 )
-    {
-        if ( (pxci & (1<<c)) == 0 )    // command completed
+    // we must handle all completed commands 
+    // active commands are between  (_hba_cmd_ptr) and (_hba_cmd_ptw-1) 
+    unsigned int current;
+    for ( current = _hba_cmd_ptr ; current != _hba_cmd_ptw ; current++ )
+    {
+        unsigned int ptr = current & 0x1F;
+        
+        if ( (pxci & (1<<ptr)) == 0 )    // command completed
         {
-            // increment read pointer;
-            _hba_cmd_ptr++;
+            // increment the 32 bits variable _hba_cmd_ptr
+            _hba_cmd_ptr = (_hba_cmd_ptr + 1);
 
             // save PXIS register
-            _hba_status[c] = _hba_get_register( HBA_PXIS );
+            _hba_status[ptr] = _hba_get_register( HBA_PXIS );
 
             // reset PXIS register
@@ -306,6 +345,6 @@
  
             // identify waiting task 
-            unsigned int remote_procid  = _hba_gtid[c]>>16;
-            unsigned int ltid           = _hba_gtid[c] & 0xFFFF;
+            unsigned int remote_procid  = _hba_gtid[ptr]>>16;
+            unsigned int ltid           = _hba_gtid[ptr] & 0xFFFF;
             unsigned int remote_cluster = remote_procid >> P_WIDTH;
             unsigned int remote_x       = remote_cluster >> Y_WIDTH;
@@ -327,12 +366,8 @@
 
 #if GIET_DEBUG_IOC_DRIVER  
-unsigned int procid  = _get_procid();
-unsigned int x       = procid >> (Y_WIDTH + P_WIDTH);
-unsigned int y       = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
-unsigned int p       = procid & ((1<<P_WIDTH)-1);
-_printf("\n[HBA DEBUG] Processor[%d,%d,%d] executes _hba_isr() :\n"
-        "  resume task %d running on P[%d,%d,%d] / status = %x at cyle %d\n",
-        x , y , p , 
-        ltid , remote_x , remote_y , remote_p , _hba_status[c] , _get_proctime() );
+_printf("\n[DEBUG HBA] _hba_isr() : command %d completed at cycle %d\n"
+        "  resume task %d running on P[%d,%d,%d] / status = %x\n",
+        ptr , _get_proctime() ,
+        ltid , remote_x , remote_y , remote_p , _hba_status[ptr] );
 #endif
         }
