Index: trunk/hal/tsar_mips32/core/hal_gpt.c
===================================================================
--- trunk/hal/tsar_mips32/core/hal_gpt.c	(revision 401)
+++ trunk/hal/tsar_mips32/core/hal_gpt.c	(revision 406)
@@ -131,11 +131,14 @@
 	page_t   * page;
     xptr_t     page_xp;
+    vpn_t      vpn;
+    error_t    error;
+    uint32_t   attr;
+
+    gpt_dmsg("\n[DMSG] %s : core[%x,%d] enter\n", 
+    __FUNCTION__ , local_cxy , CURRENT_THREAD->core->lid );
 
     // check page size
-    if( CONFIG_PPM_PAGE_SIZE != 4096 )
-    {
-        printk("\n[PANIC] in %s : For TSAR, the page must be 4 Kbytes\n", __FUNCTION__ );
-        hal_core_sleep();
-    }
+    assert( (CONFIG_PPM_PAGE_SIZE == 4096) , __FUNCTION__ ,
+    "for TSAR, the page must be 4 Kbytes\n" );
 
     // allocates 2 physical pages for PT1
@@ -146,9 +149,9 @@
 	page = (page_t *)kmem_alloc( &req );
 
-	if( page == NULL )
-    {
-		printk("\n[ERROR] in %s : cannot allocate physical memory for PT1\n", __FUNCTION__ );
+	if( page == NULL ) 
+    {
+        printk("\n[ERROR] in %s : cannot allocate memory for PT1\n", __FUNCTION__ );
         return ENOMEM;
-	}
+    }
 
     // initialize generic page table descriptor
@@ -159,5 +162,28 @@
 	gpt->page = GET_PTR( page_xp );
 
+    // identity map the kentry_vseg (must exist for all processes)
+    attr = GPT_MAPPED | GPT_SMALL | GPT_EXECUTABLE | GPT_CACHABLE | GPT_GLOBAL;
+    for( vpn = CONFIG_VMM_KENTRY_BASE;
+         vpn < (CONFIG_VMM_KENTRY_BASE + CONFIG_VMM_KENTRY_SIZE); vpn++ )
+    {
+        gpt_dmsg("\n[DMSG] %s : identity map vpn %d\n", __FUNCTION__ , vpn );
+
+        error = hal_gpt_set_pte( gpt,
+                                 vpn,
+                                 (local_cxy<<20) | (vpn & 0xFFFFF),
+                                 attr );
+
+        if( error )
+        {
+            printk("\n[ERROR] in %s : cannot identity map kentry vseg\n", __FUNCTION__ );
+            return ENOMEM;
+        }
+    }
+
+    gpt_dmsg("\n[DMSG] %s : core[%x,%d] exit\n",
+    __FUNCTION__ , local_cxy , CURRENT_THREAD->core->lid );
+
 	return 0;
+
 } // end hal_gpt_create()
 
@@ -245,6 +271,7 @@
 } // end hal_gpt_destroy()
 
-/////////////////////////////////
-void hal_gpt_print( gpt_t * gpt )
+////////////////////////////////
+void hal_gpt_print( gpt_t * gpt,
+                    pid_t   pid )
 {
 	uint32_t   ix1;
@@ -256,9 +283,11 @@
     uint32_t   pte2_attr;
     ppn_t      pte2_ppn;
-
-    printk("*** Page Table for process %x in cluster %x ***\n",
-           CURRENT_THREAD->process->pid , local_cxy );
+    vpn_t      vpn;
+
 
     pt1 = (uint32_t *)gpt->ptr;
+
+    printk("\n***** Generic Page Table for process %x : &gpt = %x / &pt1 = %x\n\n",
+    pid , gpt , pt1 );
 
     // scan the PT1
@@ -270,5 +299,6 @@
             if( (pte1 & TSAR_MMU_SMALL) == 0 )  // BIG page
             {
-                printk(" - BIG   : pt1[%d] = %x\n", ix1 , pte1 );
+                vpn = ix1 << 9;
+                printk(" - BIG   : vpn = %x / pt1[%d] = %X\n", vpn , ix1 , pte1 );
             }
             else                           // SMALL pages
@@ -283,8 +313,10 @@
                     pte2_attr = TSAR_MMU_ATTR_FROM_PTE2( pt2[2 * ix2] );
                     pte2_ppn  = TSAR_MMU_PPN_FROM_PTE2( pt2[2 * ix2 + 1] );
+
 		            if( (pte2_attr & TSAR_MMU_MAPPED) != 0 )
                     {
-                        printk(" - SMALL   : pt1[%d] = %x / pt2[%d] / pt2[%d]\n",
-                               ix1 , pt1[ix1] , 2*ix2 , pte2_attr , 2*ix2+1 , pte2_ppn );
+                        vpn = (ix1 << 9) | ix2;
+                        printk(" - SMALL : vpn = %x / PT2[%d] = %x / pt2[%d] = %x\n",
+                        vpn , 2*ix2 , pte2_attr , 2*ix2+1 , pte2_ppn );
                     }
                 }
@@ -301,13 +333,13 @@
                          uint32_t  attr )    // generic GPT attributes
 {
-    uint32_t          * pt1;                 // virtual base addres of PT1
-	volatile uint32_t * pte1_ptr;            // pointer on PT1 entry
+    uint32_t          * pt1;                 // PT1 base addres
+	uint32_t          * pte1_ptr;            // pointer on PT1 entry
 	uint32_t            pte1;                // PT1 entry value
 
 	ppn_t               pt2_ppn;             // PPN of PT2
-	uint32_t          * pt2;                 // virtual base address of PT2
+	uint32_t          * pt2;                 // PT2 base address
 
 	uint32_t            small;               // requested PTE is for a small page
-	bool_t              atomic;
+    bool_t              success;             // exit condition for while loop below
 
 	page_t            * page;                // pointer on new physical page descriptor
@@ -319,4 +351,7 @@
     uint32_t            tsar_attr;           // PTE attributes for TSAR MMU
 
+    gpt_dmsg("\n[DMSG] %s : core[%x,%d] enter for vpn = %x / ppn = %x / gpt_attr = %x\n", 
+    __FUNCTION__ , local_cxy , CURRENT_THREAD->core->lid , vpn , ppn , attr );
+ 
     // compute indexes in PT1 and PT2
     ix1 = TSAR_MMU_IX1_FROM_VPN( vpn );
@@ -329,17 +364,19 @@
     tsar_attr = gpt2tsar( attr );
 
-    // get PT1 entry value
+    gpt_dmsg("\n[DMSG] %s : core[%x,%d] / vpn = %x / &pt1 = %x / tsar_attr = %x\n", 
+    __FUNCTION__, local_cxy , CURRENT_THREAD->core->lid , vpn , pt1 , tsar_attr );
+
+    // get pointer on PT1[ix1]
 	pte1_ptr  = &pt1[ix1];
-	pte1      = *pte1_ptr;
-
-    // Big pages (PTE1) are only set for the kernel vsegs, in the kernel init phase.
+
+    // PTE1 (big page) are only set for the kernel vsegs, in the kernel init phase.
     // There is no risk of concurrent access.
 	if( small == 0 ) 
     {
-        if( pte1 != 0 )
-		{
-			panic("\n[PANIC] in %s : set a big page in a mapped PT1 entry / PT1[%d] = %x\n",
-                   __FUNCTION__ , ix1 , pte1 );
-		}
+        // get current pte1 value
+        pte1 = *pte1_ptr;
+
+        assert( (pte1 == 0) , __FUNCTION__ ,
+		"try to set a big page in a mapped PT1 entry / PT1[%d] = %x\n", ix1 , pte1 );
       
         // set the PTE1 
@@ -352,62 +389,58 @@
     // From this point, the requested PTE is a PTE2 (small page)
 
-	if( (pte1 & TSAR_MMU_MAPPED) == 0 )      // the PT1 entry is not valid
-	{
-        // allocate one physical page for the PT2
-	    kmem_req_t req;
-	    req.type  = KMEM_PAGE;
-	    req.size  = 0;                     // 1 small page 
-	    req.flags = AF_KERNEL | AF_ZERO;
-	    page = (page_t *)kmem_alloc( &req );
-        if( page == NULL )
+    // loop to access PTE1 and get pointer on PT2
+    success = false;
+    do
+    {
+        // get current pte1 value
+        pte1 = *pte1_ptr;
+        
+        gpt_dmsg("\n[DMSG] %s : core[%x,%d] / vpn = %x / current_pte1 = %x\n", 
+        __FUNCTION__, local_cxy , CURRENT_THREAD->core->lid , vpn , pte1 );
+        
+        // allocate a PT2 if PT1 entry not valid
+        if( (pte1 & TSAR_MMU_MAPPED) == 0 )             // PT1 entry not valid
+   	    {
+            // allocate one physical page for the PT2
+	        kmem_req_t req;
+	        req.type  = KMEM_PAGE;
+	        req.size  = 0;                       // 1 small page 
+	        req.flags = AF_KERNEL | AF_ZERO;
+            page = (page_t *)kmem_alloc( &req );
+            if( page == NULL )
+            {
+		        printk("\n[ERROR] in %s : cannot allocate PT2\n", __FUNCTION__ );
+                return ENOMEM;
+            }
+
+            // get the PT2 PPN
+            page_xp = XPTR( local_cxy , page );        
+            pt2_ppn = ppm_page2ppn( page_xp );
+
+            // try to atomicaly set the PT1 entry 
+            pte1 = TSAR_MMU_MAPPED | TSAR_MMU_SMALL | pt2_ppn;
+		    success = hal_atomic_cas( pte1_ptr , 0 , pte1 );
+
+            // release allocated PT2 if PT1 entry modified by another thread
+            if( success == false ) ppm_free_pages( page );
+        }
+        else                                           // PT1 entry is valid
         {
-			printk("\n[ERROR] in %s : try to set a small page but cannot allocate PT2\n",
-                    __FUNCTION__ );
-            return ENOMEM;
+            // This valid entry must be a PTD1
+            assert( (pte1 & TSAR_MMU_SMALL) , __FUNCTION__ ,
+            "try to set a small page in a big PT1 entry / PT1[%d] = %x\n", ix1 , pte1 );
+
+            success = true;
         }
 
-        page_xp = XPTR( local_cxy , page );        
-        pt2_ppn = ppm_page2ppn( page_xp );
-        pt2     = (uint32_t *)GET_PTR( ppm_page2base( page_xp ) );
-
-        // try to atomicaly set a PTD1 in the PT1 entry 
-		do 
-		{
-		    atomic = hal_atomic_cas( (void*)pte1, 0 , 
-                                      TSAR_MMU_MAPPED | TSAR_MMU_SMALL | pt2_ppn );
-        } 
-        while( (atomic == false) && (*pte1_ptr == 0) );
-
-	    if( atomic == false ) // the mapping has been done by another thread !!!
-        {
-            // release the allocated page
-	        ppm_free_pages( page );
-
-            // read PT1 entry again 
-			pte1 = *pte1_ptr;
-
-            // compute PPN of PT2 base
-			pt2_ppn = TSAR_MMU_PTBA_FROM_PTE1( pte1 );
-
-            // compute pointer on PT2 base
-			pt2 = (uint32_t*)GET_PTR( ppm_ppn2base( pt2_ppn ) );
-		}
-	}
-	else                             // The PT1 entry is valid  
-	{
-        // This valid entry must be a PTD1
-        if( (pte1 & TSAR_MMU_SMALL) == 0 )
-        {
-			printk("\n[ERROR] in %s : set a small page in a big PT1 entry / PT1[%d] = %x\n",
-                    __FUNCTION__ , ix1 , pte1 );
-            return EINVAL;
-        }
-
-        // compute PPN of PT2 base
-		pt2_ppn = TSAR_MMU_PTBA_FROM_PTE1( pte1 );
-
-        // compute pointer on PT2 base
-	    pt2 = (uint32_t*)GET_PTR( ppm_ppn2base( pt2_ppn ) );
-	}
+        // get PT2 base from pte1 
+	    pt2_ppn = TSAR_MMU_PTBA_FROM_PTE1( pte1 );
+	    pt2     = (uint32_t *)GET_PTR( ppm_ppn2base( pt2_ppn ) );
+
+        gpt_dmsg("\n[DMSG] %s : core[%x,%d] / vpn = %x / pte1 = %x / &pt2 = %x\n", 
+        __FUNCTION__, local_cxy , CURRENT_THREAD->core->lid , vpn , pte1 , pt2 );
+        
+    }
+    while (success == false);
 
     // set PTE2 in this order
@@ -417,7 +450,12 @@
 	hal_fence();
 
+    gpt_dmsg("\n[DMSG] %s : core[%x,%d] exit / vpn = %x / pte2_attr = %x / pte2_ppn = %x\n", 
+    __FUNCTION__ , local_cxy , CURRENT_THREAD->core->lid , vpn , 
+    pt2[2 * ix2] , pt2[2 * ix2 + 1] );
+ 
 	return 0;
 
 } // end of hal_gpt_set_pte()
+
 
 /////////////////////////////////////
