Index: soft/giet_vm/giet_common/kernel_malloc.c
===================================================================
--- soft/giet_vm/giet_common/kernel_malloc.c	(revision 514)
+++ soft/giet_vm/giet_common/kernel_malloc.c	(revision 594)
@@ -5,19 +5,22 @@
 // Copyright (c) UPMC-LIP6
 ////////////////////////////////////////////////////////////////////////////////
-//   Implementation note:
+// Implementation note:
 // - As this code is used to implement the SQT lock ptotecting TTY0,
 //   all functions here use the kernel _nolock_printf() function.
 // - It must exist one vseg with the HEAP type in each cluster. The length
 //   must be a power of 2, and the base address must be aligned.
+// - An array kernel_heap[x][y] containing the heap descriptors is
+//   stored in cluster[0][0].
+// - Each kernel_heap[x][y] structure contains a specific queuing spin-lock.
+////////////////////////////////////////////////////////////////////////////////
+// Allocation policy:
 // - All allocated blocks have a size that is a power of 2, larger or equal
 //   to MIN_BLOCK_SIZE (typically 64 bytes), and are aligned.
 // - All free blocks are pre-classed in 32 linked lists of free blocks, where 
-//   all blocks in the same list have the same size. 
+//   all blocks in a given list have the same size. 
 // - The NEXT pointer implementing those linked lists is written 
 //   in the 4 first bytes of the block itself, using the unsigned int type.
 // - The pointers on the first free block for each size are stored in an
 //   array of pointers free[32] in the heap[x][y) structure itself.
-// - Each kernel_heap[x][y] structure is protected by a specific 
-//   queuing spin-lock.
 // - The block size required can be any value, but the allocated block size
 //   is the smallest power of 2 value larger or equal to the requested size.
@@ -32,4 +35,17 @@
 //   the proper lists. etc... 
 ////////////////////////////////////////////////////////////////////////////////
+// Free policy:
+// - Each allocated block is registered in an alloc[] array of unsigned char.
+// - This registration is required by the free() operation, because the size
+//   of the allocated block must be obtained from the base address of the block.  
+// - The number of entries in this array is equal to the max number
+//   of allocated block is : heap_size / MIN_BLOCK_SIZE
+// - For each allocated block, the value registered in the alloc[] array
+//   is log2( size_of_allocated_block ).
+// - The index in this array is computed from the allocated block base address:
+//      index = (block_base - heap_base) / MIN_BLOCK_SIZE
+// - The alloc[] array is stored at the end of heap segment. This consume
+//   (1 / MIN_BLOCK_SIZE) of the total heap storage capacity.
+////////////////////////////////////////////////////////////////////////////////
 
 #include "giet_config.h"
@@ -38,4 +54,5 @@
 #include "kernel_malloc.h"
 #include "kernel_locks.h"
+#include "sys_handler.h"
 #include "tty0.h"
 #include "utils.h"
@@ -46,5 +63,5 @@
 
 __attribute__((section(".kdata")))
-kernel_heap_t kernel_heap[X_SIZE][Y_SIZE];
+kernel_heap_t     kernel_heap[X_SIZE][Y_SIZE];
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -86,50 +103,30 @@
 
 #if GIET_DEBUG_SYS_MALLOC
+
 ////////////////////////////////////////////////
 static void _display_free_array( unsigned int x,
                                  unsigned int y )
 {
-    _nolock_printf(" Kernel Heap[%d][%d]\n"
-                   " - heap_base   = %x\n"
-                   " - heap_size   = %x\n"
-                   " - free[0]     = %x\n"
-                   " - free[1]     = %x\n"
-                   " - free[2]     = %x\n"
-                   " - free[3]     = %x\n"
-                   " - free[4]     = %x\n"
-                   " - free[5]     = %x\n"
-                   " - free[6]     = %x\n"
-                   " - free[7]     = %x\n"
-                   " - free[8]     = %x\n"
-                   " - free[9]     = %x\n"
-                   " - free[10]    = %x\n"
-                   " - free[11]    = %x\n"
-                   " - free[12]    = %x\n"
-                   " - free[13]    = %x\n"
-                   " - free[14]    = %x\n"
-                   " - free[15]    = %x\n"
-                   " - free[16]    = %x\n"
-                   " - free[17]    = %x\n"
-                   " - free[18]    = %x\n"
-                   " - free[19]    = %x\n"
-                   " - free[20]    = %x\n"
-                   " - free[21]    = %x\n"
-                   " - free[22]    = %x\n"
-                   " - free[23]    = %x\n",
-                   x, y, 
-                   kernel_heap[x][y].heap_base, kernel_heap[x][y].heap_size, 
-                   kernel_heap[x][y].free[0] , kernel_heap[x][y].free[1], 
-                   kernel_heap[x][y].free[2] , kernel_heap[x][y].free[3],
-                   kernel_heap[x][y].free[4] , kernel_heap[x][y].free[5],
-                   kernel_heap[x][y].free[6] , kernel_heap[x][y].free[7],
-                   kernel_heap[x][y].free[8] , kernel_heap[x][y].free[9],
-                   kernel_heap[x][y].free[10], kernel_heap[x][y].free[11],
-                   kernel_heap[x][y].free[12], kernel_heap[x][y].free[13],
-                   kernel_heap[x][y].free[14], kernel_heap[x][y].free[15],
-                   kernel_heap[x][y].free[16], kernel_heap[x][y].free[17],
-                   kernel_heap[x][y].free[18], kernel_heap[x][y].free[19],
-                   kernel_heap[x][y].free[20], kernel_heap[x][y].free[21],
-                   kernel_heap[x][y].free[22], kernel_heap[x][y].free[23]);
-}  // end display_free array()
+    unsigned int next;
+    unsigned int id;
+    unsigned int iter;
+
+    _nolock_printf("\nKernel Heap[%d][%d] base = %x / size = %x\n", x , y , 
+                   kernel_heap[x][y].heap_base, kernel_heap[x][y].heap_size );
+    for ( id = 6 ; id < 24 ; id++ )
+    { 
+        next = kernel_heap[x][y].free[id];
+        _nolock_printf(" - free[%d] = " , id );
+        iter = 0;
+        while ( next != 0 )
+        {
+            _nolock_printf("%x | ", next );
+            next = (*(unsigned int*)next);
+            iter++;
+        }
+        _nolock_printf("0\n");
+    }
+}  // end _display_free_array()
+
 #endif
 
@@ -137,5 +134,8 @@
 
 
-/////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// This function returns the heap_base and heap_size values for cluster[x][y],
+// from information defined in the mapping.
+////////////////////////////////////////////////////////////////////////////////
 unsigned int _get_heap_info( unsigned int* heap_base,
                              unsigned int* heap_size,
@@ -186,4 +186,9 @@
     unsigned int heap_size;
     unsigned int heap_index;
+
+    unsigned int alloc_base; 
+    unsigned int alloc_size;
+    unsigned int alloc_index;
+
     unsigned int index;
     unsigned int x;
@@ -195,13 +200,12 @@
         for ( y = 0 ; y < Y_SIZE ; y++ )
         {
-            // get heap_base, heap size, and heap index
+            // get heap_base & heap size
             ko = _get_heap_info( &heap_base, &heap_size, x, y );
        
             if ( ko )  // no kernel heap found in cluster[x][y]
             {
-                // initialise kernel_heap[x][y] descriptor
+                // initialise kernel_heap[x][y] descriptor to empty
                 kernel_heap[x][y].heap_base  = 0;
                 kernel_heap[x][y].heap_size  = 0;
-                _spin_lock_init( &kernel_heap[x][y].lock );
             }
             else       // kernel heap found in cluster[x][y]
@@ -223,14 +227,42 @@
                 }
 
-                // initialise the free[] array 
-                for ( index = 0 ; index < 32 ; index++ ) 
+                // compute size of block containin alloc[] array 
+                alloc_size = heap_size / MIN_BLOCK_SIZE;
+                if ( alloc_size < MIN_BLOCK_SIZE) alloc_size = MIN_BLOCK_SIZE;
+
+                // get index for the corresponding block
+                alloc_index = GET_SIZE_INDEX( alloc_size );
+
+                // compute alloc[] array base address
+                alloc_base = heap_base + heap_size - alloc_size;
+
+                // reset the free[] array 
+                for ( index = 0 ; index < 32 ; index++ )
                 {
-                    if (index == heap_index) kernel_heap[x][y].free[index] = heap_base;
-                    else                     kernel_heap[x][y].free[index] = 0;
+                    kernel_heap[x][y].free[index] = 0;
                 }
 
-                // initialise kernel_heap[x][y] descriptor
+                // reset the alloc_size array
+                memset( (unsigned char*)alloc_base , 0 , alloc_size );
+ 
+                // split the heap into various sizes blocks,
+                // initializes the free[] array and NEXT pointers
+                // base is the block base address
+                unsigned int   base = heap_base;
+                unsigned int*  ptr;
+                for ( index = heap_index-1 ; index >= alloc_index ; index-- )
+                {
+                    kernel_heap[x][y].free[index] = base;
+                    ptr = (unsigned int*)base;
+                    *ptr = 0;
+                    base = base + (1<<index);
+                }
+
                 kernel_heap[x][y].heap_base  = heap_base;
                 kernel_heap[x][y].heap_size  = heap_size;
+                kernel_heap[x][y].alloc_size = alloc_size;
+                kernel_heap[x][y].alloc_base = alloc_base;
+
+                // initialise lock
                 _spin_lock_init( &kernel_heap[x][y].lock );
             }
@@ -338,23 +370,43 @@
     unsigned int requested_index = GET_SIZE_INDEX( size );
 
-    // take the lock 
+    // get the lock protecting heap[x][y]
     _spin_lock_acquire( &kernel_heap[x][y].lock );
 
-    // call the recursive function get_block
+    // call the recursive function get_block()
     unsigned int base = _get_block( &kernel_heap[x][y], 
                                     requested_index, 
                                     requested_index );
+
+    // check block found
+    if ( base == 0 )
+    {
+        _nolock_printf("\n[GIET ERROR] in _remote_malloc() : "
+                       "no more space in kernel_heap[%d][%d]\n", x , y );
+        _spin_lock_release( &kernel_heap[x][y].lock );
+        _exit();
+    }
+
+    // compute pointer in alloc[] array
+    unsigned offset    = (base - kernel_heap[x][y].heap_base) / MIN_BLOCK_SIZE;
+    unsigned char* ptr = (unsigned char*)(kernel_heap[x][y].alloc_base + offset);
+
+    // check the alloc[] array
+    if ( *ptr != 0 )
+    {
+        _nolock_printf("\n[GIET ERROR] in _remote_malloc() for heap[%d][%d] "
+                       "selected block %X already allocated...\n", x , y , base );
+        _spin_lock_release( &kernel_heap[x][y].lock );
+        _exit();
+    }
+
+    // update alloc_array
+    *ptr = requested_index;
+
     // release the lock
     _spin_lock_release( &kernel_heap[x][y].lock );
  
-    if ( base == 0 )
-    {
-        _nolock_printf("\n[GIET ERROR] _remote_malloc() : no more space "
-                       "in heap[%d][%d]", x, y );
-    }
-
 #if GIET_DEBUG_SYS_MALLOC
-_nolock_printf("\n[DEBUG KERNEL_MALLOC] malloc vaddr %x from kernel_heap[%d][%d]\n", 
-               base , x , y );
+_nolock_printf("\n[DEBUG KERNEL_MALLOC] _remote-malloc()"
+               " / vaddr %x / size = %x from heap[%d][%d]\n", base , size, x , y );
 _display_free_array(x,y);
 #endif
@@ -365,4 +417,159 @@
 
 
+
+//////////////////////////////////
+void* _malloc( unsigned int size )
+{
+    unsigned int procid  = _get_procid();
+    unsigned int x       = procid >> (Y_WIDTH + P_WIDTH);
+    unsigned int y       = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
+
+    return _remote_malloc( size , x , y );
+
+}  // end _malloc()
+
+
+///////////////////////////////////////////////
+void _update_free_array( kernel_heap_t*  heap,
+                         unsigned int    base,
+                         unsigned int    size_index )
+{
+    // This recursive function try to merge the released block 
+    // with the companion block if this companion block is free.
+    // This companion has the same size, and almost the same address
+    // (only one address bit is different)
+    // - If the companion is not in free[size_index],
+    //   the released block is pushed in free[size_index].
+    // - If the companion is found, it is evicted from free[size_index]
+    //   and the merged bloc is pushed in the free[size_index+1].
+
+    // compute released block size
+    unsigned int size = 1<<size_index;
+
+    // compute companion block and merged block base address 
+    unsigned int companion_base; 
+    unsigned int merged_base;  
+
+    if ( (base & size) == 0 )   // the released block is aligned on (size*2)
+    {
+        companion_base  = base + size;
+        merged_base     = base;
+    }
+    else
+    {
+        companion_base  = base - size;
+        merged_base     = base - size;
+    }
+
+#if GIET_DEBUG_SYS_MALLOC > 1
+_nolock_printf("\n[DEBUG KERNEL_MALLOC] _update_free_array() :\n"
+               " - size           = %X\n"
+               " - released_base  = %X\n"
+               " - companion_base = %X\n"
+               " - merged_base    = %X\n",
+               size , base , companion_base , merged_base , (base & size) );
+#endif
+
+    // scan all blocks in free[size_index]
+    // the iter & prev variables are actually addresses
+    unsigned int  found = 0;
+    unsigned int  iter  = heap->free[size_index];
+    unsigned int  prev  = (unsigned int)(&heap->free[size_index]);
+    while ( iter ) 
+    {
+        if ( iter == companion_base ) 
+        {
+            found = 1;
+            break;
+        }
+        prev = iter;
+        iter = *(unsigned int*)iter;
+    }
+
+    if ( found == 0 )  // Companion not found => register in free[size_index]
+    {
+
+#if GIET_DEBUG_SYS_MALLOC > 1
+_nolock_printf("\n[DEBUG KERNEL_MALLOC] _update_free_array() : companion "
+               " not found => register block %x in free[%d]", base , size );
+#endif
+
+        // push the block in free[size_index]  
+        *(unsigned int*)base   = heap->free[size_index];
+        heap->free[size_index] = base;
+    }
+    else               // Companion found : merge and try in free[size_index + 1]
+    {
+        // pop the companion block (address == iter) from free[size_index]
+        *(unsigned int*)prev = *(unsigned int*)iter;
+
+        // call the update_free() function for free[size_index+1]
+        _update_free_array( heap, merged_base , size_index+1 );
+    }
+}  // end _update_free_array()
+
+
+
+///////////////////////
+void _free( void* ptr )
+{
+    // get cluster coordinates from ptr value
+    unsigned int x;
+    unsigned int y;
+    _sys_xy_from_ptr( ptr, &x, &y );
+
+    // check ptr value
+    unsigned int base = (unsigned int)ptr;
+    if ( (base < kernel_heap[x][y].heap_base) || 
+         (base >= (kernel_heap[x][y].heap_base + kernel_heap[x][y].heap_size)) )
+    {
+        _printf("\n[GIET ERROR] in _free() : illegal pointer for released block");
+        _exit();
+    }
+ 
+    // get the lock protecting heap[x][y]
+    _spin_lock_acquire( &kernel_heap[x][y].lock );
+
+    // compute released block index in alloc[] array
+    unsigned index = (base - kernel_heap[x][y].heap_base ) / MIN_BLOCK_SIZE;
+ 
+    // get the released block size_index
+    unsigned char* pchar      = (unsigned char*)(kernel_heap[x][y].alloc_base + index);
+    unsigned int   size_index = (unsigned int)*pchar;
+
+    // check block allocation
+    if ( size_index == 0 )
+    {
+        _printf("\n[GIET ERROR] in _free() : released block %X not allocated "
+                "in kernel_heap[%d][%d]\n", (unsigned int)ptr , x , y );
+        _spin_lock_release( &kernel_heap[x][y].lock );
+        _exit();
+    }
+
+    // check released block alignment
+    if ( base % (1 << size_index) )
+    {
+        _printf("\n[GIET ERROR] in _free() : released block %X not aligned "
+                "in kernel_heap[%d][%d]\n", (unsigned int)ptr , x , y );
+        _spin_lock_release( &kernel_heap[x][y].lock );
+        _exit();
+    }
+
+    // remove block from allocated blocks array
+    *pchar = 0;
+
+    // call the recursive function update_free_array() 
+    _update_free_array( &kernel_heap[x][y] , base , size_index ); 
+
+    // release the lock
+    _spin_lock_release( &kernel_heap[x][y].lock );
+
+#if GIET_DEBUG_SYS_MALLOC
+_nolock_printf("\n[DEBUG KERNEL_MALLOC] _free() : vaddr = %x / size = %x "
+               "to heap[%d][%d]\n", (unsigned int)ptr , 1<<size_index , x , y );
+_display_free_array(x,y);
+#endif
+
+}  // end _free()
 
 // Local Variables:
