# HG changeset patch # User Edouard Tisserant # Date 1676637670 -3600 # Node ID 743a384f1bbbbadcefd5855753c551bd670fd19e # Parent ad3a05cf206651a14e318eb95db8fb96a89252c7# Parent 265fc8001d0a6895cfaf44a5b5ca9c19343b4e25 Merged default in wxPython4 branch diff -r ad3a05cf2066 -r 743a384f1bbb targets/Linux/plc_Linux_main.c --- a/targets/Linux/plc_Linux_main.c Tue Feb 14 19:46:27 2023 +0100 +++ b/targets/Linux/plc_Linux_main.c Fri Feb 17 13:41:10 2023 +0100 @@ -7,11 +7,20 @@ #include #include #include +#include #include #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) { @@ -30,39 +39,34 @@ 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) +{ + long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns; +#ifdef __lldiv_t_defined + lldiv_t next_div = lldiv(next_ns, 1000000000); + ts->tv_sec = next_div.quot; + ts->tv_nsec = next_div.rem; +#else + 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) { // signal(SIGTERM, catch_signal); @@ -71,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; @@ -89,8 +88,19 @@ 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){ + printf("PLC thread died with error %d \n", res); + return; + } + PLC_GetTime(&__CURRENT_TIME); __run(); + inc_timespec(&next_abs_time, period_ns); } pthread_exit(0); } @@ -98,21 +108,10 @@ #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); @@ -121,12 +120,20 @@ 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); + + /* initialize next occurence and period */ + period_ns = common_ticktime__; + clock_gettime(CLOCK_MONOTONIC, &next_abs_time); + + pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL); }else{ return 1; } @@ -154,11 +161,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); @@ -196,7 +201,7 @@ /*__DEBUG is protected by this mutex */ __DEBUG = !disable; if (disable) - pthread_mutex_unlock(&debug_mutex); + pthread_mutex_unlock(&debug_mutex); return 0; } @@ -237,6 +242,7 @@ } struct RT_to_nRT_signal_s { + int used; pthread_cond_t WakeCond; pthread_mutex_t WakeCondLock; }; @@ -245,7 +251,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;\ @@ -254,9 +260,10 @@ 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); pthread_mutex_init(&sig->WakeCondLock, NULL); @@ -266,10 +273,10 @@ void delete_RT_to_nRT_signal(void* handle){ RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle; - pthread_cond_destroy(&sig->WakeCond); - pthread_mutex_destroy(&sig->WakeCondLock); - - free(sig); + pthread_mutex_lock(&sig->WakeCondLock); + sig->used = 0; + pthread_cond_signal(&sig->WakeCond); + pthread_mutex_unlock(&sig->WakeCondLock); } int wait_RT_to_nRT_signal(void* handle){ @@ -277,7 +284,14 @@ RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle; pthread_mutex_lock(&sig->WakeCondLock); ret = pthread_cond_wait(&sig->WakeCond, &sig->WakeCondLock); + if(!sig->used) ret = -EINVAL; pthread_mutex_unlock(&sig->WakeCondLock); + + if(!sig->used){ + pthread_cond_destroy(&sig->WakeCond); + pthread_mutex_destroy(&sig->WakeCondLock); + free(sig); + } return ret; }