AMiRo-OS

/*

µRtWare is a lightweight publish/subscribe middleware for real-time

applications. It was developed as part of the software habitat for the

Autonomous Mini Robot [1] (AMiRo) but can be used for other purposes as well.

Copyright (C) 2018..2020  Thomas Schöpping et al.

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <urtware.h>

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

/* LOCAL DEFINITIONS                                                          */

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

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

/* EXPORTED VARIABLES                                                         */

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

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

/* LOCAL TYPES                                                                */

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

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

/* LOCAL VARIABLES                                                            */

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

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

/* LOCAL FUNCTIONS                                                            */

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

/**

 * @brief  Main function of a node.

 *

 * @param[in] arg  Optional Argument to the thread main function.

 */

void _main(void* arg)

{

  urt_osEventMask_t mask;

  urt_osEventFlags_t flag = 0;

  urtEventRegister(urtCoreGetEvtSource(), &(((urt_node_t*)arg)->listener), mask, flag);

  if (((urt_node_t*)arg)->setupcallback != NULL)

  {

    mask = ((urt_node_t*)arg)->setupcallback(((urt_node_t*)arg), ((urt_node_t*)arg)->setupparams);

    if (mask == urtCoreGetEventMask())

    {

      urtCoreStopNodes(URT_STATUS_NODE_INVALEVTMASK);

    }

  }

  else

  {

    mask = URT_EVENTMASK_ALL;

  }

  if (urtCoreGetStatus() == URT_STATUS_OK)

  {

    urtCoreSynchronizeNodes(((urt_node_t*)arg));

  }

  while (urtThreadShouldTerminate())

  {

    urt_osEventMask_t temp = urtEventWait(mask, URT_EVENT_WAIT_ONE, URT_DELAY_INFINITE);

    if (temp == urtCoreGetEventMask())

    {

      ((urt_node_t*)arg)->loopcallback(((urt_node_t*)arg), mask, ((urt_node_t*)arg)->loopparams);

      #if (URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING)

        ((urt_node_t*)arg)->loops++;

      #endif /* URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING */

      if (mask == urtCoreGetEventMask())

      {

        urtCoreStopNodes(URT_STATUS_NODE_INVALEVTMASK);

      }

    }

  }

  if (((urt_node_t*)arg)->shutdowncallback != NULL)

  {

    ((urt_node_t*)arg)->shutdowncallback(((urt_node_t*)arg), urtCoreGetStatus(), ((urt_node_t*)arg)->shutdownparams);

  }

  urtEventUnregister(urtCoreGetEvtSource(), &((urt_node_t*)arg)->listener);

  //Terminate all children and siblings

  urt_osThread_t* threadToTerminate = urtThreadGetSelf()->children;

  while (threadToTerminate != urtThreadGetSelf() && threadToTerminate != NULL)

  {

    urtThreadTerminate(threadToTerminate, URT_THREAD_TERMINATE_REQUEST);

    if (threadToTerminate->children != NULL)

    {

      threadToTerminate = threadToTerminate->children;

    }

    else

    {

      if (threadToTerminate->sibling != NULL)

      {

        threadToTerminate = threadToTerminate->sibling;

      }

      else

      {

        threadToTerminate = threadToTerminate->parent;

        while (threadToTerminate->sibling == NULL && threadToTerminate != urtThreadGetSelf())

        {

          threadToTerminate = threadToTerminate->parent;

        }

        if (threadToTerminate != urtThreadGetSelf())

        {

          threadToTerminate = threadToTerminate->sibling;

        }

      }

    }

  }

  urt_osThread_t* threadToJoin = urtThreadGetSelf()->children;

  while (threadToJoin != urtThreadGetSelf() && threadToJoin != NULL)

  {

    urtThreadJoin(threadToJoin);

    if (threadToJoin->children != NULL)

    {

      threadToJoin = threadToJoin->children;

    }

    else

    {

      if (threadToJoin->sibling != NULL)

      {

        threadToJoin = threadToJoin->sibling;

      }

      else

      {

        threadToJoin = threadToJoin->parent;

        while (threadToJoin->sibling == NULL && threadToJoin != urtThreadGetSelf())

        {

          threadToJoin = threadToJoin->parent;

        }

        if (threadToJoin != urtThreadGetSelf())

        {

          threadToJoin = threadToJoin->sibling;

        }

      }

    }

  }

  urtThreadExit();

  return;

}

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

/* EXPORTED FUNCTIONS                                                         */

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

/**

 * @brief  Initalize a node.

 *

 * @param[in] node  The node to initialize. Must not be NULL.

 * @param[in] thread  The thread to intialize.

 * @param[in] setupcallback  Callback function to be executed during setup.

 *                           May be NULL if no custom setup is required.

 * @param[in] setupparams  Parameters for the setup callback function.

 *                         Must be NULL if no setup callback is specified.

 *                         May be NULL if the specified setup callback does not expect parameters.

 * @param[in] loopcallback  Callback function to be executed in a loop.

 * @param[in] loopparams  Parameters for the loop callback function.

 *                        May be NULL if the specified loop callback does not expect parameters.

 * @param[in] shutdowncallback  Callback function to be executed during shutdown.

 *                              May be NULL if no custom shutdown is required.

 * @param[in] shutdownparams  Parameters for the loop callback function.

 *                            Must be NULL if no shutdown callback is specified.

 *                            May be NULL if the specified shutdown callback does not expect parameters.

 */

void urtNodeInit(urt_node_t* node, urt_osThread_t* thread, urt_osThreadPrio_t prio, urt_nodeSetupCallback_t setupcallback,

                 void* setupparams, urt_nodeLoopCallback_t loopcallback, void* loopparams,

                 urt_nodeShutdownCallback_t shutdowncallback, void* shutdownparams)

{

  urtDebugAssert(node != NULL);

  if (setupcallback == NULL)

    urtDebugAssert(setupparams == NULL);

  node->next = NULL;

  node->thread = urtThreadInit((void*)thread, sizeof(thread), prio, (urt_osThreadFunction_t) _main, (void*)node);

  node->setupcallback = setupcallback;

  node->setupparams = setupparams;

  node->loopcallback = loopcallback;

  node->loopparams = loopparams;

  node->shutdowncallback = shutdowncallback;

  node->shutdownparams = shutdownparams;

  node->stage = 0;

  urtEventListenerInit(node->listener);

  #if (URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING)

    node->loops = 0;

  #endif /* URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING */

  urt_osMutex_t* mutexTemp = urtCoreGetMutex();

  urtMutexLock(mutexTemp);

    node->next = urtCoreGetNodes();

    urtCoreAddNode(node);

  urtMutexUnlock(mutexTemp);

  return;

}