1

/**

     2

 * Linux specific code

     3

 **/

     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

#include <sys/fcntl.h>

    12

    13

#include <native/task.h>

    14

#include <native/timer.h>

    15

#include <native/mutex.h>

    16

#include <native/sem.h>

    17

#include <native/pipe.h>

    18

    19

unsigned int PLC_state = 0;

    20

#define PLC_STATE_TASK_CREATED                  1

    21

#define PLC_STATE_PYTHON_MUTEX_CREATED          2

    22

#define PLC_STATE_PYTHON_WAIT_SEM_CREATED       4

    23

#define PLC_STATE_DEBUG_MUTEX_CREATED           8

    24

#define PLC_STATE_DEBUG_FILE_OPENED             16

    25

#define PLC_STATE_DEBUG_PIPE_CREATED            32

    26

    27

#define WAITDEBUG_PIPE_DEVICE       "/dev/rtp0"

    28

#define WAITDEBUG_PIPE_MINOR        0

    29

#define WAITDEBUG_PIPE_SIZE         500

    30

    31

/* provided by POUS.C */

    33

    34

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

    35

{

    36

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    37

}

    38

    39

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    40

{

    41

    RTIME current_time = rt_timer_read();

    42

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    43

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    44

}

    45

    46

RT_TASK PLC_task;

    47

RT_PIPE WaitDebug_pipe;

    48

RT_TASK SuspendDebug_task;

    49

RT_TASK ResumeDebug_task;

    50

RT_TASK WaitPythonCommand_task;

    51

RT_TASK UnLockPython_task;

    52

RT_TASK LockPython_task;

    53

int PLC_shutdown = 0;

    54

    55

int WaitDebug_pipe_fd = -1;

    56

    57

void PLC_SetTimer(long long next, long long period)

    58

{

    59

  RTIME current_time = rt_timer_read();

    60

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

    61

}

    62

    63

void PLC_task_proc(void *arg)

    64

{

    65

    PLC_SetTimer(Ttick, Ttick);

    66

    67

    while (1) {

    68

        PLC_GetTime(&__CURRENT_TIME);

    69

        __run();

    70

        if (PLC_shutdown) break;

    71

        rt_task_wait_period(NULL);

    72

    }

    73

}

    74

    76

    77

RT_SEM python_wait_sem;

    78

RT_MUTEX python_mutex;

    79

RT_MUTEX debug_mutex;

    80

    81

void PLC_cleanup_all(void)

    82

{

    83

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    84

        rt_task_delete(&PLC_task);

    85

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    86

    }

    87

    88

    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {

    89

        rt_sem_v(&python_wait_sem);

    90

        rt_sem_delete(&python_wait_sem);

    91

        PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED;

    92

    }

    93

    94

    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {

    95

        rt_mutex_delete(&python_mutex);

    96

        PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED;

    97

    }

    98

    99

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   100

        rt_pipe_delete(&WaitDebug_pipe);

   101

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   102

    }

   103

   104

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   105

        close(WaitDebug_pipe_fd);

   106

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   107

    }

   108

   109

    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {

   110

        rt_mutex_delete(&debug_mutex);

   111

        PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED;

   112

    }

   113

}

   114

   115

int stopPLC()

   116

{

   117

    PLC_shutdown = 1;

   118

    /* Stop the PLC */

   119

    PLC_SetTimer(0, 0);

   120

    __cleanup();

   121

    PLC_cleanup_all();

   122

    __debug_tick = -1;

   123

}

   124

   125

//

   126

void catch_signal(int sig)

   127

{

   128

    stopPLC();

   129

//  signal(SIGTERM, catch_signal);

   130

    signal(SIGINT, catch_signal);

   131

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

   132

    exit(0);

   133

}

   134

   135

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

   136

int startPLC(int argc,char **argv)

   137

{

   138

    int ret = 0;

   139

   140

    signal(SIGINT, catch_signal);

   141

   142

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

   143

    mlockall(MCL_CURRENT | MCL_FUTURE);

   144

   147

   148

    /* create python_wait_sem */

   149

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

   150

    if (ret) goto error;

   151

    PLC_state |= PLC_STATE_PYTHON_WAIT_SEM_CREATED;

   152

   153

    /* create python_mutex */

   154

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

   155

    if (ret) goto error;

   156

    PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED;

   157

   158

    /* create WaitDebug_pipe */

   159

    ret = rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR,

   160

          WAITDEBUG_PIPE_SIZE * sizeof(char));

   161

    if (ret) goto error;

   162

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   163

   164

    /* open WaitDebug_pipe*/

   165

    WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR);

   166

    if (WaitDebug_pipe_fd == -1) {

   167

        ret = -EBADF;

   168

        goto error;

   169

    }

   170

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   171

   172

    /* create debug_mutex */

   173

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

   174

    if (ret) goto error;

   175

    PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED;

   176

   177

    /* create can_driver_task */

   178

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

   179

    if (ret) goto error;

   180

    PLC_state |= PLC_STATE_TASK_CREATED;

   181

   182

    ret = __init(argc,argv);

   183

    if (ret) goto error;

   184

   185

    /* start can_driver_task */

   186

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

   187

    if (ret) goto error;

   188

   189

    return 0;

   190

   191

error:

   192

    PLC_cleanup_all();

   193

    return 1;

   194

}

   195

   196

int TryEnterDebugSection(void)

   197

{

   198

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

   199

}

   200

   201

void LeaveDebugSection(void)

   202

{

   203

    rt_mutex_release(&debug_mutex);

   204

}

   205

   207

/* from plc_debugger.c */

   209

{

   210

    char message;

   211

    int res;

   212

    /* Wait signal from PLC thread */

   213

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   214

        res = read(WaitDebug_pipe_fd, &message, sizeof(char));

   215

        if (res == sizeof(char))

   216

            return __debug_tick;

   217

    }

   218

    return -1;

   219

}

   220

   221

/* Called by PLC thread when debug_publish finished

   222

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

   223

void InitiateDebugTransfer()

   224

{

   225

    char message = 1;

   226

    /* remember tick */

   227

    __debug_tick = __tick;

   228

    /* signal debugger thread it can read data */

   229

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED)

   230

        rt_pipe_write(&WaitDebug_pipe, &message, sizeof(char), P_NORMAL);

   231

}

   232

   233

void suspendDebug(void)

   234

{

   235

    __DEBUG = 0;

   236

    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {

   237

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

   238

        /* Prevent PLC to enter debug code */

   239

        rt_mutex_acquire(&debug_mutex, TM_INFINITE);

   240

    }

   241

}

   242

   243

void resumeDebug(void)

   244

{

   245

    __DEBUG = 1;

   246

    if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) {

   247

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

   248

        /* Let PLC enter debug code */

   249

        rt_mutex_release(&debug_mutex);

   250

    }

   251

}

   252

   253

/* from plc_python.c */

   254

int WaitPythonCommands(void)

   255

{

   256

    /* Wait signal from PLC thread */

   257

    if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) {

   258

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

   259

        return rt_sem_p(&python_wait_sem, TM_INFINITE);

   260

    }

   261

    return -1;

   262

}

   263

   264

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

   265

void UnBlockPythonCommands(void)

   266

{

   267

    /* signal debugger thread it can read data */

   268

    rt_sem_v(&python_wait_sem);

   269

}

   270

   271

int TryLockPython(void)

   272

{

   273

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

   274

}

   275

   276

void UnLockPython(void)

   277

{

   278

    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {

   279

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

   280

        rt_mutex_release(&python_mutex);

   281

    }

   282

}

   283

   284

void LockPython(void)

   285

{

   286

    if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) {

   287

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

   288

        rt_mutex_acquire(&python_mutex, TM_INFINITE);

   289

    }

   290

}