| Directory: | ./ |
|---|---|
| File: | src/thread.cpp |
| Date: | 2022-06-29 13:58:11 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 17 | 18 | 94.4% |
| Branches: | 6 | 10 | 60.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * @file thread.cpp | ||
| 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) | ||
| 4 | * license License BSD-3-Clause | ||
| 5 | * @copyright Copyright (c) 2019, New York University and Max Planck | ||
| 6 | * Gesellschaft. | ||
| 7 | * @date 2019-05-22 | ||
| 8 | * | ||
| 9 | * @brief Implement method to create and join threads | ||
| 10 | */ | ||
| 11 | #include "real_time_tools/thread.hpp" | ||
| 12 | #include <stdexcept> | ||
| 13 | #include "real_time_tools/process_manager.hpp" | ||
| 14 | |||
| 15 | namespace real_time_tools | ||
| 16 | { | ||
| 17 | #if defined RT_PREEMPT | ||
| 18 | |||
| 19 | RealTimeThread::RealTimeThread() | ||
| 20 | { | ||
| 21 | thread_.reset(nullptr); | ||
| 22 | } | ||
| 23 | |||
| 24 | RealTimeThread::~RealTimeThread() | ||
| 25 | { | ||
| 26 | join(); | ||
| 27 | thread_.reset(nullptr); | ||
| 28 | } | ||
| 29 | |||
| 30 | void* exec(void (*f)(void*), void* args) | ||
| 31 | { | ||
| 32 | f(args); | ||
| 33 | return nullptr; | ||
| 34 | } | ||
| 35 | |||
| 36 | /** | ||
| 37 | * @brief rt_preempt_error_message id common message for all things that could | ||
| 38 | * go wrong. | ||
| 39 | */ | ||
| 40 | const std::string rt_preempt_error_message( | ||
| 41 | "NOTE: This program must be executed with special permission to get " | ||
| 42 | "the required real time permissions.\n" | ||
| 43 | "Either use sudo or be part of the \'realtime\' group" | ||
| 44 | "Aborting thread creation."); | ||
| 45 | |||
| 46 | int RealTimeThread::create_realtime_thread(void* (*thread_function)(void*), | ||
| 47 | void* args) | ||
| 48 | { | ||
| 49 | if (thread_ != nullptr) | ||
| 50 | { | ||
| 51 | printf("Thread already running"); | ||
| 52 | } | ||
| 53 | |||
| 54 | if (parameters_.cpu_dma_latency_ >= 0) | ||
| 55 | { | ||
| 56 | set_cpu_dma_latency(parameters_.cpu_dma_latency_); | ||
| 57 | } | ||
| 58 | |||
| 59 | thread_.reset(new pthread_t()); | ||
| 60 | |||
| 61 | if (parameters_.block_memory_) | ||
| 62 | { | ||
| 63 | block_memory(); | ||
| 64 | } | ||
| 65 | |||
| 66 | struct sched_param param; | ||
| 67 | pthread_attr_t attr; | ||
| 68 | int ret; | ||
| 69 | |||
| 70 | ret = pthread_attr_init(&attr); | ||
| 71 | if (ret) | ||
| 72 | { | ||
| 73 | printf( | ||
| 74 | "%s %d\n", | ||
| 75 | ("init pthread attributes failed. Ret=" + rt_preempt_error_message) | ||
| 76 | .c_str(), | ||
| 77 | ret); | ||
| 78 | return ret; | ||
| 79 | } | ||
| 80 | |||
| 81 | /* Set a specific stack size */ | ||
| 82 | ret = pthread_attr_setstacksize(&attr, parameters_.stack_size_); | ||
| 83 | if (ret) | ||
| 84 | { | ||
| 85 | printf("%s %d\n", | ||
| 86 | ("pthread setstacksize failed. Ret=" + rt_preempt_error_message) | ||
| 87 | .c_str(), | ||
| 88 | ret); | ||
| 89 | return ret; | ||
| 90 | } | ||
| 91 | |||
| 92 | /* Set scheduler policy and priority of pthread */ | ||
| 93 | ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); | ||
| 94 | // ret = pthread_attr_setschedpolicy(&attr, SCHED_RR); // WARNING LAAS is | ||
| 95 | // using this one!!!! | ||
| 96 | if (ret) | ||
| 97 | { | ||
| 98 | printf( | ||
| 99 | "%s %d\n", | ||
| 100 | ("pthread setschedpolicy failed. Ret=" + rt_preempt_error_message) | ||
| 101 | .c_str(), | ||
| 102 | ret); | ||
| 103 | return ret; | ||
| 104 | } | ||
| 105 | param.sched_priority = parameters_.priority_; | ||
| 106 | ret = pthread_attr_setschedparam(&attr, ¶m); | ||
| 107 | if (ret) | ||
| 108 | { | ||
| 109 | printf("%s %d\n", | ||
| 110 | ("pthread setschedparam failed. Ret=" + rt_preempt_error_message) | ||
| 111 | .c_str(), | ||
| 112 | ret); | ||
| 113 | return ret; | ||
| 114 | } | ||
| 115 | /* Use scheduling parameters of attr */ | ||
| 116 | ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); | ||
| 117 | if (ret) | ||
| 118 | { | ||
| 119 | printf( | ||
| 120 | "%s %d\n", | ||
| 121 | ("pthread setinheritsched failed. Ret=" + rt_preempt_error_message) | ||
| 122 | .c_str(), | ||
| 123 | ret); | ||
| 124 | return ret; | ||
| 125 | } | ||
| 126 | |||
| 127 | /* Create a pthread with specified attributes */ | ||
| 128 | ret = pthread_create(thread_.get(), &attr, thread_function, args); | ||
| 129 | if (ret) | ||
| 130 | { | ||
| 131 | printf( | ||
| 132 | "%s %d\n", | ||
| 133 | ("create pthread failed. Ret=" + rt_preempt_error_message).c_str(), | ||
| 134 | ret); | ||
| 135 | return ret; | ||
| 136 | } | ||
| 137 | |||
| 138 | if (parameters_.cpu_id_.size() > 0) | ||
| 139 | { | ||
| 140 | cpu_set_t cpuset; | ||
| 141 | CPU_ZERO(&cpuset); | ||
| 142 | for (unsigned i = 0; i < parameters_.cpu_id_.size(); ++i) | ||
| 143 | { | ||
| 144 | CPU_SET(parameters_.cpu_id_[i], &cpuset); | ||
| 145 | } | ||
| 146 | ret = pthread_setaffinity_np(*thread_, sizeof(cpu_set_t), &cpuset); | ||
| 147 | if (ret) | ||
| 148 | { | ||
| 149 | printf("%s %d\n", | ||
| 150 | ("Associate thread to a specific cpu failed. Ret=" + | ||
| 151 | rt_preempt_error_message) | ||
| 152 | .c_str(), | ||
| 153 | ret); | ||
| 154 | } | ||
| 155 | |||
| 156 | int get_aff_error = 0; | ||
| 157 | get_aff_error = | ||
| 158 | pthread_getaffinity_np(*thread_, sizeof(cpu_set_t), &cpuset); | ||
| 159 | if (get_aff_error) | ||
| 160 | { | ||
| 161 | printf("%s %d\n", | ||
| 162 | ("Check the thread cpu affinity failed. Ret=" + | ||
| 163 | rt_preempt_error_message) | ||
| 164 | .c_str(), | ||
| 165 | ret); | ||
| 166 | } | ||
| 167 | printf("Set returned by pthread_getaffinity_np() contained: "); | ||
| 168 | for (unsigned j = 0; j < CPU_SETSIZE; j++) | ||
| 169 | { | ||
| 170 | if (CPU_ISSET(j, &cpuset)) | ||
| 171 | { | ||
| 172 | printf("CPU %d, ", j); | ||
| 173 | } | ||
| 174 | } | ||
| 175 | printf("\n"); | ||
| 176 | } | ||
| 177 | return ret; | ||
| 178 | } | ||
| 179 | |||
| 180 | int RealTimeThread::join() | ||
| 181 | { | ||
| 182 | int ret = 0; | ||
| 183 | if (thread_ != nullptr) | ||
| 184 | { | ||
| 185 | /* Join the thread and wait until it is done */ | ||
| 186 | ret = pthread_join(*thread_, nullptr); | ||
| 187 | if (ret) | ||
| 188 | { | ||
| 189 | printf("join pthread failed.\n"); | ||
| 190 | } | ||
| 191 | thread_.reset(nullptr); | ||
| 192 | } | ||
| 193 | return ret; | ||
| 194 | } | ||
| 195 | |||
| 196 | void RealTimeThread::block_memory() | ||
| 197 | { | ||
| 198 | /* Lock memory */ | ||
| 199 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) | ||
| 200 | { | ||
| 201 | printf("mlockall failed: %m\n"); | ||
| 202 | exit(-2); | ||
| 203 | } | ||
| 204 | } | ||
| 205 | #endif // Defined RT_PREEMPT | ||
| 206 | |||
| 207 | /********************************************************** | ||
| 208 | * TODO: Check the implementation of this thread creation * | ||
| 209 | **********************************************************/ | ||
| 210 | #if defined XENOMAI | ||
| 211 | |||
| 212 | RealTimeThread::RealTimeThread() | ||
| 213 | { | ||
| 214 | } | ||
| 215 | |||
| 216 | RealTimeThread::~RealTimeThread() | ||
| 217 | { | ||
| 218 | join(); | ||
| 219 | } | ||
| 220 | |||
| 221 | int RealTimeThread::create_realtime_thread(void (*thread_function)(void*), | ||
| 222 | void* args) | ||
| 223 | { | ||
| 224 | // initializing rt_printf, | ||
| 225 | // nothing would get printed otherwise | ||
| 226 | rt_print_auto_init(1); | ||
| 227 | |||
| 228 | int ret; | ||
| 229 | if (parameters_.dedicated_cpu_id_ >= 0) | ||
| 230 | { | ||
| 231 | ret = rt_task_spawn( | ||
| 232 | &thread_, | ||
| 233 | parameters_.keyword_.c_str(), | ||
| 234 | parameters_.stack_size_, | ||
| 235 | parameters_.priority_, | ||
| 236 | T_FPU | T_JOINABLE | T_CPU(parameters_.dedicated_cpu_id_), | ||
| 237 | thread_function, | ||
| 238 | args); | ||
| 239 | } | ||
| 240 | else | ||
| 241 | { | ||
| 242 | ret = rt_task_spawn(&thread_, | ||
| 243 | parameters_.keyword_.c_str(), | ||
| 244 | parameters_.stack_size_, | ||
| 245 | parameters_.priority_, | ||
| 246 | T_FPU | T_JOINABLE, | ||
| 247 | thread_function, | ||
| 248 | args); | ||
| 249 | } | ||
| 250 | |||
| 251 | return ret; | ||
| 252 | } | ||
| 253 | |||
| 254 | int RealTimeThread::join() | ||
| 255 | { | ||
| 256 | int ret = rt_task_join(&thread_); | ||
| 257 | return ret; | ||
| 258 | } | ||
| 259 | |||
| 260 | void RealTimeThread::block_memory() | ||
| 261 | { | ||
| 262 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) | ||
| 263 | { | ||
| 264 | printf("mlockall failed: %m\n"); | ||
| 265 | exit(-2); | ||
| 266 | } | ||
| 267 | } | ||
| 268 | |||
| 269 | #endif // Defined XENOMAI | ||
| 270 | |||
| 271 | /********************************************************** | ||
| 272 | * TODO: Check the implementation of this thread creation * | ||
| 273 | **********************************************************/ | ||
| 274 | #if defined NON_REAL_TIME | ||
| 275 | |||
| 276 | 1 | RealTimeThread::RealTimeThread() | |
| 277 | { | ||
| 278 | 1 | thread_.reset(nullptr); | |
| 279 | 1 | } | |
| 280 | |||
| 281 | 2 | RealTimeThread::~RealTimeThread() | |
| 282 | { | ||
| 283 | 1 | join(); | |
| 284 | 1 | thread_.reset(nullptr); | |
| 285 | 1 | } | |
| 286 | |||
| 287 | 1 | int RealTimeThread::create_realtime_thread(void* (*thread_function)(void*), | |
| 288 | void* args) | ||
| 289 | { | ||
| 290 | 1 | printf("Warning this thread is not going to be real time.\n"); | |
| 291 | |||
| 292 | /* Create a standard thread for non-real time OS */ | ||
| 293 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | thread_.reset(new std::thread(thread_function, args)); |
| 294 | 1 | return 0; | |
| 295 | } | ||
| 296 | |||
| 297 | 2 | int RealTimeThread::join() | |
| 298 | { | ||
| 299 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (thread_ != nullptr) |
| 300 | { | ||
| 301 |
2/2✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
|
2 | if (thread_->joinable()) |
| 302 | { | ||
| 303 | 1 | thread_->join(); | |
| 304 | } | ||
| 305 | } | ||
| 306 | 2 | return 0; | |
| 307 | } | ||
| 308 | |||
| 309 | ✗ | void RealTimeThread::block_memory() | |
| 310 | { | ||
| 311 | // do nothing | ||
| 312 | } | ||
| 313 | #endif // Defined NON_REAL_TIME | ||
| 314 | |||
| 315 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
|
42 | } // namespace real_time_tools |
| 316 |