# HG changeset patch # User Edouard Tisserant # Date 1676545996 -3600 # Node ID 516779f11803ed3c059d085c673a44b6ace40d39 # Parent 0043e2b9dbec0223b66fa5ee0c87dd42079430f5 runtime: Change Linux target to use clock_nanosleep instead of timer (better rt-preempt perf). diff -r 0043e2b9dbec -r 516779f11803 targets/Linux/plc_Linux_main.c --- a/targets/Linux/plc_Linux_main.c Tue Feb 14 12:10:35 2023 +0100 +++ b/targets/Linux/plc_Linux_main.c Thu Feb 16 12:13:16 2023 +0100 @@ -12,7 +12,15 @@ #include #include -static sem_t Run_PLC; +static unsigned long __debug_tick; + +static pthread_t PLC_thread; +static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER; + +static int PLC_shutdown = 0; long AtomicCompareExchange(long* atomicvar,long compared, long exchange) { @@ -31,37 +39,32 @@ CURRENT_TIME->tv_nsec = tmp.tv_nsec; } -void PLC_timer_notify(sigval_t val) -{ - PLC_GetTime(&__CURRENT_TIME); - sem_post(&Run_PLC); -} - -timer_t PLC_timer; +static long long period_ns = 0; +struct timespec next_abs_time; + +static void inc_timespec(struct timespec *ts, unsigned long long value_ns) +{ +#ifdef __lldiv_t_defined + lldiv_t next_div = lldiv(ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns, 1000000000); + ts->tv_sec = next_div.quot; + ts->tv_nsec = next_div.rem; +#else + long long next_ns = ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns; + ts->tv_sec = next_ns / 1000000000; + ts->tv_nsec = next_ns % 1000000000; +#endif +} void PLC_SetTimer(unsigned long long next, unsigned long long period) { - struct itimerspec timerValues; - /* - printf("SetTimer(%lld,%lld)\n",next, period); - */ - memset (&timerValues, 0, sizeof (struct itimerspec)); - { -#ifdef __lldiv_t_defined - lldiv_t nxt_div = lldiv(next, 1000000000); - lldiv_t period_div = lldiv(period, 1000000000); - timerValues.it_value.tv_sec = nxt_div.quot; - timerValues.it_value.tv_nsec = nxt_div.rem; - timerValues.it_interval.tv_sec = period_div.quot; - timerValues.it_interval.tv_nsec = period_div.rem; -#else - timerValues.it_value.tv_sec = next / 1000000000; - timerValues.it_value.tv_nsec = next % 1000000000; - timerValues.it_interval.tv_sec = period / 1000000000; - timerValues.it_interval.tv_nsec = period % 1000000000; -#endif - } - timer_settime (PLC_timer, 0, &timerValues, NULL); + /* + printf("SetTimer(%lld,%lld)\n",next, period); + */ + period_ns = period; + clock_gettime(CLOCK_MONOTONIC, &next_abs_time); + inc_timespec(&next_abs_time, next); + // interrupt clock_nanpsleep + pthread_kill(PLC_thread, SIGUSR1); } // void catch_signal(int sig) @@ -72,16 +75,11 @@ exit(0); } - -static unsigned long __debug_tick; - -pthread_t PLC_thread; -static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER; - -int PLC_shutdown = 0; +void PLCThreadSignalHandler(int sig) +{ + if (sig == SIGUSR2) + pthread_exit(NULL); +} int ForceSaveRetainReq(void) { return PLC_shutdown; @@ -90,8 +88,14 @@ void PLC_thread_proc(void *arg) { while (!PLC_shutdown) { - sem_wait(&Run_PLC); + // Sleep until next PLC run + // TODO check result of clock_nanosleep and wait again or exit eventually + int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL); + if(res==EINTR) continue; + if(res!=0) return; + PLC_GetTime(&__CURRENT_TIME); __run(); + inc_timespec(&next_abs_time, period_ns); } pthread_exit(0); } @@ -99,21 +103,12 @@ #define maxval(a,b) ((a>b)?a:b) int startPLC(int argc,char **argv) { - struct sigevent sigev; setlocale(LC_NUMERIC, "C"); PLC_shutdown = 0; - sem_init(&Run_PLC, 0, 0); - pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL); - memset (&sigev, 0, sizeof (struct sigevent)); - sigev.sigev_value.sival_int = 0; - sigev.sigev_notify = SIGEV_THREAD; - sigev.sigev_notify_attributes = NULL; - sigev.sigev_notify_function = PLC_timer_notify; - pthread_mutex_init(&debug_wait_mutex, NULL); pthread_mutex_init(&debug_mutex, NULL); pthread_mutex_init(&python_wait_mutex, NULL); @@ -122,12 +117,16 @@ pthread_mutex_lock(&debug_wait_mutex); pthread_mutex_lock(&python_wait_mutex); - timer_create (CLOCK_MONOTONIC, &sigev, &PLC_timer); if( __init(argc,argv) == 0 ){ - PLC_SetTimer(common_ticktime__,common_ticktime__); - + + /* Signal to wakeup PLC thread when period changes */ + signal(SIGUSR1, PLCThreadSignalHandler); + /* Signal to end PLC thread */ + signal(SIGUSR2, PLCThreadSignalHandler); /* install signal handler for manual break */ signal(SIGINT, catch_signal); + + PLC_SetTimer(common_ticktime__,common_ticktime__); }else{ return 1; } @@ -155,11 +154,9 @@ { /* Stop the PLC */ PLC_shutdown = 1; - sem_post(&Run_PLC); - PLC_SetTimer(0,0); - pthread_join(PLC_thread, NULL); - sem_destroy(&Run_PLC); - timer_delete (PLC_timer); + /* Order PLCThread to exit */ + pthread_kill(PLC_thread, SIGUSR2); + pthread_join(PLC_thread, NULL); __cleanup(); pthread_mutex_destroy(&debug_wait_mutex); pthread_mutex_destroy(&debug_mutex); @@ -197,7 +194,7 @@ /*__DEBUG is protected by this mutex */ __DEBUG = !disable; if (disable) - pthread_mutex_unlock(&debug_mutex); + pthread_mutex_unlock(&debug_mutex); return 0; } @@ -247,7 +244,7 @@ #define _LogAndReturnNull(text) \ {\ - char mstr[256] = text " for ";\ + char mstr[256] = text " for ";\ strncat(mstr, name, 255);\ LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ return NULL;\ @@ -256,8 +253,8 @@ void *create_RT_to_nRT_signal(char* name){ RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t)); - if(!sig) - _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal"); + if(!sig) + _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal"); sig->used = 1; pthread_cond_init(&sig->WakeCond, NULL);