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/mutex.h>

    17

#include <native/sem.h>

    18

#include <native/pipe.h>

    19

    20

unsigned int PLC_state = 0;

    21

#define PLC_STATE_TASK_CREATED                 1

    22

#define PLC_STATE_DEBUG_FILE_OPENED            2 

    23

#define PLC_STATE_DEBUG_PIPE_CREATED           4 

    24

#define PLC_STATE_PYTHON_FILE_OPENED           8 

    25

#define PLC_STATE_PYTHON_PIPE_CREATED          16   

    26

#define PLC_STATE_WAITDEBUG_FILE_OPENED        32   

    27

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       64

    28

#define PLC_STATE_WAITPYTHON_FILE_OPENED       128

    29

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      256

    30

    31

#define WAITDEBUG_PIPE_DEVICE        "/dev/rtp0"

    32

#define WAITDEBUG_PIPE_MINOR         0

    33

#define DEBUG_PIPE_DEVICE            "/dev/rtp1"

    34

#define DEBUG_PIPE_MINOR             1

    35

#define WAITPYTHON_PIPE_DEVICE       "/dev/rtp2"

    36

#define WAITPYTHON_PIPE_MINOR        2

    37

#define PYTHON_PIPE_DEVICE           "/dev/rtp3"

    38

#define PYTHON_PIPE_MINOR            3

    39

#define PIPE_SIZE                    1 

    40

    41

    42

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

    43

{

    44

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    45

}

    46

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

    47

{

    48

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    49

}

    50

    51

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    52

{

    53

    RTIME current_time = rt_timer_read();

    54

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    55

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    56

}

    57

    58

RT_TASK PLC_task;

    59

RT_PIPE WaitDebug_pipe;

    60

RT_PIPE WaitPython_pipe;

    61

RT_PIPE Debug_pipe;

    62

RT_PIPE Python_pipe;

    63

int WaitDebug_pipe_fd;

    64

int WaitPython_pipe_fd;

    65

int Debug_pipe_fd;

    66

int Python_pipe_fd;

    67

    68

int PLC_shutdown = 0;

    69

    70

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

    71

{

    72

  RTIME current_time = rt_timer_read();

    73

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

    74

}

    75

    76

void PLC_task_proc(void *arg)

    77

{

    78

    PLC_SetTimer(Ttick, Ttick);

    79

    80

    while (!PLC_shutdown) {

    81

        PLC_GetTime(&__CURRENT_TIME);

    82

        __run();

    83

        if (PLC_shutdown) break;

    84

        rt_task_wait_period(NULL);

    85

    }

    86

}

    87

    88

static unsigned long __debug_tick;

    89

    90

void PLC_cleanup_all(void)

    91

{

    92

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    93

        rt_task_delete(&PLC_task);

    94

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    95

    }

    96

    97

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

    98

        rt_pipe_delete(&WaitDebug_pipe);

    99

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   100

    }

   101

   102

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   103

        close(WaitDebug_pipe_fd);

   104

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   105

    }

   106

   107

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   108

        rt_pipe_delete(&WaitPython_pipe);

   109

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   110

    }

   111

   112

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   113

        close(WaitPython_pipe_fd);

   114

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   115

    }

   116

   117

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   118

        rt_pipe_delete(&Debug_pipe);

   119

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   120

    }

   121

   122

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   123

        close(Debug_pipe_fd);

   124

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   125

    }

   126

   127

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   128

        rt_pipe_delete(&Python_pipe);

   129

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   130

    }

   131

   132

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   133

        close(Python_pipe_fd);

   134

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   135

    }

   136

   137

}

   138

   139

int stopPLC()

   140

{

   141

    /* Stop the PLC */

   142

    PLC_shutdown = 1;

   143

   144

    /* Wait until PLC task stops */

   145

    rt_task_join(&PLC_task);

   146

   147

    PLC_cleanup_all();

   148

    __cleanup();

   149

    __debug_tick = -1;

   150

    return 0;

   151

}

   152

   153

//

   154

void catch_signal(int sig)

   155

{

   156

    stopPLC();

   157

//  signal(SIGTERM, catch_signal);

   158

    signal(SIGINT, catch_signal);

   159

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

   160

    exit(0);

   161

}

   162

   163

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

   164

int startPLC(int argc,char **argv)

   165

{

   166

    signal(SIGINT, catch_signal);

   167

   168

    /* no memory swapping for that process */

   169

    mlockall(MCL_CURRENT | MCL_FUTURE);

   170

   171

    PLC_shutdown = 0;

   172

   173

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

   174

    /* create Debug_pipe */

   175

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

   176

        goto error;

   177

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   178

   179

    /* open Debug_pipe*/

   180

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

   181

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   182

   183

    /* create Python_pipe */

   184

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

   185

        goto error;

   186

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   187

   188

    /* open Python_pipe*/

   189

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

   190

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   191

   192

    /* create WaitDebug_pipe */

   193

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

   194

        goto error;

   195

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   196

   197

    /* open WaitDebug_pipe*/

   198

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

   199

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   200

   201

    /* create WaitPython_pipe */

   202

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

   203

        goto error;

   204

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   205

   206

    /* open WaitPython_pipe*/

   207

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

   208

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   209

   210

    /*** create PLC task ***/

   211

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

   212

    PLC_state |= PLC_STATE_TASK_CREATED;

   213

   214

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

   215

   216

    /* start PLC task */

   217

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

   218

   219

    return 0;

   220

   221

error:

   222

   223

    PLC_cleanup_all();

   224

    return 1;

   225

}

   226

   227

#define DEBUG_FREE 0

   228

#define DEBUG_BUSY 1

   229

static long debug_state = DEBUG_FREE;

   230

   231

int TryEnterDebugSection(void)

   232

{

   233

    if(AtomicCompareExchange(

   234

        &debug_state,

   235

        DEBUG_FREE,

   236

        DEBUG_BUSY) == DEBUG_FREE){

   237

        if(__DEBUG){

   238

            return 1;

   239

        }

   240

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   241

    }

   242

    return 0;

   243

}

   244

   245

#define DEBUG_UNLOCK 1

   246

void LeaveDebugSection(void)

   247

{

   248

    if(AtomicCompareExchange( &debug_state, 

   249

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   250

        char msg = DEBUG_UNLOCK;

   251

        /* signal to NRT for wakeup */

   252

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

   253

    }

   254

}

   255

   256

extern unsigned long __tick;

   257

   258

#define DEBUG_PENDING_DATA 1

   259

int WaitDebugData(unsigned long *tick)

   260

{

   261

    char cmd;

   262

    int res;

   263

    if (PLC_shutdown) return -1;

   264

    /* Wait signal from PLC thread */

   265

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

   266

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

   267

        *tick = __debug_tick;

   268

        return 0;

   269

    }

   270

    return -1;

   271

}

   272

   273

/* Called by PLC thread when debug_publish finished

   274

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

   275

void InitiateDebugTransfer()

   276

{

   277

    char msg = DEBUG_PENDING_DATA;

   278

    /* remember tick */

   279

    __debug_tick = __tick;

   280

    /* signal debugger thread it can read data */

   281

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

   282

}

   283

   284

int suspendDebug(int disable)

   285

{

   286

    char cmd = DEBUG_UNLOCK;

   287

    if (PLC_shutdown) return -1;

   288

    while(AtomicCompareExchange(

   289

            &debug_state,

   290

            DEBUG_FREE,

   291

            DEBUG_BUSY) != DEBUG_FREE &&

   292

            cmd == DEBUG_UNLOCK){

   293

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

   294

           return -1;

   295

       }

   296

    }

   297

    __DEBUG = !disable;

   298

    if (disable)

   299

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   300

    return 0;

   301

}

   302

   303

void resumeDebug(void)

   304

{

   305

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   306

}

   307

   308

#define PYTHON_PENDING_COMMAND 1

   309

   310

#define PYTHON_FREE 0

   311

#define PYTHON_BUSY 1

   312

static long python_state = PYTHON_FREE;

   313

   314

int WaitPythonCommands(void)

   315

{ 

   316

    char cmd;

   317

    if (PLC_shutdown) return -1;

   318

    /* Wait signal from PLC thread */

   319

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

   320

        return 0;

   321

    }

   322

    return -1;

   323

}

   324

   325

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

   326

void UnBlockPythonCommands(void)

   327

{

   328

    char msg = PYTHON_PENDING_COMMAND;

   329

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

   330

}

   331

   332

int TryLockPython(void)

   333

{

   334

    return AtomicCompareExchange(

   335

        &python_state,

   336

        PYTHON_FREE,

   337

        PYTHON_BUSY) == PYTHON_FREE;

   338

}

   339

   340

#define UNLOCK_PYTHON 1

   341

void LockPython(void)

   342

{

   343

    char cmd = UNLOCK_PYTHON;

   344

    if (PLC_shutdown) return;

   345

    while(AtomicCompareExchange(

   346

            &python_state,

   347

            PYTHON_FREE,

   348

            PYTHON_BUSY) != PYTHON_FREE &&

   349

            cmd == UNLOCK_PYTHON){

   350

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

   351

    }

   352

}

   353

   354

void UnLockPython(void)

   355

{

   356

    if(AtomicCompareExchange(

   357

            &python_state,

   358

            PYTHON_BUSY,

   359

            PYTHON_FREE) == PYTHON_BUSY){

   360

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

   361

           char cmd = UNLOCK_PYTHON;

   362

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

   363

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

   364

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

   365

}

   366

   367

int CheckRetainBuffer(void)

   368

{

   369

	return 1;

   370

}

   371

   372

void ValidateRetainBuffer(void)

   373

{

   374

}

   375

   376

void InValidateRetainBuffer(void)

   377

{

   378

}

   379

   380

void Retain(unsigned int offset, unsigned int count, void *p)

   381

{

   382

}

   383

   384

void Remind(unsigned int offset, unsigned int count, void *p)

   385

{

   386

}