Index: soft/giet_vm/giet_drivers/sdc_driver.c
===================================================================
--- soft/giet_vm/giet_drivers/sdc_driver.c	(revision 545)
+++ soft/giet_vm/giet_drivers/sdc_driver.c	(revision 563)
@@ -1,328 +1,112 @@
 ///////////////////////////////////////////////////////////////////////////////////
 // File     : sdc_driver.c
-// Date     : 31/08/2012
-// Author   : cesar fuguet
+// Date     : 31/04/2015
+// Author   : Alain Greiner
 // Copyright (c) UPMC-LIP6
 ///////////////////////////////////////////////////////////////////////////////////
 
-#include <hard_config.h>
-#include <sdc_driver.h>
-#include <tty0.h>
-#include <utils.h>
-
-#define SYSCLK_FREQ           50000000
-#define SDCARD_RESET_ITER_MAX 4
-
-///////////////////////////////////////////////////////////////////////////////
-//   Global variables
-///////////////////////////////////////////////////////////////////////////////
-
-__attribute__((section(".kdata")))
-static struct sdcard_dev sdcard;
-
-__attribute__((section(".kdata")))
-static struct spi_dev*   spi;
-
-///////////////////////////////////////////////////////////////////////////////
-// This function enables SD Card select signal
-///////////////////////////////////////////////////////////////////////////////
-static void _sdc_enable()
+#include "hard_config.h"
+#include "giet_config.h"
+#include "sdc_driver.h"
+#include "tty0.h"
+#include "utils.h"
+#include "vmem.h"
+#include "kernel_locks.h"
+#include "mmc_driver.h"
+#include "xcu_driver.h"
+#include "ctx_handler.h"
+
+#define  SDC_RSP_TIMEOUT      100       // number of retries for a config RSP
+
+#define  SDC_POLLING_TIMEOUT  1000000   // number of retries for polling PXCI 
+
+///////////////////////////////////////////////////////////////////////////////////
+//          AHCI related global variables
+///////////////////////////////////////////////////////////////////////////////////
+
+// global index ot the task, for each entry in the command list
+__attribute__((section(".kdata")))
+unsigned int     _ahci_gtid[32];
+
+// status of the command, for each entry in the command list
+__attribute__((section(".kdata")))
+unsigned int     _ahci_status[32];
+
+// command list : up to 32 commands
+__attribute__((section(".kdata")))
+ahci_cmd_desc_t  _ahci_cmd_list[32] __attribute__((aligned(0x40)));   
+
+// command tables array : one command table per entry in command list
+__attribute__((section(".kdata")))
+ahci_cmd_table_t _ahci_cmd_table[32] __attribute__((aligned(0x40))); 
+
+// command list write index : next slot to register a command 
+__attribute__((section(".kdata")))
+unsigned int     _ahci_cmd_ptw;
+
+// command list read index : next slot to poll a completed command 
+__attribute__((section(".kdata")))
+unsigned int     _ahci_cmd_ptr;
+
+
+///////////////////////////////////////////////////////////////////////////////////
+//          SD Card related global variables
+///////////////////////////////////////////////////////////////////////////////////
+
+// SD card relative address
+__attribute__((section(".kdata")))
+unsigned int     _sdc_rca;
+
+// SD Card Hih Capacity Support when non zero
+__attribute__((section(".kdata")))
+unsigned int     _sdc_sdhc;
+
+
+///////////////////////////////////////////////////////////////////////////////
+// This low_level function returns the value contained in register (index).
+///////////////////////////////////////////////////////////////////////////////
+static unsigned int _sdc_get_register( unsigned int index )
 {
-    spi_ss_assert(sdcard.spi, sdcard.slave_id);
+    unsigned int* vaddr = (unsigned int*)SEG_IOC_BASE + index;
+    return _io_extended_read( vaddr );
 }
 
 ///////////////////////////////////////////////////////////////////////////////
-// This function disables SD Card select signal
-///////////////////////////////////////////////////////////////////////////////
-static void _sdc_disable()
+// This low-level function set a new value in register (index).
+///////////////////////////////////////////////////////////////////////////////
+static void _sdc_set_register( unsigned int index,
+                               unsigned int value ) 
 {
-    spi_ss_deassert(sdcard.spi, sdcard.slave_id);
+    unsigned int* vaddr = (unsigned int*)SEG_IOC_BASE + index;
+    _io_extended_write( vaddr, value );
 }
 
 ///////////////////////////////////////////////////////////////////////////////
-// This function writes on the SPI tx register to generate SD card clock ticks
-// - tick_count: number of ticks to generate (1 tick -> 8 clocks) 
-///////////////////////////////////////////////////////////////////////////////
-static void _sdc_gen_tick(unsigned int tick_count)
+// This function sends a command to the SD card and returns the response.
+// - index      : CMD index
+// - arg        : CMD argument
+// - return Card response
+///////////////////////////////////////////////////////////////////////////////
+static unsigned int _sdc_send_cmd ( unsigned int   index,
+                                    unsigned int   arg )
 {
-    register int i = 0;
-    while(i++ < tick_count) spi_put_tx(sdcard.spi, 0xFF, 0);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function changes the SD card access pointer position in terms of
-// blocks
-// - lba: number of logical block to move the pointer
-///////////////////////////////////////////////////////////////////////////////
-void _sdc_lseek(unsigned int lba)
-{
-    sdcard.access_pointer = sdcard.block_length * lba;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function gets a byte from the SD card
-///////////////////////////////////////////////////////////////////////////////
-static unsigned char _sdc_receive_char()
-{
-    _sdc_gen_tick(1);
-
-    return spi_get_rx(sdcard.spi, 0);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function returns when a valid response from the SD card is received or
-// a timeout has been triggered
-// Returns the SD card response value
-///////////////////////////////////////////////////////////////////////////////
-static unsigned char _sdc_wait_response()
-{
-    unsigned char sdcard_rsp;
-    register int  iter;
-
-    iter       = 0;
-    sdcard_rsp = _sdc_receive_char();
-    while (
-            (iter < SDCARD_COMMAND_TIMEOUT) &&
-            !SDCARD_CHECK_R1_VALID(sdcard_rsp)
-          )
-    {
-        sdcard_rsp = _sdc_receive_char();
+    unsigned int  sdc_rsp;
+    register int  iter = 0;
+
+    // load argument
+    _sdc_set_register( SDC_CMD_ARG, arg );
+
+    // lauch command
+    _sdc_set_register( SDC_CMD_ID , index );
+
+    // get response
+    do
+    {
+        sdc_rsp = _sdc_get_register( SDC_RSP_STS );
         iter++;
     }
-
-    return sdcard_rsp;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function returns when a data block from the SD card is received (data
-// block start marker received).
-// It must be called after a read command
-///////////////////////////////////////////////////////////////////////////////
-static void _sdc_wait_data_block()
-{
-	while (_sdc_receive_char() != 0xFE);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function sends a command to the SD card
-// - index: CMD index
-// - app: type of command
-// - args: CMD arguments vector
-// - crc7: CRC (7 bits) to send
-///////////////////////////////////////////////////////////////////////////////
-static int _sdc_send_command ( int      index,
-                               int      app  ,
-                               void *   args ,
-                               unsigned crc7 )
-{
-    unsigned char sdcard_rsp;
-    unsigned char * _args;
-
-    _sdc_gen_tick(5);  
-
-    if (app == SDCARD_ACMD)
-    {
-        spi_put_tx(sdcard.spi, 0x40 | 55         , 0 );// CMD and START bit
-        spi_put_tx(sdcard.spi, 0x00              , 0 );// Argument[0]
-        spi_put_tx(sdcard.spi, 0x00              , 0 );// Argument[1]
-        spi_put_tx(sdcard.spi, 0x00              , 0 );// Argument[2]
-        spi_put_tx(sdcard.spi, 0x00              , 0 );// Argument[3]
-        spi_put_tx(sdcard.spi, 0x01 | (crc7 << 1), 0 );// END bit
-
-        sdcard_rsp = _sdc_wait_response();
-        if (SDCARD_CHECK_R1_ERROR(sdcard_rsp))
-        {
-            return sdcard_rsp;        
-        }
-    }
-
-    _args = (unsigned char *) args;
-
-    _sdc_gen_tick(1);  
-
-    spi_put_tx(sdcard.spi, 0x40 | index      , 0 );
-    spi_put_tx(sdcard.spi, _args[0]          , 0 );
-    spi_put_tx(sdcard.spi, _args[1]          , 0 );
-    spi_put_tx(sdcard.spi, _args[2]          , 0 );
-    spi_put_tx(sdcard.spi, _args[3]          , 0 );
-    spi_put_tx(sdcard.spi, 0x01 | (crc7 << 1), 0 );
-
-    return _sdc_wait_response();
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function initializes the SD card (reset procedure)
-// - channel: channel index (only channel 0 is supported)
-// Returns 0 if success, other value if failure
-///////////////////////////////////////////////////////////////////////////////
-static int _sdc_open( unsigned int channel )
-{
-	unsigned char args[4];
-	unsigned char sdcard_rsp;
-	unsigned int  iter, ersp;
-
-	sdcard.spi      = spi;
-	sdcard.slave_id = channel;
-
-	// supply SD card ramp up time (min 74 cycles)
-	_sdc_gen_tick(10);
-
-	// Assert slave select signal
-	// Send CMD0 (Reset Command)
-	// Deassert slave select signal
-	_sdc_enable();
-
-	args[0] = 0;
-	args[1] = 0;
-	args[2] = 0;
-	args[3] = 0;
-	sdcard_rsp = _sdc_send_command(0, SDCARD_CMD, args, 0x4A);
-	if ( sdcard_rsp != 0x01 )
-	{
-		_puts("[SDC ERROR] card CMD0 failed\n");
-		return sdcard_rsp;
-	}
-
-	_sdc_disable();
-
-	// send CMD8. If card is pre-v2, It will reply with illegal command.
-	// Otherwise we announce sdhc support.
-	_sdc_enable();
-	args[0] = 0;
-	args[1] = 0;
-	args[2] = 0x01;
-	args[3] = 0x01;
-	sdcard_rsp = _sdc_send_command(8, SDCARD_CMD, args, 0x63);
-	if (!SDCARD_CHECK_R1_VALID(sdcard_rsp))
-    {
-		_puts("[SDC ERROR] card CMD8 failed\n");
-		return sdcard_rsp;
-	}
-	if (!SDCARD_CHECK_R1_ERROR(sdcard_rsp))
-    {
-		// no error, command accepted. get whole reply
-		ersp = _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		if ((ersp & 0xffff) != 0x0101) 
-        {
-			// voltage mismatch
-			_puts("[SDC ERROR] card CMD8 mismatch : ersp = %x\n");
-			return sdcard_rsp;
-		}
-		_puts("[SDC WARNING] v2 or later ");
-		sdcard.sdhc = 1;
-	}
-    else if ((sdcard_rsp & SDCARD_R1_ILLEGAL_CMD) == 0)
-    {
-		// other error
-		_puts("[SDC ERROR] card CMD8 error\n");
-		return sdcard_rsp;
-	}
-    else
-    {
-		sdcard.sdhc = 0;
-	}
-	_sdc_disable();
-
-	// send CMD41, enabling the card
-	_sdc_enable();
-	args[0] = sdcard.sdhc ? 0x40: 0;
-	args[1] = 0;
-	args[2] = 0;
-	args[3] = 0;
-
-	iter = 0;
-	while( iter++ < SDCARD_COMMAND_TIMEOUT )
-	{
-		sdcard_rsp = _sdc_send_command(41, SDCARD_ACMD, args, 0x00);
-		if( sdcard_rsp == 0x01 )
-		{
-			continue;
-		}
-
-		break;
-	}
-
-	_sdc_disable();
-	if (sdcard_rsp)
-    {
-		_puts("[SDC ERROR] ACMD41 failed\n");
-		return sdcard_rsp;
-	}
-	if (sdcard.sdhc != 0)
-    {
-		// get the card capacity to see if it's really HC
-		_sdc_enable();
-		args[0] = sdcard.sdhc ? 0x40: 0;
-		args[1] = 0;
-		args[2] = 0;
-		args[3] = 0;
-        sdcard_rsp = _sdc_send_command(58, SDCARD_CMD, args, 0x00);
-		if (sdcard_rsp)
-        {
-			_puts("[SDC ERROR] CMD58 failed\n");
-			return sdcard_rsp;
-		}
-		ersp = _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		ersp = (ersp << 8) | _sdc_receive_char();
-		if (ersp & 0x40000000)
-        {
-			_puts(" SDHC ");
-		} 
-        else
-        {
-			sdcard.sdhc = 0;
-		}
-		_sdc_disable();
-	}
-	_puts("card detected\n");
-	return 0;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// This function sets the block size in the SD card.
-// - len: block size in bytes (only 512 bytes supported)
-// Returns 0 if success, other value if failure
-///////////////////////////////////////////////////////////////////////////////
-static unsigned int _sdc_set_block_size(unsigned int len)
-{
-    unsigned char args[4];
-    unsigned char sdcard_rsp;
-    register int i;
-
-    // For now, supported block size is 512 bytes
-    if (len != 512) return 1;
-
-    // When using high capacity SDCARD, the block_length is not a number of bytes
-    // but a number of blocks (transfer unit)
-    if (sdcard.sdhc)
-    {
-        sdcard.block_length = len / 512;
-        return 0;
-    }
-
-    for (i = 0; i < 4; i++)
-    {
-        args[i] = (len >> (32 - (i+1)*8)) & 0xFF;
-    }
-
-    _sdc_enable();
-
-    sdcard_rsp = _sdc_send_command(16, SDCARD_CMD, args, 0x00);
-    if ( SDCARD_CHECK_R1_ERROR(sdcard_rsp) )
-    {
-        _sdc_disable();
-        return sdcard_rsp;
-    }
-
-    _sdc_disable();
-
-    sdcard.block_length = len;
-
-	return 0;
+    while ( (sdc_rsp == 0xFFFFFFFF) && (iter < SDC_RSP_TIMEOUT) ); 
+
+    return sdc_rsp;
 }
 
@@ -334,71 +118,137 @@
 unsigned int _sdc_init()
 {
-    spi = (struct spi_dev*)SEG_IOC_BASE;
-
-    // initializing the SPI controller
-    _spi_init (
-        spi           ,
-        200000        , /**< SPI_clk: 200 Khz */
-        SYSCLK_FREQ   , /**< Sys_clk          */
-        8             , /**< Charlen: 8       */
-        SPI_TX_NEGEDGE,
-        SPI_RX_POSEDGE
-    );
-
-    // initializing the SD Card
-    unsigned int iter = 0;
-    unsigned char sdcard_rsp;
-    unsigned int i;
-
-    while(1)
-    {
-        _puts("[SDC WARNING] Trying to initialize SD card...\n");
-
-        sdcard_rsp = _sdc_open( 0 );  // only channel 0
-        if (sdcard_rsp == 0)
-        {
-            _puts("OK\n");
-            break;
-        }
-
-        _puts("KO\n");
-
-        for (i = 0; i < 1000; i++);
-
-        if (++iter >= SDCARD_RESET_ITER_MAX)
-        {
-            _puts("\n[SDC ERROR] During SD card reset / card response = ");
-            _putx( sdcard_rsp );
-            _puts("\n");
-            _exit();
-        }
-    }
-
-    // set the block length of the SD Card
-    sdcard_rsp = _sdc_set_block_size(512);
-    if (sdcard_rsp)
-    {
-        _puts("[SDC ERROR] During SD card block size initialization\n");
-        _exit();
-    }
-
-    // incrementing SDCARD clock frequency for normal function
-    _spi_init (
-        spi         ,
-        10000000    , // SPI_clk 10 Mhz
-        SYSCLK_FREQ , // Sys_clk
-        -1          , // Charlen: 8
-        -1          ,
-        -1
-    );
-
-    _puts("[SDC WARNING] Finish SD card initialization\n\r");
-
-    return 0;
+    //////////// SD Card initialisation //////////
+  
+    unsigned int rsp;
+
+    // define the SD card clock period
+    _sdc_set_register( SDC_PERIOD , GIET_SDC_PERIOD );
+
+    // send CMD0 (soft reset / no argument)
+    rsp = _sdc_send_cmd( SDC_CMD0 , 0 );
+    if ( rsp == 0xFFFFFFFF )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : no acknowledge to CMD0\n");
+        return 1;
+    }
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : SDC_CMD0 done at cycle %d\n", _get_proctime() );
+#endif
+
+    // send CMD8 command 
+    rsp = _sdc_send_cmd( SDC_CMD8 , SDC_CMD8_ARGUMENT );
+    if ( rsp == 0xFFFFFFFF )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : no response to CMD8\n");
+        return 1;
+    }
+    else if ( rsp != SDC_CMD8_ARGUMENT )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : response to CMD8 = %x / expected = %x\n",
+                rsp , SDC_CMD8_ARGUMENT );
+        return 1;
+    }
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : SDC_CMD8 done at cycle %d\n", _get_proctime() );
+#endif
+
+    // send CMD41 to get SDHC
+    if ( rsp == 0xFFFFFFFF )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : no response to CMD41\n");
+        return 1;
+    }
+    _sdc_sdhc = ( (rsp & SDC_CMD41_RSP_CCS) != 0 );
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : SDC_CMD41 done at cycle %d\n", _get_proctime() );
+#endif
+
+    // send CMD3 to get RCA
+    rsp = _sdc_send_cmd( SDC_CMD3 , 0 );
+    if ( rsp == 0xFFFFFFFF )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : no response to CMD3\n");
+        return 1;
+    }
+    _sdc_rca = rsp;
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : SDC_CMD3 done at cycle %d\n", _get_proctime() );
+#endif
+
+    // send CMD7
+    rsp = _sdc_send_cmd( SDC_CMD7 , _sdc_rca );
+    if ( rsp == 0xFFFFFFFF )
+    {
+        _printf("\n[SDC ERROR] in _sdc_init() : no response to CMD7\n");
+        return 1;
+    }
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : SDC_CMD7 done at cycle %d\n", _get_proctime() );
+#endif
+
+    //////////// AHCI interface initialisation  ///////
+
+    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)(&_ahci_cmd_list[0]);
+    cmd_table_vaddr = (unsigned int)(&_ahci_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
+    _ahci_cmd_ptw = 0;
+    _ahci_cmd_ptr = 0;
+
+    // initialise Command Descriptors in Command List
+    unsigned int         c;      
+    unsigned long long   paddr;
+    for( c=0 ; c<32 ; c++ )
+    {
+        paddr = cmd_table_paddr + c * sizeof(ahci_cmd_table_t);
+        _ahci_cmd_list[c].ctba  = (unsigned int)(paddr);
+        _ahci_cmd_list[c].ctbau = (unsigned int)(paddr>>32);
+    }
+
+    // initialise AHCI registers 
+    _sdc_set_register( AHCI_PXCLB  , (unsigned int)(cmd_list_paddr) );
+    _sdc_set_register( AHCI_PXCLBU , (unsigned int)(cmd_list_paddr>>32) );
+    _sdc_set_register( AHCI_PXIE   , 0 );
+    _sdc_set_register( AHCI_PXIS   , 0 );
+    _sdc_set_register( AHCI_PXCI   , 0 );
+    _sdc_set_register( AHCI_PXCMD  , 1 );
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_init() : AHCI init done at cycle %d\n", _get_proctime() );
+#endif
+
 } // end _sdc_init()
 
 
 /////////////////////////////////////////////////////
-unsigned int _sdc_access( unsigned int       use_irq,  // unused
+unsigned int _sdc_access( unsigned int       use_irq, 
                           unsigned int       to_mem,
                           unsigned int       lba,
@@ -406,58 +256,209 @@
                           unsigned int       count )
 {
-    unsigned char args[4];
-    unsigned char sdcard_rsp;
-    unsigned int i;
-    unsigned int curr = lba;
-    unsigned int last = lba + count;
-
-    if ( to_mem )  // read access
-    {
-        for ( ; curr < last ; curr++ )
+    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);
+    unsigned int iter;
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_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;              // AHCI_PXCI register value
+    unsigned int       ptw;               // command list write pointer
+    unsigned int       pxis;              // AHCI_PXIS register value
+    ahci_cmd_desc_t*   cmd_desc;          // command descriptor pointer   
+    ahci_cmd_table_t*  cmd_table;         // command table pointer
+
+    // check buffer alignment
+    if( buf_paddr & 0x3F )
+    {
+        _printf("\n[SDC ERROR] in _sdc_access() : buffer not 64 bytes aligned\n");
+        return 1;
+    }
+
+    // get one entry in Command List, using an
+    // atomic increment on the _ahci_cmd_ptw allocator
+    // only the 5 LSB bits are used to index the Command List
+    ptw = _atomic_increment( &_ahci_cmd_ptw , 1 ) & 0x1F;
+
+    // blocked until allocated entry in Command List is empty
+    iter = SDC_POLLING_TIMEOUT;
+    do
+    {
+        // get PXCI register
+        pxci = _sdc_get_register( AHCI_PXCI );
+
+        // check livelock
+        iter--;
+        if ( iter == 0 )
         {
-            _sdc_lseek(curr);
-
-            for (i = 0; i < 4; i++)
+            _printf("\n[SDC ERROR] in _sdc_access() : cannot get PXCI slot\n");
+            return 1;
+        }
+    } 
+    while ( pxci & (1<<ptw) );
+
+    // compute pointers on command descriptor and command table    
+    cmd_desc  = &_ahci_cmd_list[ptw];
+    cmd_table = &_ahci_cmd_table[ptw];
+
+    // set  buffer descriptor in command table 
+    cmd_table->buffer.dba  = (unsigned int)(buf_paddr);
+    cmd_table->buffer.dbau = (unsigned int)(buf_paddr >> 32);
+    cmd_table->buffer.dbc  = count * 512;
+
+    // initialize command table header
+    cmd_table->header.lba0 = (char)lba;
+    cmd_table->header.lba1 = (char)(lba>>8);
+    cmd_table->header.lba2 = (char)(lba>>16);
+    cmd_table->header.lba3 = (char)(lba>>24);
+    cmd_table->header.lba4 = 0;
+    cmd_table->header.lba5 = 0;
+
+    // initialise command descriptor
+    cmd_desc->prdtl[0] = 1;
+    cmd_desc->prdtl[1] = 0;
+    if( to_mem ) cmd_desc->flag[0] = 0x00;
+    else         cmd_desc->flag[0] = 0x40;     
+
+#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(ahci_cmd_table_t) );
+
+    // update external memory for command descriptor
+    _mmc_sync( cmd_desc_paddr & (~0x3F) , sizeof(ahci_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
+
+    /////////////////////////////////////////////////////////////////////
+    // In synchronous mode, we poll the PXCI register until completion
+    /////////////////////////////////////////////////////////////////////
+    if ( use_irq == 0 ) 
+    {
+        // start transfer
+        _sdc_set_register( AHCI_PXCI, (1<<ptw) );
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_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
+        _sdc_set_register( AHCI_PXIE , 0 );
+
+        // poll PXCI[ptw] until command completed
+        iter = SDC_POLLING_TIMEOUT;
+        do
+        {
+            pxci = _sdc_get_register( AHCI_PXCI );
+
+            // check livelock
+            iter--;
+            if ( iter == 0 )
             {
-                args[i] = (sdcard.access_pointer >> (32 - (i+1)*8)) & 0xFF;
-            }
-
-            _sdc_enable();
-
-            sdcard_rsp = _sdc_send_command(17, SDCARD_CMD, args, 0x00);
-            if ( SDCARD_CHECK_R1_ERROR(sdcard_rsp) )
-            {
-                _sdc_disable();
-                return sdcard_rsp;
-            }
-
-            _sdc_wait_data_block();
-
-            if (spi_get_data(sdcard.spi, buf_paddr, 512 ))
-            {
-                _sdc_disable();
+                _printf("\n[SDC ERROR] in _sdc_access() : polling PXCI timeout\n");
                 return 1;
             }
-
-            // Get the CRC16 (comes at the end of the data block)
-            _sdc_receive_char(); // first byte
-            _sdc_receive_char(); // second byte
-
-            _sdc_disable();
-
-            buf_paddr += 512;
         }
-    }
-    else            // write access
-    {
-        _printf("[SDC ERROR] function _sdc_write() not iplemented yet\n");
-        _exit();
-    }
-
-    return 0;
-}  // _end sdc_access()
-
-///////////////////////////////////////////////////////////////////////////////
-// This ISR handles the IRQ generated by a SDC controler
+        while( pxci & (1<<ptw) ); 
+             
+        // get PXIS register
+        pxis = _sdc_get_register( AHCI_PXIS );
+
+        // reset PXIS register
+        _sdc_set_register( AHCI_PXIS , 0 );
+    }
+
+    /////////////////////////////////////////////////////////////////
+    // in descheduling mode, we deschedule the task
+    // and use an interrupt to reschedule the task.
+    // We need a critical section, because we must reset the RUN bit
+	// before to launch the transfer, and we don't want to be 
+    // descheduled between these two operations. 
+    /////////////////////////////////////////////////////////////////
+    else
+    {
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_access() : command %d for P[%d,%d,%d] "
+        "at cycle %d / descheduling\n",
+        ptw , x , y , p , _get_proctime() );
+#endif
+        unsigned int save_sr;
+        unsigned int ltid = _get_current_task_id();
+
+        // activates interrupt 
+        _sdc_set_register( AHCI_PXIE , 0x00000001 ); 
+
+        // set _ahci_gtid[ptw] 
+        _ahci_gtid[ptw] = (procid<<16) + ltid;
+
+        // enters critical section
+        _it_disable( &save_sr ); 
+
+        // reset runnable 
+        _set_task_slot( x, y, p, ltid, CTX_RUN_ID, 0 );  
+
+        // start transfer
+        _sdc_set_register( AHCI_PXCI, (1<<ptw) );
+
+        // deschedule task
+        _ctx_switch();                      
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_access() : task %d on P[%d,%d,%d] resume at cycle %d\n",
+        ltid , x , y , p , _get_proctime() );
+#endif
+
+        // restore SR
+        _it_restore( &save_sr );
+
+        // get command status
+        pxis = _ahci_status[ptw];
+    }    
+
+#if GIET_DEBUG_IOC_DRIVER
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_access() : P[%d,%d,%d] exit at cycle %d\n",
+        x , y , p , _get_proctime() );
+#endif
+
+    if ( pxis & 0x40000000 ) return pxis;
+    else                     return 0;
+
+} // end _sdc_access()
+
+
+///////////////////////////////////////////////////////////////////////////////
+// This ISR handles the IRQ generated by the AHCI_SDC controler
 ///////////////////////////////////////////////////////////////////////////////
 void _sdc_isr( unsigned int irq_type,
@@ -465,7 +466,60 @@
                unsigned int channel )
 {
-    _puts("\n[GIET ERROR] _sdc_isr() not implemented\n");
-    _exit();
-}
+    // get AHCI_PXCI containing commands status
+    unsigned int pxci = _sdc_get_register( AHCI_PXCI );
+
+    // we must handle all completed commands 
+    // active commands are between  (_ahci_cmd_ptr) and (_ahci_cmd_ptw-1) 
+    unsigned int current;
+    for ( current = _ahci_cmd_ptr ; current != _ahci_cmd_ptw ; current++ )
+    {
+        unsigned int ptr = current & 0x1F;
+        
+        if ( (pxci & (1<<ptr)) == 0 )    // command completed
+        {
+            // increment the 32 bits variable _ahci_cmd_ptr
+            _ahci_cmd_ptr++;
+
+            // save AHCI_PXIS register
+            _ahci_status[ptr] = _sdc_get_register( AHCI_PXIS );
+
+            // reset AHCI_PXIS register
+            _sdc_set_register( AHCI_PXIS , 0 );
+ 
+            // identify waiting task 
+            unsigned int remote_procid  = _ahci_gtid[ptr]>>16;
+            unsigned int ltid           = _ahci_gtid[ptr] & 0xFFFF;
+            unsigned int remote_cluster = remote_procid >> P_WIDTH;
+            unsigned int remote_x       = remote_cluster >> Y_WIDTH;
+            unsigned int remote_y       = remote_cluster & ((1<<Y_WIDTH)-1);
+            unsigned int remote_p       = remote_procid & ((1<<P_WIDTH)-1);
+ 
+            // re-activates waiting task
+            _set_task_slot( remote_x,
+                            remote_y,
+                            remote_p,
+                            ltid,
+                            CTX_RUN_ID,
+                            1 );
+
+            // send a WAKUP WTI to processor running the waiting task 
+            _xcu_send_wti( remote_cluster , 
+                           remote_p , 
+                           0 );          // don't force context switch
+
+#if GIET_DEBUG_IOC_DRIVER  
+if (_get_proctime() > GIET_DEBUG_IOC_DRIVER)
+_printf("\n[DEBUG SDC] _sdc_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 , _ahci_status[ptr] );
+#endif
+        }
+        else                         // command non completed
+        {
+            break;
+        }
+    }
+}  // end _sdc_isr()
 
 // Local Variables:
