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