Mercurial Mercurial > beremiz / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
targets/Linux/plc_Linux_main.c
changeset 3726 516779f11803
parent 3725 0043e2b9dbec
child 3727 265fc8001d0a
equal deleted inserted replaced
3725:0043e2b9dbec 3726:516779f11803 
    10 #include <errno.h>
 
    10 #include <errno.h>
 
    11 #include <pthread.h>
 
    11 #include <pthread.h>
 
    12 #include <locale.h>
 
    12 #include <locale.h>
 
    13 #include <semaphore.h>
 
    13 #include <semaphore.h>
 
    14 
    14 
    15 static sem_t Run_PLC;
 
    15 static unsigned long __debug_tick;
 
       
    16 
       
    17 static pthread_t PLC_thread;
 
       
    18 static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    19 static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    20 static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    21 static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    22 
       
    23 static int PLC_shutdown = 0;
 
    16 
    24 
    17 long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
 
    25 long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
 
    18 {
 
    26 {
 
    19     return __sync_val_compare_and_swap(atomicvar, compared, exchange);
 
    27     return __sync_val_compare_and_swap(atomicvar, compared, exchange);
 
    20 }
 
    28 }
 
    29     clock_gettime(CLOCK_REALTIME, &tmp);
 
    37     clock_gettime(CLOCK_REALTIME, &tmp);
 
    30     CURRENT_TIME->tv_sec = tmp.tv_sec;
 
    38     CURRENT_TIME->tv_sec = tmp.tv_sec;
 
    31     CURRENT_TIME->tv_nsec = tmp.tv_nsec;
 
    39     CURRENT_TIME->tv_nsec = tmp.tv_nsec;
 
    32 }
 
    40 }
 
    33 
    41 
    34 void PLC_timer_notify(sigval_t val)
 
    42 static long long period_ns = 0;
 
    35 {
 
    43 struct timespec next_abs_time;
 
    36     PLC_GetTime(&__CURRENT_TIME);
 
    44 
    37     sem_post(&Run_PLC);
 
    45 static void inc_timespec(struct timespec *ts, unsigned long long value_ns)
 
    38 }
 
    46 {
 
    39 
    47 #ifdef __lldiv_t_defined
 
    40 timer_t PLC_timer;
 
    48     lldiv_t next_div = lldiv(ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns, 1000000000);
 
       
    49     ts->tv_sec = next_div.quot;
 
       
    50     ts->tv_nsec = next_div.rem;
 
       
    51 #else
 
       
    52     long long next_ns = ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns;
 
       
    53     ts->tv_sec = next_ns / 1000000000;
 
       
    54     ts->tv_nsec = next_ns % 1000000000;
 
       
    55 #endif
 
       
    56 }
 
    41 
    57 
    42 void PLC_SetTimer(unsigned long long next, unsigned long long period)
 
    58 void PLC_SetTimer(unsigned long long next, unsigned long long period)
 
    43 {
 
    59 {
 
    44     struct itimerspec timerValues;
 
    60     /*
 
    45 	/*
 
    61     printf("SetTimer(%lld,%lld)\n",next, period);
 
    46 	printf("SetTimer(%lld,%lld)\n",next, period);
 
    62     */
 
    47 	*/
 
    63     period_ns = period;
 
    48     memset (&timerValues, 0, sizeof (struct itimerspec));
 
    64     clock_gettime(CLOCK_MONOTONIC, &next_abs_time);
 
    49 	{
 
    65     inc_timespec(&next_abs_time, next);
 
    50 #ifdef __lldiv_t_defined
 
    66     // interrupt clock_nanpsleep
 
    51 		lldiv_t nxt_div = lldiv(next, 1000000000);
 
    67     pthread_kill(PLC_thread, SIGUSR1);
 
    52 		lldiv_t period_div = lldiv(period, 1000000000);
 
       
    53 	    timerValues.it_value.tv_sec = nxt_div.quot;
 
       
    54 	    timerValues.it_value.tv_nsec = nxt_div.rem;
 
       
    55 	    timerValues.it_interval.tv_sec = period_div.quot;
 
       
    56 	    timerValues.it_interval.tv_nsec = period_div.rem;
 
       
    57 #else
 
       
    58 	    timerValues.it_value.tv_sec = next / 1000000000;
 
       
    59 	    timerValues.it_value.tv_nsec = next % 1000000000;
 
       
    60 	    timerValues.it_interval.tv_sec = period / 1000000000;
 
       
    61 	    timerValues.it_interval.tv_nsec = period % 1000000000;
 
       
    62 #endif
 
       
    63 	}
 
       
    64     timer_settime (PLC_timer, 0, &timerValues, NULL);
 
       
    65 }
 
    68 }
 
    66 //
 
    69 //
 
    67 void catch_signal(int sig)
 
    70 void catch_signal(int sig)
 
    68 {
 
    71 {
 
    69 //  signal(SIGTERM, catch_signal);
 
    72 //  signal(SIGTERM, catch_signal);
 
    70   signal(SIGINT, catch_signal);
 
    73   signal(SIGINT, catch_signal);
 
    71   printf("Got Signal %d\n",sig);
 
    74   printf("Got Signal %d\n",sig);
 
    72   exit(0);
 
    75   exit(0);
 
    73 }
 
    76 }
 
    74 
    77 
    75 
    78 void PLCThreadSignalHandler(int sig)
 
    76 static unsigned long __debug_tick;
 
    79 {
 
    77 
    80     if (sig == SIGUSR2)
 
    78 pthread_t PLC_thread;
 
    81         pthread_exit(NULL);
 
    79 static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
    82 }
 
    80 static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    81 static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    82 static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    83 
       
    84 int PLC_shutdown = 0;
 
       
    85 
    83 
    86 int ForceSaveRetainReq(void) {
 
    84 int ForceSaveRetainReq(void) {
 
    87     return PLC_shutdown;
 
    85     return PLC_shutdown;
 
    88 }
 
    86 }
 
    89 
    87 
    90 void PLC_thread_proc(void *arg)
 
    88 void PLC_thread_proc(void *arg)
 
    91 {
 
    89 {
 
    92     while (!PLC_shutdown) {
 
    90     while (!PLC_shutdown) {
 
    93         sem_wait(&Run_PLC);
 
    91         // Sleep until next PLC run
 
       
    92         // TODO check result of clock_nanosleep and wait again or exit eventually
 
       
    93         int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL);
 
       
    94         if(res==EINTR) continue;
 
       
    95         if(res!=0) return;
 
       
    96         PLC_GetTime(&__CURRENT_TIME);
 
    94         __run();
 
    97         __run();
 
       
    98         inc_timespec(&next_abs_time, period_ns);
 
    95     }
 
    99     }
 
    96     pthread_exit(0);
 
   100     pthread_exit(0);
 
    97 }
 
   101 }
 
    98 
   102 
    99 #define maxval(a,b) ((a>b)?a:b)
 
   103 #define maxval(a,b) ((a>b)?a:b)
 
   100 int startPLC(int argc,char **argv)
 
   104 int startPLC(int argc,char **argv)
 
   101 {
 
   105 {
 
   102     struct sigevent sigev;
 
       
   103     setlocale(LC_NUMERIC, "C");
 
   106     setlocale(LC_NUMERIC, "C");
 
   104 
   107 
   105     PLC_shutdown = 0;
 
   108     PLC_shutdown = 0;
 
   106 
   109 
   107     sem_init(&Run_PLC, 0, 0);
 
       
   108 
       
   109     pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL);
 
   110     pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL);
 
   110 
       
   111     memset (&sigev, 0, sizeof (struct sigevent));
 
       
   112     sigev.sigev_value.sival_int = 0;
 
       
   113     sigev.sigev_notify = SIGEV_THREAD;
 
       
   114     sigev.sigev_notify_attributes = NULL;
 
       
   115     sigev.sigev_notify_function = PLC_timer_notify;
 
       
   116 
   111 
   117     pthread_mutex_init(&debug_wait_mutex, NULL);
 
   112     pthread_mutex_init(&debug_wait_mutex, NULL);
 
   118     pthread_mutex_init(&debug_mutex, NULL);
 
   113     pthread_mutex_init(&debug_mutex, NULL);
 
   119     pthread_mutex_init(&python_wait_mutex, NULL);
 
   114     pthread_mutex_init(&python_wait_mutex, NULL);
 
   120     pthread_mutex_init(&python_mutex, NULL);
 
   115     pthread_mutex_init(&python_mutex, NULL);
 
   121 
   116 
   122     pthread_mutex_lock(&debug_wait_mutex);
 
   117     pthread_mutex_lock(&debug_wait_mutex);
 
   123     pthread_mutex_lock(&python_wait_mutex);
 
   118     pthread_mutex_lock(&python_wait_mutex);
 
   124 
   119 
   125     timer_create (CLOCK_MONOTONIC, &sigev, &PLC_timer);
 
       
   126     if(  __init(argc,argv) == 0 ){
 
   120     if(  __init(argc,argv) == 0 ){
 
   127         PLC_SetTimer(common_ticktime__,common_ticktime__);
 
   121 
   128 
   122         /* Signal to wakeup PLC thread when period changes */
 
       
   123         signal(SIGUSR1, PLCThreadSignalHandler);
 
       
   124         /* Signal to end PLC thread */
 
       
   125         signal(SIGUSR2, PLCThreadSignalHandler);
 
   129         /* install signal handler for manual break */
 
   126         /* install signal handler for manual break */
 
   130         signal(SIGINT, catch_signal);
 
   127         signal(SIGINT, catch_signal);
 
       
   128 
       
   129         PLC_SetTimer(common_ticktime__,common_ticktime__);
 
   131     }else{
 
   130     }else{
 
   132         return 1;
 
   131         return 1;
 
   133     }
 
   132     }
 
   134     return 0;
 
   133     return 0;
 
   135 }
 
   134 }
 
   153 
   152 
   154 int stopPLC()
 
   153 int stopPLC()
 
   155 {
 
   154 {
 
   156     /* Stop the PLC */
 
   155     /* Stop the PLC */
 
   157     PLC_shutdown = 1;
 
   156     PLC_shutdown = 1;
 
   158     sem_post(&Run_PLC);
 
   157     /* Order PLCThread to exit */
 
   159     PLC_SetTimer(0,0);
 
   158     pthread_kill(PLC_thread, SIGUSR2);
 
   160 	pthread_join(PLC_thread, NULL);
 
   159     pthread_join(PLC_thread, NULL);
 
   161 	sem_destroy(&Run_PLC);
 
       
   162     timer_delete (PLC_timer);
 
       
   163     __cleanup();
 
   160     __cleanup();
 
   164     pthread_mutex_destroy(&debug_wait_mutex);
 
   161     pthread_mutex_destroy(&debug_wait_mutex);
 
   165     pthread_mutex_destroy(&debug_mutex);
 
   162     pthread_mutex_destroy(&debug_mutex);
 
   166     pthread_mutex_destroy(&python_wait_mutex);
 
   163     pthread_mutex_destroy(&python_wait_mutex);
 
   167     pthread_mutex_destroy(&python_mutex);
 
   164     pthread_mutex_destroy(&python_mutex);
 
   195     /* Prevent PLC to enter debug code */
 
   192     /* Prevent PLC to enter debug code */
 
   196     pthread_mutex_lock(&debug_mutex);
 
   193     pthread_mutex_lock(&debug_mutex);
 
   197     /*__DEBUG is protected by this mutex */
 
   194     /*__DEBUG is protected by this mutex */
 
   198     __DEBUG = !disable;
 
   195     __DEBUG = !disable;
 
   199     if (disable)
 
   196     if (disable)
 
   200     	pthread_mutex_unlock(&debug_mutex);
 
   197         pthread_mutex_unlock(&debug_mutex);
 
   201     return 0;
 
   198     return 0;
 
   202 }
 
   199 }
 
   203 
   200 
   204 void resumeDebug(void)
 
   201 void resumeDebug(void)
 
   205 {
 
   202 {
 
   245 
   242 
   246 typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;
 
   243 typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;
 
   247 
   244 
   248 #define _LogAndReturnNull(text) \
 
   245 #define _LogAndReturnNull(text) \
 
   249     {\
 
   246     {\
 
   250     	char mstr[256] = text " for ";\
 
   247         char mstr[256] = text " for ";\
 
   251         strncat(mstr, name, 255);\
 
   248         strncat(mstr, name, 255);\
 
   252         LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\
 
   249         LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\
 
   253         return NULL;\
 
   250         return NULL;\
 
   254     }
 
   251     }
 
   255 
   252 
   256 void *create_RT_to_nRT_signal(char* name){
 
   253 void *create_RT_to_nRT_signal(char* name){
 
   257     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t));
 
   254     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t));
 
   258 
   255 
   259     if(!sig) 
   256     if(!sig)
 
   260     	_LogAndReturnNull("Failed allocating memory for RT_to_nRT signal");
 
   257         _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal");
 
   261 
   258 
   262     sig->used = 1;
 
   259     sig->used = 1;
 
   263     pthread_cond_init(&sig->WakeCond, NULL);
 
   260     pthread_cond_init(&sig->WakeCond, NULL);
 
   264     pthread_mutex_init(&sig->WakeCondLock, NULL);
 
   261     pthread_mutex_init(&sig->WakeCondLock, NULL);
 
   265 
   262
beremiz