lbessard@321: /**
lbessard@321:  * Linux specific code
greg@336:  **/
lbessard@321: 
lbessard@321: #include <stdio.h>
lbessard@321: #include <string.h>
lbessard@321: #include <time.h>
lbessard@321: #include <signal.h>
lbessard@321: #include <stdlib.h>
lbessard@321: #include <sys/mman.h>
greg@342: #include <sys/fcntl.h>
lbessard@321: 
lbessard@321: #include <native/task.h>
lbessard@321: #include <native/timer.h>
lbessard@321: #include <native/mutex.h>
lbessard@321: #include <native/sem.h>
greg@342: #include <native/pipe.h>
lbessard@321: 
lbessard@321: unsigned int PLC_state = 0;
lbessard@321: #define PLC_STATE_TASK_CREATED                  1
lbessard@321: #define PLC_STATE_PYTHON_MUTEX_CREATED          2
lbessard@321: #define PLC_STATE_PYTHON_WAIT_SEM_CREATED       4
lbessard@321: #define PLC_STATE_DEBUG_MUTEX_CREATED           8
greg@342: #define PLC_STATE_DEBUG_FILE_OPENED             16
greg@342: #define PLC_STATE_DEBUG_PIPE_CREATED            32
greg@342: 
greg@342: #define WAITDEBUG_PIPE_DEVICE       "/dev/rtp0"
greg@342: #define WAITDEBUG_PIPE_MINOR        0
greg@342: #define WAITDEBUG_PIPE_SIZE         500
lbessard@321: 
lbessard@321: /* provided by POUS.C */
laurent@397: extern unsigned long common_ticktime__;
lbessard@321: 
lbessard@321: long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
lbessard@321: {
lbessard@321:     return __sync_val_compare_and_swap(atomicvar, compared, exchange);
lbessard@321: }
lbessard@321: 
lbessard@321: void PLC_GetTime(IEC_TIME *CURRENT_TIME)
lbessard@321: {
lbessard@321:     RTIME current_time = rt_timer_read();
lbessard@321:     CURRENT_TIME->tv_sec = current_time / 1000000000;
lbessard@321:     CURRENT_TIME->tv_nsec = current_time % 1000000000;
lbessard@321: }
lbessard@321: 
lbessard@321: RT_TASK PLC_task;
greg@342: RT_PIPE WaitDebug_pipe;
greg@342: RT_TASK SuspendDebug_task;
greg@342: RT_TASK ResumeDebug_task;
lbessard@321: RT_TASK WaitPythonCommand_task;
lbessard@321: RT_TASK UnLockPython_task;
lbessard@321: RT_TASK LockPython_task;
lbessard@321: int PLC_shutdown = 0;
lbessard@321: 
greg@342: int WaitDebug_pipe_fd = -1;
greg@342: 
lbessard@321: void PLC_SetTimer(long long next, long long period)
lbessard@321: {
lbessard@321:   RTIME current_time = rt_timer_read();
lbessard@321:   rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period));
lbessard@321: }
lbessard@321: 
lbessard@321: void PLC_task_proc(void *arg)
lbessard@321: {
lbessard@321:     PLC_SetTimer(Ttick, Ttick);
greg@336: 
lbessard@321:     while (1) {
lbessard@321:         PLC_GetTime(&__CURRENT_TIME);
lbessard@321:         __run();
lbessard@321:         if (PLC_shutdown) break;
lbessard@321:         rt_task_wait_period(NULL);
lbessard@321:     }
lbessard@321: }
lbessard@321: 
laurent@397: static unsigned long __debug_tick;
lbessard@321: 
lbessard@321: RT_SEM python_wait_sem;
lbessard@321: RT_MUTEX python_mutex;
lbessard@321: RT_MUTEX debug_mutex;
lbessard@321: 
lbessard@321: void PLC_cleanup_all(void)
lbessard@321: {
lbessard@321:     if (PLC_state & PLC_STATE_TASK_CREATED) {
lbessard@321:         rt_task_delete(&PLC_task);
lbessard@321:         PLC_state &= ~PLC_STATE_TASK_CREATED;
lbessard@321:     }
lbessard@321: 
lbessard@321:     if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {
greg@342:         rt_sem_v(&python_wait_sem);
lbessard@321:         rt_sem_delete(&python_wait_sem);
lbessard@321:         PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED;
lbessard@321:     }
lbessard@321: 
lbessard@321:     if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
lbessard@321:         rt_mutex_delete(&python_mutex);
lbessard@321:         PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED;
lbessard@321:     }
greg@336: 
greg@342:     if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {
greg@342:         rt_pipe_delete(&WaitDebug_pipe);
greg@342:         PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;
greg@342:     }
greg@342: 
greg@342:     if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {
greg@342:         close(WaitDebug_pipe_fd);
greg@342:         PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;
lbessard@321:     }
lbessard@321: 
lbessard@321:     if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
lbessard@321:         rt_mutex_delete(&debug_mutex);
lbessard@321:         PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED;
lbessard@321:     }
lbessard@321: }
lbessard@321: 
lbessard@321: int stopPLC()
lbessard@321: {
lbessard@321:     PLC_shutdown = 1;
lbessard@321:     /* Stop the PLC */
lbessard@321:     PLC_SetTimer(0, 0);
greg@342:     __cleanup();
lbessard@321:     PLC_cleanup_all();
lbessard@321:     __debug_tick = -1;
lbessard@321: }
lbessard@321: 
lbessard@321: //
lbessard@321: void catch_signal(int sig)
lbessard@321: {
lbessard@321:     stopPLC();
lbessard@321: //  signal(SIGTERM, catch_signal);
lbessard@321:     signal(SIGINT, catch_signal);
lbessard@321:     printf("Got Signal %d\n",sig);
lbessard@321:     exit(0);
lbessard@321: }
lbessard@321: 
lbessard@321: #define max_val(a,b) ((a>b)?a:b)
lbessard@321: int startPLC(int argc,char **argv)
lbessard@321: {
lbessard@321:     int ret = 0;
greg@336: 
lbessard@321:     signal(SIGINT, catch_signal);
greg@336: 
lbessard@321:     /* ne-memory-swapping for this program */
lbessard@321:     mlockall(MCL_CURRENT | MCL_FUTURE);
greg@336: 
laurent@397:     /* Define Ttick to 1ms if common_ticktime not defined */
laurent@397:     Ttick = common_ticktime__?common_ticktime__:1000000;
greg@336: 
lbessard@321:     /* create python_wait_sem */
lbessard@321:     ret = rt_sem_create(&python_wait_sem, "python_wait_sem", 0, S_FIFO);
lbessard@321:     if (ret) goto error;
lbessard@321:     PLC_state |= PLC_STATE_PYTHON_WAIT_SEM_CREATED;
greg@336: 
lbessard@321:     /* create python_mutex */
lbessard@321:     ret = rt_mutex_create(&python_mutex, "python_mutex");
lbessard@321:     if (ret) goto error;
lbessard@321:     PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED;
greg@336: 
greg@342:     /* create WaitDebug_pipe */
greg@342:     ret = rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR,
greg@342:           WAITDEBUG_PIPE_SIZE * sizeof(char));
greg@342:     if (ret) goto error;
greg@342:     PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;
greg@342: 
greg@342:     /* open WaitDebug_pipe*/
greg@342:     WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR);
greg@342:     if (WaitDebug_pipe_fd == -1) {
greg@342:         ret = -EBADF;
greg@342:         goto error;
greg@342:     }
greg@342:     PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;
greg@336: 
lbessard@321:     /* create debug_mutex */
lbessard@321:     ret = rt_mutex_create(&debug_mutex, "debug_mutex");
lbessard@321:     if (ret) goto error;
lbessard@321:     PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED;
greg@336: 
lbessard@321:     /* create can_driver_task */
lbessard@321:     ret = rt_task_create(&PLC_task, "PLC_task", 0, 50, 0);
lbessard@321:     if (ret) goto error;
lbessard@321:     PLC_state |= PLC_STATE_TASK_CREATED;
greg@336: 
lbessard@321:     ret = __init(argc,argv);
lbessard@321:     if (ret) goto error;
lbessard@321: 
lbessard@321:     /* start can_driver_task */
lbessard@321:     ret = rt_task_start(&PLC_task, &PLC_task_proc, NULL);
lbessard@321:     if (ret) goto error;
lbessard@321: 
lbessard@321:     return 0;
lbessard@321: 
lbessard@321: error:
lbessard@321:     PLC_cleanup_all();
lbessard@321:     return 1;
lbessard@321: }
lbessard@321: 
lbessard@321: int TryEnterDebugSection(void)
lbessard@321: {
lbessard@321:     return rt_mutex_acquire(&debug_mutex, TM_NONBLOCK) == 0;
lbessard@321: }
lbessard@321: 
lbessard@321: void LeaveDebugSection(void)
lbessard@321: {
lbessard@321:     rt_mutex_release(&debug_mutex);
lbessard@321: }
lbessard@321: 
laurent@397: extern unsigned long __tick;
lbessard@321: /* from plc_debugger.c */
ed@446: int WaitDebugData(unsigned long *tick)
lbessard@321: {
greg@342:     char message;
greg@345:     int res;
ed@446:     *tick = __debug_tick;
lbessard@321:     /* Wait signal from PLC thread */
greg@345:     if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {
greg@345:         res = read(WaitDebug_pipe_fd, &message, sizeof(char));
greg@345:         if (res == sizeof(char))
ed@446:             return 0;
greg@345:     }
greg@345:     return -1;
lbessard@321: }
greg@336: 
lbessard@321: /* Called by PLC thread when debug_publish finished
lbessard@321:  * This is supposed to unlock debugger thread in WaitDebugData*/
lbessard@321: void InitiateDebugTransfer()
lbessard@321: {
greg@342:     char message = 1;
lbessard@321:     /* remember tick */
lbessard@321:     __debug_tick = __tick;
lbessard@321:     /* signal debugger thread it can read data */
greg@342:     if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED)
greg@342:         rt_pipe_write(&WaitDebug_pipe, &message, sizeof(char), P_NORMAL);
lbessard@321: }
lbessard@321: 
lbessard@321: void suspendDebug(void)
lbessard@321: {
lbessard@321:     __DEBUG = 0;
greg@342:     if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
greg@342:         rt_task_shadow(&SuspendDebug_task, "SuspendDebug_task", 0, 0);
greg@342:         /* Prevent PLC to enter debug code */
greg@342:         rt_mutex_acquire(&debug_mutex, TM_INFINITE);
greg@342:     }
lbessard@321: }
lbessard@321: 
lbessard@321: void resumeDebug(void)
lbessard@321: {
lbessard@321:     __DEBUG = 1;
greg@342:     if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
greg@342:         rt_task_shadow(&ResumeDebug_task, "ResumeDebug_task", 0, 0);
greg@342:         /* Let PLC enter debug code */
greg@342:         rt_mutex_release(&debug_mutex);
greg@342:     }
lbessard@321: }
lbessard@321: 
lbessard@321: /* from plc_python.c */
lbessard@321: int WaitPythonCommands(void)
lbessard@321: {
lbessard@321:     /* Wait signal from PLC thread */
greg@342:     if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {
greg@342:         rt_task_shadow(&WaitPythonCommand_task, "WaitPythonCommand_task", 0, 0);
greg@345:         return rt_sem_p(&python_wait_sem, TM_INFINITE);
greg@345:     }
greg@345:     return -1;
lbessard@321: }
greg@336: 
lbessard@321: /* Called by PLC thread on each new python command*/
lbessard@321: void UnBlockPythonCommands(void)
lbessard@321: {
lbessard@321:     /* signal debugger thread it can read data */
lbessard@321:     rt_sem_v(&python_wait_sem);
lbessard@321: }
lbessard@321: 
lbessard@321: int TryLockPython(void)
lbessard@321: {
lbessard@321:     return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0;
lbessard@321: }
lbessard@321: 
lbessard@321: void UnLockPython(void)
lbessard@321: {
greg@342:     if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
greg@342:         rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0);
greg@342:         rt_mutex_release(&python_mutex);
greg@342:     }
lbessard@321: }
lbessard@321: 
lbessard@321: void LockPython(void)
lbessard@321: {
greg@342:     if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
greg@342:         rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0);
greg@342:         rt_mutex_acquire(&python_mutex, TM_INFINITE);
greg@342:     }
greg@342: }