Index: soft/giet_vm/sys/drivers.c
===================================================================
--- soft/giet_vm/sys/drivers.c	(revision 167)
+++ soft/giet_vm/sys/drivers.c	(revision 169)
@@ -85,16 +85,21 @@
 #define in_unckdata __attribute__((section (".unckdata")))
 
-in_unckdata volatile unsigned int  _dma_status[NB_DMAS];
-in_unckdata volatile unsigned char _dma_busy[NB_DMAS] = { [0 ... NB_DMAS-1] = 0 };
-
+// IOC variables
 in_unckdata volatile unsigned char _ioc_status       = 0;
 in_unckdata volatile unsigned char _ioc_done         = 0;
 in_unckdata unsigned int		   _ioc_lock         = 0;
 in_unckdata unsigned int		   _ioc_iommu_ix1    = 0;
-in_unckdata unsigned int		   _ioc_iommu_npages = 0;
-
+in_unckdata unsigned int		   _ioc_iommu_npages; 
+
+// DMA variables
+in_unckdata volatile unsigned int  _dma_status[NB_DMAS];
+in_unckdata volatile unsigned char _dma_busy[NB_DMAS] = { [0 ... NB_DMAS-1] = 0 };
+in_unckdata volatile unsigned char _dma_iommu_ix1     = 1;
+in_unckdata volatile unsigned char _dma_iommu_npages[NB_DMAS];
+
+// TTY variables
 in_unckdata volatile unsigned char _tty_get_buf[NB_TTYS];
 in_unckdata volatile unsigned char _tty_get_full[NB_TTYS] = { [0 ... NB_TTYS-1] = 0 };
-in_unckdata unsigned int           _tty_put_lock;
+in_unckdata unsigned int           _tty_put_lock          = 0;
 
 //////////////////////////////////////////////////////////////////////////////
@@ -610,5 +615,5 @@
 
             // clear IOMMU TLB
-            iob_address[IOB_INVAL_PTE] = (_ioc_iommu_ix1 << 21) | (ix2) << 12; 
+            iob_address[IOB_INVAL_PTE] = (_ioc_iommu_ix1 << 21) | (ix2 << 12); 
         }
     }
@@ -668,11 +673,13 @@
 // implement the transfer between a data buffer (user space) and the frame
 // buffer (kernel space). They are blocking until completion of the transfer.
+//
 // The '_fb_write()', '_fb_read()' and '_fb_completed()' functions use the DMA
 // coprocessor to transfer data between the user buffer and the frame buffer.
 // These  functions use a polling policy to test the global variables _dma_busy[i] 
 // and detect the transfer completion.  
-// There is  NB_PROCS DMA channels, that are indexed by the proc_id.
+// There is  NB_DMA channels, that are indexed by the dma_id stored in the 
+// task context.
 // The _dma_busy[i] synchronisation variables (one per channel) are set by the OS, 
-// and reset by the ISR.
+// and reset by the DMA ISR.
 //////////////////////////////////////////////////////////////////////////////////
 
@@ -692,12 +699,11 @@
     volatile unsigned char *fb_address;
 
-    /* buffer must be in user space */
-    if (((unsigned int)buffer >= 0x80000000)
-            || (((unsigned int)buffer + length ) >= 0x80000000 ))
+    // buffer must be mapped in user space 
+    if ( ((unsigned int)buffer + length ) >= 0x80000000 )
         return 1;
 
     fb_address = (unsigned char*)&seg_fb_base + offset;
 
-    /* buffer copy */
+    // buffer copy 
     memcpy((void*)fb_address, (void*)buffer, length);
 
@@ -720,13 +726,11 @@
     volatile unsigned char *fb_address;
 
-    /* parameters checking */
-    /* buffer must be in user space */
-    if (((unsigned int)buffer >= 0x80000000)
-            || (((unsigned int)buffer + length ) >= 0x80000000 ))
+    // buffer must be mapped in user space 
+    if ( ((unsigned int)buffer + length ) >= 0x80000000 )
         return 1;
 
     fb_address = (unsigned char*)&seg_fb_base + offset;
 
-    /* buffer copy */
+    // buffer copy 
     memcpy((void*)buffer, (void*)fb_address, length);
 
@@ -735,7 +739,129 @@
 
 //////////////////////////////////////////////////////////////////////////////////
+// _fb_access()
+// Transfer data between a memory buffer and the frame_buffer device using DMA.
+// - to_mem     : from frame buffer to memory when true.
+// - offset     : offset (in bytes) in the frame buffer.
+// - user_vaddr : virtual base address of the memory buffer.
+// - length     : number of bytes to be transfered.
+// The memory buffer must be mapped in user address space and word-aligned.
+// The user buffer length must be multiple of 4 bytes. 
+// Returns 0 if success, > 0 if error.
+//////////////////////////////////////////////////////////////////////////////////
+unsigned int _fb_access( unsigned int 	to_mem,
+                         unsigned int   offset,
+                         unsigned int   user_vaddr,
+                         unsigned int   length )
+{
+    static_scheduler_t*	psched;		// pointer on the current task scheduler
+    unsigned char*		fb_base;	// frame buffer base address
+    unsigned int*		dma_base;	// dma component base address
+    unsigned int		task_id;	// task local index (for scheduler)
+    unsigned int		dma_id;		// DMA channel index
+    unsigned int		vpn;		// current virtual page number
+    unsigned int		flags;		// protection flags
+    unsigned int		ppn;		// current physical page number
+    unsigned int		buf_base;	// buffer base address for DMA access
+    unsigned int		ppn_first;	// first physical page index for user buffer
+
+    fb_base  = (unsigned char*)&seg_fb_base + offset;
+
+    psched   = &_scheduler[_procid()];
+    task_id  = psched->current;
+    dma_id   = psched->context[task_id][CTX_FBDMA_ID];
+    dma_base = (unsigned int*)&seg_dma_base + (dma_id * DMA_SPAN);
+
+    // check buffer address and ength alignment
+    if ( user_vaddr & 0x3 ) return 1;
+    if ( length     & 0x3 ) return 1;
+
+    // get user space page table virtual address
+    unsigned int user_ptp = psched->context[task_id][CTX_PTAB_ID];
+
+    unsigned int user_vpn_min = user_vaddr >> 12;
+    unsigned int user_vpn_max = (user_vaddr + length - 1) >> 12;
+    unsigned int ix2          = 0;
+    unsigned int ix1          = _dma_iommu_ix1 + dma_id;
+    unsigned int ko;
+    unsigned int i;
+
+    // loop on all virtual pages covering the user buffer
+    for ( vpn = user_vpn_min ; vpn <= user_vpn_max ; vpn++ )
+    {
+        // get ppn and flags for each vpn
+        ko = _v2p_translate( (page_table_t*)user_ptp,
+                             vpn,
+                             &ppn,
+                             &flags );
+
+        // check access rights
+        if ( ko )                                 return 2;     // unmapped
+        if ( (flags & PTE_U) == 0 )               return 3;     // not in user space
+        if ( ( (flags & PTE_W) == 0 ) && to_mem ) return 4;     // not writable
+
+        // save first ppn value
+        if ( ix2 == 0 ) ppn_first = ppn;
+
+        if ( GIET_IOMMU_ACTIVE )    // the user buffer must be remapped in the I/0 space
+        {
+            // check buffer length < 2 Mbytes
+            if ( ix2 > 511 ) return 2;
+
+            // map the physical page in IOMMU page table
+            _iommu_add_pte2( ix1,		// PT1 index
+                             ix2,		// PT2 index
+                             ppn,		// physical page number 
+                             flags );	// protection flags
+        }
+        else            // no IOMMU : check that physical pages are contiguous
+        {
+            if ( (ppn - ppn_first) != ix2 )       return 5;     // split physical buffer  
+        }
+
+        // increment page index
+        ix2++;
+    } // end for vpn
+
+    // register the number of pages to be unmapped
+    _dma_iommu_npages[dma_id] = (user_vpn_max - user_vpn_min) + 1;
+
+    // invalidate data cache in case of memory write
+    if ( to_mem ) _dcache_buf_invalidate( (void*)user_vaddr, length );
+
+    // compute buffer base address for DMA depending on IOMMU activation
+    if ( GIET_IOMMU_ACTIVE ) buf_base = ( ix1 ) << 21 | (user_vaddr & 0xFFF);
+    else                     buf_base = (ppn_first << 12) | (user_vaddr & 0xFFF);
+
+
+    // waiting until DMA device is available 
+    while (_dma_busy[dma_id] != 0)
+    {
+        // busy wait with a pseudo random delay between bus access
+        unsigned int delay = (_proctime() & 0xF) << 4;
+        for (i = 0; i < delay; i++)
+            asm volatile("nop");
+    }
+
+    _dma_busy[dma_id] = 1;
+
+    // DMA configuration 
+    dma_base[DMA_IRQ_DISABLE] = 0;
+    if ( to_mem )
+    {
+        dma_base[DMA_SRC] = (unsigned int)fb_base;
+        dma_base[DMA_DST] = (unsigned int)buf_base;
+    }
+    else
+    {
+        dma_base[DMA_SRC] = (unsigned int)buf_base;
+        dma_base[DMA_DST] = (unsigned int)fb_base;
+    }
+    dma_base[DMA_LEN] = (unsigned int)length;
+    
+    return 0;
+}  
+//////////////////////////////////////////////////////////////////////////////////
 // _fb_write()
-// Transfer data from an memory buffer to the frame_buffer device using a DMA.
-// The source memory buffer must be in user address space.
+// Transfer data from a memory buffer to the frame_buffer device using  DMA.
 // - offset : offset (in bytes) in the frame buffer.
 // - buffer : base address of the memory buffer.
@@ -744,115 +870,73 @@
 //////////////////////////////////////////////////////////////////////////////////
 unsigned int _fb_write( unsigned int 	offset, 
-                        const void*	buffer, 
+                        const void*		buffer, 
                         unsigned int 	length )
 {
-    volatile unsigned char *fb_address;
-    volatile unsigned int *dma;
-
-    unsigned int proc_id;
-    unsigned int delay;
-    unsigned int i;
-
-    /* buffer must be in user space */
-    if (((unsigned int)buffer >= 0x80000000)
-            || (((unsigned int)buffer + length ) >= 0x80000000 ))
-        return 1;
-
-    proc_id = _procid();
-    fb_address = (unsigned char*)&seg_fb_base + offset;
-    dma = (unsigned int*)&seg_dma_base + (proc_id * DMA_SPAN);
-
-    /* waiting until DMA device is available */
-    while (_dma_busy[proc_id] != 0)
-    {
-        /* if the lock failed, busy wait with a pseudo random delay between bus
-         * accesses */
-        delay = (_proctime() & 0xF) << 4;
+    return _fb_access( 0,						// write to frame buffer
+                       offset,
+                       (unsigned int)buffer,
+                       length );	
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// _fb_read()
+// Transfer data from the frame_buffer device to a memory buffer using  DMA.
+// - offset : offset (in bytes) in the frame buffer.
+// - buffer : base address of the memory buffer.
+// - length : number of bytes to be transfered.
+// Returns 0 if success, > 0 if error.
+//////////////////////////////////////////////////////////////////////////////////
+unsigned int _fb_read( unsigned int 	offset, 
+                       const void*		buffer, 
+                       unsigned int 	length )
+{
+    return _fb_access( 1,						// read from frame buffer
+                       offset,
+                       (unsigned int)buffer,
+                       length );	
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// _fb_completed()
+// This function checks completion of a DMA transfer to or fom the frame buffer.
+// As it is a blocking call, the processor is busy waiting.
+// Returns 0 if success, > 0 if error 
+// (1 == read error / 2 == DMA idle error / 3 == write error)
+//////////////////////////////////////////////////////////////////////////////////
+unsigned int _fb_completed()
+{
+    static_scheduler_t* psched  = &_scheduler[_procid()];
+    unsigned int        task_id = psched->current;
+
+    volatile unsigned int   dma_id  = psched->context[task_id][CTX_FBDMA_ID];
+
+    // busy waiting with a pseudo random delay between bus access
+    while (_dma_busy[dma_id] != 0)
+    {
+	    unsigned int i;
+        unsigned int delay = (_proctime() & 0xF) << 4;
         for (i = 0; i < delay; i++)
             asm volatile("nop");
     }
-    _dma_busy[proc_id] = 1;
-
-    /* DMA configuration for write transfer */
-    dma[DMA_IRQ_DISABLE] = 0;
-    dma[DMA_SRC] = (unsigned int)buffer;
-    dma[DMA_DST] = (unsigned int)fb_address;
-    dma[DMA_LEN] = (unsigned int)length;
-    return 0;
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// _fb_read()
-// Transfer data from the frame_buffer device to an memory buffer using a DMA.
-// The destination memory buffer must be in user address space.
-// - offset : offset (in bytes) in the frame buffer.
-// - buffer : base address of the memory buffer.
-// - length : number of bytes to be transfered.
-// All cache lines corresponding to the the target buffer are invalidated
-// for cache coherence.
-// Returns 0 if success, > 0 if error.
-//////////////////////////////////////////////////////////////////////////////////
-unsigned int _fb_read( unsigned int 	offset, 
-                       const void*	buffer, 
-                       unsigned int 	length )
-{
-    volatile unsigned char *fb_address;
-    volatile unsigned int *dma;
-
-    unsigned int proc_id;
-    unsigned int delay;
-    unsigned int i;
-
-    /* buffer must be in user space */
-    if (((unsigned int)buffer >= 0x80000000)
-            || (((unsigned int)buffer + length ) >= 0x80000000 ))
-        return 1;
-
-    proc_id = _procid();
-    fb_address = (unsigned char*)&seg_fb_base + offset;
-    dma = (unsigned int*)&seg_dma_base + (proc_id * DMA_SPAN);
-
-    /* waiting until DMA device is available */
-    while (_dma_busy[proc_id] != 0)
-    {
-        /* if the lock failed, busy wait with a pseudo random delay between bus
-         * accesses */
-        delay = (_proctime() & 0xF) << 4;
-        for (i = 0; i < delay; i++)
-            asm volatile("nop");
+    
+    // unmap the buffer from IOMMU page table if IOMMU is activated
+    if ( GIET_IOMMU_ACTIVE )
+    {
+        unsigned int* iob_address = (unsigned int*)&seg_iob_base;
+        unsigned int  ix1         = _dma_iommu_ix1 + dma_id;
+        unsigned int  ix2;
+
+        for ( ix2 = 0 ; ix2 < _dma_iommu_npages[dma_id] ; ix2++ )
+        {
+            // unmap the page in IOMMU page table
+            _iommu_inval_pte2( ix1,		// PT1 index 
+                               ix2 );   // PT2 index
+
+            // clear IOMMU TLB
+            iob_address[IOB_INVAL_PTE] = (ix1 << 21) | (ix2 << 12);
+        }
     }
-    _dma_busy[proc_id] = 1;
-
-    /* DMA configuration for write transfer */
-    dma[DMA_IRQ_DISABLE] = 0;
-    dma[DMA_SRC] = (unsigned int)fb_address;
-    dma[DMA_DST] = (unsigned int)buffer;
-    dma[DMA_LEN] = (unsigned int)length;
-
-    /* invalidation of data cache */
-    _dcache_buf_invalidate(buffer, length);
-
-    return 0;
-}
-
-//////////////////////////////////////////////////////////////////////////////////
-// _fb_completed()
-// This function checks completion of a DMA transfer to or fom the frame buffer.
-// As it is a blocking call, the processor is stalled until the next interrupt.
-// Returns 0 if success, > 0 if error.
-//////////////////////////////////////////////////////////////////////////////////
-unsigned int _fb_completed()
-{
-    unsigned int proc_id;
-
-    proc_id = _procid();
-
-    while (_dma_busy[proc_id] != 0)
-        asm volatile("nop");
-
-    if (_dma_status[proc_id] != 0)
-        return 1;
-
-    return 0;
-}
-
+
+    return _dma_status[dma_id];
+}
+
