targets/Xenomai/plc_Xenomai_main.c
author Lolitech
Thu, 03 Jun 2010 17:21:40 +0200
changeset 547 5748d695beee
parent 446 1edde533db19
child 615 72bc3e53a1fa
permissions -rw-r--r--
Reorganization of threading for command line and wx main loops. Commands are now cleanly serialized through calls to wx.CallAfter. wx mainloop now runs on main thread.
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     1
/**
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     2
 * Linux specific code
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
     3
 **/
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     4
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     5
#include <stdio.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     6
#include <string.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     7
#include <time.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     8
#include <signal.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
     9
#include <stdlib.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    10
#include <sys/mman.h>
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    11
#include <sys/fcntl.h>
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    12
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    13
#include <native/task.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    14
#include <native/timer.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    15
#include <native/mutex.h>
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    16
#include <native/sem.h>
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    17
#include <native/pipe.h>
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    18
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    19
unsigned int PLC_state = 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    20
#define PLC_STATE_TASK_CREATED                  1
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    21
#define PLC_STATE_PYTHON_MUTEX_CREATED          2
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    22
#define PLC_STATE_PYTHON_WAIT_SEM_CREATED       4
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    23
#define PLC_STATE_DEBUG_MUTEX_CREATED           8
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    24
#define PLC_STATE_DEBUG_FILE_OPENED             16
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    25
#define PLC_STATE_DEBUG_PIPE_CREATED            32
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    26
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    27
#define WAITDEBUG_PIPE_DEVICE       "/dev/rtp0"
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    28
#define WAITDEBUG_PIPE_MINOR        0
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    29
#define WAITDEBUG_PIPE_SIZE         500
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    30
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    31
/* provided by POUS.C */
397
6a7ff66a811d Adding support for forcing tick count to return to zero as the same time than all tasks firing are synchronized
laurent
parents: 345
diff changeset
    32
extern unsigned long common_ticktime__;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    33
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    34
long AtomicCompareExchange(long* atomicvar,long compared, long exchange)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    35
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    36
    return __sync_val_compare_and_swap(atomicvar, compared, exchange);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    37
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    38
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    39
void PLC_GetTime(IEC_TIME *CURRENT_TIME)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    40
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    41
    RTIME current_time = rt_timer_read();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    42
    CURRENT_TIME->tv_sec = current_time / 1000000000;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    43
    CURRENT_TIME->tv_nsec = current_time % 1000000000;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    44
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    45
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    46
RT_TASK PLC_task;
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    47
RT_PIPE WaitDebug_pipe;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    48
RT_TASK SuspendDebug_task;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    49
RT_TASK ResumeDebug_task;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    50
RT_TASK WaitPythonCommand_task;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    51
RT_TASK UnLockPython_task;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    52
RT_TASK LockPython_task;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    53
int PLC_shutdown = 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    54
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    55
int WaitDebug_pipe_fd = -1;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    56
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    57
void PLC_SetTimer(long long next, long long period)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    58
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    59
  RTIME current_time = rt_timer_read();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    60
  rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period));
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    61
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    62
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    63
void PLC_task_proc(void *arg)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    64
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    65
    PLC_SetTimer(Ttick, Ttick);
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
    66
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    67
    while (1) {
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    68
        PLC_GetTime(&__CURRENT_TIME);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    69
        __run();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    70
        if (PLC_shutdown) break;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    71
        rt_task_wait_period(NULL);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    72
    }
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    73
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    74
397
6a7ff66a811d Adding support for forcing tick count to return to zero as the same time than all tasks firing are synchronized
laurent
parents: 345
diff changeset
    75
static unsigned long __debug_tick;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    76
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    77
RT_SEM python_wait_sem;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    78
RT_MUTEX python_mutex;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    79
RT_MUTEX debug_mutex;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    80
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    81
void PLC_cleanup_all(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    82
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    83
    if (PLC_state & PLC_STATE_TASK_CREATED) {
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    84
        rt_task_delete(&PLC_task);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    85
        PLC_state &= ~PLC_STATE_TASK_CREATED;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    86
    }
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    87
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    88
    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    89
        rt_sem_v(&python_wait_sem);
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    90
        rt_sem_delete(&python_wait_sem);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    91
        PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    92
    }
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    93
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    94
    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    95
        rt_mutex_delete(&python_mutex);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    96
        PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    97
    }
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
    98
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
    99
    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   100
        rt_pipe_delete(&WaitDebug_pipe);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   101
        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   102
    }
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   103
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   104
    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   105
        close(WaitDebug_pipe_fd);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   106
        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   107
    }
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   108
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   109
    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   110
        rt_mutex_delete(&debug_mutex);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   111
        PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   112
    }
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   113
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   114
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   115
int stopPLC()
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   116
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   117
    PLC_shutdown = 1;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   118
    /* Stop the PLC */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   119
    PLC_SetTimer(0, 0);
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   120
    __cleanup();
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   121
    PLC_cleanup_all();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   122
    __debug_tick = -1;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   123
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   124
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   125
//
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   126
void catch_signal(int sig)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   127
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   128
    stopPLC();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   129
//  signal(SIGTERM, catch_signal);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   130
    signal(SIGINT, catch_signal);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   131
    printf("Got Signal %d\n",sig);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   132
    exit(0);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   133
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   134
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   135
#define max_val(a,b) ((a>b)?a:b)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   136
int startPLC(int argc,char **argv)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   137
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   138
    int ret = 0;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   139
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   140
    signal(SIGINT, catch_signal);
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   141
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   142
    /* ne-memory-swapping for this program */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   143
    mlockall(MCL_CURRENT | MCL_FUTURE);
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   144
397
6a7ff66a811d Adding support for forcing tick count to return to zero as the same time than all tasks firing are synchronized
laurent
parents: 345
diff changeset
   145
    /* Define Ttick to 1ms if common_ticktime not defined */
6a7ff66a811d Adding support for forcing tick count to return to zero as the same time than all tasks firing are synchronized
laurent
parents: 345
diff changeset
   146
    Ttick = common_ticktime__?common_ticktime__:1000000;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   147
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   148
    /* create python_wait_sem */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   149
    ret = rt_sem_create(&python_wait_sem, "python_wait_sem", 0, S_FIFO);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   150
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   151
    PLC_state |= PLC_STATE_PYTHON_WAIT_SEM_CREATED;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   152
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   153
    /* create python_mutex */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   154
    ret = rt_mutex_create(&python_mutex, "python_mutex");
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   155
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   156
    PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   157
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   158
    /* create WaitDebug_pipe */
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   159
    ret = rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR,
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   160
          WAITDEBUG_PIPE_SIZE * sizeof(char));
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   161
    if (ret) goto error;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   162
    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   163
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   164
    /* open WaitDebug_pipe*/
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   165
    WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   166
    if (WaitDebug_pipe_fd == -1) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   167
        ret = -EBADF;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   168
        goto error;
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   169
    }
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   170
    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   171
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   172
    /* create debug_mutex */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   173
    ret = rt_mutex_create(&debug_mutex, "debug_mutex");
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   174
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   175
    PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   176
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   177
    /* create can_driver_task */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   178
    ret = rt_task_create(&PLC_task, "PLC_task", 0, 50, 0);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   179
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   180
    PLC_state |= PLC_STATE_TASK_CREATED;
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   181
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   182
    ret = __init(argc,argv);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   183
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   184
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   185
    /* start can_driver_task */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   186
    ret = rt_task_start(&PLC_task, &PLC_task_proc, NULL);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   187
    if (ret) goto error;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   188
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   189
    return 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   190
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   191
error:
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   192
    PLC_cleanup_all();
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   193
    return 1;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   194
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   195
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   196
int TryEnterDebugSection(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   197
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   198
    return rt_mutex_acquire(&debug_mutex, TM_NONBLOCK) == 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   199
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   200
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   201
void LeaveDebugSection(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   202
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   203
    rt_mutex_release(&debug_mutex);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   204
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   205
397
6a7ff66a811d Adding support for forcing tick count to return to zero as the same time than all tasks firing are synchronized
laurent
parents: 345
diff changeset
   206
extern unsigned long __tick;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   207
/* from plc_debugger.c */
446
1edde533db19 Some cleanup in PLC status - removed that \"Starting\" state ...
ed
parents: 397
diff changeset
   208
int WaitDebugData(unsigned long *tick)
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   209
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   210
    char message;
345
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   211
    int res;
446
1edde533db19 Some cleanup in PLC status - removed that \"Starting\" state ...
ed
parents: 397
diff changeset
   212
    *tick = __debug_tick;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   213
    /* Wait signal from PLC thread */
345
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   214
    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   215
        res = read(WaitDebug_pipe_fd, &message, sizeof(char));
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   216
        if (res == sizeof(char))
446
1edde533db19 Some cleanup in PLC status - removed that \"Starting\" state ...
ed
parents: 397
diff changeset
   217
            return 0;
345
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   218
    }
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   219
    return -1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   220
}
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   221
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   222
/* Called by PLC thread when debug_publish finished
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   223
 * This is supposed to unlock debugger thread in WaitDebugData*/
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   224
void InitiateDebugTransfer()
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   225
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   226
    char message = 1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   227
    /* remember tick */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   228
    __debug_tick = __tick;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   229
    /* signal debugger thread it can read data */
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   230
    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED)
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   231
        rt_pipe_write(&WaitDebug_pipe, &message, sizeof(char), P_NORMAL);
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   232
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   233
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   234
void suspendDebug(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   235
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   236
    __DEBUG = 0;
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   237
    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   238
        rt_task_shadow(&SuspendDebug_task, "SuspendDebug_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   239
        /* Prevent PLC to enter debug code */
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   240
        rt_mutex_acquire(&debug_mutex, TM_INFINITE);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   241
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   242
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   243
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   244
void resumeDebug(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   245
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   246
    __DEBUG = 1;
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   247
    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   248
        rt_task_shadow(&ResumeDebug_task, "ResumeDebug_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   249
        /* Let PLC enter debug code */
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   250
        rt_mutex_release(&debug_mutex);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   251
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   252
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   253
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   254
/* from plc_python.c */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   255
int WaitPythonCommands(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   256
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   257
    /* Wait signal from PLC thread */
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   258
    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   259
        rt_task_shadow(&WaitPythonCommand_task, "WaitPythonCommand_task", 0, 0);
345
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   260
        return rt_sem_p(&python_wait_sem, TM_INFINITE);
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   261
    }
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   262
    return -1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   263
}
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   264
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   265
/* Called by PLC thread on each new python command*/
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   266
void UnBlockPythonCommands(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   267
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   268
    /* signal debugger thread it can read data */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   269
    rt_sem_v(&python_wait_sem);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   270
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   271
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   272
int TryLockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   273
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   274
    return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   275
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   276
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   277
void UnLockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   278
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   279
    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   280
        rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   281
        rt_mutex_release(&python_mutex);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   282
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   283
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   284
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   285
void LockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   286
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   287
    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   288
        rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   289
        rt_mutex_acquire(&python_mutex, TM_INFINITE);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   290
    }
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   291
}