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,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 +246,11 @@ 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.*/

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

+#else

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

+#endif

 }

 /**

@@ -300,6 +360,7 @@ thread_t *chThdCreate(const thread_descriptor_t *tdp) {

   return tp;

 }

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

 /**

  * @brief   Creates a new thread into a static memory area.

  * @post    The created thread has a reference counter set to one, it is

@@ -315,13 +376,20 @@ 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

  */

+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 +426,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);

+#if CH_CFG_USE_THREADHIERARCHY == TRUE

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

+#else

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

+#endif

   /* Starting the thread immediately.*/

   chSchWakeupS(tp, MSG_OK);

@@ -529,6 +601,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);

@@ -610,6 +706,43 @@ tprio_t chThdSetPriority(tprio_t newprio) {

 #else

   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;