targets/Xenomai/plc_Xenomai_main.c
author greg
Thu, 28 May 2009 17:29:19 +0200
changeset 352 81777d4e379c
parent 345 a3520d75a722
child 397 6a7ff66a811d
permissions -rw-r--r--
fixed bug : Lock _FreePLC until _stopPLC finish
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 */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    32
extern int common_ticktime__;
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
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
    75
static int __debug_tick;
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
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   145
    /* Translate PLC's microseconds to Ttick nanoseconds */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   146
    Ttick = 1000000 * max_val(common_ticktime__,1);
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
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   206
extern int __tick;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   207
/* from plc_debugger.c */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   208
int WaitDebugData()
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;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   212
    /* 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
   213
    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
   214
        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
   215
        if (res == 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
            return __debug_tick;
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   217
    }
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   218
    return -1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   219
}
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   220
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   221
/* Called by PLC thread when debug_publish finished
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   222
 * This is supposed to unlock debugger thread in WaitDebugData*/
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   223
void InitiateDebugTransfer()
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   224
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   225
    char message = 1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   226
    /* remember tick */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   227
    __debug_tick = __tick;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   228
    /* signal debugger thread it can read data */
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   229
    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED)
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   230
        rt_pipe_write(&WaitDebug_pipe, &message, sizeof(char), P_NORMAL);
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   231
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   232
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   233
void suspendDebug(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   234
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   235
    __DEBUG = 0;
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   236
    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   237
        rt_task_shadow(&SuspendDebug_task, "SuspendDebug_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   238
        /* Prevent PLC to enter debug code */
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   239
        rt_mutex_acquire(&debug_mutex, TM_INFINITE);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   240
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   241
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   242
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   243
void resumeDebug(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   244
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   245
    __DEBUG = 1;
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   246
    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   247
        rt_task_shadow(&ResumeDebug_task, "ResumeDebug_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   248
        /* Let PLC enter debug code */
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   249
        rt_mutex_release(&debug_mutex);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   250
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   251
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   252
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   253
/* from plc_python.c */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   254
int WaitPythonCommands(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   255
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   256
    /* Wait signal from PLC thread */
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   257
    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   258
        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
   259
        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
   260
    }
a3520d75a722 get the WaitPythonCommands return (semaphore error code) to quit python_iterator loop when semaphore is destroyed
greg
parents: 342
diff changeset
   261
    return -1;
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   262
}
336
ae3488c79283 Fixed bug : Segmentation fault or locks when stop PLC if no CAN network.
greg
parents: 321
diff changeset
   263
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   264
/* Called by PLC thread on each new python command*/
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   265
void UnBlockPythonCommands(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   266
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   267
    /* signal debugger thread it can read data */
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   268
    rt_sem_v(&python_wait_sem);
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   269
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   270
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   271
int TryLockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   272
{
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   273
    return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0;
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   274
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   275
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   276
void UnLockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   277
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   278
    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   279
        rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   280
        rt_mutex_release(&python_mutex);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   281
    }
321
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   282
}
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   283
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   284
void LockPython(void)
5a4e6278a18b Adding support for Xenomai targets.
lbessard
parents:
diff changeset
   285
{
342
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   286
    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   287
        rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   288
        rt_mutex_acquire(&python_mutex, TM_INFINITE);
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   289
    }
80e5876bc53b Replacing Debug rt_sem by rt_pipe
greg
parents: 336
diff changeset
   290
}