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.

c/src/exec/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,9 +174,13 @@ 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,9 +210,11 @@ 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,7 +301,7 @@ 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 ) )
@@ -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 ) ) {

c/src/exec/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 )

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,9 +174,13 @@ 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,9 +210,11 @@ 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,7 +301,7 @@ 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 ) )
@@ -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 ) ) {

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 )