Statistics
| Branch: | Revision:

urtware / src / urt_node.c @ 33aa05c5

History | View | Annotate | Download (7.62 KB)

1
/*
2
µRtWare is a lightweight publish/subscribe middleware for real-time
3
applications. It was developed as part of the software habitat for the
4
Autonomous Mini Robot [1] (AMiRo) but can be used for other purposes as well.
5

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

8
This program is free software: you can redistribute it and/or modify
9
it under the terms of the GNU General Public License as published by
10
the Free Software Foundation, either version 3 of the License, or
11
(at your option) any later version.
12

13
This program is distributed in the hope that it will be useful,
14
but WITHOUT ANY WARRANTY; without even the implied warranty of
15
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
GNU General Public License for more details.
17

18
You should have received a copy of the GNU General Public License
19
along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
*/
21

    
22
#include <urtware.h>
23

    
24
/******************************************************************************/
25
/* LOCAL DEFINITIONS                                                          */
26
/******************************************************************************/
27

    
28
/******************************************************************************/
29
/* EXPORTED VARIABLES                                                         */
30
/******************************************************************************/
31

    
32
/******************************************************************************/
33
/* LOCAL TYPES                                                                */
34
/******************************************************************************/
35

    
36
/******************************************************************************/
37
/* LOCAL VARIABLES                                                            */
38
/******************************************************************************/
39

    
40
/******************************************************************************/
41
/* LOCAL FUNCTIONS                                                            */
42
/******************************************************************************/
43

    
44
/**
45
 * @brief  Main function of a node.
46
 *
47
 * @param[in] arg  Optional Argument to the thread main function.
48
 */
49
void _main(void* arg)
50
{
51
  urt_osEventMask_t mask;
52
  urt_osEventFlags_t flag = 0;
53
  urtEventRegister(urtCoreGetEvtSource(), &(((urt_node_t*)arg)->listener), mask, flag);
54
  if (((urt_node_t*)arg)->setupcallback != NULL)
55
  {
56
    mask = ((urt_node_t*)arg)->setupcallback(((urt_node_t*)arg), ((urt_node_t*)arg)->setupparams);
57
    if (mask == urtCoreGetEventMask())
58
    {
59
      urtCoreStopNodes(URT_STATUS_NODE_INVALEVTMASK);
60
    }
61
  }
62
  else
63
  {
64
    mask = URT_EVENTMASK_ALL;
65
  }
66

    
67
  if (urtCoreGetStatus() == URT_STATUS_OK)
68
  {
69
    urtCoreSynchronizeNodes(((urt_node_t*)arg));
70
  }
71

    
72
  while (!urtThreadShouldTerminate())
73
  {
74
    urt_osEventMask_t temp = urtEventWait(mask, URT_EVENT_WAIT_ONE, URT_DELAY_INFINITE);
75
    if (temp == urtCoreGetEventMask())
76
    {
77
      ((urt_node_t*)arg)->loopcallback(((urt_node_t*)arg), mask, ((urt_node_t*)arg)->loopparams);
78
      #if (URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING)
79
        ((urt_node_t*)arg)->loops++;
80
      #endif /* URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING */
81
      if (mask == urtCoreGetEventMask())
82
      {
83
        urtCoreStopNodes(URT_STATUS_NODE_INVALEVTMASK);
84
      }
85
    }
86
  }
87

    
88
  if (((urt_node_t*)arg)->shutdowncallback != NULL)
89
  {
90
    ((urt_node_t*)arg)->shutdowncallback(((urt_node_t*)arg), urtCoreGetStatus(), ((urt_node_t*)arg)->shutdownparams);
91
  }
92
  urtEventUnregister(urtCoreGetEvtSource(), &((urt_node_t*)arg)->listener);
93

    
94
  //Terminate all children and siblings
95
  urt_osThread_t* threadToTerminate = urtThreadGetSelf()->children;
96
  while (threadToTerminate != urtThreadGetSelf() && threadToTerminate != NULL)
97
  {
98
    urtThreadTerminate(threadToTerminate, URT_THREAD_TERMINATE_REQUEST);
99
    if (threadToTerminate->children != NULL)
100
    {
101
      threadToTerminate = threadToTerminate->children;
102
    }
103
    else
104
    {
105
      if (threadToTerminate->sibling != NULL)
106
      {
107
        threadToTerminate = threadToTerminate->sibling;
108
      }
109
      else
110
      {
111
        threadToTerminate = threadToTerminate->parent;
112
        while (threadToTerminate->sibling == NULL && threadToTerminate != urtThreadGetSelf())
113
        {
114
          threadToTerminate = threadToTerminate->parent;
115
        }
116
        if (threadToTerminate != urtThreadGetSelf())
117
        {
118
          threadToTerminate = threadToTerminate->sibling;
119
        }
120
      }
121
    }
122
  }
123

    
124
  urt_osThread_t* threadToJoin = urtThreadGetSelf()->children;
125
  while (threadToJoin != urtThreadGetSelf() && threadToJoin != NULL)
126
  {
127
    urtThreadJoin(threadToJoin);
128
    if (threadToJoin->children != NULL)
129
    {
130
      threadToJoin = threadToJoin->children;
131
    }
132
    else
133
    {
134
      if (threadToJoin->sibling != NULL)
135
      {
136
        threadToJoin = threadToJoin->sibling;
137
      }
138
      else
139
      {
140
        threadToJoin = threadToJoin->parent;
141
        while (threadToJoin->sibling == NULL && threadToJoin != urtThreadGetSelf())
142
        {
143
          threadToJoin = threadToJoin->parent;
144
        }
145
        if (threadToJoin != urtThreadGetSelf())
146
        {
147
          threadToJoin = threadToJoin->sibling;
148
        }
149
      }
150
    }
151
  }
152

    
153
  urtThreadExit();
154
  return;
155
}
156

    
157
/******************************************************************************/
158
/* EXPORTED FUNCTIONS                                                         */
159
/******************************************************************************/
160

    
161
/**
162
 * @brief  Initalize a node.
163
 *
164
 * @param[in] node  The node to initialize. Must not be NULL.
165
 * @param[in] thread  The thread to intialize.
166
 * @param[in] setupcallback  Callback function to be executed during setup.
167
 *                           May be NULL if no custom setup is required.
168
 * @param[in] setupparams  Parameters for the setup callback function.
169
 *                         Must be NULL if no setup callback is specified.
170
 *                         May be NULL if the specified setup callback does not expect parameters.
171
 * @param[in] loopcallback  Callback function to be executed in a loop.
172
 * @param[in] loopparams  Parameters for the loop callback function.
173
 *                        May be NULL if the specified loop callback does not expect parameters.
174
 * @param[in] shutdowncallback  Callback function to be executed during shutdown.
175
 *                              May be NULL if no custom shutdown is required.
176
 * @param[in] shutdownparams  Parameters for the loop callback function.
177
 *                            Must be NULL if no shutdown callback is specified.
178
 *                            May be NULL if the specified shutdown callback does not expect parameters.
179
 */
180
void urtNodeInit(urt_node_t* node, urt_osThread_t* thread, urt_osThreadPrio_t prio, urt_nodeSetupCallback_t setupcallback,
181
                 void* setupparams, urt_nodeLoopCallback_t loopcallback, void* loopparams,
182
                 urt_nodeShutdownCallback_t shutdowncallback, void* shutdownparams)
183
{
184
  urtDebugAssert(node != NULL);
185
  if (setupcallback == NULL)
186
    urtDebugAssert(setupparams == NULL);
187

    
188
  node->next = NULL;
189
  node->thread = urtThreadInit((void*)thread, sizeof(thread), prio, (urt_osThreadFunction_t) _main, (void*)node);
190
  node->setupcallback = setupcallback;
191
  node->setupparams = setupparams;
192
  node->loopcallback = loopcallback;
193
  node->loopparams = loopparams;
194
  node->shutdowncallback = shutdowncallback;
195
  node->shutdownparams = shutdownparams;
196
  node->stage = 0;
197
  urtEventListenerInit(node->listener);
198
  #if (URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING)
199
    node->loops = 0;
200
  #endif /* URT_CFG_PUBSUB_PROFILING || URT_CFG_RPC_PROFILING */
201
  node->next = urtCoreGetNodes();
202
  urtCoreAddNode(node);
203
  return;
204
}