Modifications from Tony Bennett accepted to return aligned block.

Thread_Close fixed to avoid returning FP context area when one was not
allocated at initialization time.

2 files changed, 40 insertions, 12 deletions

diff --git a/cpukit/score/src/heap.c b/cpukit/score/src/heap.c
index b16cfde9de..0820d2ead4 100644
--- a/cpukit/score/src/heap.c
+++ b/cpukit/score/src/heap.c
@@ -14,8 +14,8 @@
 
 
 #include <rtems/system.h>
-#include <rtems/core/heap.h>
 #include <rtems/sysstate.h>
+#include <rtems/core/heap.h>
 
 /*PAGE
  *
@@ -30,7 +30,7 @@
  *    page_size        - allocatable unit of memory
  *
  *  Output parameters:
- *    returns - maximum memory available if successfully initialized
+ *    returns - maximum memory available if RTEMS_SUCCESSFUL
  *    0       - otherwise
  *
  *    This is what a heap looks like in memory immediately
@@ -116,6 +116,7 @@ Heap_Extend_status _Heap_Extend(
 )
 {
   Heap_Block        *the_block;
+  unsigned32        *p;
   
   /*
    *  The overhead was taken from the original heap memory.
@@ -173,10 +174,14 @@ Heap_Extend_status _Heap_Extend(
 
   /*
    *  Must pass in address of "user" area
+   *  So add in the offset field.
    */
 
-  _Heap_Free( the_heap, &old_final->next );
-
+  p = (unsigned32 *) &old_final->next;
+  *p = sizeof(unsigned32);
+  p++;
+  _Heap_Free( the_heap, p );
+  
   return HEAP_EXTEND_SUCCESSFUL;
 }
 
@@ -205,10 +210,12 @@ void *_Heap_Allocate(
   Heap_Block *the_block;
   Heap_Block *next_block;
   Heap_Block *temporary_block;
-
+  void       *ptr;
+  unsigned32  offset;
+  
   excess   = size % the_heap->page_size;
-  the_size = size + HEAP_BLOCK_USED_OVERHEAD;
-
+  the_size = size + the_heap->page_size + HEAP_BLOCK_USED_OVERHEAD;
+  
   if ( excess )
     the_size += the_heap->page_size - excess;
 
@@ -234,7 +241,7 @@ void *_Heap_Allocate(
     temporary_block->back_flag =
     next_block->front_flag     = _Heap_Build_flag( the_size,
                                     HEAP_BLOCK_USED );
-    return( _Heap_Start_of_user_area( next_block ) );
+    ptr = _Heap_Start_of_user_area( next_block );
   } else {
     next_block                = _Heap_Next_block( the_block );
     next_block->back_flag     = _Heap_Build_flag( the_block->front_flag,
@@ -242,8 +249,28 @@ void *_Heap_Allocate(
     the_block->front_flag     = next_block->back_flag;
     the_block->next->previous = the_block->previous;
     the_block->previous->next = the_block->next;
-    return( _Heap_Start_of_user_area( the_block ) );
+    ptr = _Heap_Start_of_user_area( the_block );
   }
+  
+  /*
+   * round ptr up to a multiple of page size
+   * Have to save the bump amount in the buffer so that free can figure it out
+   */
+  
+  offset = the_heap->page_size - (((unsigned32) ptr) & (the_heap->page_size - 1));
+  ptr += offset;
+  *(((unsigned32 *) ptr) - 1) = offset;
+
+#ifdef RTEMS_DEBUG
+  {
+      unsigned32 ptr_u32;
+      ptr_u32 = (unsigned32) ptr;
+      if (ptr_u32 & (the_heap->page_size - 1))
+          abort();
+  }
+#endif
+
+  return ptr;
 }
 
 /*PAGE
@@ -274,8 +301,8 @@ boolean _Heap_Size_of_user_area(
   Heap_Block        *next_block;
   unsigned32         the_size;
 
-  the_block = _Heap_Block_at( starting_address, - (sizeof( void * ) * 2) );
-
+  the_block = _Heap_User_Block_at( starting_address );
+  
   if ( !_Heap_Is_block_in( the_heap, the_block ) ||
         _Heap_Is_block_free( the_block ) )
     return( FALSE );
@@ -319,7 +346,7 @@ boolean _Heap_Free(
   Heap_Block        *temporary_block;
   unsigned32         the_size;
 
-  the_block = _Heap_Block_at( starting_address, - (sizeof( void * ) * 2) );
+  the_block = _Heap_User_Block_at( starting_address );
 
   if ( !_Heap_Is_block_in( the_heap, the_block ) ||
         _Heap_Is_block_free( the_block ) ) {
diff --git a/cpukit/score/src/thread.c b/cpukit/score/src/thread.c
index 3f58c8ef12..a380d76c44 100644
--- a/cpukit/score/src/thread.c
+++ b/cpukit/score/src/thread.c
@@ -464,6 +464,7 @@ void _Thread_Close(
 #endif
   the_thread->fp_context = NULL;
 
+  if ( the_thread->Start.fp_context )
   (void) _Workspace_Free( the_thread->Start.fp_context );
 
   if ( the_thread->Start.stack )