--- 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 <time.h>
 #include <signal.h>
 #include <stdlib.h>
+#include <errno.h>
 #include <pthread.h>
 #include <locale.h>
 #include <semaphore.h>
 
-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;
 }