etisserant@280: /** etisserant@280: * Linux specific code greg@329: **/ etisserant@280: greg@205: #include greg@205: #include greg@205: #include greg@205: #include greg@205: #include Edouard@3725: #include greg@329: #include edouard@568: #include Laurent@876: #include Edouard@3732: #ifdef REALTIME_LINUX Edouard@3732: #include Edouard@3732: #endif greg@205: Edouard@3740: #define _Log(level,text,...) \ Edouard@3740: {\ Edouard@3740: char mstr[256];\ Edouard@3740: snprintf(mstr, 255, text, ##__VA_ARGS__);\ Edouard@3740: LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ Edouard@3740: } Edouard@3740: Edouard@3740: #define _LogError(text,...) _Log(LOG_CRITICAL, text, ##__VA_ARGS__) Edouard@3740: #define _LogWarning(text,...) _Log(LOG_WARNING, text, ##__VA_ARGS__) Edouard@3740: Edouard@3726: static unsigned long __debug_tick; Edouard@3726: Edouard@3726: static pthread_t PLC_thread; Edouard@3726: static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER; Edouard@3726: static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER; Edouard@3726: static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER; Edouard@3726: static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER; Edouard@3726: Edouard@3726: static int PLC_shutdown = 0; etisserant@280: etisserant@236: long AtomicCompareExchange(long* atomicvar,long compared, long exchange) greg@205: { greg@205: return __sync_val_compare_and_swap(atomicvar, compared, exchange); greg@205: } Edouard@954: long long AtomicCompareExchange64(long long* atomicvar, long long compared, long long exchange) Edouard@954: { Edouard@954: return __sync_val_compare_and_swap(atomicvar, compared, exchange); Edouard@954: } greg@205: greg@205: void PLC_GetTime(IEC_TIME *CURRENT_TIME) greg@205: { Edouard@592: struct timespec tmp; Edouard@592: clock_gettime(CLOCK_REALTIME, &tmp); Edouard@592: CURRENT_TIME->tv_sec = tmp.tv_sec; Edouard@592: CURRENT_TIME->tv_nsec = tmp.tv_nsec; greg@205: } greg@205: Edouard@3726: static long long period_ns = 0; Edouard@3740: struct timespec next_cycle_time; Edouard@3726: Edouard@3726: static void inc_timespec(struct timespec *ts, unsigned long long value_ns) Edouard@3726: { Edouard@3727: long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns; Edouard@3726: #ifdef __lldiv_t_defined Edouard@3727: lldiv_t next_div = lldiv(next_ns, 1000000000); Edouard@3726: ts->tv_sec = next_div.quot; Edouard@3726: ts->tv_nsec = next_div.rem; Edouard@3726: #else Edouard@3726: ts->tv_sec = next_ns / 1000000000; Edouard@3726: ts->tv_nsec = next_ns % 1000000000; Edouard@3726: #endif Edouard@3726: } greg@205: edouard@518: void PLC_SetTimer(unsigned long long next, unsigned long long period) greg@205: { Edouard@3726: /* Edouard@3726: printf("SetTimer(%lld,%lld)\n",next, period); Edouard@3726: */ Edouard@3726: period_ns = period; Edouard@3740: clock_gettime(CLOCK_MONOTONIC, &next_cycle_time); Edouard@3740: inc_timespec(&next_cycle_time, next); Edouard@3726: // interrupt clock_nanpsleep Edouard@3726: pthread_kill(PLC_thread, SIGUSR1); greg@205: } Edouard@3727: greg@205: void catch_signal(int sig) greg@205: { greg@205: // signal(SIGTERM, catch_signal); greg@205: signal(SIGINT, catch_signal); greg@205: printf("Got Signal %d\n",sig); greg@205: exit(0); greg@205: } greg@205: Edouard@3726: void PLCThreadSignalHandler(int sig) Edouard@3726: { Edouard@3726: if (sig == SIGUSR2) Edouard@3726: pthread_exit(NULL); Edouard@3726: } Laurent@876: andrej@2173: int ForceSaveRetainReq(void) { andrej@2173: return PLC_shutdown; andrej@2173: } andrej@2173: Edouard@3740: #define MAX_JITTER period_ns/10 Edouard@3740: #define MIN_IDLE_TIME_NS 1000000 /* 1ms */ Edouard@3740: /* Macro to compare timespec, evaluate to True if a is past b */ Edouard@3740: #define timespec_gt(a,b) (a.tv_sec > b.tv_sec || (a.tv_sec == b.tv_sec && a.tv_nsec > b.tv_nsec)) Laurent@876: void PLC_thread_proc(void *arg) Laurent@876: { Edouard@3740: /* initialize next occurence and period */ Edouard@3740: period_ns = common_ticktime__; Edouard@3740: clock_gettime(CLOCK_MONOTONIC, &next_cycle_time); Edouard@3740: Laurent@876: while (!PLC_shutdown) { Edouard@3740: int res; Edouard@3740: struct timespec plc_end_time; Edouard@3740: int periods = 0; Edouard@3740: #ifdef REALTIME_LINUX Edouard@3740: struct timespec deadline_time; Edouard@3740: struct timespec plc_start_time; Edouard@3740: #endif Edouard@3740: Edouard@3726: // Sleep until next PLC run Edouard@3740: res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_cycle_time, NULL); Edouard@3727: if(res==EINTR){ Edouard@3727: continue; Edouard@3727: } Edouard@3727: if(res!=0){ Edouard@3740: _LogError("PLC thread timer returned error %d \n", res); Edouard@3727: return; Edouard@3727: } Edouard@3740: Edouard@3740: #ifdef REALTIME_LINUX Edouard@3740: // timer overrun detection Edouard@3740: clock_gettime(CLOCK_MONOTONIC, &plc_start_time); Edouard@3740: deadline_time=next_cycle_time; Edouard@3740: inc_timespec(&deadline_time, MAX_JITTER); Edouard@3740: if(timespec_gt(plc_start_time, deadline_time)){ Edouard@3740: _LogWarning("PLC thread woken up too late. PLC cyclic task interval is too small.\n"); Edouard@3740: } Edouard@3740: #endif Edouard@3740: Edouard@3726: PLC_GetTime(&__CURRENT_TIME); Laurent@876: __run(); Edouard@3740: Edouard@3740: // ensure next PLC cycle occurence is in the future Edouard@3740: clock_gettime(CLOCK_MONOTONIC, &plc_end_time); Edouard@3740: while(timespec_gt(plc_end_time, next_cycle_time)){ Edouard@3740: periods += 1; Edouard@3740: inc_timespec(&next_cycle_time, period_ns); Edouard@3740: } Edouard@3740: Edouard@3740: // plc execution time overrun detection Edouard@3740: if(periods > 1) { Edouard@3740: // Mitigate CPU hogging, in case of too small cyclic task interval: Edouard@3740: // - since cycle deadline already missed, better keep system responsive Edouard@3740: // - test if next cycle occurs after minimal idle Edouard@3740: // - enforce minimum idle time if not Edouard@3740: Edouard@3740: struct timespec earliest_possible_time = plc_end_time; Edouard@3740: inc_timespec(&earliest_possible_time, MIN_IDLE_TIME_NS); Edouard@3740: while(timespec_gt(earliest_possible_time, next_cycle_time)){ Edouard@3740: periods += 1; Edouard@3740: inc_timespec(&next_cycle_time, period_ns); Edouard@3740: } Edouard@3740: Edouard@3740: // increment tick count anyhow, so that task scheduling keeps consistent Edouard@3740: __tick+=periods-1; Edouard@3740: Edouard@3740: _LogWarning("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", periods); Edouard@3740: } Edouard@3740: } Edouard@3740: Laurent@876: pthread_exit(0); Laurent@876: } Laurent@876: etisserant@280: #define maxval(a,b) ((a>b)?a:b) greg@205: int startPLC(int argc,char **argv) greg@205: { greg@329: Edouard@3732: int ret; Edouard@3732: pthread_attr_t *pattr = NULL; Edouard@3732: Edouard@3732: #ifdef REALTIME_LINUX Edouard@3732: struct sched_param param; Edouard@3732: pthread_attr_t attr; Edouard@3732: Edouard@3732: /* Lock memory */ Edouard@3732: ret = mlockall(MCL_CURRENT|MCL_FUTURE); Edouard@3732: if(ret == -1) { Edouard@3732: _LogError("mlockall failed: %m\n"); Edouard@3732: return ret; Edouard@3732: } Edouard@3732: Edouard@3732: /* Initialize pthread attributes (default values) */ Edouard@3732: ret = pthread_attr_init(&attr); Edouard@3732: if (ret) { Edouard@3732: _LogError("init pthread attributes failed\n"); Edouard@3732: return ret; Edouard@3732: } Edouard@3732: Edouard@3732: /* Set scheduler policy and priority of pthread */ Edouard@3732: ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); Edouard@3732: if (ret) { Edouard@3732: _LogError("pthread setschedpolicy failed\n"); Edouard@3732: return ret; Edouard@3732: } Edouard@3732: param.sched_priority = PLC_THREAD_PRIORITY; Edouard@3732: ret = pthread_attr_setschedparam(&attr, ¶m); Edouard@3732: if (ret) { Edouard@3732: _LogError("pthread setschedparam failed\n"); Edouard@3732: return ret; Edouard@3732: } Edouard@3732: Edouard@3732: /* Use scheduling parameters of attr */ Edouard@3732: ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); Edouard@3732: if (ret) { Edouard@3732: _LogError("pthread setinheritsched failed\n"); Edouard@3732: return ret; Edouard@3732: } Edouard@3732: Edouard@3732: pattr = &attr; Edouard@3732: #endif Edouard@3732: Laurent@876: PLC_shutdown = 0; Laurent@876: lbessard@333: pthread_mutex_init(&debug_wait_mutex, NULL); edouard@462: pthread_mutex_init(&debug_mutex, NULL); lbessard@333: pthread_mutex_init(&python_wait_mutex, NULL); edouard@462: pthread_mutex_init(&python_mutex, NULL); greg@329: etisserant@280: pthread_mutex_lock(&debug_wait_mutex); etisserant@280: pthread_mutex_lock(&python_wait_mutex); etisserant@239: Edouard@3732: if((ret = __init(argc,argv)) == 0 ){ Edouard@3726: Edouard@3726: /* Signal to wakeup PLC thread when period changes */ Edouard@3726: signal(SIGUSR1, PLCThreadSignalHandler); Edouard@3726: /* Signal to end PLC thread */ Edouard@3726: signal(SIGUSR2, PLCThreadSignalHandler); greg@205: /* install signal handler for manual break */ greg@205: signal(SIGINT, catch_signal); Edouard@3726: Edouard@3732: ret = pthread_create(&PLC_thread, pattr, (void*) &PLC_thread_proc, NULL); Edouard@3732: if (ret) { Edouard@3732: _LogError("create pthread failed\n"); Edouard@3732: return ret; Edouard@3732: } greg@205: }else{ Edouard@3732: return ret; greg@205: } greg@205: return 0; greg@205: } greg@205: etisserant@239: int TryEnterDebugSection(void) etisserant@239: { edouard@462: if (pthread_mutex_trylock(&debug_mutex) == 0){ edouard@462: /* Only enter if debug active */ edouard@462: if(__DEBUG){ edouard@462: return 1; edouard@462: } edouard@483: pthread_mutex_unlock(&debug_mutex); edouard@462: } edouard@462: return 0; etisserant@239: } etisserant@235: etisserant@239: void LeaveDebugSection(void) etisserant@235: { etisserant@239: pthread_mutex_unlock(&debug_mutex); etisserant@235: } etisserant@235: greg@205: int stopPLC() greg@205: { greg@205: /* Stop the PLC */ Laurent@876: PLC_shutdown = 1; Edouard@3726: /* Order PLCThread to exit */ Edouard@3726: pthread_kill(PLC_thread, SIGUSR2); Edouard@3726: pthread_join(PLC_thread, NULL); greg@205: __cleanup(); greg@329: pthread_mutex_destroy(&debug_wait_mutex); edouard@483: pthread_mutex_destroy(&debug_mutex); greg@329: pthread_mutex_destroy(&python_wait_mutex); edouard@483: pthread_mutex_destroy(&python_mutex); laurent@386: return 0; greg@205: } greg@205: laurent@397: extern unsigned long __tick; ed@446: ed@446: int WaitDebugData(unsigned long *tick) greg@205: { Edouard@617: int res; Laurent@876: if (PLC_shutdown) return 1; Edouard@617: /* Wait signal from PLC thread */ Edouard@617: res = pthread_mutex_lock(&debug_wait_mutex); ed@446: *tick = __debug_tick; Edouard@617: return res; greg@205: } greg@329: greg@205: /* Called by PLC thread when debug_publish finished greg@205: * This is supposed to unlock debugger thread in WaitDebugData*/ greg@205: void InitiateDebugTransfer() greg@205: { etisserant@239: /* remember tick */ etisserant@227: __debug_tick = __tick; etisserant@239: /* signal debugger thread it can read data */ etisserant@280: pthread_mutex_unlock(&debug_wait_mutex); greg@205: } etisserant@239: Edouard@614: int suspendDebug(int disable) edouard@462: { etisserant@239: /* Prevent PLC to enter debug code */ etisserant@239: pthread_mutex_lock(&debug_mutex); edouard@462: /*__DEBUG is protected by this mutex */ edouard@462: __DEBUG = !disable; laurent@485: if (disable) Edouard@3726: pthread_mutex_unlock(&debug_mutex); Edouard@614: return 0; etisserant@239: } etisserant@239: etisserant@280: void resumeDebug(void) etisserant@239: { etisserant@290: __DEBUG = 1; etisserant@239: /* Let PLC enter debug code */ etisserant@239: pthread_mutex_unlock(&debug_mutex); etisserant@239: } etisserant@239: etisserant@280: /* from plc_python.c */ etisserant@280: int WaitPythonCommands(void) etisserant@280: { etisserant@280: /* Wait signal from PLC thread */ greg@329: return pthread_mutex_lock(&python_wait_mutex); etisserant@280: } greg@329: etisserant@280: /* Called by PLC thread on each new python command*/ etisserant@280: void UnBlockPythonCommands(void) etisserant@280: { Edouard@3334: /* signal python thread it can read data */ etisserant@280: pthread_mutex_unlock(&python_wait_mutex); etisserant@280: } etisserant@280: etisserant@280: int TryLockPython(void) etisserant@280: { etisserant@280: return pthread_mutex_trylock(&python_mutex) == 0; etisserant@280: } etisserant@280: etisserant@280: void UnLockPython(void) etisserant@280: { etisserant@280: pthread_mutex_unlock(&python_mutex); etisserant@280: } etisserant@280: etisserant@280: void LockPython(void) etisserant@280: { etisserant@280: pthread_mutex_lock(&python_mutex); etisserant@280: } Edouard@2820: edouard@3294: struct RT_to_nRT_signal_s { Edouard@3725: int used; edouard@3294: pthread_cond_t WakeCond; edouard@3294: pthread_mutex_t WakeCondLock; edouard@3294: }; edouard@3294: edouard@3294: typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t; edouard@3294: edouard@3294: #define _LogAndReturnNull(text) \ edouard@3294: {\ Edouard@3726: char mstr[256] = text " for ";\ edouard@3294: strncat(mstr, name, 255);\ edouard@3294: LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ edouard@3294: return NULL;\ edouard@3294: } edouard@3294: edouard@3294: void *create_RT_to_nRT_signal(char* name){ edouard@3294: RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t)); edouard@3294: Edouard@3726: if(!sig) Edouard@3726: _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal"); edouard@3294: Edouard@3725: sig->used = 1; edouard@3294: pthread_cond_init(&sig->WakeCond, NULL); edouard@3294: pthread_mutex_init(&sig->WakeCondLock, NULL); edouard@3294: edouard@3294: return (void*)sig; edouard@3294: } edouard@3294: edouard@3294: void delete_RT_to_nRT_signal(void* handle){ edouard@3294: RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle; edouard@3294: Edouard@3725: pthread_mutex_lock(&sig->WakeCondLock); Edouard@3725: sig->used = 0; Edouard@3725: pthread_cond_signal(&sig->WakeCond); Edouard@3725: pthread_mutex_unlock(&sig->WakeCondLock); edouard@3294: } edouard@3294: edouard@3294: int wait_RT_to_nRT_signal(void* handle){ edouard@3294: int ret; edouard@3294: RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle; edouard@3294: pthread_mutex_lock(&sig->WakeCondLock); edouard@3294: ret = pthread_cond_wait(&sig->WakeCond, &sig->WakeCondLock); Edouard@3725: if(!sig->used) ret = -EINVAL; edouard@3294: pthread_mutex_unlock(&sig->WakeCondLock); Edouard@3725: Edouard@3725: if(!sig->used){ Edouard@3725: pthread_cond_destroy(&sig->WakeCond); Edouard@3725: pthread_mutex_destroy(&sig->WakeCondLock); Edouard@3725: free(sig); Edouard@3725: } edouard@3294: return ret; edouard@3294: } edouard@3294: edouard@3294: int unblock_RT_to_nRT_signal(void* handle){ edouard@3294: RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle; edouard@3294: return pthread_cond_signal(&sig->WakeCond); edouard@3294: } edouard@3294: edouard@3295: void nRT_reschedule(void){ edouard@3295: sched_yield(); edouard@3295: }