cleaned up a bit, no functional change

This commit is contained in:
Robin Müller 2021-03-06 20:36:54 +01:00
parent 17b8d3fed0
commit 778ef4ef23

View File

@ -6,252 +6,250 @@
#include <errno.h> #include <errno.h>
PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_): PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_):
thread(0),priority(priority_),stackSize(stackSize_) { thread(0),priority(priority_),stackSize(stackSize_) {
name[0] = '\0'; name[0] = '\0';
std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1); std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1);
} }
PosixThread::~PosixThread() { PosixThread::~PosixThread() {
//No deletion and no free of Stack Pointer //No deletion and no free of Stack Pointer
} }
ReturnValue_t PosixThread::sleep(uint64_t ns) { ReturnValue_t PosixThread::sleep(uint64_t ns) {
//TODO sleep might be better with timer instead of sleep() //TODO sleep might be better with timer instead of sleep()
timespec time; timespec time;
time.tv_sec = ns/1000000000; time.tv_sec = ns/1000000000;
time.tv_nsec = ns - time.tv_sec*1e9; time.tv_nsec = ns - time.tv_sec*1e9;
//Remaining Time is not set here //Remaining Time is not set here
int status = nanosleep(&time,NULL); int status = nanosleep(&time,NULL);
if(status != 0){ if(status != 0){
switch(errno){ switch(errno){
case EINTR: case EINTR:
//The nanosleep() function was interrupted by a signal. //The nanosleep() function was interrupted by a signal.
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
case EINVAL: case EINVAL:
//The rqtp argument specified a nanosecond value less than zero or //The rqtp argument specified a nanosecond value less than zero or
// greater than or equal to 1000 million. // greater than or equal to 1000 million.
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
default: default:
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
} }
} }
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
void PosixThread::suspend() { void PosixThread::suspend() {
//Wait for SIGUSR1 //Wait for SIGUSR1
int caughtSig = 0; int caughtSig = 0;
sigset_t waitSignal; sigset_t waitSignal;
sigemptyset(&waitSignal); sigemptyset(&waitSignal);
sigaddset(&waitSignal, SIGUSR1); sigaddset(&waitSignal, SIGUSR1);
sigwait(&waitSignal, &caughtSig); sigwait(&waitSignal, &caughtSig);
if (caughtSig != SIGUSR1) { if (caughtSig != SIGUSR1) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "FixedTimeslotTask: Unknown Signal received: " << sif::error << "FixedTimeslotTask: Unknown Signal received: " <<
caughtSig << std::endl; caughtSig << std::endl;
#endif #endif
} }
} }
void PosixThread::resume(){ void PosixThread::resume(){
/* Signal the thread to start. Makes sense to call kill to start or? ;) /* Signal the thread to start. Makes sense to call kill to start or? ;)
* According to POSIX raise(signal) will call pthread_kill(pthread_self(), sig),
* According to Posix raise(signal) will call pthread_kill(pthread_self(), sig), but as the call must be done from the thread itself this is not possible here */
* but as the call must be done from the thread itsself this is not possible here pthread_kill(thread,SIGUSR1);
*/
pthread_kill(thread,SIGUSR1);
} }
bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms, bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms,
const uint64_t delayTime_ms) { const uint64_t delayTime_ms) {
uint64_t nextTimeToWake_ms; uint64_t nextTimeToWake_ms;
bool shouldDelay = false; bool shouldDelay = false;
//Get current Time /* Get current Time */
const uint64_t currentTime_ms = getCurrentMonotonicTimeMs(); const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
/* Generate the tick time at which the task wants to wake. */ /* Generate the tick time at which the task wants to wake. */
nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms; nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
if (currentTime_ms < *prevoiusWakeTime_ms) { if (currentTime_ms < *prevoiusWakeTime_ms) {
/* The tick count has overflowed since this function was /* The tick count has overflowed since this function was
lasted called. In this case the only time we should ever lasted called. In this case the only time we should ever
actually delay is if the wake time has also overflowed, actually delay is if the wake time has also overflowed,
and the wake time is greater than the tick time. When this and the wake time is greater than the tick time. When this
is the case it is as if neither time had overflowed. */ is the case it is as if neither time had overflowed. */
if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
&& (nextTimeToWake_ms > currentTime_ms)) { && (nextTimeToWake_ms > currentTime_ms)) {
shouldDelay = true; shouldDelay = true;
} }
} else { } else {
/* The tick time has not overflowed. In this case we will /* The tick time has not overflowed. In this case we will
delay if either the wake time has overflowed, and/or the delay if either the wake time has overflowed, and/or the
tick time is less than the wake time. */ tick time is less than the wake time. */
if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
|| (nextTimeToWake_ms > currentTime_ms)) { || (nextTimeToWake_ms > currentTime_ms)) {
shouldDelay = true; shouldDelay = true;
} }
} }
/* Update the wake time ready for the next call. */ /* Update the wake time ready for the next call. */
(*prevoiusWakeTime_ms) = nextTimeToWake_ms; (*prevoiusWakeTime_ms) = nextTimeToWake_ms;
if (shouldDelay) { if (shouldDelay) {
uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms; uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms;
PosixThread::sleep(sleepTime * 1000000ull); PosixThread::sleep(sleepTime * 1000000ull);
return true; return true;
} }
//We are shifting the time in case the deadline was missed like rtems /* We are shifting the time in case the deadline was missed like RTEMS */
(*prevoiusWakeTime_ms) = currentTime_ms; (*prevoiusWakeTime_ms) = currentTime_ms;
return false; return false;
} }
uint64_t PosixThread::getCurrentMonotonicTimeMs(){ uint64_t PosixThread::getCurrentMonotonicTimeMs(){
timespec timeNow; timespec timeNow;
clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow); clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow);
uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000 uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000
+ timeNow.tv_nsec / 1000000; + timeNow.tv_nsec / 1000000;
return currentTime_ms; return currentTime_ms;
} }
void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
//sif::debug << "PosixThread::createTask" << std::endl; //sif::debug << "PosixThread::createTask" << std::endl;
#endif #endif
/* /*
* The attr argument points to a pthread_attr_t structure whose contents * The attr argument points to a pthread_attr_t structure whose contents
are used at thread creation time to determine attributes for the new * are used at thread creation time to determine attributes for the new
thread; this structure is initialized using pthread_attr_init(3) and * thread; this structure is initialized using pthread_attr_init(3) and
related functions. If attr is NULL, then the thread is created with * related functions. If attr is NULL, then the thread is created with
default attributes. * default attributes.
*/ */
pthread_attr_t attributes; pthread_attr_t attributes;
int status = pthread_attr_init(&attributes); int status = pthread_attr_init(&attributes);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread attribute init failed with: " << sif::error << "Posix Thread attribute init failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
void* stackPointer; void* stackPointer;
status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize); status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: Stack init failed with: " << sif::error << "PosixThread::createTask: Stack init failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
if(errno == ENOMEM) { if(errno == ENOMEM) {
size_t stackMb = stackSize/10e6; size_t stackMb = stackSize/10e6;
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: Insufficient memory for" sif::error << "PosixThread::createTask: Insufficient memory for"
" the requested " << stackMb << " MB" << std::endl; " the requested " << stackMb << " MB" << std::endl;
#else #else
sif::printError("PosixThread::createTask: Insufficient memory for " sif::printError("PosixThread::createTask: Insufficient memory for "
"the requested %lu MB\n", static_cast<unsigned long>(stackMb)); "the requested %lu MB\n", static_cast<unsigned long>(stackMb));
#endif #endif
} }
else if(errno == EINVAL) { else if(errno == EINVAL) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: Wrong alignment argument!" sif::error << "PosixThread::createTask: Wrong alignment argument!"
<< std::endl; << std::endl;
#else #else
sif::printError("PosixThread::createTask: " sif::printError("PosixThread::createTask: "
"Wrong alignment argument!\n"); "Wrong alignment argument!\n");
#endif #endif
} }
return; return;
} }
status = pthread_attr_setstack(&attributes, stackPointer, stackSize); status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: pthread_attr_setstack " sif::error << "PosixThread::createTask: pthread_attr_setstack "
" failed with: " << strerror(status) << std::endl; " failed with: " << strerror(status) << std::endl;
sif::error << "Make sure the specified stack size is valid and is " sif::error << "Make sure the specified stack size is valid and is "
"larger than the minimum allowed stack size." << std::endl; "larger than the minimum allowed stack size." << std::endl;
#endif #endif
} }
status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED); status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread attribute setinheritsched failed with: " << sif::error << "Posix Thread attribute setinheritsched failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
// TODO FIFO -> This needs root privileges for the process // TODO FIFO -> This needs root privileges for the process
status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO); status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread attribute schedule policy failed with: " << sif::error << "Posix Thread attribute schedule policy failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
sched_param scheduleParams; sched_param scheduleParams;
scheduleParams.__sched_priority = priority; scheduleParams.__sched_priority = priority;
status = pthread_attr_setschedparam(&attributes, &scheduleParams); status = pthread_attr_setschedparam(&attributes, &scheduleParams);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread attribute schedule params failed with: " << sif::error << "Posix Thread attribute schedule params failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
//Set Signal Mask for suspend until startTask is called //Set Signal Mask for suspend until startTask is called
sigset_t waitSignal; sigset_t waitSignal;
sigemptyset(&waitSignal); sigemptyset(&waitSignal);
sigaddset(&waitSignal, SIGUSR1); sigaddset(&waitSignal, SIGUSR1);
status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL); status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread sigmask failed failed with: " << sif::error << "Posix Thread sigmask failed failed with: " <<
strerror(status) << " errno: " << strerror(errno) << std::endl; strerror(status) << " errno: " << strerror(errno) << std::endl;
#endif #endif
} }
status = pthread_create(&thread,&attributes,fnc_,arg_); status = pthread_create(&thread,&attributes,fnc_,arg_);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread create failed with: " << sif::error << "Posix Thread create failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
status = pthread_setname_np(thread,name); status = pthread_setname_np(thread,name);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: setname failed with: " << sif::error << "PosixThread::createTask: setname failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
if(status == ERANGE) { if(status == ERANGE) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: Task name length longer" sif::error << "PosixThread::createTask: Task name length longer"
" than 16 chars. Truncating.." << std::endl; " than 16 chars. Truncating.." << std::endl;
#endif #endif
name[15] = '\0'; name[15] = '\0';
status = pthread_setname_np(thread,name); status = pthread_setname_np(thread,name);
if(status != 0){ if(status != 0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PosixThread::createTask: Setting name" sif::error << "PosixThread::createTask: Setting name"
" did not work.." << std::endl; " did not work.." << std::endl;
#endif #endif
} }
} }
} }
status = pthread_attr_destroy(&attributes); status = pthread_attr_destroy(&attributes);
if(status!=0){ if(status!=0){
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Posix Thread attribute destroy failed with: " << sif::error << "Posix Thread attribute destroy failed with: " <<
strerror(status) << std::endl; strerror(status) << std::endl;
#endif #endif
} }
} }