TLSF allocator from rtportal, not working properly

SConstruct

@@ -1,7 +1,7 @@
 import os, sys 
 target = ARGUMENTS.get( 'target', 'lua' ).lower() 
 cputype = ARGUMENTS.get( 'cpu', 'at91sam7x256' ).lower()
-allocator = ARGUMENTS.get( 'allocator', 'newlib' ).lower()
+allocator = ARGUMENTS.get( 'allocator', '' ).lower()
 
 # List of platform/CPU combinations
 cpu_list = { 'at91sam7x' : [ 'at91sam7x256', 'at91sam7x512' ], 
@@ -10,6 +10,13 @@ cpu_list = { 'at91sam7x' : [ 'at91sam7x256', 'at91sam7x512' ],
               'i386' : [ 'i386' ],
               'lpc288x' : [ 'lpc2888' ]
             }
+
+# CPU -> allocator mapping (if an allocator is not specified)
+if allocator == '':
+  if cputype in [ 'lpc2888' ]:
+    allocator = 'tlsf'
+  else:
+    allocator = 'newlib'
             
 platform = None        
 # Look for the given CPU in the list of platforms            

inc/tlsf.h

@@ -39,5 +39,6 @@ extern void *tlsf_calloc(size_t nelem, size_t elem_size);
 // BogdanM - added for eLua
 void tlsf_elua_init();
 size_t tlsf_elua_get_block_size( void* ptr );
+void* tlsf_elua_align_addr( void* ptr );
 
 #endif

src/newlib/stubs.c

@@ -304,10 +304,11 @@ void* _malloc_r( struct _reent* r, size_t size )
   {
     if( ( temp = platform_get_first_free_ram( i ) ) == NULL )
       break;
+    temp = tlsf_elua_align_addr( temp );
     if( ( temp = malloc_ex( size, temp ) ) != NULL )
       break;
     i ++;
-  }  
+  }
   return temp;
 }
 
@@ -321,6 +322,7 @@ void* _calloc_r( struct _reent* r, size_t nelem, size_t elem_size )
   {
     if( ( temp = platform_get_first_free_ram( i ) ) == NULL )
       break;
+    temp = tlsf_elua_align_addr( temp );
     if( ( temp = calloc_ex( nelem, elem_size, temp ) ) != NULL )
       break;
     i ++;
@@ -338,6 +340,7 @@ void _free_r( struct _reent* r, void* ptr )
   {
     if( ( lstart = ( u32 )platform_get_first_free_ram( i ) ) == 0 )
       break;
+    lstart = ( u32 )tlsf_elua_align_addr( ( void* )lstart );
     lend = ( u32 )platform_get_last_free_ram( i );
     if( ( lstart <= ( u32 )ptr ) && ( ( u32 )ptr <= lend ) )
     {
@@ -351,7 +354,7 @@ void _free_r( struct _reent* r, void* ptr )
 // realloc: this is a bit more complex. First we identify the correct memory
 // pool and try to realloc there. If this doesn't work, we try to realloc in 
 // another pool before giving up.
-void* _relloc_r( struct _reent* r, void* ptr, size_t size )
+void* _realloc_r( struct _reent* r, void* ptr, size_t size )
 {
   void* temp;
   u32 lstart, lend;
@@ -374,6 +377,7 @@ void* _relloc_r( struct _reent* r, void* ptr, size_t size )
   {
     if( ( lstart = ( u32 )platform_get_first_free_ram( i ) ) == 0 )
       return NULL;
+    lstart = ( u32 )tlsf_elua_align_addr( ( void* )lstart );
     lend = ( u32 )platform_get_last_free_ram( i );
     if( ( lstart <= ( u32 )ptr ) && ( ( u32 )ptr <= lend ) )
       break;
@@ -391,6 +395,7 @@ void* _relloc_r( struct _reent* r, void* ptr, size_t size )
   {
     if( ( temp = platform_get_first_free_ram( i ) ) == NULL )
       break;
+    temp = tlsf_elua_align_addr( temp );
     if( ( u32 )temp != lstart )
     {
       if( ( temp = malloc_ex( size, temp ) ) != NULL )

src/shell.c

@@ -170,14 +170,16 @@ static void shell_mem( char* args )
     lend = ( u32 )platform_get_last_free_ram( i );
     printf( "Start:0x%08lX  Size:%8ld  ", lstart, lend - lstart + 1 );
 #ifdef USE_TLSF
+    lstart = ( u32 )tlsf_elua_align_addr( ( void* )lstart );
     u32 temp = get_used_size( ( void* )lstart );
     printf( "Used:%8ld  Free:%8ld\n", temp, lend - lstart + 1 - temp );
+    i ++;
 #else
     struct mallinfo allocdata;
     allocdata = mallinfo();  
     printf( "Used:%8ld Free:%8ld\n", ( long )allocdata.uordblks, ( long )allocdata.fordblks );      
+    break;
 #endif    
-    i ++;
   }
 }
 

src/tlsf.c

@@ -1020,15 +1020,21 @@ void tlsf_elua_init()
   {
     if( ( pstart = ( char* )platform_get_first_free_ram( i ) ) == NULL )
       break;
+    pstart = ( char* )tlsf_elua_align_addr( pstart );
     pend = ( char* )platform_get_last_free_ram( i );
-    // Align start address
-    if( ( u32 )pstart & PTR_MASK ) 
-      pstart = ( char* )( ( ( u32 )pstart + sizeof( void* ) ) & ~PTR_MASK );
     init_memory_pool( ( size_t )( pend - pstart + 1 ), pstart );
     i ++;
   }
 }
 
+// Get the "real" start address (aligned if needed)
+void* tlsf_elua_align_addr( void* ptr )
+{
+  if( ( u32 )ptr & PTR_MASK )
+    ptr = ( void* )( ( ( u32 )ptr + sizeof( void* ) ) & ~PTR_MASK );
+  return ptr;
+}
+
 #else // #ifdef USE_TLSF
 
 void tlsf_elua_init()