Index: /soft/giet_vm/giet_libs/malloc.c
===================================================================
--- /soft/giet_vm/giet_libs/malloc.c	(revision 393)
+++ /soft/giet_vm/giet_libs/malloc.c	(revision 394)
@@ -46,105 +46,38 @@
                                             (size <= 0x80000000) ? 31 :\
                                                                    32
-////////////////////////////////
-void heap_init( unsigned int x,
-                unsigned int y )
-{
-    unsigned int heap_base;        // heap segment base
-    unsigned int heap_size;        // heap segment size
-    unsigned int heap_index;       // size_index in free[array]
-
-    unsigned int alloc_base;       // alloc[] array base 
-    unsigned int alloc_size;       // alloc[] array size
-    unsigned int alloc_index;      // size_index in free[array]
-
-    unsigned int index;            // iterator
-
-    // get heap_base, heap size, and heap index
-    giet_heap_info( &heap_base, &heap_size, x, y );
-    heap_index = GET_SIZE_INDEX( heap_size );
-
-    // checking heap segment constraints
-    if ( heap_size == 0 )                                    // heap segment exist
-    {
-        giet_exit("ERROR in malloc() : heap not found \n");
-    }
-    if ( heap_size != (1<<heap_index) )                      // heap size power of 2
-    {
-        giet_exit("ERROR in malloc() : heap size must be power of 2\n");
-    }
-    if ( heap_base % heap_size )                             // heap segment aligned
-    {
-        giet_exit("ERROR in malloc() : heap segment must be aligned\n");
-    }
-
-    // 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++ )
-    {
-        heap[x][y].free[index] = 0;
-    }
-
-    // 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-- )
-    {
-        heap[x][y].free[index] = base;
-        ptr = (unsigned int*)base;
-        *ptr = 0;
-        base = base + (1<<index);
-    }
-
-    heap[x][y].init       = HEAP_INITIALIZED;
-    heap[x][y].x          = x;
-    heap[x][y].y          = y;
-    heap[x][y].heap_base  = heap_base;
-    heap[x][y].heap_size  = heap_size;
-    heap[x][y].alloc_size = alloc_size;
-    heap[x][y].alloc_base = alloc_base;
-
-    lock_release( &heap[x][y].lock );
-
-#if GIET_DEBUG_MALLOC
-giet_shr_printf("\n[MALLOC DEBUG] Heap[%d][%d] initialisation\n"
-                " - heap_base  = %x\n"
-                " - heap_size  = %x\n"
-                " - alloc_base = %x\n"
-                " - alloc_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",
+////////////////////////////////////////
+void display_free_array( unsigned int x,
+                         unsigned int y )
+{
+    giet_shr_printf(
+                " - coordinates = [%d][%d]\n"
+                " - heap_base   = %x\n"
+                " - heap_size   = %x\n"
+                " - alloc_base  = %x\n"
+                " - alloc_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",
                 heap[x][y].x, heap[x][y].y, 
                 heap[x][y].heap_base, heap[x][y].heap_size, 
@@ -162,4 +95,80 @@
                 heap[x][y].free[20], heap[x][y].free[21],
                 heap[x][y].free[22], heap[x][y].free[23] );
+}  // end display_free array()
+
+////////////////////////////////
+void heap_init( unsigned int x,
+                unsigned int y )
+{
+    unsigned int heap_base;        // heap segment base
+    unsigned int heap_size;        // heap segment size
+    unsigned int heap_index;       // size_index in free[array]
+
+    unsigned int alloc_base;       // alloc[] array base 
+    unsigned int alloc_size;       // alloc[] array size
+    unsigned int alloc_index;      // size_index in free[array]
+
+    unsigned int index;            // iterator
+
+    // get heap_base, heap size, and heap index
+    giet_heap_info( &heap_base, &heap_size, x, y );
+    heap_index = GET_SIZE_INDEX( heap_size );
+
+    // checking heap segment constraints
+    if ( heap_size == 0 )                                    // heap segment exist
+    {
+        giet_exit("ERROR in malloc() : heap not found \n");
+    }
+    if ( heap_size != (1<<heap_index) )                      // heap size power of 2
+    {
+        giet_exit("ERROR in malloc() : heap size must be power of 2\n");
+    }
+    if ( heap_base % heap_size )                             // heap segment aligned
+    {
+        giet_exit("ERROR in malloc() : heap segment must be aligned\n");
+    }
+
+    // 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++ )
+    {
+        heap[x][y].free[index] = 0;
+    }
+
+    // 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-- )
+    {
+        heap[x][y].free[index] = base;
+        ptr = (unsigned int*)base;
+        *ptr = 0;
+        base = base + (1<<index);
+    }
+
+    heap[x][y].init       = HEAP_INITIALIZED;
+    heap[x][y].x          = x;
+    heap[x][y].y          = y;
+    heap[x][y].heap_base  = heap_base;
+    heap[x][y].heap_size  = heap_size;
+    heap[x][y].alloc_size = alloc_size;
+    heap[x][y].alloc_base = alloc_base;
+
+    lock_release( &heap[x][y].lock );
+
+#if GIET_DEBUG_MALLOC
+giet_shr_printf("\n[MALLOC DEBUG] Completing Heap[%d][%d] initialisation\n", x, y );
+display_free_array(x,y);
 #endif
                 
@@ -254,5 +263,5 @@
     unsigned int requested_index = GET_SIZE_INDEX( size );
 
-    // take the lock protecting access to heap(x,y)
+    // take the lock protecting access to heap[x][y]
     lock_acquire( &heap[x][y].lock );
 
@@ -273,4 +282,10 @@
     lock_release( &heap[x][y].lock );
  
+#if GIET_DEBUG_MALLOC
+giet_shr_printf("\n[MALLOC DEBUG] Malloc for Heap[%d][%d] / size = %x / base = %x\n", 
+                 x, y, size, base );
+display_free_array(x,y);
+#endif
+
     return (void*)base;
 
@@ -289,29 +304,113 @@
 } 
 
+///////////////////////////////////////////
+void update_free_array( giet_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_base and merged_base
+    unsigned int companion_base;   // companion block base address
+    unsigned int merged_base;      // merged block base address
+    if ( base % (size<<1) )
+    {
+        companion_base  = base + size;
+        merged_base     = base;
+    }
+    else
+    {
+        companion_base  = base - size;
+        merged_base     = base - size;
+    }
+
+    // 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 != 0 ) 
+    {
+        if ( iter == companion_base ) 
+        {
+            found = 1;
+            break;
+        }
+        iter = *(unsigned int*)iter;
+        prev = iter;
+    }
+
+    if ( found == 0 )  // Companion not found 
+    {
+        // push the block in free[size_index]  
+        *(unsigned int*)base   = heap->free[size_index];
+        heap->free[size_index] = base;
+    }
+    else               // Companion found : merge
+    {
+        // evict the searched block 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 );
+    }
+}
 
 //////////////////////
 void free( void* ptr )
 {
-    unsigned int vaddr = (unsigned int)ptr;
-
-    // get the cluster coordinate
+    // get the cluster coordinate from ptr value
     unsigned int x;
     unsigned int y;
     giet_get_xy( ptr, &x, &y );
 
-    // compute index in alloc[] array
-    unsigned index = ( (unsigned int)ptr - heap[x][y].heap_base ) / MAX_BLOCK_SIZE;
+    // get the lock protecting heap[x][y]
+    lock_acquire( &heap[x][y].lock );
+
+    // check ptr value
+    unsigned int base = (unsigned int)ptr;
+    if ( (base < heap[x][y].heap_base) || 
+         (base >= (heap[x][y].heap_base + heap[x][y].heap_size)) )
+    {
+        giet_exit("ERROR in free() : illegal pointer for released block");
+    }
  
-    // get the freed block size_index
-    unsigned char* p i        = (unsigned char*)(heap[x][y].alloc_base + index);
-    unsigned int   size_index = (unsigned int)*p;
-
-    // update the free[] array and NEXT pointer
-    *(unsigned int*)ptr         = heap[x][y].free[size_index];
-    heap[x][y].free[size_index] = (unsigned int)ptr;
-    
-    // TODO try to concatenate with another free block of same size
-    // this require probably to replace this simple push by a recursive function
-}
+    // compute released block index in alloc[] array
+    unsigned index = (base - heap[x][y].heap_base ) / MIN_BLOCK_SIZE;
+ 
+    // get the released block size_index
+    unsigned char* pchar      = (unsigned char*)(heap[x][y].alloc_base + index);
+    unsigned int   size_index = (unsigned int)*pchar;
+
+    // check released block alignment
+    if ( base % (1 << size_index) )
+    {
+        giet_exit("ERROR in free() : released block not aligned");
+    }
+
+    // call the recursive function update_free_array() 
+    update_free_array( &heap[x][y], base, size_index ); 
+
+    // release the lock
+    lock_release( &heap[x][y].lock );
+
+#if GIET_DEBUG_MALLOC
+giet_shr_printf("\n[MALLOC DEBUG] Free for Heap[%d][%d] / base = %x / size = %x\n",
+                 x, y, base, 1<<size_index );
+display_free_array(x,y);
+#endif
+
+} // end free()