Index: soft/giet_vm/sys/common.c
===================================================================
--- soft/giet_vm/sys/common.c	(revision 232)
+++ soft/giet_vm/sys/common.c	(revision 238)
@@ -20,6 +20,4 @@
 ///////////////////////////////////////////////////////////////////////////////////
 
-// current context cache TODO
-
 // SR save (used by _it_mask() / it_restore()
 unsigned int _status_register_save[NB_CLUSTERS*NB_PROCS_MAX];
@@ -27,14 +25,20 @@
 ///////////////////////////////////////////////////////////////////////////////////
 //       _get_sched()
-// Access CP0 and returns scheduler physical address.
-///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_sched() {
-    unsigned int ret;
-    asm volatile(
-            "mfc0    %0,        $22" 
-            : "=r"(ret));
-    return ret;
-}
-
+// Access CP0 and returns a pointer (virtual address) on the calling
+// processor scheduler (taking into account the processor local index).
+///////////////////////////////////////////////////////////////////////////////////
+static_scheduler_t* _get_sched() 
+{
+    static_scheduler_t* psched;
+    unsigned int        vaddr;
+    unsigned int        lpid = _procid() % NB_PROCS_MAX;
+
+    asm volatile(
+            "mfc0    %0,   $22    \n" 
+            : "=r"(vaddr) );
+
+    psched = (static_scheduler_t*)vaddr;
+    return (psched + lpid);
+}
 
 ///////////////////////////////////////////////////////////////////////////////////
@@ -42,5 +46,6 @@
 // Access CP2 and returns PTPR register.
 ///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_ptpr() {
+inline unsigned int _get_ptpr() 
+{
     unsigned int ret;
     asm volatile(
@@ -50,10 +55,10 @@
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////
 //       _get_epc()
 // Access CP0 and returns EPC register.
 ///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_epc() {
+inline unsigned int _get_epc() 
+{
     unsigned int ret;
     asm volatile("mfc0    %0,        $14" 
@@ -62,10 +67,10 @@
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////
 //       _get_bar()
 // Access CP0 and returns BAR register.
 ///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_bvar() {
+inline unsigned int _get_bvar() 
+{
     unsigned int ret;
     asm volatile(
@@ -75,10 +80,10 @@
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////
 //       _get_cr()
 // Access CP0 and returns CR register.
 ///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_cause() {
+inline unsigned int _get_cause() 
+{
     unsigned int ret;
     asm volatile("mfc0    %0,        $13" 
@@ -87,10 +92,10 @@
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////
 //       _get_sr()
 // Access CP0 and returns SR register.
 ///////////////////////////////////////////////////////////////////////////////////
-inline unsigned int _get_sr() {
+inline unsigned int _get_sr() 
+{
     unsigned int ret;
     asm volatile(
@@ -106,5 +111,6 @@
 // This function is NOT USED and NOT TESTED
 ///////////////////////////////////////////////////////////////////////////////////
-inline void _it_mask() {
+inline void _it_mask() 
+{
     unsigned int sr_value;
     unsigned int proc_id;
@@ -121,5 +127,4 @@
 }
 
-
 ///////////////////////////////////////////////////////////////////////////////////
 //    _it_restore()
@@ -128,5 +133,6 @@
 // This function is NOT USED and NOT TESTED
 ///////////////////////////////////////////////////////////////////////////////////
-inline void _it_restore() {
+inline void _it_restore() 
+{
     unsigned int proc_id;
     // get the processor id to index the _status_register_save table
@@ -140,5 +146,6 @@
 // Access CP0 and disables IRQs
 ///////////////////////////////////////////////////////////////////////////////////
-inline void _it_disable() {
+inline void _it_disable() 
+{
     asm volatile(
             "li      $3,        0xFFFFFFFE    \n"
@@ -153,5 +160,6 @@
 // Access CP0 and enables IRQs
 ///////////////////////////////////////////////////////////////////////////////////
-inline void _it_enable() {
+inline void _it_enable() 
+{
     asm volatile(
             "li      $3,        0x00000001    \n"
@@ -162,5 +170,4 @@
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 //    _get_lock()
@@ -169,5 +176,6 @@
 // (delay average value = 100 cycles)
 ////////////////////////////////////////////////////////////////////////////
-inline void _get_lock(unsigned int * plock) {
+inline void _get_lock(unsigned int * plock) 
+{
     register unsigned int delay = ( _proctime() ^ _procid() << 4) & 0xFF;
 
@@ -191,9 +199,9 @@
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 // _release_lock()
 ////////////////////////////////////////////////////////////////////////////
-inline void _release_lock(unsigned int * plock) {
+inline void _release_lock(unsigned int * plock) 
+{
     asm volatile (
             "sync\n" /* necessary because of the consistency model in tsar */
@@ -202,27 +210,26 @@
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 //    _puts()
 // display a string on TTY0 / used for system code debug and log
 ////////////////////////////////////////////////////////////////////////////
-void _puts(char * buffer) {
+void _puts(char * buffer) 
+{
     unsigned int * tty_address = (unsigned int *) &seg_tty_base;
     unsigned int n;
 
-    for (n = 0; n < 100; n++) {
-        if (buffer[n] == 0) {
-            break;
-        }
+    for (n = 0; n < 100; n++) 
+    {
+        if (buffer[n] == 0)  break; 
         tty_address[TTY_WRITE] = (unsigned int) buffer[n];
     }
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 //    _putx() 
-// display an int (hexa) on TTY0 / used for system code debug and log
-////////////////////////////////////////////////////////////////////////////
-void _putx(unsigned int val) {
+// display a 32 bits unsigned int as an hexadecimal string on TTY0 
+////////////////////////////////////////////////////////////////////////////
+void _putx(unsigned int val) 
+{
     static const char HexaTab[] = "0123456789ABCDEF";
     char buf[11];
@@ -233,5 +240,6 @@
     buf[10] = 0;
 
-    for (c = 0; c < 8; c++) { 
+    for (c = 0; c < 8; c++) 
+    { 
         buf[9 - c] = HexaTab[val & 0xF];
         val = val >> 4;
@@ -240,4 +248,25 @@
 }
 
+////////////////////////////////////////////////////////////////////////////
+//    _putl() 
+// display a 64 bits unsigned long as an hexadecimal string on TTY0 
+////////////////////////////////////////////////////////////////////////////
+void _putl(paddr_t val) 
+{
+    static const char HexaTab[] = "0123456789ABCDEF";
+    char buf[19];
+    unsigned int c;
+
+    buf[0] = '0';
+    buf[1] = 'x';
+    buf[18] = 0;
+
+    for (c = 0; c < 16; c++) 
+    { 
+        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
+        val = val >> 4;
+    }
+    _puts(buf);
+}
 
 ////////////////////////////////////////////////////////////////////////////
@@ -266,5 +295,4 @@
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 //    _strncmp()
@@ -284,5 +312,4 @@
 }
 
-
 ////////////////////////////////////////////////////////////////////////////
 //        _dcache_buf_invalidate()
@@ -310,140 +337,64 @@
 }
 
-
-////////////////////////////////////////////////////////////////////////////
-//    _physical_read_access()
-// This function makes a physical read access to a 32 bits word in memory, 
-// after a temporary DTLB desactivation. 
-////////////////////////////////////////////////////////////////////////////
-unsigned int _physical_read_access(unsigned int * paddr) {
-    unsigned int value;
-
-    asm volatile(
-            "li     $3,     0xFFFFFFFE         \n"
-            "mfc0   $2,     $12                \n"        /* $2 <= SR        */
-            "and    $3,     $3,        $2      \n"
-            "mtc0   $3,     $12                \n"        /* interrupt masked */
-            "li     $3,     0xB                \n"
-            "mtc2   $3,     $1                 \n"        /* DTLB off            */    
-
-            "lw     %0,     0(%1)              \n"        /* entry <= *pslot    */
-
-            "li     $3,     0xF                \n"
-            "mtc2   $3,     $1                 \n"        /* DTLB on             */    
-            "mtc0   $2,     $12                \n"        /* restore SR        */
-            : "=r" (value)
-            : "r" (paddr)
-            : "$2", "$3");
-    return value;
-}
-
-
-////////////////////////////////////////////////////////////////////////////
-//    _physical_write_access()
-// This function makes a physical write access to a 32 bits word in memory, 
-// after a temporary DTLB desactivation. 
-////////////////////////////////////////////////////////////////////////////
-void _physical_write_access(unsigned int * paddr, unsigned int value) {
-    asm volatile(
-            "li     $3,     0xFFFFFFFE         \n"
-            "mfc0   $2,     $12                \n"        /* $26 <= SR        */
-            "and    $3,     $3,        $2      \n"
-            "mtc0   $3,     $12                \n"        /* interrupt masked */
-            "li     $3,     0xB                \n"
-            "mtc2   $3,     $1                 \n"        /* DTLB off            */
-
-            "sw     %0,     0(%1)              \n"        /* entry <= *pslot    */
-
-            "li     $3,     0xF                \n"
-            "mtc2   $3,     $1                 \n"        /* DTLB on             */    
-            "mtc0   $2,     $12                \n"        /* restore SR        */
-            :
-            : "r" (value), "r" (paddr)
-            : "$2", "$3");
-}
-
-
-////////////////////////////////////////////////////////////////////////////
-//    _get_tasks_number()
-// This function returns the number of tasks allocated to processor.
-////////////////////////////////////////////////////////////////////////////
-unsigned int _get_tasks_number() {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    return _physical_read_access(&(psched->tasks));
-}
-
-
-////////////////////////////////////////////////////////////////////////////
-//    _get_proc_task_id()
-// This function returns the index of the currently running task.
-////////////////////////////////////////////////////////////////////////////
-unsigned int _get_proc_task_id() {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    return _physical_read_access(&(psched->current));
-}
-
-
-////////////////////////////////////////////////////////////////////////////
-//    _set_proc_task_id()
-// This function returns the index of the currently running task.
-////////////////////////////////////////////////////////////////////////////
-void _set_proc_task_id(unsigned int value) {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    _physical_write_access(&(psched->current), value);
-}
-
-
-////////////////////////////////////////////////////////////////////////////
-//    _get_global_task_id()
-// This function returns the global index of the running task.
-////////////////////////////////////////////////////////////////////////////
-unsigned int _get_global_task_id() {
-  return _get_context_slot(_get_proc_task_id(), CTX_GTID_ID);
-}
-
-
-///////////////////////////////////////////////////////////////////////////////
-//    _get_context_slot()
-// This function returns a slot content for the task defined by task_id.
-///////////////////////////////////////////////////////////////////////////////
-unsigned int _get_context_slot(unsigned int task_id, unsigned int slot_id) {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    return _physical_read_access(&(psched->context[task_id][slot_id]));
-}
-
-
-///////////////////////////////////////////////////////////////////////////////
-//    _set_context_slot()
-// This function returns a slot content for the task defined by task_id.
-///////////////////////////////////////////////////////////////////////////////
-void _set_context_slot( unsigned int task_id,
-        unsigned int slot_id,
-        unsigned int value) {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    _physical_write_access(&(psched->context[task_id][slot_id]), value);
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-//    _get_interrupt_vector_entry()
-// This function returns the interrupt_vector entry defined by argument index.
-////////////////////////////////////////////////////////////////////////////////
-unsigned int _get_interrupt_vector_entry(unsigned int index) {
-    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
-    return _physical_read_access( &(psched->interrupt_vector[index]));
-}
-
+/////////////////////////////////////////////////////////////////////////////
+//      _get_task_slot()
+// This function returns the content of a context slot 
+// for the task identified by the ltid argument (local index).
+/////////////////////////////////////////////////////////////////////////////
+unsigned int _get_task_slot( unsigned int ltid,
+                             unsigned int slot )
+{
+    static_scheduler_t* psched  = _get_sched();
+    return psched->context[ltid][slot];
+}
+
+/////////////////////////////////////////////////////////////////////////////
+//      _set_task_slot()
+// This function updates the content of a context slot 
+// for the task identified by the ltid argument (local index).
+/////////////////////////////////////////////////////////////////////////////
+void _set_task_slot( unsigned int ltid,
+                     unsigned int slot,
+                     unsigned int value )
+{
+    static_scheduler_t* psched  = _get_sched();
+    psched->context[ltid][slot] = value;
+}
+
+/////////////////////////////////////////////////////////////////////////////
+//      _get_context_slot()
+// This function returns the content of a context slot 
+// for the running task (defined by the scheduler current field).
+/////////////////////////////////////////////////////////////////////////////
+unsigned int _get_context_slot( unsigned int slot )
+{
+    static_scheduler_t* psched  = _get_sched();
+    unsigned int        task_id = psched->current;
+    return psched->context[task_id][slot];
+}
+
+/////////////////////////////////////////////////////////////////////////////
+//      _set_context_slot()
+// This function updates the content of a context slot for the running task.
+/////////////////////////////////////////////////////////////////////////////
+void _set_context_slot( unsigned int slot,
+                       unsigned int value )
+{
+    static_scheduler_t* psched  = _get_sched();
+    unsigned int        task_id = psched->current;
+    psched->context[task_id][slot] = value;
+}
 
 /////////////////////////////////////////////////////////////////////////////
 //      access functions to mapping_info data structure
 /////////////////////////////////////////////////////////////////////////////
-mapping_cluster_t * _get_cluster_base(mapping_header_t * header) {
+mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
+{
     return (mapping_cluster_t *) ((char *) header +
             MAPPING_HEADER_SIZE);
 }
-
-
-/////////////////////////////////////////////////////////////////////////////
-mapping_pseg_t * _get_pseg_base(mapping_header_t * header) {
+/////////////////////////////////////////////////////////////////////////////
+mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
+{
     return (mapping_pseg_t *) ((char *) header +
             MAPPING_HEADER_SIZE +
@@ -451,5 +402,6 @@
 }
 /////////////////////////////////////////////////////////////////////////////
-mapping_vspace_t * _get_vspace_base(mapping_header_t * header) {
+mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
+{
     return (mapping_vspace_t *)  ((char *) header +
             MAPPING_HEADER_SIZE +
@@ -471,5 +423,6 @@
 
 /////////////////////////////////////////////////////////////////////////////
-mapping_vobj_t * _get_vobj_base(mapping_header_t * header) {
+mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
+{
     return (mapping_vobj_t *) ((char *) header +
             MAPPING_HEADER_SIZE +
@@ -482,5 +435,6 @@
 
 /////////////////////////////////////////////////////////////////////////////
-mapping_task_t * _get_task_base(mapping_header_t * header) {
+mapping_task_t * _get_task_base(mapping_header_t * header) 
+{
     return (mapping_task_t *) ((char *) header +
             MAPPING_HEADER_SIZE +
