score: Fix per-CPU data allocation

Allocate the per-CPU data for secondary processors directly from the
heap areas before heap initialization and not via
_Workspace_Allocate_aligned().  This avoids dependency on the workspace
allocator.  It fixes also a problem on some platforms (e.g. QorIQ) where
at this early point in the system initialization the top of the RAM is
used by low-level startup code on secondary processors (boot pages).

Update #3507.

10 files changed, 181 insertions, 30 deletions

diff --git a/cpukit/include/rtems/score/interr.h b/cpukit/include/rtems/score/interr.h
index df1a1298d9..54e6ec759d 100644
--- a/cpukit/include/rtems/score/interr.h
+++ b/cpukit/include/rtems/score/interr.h
@@ -193,7 +193,8 @@ typedef enum {
   INTERNAL_ERROR_LIBIO_STDOUT_FD_OPEN_FAILED = 36,
   INTERNAL_ERROR_LIBIO_STDERR_FD_OPEN_FAILED = 37,
   INTERNAL_ERROR_ILLEGAL_USE_OF_FLOATING_POINT_UNIT = 38,
-  INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL = 39
+  INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL = 39,
+  INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA = 40
 } Internal_errors_Core_list;
 
 typedef CPU_Uint32ptr Internal_errors_t;
diff --git a/cpukit/sapi/src/interrtext.c b/cpukit/sapi/src/interrtext.c
index 79ba7948ef..bdd567c0b2 100644
--- a/cpukit/sapi/src/interrtext.c
+++ b/cpukit/sapi/src/interrtext.c
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2012, 2017 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2012, 2018 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -66,7 +66,8 @@ static const char *const internal_error_text[] = {
   "INTERNAL_ERROR_LIBIO_STDOUT_FD_OPEN_FAILED",
   "INTERNAL_ERROR_LIBIO_STDERR_FD_OPEN_FAILED",
   "INTERNAL_ERROR_ILLEGAL_USE_OF_FLOATING_POINT_UNIT",
-  "INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL"
+  "INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL",
+  "INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA"
 };
 
 const char *rtems_internal_error_text( rtems_fatal_code error )
diff --git a/cpukit/score/src/wkspace.c b/cpukit/score/src/wkspace.c
index 95c4127056..9f91ac5a5d 100644
--- a/cpukit/score/src/wkspace.c
+++ b/cpukit/score/src/wkspace.c
@@ -77,37 +77,43 @@ static uintptr_t _Workspace_Space_for_TLS( uintptr_t page_size )
   return space;
 }
 
-static uintptr_t _Workspace_Space_for_per_CPU_data( uintptr_t page_size )
+#ifdef RTEMS_SMP
+static void *_Workspace_Allocate_from_areas(
+  Heap_Area *areas,
+  size_t     area_count,
+  uintptr_t  size,
+  uintptr_t  alignment
+)
 {
-  uintptr_t space;
+  size_t i;
 
-#ifdef RTEMS_SMP
-  uintptr_t size;
+  for ( i = 0; i < area_count; ++i ) {
+    Heap_Area *area;
+    uintptr_t  alloc_begin;
+    uintptr_t  alloc_size;
 
-  size = RTEMS_LINKER_SET_SIZE( _Per_CPU_Data );
-  _Assert( size % CPU_CACHE_LINE_BYTES == 0 );
+    area = &areas[ i ];
+    alloc_begin = (uintptr_t) area->begin;
+    alloc_begin = ( alloc_begin + alignment - 1 ) & ~( alignment - 1 );
+    alloc_size = size;
+    alloc_size += alloc_begin - (uintptr_t) area->begin;
 
-  if ( size > 0 ) {
-    /*
-     * Memory allocated with an alignment constraint is allocated from the end of
-     * a free block.  The last allocation may need one free block of minimum
-     * size.
-     */
-    space = _Heap_Min_block_size( page_size );
+    if ( area->size >= alloc_size ) {
+      area->begin = (void *) ( alloc_begin + size );
+      area->size -= alloc_size;
 
-    space += ( rtems_configuration_get_maximum_processors() - 1 )
-      * _Heap_Size_with_overhead( page_size, size, CPU_CACHE_LINE_BYTES );
-  } else {
-    space = 0;
+      return (void *) alloc_begin;
+    }
   }
-#else
-  space = 0;
-#endif
 
-  return space;
+  return NULL;
 }
+#endif
 
-static void _Workspace_Allocate_per_CPU_data( void )
+static void _Workspace_Allocate_per_CPU_data(
+  Heap_Area *areas,
+  size_t area_count
+)
 {
 #ifdef RTEMS_SMP
   uintptr_t size;
@@ -126,11 +132,23 @@ static void _Workspace_Allocate_per_CPU_data( void )
 
     for ( cpu_index = 1 ; cpu_index < cpu_max ; ++cpu_index ) {
       cpu = _Per_CPU_Get_by_index( cpu_index );
-      cpu->data = _Workspace_Allocate_aligned( size, CPU_CACHE_LINE_BYTES );
-      _Assert( cpu->data != NULL );
+      cpu->data = _Workspace_Allocate_from_areas(
+        areas,
+        area_count,
+        size,
+        CPU_CACHE_LINE_BYTES
+      );
+
+      if( cpu->data == NULL ) {
+        _Internal_error( INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA );
+      }
+
       memcpy( cpu->data, RTEMS_LINKER_SET_BEGIN( _Per_CPU_Data ), size);
     }
   }
+#else
+  (void) areas;
+  (void) area_count;
 #endif
 }
 
@@ -148,11 +166,12 @@ void _Workspace_Handler_initialization(
   uintptr_t                             overhead;
   size_t                                i;
 
+  _Workspace_Allocate_per_CPU_data( areas, area_count );
+
   page_size = CPU_HEAP_ALIGNMENT;
 
   remaining = rtems_configuration_get_work_space_size();
   remaining += _Workspace_Space_for_TLS( page_size );
-  remaining += _Workspace_Space_for_per_CPU_data( page_size );
 
   init_or_extend = _Heap_Initialize;
   do_zero = rtems_configuration_get_do_zero_of_workspace();
@@ -208,7 +227,6 @@ void _Workspace_Handler_initialization(
   }
 
   _Heap_Protection_set_delayed_free_fraction( &_Workspace_Area, 1 );
-  _Workspace_Allocate_per_CPU_data();
 }
 
 void *_Workspace_Allocate(
diff --git a/testsuites/smptests/Makefile.am b/testsuites/smptests/Makefile.am
index 2e8e5b1782..b5c2803d02 100644
--- a/testsuites/smptests/Makefile.am
+++ b/testsuites/smptests/Makefile.am
@@ -238,6 +238,17 @@ endif
 endif
 
 if HAS_SMP
+if TEST_smpfatal09
+smp_tests += smpfatal09
+smp_screens += smpfatal09/smpfatal09.scn
+smp_docs += smpfatal09/smpfatal09.doc
+smpfatal09_SOURCES = smpfatal09/init.c
+smpfatal09_CPPFLAGS = $(AM_CPPFLAGS) $(TEST_FLAGS_smpfatal09) \
+	$(support_includes)
+endif
+endif
+
+if HAS_SMP
 if TEST_smpipi01
 smp_tests += smpipi01
 smp_screens += smpipi01/smpipi01.scn
diff --git a/testsuites/smptests/configure.ac b/testsuites/smptests/configure.ac
index 9c5b5e0031..b3fca201b2 100644
--- a/testsuites/smptests/configure.ac
+++ b/testsuites/smptests/configure.ac
@@ -53,6 +53,7 @@ RTEMS_TEST_CHECK([smpfatal04])
 RTEMS_TEST_CHECK([smpfatal05])
 RTEMS_TEST_CHECK([smpfatal06])
 RTEMS_TEST_CHECK([smpfatal08])
+RTEMS_TEST_CHECK([smpfatal09])
 RTEMS_TEST_CHECK([smpipi01])
 RTEMS_TEST_CHECK([smpload01])
 RTEMS_TEST_CHECK([smplock01])
diff --git a/testsuites/smptests/smpfatal09/init.c b/testsuites/smptests/smpfatal09/init.c
new file mode 100644
index 0000000000..ecbb54d586
--- /dev/null
+++ b/testsuites/smptests/smpfatal09/init.c
@@ -0,0 +1,87 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (C) 2018 embedded brains GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <rtems.h>
+#include <rtems/score/percpudata.h>
+#include <rtems/score/wkspace.h>
+
+#include <tmacros.h>
+
+static PER_CPU_DATA_ITEM(int, i) = 123;
+
+const char rtems_test_name[] = "SMPFATAL 9";
+
+static void Init( rtems_task_argument arg )
+{
+  Heap_Area area = { .begin = NULL, .size = 0 };
+  int i;
+
+  TEST_BEGIN();
+  i = *PER_CPU_DATA_GET( _Per_CPU_Get_snapshot(), int, i );
+  RTEMS_OBFUSCATE_VARIABLE( i );
+  rtems_test_assert( i == 123 );
+
+  _Workspace_Handler_initialization( &area, 1, NULL );
+  rtems_test_assert( 0 );
+}
+
+static void fatal_extension(
+  rtems_fatal_source source,
+  bool always_set_to_false,
+  rtems_fatal_code code
+)
+{
+  if (
+    source == INTERNAL_ERROR_CORE
+      && !always_set_to_false
+      && code == INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA
+  ) {
+    TEST_END();
+  }
+}
+
+#define CONFIGURE_APPLICATION_DOES_NOT_NEED_CLOCK_DRIVER
+
+#define CONFIGURE_APPLICATION_NEEDS_SIMPLE_CONSOLE_DRIVER
+
+#define CONFIGURE_INITIAL_EXTENSIONS \
+  { .fatal = fatal_extension }, \
+  RTEMS_TEST_INITIAL_EXTENSION
+
+#define CONFIGURE_MAXIMUM_PROCESSORS 2
+
+#define CONFIGURE_MAXIMUM_TASKS 1
+
+#define CONFIGURE_RTEMS_INIT_TASKS_TABLE
+
+#define CONFIGURE_INIT
+
+#include <rtems/confdefs.h>
diff --git a/testsuites/smptests/smpfatal09/smpfatal09.doc b/testsuites/smptests/smpfatal09/smpfatal09.doc
new file mode 100644
index 0000000000..c154a873b0
--- /dev/null
+++ b/testsuites/smptests/smpfatal09/smpfatal09.doc
@@ -0,0 +1,11 @@
+This file describes the directives and concepts tested by this test set.
+
+test set name: smpfatal09
+
+directives:
+
+  - _Workspace_Handler_initialization()
+
+concepts:
+
+  - Provoke the INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA fatal error.
diff --git a/testsuites/smptests/smpfatal09/smpfatal09.scn b/testsuites/smptests/smpfatal09/smpfatal09.scn
new file mode 100644
index 0000000000..9ef7cb38f0
--- /dev/null
+++ b/testsuites/smptests/smpfatal09/smpfatal09.scn
@@ -0,0 +1,7 @@
+*** BEGIN OF TEST SMPFATAL 9 ***
+*** TEST VERSION: 5.0.0.cfa82b34b0c53ab4e3d84dd8ab5225793d48fcd0
+*** TEST STATE: EXPECTED-PASS
+*** TEST BUILD: RTEMS_SMP
+*** TEST TOOLS: 7.4.0 20181206 (RTEMS 5, RSB ddba5372522da341fa20b2c75dfe966231cb6790, Newlib df6915f029ac9acd2b479ea898388cbd7dda4974)
+
+*** END OF TEST SMPFATAL 9 ***
diff --git a/testsuites/sptests/spinternalerror02/init.c b/testsuites/sptests/spinternalerror02/init.c
index 7b65369708..43ef3f3baa 100644
--- a/testsuites/sptests/spinternalerror02/init.c
+++ b/testsuites/sptests/spinternalerror02/init.c
@@ -36,7 +36,7 @@ static void test_internal_error_text(void)
   } while ( text != text_last );
 
   rtems_test_assert(
-    error - 3 == INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL
+    error - 3 == INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA
   );
 }
 
diff --git a/testsuites/sptests/spinternalerror02/spinternalerror02.scn b/testsuites/sptests/spinternalerror02/spinternalerror02.scn
index 08dae2e25e..b081f4787a 100644
--- a/testsuites/sptests/spinternalerror02/spinternalerror02.scn
+++ b/testsuites/sptests/spinternalerror02/spinternalerror02.scn
@@ -1,4 +1,8 @@
 *** BEGIN OF TEST SPINTERNALERROR 2 ***
+*** TEST VERSION: 5.0.0.cfa82b34b0c53ab4e3d84dd8ab5225793d48fcd0
+*** TEST STATE: EXPECTED-PASS
+*** TEST BUILD:
+*** TEST TOOLS: 7.4.0 20181206 (RTEMS 5, RSB ddba5372522da341fa20b2c75dfe966231cb6790, Newlib df6915f029ac9acd2b479ea898388cbd7dda4974)
 INTERNAL_ERROR_NO_CONFIGURATION_TABLE
 INTERNAL_ERROR_NO_CPU_TABLE
 INTERNAL_ERROR_TOO_LITTLE_WORKSPACE
@@ -31,6 +35,15 @@ INTERNAL_ERROR_THREAD_QUEUE_DEADLOCK
 INTERNAL_ERROR_THREAD_QUEUE_ENQUEUE_STICKY_FROM_BAD_STATE
 INTERNAL_ERROR_BAD_THREAD_DISPATCH_DISABLE_LEVEL
 INTERNAL_ERROR_BAD_THREAD_DISPATCH_ENVIRONMENT
+INTERNAL_ERROR_RTEMS_INIT_TASK_CREATE_FAILED
+INTERNAL_ERROR_POSIX_INIT_THREAD_CREATE_FAILED
+INTERNAL_ERROR_LIBIO_USER_ENV_KEY_CREATE_FAILED
+INTERNAL_ERROR_LIBIO_SEM_CREATE_FAILED
+INTERNAL_ERROR_LIBIO_STDOUT_FD_OPEN_FAILED
+INTERNAL_ERROR_LIBIO_STDERR_FD_OPEN_FAILED
+INTERNAL_ERROR_ILLEGAL_USE_OF_FLOATING_POINT_UNIT
+INTERNAL_ERROR_ARC4RANDOM_GETENTROPY_FAIL
+INTERNAL_ERROR_NO_MEMORY_FOR_PER_CPU_DATA
 ?
 ?
 INTERNAL_ERROR_CORE
@@ -79,4 +92,5 @@ RTEMS_IO_ERROR
 RTEMS_PROXY_BLOCKING
 ?
 ?
+
 *** END OF TEST SPINTERNALERROR 2 ***