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

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

/**

 * @brief  core

 */

typedef struct urt_core

{

  urt_node_t* _nodes;

  urt_osEventSource_t _evtSource;

  urt_osMutex_t _lock;

  urt_status_t _status;

  #if (URT_CFG_PUBSUB_ENABLED)

    urt_topic_t* _topics;

  #endif /* URT_CFG_PUBSUB_ENABLED */

  #if (URT_CFG_RPC_ENABLED)

    urt_service_t* _services;

  #endif /* URT_CFG_RPC_ENABLED */

}urt_core_t;

static urt_core_t core;

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

/* LOCAL VARIABLES                                                            */

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

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

/* LOCAL FUNCTIONS                                                            */

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

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

/* EXPORTED FUNCTIONS                                                         */

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

/**

 * @brief   Initialize the Core.

 */

void urtCoreInit(void)

{

  core._nodes = NULL;

  core._status = URT_STATUS_OK;

  urtEventSourceInit(&core._evtSource);

  urtMutexInit(&core._lock);

# if (URT_CFG_PUBSUB_ENABLED)

    core._topics = NULL;

# endif /* URT_CFG_PUBSUB_ENABLED */

# if (URT_CFG_RPC_ENABLED)

    core.urt_service_t = NULL;

# endif /* URT_CFG_RPC_ENABLED */

  return;

}

/**

 * @brief   Get Core status.

 *

 * @return  Current system status.

 */

urt_status_t urtCoreGetStatus(void)

{

  return core._status;

}

urt_osEventMask_t urtCoreGetEventMask(void)

{

    return URT_EVENTMASK_MAXPRIO;

}

/**

 * @brief   Get Core mutex.

 *

 * @return  Current system mutex.

 */

urt_osMutex_t* urtCoreGetMutex(void)

{

    return &core._lock;

}

/**

 * @brief   Get Core event source.

 *

 * @return  Current system event source.

 */

urt_osEventSource_t* urtCoreGetEvtSource(void)

{

    return &core._evtSource;

}

/**

 * @brief   Start threads of all nodes of the Core.

 */

void urtCoreStartNodes(void)

{

  urtMutexLock(&core._lock);

  urt_node_t* node = core._nodes;

  while (node)

  {

    urtThreadStart(node->thread);

    node = node->next;

    urtThreadMSleep(100); //TODO: delete

  }

  urtMutexUnlock(&core._lock);

  return;

}

/**

 * @brief   Synchronize all nodes of the core.

 *

 * @param[in] node  Pointer to a node to synchronize. Must not be NULL.

 *

 * @return  Returns URT_STATUS_OK if all nodes are synchronized and proceed.

 *          Returns URT_STATUS_SYNC_ERROR if an exception occurred (faulty stage value detected).

 *          Returns URT_STATUS_SYNC_PENDING if there are nodes left to synchronize.

 *          In the latter case, the node thread must still wait for the control event (proceed) to synchronize.

 */

urt_status_t urtCoreSynchronizeNodes(urt_node_t* node)

{

  urtDebugAssert(node != NULL);

  urtMutexLock(&core._lock);

  node->stage++;

  urt_node_t* nodeFromCore = core._nodes;

  while (nodeFromCore && nodeFromCore->stage == node->stage)

  {

    nodeFromCore = nodeFromCore->next;

  }

  if (!nodeFromCore)

  {

    urt_osEventFlags_t flag = URT_EVENTFLAG_PROCEED;

    urtEventSourceBroadcast(&core._evtSource, flag);

    urtMutexUnlock(&core._lock);

    return URT_STATUS_OK;

  }

  else if (nodeFromCore->stage == (node->stage - 1))

  {

    urtMutexUnlock(&core._lock);

    return URT_STATUS_SYNC_PENDING;

  }

  else

  {

      urtCoreStopNodes(URT_STATUS_SYNC_ERROR);

      urtMutexUnlock(&core._lock);

      return URT_STATUS_SYNC_ERROR;

  }

}

/**

 * @brief   Stop threads of all nodes of the Core.

 *

 * @param[in] reason  The reason why the function was called. For normal shutdown URT_STATUS_OK should be used.

 *

 * @return  Returns URT_STATUS_OK if there was no call with another reason than URT_STATUS_OK before.

 *          If the function has been called before with a different reason, that reason is returned.

 */

urt_status_t urtCoreStopNodes(urt_status_t reason)

{

  urtMutexLock(&core._lock);

  bool priorityBoosted = false;

  urt_osThreadPrio_t oldPrio;

  if (core._status == URT_STATUS_OK)

  {

    if (core._nodes->thread->prio < URT_THREAD_PRIO_HIGH_MAX)

    {

      oldPrio = core._nodes->thread->prio;

      priorityBoosted = true;

      core._nodes->thread->prio = URT_THREAD_PRIO_HIGH_MAX;

    }

    core._status = reason;

    urt_node_t* node = core._nodes;

    while (node)

    {

      urtThreadTerminate(node->thread, URT_THREAD_TERMINATE_REQUEST);

      node = node->next;

    }

    urt_osEventFlags_t flag = URT_EVENTFLAG_TERMINATE;

    urtEventSourceBroadcast(&core._evtSource, flag);

    urtMutexUnlock(&core._lock);

    if (priorityBoosted)

      core._nodes->thread->prio = oldPrio;

    return URT_STATUS_OK;

  }

  else

  {

    urtMutexUnlock(&core._lock);

    return core._status;

  }

}

/**

 * @brief   Get Core nodes.

 *

 * @return  Nodes registered to the core.

 */

urt_node_t* urtCoreGetNodes(void)

{

  return core._nodes;

}

/**

 * @brief   Prepend node to core's list of nodes.

 *

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

 */

void urtCoreAddNode(urt_node_t* node)

{

  urtDebugAssert(node);

  node->next = core._nodes;

  core._nodes = node;

  return;

}

/**

 * @brief   Get the topic of the Core.

 *

 * @param[in] id  Identifier of the topic to retrieve.

 *

 * @return  Returns a pointer to the requested service. Returns NULL if no service matches the given ID.

 */

# if (URT_CFG_PUBSUB_ENABLED)

/**

 * @brief   Append topic to core's list of topics.

 *

 * @param[in] node The topic to append.

 */

void urtCoreAddTopic(urt_topic_t* topic)

{

  urt_topic_t* lastTopic = core._topics;

  while (lastTopic->next != NULL)

  {

    lastTopic = lastTopic->next;

  }

  lastTopic->next = topic;

  return;

}

/**

 * @brief   Get core's list of topics.

 *

 * @return  The first topic of the core.

 */

urt_topic_t* urtCoreGetTopic(urt_topicid_t id)

{

  urtMutexLock(&core._lock);

  urt_topic_t* topic = core._topics;

  while (topic != NULL && topic->id < id)

      topic = topic->next;

  urtMutexUnlock(&core._lock);

  if (topic != NULL && topic->id == id)

      return topic;

  else

    return NULL;

}

# endif /* URT_CFG_PUBSUB_ENABLED */

# if (URT_CFG_RPC_ENABLED)

/**

 * @brief   Get the service of the Core.

 *

 * @param[in] id  Identifier of the service to retrieve.

 *

 * @return Returns a pointer to the requested service. Returns NULL if no service matches the given ID.

 */

urt_service_t urtCoreGetService(urt_serviceid_t id) {return urt_service_t;}

# endif /* URT_CFG_RPC_ENABLED */