etisserant@280: /**
etisserant@280: * Linux specific code
greg@329: **/
etisserant@280:
greg@205: #include <stdio.h>
greg@205: #include <string.h>
greg@205: #include <time.h>
greg@205: #include <signal.h>
greg@205: #include <stdlib.h>
Edouard@3725: #include <errno.h>
greg@329: #include <pthread.h>
edouard@568: #include <locale.h>
Laurent@876: #include <semaphore.h>
Edouard@3732: #ifdef REALTIME_LINUX
Edouard@3732: #include <sys/mman.h>
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@3953: LogMessage(level, 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@3947: static unsigned int __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@3748: // BEREMIZ_TEST_CYCLES is defined in tests that need to emulate time:
edouard@3748: // - all BEREMIZ_TEST_CYCLES cycles are executed in a row with no pause
edouard@3748: // - __CURRENT_TIME is incremented each cycle according to emulated cycle period
edouard@3748:
edouard@3748: #ifndef BEREMIZ_TEST_CYCLES
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@3748: #endif // BEREMIZ_TEST_CYCLES
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@3748: #ifdef BEREMIZ_TEST_CYCLES
edouard@3748: #define xstr(s) str(s)
edouard@3748: #define str(arg) #arg
edouard@3748: // fake current time
edouard@3748: __CURRENT_TIME.tv_sec = next_cycle_time.tv_sec;
edouard@3748: __CURRENT_TIME.tv_nsec = next_cycle_time.tv_nsec;
edouard@3748: // exit loop when enough cycles
edouard@3748: if(__tick >= BEREMIZ_TEST_CYCLES) {
edouard@3748: _LogWarning("TEST PLC thread ended after "xstr(BEREMIZ_TEST_CYCLES)" cycles.\n");
edouard@3748: // After pre-defined test cycles count, PLC thread exits.
edouard@3748: // Remaining PLC runtime is expected to be cleaned-up/killed by test script
edouard@3748: return;
edouard@3748: }
edouard@3748: #else
Edouard@3726: PLC_GetTime(&__CURRENT_TIME);
edouard@3748: #endif
Laurent@876: __run();
Edouard@3740:
edouard@3748: #ifndef BEREMIZ_TEST_CYCLES
Edouard@3740: // ensure next PLC cycle occurence is in the future
Edouard@3740: clock_gettime(CLOCK_MONOTONIC, &plc_end_time);
edouard@3748: while(timespec_gt(plc_end_time, next_cycle_time))
edouard@3748: #endif
edouard@3748: {
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:
edouard@3947: extern unsigned int __tick;
edouard@3947:
edouard@3947: int WaitDebugData(unsigned int *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: }