score: Append to free list in _Heap_Extend()

3 files changed, 65 insertions, 1 deletions

diff --git a/cpukit/score/inline/rtems/score/heap.inl b/cpukit/score/inline/rtems/score/heap.inl
index 6a89555ede..b9553714c4 100644
--- a/cpukit/score/inline/rtems/score/heap.inl
+++ b/cpukit/score/inline/rtems/score/heap.inl
@@ -87,6 +87,19 @@ RTEMS_INLINE_ROUTINE void _Heap_Free_list_insert_after(
   next->prev = new_block;
 }
 
+RTEMS_INLINE_ROUTINE void _Heap_Free_list_insert_before(
+  Heap_Block *block_next,
+  Heap_Block *new_block
+)
+{
+  Heap_Block *prev = block_next->prev;
+
+  new_block->next = block_next;
+  new_block->prev = prev;
+  prev->next = new_block;
+  block_next->prev = new_block;
+}
+
 RTEMS_INLINE_ROUTINE bool _Heap_Is_aligned(
   uintptr_t value,
   uintptr_t alignment
diff --git a/cpukit/score/src/heapextend.c b/cpukit/score/src/heapextend.c
index f93d71f906..83e0928dba 100644
--- a/cpukit/score/src/heapextend.c
+++ b/cpukit/score/src/heapextend.c
@@ -28,12 +28,21 @@
 static void _Heap_Free_block( Heap_Control *heap, Heap_Block *block )
 {
   Heap_Statistics *const stats = &heap->stats;
+  Heap_Block *first_free;
 
   /* Statistics */
   ++stats->used_blocks;
   --stats->frees;
 
-  _Heap_Free( heap, (void *) _Heap_Alloc_area_of_block( block ));
+  /*
+   * The _Heap_Free() will place the block to the head of free list.  We want
+   * the new block at the end of the free list.  So that initial and earlier
+   * areas are consumed first.
+   */
+  _Heap_Free( heap, (void *) _Heap_Alloc_area_of_block( block ) );
+  first_free = _Heap_Free_list_first( heap );
+  _Heap_Free_list_remove( first_free );
+  _Heap_Free_list_insert_before( _Heap_Free_list_tail( heap ), first_free );
 }
 
 static void _Heap_Merge_below(
diff --git a/testsuites/libtests/malloctest/init.c b/testsuites/libtests/malloctest/init.c
index c2080e8469..f7839246c3 100644
--- a/testsuites/libtests/malloctest/init.c
+++ b/testsuites/libtests/malloctest/init.c
@@ -984,6 +984,46 @@ static void test_heap_extend(void)
   test_heap_assert( ret, true );
 }
 
+static void test_heap_extend_allocation_order(void)
+{
+  Heap_Control *heap = &TestHeap;
+  uintptr_t size = 256;
+  uintptr_t gap = 256;
+  uint8_t *init_area_begin = TestHeapMemory;
+  uint8_t *extend_area_begin = init_area_begin + size + gap;
+  bool ret;
+  uint8_t *p;
+
+  _Heap_Initialize( heap, init_area_begin, size, 0 );
+
+  ret = _Protected_heap_Extend( heap, extend_area_begin, size );
+  test_heap_assert( ret, true );
+
+  p = _Heap_Allocate( heap, 1 );
+  rtems_test_assert( (uintptr_t) (p - init_area_begin) < size );
+}
+
+static void test_heap_extend_allocation_order_with_empty_heap(void)
+{
+  Heap_Control *heap = &TestHeap;
+  uintptr_t size = 256;
+  uintptr_t gap = 256;
+  uint8_t *init_area_begin = TestHeapMemory;
+  uint8_t *extend_area_begin = init_area_begin + size + gap;
+  bool ret;
+  uint8_t *p;
+
+  _Heap_Initialize( heap, init_area_begin, size, 0 );
+
+  _Heap_Greedy_allocate( heap, NULL, 0 );
+
+  ret = _Protected_heap_Extend( heap, extend_area_begin, size );
+  test_heap_assert( ret, true );
+
+  p = _Heap_Allocate( heap, 1 );
+  rtems_test_assert( (uintptr_t) (p - extend_area_begin) < size );
+}
+
 static void test_heap_no_extend(void)
 {
   uintptr_t extended_space = _Heap_No_extend( NULL, 0, 0, 0 );
@@ -1148,6 +1188,8 @@ rtems_task Init(
   test_realloc();
   test_heap_cases_1();
   test_heap_extend();
+  test_heap_extend_allocation_order();
+  test_heap_extend_allocation_order_with_empty_heap();
   test_heap_no_extend();
   test_heap_info();
   test_protected_heap_info();