lbessard@321: /** Edouard@954: * Xenomai Linux specific code greg@336: **/ lbessard@321: lbessard@321: #include Edouard@615: #include lbessard@321: #include lbessard@321: #include lbessard@321: #include lbessard@321: #include Edouard@3315: #include lbessard@321: #include greg@342: #include lbessard@321: Edouard@1974: #include Edouard@1974: #include Edouard@1974: #include Edouard@1974: #include lbessard@321: 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@1428: PLC_SetTimer(common_ticktime__, common_ticktime__); greg@336: Edouard@695: while (!PLC_shutdown) { lbessard@321: PLC_GetTime(&__CURRENT_TIME); lbessard@321: __run(); lbessard@321: if (PLC_shutdown) break; lbessard@321: rt_task_wait_period(NULL); 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@3315: #define _Log(text, err) \ edouard@3294: {\ Edouard@3315: char mstr[256];\ Edouard@3315: snprintf(mstr, 255, text " for %s (%d)", name, err);\ edouard@3294: LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ edouard@3294: } edouard@3294: 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@3315: _Log("Maximum count of RT-PIPE reached while creating pipe", 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@3315: _Log("Failed opening real-time end of RT-PIPE", 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@3315: _Log("Failed opening non-real-time end of RT-PIPE", 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@3315: _Log("Failed closing real-time end of RT-PIPE", ret); Edouard@3315: } Edouard@3315: Edouard@3315: if(close(sig->pipe_fd) != 0){ Edouard@3315: _Log("Failed closing non-real-time end of RT-PIPE", 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: laurent@397: extern unsigned long __tick; Edouard@615: ed@446: int WaitDebugData(unsigned long *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