AMiRo-OS

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

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

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

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

    * @brief   References to this 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

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

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

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

    * @brief   Thread argument.

    */

   void              *arg;

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

+  /**

+   * @brief   Pointer to the parent thread.

+   */

+  thread_t          *parent;

+#endif

 } thread_descriptor_t;

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

@@ -220,7 +226,11 @@ typedef struct {

 #ifdef __cplusplus

 extern "C" {

 #endif

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

+#else

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

+#endif

 #if CH_DBG_FILL_THREADS == TRUE

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

 #endif

@@ -228,8 +238,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

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

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

@@ -126,14 +126,26 @@ void chSysInit(void) {

 #if CH_CFG_NO_IDLE_THREAD == FALSE

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

 #if CH_CFG_USE_REGISTRY == TRUE

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

+#else

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

+#endif

+#else

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

 #else

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

 #endif

+#endif

 #else

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

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

+#else

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

 #endif

+#endif

 #if CH_DBG_ENABLE_STACK_CHECK == TRUE

   {

@@ -172,6 +184,9 @@ void chSysInit(void) {

       THD_WORKING_AREA_END(ch_idle_thread_wa),

       IDLEPRIO,

       _idle_thread,

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

+      NULL,

+#endif

       NULL

     };

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

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

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

@@ -70,22 +70,67 @@

 /* 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.                                                */

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

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

 /**

  * @brief   Initializes a thread structure.

  * @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, thread_t *parent,  const char *name, tprio_t prio) {

+#else

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

+#endif

   tp->prio      = prio;

   tp->state     = CH_STATE_WTSTART;

@@ -110,6 +155,15 @@ 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;

+#endif