1

/**

     2

 * Linux specific code

     4

     5

#include <stdio.h>

     6

#include <string.h>

     7

#include <time.h>

     8

#include <signal.h>

     9

#include <stdlib.h>

    10

#include <sys/mman.h>

    11

    12

#include <native/task.h>

    13

#include <native/timer.h>

    14

#include <native/mutex.h>

    15

#include <native/sem.h>

    16

    17

unsigned int PLC_state = 0;

    18

#define PLC_STATE_TASK_CREATED                  1

    19

#define PLC_STATE_PYTHON_MUTEX_CREATED          2

    20

#define PLC_STATE_PYTHON_WAIT_SEM_CREATED       4

    21

#define PLC_STATE_DEBUG_MUTEX_CREATED           8

    22

#define PLC_STATE_DEBUG_WAIT_SEM_CREATED       16

    23

    24

/* provided by POUS.C */

    25

extern int common_ticktime__;

    26

    27

long AtomicCompareExchange(long* atomicvar,long compared, long exchange)

    28

{

    29

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    30

}

    31

    32

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    33

{

    34

    RTIME current_time = rt_timer_read();

    35

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    36

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    37

}

    38

    39

RT_TASK PLC_task;

    40

RT_TASK WaitDebug_task;

    41

RT_TASK WaitPythonCommand_task;

    42

RT_TASK UnLockPython_task;

    43

RT_TASK LockPython_task;

    44

int PLC_shutdown = 0;

    45

    46

void PLC_SetTimer(long long next, long long period)

    47

{

    48

  RTIME current_time = rt_timer_read();

    49

  rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period));

    50

}

    51

    52

void PLC_task_proc(void *arg)

    53

{

    54

    PLC_SetTimer(Ttick, Ttick);

    56

    while (1) {

    57

        PLC_GetTime(&__CURRENT_TIME);

    58

        __run();

    59

        if (PLC_shutdown) break;

    60

        rt_task_wait_period(NULL);

    61

    }

    62

}

    63

    64

static int __debug_tick;

    65

    66

RT_SEM python_wait_sem;

    67

RT_MUTEX python_mutex;

    68

RT_SEM debug_wait_sem;

    69

RT_MUTEX debug_mutex;

    70

    71

void PLC_cleanup_all(void)

    72

{

    73

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    74

        rt_task_delete(&PLC_task);

    75

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    76

    }

    77

    78

    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {

    79

        rt_sem_delete(&python_wait_sem);

    80

        PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED;

    81

    }

    82

    83

    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {

    84

        rt_mutex_delete(&python_mutex);

    85

        PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED;

    86

    }

    88

    if (PLC_state & PLC_STATE_DEBUG_WAIT_SEM_CREATED) {

    89

        rt_sem_delete(&debug_wait_sem);

    90

        PLC_state &= ~ PLC_STATE_DEBUG_WAIT_SEM_CREATED;

    91

    }

    92

    93

    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {

    94

        rt_mutex_delete(&debug_mutex);

    95

        PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED;

    96

    }

    97

}

    98

    99

int stopPLC()

   100

{

   101

    PLC_shutdown = 1;

   102

    /* Stop the PLC */

   103

    PLC_SetTimer(0, 0);

   104

    PLC_cleanup_all();

   105

    __cleanup();

   106

    __debug_tick = -1;

   107

    rt_sem_v(&debug_wait_sem);

   108

    rt_sem_v(&python_wait_sem);

   109

}

   110

   111

//

   112

void catch_signal(int sig)

   113

{

   114

    stopPLC();

   115

//  signal(SIGTERM, catch_signal);

   116

    signal(SIGINT, catch_signal);

   117

    printf("Got Signal %d\n",sig);

   118

    exit(0);

   119

}

   120

   121

#define max_val(a,b) ((a>b)?a:b)

   122

int startPLC(int argc,char **argv)

   123

{

   124

    int ret = 0;

   126

    signal(SIGINT, catch_signal);

   128

    /* ne-memory-swapping for this program */

   129

    mlockall(MCL_CURRENT | MCL_FUTURE);

   131

    /* Translate PLC's microseconds to Ttick nanoseconds */

   132

    Ttick = 1000000 * max_val(common_ticktime__,1);

   134

    /* create python_wait_sem */

   135

    ret = rt_sem_create(&python_wait_sem, "python_wait_sem", 0, S_FIFO);

   136

    if (ret) goto error;

   137

    PLC_state |= PLC_STATE_PYTHON_WAIT_SEM_CREATED;

   139

    /* create python_mutex */

   140

    ret = rt_mutex_create(&python_mutex, "python_mutex");

   141

    if (ret) goto error;

   142

    PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED;

   144

    /* create debug_wait_sem */

   145

    ret = rt_sem_create(&debug_wait_sem, "debug_wait_sem", 0, S_FIFO);

   146

    if (ret) goto error;

   147

    PLC_state |= PLC_STATE_DEBUG_WAIT_SEM_CREATED;

   149

    /* create debug_mutex */

   150

    ret = rt_mutex_create(&debug_mutex, "debug_mutex");

   151

    if (ret) goto error;

   152

    PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED;

   154

    /* create can_driver_task */

   155

    ret = rt_task_create(&PLC_task, "PLC_task", 0, 50, 0);

   156

    if (ret) goto error;

   157

    PLC_state |= PLC_STATE_TASK_CREATED;

   159

    ret = __init(argc,argv);

   160

    if (ret) goto error;

   161

   162

    /* start can_driver_task */

   163

    ret = rt_task_start(&PLC_task, &PLC_task_proc, NULL);

   164

    if (ret) goto error;

   165

   166

    return 0;

   167

   168

error:

   169

    PLC_cleanup_all();

   170

    return 1;

   171

}

   172

   173

int TryEnterDebugSection(void)

   174

{

   175

    return rt_mutex_acquire(&debug_mutex, TM_NONBLOCK) == 0;

   176

}

   177

   178

void LeaveDebugSection(void)

   179

{

   180

    rt_mutex_release(&debug_mutex);

   181

}

   182

   183

extern int __tick;

   184

/* from plc_debugger.c */

   185

int WaitDebugData()

   186

{

   187

    rt_task_shadow(&WaitDebug_task, "WaitDebug_task", 0, 0);

   188

    /* Wait signal from PLC thread */

   189

    rt_sem_p(&debug_wait_sem, TM_INFINITE);

   190

    return __debug_tick;

   191

}

   193

/* Called by PLC thread when debug_publish finished

   194

 * This is supposed to unlock debugger thread in WaitDebugData*/

   195

void InitiateDebugTransfer()

   196

{

   197

    /* remember tick */

   198

    __debug_tick = __tick;

   199

    /* signal debugger thread it can read data */

   200

    rt_sem_v(&debug_wait_sem);

   201

}

   202

   203

void suspendDebug(void)

   204

{

   205

    __DEBUG = 0;

   206

    /* Prevent PLC to enter debug code */

   207

    rt_mutex_acquire(&debug_mutex, TM_INFINITE);

   208

}

   209

   210

void resumeDebug(void)

   211

{

   212

    __DEBUG = 1;

   213

    /* Let PLC enter debug code */

   214

    rt_mutex_release(&debug_mutex);

   215

}

   216

   217

/* from plc_python.c */

   218

int WaitPythonCommands(void)

   219

{

   220

    rt_task_shadow(&WaitPythonCommand_task, "WaitPythonCommand_task", 0, 0);

   221

    /* Wait signal from PLC thread */

   222

    rt_sem_p(&python_wait_sem, TM_INFINITE);

   223

}

   225

/* Called by PLC thread on each new python command*/

   226

void UnBlockPythonCommands(void)

   227

{

   228

    /* signal debugger thread it can read data */

   229

    rt_sem_v(&python_wait_sem);

   230

}

   231

   232

int TryLockPython(void)

   233

{

   234

    return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0;

   235

}

   236

   237

void UnLockPython(void)

   238

{

   239

    rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0);

   240

    rt_mutex_release(&python_mutex);

   241

}

   242

   243

void LockPython(void)

   244

{

   245

    rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0);

   246

    rt_mutex_acquire(&python_mutex, TM_INFINITE);

   247

}