1

/**

     2

 * Xenomai Linux specific code

     3

 **/

     4

     5

#include <stdio.h>

     6

#include <unistd.h>

     7

#include <string.h>

     8

#include <time.h>

     9

#include <signal.h>

    10

#include <stdlib.h>

    11

#include <sys/mman.h>

    12

#include <sys/fcntl.h>

    13

    14

#include <native/task.h>

    15

#include <native/timer.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_DEBUG_FILE_OPENED            2 

    22

#define PLC_STATE_DEBUG_PIPE_CREATED           4 

    23

#define PLC_STATE_PYTHON_FILE_OPENED           8 

    24

#define PLC_STATE_PYTHON_PIPE_CREATED          16   

    25

#define PLC_STATE_WAITDEBUG_FILE_OPENED        32   

    26

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       64

    27

#define PLC_STATE_WAITPYTHON_FILE_OPENED       128

    28

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      256

    29

    30

#define WAITDEBUG_PIPE_DEVICE        "/dev/rtp0"

    31

#define WAITDEBUG_PIPE_MINOR         0

    32

#define DEBUG_PIPE_DEVICE            "/dev/rtp1"

    33

#define DEBUG_PIPE_MINOR             1

    34

#define WAITPYTHON_PIPE_DEVICE       "/dev/rtp2"

    35

#define WAITPYTHON_PIPE_MINOR        2

    36

#define PYTHON_PIPE_DEVICE           "/dev/rtp3"

    37

#define PYTHON_PIPE_MINOR            3

    38

#define PIPE_SIZE                    1 

    39

    40

    41

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

    42

{

    43

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    44

}

    45

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

    46

{

    47

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    48

}

    49

    50

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    51

{

    52

    RTIME current_time = rt_timer_read();

    53

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    54

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    55

}

    56

    57

RT_TASK PLC_task;

    58

RT_PIPE WaitDebug_pipe;

    59

RT_PIPE WaitPython_pipe;

    60

RT_PIPE Debug_pipe;

    61

RT_PIPE Python_pipe;

    62

int WaitDebug_pipe_fd;

    63

int WaitPython_pipe_fd;

    64

int Debug_pipe_fd;

    65

int Python_pipe_fd;

    66

    67

int PLC_shutdown = 0;

    68

    69

void PLC_SetTimer(unsigned long long next, unsigned long long period)

    70

{

    71

  RTIME current_time = rt_timer_read();

    72

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

    73

}

    74

    75

void PLC_task_proc(void *arg)

    76

{

    77

    PLC_SetTimer(common_ticktime__, common_ticktime__);

    78

    79

    while (!PLC_shutdown) {

    80

        PLC_GetTime(&__CURRENT_TIME);

    81

        __run();

    82

        if (PLC_shutdown) break;

    83

        rt_task_wait_period(NULL);

    84

    }

    85

}

    86

    87

static unsigned long __debug_tick;

    88

    89

void PLC_cleanup_all(void)

    90

{

    91

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    92

        rt_task_delete(&PLC_task);

    93

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    94

    }

    95

    96

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

    97

        rt_pipe_delete(&WaitDebug_pipe);

    98

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

    99

    }

   100

   101

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   102

        close(WaitDebug_pipe_fd);

   103

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   104

    }

   105

   106

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   107

        rt_pipe_delete(&WaitPython_pipe);

   108

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   109

    }

   110

   111

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   112

        close(WaitPython_pipe_fd);

   113

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   114

    }

   115

   116

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   117

        rt_pipe_delete(&Debug_pipe);

   118

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   119

    }

   120

   121

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   122

        close(Debug_pipe_fd);

   123

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   124

    }

   125

   126

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   127

        rt_pipe_delete(&Python_pipe);

   128

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   129

    }

   130

   131

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   132

        close(Python_pipe_fd);

   133

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   134

    }

   135

   136

}

   137

   138

int stopPLC()

   139

{

   140

    /* Stop the PLC */

   141

    PLC_shutdown = 1;

   142

   143

    /* Wait until PLC task stops */

   144

    rt_task_join(&PLC_task);

   145

   146

    PLC_cleanup_all();

   147

    __cleanup();

   148

    __debug_tick = -1;

   149

    return 0;

   150

}

   151

   152

//

   153

void catch_signal(int sig)

   154

{

   155

    stopPLC();

   156

//  signal(SIGTERM, catch_signal);

   157

    signal(SIGINT, catch_signal);

   158

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

   159

    exit(0);

   160

}

   161

   162

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

   163

int startPLC(int argc,char **argv)

   164

{

   165

    signal(SIGINT, catch_signal);

   166

   167

    /* no memory swapping for that process */

   168

    mlockall(MCL_CURRENT | MCL_FUTURE);

   169

   170

    PLC_shutdown = 0;

   171

   172

    /*** RT Pipes creation and opening ***/

   173

    /* create Debug_pipe */

   174

    if(rt_pipe_create(&Debug_pipe, "Debug_pipe", DEBUG_PIPE_MINOR, PIPE_SIZE)) 

   175

        goto error;

   176

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   177

   178

    /* open Debug_pipe*/

   179

    if((Debug_pipe_fd = open(DEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   180

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   181

   182

    /* create Python_pipe */

   183

    if(rt_pipe_create(&Python_pipe, "Python_pipe", PYTHON_PIPE_MINOR, PIPE_SIZE)) 

   184

        goto error;

   185

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   186

   187

    /* open Python_pipe*/

   188

    if((Python_pipe_fd = open(PYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   189

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   190

   191

    /* create WaitDebug_pipe */

   192

    if(rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR, PIPE_SIZE))

   193

        goto error;

   194

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   195

   196

    /* open WaitDebug_pipe*/

   197

    if((WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   198

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   199

   200

    /* create WaitPython_pipe */

   201

    if(rt_pipe_create(&WaitPython_pipe, "WaitPython_pipe", WAITPYTHON_PIPE_MINOR, PIPE_SIZE))

   202

        goto error;

   203

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   204

   205

    /* open WaitPython_pipe*/

   206

    if((WaitPython_pipe_fd = open(WAITPYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   207

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   208

   209

    /*** create PLC task ***/

   210

    if(rt_task_create(&PLC_task, "PLC_task", 0, 50, T_JOINABLE)) goto error;

   211

    PLC_state |= PLC_STATE_TASK_CREATED;

   212

   213

    if(__init(argc,argv)) goto error;

   214

   215

    /* start PLC task */

   216

    if(rt_task_start(&PLC_task, &PLC_task_proc, NULL)) goto error;

   217

   218

    return 0;

   219

   220

error:

   221

   222

    PLC_cleanup_all();

   223

    return 1;

   224

}

   225

   226

#define DEBUG_FREE 0

   227

#define DEBUG_BUSY 1

   228

static long debug_state = DEBUG_FREE;

   229

   230

int TryEnterDebugSection(void)

   231

{

   232

    if(AtomicCompareExchange(

   233

        &debug_state,

   234

        DEBUG_FREE,

   235

        DEBUG_BUSY) == DEBUG_FREE){

   236

        if(__DEBUG){

   237

            return 1;

   238

        }

   239

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   240

    }

   241

    return 0;

   242

}

   243

   244

#define DEBUG_UNLOCK 1

   245

void LeaveDebugSection(void)

   246

{

   247

    if(AtomicCompareExchange( &debug_state, 

   248

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   249

        char msg = DEBUG_UNLOCK;

   250

        /* signal to NRT for wakeup */

   251

        rt_pipe_write(&Debug_pipe, &msg, sizeof(msg), P_NORMAL);

   252

    }

   253

}

   254

   255

extern unsigned long __tick;

   256

   257

#define DEBUG_PENDING_DATA 1

   258

int WaitDebugData(unsigned long *tick)

   259

{

   260

    char cmd;

   261

    int res;

   262

    if (PLC_shutdown) return -1;

   263

    /* Wait signal from PLC thread */

   264

    res = read(WaitDebug_pipe_fd, &cmd, sizeof(cmd));

   265

    if (res == sizeof(cmd) && cmd == DEBUG_PENDING_DATA){

   266

        *tick = __debug_tick;

   267

        return 0;

   268

    }

   269

    return -1;

   270

}

   271

   272

/* Called by PLC thread when debug_publish finished

   273

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

   274

void InitiateDebugTransfer()

   275

{

   276

    char msg = DEBUG_PENDING_DATA;

   277

    /* remember tick */

   278

    __debug_tick = __tick;

   279

    /* signal debugger thread it can read data */

   280

    rt_pipe_write(&WaitDebug_pipe, &msg, sizeof(msg), P_NORMAL);

   281

}

   282

   283

int suspendDebug(int disable)

   284

{

   285

    char cmd = DEBUG_UNLOCK;

   286

    if (PLC_shutdown) return -1;

   287

    while(AtomicCompareExchange(

   288

            &debug_state,

   289

            DEBUG_FREE,

   290

            DEBUG_BUSY) != DEBUG_FREE &&

   291

            cmd == DEBUG_UNLOCK){

   292

       if(read(Debug_pipe_fd, &cmd, sizeof(cmd)) != sizeof(cmd)){

   293

           return -1;

   294

       }

   295

    }

   296

    __DEBUG = !disable;

   297

    if (disable)

   298

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   299

    return 0;

   300

}

   301

   302

void resumeDebug(void)

   303

{

   304

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   305

}

   306

   307

#define PYTHON_PENDING_COMMAND 1

   308

   309

#define PYTHON_FREE 0

   310

#define PYTHON_BUSY 1

   311

static long python_state = PYTHON_FREE;

   312

   313

int WaitPythonCommands(void)

   314

{ 

   315

    char cmd;

   316

    if (PLC_shutdown) return -1;

   317

    /* Wait signal from PLC thread */

   318

    if(read(WaitPython_pipe_fd, &cmd, sizeof(cmd))==sizeof(cmd) && cmd==PYTHON_PENDING_COMMAND){

   319

        return 0;

   320

    }

   321

    return -1;

   322

}

   323

   324

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

   325

void UnBlockPythonCommands(void)

   326

{

   327

    char msg = PYTHON_PENDING_COMMAND;

   328

    rt_pipe_write(&WaitPython_pipe, &msg, sizeof(msg), P_NORMAL);

   329

}

   330

   331

int TryLockPython(void)

   332

{

   333

    return AtomicCompareExchange(

   334

        &python_state,

   335

        PYTHON_FREE,

   336

        PYTHON_BUSY) == PYTHON_FREE;

   337

}

   338

   339

#define UNLOCK_PYTHON 1

   340

void LockPython(void)

   341

{

   342

    char cmd = UNLOCK_PYTHON;

   343

    if (PLC_shutdown) return;

   344

    while(AtomicCompareExchange(

   345

            &python_state,

   346

            PYTHON_FREE,

   347

            PYTHON_BUSY) != PYTHON_FREE &&

   348

            cmd == UNLOCK_PYTHON){

   349

       read(Python_pipe_fd, &cmd, sizeof(cmd));

   350

    }

   351

}

   352

   353

void UnLockPython(void)

   354

{

   355

    if(AtomicCompareExchange(

   356

            &python_state,

   357

            PYTHON_BUSY,

   358

            PYTHON_FREE) == PYTHON_BUSY){

   359

        if(rt_task_self()){/*is that the real time task ?*/

   360

           char cmd = UNLOCK_PYTHON;

   361

           rt_pipe_write(&Python_pipe, &cmd, sizeof(cmd), P_NORMAL);

   362

        }/* otherwise, no signaling from non real time */

   363

    }    /* as plc does not wait for lock. */

   364

}

   365