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
branchwxPython4
changeset 3728 743a384f1bbb
parent 3727 265fc8001d0a
child 3731 549763a28934
equal deleted inserted replaced
3724:ad3a05cf2066 3728:743a384f1bbb 
     5 #include <stdio.h>
 
     5 #include <stdio.h>
 
     6 #include <string.h>
 
     6 #include <string.h>
 
     7 #include <time.h>
 
     7 #include <time.h>
 
     8 #include <signal.h>
 
     8 #include <signal.h>
 
     9 #include <stdlib.h>
 
     9 #include <stdlib.h>
 
       
    10 #include <errno.h>
 
    10 #include <pthread.h>
 
    11 #include <pthread.h>
 
    11 #include <locale.h>
 
    12 #include <locale.h>
 
    12 #include <semaphore.h>
 
    13 #include <semaphore.h>
 
    13 
    14 
    14 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;
 
    15 
    24 
    16 long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
 
    25 long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
 
    17 {
 
    26 {
 
    18     return __sync_val_compare_and_swap(atomicvar, compared, exchange);
 
    27     return __sync_val_compare_and_swap(atomicvar, compared, exchange);
 
    19 }
 
    28 }
 
    28     clock_gettime(CLOCK_REALTIME, &tmp);
 
    37     clock_gettime(CLOCK_REALTIME, &tmp);
 
    29     CURRENT_TIME->tv_sec = tmp.tv_sec;
 
    38     CURRENT_TIME->tv_sec = tmp.tv_sec;
 
    30     CURRENT_TIME->tv_nsec = tmp.tv_nsec;
 
    39     CURRENT_TIME->tv_nsec = tmp.tv_nsec;
 
    31 }
 
    40 }
 
    32 
    41 
    33 void PLC_timer_notify(sigval_t val)
 
    42 static long long period_ns = 0;
 
    34 {
 
    43 struct timespec next_abs_time;
 
    35     PLC_GetTime(&__CURRENT_TIME);
 
    44 
    36     sem_post(&Run_PLC);
 
    45 static void inc_timespec(struct timespec *ts, unsigned long long value_ns)
 
    37 }
 
    46 {
 
    38 
    47     long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns;
 
    39 timer_t PLC_timer;
 
    48 #ifdef __lldiv_t_defined
 
       
    49     lldiv_t next_div = lldiv(next_ns, 1000000000);
 
       
    50     ts->tv_sec = next_div.quot;
 
       
    51     ts->tv_nsec = next_div.rem;
 
       
    52 #else
 
       
    53     ts->tv_sec = next_ns / 1000000000;
 
       
    54     ts->tv_nsec = next_ns % 1000000000;
 
       
    55 #endif
 
       
    56 }
 
    40 
    57 
    41 void PLC_SetTimer(unsigned long long next, unsigned long long period)
 
    58 void PLC_SetTimer(unsigned long long next, unsigned long long period)
 
    42 {
 
    59 {
 
    43     struct itimerspec timerValues;
 
    60     /*
 
    44 	/*
 
    61     printf("SetTimer(%lld,%lld)\n",next, period);
 
    45 	printf("SetTimer(%lld,%lld)\n",next, period);
 
    62     */
 
    46 	*/
 
    63     period_ns = period;
 
    47     memset (&timerValues, 0, sizeof (struct itimerspec));
 
    64     clock_gettime(CLOCK_MONOTONIC, &next_abs_time);
 
    48 	{
 
    65     inc_timespec(&next_abs_time, next);
 
    49 #ifdef __lldiv_t_defined
 
    66     // interrupt clock_nanpsleep
 
    50 		lldiv_t nxt_div = lldiv(next, 1000000000);
 
    67     pthread_kill(PLC_thread, SIGUSR1);
 
    51 		lldiv_t period_div = lldiv(period, 1000000000);
 
    68 }
 
    52 	    timerValues.it_value.tv_sec = nxt_div.quot;
 
    69 
    53 	    timerValues.it_value.tv_nsec = nxt_div.rem;
 
       
    54 	    timerValues.it_interval.tv_sec = period_div.quot;
 
       
    55 	    timerValues.it_interval.tv_nsec = period_div.rem;
 
       
    56 #else
 
       
    57 	    timerValues.it_value.tv_sec = next / 1000000000;
 
       
    58 	    timerValues.it_value.tv_nsec = next % 1000000000;
 
       
    59 	    timerValues.it_interval.tv_sec = period / 1000000000;
 
       
    60 	    timerValues.it_interval.tv_nsec = period % 1000000000;
 
       
    61 #endif
 
       
    62 	}
 
       
    63     timer_settime (PLC_timer, 0, &timerValues, NULL);
 
       
    64 }
 
       
    65 //
 
       
    66 void catch_signal(int sig)
 
    70 void catch_signal(int sig)
 
    67 {
 
    71 {
 
    68 //  signal(SIGTERM, catch_signal);
 
    72 //  signal(SIGTERM, catch_signal);
 
    69   signal(SIGINT, catch_signal);
 
    73   signal(SIGINT, catch_signal);
 
    70   printf("Got Signal %d\n",sig);
 
    74   printf("Got Signal %d\n",sig);
 
    71   exit(0);
 
    75   exit(0);
 
    72 }
 
    76 }
 
    73 
    77 
    74 
    78 void PLCThreadSignalHandler(int sig)
 
    75 static unsigned long __debug_tick;
 
    79 {
 
    76 
    80     if (sig == SIGUSR2)
 
    77 pthread_t PLC_thread;
 
    81         pthread_exit(NULL);
 
    78 static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
    82 }
 
    79 static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    80 static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    81 static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;
 
       
    82 
       
    83 int PLC_shutdown = 0;
 
       
    84 
    83 
    85 int ForceSaveRetainReq(void) {
 
    84 int ForceSaveRetainReq(void) {
 
    86     return PLC_shutdown;
 
    85     return PLC_shutdown;
 
    87 }
 
    86 }
 
    88 
    87 
    89 void PLC_thread_proc(void *arg)
 
    88 void PLC_thread_proc(void *arg)
 
    90 {
 
    89 {
 
    91     while (!PLC_shutdown) {
 
    90     while (!PLC_shutdown) {
 
    92         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){
 
       
    95             continue;
 
       
    96         }
 
       
    97         if(res!=0){
 
       
    98             printf("PLC thread died with error %d \n", res);
 
       
    99             return;
 
       
   100         }
 
       
   101         PLC_GetTime(&__CURRENT_TIME);
 
    93         __run();
 
   102         __run();
 
       
   103         inc_timespec(&next_abs_time, period_ns);
 
    94     }
 
   104     }
 
    95     pthread_exit(0);
 
   105     pthread_exit(0);
 
    96 }
 
   106 }
 
    97 
   107 
    98 #define maxval(a,b) ((a>b)?a:b)
 
   108 #define maxval(a,b) ((a>b)?a:b)
 
    99 int startPLC(int argc,char **argv)
 
   109 int startPLC(int argc,char **argv)
 
   100 {
 
   110 {
 
   101     struct sigevent sigev;
 
       
   102     setlocale(LC_NUMERIC, "C");
 
   111     setlocale(LC_NUMERIC, "C");
 
   103 
   112 
   104     PLC_shutdown = 0;
 
   113     PLC_shutdown = 0;
 
   105 
       
   106     sem_init(&Run_PLC, 0, 0);
 
       
   107 
       
   108     pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL);
 
       
   109 
       
   110     memset (&sigev, 0, sizeof (struct sigevent));
 
       
   111     sigev.sigev_value.sival_int = 0;
 
       
   112     sigev.sigev_notify = SIGEV_THREAD;
 
       
   113     sigev.sigev_notify_attributes = NULL;
 
       
   114     sigev.sigev_notify_function = PLC_timer_notify;
 
       
   115 
   114 
   116     pthread_mutex_init(&debug_wait_mutex, NULL);
 
   115     pthread_mutex_init(&debug_wait_mutex, NULL);
 
   117     pthread_mutex_init(&debug_mutex, NULL);
 
   116     pthread_mutex_init(&debug_mutex, NULL);
 
   118     pthread_mutex_init(&python_wait_mutex, NULL);
 
   117     pthread_mutex_init(&python_wait_mutex, NULL);
 
   119     pthread_mutex_init(&python_mutex, NULL);
 
   118     pthread_mutex_init(&python_mutex, NULL);
 
   120 
   119 
   121     pthread_mutex_lock(&debug_wait_mutex);
 
   120     pthread_mutex_lock(&debug_wait_mutex);
 
   122     pthread_mutex_lock(&python_wait_mutex);
 
   121     pthread_mutex_lock(&python_wait_mutex);
 
   123 
   122 
   124     timer_create (CLOCK_MONOTONIC, &sigev, &PLC_timer);
 
       
   125     if(  __init(argc,argv) == 0 ){
 
   123     if(  __init(argc,argv) == 0 ){
 
   126         PLC_SetTimer(common_ticktime__,common_ticktime__);
 
   124 
   127 
   125         /* Signal to wakeup PLC thread when period changes */
 
       
   126         signal(SIGUSR1, PLCThreadSignalHandler);
 
       
   127         /* Signal to end PLC thread */
 
       
   128         signal(SIGUSR2, PLCThreadSignalHandler);
 
   128         /* install signal handler for manual break */
 
   129         /* install signal handler for manual break */
 
   129         signal(SIGINT, catch_signal);
 
   130         signal(SIGINT, catch_signal);
 
       
   131 
       
   132         /* initialize next occurence and period */
 
       
   133         period_ns = common_ticktime__;
 
       
   134         clock_gettime(CLOCK_MONOTONIC, &next_abs_time);
 
       
   135 
       
   136         pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL);
 
   130     }else{
 
   137     }else{
 
   131         return 1;
 
   138         return 1;
 
   132     }
 
   139     }
 
   133     return 0;
 
   140     return 0;
 
   134 }
 
   141 }
 
   152 
   159 
   153 int stopPLC()
 
   160 int stopPLC()
 
   154 {
 
   161 {
 
   155     /* Stop the PLC */
 
   162     /* Stop the PLC */
 
   156     PLC_shutdown = 1;
 
   163     PLC_shutdown = 1;
 
   157     sem_post(&Run_PLC);
 
   164     /* Order PLCThread to exit */
 
   158     PLC_SetTimer(0,0);
 
   165     pthread_kill(PLC_thread, SIGUSR2);
 
   159 	pthread_join(PLC_thread, NULL);
 
   166     pthread_join(PLC_thread, NULL);
 
   160 	sem_destroy(&Run_PLC);
 
       
   161     timer_delete (PLC_timer);
 
       
   162     __cleanup();
 
   167     __cleanup();
 
   163     pthread_mutex_destroy(&debug_wait_mutex);
 
   168     pthread_mutex_destroy(&debug_wait_mutex);
 
   164     pthread_mutex_destroy(&debug_mutex);
 
   169     pthread_mutex_destroy(&debug_mutex);
 
   165     pthread_mutex_destroy(&python_wait_mutex);
 
   170     pthread_mutex_destroy(&python_wait_mutex);
 
   166     pthread_mutex_destroy(&python_mutex);
 
   171     pthread_mutex_destroy(&python_mutex);
 
   194     /* Prevent PLC to enter debug code */
 
   199     /* Prevent PLC to enter debug code */
 
   195     pthread_mutex_lock(&debug_mutex);
 
   200     pthread_mutex_lock(&debug_mutex);
 
   196     /*__DEBUG is protected by this mutex */
 
   201     /*__DEBUG is protected by this mutex */
 
   197     __DEBUG = !disable;
 
   202     __DEBUG = !disable;
 
   198     if (disable)
 
   203     if (disable)
 
   199     	pthread_mutex_unlock(&debug_mutex);
 
   204         pthread_mutex_unlock(&debug_mutex);
 
   200     return 0;
 
   205     return 0;
 
   201 }
 
   206 }
 
   202 
   207 
   203 void resumeDebug(void)
 
   208 void resumeDebug(void)
 
   204 {
 
   209 {
 
   235 {
 
   240 {
 
   236     pthread_mutex_lock(&python_mutex);
 
   241     pthread_mutex_lock(&python_mutex);
 
   237 }
 
   242 }
 
   238 
   243 
   239 struct RT_to_nRT_signal_s {
 
   244 struct RT_to_nRT_signal_s {
 
       
   245     int used;
 
   240     pthread_cond_t WakeCond;
 
   246     pthread_cond_t WakeCond;
 
   241     pthread_mutex_t WakeCondLock;
 
   247     pthread_mutex_t WakeCondLock;
 
   242 };
 
   248 };
 
   243 
   249 
   244 typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;
 
   250 typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;
 
   245 
   251 
   246 #define _LogAndReturnNull(text) \
 
   252 #define _LogAndReturnNull(text) \
 
   247     {\
 
   253     {\
 
   248     	char mstr[256] = text " for ";\
 
   254         char mstr[256] = text " for ";\
 
   249         strncat(mstr, name, 255);\
 
   255         strncat(mstr, name, 255);\
 
   250         LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\
 
   256         LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\
 
   251         return NULL;\
 
   257         return NULL;\
 
   252     }
 
   258     }
 
   253 
   259 
   254 void *create_RT_to_nRT_signal(char* name){
 
   260 void *create_RT_to_nRT_signal(char* name){
 
   255     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t));
 
   261     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t));
 
   256 
   262 
   257     if(!sig) 
   263     if(!sig)
 
   258     	_LogAndReturnNull("Failed allocating memory for RT_to_nRT signal");
 
   264         _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal");
 
   259 
   265 
       
   266     sig->used = 1;
 
   260     pthread_cond_init(&sig->WakeCond, NULL);
 
   267     pthread_cond_init(&sig->WakeCond, NULL);
 
   261     pthread_mutex_init(&sig->WakeCondLock, NULL);
 
   268     pthread_mutex_init(&sig->WakeCondLock, NULL);
 
   262 
   269 
   263     return (void*)sig;
 
   270     return (void*)sig;
 
   264 }
 
   271 }
 
   265 
   272 
   266 void delete_RT_to_nRT_signal(void* handle){
 
   273 void delete_RT_to_nRT_signal(void* handle){
 
   267     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
 
   274     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
 
   268 
   275 
   269     pthread_cond_destroy(&sig->WakeCond);
 
   276     pthread_mutex_lock(&sig->WakeCondLock);
 
   270     pthread_mutex_destroy(&sig->WakeCondLock);
 
   277     sig->used = 0;
 
   271 
   278     pthread_cond_signal(&sig->WakeCond);
 
   272     free(sig);
 
   279     pthread_mutex_unlock(&sig->WakeCondLock);
 
   273 }
 
   280 }
 
   274 
   281 
   275 int wait_RT_to_nRT_signal(void* handle){
 
   282 int wait_RT_to_nRT_signal(void* handle){
 
   276     int ret;
 
   283     int ret;
 
   277     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
 
   284     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
 
   278     pthread_mutex_lock(&sig->WakeCondLock);
 
   285     pthread_mutex_lock(&sig->WakeCondLock);
 
   279     ret = pthread_cond_wait(&sig->WakeCond, &sig->WakeCondLock);
 
   286     ret = pthread_cond_wait(&sig->WakeCond, &sig->WakeCondLock);
 
       
   287     if(!sig->used) ret = -EINVAL;
 
   280     pthread_mutex_unlock(&sig->WakeCondLock);
 
   288     pthread_mutex_unlock(&sig->WakeCondLock);
 
       
   289 
       
   290     if(!sig->used){
 
       
   291         pthread_cond_destroy(&sig->WakeCond);
 
       
   292         pthread_mutex_destroy(&sig->WakeCondLock);
 
       
   293         free(sig);
 
       
   294     }
 
   281     return ret;
 
   295     return ret;
 
   282 }
 
   296 }
 
   283 
   297 
   284 int unblock_RT_to_nRT_signal(void* handle){
 
   298 int unblock_RT_to_nRT_signal(void* handle){
 
   285     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
 
   299     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
beremiz