lbessard@321: /**
Edouard@954:  * Xenomai Linux specific code
greg@336:  **/
lbessard@321: 
lbessard@321: #include <stdio.h>
Edouard@615: #include <unistd.h>
lbessard@321: #include <string.h>
lbessard@321: #include <time.h>
lbessard@321: #include <signal.h>
lbessard@321: #include <stdlib.h>
Edouard@3315: #include <errno.h>
lbessard@321: #include <sys/mman.h>
greg@342: #include <sys/fcntl.h>
lbessard@321: 
Edouard@1974: #include <alchemy/task.h>
Edouard@1974: #include <alchemy/timer.h>
Edouard@1974: #include <alchemy/sem.h>
Edouard@1974: #include <alchemy/pipe.h>
lbessard@321: 
edouard@3953: #define _Log(level,text,...) \
edouard@3953:     {\
edouard@3953:         char mstr[256];\
edouard@3953:         snprintf(mstr, 255, text, ##__VA_ARGS__);\
edouard@3953:         LogMessage(level, mstr, strlen(mstr));\
edouard@3953:     }
edouard@3953: 
edouard@3953: #define _LogError(text,...) _Log(LOG_CRITICAL, text, ##__VA_ARGS__)
edouard@3953: #define _LogWarning(text,...) _Log(LOG_WARNING, text, ##__VA_ARGS__)
edouard@3953: 
lbessard@321: unsigned int PLC_state = 0;
Edouard@615: #define PLC_STATE_TASK_CREATED                 1
Edouard@3298: #define PLC_STATE_DEBUG_PIPE_CREATED           2
Edouard@3298: #define PLC_STATE_PYTHON_PIPE_CREATED          8
edouard@3294: #define PLC_STATE_WAITDEBUG_PIPE_CREATED       16
edouard@3294: #define PLC_STATE_WAITPYTHON_PIPE_CREATED      32
edouard@3294: 
Edouard@3298: #define PIPE_SIZE                    1
lbessard@321: 
Edouard@1990: // rt-pipes commands
Edouard@1990: 
Edouard@1990: #define PYTHON_PENDING_COMMAND 1
Edouard@1990: #define PYTHON_FINISH 2
Edouard@1990: 
Edouard@1990: #define DEBUG_FINISH 2
Edouard@1990: 
Edouard@1990: #define DEBUG_PENDING_DATA 1
Edouard@1990: #define DEBUG_UNLOCK 1
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: }
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: }
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;
edouard@3294: void *WaitDebug_handle;
edouard@3294: void *WaitPython_handle;
edouard@3294: void *Debug_handle;
edouard@3294: void *Python_handle;
edouard@3294: void *svghmi_handle;
edouard@3294: 
edouard@3294: struct RT_to_nRT_signal_s {
edouard@3294:     int used;
edouard@3294:     RT_PIPE pipe;
edouard@3294:     int pipe_fd;
Edouard@3298:     char *name;
edouard@3294: };
edouard@3294: typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;
edouard@3294: 
edouard@3294: #define max_RT_to_nRT_signals 16
edouard@3294: 
edouard@3294: static RT_to_nRT_signal_t RT_to_nRT_signal_pool[max_RT_to_nRT_signals];
edouard@3294: 
edouard@3294: int recv_RT_to_nRT_signal(void* handle, char* payload){
edouard@3294:     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
edouard@3294:     if(!sig->used) return -EINVAL;
edouard@3294:     return read(sig->pipe_fd, payload, 1);
edouard@3294: }
edouard@3294: 
edouard@3294: int send_RT_to_nRT_signal(void* handle, char payload){
edouard@3294:     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
edouard@3294:     if(!sig->used) return -EINVAL;
edouard@3294:     return rt_pipe_write(&sig->pipe, &payload, 1, P_NORMAL);
edouard@3294: }
edouard@3294: 
Edouard@615: 
lbessard@321: int PLC_shutdown = 0;
lbessard@321: 
Edouard@615: void PLC_SetTimer(unsigned long long next, unsigned 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: {
edouard@3953:     unsigned long overruns = 0;
Edouard@1428:     PLC_SetTimer(common_ticktime__, common_ticktime__);
greg@336: 
Edouard@695:     while (!PLC_shutdown) {
lbessard@321:         PLC_GetTime(&__CURRENT_TIME);
edouard@3953:         if(overruns == 0){
edouard@3953:             __run();
edouard@3953:         } else {
edouard@3953:             // in case of overrun, don't run PLC on next cycle, to prevent CPU hogging.
edouard@3953:             _LogWarning("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns);
edouard@3953:             // rt_printf("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns);
edouard@3953:             // increment tick count anyhow, so that task scheduling keeps consistent
edouard@3953:             __tick += overruns;
edouard@3953:         }
lbessard@321:         if (PLC_shutdown) break;
edouard@3953:         rt_task_wait_period(&overruns);
lbessard@321:     }
Edouard@3298:     /* since xenomai 3 it is not enough to close()
Edouard@1990:        file descriptor to unblock read()... */
Edouard@1990:     {
Edouard@1990:         /* explicitely finish python thread */
Edouard@1990:         char msg = PYTHON_FINISH;
edouard@3294:         send_RT_to_nRT_signal(WaitPython_handle, msg);
Edouard@1990:     }
Edouard@1990:     {
Edouard@1990:         /* explicitely finish debug thread */
Edouard@1990:         char msg = DEBUG_FINISH;
edouard@3294:         send_RT_to_nRT_signal(WaitDebug_handle, msg);
Edouard@1990:     }
lbessard@321: }
lbessard@321: 
laurent@397: static unsigned long __debug_tick;
lbessard@321: 
edouard@3294: void *create_RT_to_nRT_signal(char* name){
edouard@3294:     int new_index = -1;
Edouard@3315:     int ret;
edouard@3294:     RT_to_nRT_signal_t *sig;
edouard@3294:     char pipe_dev[64];
edouard@3294: 
edouard@3294:     /* find a free slot */
edouard@3294:     for(int i=0; i < max_RT_to_nRT_signals; i++){
edouard@3294:         sig = &RT_to_nRT_signal_pool[i];
edouard@3294:         if(!sig->used){
edouard@3294:             new_index = i;
edouard@3294:             break;
edouard@3294:         }
edouard@3294:     }
edouard@3294: 
edouard@3294:     /* fail if none found */
edouard@3294:     if(new_index == -1) {
edouard@3953:     	_LogError("Maximum count of RT-PIPE reached while creating pipe for %s (%d)", name, max_RT_to_nRT_signals);
Edouard@3315:         return NULL;
edouard@3294:     }
edouard@3294: 
edouard@3294:     /* create rt pipe */
Edouard@3315:     if(ret = rt_pipe_create(&sig->pipe, name, new_index, PIPE_SIZE) < 0){
edouard@3953:     	_LogError("Failed opening real-time end of RT-PIPE for %s (%d)", name, ret);
Edouard@3315:         return NULL;
edouard@3294:     }
edouard@3294: 
edouard@3294:     /* open pipe's userland */
Edouard@3315:     snprintf(pipe_dev, 63, "/dev/rtp%d", new_index);
edouard@3294:     if((sig->pipe_fd = open(pipe_dev, O_RDWR)) == -1){
edouard@3294:         rt_pipe_delete(&sig->pipe);
edouard@3953:     	_LogError("Failed opening non-real-time end of RT-PIPE for %s (%d)", name, errno);
Edouard@3315:         return NULL;
edouard@3294:     }
edouard@3294: 
edouard@3294:     sig->used = 1;
edouard@3294:     sig->name = name;
edouard@3294: 
edouard@3294:     return sig;
edouard@3294: }
edouard@3294: 
edouard@3294: void delete_RT_to_nRT_signal(void* handle){
Edouard@3315:     int ret;
edouard@3294:     RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;
Edouard@3315:     char *name = sig->name;
edouard@3294: 
edouard@3294:     if(!sig->used) return;
edouard@3294: 
Edouard@3315:     if(ret = rt_pipe_delete(&sig->pipe) != 0){
edouard@3953:     	_LogError("Failed closing real-time end of RT-PIPE for %s (%d)", name, ret);
Edouard@3315:     }
Edouard@3315: 
Edouard@3315:     if(close(sig->pipe_fd) != 0){
edouard@3953:     	_LogError("Failed closing non-real-time end of RT-PIPE for %s (%d)", name, errno);
Edouard@3315:     }
edouard@3294: 
edouard@3294:     sig->used = 0;
edouard@3294: }
edouard@3294: 
edouard@3294: int wait_RT_to_nRT_signal(void* handle){
edouard@3294:     char cmd;
edouard@3294:     int ret = recv_RT_to_nRT_signal(handle, &cmd);
edouard@3294:     return (ret == 1) ? 0 : ((ret == 0) ? ENODATA : -ret);
edouard@3294: }
edouard@3294: 
edouard@3294: int unblock_RT_to_nRT_signal(void* handle){
edouard@3294:     int ret = send_RT_to_nRT_signal(handle, 0);
edouard@3294:     return (ret == 1) ? 0 : ((ret == 0) ? EINVAL : -ret);
edouard@3294: }
edouard@3294: 
edouard@3295: void nRT_reschedule(void){
edouard@3295:     sched_yield();
edouard@3295: }
edouard@3295: 
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: 
Edouard@615:     if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {
edouard@3294:         delete_RT_to_nRT_signal(WaitDebug_handle);
Edouard@615:         PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;
Edouard@615:     }
Edouard@615: 
Edouard@615:     if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {
edouard@3294:         delete_RT_to_nRT_signal(WaitPython_handle);
laurent@745:         PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;
laurent@745:     }
laurent@745: 
greg@342:     if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {
edouard@3294:         delete_RT_to_nRT_signal(Debug_handle);
greg@342:         PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;
greg@342:     }
greg@342: 
Edouard@615:     if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {
edouard@3294:         delete_RT_to_nRT_signal(Python_handle);
laurent@745:         PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;
laurent@745:     }
lbessard@321: }
lbessard@321: 
lbessard@321: int stopPLC()
lbessard@321: {
laurent@745:     /* Stop the PLC */
lbessard@321:     PLC_shutdown = 1;
laurent@745: 
laurent@745:     /* Wait until PLC task stops */
laurent@745:     rt_task_join(&PLC_task);
laurent@745: 
laurent@745:     PLC_cleanup_all();
greg@342:     __cleanup();
lbessard@321:     __debug_tick = -1;
Edouard@615:     return 0;
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: 
Edouard@1981: #define _startPLCLog(text) \
Edouard@1981:     {\
Edouard@1981:     	char mstr[] = text;\
Edouard@1981:         LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));\
Edouard@1981:         goto error;\
Edouard@1981:     }
Edouard@1981: 
Edouard@1981: #define FO "Failed opening "
Edouard@1981: 
lbessard@321: #define max_val(a,b) ((a>b)?a:b)
lbessard@321: int startPLC(int argc,char **argv)
lbessard@321: {
lbessard@321:     signal(SIGINT, catch_signal);
greg@336: 
Edouard@615:     /* no memory swapping for that process */
lbessard@321:     mlockall(MCL_CURRENT | MCL_FUTURE);
greg@336: 
edouard@3294:     /* memory initialization */
laurent@745:     PLC_shutdown = 0;
edouard@3294:     bzero(RT_to_nRT_signal_pool, sizeof(RT_to_nRT_signal_pool));
edouard@3294: 
edouard@3294:     /*** RT Pipes ***/
Edouard@615:     /* create Debug_pipe */
Edouard@3298:     if(!(Debug_handle = create_RT_to_nRT_signal("Debug_pipe"))) goto error;
Edouard@615:     PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;
Edouard@3298: 
Edouard@615:     /* create Python_pipe */
Edouard@3298:     if(!(Python_handle = create_RT_to_nRT_signal("Python_pipe"))) goto error;
Edouard@615:     PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;
Edouard@615: 
greg@342:     /* create WaitDebug_pipe */
Edouard@3298:     if(!(WaitDebug_handle = create_RT_to_nRT_signal("WaitDebug_pipe"))) goto error;
Edouard@615:     PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;
greg@342: 
Edouard@615:     /* create WaitPython_pipe */
Edouard@3298:     if(!(WaitPython_handle = create_RT_to_nRT_signal("WaitPython_pipe"))) goto error;
Edouard@615:     PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;
Edouard@615: 
Edouard@615:     /*** create PLC task ***/
Edouard@1981:     if(rt_task_create(&PLC_task, "PLC_task", 0, 50, T_JOINABLE))
Edouard@1981:         _startPLCLog("Failed creating PLC task");
lbessard@321:     PLC_state |= PLC_STATE_TASK_CREATED;
greg@336: 
Edouard@615:     if(__init(argc,argv)) goto error;
Edouard@615: 
Edouard@615:     /* start PLC task */
Edouard@1981:     if(rt_task_start(&PLC_task, &PLC_task_proc, NULL))
Edouard@1981:         _startPLCLog("Failed starting PLC task");
lbessard@321: 
lbessard@321:     return 0;
lbessard@321: 
lbessard@321: error:
Edouard@616: 
lbessard@321:     PLC_cleanup_all();
lbessard@321:     return 1;
lbessard@321: }
lbessard@321: 
Edouard@615: #define DEBUG_FREE 0
Edouard@615: #define DEBUG_BUSY 1
Edouard@615: static long debug_state = DEBUG_FREE;
Edouard@615: 
lbessard@321: int TryEnterDebugSection(void)
lbessard@321: {
Edouard@616:     if(AtomicCompareExchange(
Edouard@615:         &debug_state,
Edouard@615:         DEBUG_FREE,
Edouard@616:         DEBUG_BUSY) == DEBUG_FREE){
Edouard@616:         if(__DEBUG){
Edouard@616:             return 1;
Edouard@616:         }
Edouard@616:         AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);
Edouard@616:     }
Edouard@616:     return 0;
Edouard@615: }
Edouard@615: 
lbessard@321: void LeaveDebugSection(void)
lbessard@321: {
Edouard@3298:     if(AtomicCompareExchange( &debug_state,
Edouard@615:         DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){
Edouard@615:         char msg = DEBUG_UNLOCK;
Edouard@615:         /* signal to NRT for wakeup */
edouard@3294:         send_RT_to_nRT_signal(Debug_handle, msg);
Edouard@615:     }
lbessard@321: }
lbessard@321: 
edouard@3947: extern unsigned int __tick;
edouard@3947: 
edouard@3947: int WaitDebugData(unsigned int *tick)
lbessard@321: {
Edouard@615:     char cmd;
greg@345:     int res;
laurent@745:     if (PLC_shutdown) return -1;
lbessard@321:     /* Wait signal from PLC thread */
Edouard@3298:     res = recv_RT_to_nRT_signal(WaitDebug_handle, &cmd);
edouard@3294:     if (res == 1 && cmd == DEBUG_PENDING_DATA){
Edouard@617:         *tick = __debug_tick;
Edouard@615:         return 0;
Edouard@617:     }
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: {
Edouard@615:     char msg = DEBUG_PENDING_DATA;
lbessard@321:     /* remember tick */
lbessard@321:     __debug_tick = __tick;
lbessard@321:     /* signal debugger thread it can read data */
edouard@3294:     send_RT_to_nRT_signal(WaitDebug_handle, msg);
Edouard@615: }
Edouard@615: 
Edouard@615: int suspendDebug(int disable)
Edouard@615: {
Edouard@615:     char cmd = DEBUG_UNLOCK;
laurent@745:     if (PLC_shutdown) return -1;
Edouard@615:     while(AtomicCompareExchange(
Edouard@615:             &debug_state,
Edouard@615:             DEBUG_FREE,
Edouard@615:             DEBUG_BUSY) != DEBUG_FREE &&
Edouard@615:             cmd == DEBUG_UNLOCK){
edouard@3294:        if(recv_RT_to_nRT_signal(Debug_handle, &cmd) != 1){
Edouard@615:            return -1;
Edouard@615:        }
Edouard@615:     }
Edouard@615:     __DEBUG = !disable;
Edouard@617:     if (disable)
Edouard@617:         AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);
Edouard@615:     return 0;
lbessard@321: }
lbessard@321: 
lbessard@321: void resumeDebug(void)
lbessard@321: {
Edouard@3398:     __DEBUG = 1;
Edouard@615:     AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);
Edouard@615: }
Edouard@615: 
Edouard@615: #define PYTHON_FREE 0
Edouard@615: #define PYTHON_BUSY 1
Edouard@615: static long python_state = PYTHON_FREE;
Edouard@615: 
lbessard@321: int WaitPythonCommands(void)
Edouard@3298: {
Edouard@615:     char cmd;
laurent@745:     if (PLC_shutdown) return -1;
lbessard@321:     /* Wait signal from PLC thread */
edouard@3294:     if(recv_RT_to_nRT_signal(WaitPython_handle, &cmd) == 1 && cmd==PYTHON_PENDING_COMMAND){
Edouard@615:         return 0;
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: {
Edouard@615:     char msg = PYTHON_PENDING_COMMAND;
edouard@3294:     send_RT_to_nRT_signal(WaitPython_handle, msg);
lbessard@321: }
lbessard@321: 
lbessard@321: int TryLockPython(void)
lbessard@321: {
Edouard@615:     return AtomicCompareExchange(
Edouard@615:         &python_state,
Edouard@615:         PYTHON_FREE,
Edouard@615:         PYTHON_BUSY) == PYTHON_FREE;
Edouard@615: }
Edouard@615: 
Edouard@615: #define UNLOCK_PYTHON 1
Edouard@615: void LockPython(void)
Edouard@615: {
Edouard@615:     char cmd = UNLOCK_PYTHON;
laurent@745:     if (PLC_shutdown) return;
Edouard@615:     while(AtomicCompareExchange(
Edouard@615:             &python_state,
Edouard@615:             PYTHON_FREE,
Edouard@615:             PYTHON_BUSY) != PYTHON_FREE &&
Edouard@615:             cmd == UNLOCK_PYTHON){
edouard@3294:        recv_RT_to_nRT_signal(Python_handle, &cmd);
Edouard@615:     }
lbessard@321: }
lbessard@321: 
lbessard@321: void UnLockPython(void)
lbessard@321: {
Edouard@615:     if(AtomicCompareExchange(
Edouard@615:             &python_state,
Edouard@615:             PYTHON_BUSY,
Edouard@615:             PYTHON_FREE) == PYTHON_BUSY){
Edouard@615:         if(rt_task_self()){/*is that the real time task ?*/
Edouard@615:            char cmd = UNLOCK_PYTHON;
edouard@3294:            send_RT_to_nRT_signal(Python_handle, cmd);
Edouard@615:         }/* otherwise, no signaling from non real time */
Edouard@615:     }    /* as plc does not wait for lock. */
Edouard@615: }
Edouard@615: 
Edouard@1903: #ifndef HAVE_RETAIN
wuyangtang@1717: int CheckRetainBuffer(void)
wuyangtang@1717: {
wuyangtang@1717: 	return 1;
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void ValidateRetainBuffer(void)
wuyangtang@1717: {
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void InValidateRetainBuffer(void)
wuyangtang@1717: {
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void Retain(unsigned int offset, unsigned int count, void *p)
wuyangtang@1717: {
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void Remind(unsigned int offset, unsigned int count, void *p)
wuyangtang@1717: {
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void CleanupRetain(void)
wuyangtang@1717: {
wuyangtang@1717: }
wuyangtang@1717: 
wuyangtang@1717: void InitRetain(void)
wuyangtang@1717: {
wuyangtang@1717: }
Edouard@1903: #endif // !HAVE_RETAIN