AMiRo-OS

diff --git a/os/rt/include/chschd.h b/os/rt/include/chschd.h

index 1d11d17..c71d8b4 100644

--- a/os/rt/include/chschd.h

+++ b/os/rt/include/chschd.h

@@ -177,6 +177,23 @@ struct ch_thread {

    */

   trefs_t               refs;

 #endif

+#if (CH_CFG_USE_THREADHIERARCHY == TRUE) || defined(__DOXYGEN__)

+  /**

+   * @brief   Pointer to the parent thread.

+   * @note    NULL only for the main thread.

+   */

+  thread_t              *parent;

+  /**

+   * @brief   Pointer to the first child thread.

+   * @note    NULL if there are no child threads.

+   */

+  thread_t              *children;

+  /**

+   * @brief   Pointer to the next sibling thread.

+   * @brief   NULL if there are no more child threads.

+   */

+  thread_t              *sibling;

+#endif

   /**

    * @brief   Number of ticks remaining to this thread.

    */

diff --git a/os/rt/include/chthreads.h b/os/rt/include/chthreads.h

index 166d566..b767d74 100644

--- a/os/rt/include/chthreads.h

+++ b/os/rt/include/chthreads.h

@@ -79,6 +79,10 @@ typedef struct {

    * @brief   Thread argument.

    */

   void              *arg;

+  /**

+   * @brief   Pointer to the parent thread.

+   */

+  thread_t          *parent;

 } thread_descriptor_t;

 /*===========================================================================*/

@@ -220,7 +224,7 @@ typedef struct {

 #ifdef __cplusplus

 extern "C" {

 #endif

-   thread_t *_thread_init(thread_t *tp, const char *name, tprio_t prio);

+   thread_t *_thread_init(thread_t *tp, thread_t* parent, const char *name, tprio_t prio);

 #if CH_DBG_FILL_THREADS == TRUE

   void _thread_memfill(uint8_t *startp, uint8_t *endp, uint8_t v);

 #endif

@@ -228,8 +232,13 @@ extern "C" {

   thread_t *chThdCreateSuspended(const thread_descriptor_t *tdp);

   thread_t *chThdCreateI(const thread_descriptor_t *tdp);

   thread_t *chThdCreate(const thread_descriptor_t *tdp);

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+  thread_t *chThdCreateStatic(void *wsp, size_t size,

+                              tprio_t prio, tfunc_t pf, void *arg, thread_t *parent);

+#else

   thread_t *chThdCreateStatic(void *wsp, size_t size,

                               tprio_t prio, tfunc_t pf, void *arg);

+#endif

   thread_t *chThdStart(thread_t *tp);

 #if CH_CFG_USE_REGISTRY == TRUE

   thread_t *chThdAddRef(thread_t *tp);

diff --git a/os/rt/src/chsys.c b/os/rt/src/chsys.c

index 346a8ba..c789d01 100644

--- a/os/rt/src/chsys.c

+++ b/os/rt/src/chsys.c

@@ -126,13 +126,13 @@ void chSysInit(void) {

 #if CH_CFG_NO_IDLE_THREAD == FALSE

   /* Now this instructions flow becomes the main thread.*/

 #if CH_CFG_USE_REGISTRY == TRUE

-  currp = _thread_init(&ch.mainthread, (const char *)&ch_debug, NORMALPRIO);

+  currp = _thread_init(&ch.mainthread, NULL, (const char *)&ch_debug, NORMALPRIO);

 #else

-  currp = _thread_init(&ch.mainthread, "main", NORMALPRIO);

+  currp = _thread_init(&ch.mainthread, NULL, "main", NORMALPRIO);

 #endif

 #else

   /* Now this instructions flow becomes the idle thread.*/

-  currp = _thread_init(&ch.mainthread, "idle", IDLEPRIO);

+  currp = _thread_init(&ch.mainthread, NULL, "idle", IDLEPRIO);

 #endif

 #if CH_DBG_ENABLE_STACK_CHECK == TRUE

@@ -172,6 +172,7 @@ void chSysInit(void) {

       THD_WORKING_AREA_END(ch_idle_thread_wa),

       IDLEPRIO,

       _idle_thread,

+      NULL,

       NULL

     };

diff --git a/os/rt/src/chthreads.c b/os/rt/src/chthreads.c

index 171c683..6bc0f78 100644

--- a/os/rt/src/chthreads.c

+++ b/os/rt/src/chthreads.c

@@ -70,6 +70,46 @@

 /* Module local functions.                                                   */

 /*===========================================================================*/

+#if CH_CFG_USE_THREADHIERARCHY == TRUE || defined(__DOXYGEN__)

+/**

+ * @brief   Insert a thread to the list of children of another thread.

+ * @details If @p CH_CFG_THREADHIERARCHY_ORDERED is @p TRUE, children are ordered by their priorities (high to low).

+ *          Children with identical priority are ordered by 'age' (youngest first).

+ *

+ * @param[in] parent  Pointer to the parent thread (must not be NULL).

+ * @param[in] child   Pointer to the child thread (must not be NULL).

+ */

+inline void _thread_addChild(thread_t *parent, thread_t *child) {

+#if CH_CFG_THREADHIERARCHY_ORDERED == TRUE

+  thread_t *sibling = parent->children;

+  child->parent = parent;

+#if CH_CFG_USE_MUTEXES == TRUE

+  if (sibling == NULL || sibling->realprio <= child->realprio) {

+#else

+  if (sibling == NULL || sibling->prio <= child->prio) {

+#endif

+    child->sibling = sibling;

+    parent->children = child;

+  } else {

+#if CH_CFG_USE_MUTEXES == TRUE

+    while (sibling->sibling != NULL && sibling->sibling->realprio > child->realprio) {

+#else

+    while (sibling->sibling != NULL && sibling->sibling->prio > child->prio) {

+#endif

+      sibling = sibling->sibling;

+    }

+    child->sibling = sibling->sibling;

+    sibling->sibling = child;

+  }

+#else

+  child->parent = parent;

+  child->sibling = parent->children;

+  parent->children = child;

+#endif

+  return;

+}

+#endif

+

 /*===========================================================================*/

 /* Module exported functions.                                                */

 /*===========================================================================*/

@@ -79,13 +119,14 @@

  * @note    This is an internal functions, do not use it in application code.

  *

  * @param[in] tp        pointer to the thread

+ * @param[in] parent    pointer to the parent thread

  * @param[in] name      thread name

  * @param[in] prio      the priority level for the new thread

  * @return              The same thread pointer passed as parameter.

  *

  * @notapi

  */

-thread_t *_thread_init(thread_t *tp, const char *name, tprio_t prio) {

+thread_t *_thread_init(thread_t *tp, thread_t *parent,  const char *name, tprio_t prio) {

   tp->prio      = prio;

   tp->state     = CH_STATE_WTSTART;

@@ -110,6 +151,17 @@ thread_t *_thread_init(thread_t *tp, const char *name, tprio_t prio) {

 #else

   (void)name;

 #endif

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+  if (parent != NULL) {

+    _thread_addChild(parent, tp);

+  } else {

+    tp->parent = parent;

+    tp->sibling = NULL;

+  }

+  tp->children = NULL;

+#else

+  (void)parent;

+#endif

 #if CH_CFG_USE_WAITEXIT == TRUE

   list_init(&tp->waiting);

 #endif

@@ -190,7 +242,7 @@ thread_t *chThdCreateSuspendedI(const thread_descriptor_t *tdp) {

   PORT_SETUP_CONTEXT(tp, tdp->wbase, tp, tdp->funcp, tdp->arg);

   /* The driver object is initialized but not started.*/

-  return _thread_init(tp, tdp->name, tdp->prio);

+  return _thread_init(tp, tdp->parent, tdp->name, tdp->prio);

 }

 /**

@@ -315,13 +367,21 @@ thread_t *chThdCreate(const thread_descriptor_t *tdp) {

  * @param[in] pf        the thread function

  * @param[in] arg       an argument passed to the thread function. It can be

  *                      @p NULL.

+ * @param[in] parent    pointer to a parent thread. Parameter only available if

+ *                      @p CH_CFG_USE_THREADHIERARCHY is @p TRUE. It can be

+ *                      @p NULL.

  * @return              The pointer to the @p thread_t structure allocated for

  *                      the thread into the working space area.

  *

  * @api

  */

+#if (CH_CFG_USE_THREADHIERARCHY == TRUE) || defined(__DOXYGEN__)

+thread_t *chThdCreateStatic(void *wsp, size_t size,

+                            tprio_t prio, tfunc_t pf, void *arg, thread_t *parent) {

+#else

 thread_t *chThdCreateStatic(void *wsp, size_t size,

                             tprio_t prio, tfunc_t pf, void *arg) {

+#endif

   thread_t *tp;

   chDbgCheck((wsp != NULL) &&

@@ -358,7 +418,11 @@ thread_t *chThdCreateStatic(void *wsp, size_t size,

   /* Setting up the port-dependent part of the working area.*/

   PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg);

-  tp = _thread_init(tp, "noname", prio);

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+  tp = _thread_init(tp, parent, "noname", prio);

+#else

+  tp = _thread_init(tp, NULL, "noname", prio);

+#endif

   /* Starting the thread immediately.*/

   chSchWakeupS(tp, MSG_OK);

@@ -529,6 +593,30 @@ void chThdExitS(msg_t msg) {

 #endif

 #endif

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+  thread_t *child;

+  /* Remove from parent's list of children. */

+  if (tp->parent != NULL) {

+    if (tp->parent->children == tp) {

+      tp->parent->children = tp->sibling;

+    } else {

+      child = tp->parent->children;

+      while (child->sibling != tp) {

+        child = child->sibling;

+      }

+      child->sibling = tp->sibling;

+    }

+    tp->parent = NULL;

+  }

+  tp->sibling = NULL;

+  /* Move any child threads to the main thread. */

+  while (tp->children != NULL) {

+    child = tp->children;

+    tp->children = child->sibling;

+    _thread_addChild(&ch.mainthread, child);

+  }

+#endif

+

   /* Going into final state.*/

   chSchGoSleepS(CH_STATE_FINAL);

@@ -611,6 +699,43 @@ tprio_t chThdSetPriority(tprio_t newprio) {

   oldprio = currp->prio;

   currp->prio = newprio;

 #endif

+#if (CH_CFG_USE_THREADHIERARCHY == TRUE) && (CH_CFG_THREADHIERARCHY_ORDERED == TRUE)

+  /* Reorder sibling list. */

+  if (currp->parent != NULL && newprio != oldprio) {

+    thread_t *sibling, *oldsibling;

+    if (newprio > oldprio) {

+      oldsibling = currp->sibling;

+      _thread_addChild(currp->parent, currp);

+      sibling = currp->sibling;

+      if (sibling != NULL) {

+        while (sibling->sibling != currp) {

+          sibling = sibling->sibling;

+        }

+        sibling->sibling = oldsibling;

+      }

+    } else /*if (newprio < oldprio)*/ {

+      sibling = currp->parent->children;

+      if (sibling == currp) {

+        currp->parent->children = currp->sibling;

+        _thread_addChild(currp->parent, currp);

+      } else {

+        while (sibling->sibling != currp) {

+          sibling = sibling->sibling;

+        }

+        sibling->sibling = currp->sibling;

+#if CH_CFG_USE_MUTEXES == TRUE

+        while (sibling->sibling != NULL && sibling->sibling->realprio > currp->realprio) {

+#else

+        while (sibling->sibling != NULL && sibling->sibling->prio > currp->prio) {

+#endif

+          sibling = sibling->sibling;

+        }

+        currp->sibling = sibling->sibling;

+        sibling->sibling = currp;

+      }

+    }

+  }

+#endif

   chSchRescheduleS();

   chSysUnlock();

diff --git a/test/rt/source/test/rt_test_root.c b/test/rt/source/test/rt_test_root.c

index e03d0f1..150d33d 100644

--- a/test/rt/source/test/rt_test_root.c

+++ b/test/rt/source/test/rt_test_root.c

@@ -106,6 +106,14 @@ void * ROMCONST wa[5] = {test_buffer + (WA_SIZE * 0),

                          test_buffer + (WA_SIZE * 3),

                          test_buffer + (WA_SIZE * 4)};

+thread_t *test_create_thread(void *wsp, size_t size, tprio_t prio, tfunc_t pf, void *arg) {

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+  return chThdCreateStatic(wsp, size, prio, pf, arg, chThdGetSelfX());

+#else

+  return chThdCreateStatic(wsp, size, prio, pf, arg);

+#endif

+}

+

 /*

  * Sets a termination request in all the test-spawned threads.

  */

diff --git a/test/rt/source/test/rt_test_root.h b/test/rt/source/test/rt_test_root.h

index 6da777b..0cd40fc 100644

--- a/test/rt/source/test/rt_test_root.h

+++ b/test/rt/source/test/rt_test_root.h

@@ -88,6 +88,7 @@ extern thread_t *threads[MAX_THREADS];

 extern void * ROMCONST wa[5];

 void test_print_port_info(void);

+thread_t *test_create_thread(void *wsp, size_t size, tprio_t prio, tfunc_t pf, void *arg);

 void test_terminate_threads(void);

 void test_wait_threads(void);

 systime_t test_wait_tick(void);

diff --git a/test/rt/source/test/rt_test_sequence_001.c b/test/rt/source/test/rt_test_sequence_001.c

index 6f2b96f..f91c23f 100644

--- a/test/rt/source/test/rt_test_sequence_001.c

+++ b/test/rt/source/test/rt_test_sequence_001.c

@@ -193,6 +193,12 @@ static void rt_test_001_003_execute(void) {

     test_print("--- CH_CFG_USE_REGISTRY:                ");

     test_printn(CH_CFG_USE_REGISTRY);

     test_println("");

+    test_print("--- CH_CFG_USE_THREADHIERARCHY:         ");

+    test_printn(CH_CFG_USE_THREADHIERARCHY);

+    test_println("");

+    test_print("--- CH_CFG_THREADHIERARCHY_ORDERED:     ");

+    test_printn(CH_CFG_THREADHIERARCHY_ORDERED);

+    test_println("");

     test_print("--- CH_CFG_USE_WAITEXIT:                ");

     test_printn(CH_CFG_USE_WAITEXIT);

     test_println("");

@@ -207,7 +213,7 @@ static void rt_test_001_003_execute(void) {

     test_println("");

     test_print("--- CH_CFG_USE_MUTEXES_RECURS:          ");

     test_printn(CH_CFG_USE_MUTEXES_RECURSIVE);

-    test_println("");   

+    test_println("");

     test_print("--- CH_CFG_USE_CONDVARS:                ");

     test_printn(CH_CFG_USE_CONDVARS);

     test_println("");

diff --git a/test/rt/source/test/rt_test_sequence_003.c b/test/rt/source/test/rt_test_sequence_003.c

index 8598b12..f1ece71 100644

--- a/test/rt/source/test/rt_test_sequence_003.c

+++ b/test/rt/source/test/rt_test_sequence_003.c

@@ -46,6 +46,20 @@ static THD_FUNCTION(thread, p) {

   test_emit_token(*(char *)p);

 }

+#if (CH_CFG_USE_THREADHIERARCHY)

+static THD_FUNCTION(hierarchythread, p) {

+

+  do {

+    if (*(tprio_t *)p != chThdGetPriorityX()) {

+      chThdSetPriority(*(tprio_t *)p);

+    }

+    chThdSleepMilliseconds(10);

+  } while (!chThdShouldTerminateX());

+

+  chThdExit(MSG_OK);

+}

+#endif

+

 /****************************************************************************

  * Test cases.

  ****************************************************************************/

@@ -166,11 +180,11 @@ static void rt_test_003_002_execute(void) {

      sequence is tested.*/

   test_set_step(1);

   {

-    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

-    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

-    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

-    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

-    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

+    threads[0] = test_create_thread(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

+    threads[1] = test_create_thread(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

+    threads[2] = test_create_thread(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

+    threads[3] = test_create_thread(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

+    threads[4] = test_create_thread(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

     test_wait_threads();

     test_assert_sequence("ABCDE", "invalid sequence");

   }

@@ -179,11 +193,11 @@ static void rt_test_003_002_execute(void) {

      sequence is tested.*/

   test_set_step(2);

   {

-    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

-    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

-    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

-    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

-    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

+    threads[4] = test_create_thread(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

+    threads[3] = test_create_thread(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

+    threads[2] = test_create_thread(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

+    threads[1] = test_create_thread(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

+    threads[0] = test_create_thread(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

     test_wait_threads();

     test_assert_sequence("ABCDE", "invalid sequence");

   }

@@ -192,11 +206,11 @@ static void rt_test_003_002_execute(void) {

      sequence is tested.*/

   test_set_step(3);

   {

-    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

-    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

-    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

-    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

-    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

+    threads[1] = test_create_thread(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");

+    threads[0] = test_create_thread(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");

+    threads[4] = test_create_thread(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");

+    threads[3] = test_create_thread(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");

+    threads[2] = test_create_thread(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");

     test_wait_threads();

     test_assert_sequence("ABCDE", "invalid sequence");

   }

@@ -325,6 +339,405 @@ static const testcase_t rt_test_003_004 = {

 };

 #endif /* CH_CFG_USE_MUTEXES */

+#if (CH_CFG_USE_THREADHIERARCHY) || defined(__DOXYGEN__)

+/**

+ * @page rt_test_003_005 [3.5] Thread hierarach with (un)ordered lists.

+ *

+ * <h2>Description</h2>

+ * Todo

+ *

+ * <h2>Conditions</h2>

+ * This test is only executed if the following preprocessor condition

+ * evaluates to true:

+ * - CH_CFG_USE_THREADHIERARCHY

+ * This test comprises additional tests if the following preprocessor

+ * condition evaluates to true:

+ * - CH_CFG_THREADHIERARCHY_ORDERED

+ * .

+ *

+ * <h2>Test Steps</h2>

+ * Todo

+ * .

+ */

+

+static void rt_test_003_005_execute(void) {

+

+  /* [3.5.1] Creating 1 parent and 4 child threads with increasing priority,

+   * hierarchy information is tested */

+  test_set_step(1);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    thread_t *parents[MAX_THREADS];

+    thread_t *siblings[MAX_THREADS];

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-5;

+    prios[2] = chThdGetPriorityX()-4;

+    prios[3] = chThdGetPriorityX()-3;

+    prios[4] = chThdGetPriorityX()-2;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    parents[0] = threads[0]->parent;

+    parents[1] = threads[1]->parent;

+    parents[2] = threads[2]->parent;

+    parents[3] = threads[3]->parent;

+    parents[4] = threads[4]->parent;

+    siblings[0] = threads[0]->children;

+    siblings[1] = threads[1]->sibling;

+    siblings[2] = threads[2]->sibling;

+    siblings[3] = threads[3]->sibling;

+    siblings[4] = threads[4]->sibling;

+    chThdTerminate(threads[0]);

+    chThdTerminate(threads[1]);

+    chThdTerminate(threads[2]);

+    chThdTerminate(threads[3]);

+    chThdTerminate(threads[4]);

+    test_wait_threads();

+    test_assert(parents[0] == chThdGetSelfX() &&

+                parents[1] == threads_cpy[0] &&

+                parents[2] == threads_cpy[0] &&

+                parents[3] == threads_cpy[0] &&

+                parents[4] == threads_cpy[0] &&

+                siblings[0] == threads_cpy[4] &&

+                siblings[1] == NULL &&

+                siblings[2] == threads_cpy[1] &&

+                siblings[3] == threads_cpy[2] &&

+                siblings[4] == threads_cpy[3], "invalid children list");

+  }

+

+  /* [3.5.2] Creating 1 parent and 4 child threads with decreasing priority,

+   * hierarchy information is tested.*/

+  test_set_step(2);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    thread_t *parents[MAX_THREADS];

+    thread_t *siblings[MAX_THREADS];

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-2;

+    prios[2] = chThdGetPriorityX()-3;

+    prios[3] = chThdGetPriorityX()-4;

+    prios[4] = chThdGetPriorityX()-5;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    parents[0] = threads[0]->parent;

+    parents[1] = threads[1]->parent;

+    parents[2] = threads[2]->parent;

+    parents[3] = threads[3]->parent;

+    parents[4] = threads[4]->parent;

+    siblings[0] = threads[0]->children;

+    siblings[1] = threads[1]->sibling;

+    siblings[2] = threads[2]->sibling;

+    siblings[3] = threads[3]->sibling;

+    siblings[4] = threads[4]->sibling;

+    test_terminate_threads();

+    test_wait_threads();

+#if CH_CFG_THREADHIERARCHY_ORDERED == TRUE

+    test_assert(parents[0] == chThdGetSelfX() &&

+                parents[1] == threads_cpy[0] &&

+                parents[2] == threads_cpy[0] &&

+                parents[3] == threads_cpy[0] &&

+                parents[4] == threads_cpy[0] &&

+                siblings[0] == threads_cpy[1] &&

+                siblings[1] == threads_cpy[2] &&

+                siblings[2] == threads_cpy[3] &&

+                siblings[3] == threads_cpy[4] &&

+                siblings[4] == NULL, "invalid children list");

+#else

+    test_assert(parents[0] == chThdGetSelfX() &&

+                parents[1] == threads_cpy[0] &&

+                parents[2] == threads_cpy[0] &&

+                parents[3] == threads_cpy[0] &&

+                parents[4] == threads_cpy[0] &&

+                siblings[0] == threads_cpy[4] &&

+                siblings[4] == threads_cpy[3] &&

+                siblings[3] == threads_cpy[2] &&

+                siblings[2] == threads_cpy[1] &&

+                siblings[1] == NULL, "invalid children list");

+#endif

+  }

+

+  /* [3.5.3] Creating 1 parent and 4 child threads with identical priority,

+   * hierarchy information is tested.*/

+  test_set_step(3);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    thread_t *parents[MAX_THREADS];

+    thread_t *siblings[MAX_THREADS];

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-2;

+    prios[2] = chThdGetPriorityX()-2;

+    prios[3] = chThdGetPriorityX()-2;

+    prios[4] = chThdGetPriorityX()-2;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    parents[0] = threads[0]->parent;

+    parents[1] = threads[1]->parent;

+    parents[2] = threads[2]->parent;

+    parents[3] = threads[3]->parent;

+    parents[4] = threads[4]->parent;

+    siblings[0] = threads[0]->children;

+    siblings[1] = threads[1]->sibling;

+    siblings[2] = threads[2]->sibling;

+    siblings[3] = threads[3]->sibling;

+    siblings[4] = threads[4]->sibling;

+    test_terminate_threads();

+    test_wait_threads();

+    test_assert(parents[0] == chThdGetSelfX() &&

+                parents[1] == threads_cpy[0] &&

+                parents[2] == threads_cpy[0] &&

+                parents[3] == threads_cpy[0] &&

+                parents[4] == threads_cpy[0] &&

+                siblings[0] == threads_cpy[4] &&

+                siblings[1] == NULL &&

+                siblings[2] == threads_cpy[1] &&

+                siblings[3] == threads_cpy[2] &&

+                siblings[4] == threads_cpy[3] , "invalid children list");

+  }

+

+  /* [3.5.4] Creating 1 parent and 4 child threads with increasing priority

+   * which are terminated in a controlled manner, hierarchy information is

+   * tested.*/

+  test_set_step(4);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    thread_t *parents[MAX_THREADS];

+    thread_t *siblings[MAX_THREADS];

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-5;

+    prios[2] = chThdGetPriorityX()-4;

+    prios[3] = chThdGetPriorityX()-3;

+    prios[4] = chThdGetPriorityX()-2;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    chThdTerminate(threads[1]);

+    chThdTerminate(threads[4]);

+    chThdWait(threads[1]);

+    chThdWait(threads[4]);

+    parents[0] = threads[0]->parent;

+    parents[1] = threads[1]->parent;

+    parents[2] = threads[2]->parent;

+    parents[3] = threads[3]->parent;

+    parents[4] = threads[4]->parent;

+    siblings[0] = threads[0]->children;

+    siblings[1] = threads[1]->sibling;

+    siblings[2] = threads[2]->sibling;

+    siblings[3] = threads[3]->sibling;

+    siblings[4] = threads[4]->sibling;

+    test_terminate_threads();

+    test_wait_threads();

+    test_assert(parents[0] == chThdGetSelfX() &&

+                parents[2] == threads_cpy[0] &&

+                parents[3] == threads_cpy[0] &&

+                siblings[0] == threads_cpy[3] &&

+                siblings[2] == NULL &&

+                siblings[3] == threads_cpy[2], "invalid children list");

+  }

+

+  /* [3.5.5] Creating 1 parent and 4 child threads and then terminating the

+   * parent, hierarchy information is tested.*/

+  test_set_step(5);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    thread_t *parents[MAX_THREADS];

+    uint8_t thdmask = 0;

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-1;

+    prios[2] = chThdGetPriorityX()-1;

+    prios[3] = chThdGetPriorityX()-1;

+    prios[4] = chThdGetPriorityX()-1;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    chThdTerminate(threads[0]);

+    chThdWait(threads[0]);

+    parents[0] = threads_cpy[0]->parent;

+    parents[1] = threads_cpy[1]->parent;

+    parents[2] = threads_cpy[2]->parent;

+    parents[3] = threads_cpy[3]->parent;

+    parents[4] = threads_cpy[4]->parent;

+    for (thread_t* sibling = ch.mainthread.children; sibling != NULL; sibling = sibling->sibling) {

+      thdmask |= (sibling == threads[0]) ? 1 << 0 : 0;

+      thdmask |= (sibling == threads[1]) ? 1 << 1 : 0;

+      thdmask |= (sibling == threads[2]) ? 1 << 2 : 0;

+      thdmask |= (sibling == threads[3]) ? 1 << 3 : 0;

+      thdmask |= (sibling == threads[4]) ? 1 << 4 : 0;

+    }

+    test_terminate_threads();

+    test_wait_threads();

+    test_assert(parents[1] == &ch.mainthread &&

+                parents[2] == &ch.mainthread &&

+                parents[3] == &ch.mainthread &&

+                parents[4] == &ch.mainthread &&

+                thdmask == ((1 << 1) | (1 << 2) | (1 << 3) | (1 << 4)), "child thread recovery failed");

+  }

+

+  /* [3.5.6] Creating 1 parent and 4 child threads with increasing priority

+   * and then increasing the priority of a low-priority child, hierarchy

+   * information is tested.*/

+  test_set_step(6);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    tprio_t testprios[2];

+    thread_t *siblings[MAX_THREADS];

+    prios[0] = chThdGetPriorityX()-1;

+    prios[1] = chThdGetPriorityX()-5;

+    prios[2] = chThdGetPriorityX()-4;

+    prios[3] = chThdGetPriorityX()-3;

+    prios[4] = chThdGetPriorityX()-2;

+    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prios[0], hierarchythread, &prios[0], chThdGetSelfX());

+    threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prios[1], hierarchythread, &prios[1], threads[0]);

+    threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prios[2], hierarchythread, &prios[2], threads[0]);

+    threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prios[3], hierarchythread, &prios[3], threads[0]);

+    threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prios[4], hierarchythread, &prios[4], threads[0]);

+    threads_cpy[0] = threads[0];

+    threads_cpy[1] = threads[1];

+    threads_cpy[2] = threads[2];

+    threads_cpy[3] = threads[3];

+    threads_cpy[4] = threads[4];

+    chThdSleepMilliseconds(10);

+    testprios[0] = threads[1]->prio;

+    prios[1] = prios[4];

+    chThdSleepMilliseconds(10);

+    testprios[1] = threads[1]->prio;

+    siblings[0] = threads[0]->children;

+    siblings[1] = threads[1]->sibling;

+    siblings[2] = threads[2]->sibling;

+    siblings[3] = threads[3]->sibling;

+    siblings[4] = threads[4]->sibling;

+    test_terminate_threads();

+    test_wait_threads();

+#if CH_CFG_THREADHIERARCHY_ORDERED == TRUE

+    test_assert(testprios[0] == chThdGetPriorityX()-5 &&

+                testprios[1] == prios[4] &&

+                siblings[0] == threads_cpy[1] &&

+                siblings[1] == threads_cpy[4] &&

+                siblings[2] == NULL &&

+                siblings[3] == threads_cpy[2] &&

+                siblings[4] == threads_cpy[3], "invalid children list");

+#else

+    test_assert(testprios[0] == chThdGetPriorityX()-5 &&

+                testprios[1] == prios[4] &&

+                siblings[0] == threads_cpy[4] &&

+                siblings[1] == NULL &&

+                siblings[2] == threads_cpy[1] &&

+                siblings[3] == threads_cpy[2] &&

+                siblings[4] == threads_cpy[3], "invalid children list");

+#endif

+  }

+

+  /* [3.5.7] Creating 1 parent and 4 child threads with increasing priority

+   * and the decreasing the priority of a high-priority child, hierarchy

+   * information is tested.*/

+  test_set_step(7);

+  {

+    tprio_t prios[MAX_THREADS];

+    thread_t *threads_cpy[MAX_THREADS];

+    tprio_t testprios[2];