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

/* provided by POUS.C */

    42

extern unsigned long common_ticktime__;

    43

    44

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

    45

{

    46

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    47

}

    48

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

    49

{

    50

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    51

}

    52

    53

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    54

{

    55

    RTIME current_time = rt_timer_read();

    56

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    57

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    58

}

    59

    60

RT_TASK PLC_task;

    61

RT_PIPE WaitDebug_pipe;

    62

RT_PIPE WaitPython_pipe;

    63

RT_PIPE Debug_pipe;

    64

RT_PIPE Python_pipe;

    65

int WaitDebug_pipe_fd;

    66

int WaitPython_pipe_fd;

    67

int Debug_pipe_fd;

    68

int Python_pipe_fd;

    69

    70

int PLC_shutdown = 0;

    71

    72

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

    73

{

    74

  RTIME current_time = rt_timer_read();

    75

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

    76

}

    77

    78

void PLC_task_proc(void *arg)

    79

{

    80

    PLC_SetTimer(Ttick, Ttick);

    81

    82

    while (!PLC_shutdown) {

    83

        PLC_GetTime(&__CURRENT_TIME);

    84

        __run();

    85

        if (PLC_shutdown) break;

    86

        rt_task_wait_period(NULL);

    87

    }

    88

}

    89

    90

static unsigned long __debug_tick;

    91

    92

void PLC_cleanup_all(void)

    93

{

    94

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    95

        rt_task_delete(&PLC_task);

    96

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    97

    }

    98

    99

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   100

        rt_pipe_delete(&WaitDebug_pipe);

   101

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   102

    }

   103

   104

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   105

        close(WaitDebug_pipe_fd);

   106

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   107

    }

   108

   109

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   110

        rt_pipe_delete(&WaitPython_pipe);

   111

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   112

    }

   113

   114

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   115

        close(WaitPython_pipe_fd);

   116

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   117

    }

   118

   119

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   120

        rt_pipe_delete(&Debug_pipe);

   121

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   122

    }

   123

   124

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   125

        close(Debug_pipe_fd);

   126

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   127

    }

   128

   129

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   130

        rt_pipe_delete(&Python_pipe);

   131

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   132

    }

   133

   134

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   135

        close(Python_pipe_fd);

   136

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   137

    }

   138

   139

}

   140

   141

int stopPLC()

   142

{

   143

    /* Stop the PLC */

   144

    PLC_shutdown = 1;

   145

   146

    /* Wait until PLC task stops */

   147

    rt_task_join(&PLC_task);

   148

   149

    PLC_cleanup_all();

   150

    __cleanup();

   151

    __debug_tick = -1;

   152

    return 0;

   153

}

   154

   155

//

   156

void catch_signal(int sig)

   157

{

   158

    stopPLC();

   159

//  signal(SIGTERM, catch_signal);

   160

    signal(SIGINT, catch_signal);

   161

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

   162

    exit(0);

   163

}

   164

   165

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

   166

int startPLC(int argc,char **argv)

   167

{

   168

    signal(SIGINT, catch_signal);

   169

   170

    /* no memory swapping for that process */

   171

    mlockall(MCL_CURRENT | MCL_FUTURE);

   172

   173

    /* Define Ttick to 1ms if common_ticktime not defined */

   174

    Ttick = common_ticktime__?common_ticktime__:1000000;

   175

   176

    PLC_shutdown = 0;

   177

   178

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

   179

    /* create Debug_pipe */

   180

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

   181

        goto error;

   182

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   183

   184

    /* open Debug_pipe*/

   185

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

   186

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   187

   188

    /* create Python_pipe */

   189

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

   190

        goto error;

   191

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   192

   193

    /* open Python_pipe*/

   194

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

   195

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   196

   197

    /* create WaitDebug_pipe */

   198

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

   199

        goto error;

   200

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   201

   202

    /* open WaitDebug_pipe*/

   203

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

   204

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   205

   206

    /* create WaitPython_pipe */

   207

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

   208

        goto error;

   209

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   210

   211

    /* open WaitPython_pipe*/

   212

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

   213

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   214

   215

    /*** create PLC task ***/

   216

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

   217

    PLC_state |= PLC_STATE_TASK_CREATED;

   218

   219

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

   220

   221

    /* start PLC task */

   222

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

   223

   224

    return 0;

   225

   226

error:

   227

   228

    PLC_cleanup_all();

   229

    return 1;

   230

}

   231

   232

#define DEBUG_FREE 0

   233

#define DEBUG_BUSY 1

   234

static long debug_state = DEBUG_FREE;

   235

   236

int TryEnterDebugSection(void)

   237

{

   238

    if(AtomicCompareExchange(

   239

        &debug_state,

   240

        DEBUG_FREE,

   241

        DEBUG_BUSY) == DEBUG_FREE){

   242

        if(__DEBUG){

   243

            return 1;

   244

        }

   245

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   246

    }

   247

    return 0;

   248

}

   249

   250

#define DEBUG_UNLOCK 1

   251

void LeaveDebugSection(void)

   252

{

   253

    if(AtomicCompareExchange( &debug_state, 

   254

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   255

        char msg = DEBUG_UNLOCK;

   256

        /* signal to NRT for wakeup */

   257

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

   258

    }

   259

}

   260

   261

extern unsigned long __tick;

   262

   263

#define DEBUG_PENDING_DATA 1

   264

int WaitDebugData(unsigned long *tick)

   265

{

   266

    char cmd;

   267

    int res;

   268

    if (PLC_shutdown) return -1;

   269

    /* Wait signal from PLC thread */

   270

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

   271

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

   272

        *tick = __debug_tick;

   273

        return 0;

   274

    }

   275

    return -1;

   276

}

   277

   278

/* Called by PLC thread when debug_publish finished

   279

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

   280

void InitiateDebugTransfer()

   281

{

   282

    char msg = DEBUG_PENDING_DATA;

   283

    /* remember tick */

   284

    __debug_tick = __tick;

   285

    /* signal debugger thread it can read data */

   286

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

   287

}

   288

   289

int suspendDebug(int disable)

   290

{

   291

    char cmd = DEBUG_UNLOCK;

   292

    if (PLC_shutdown) return -1;

   293

    while(AtomicCompareExchange(

   294

            &debug_state,

   295

            DEBUG_FREE,

   296

            DEBUG_BUSY) != DEBUG_FREE &&

   297

            cmd == DEBUG_UNLOCK){

   298

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

   299

           return -1;

   300

       }

   301

    }

   302

    __DEBUG = !disable;

   303

    if (disable)

   304

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   305

    return 0;

   306

}

   307

   308

void resumeDebug(void)

   309

{

   310

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   311

}

   312

   313

#define PYTHON_PENDING_COMMAND 1

   314

   315

#define PYTHON_FREE 0

   316

#define PYTHON_BUSY 1

   317

static long python_state = PYTHON_FREE;

   318

   319

int WaitPythonCommands(void)

   320

{ 

   321

    char cmd;

   322

    if (PLC_shutdown) return -1;

   323

    /* Wait signal from PLC thread */

   324

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

   325

        return 0;

   326

    }

   327

    return -1;

   328

}

   329

   330

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

   331

void UnBlockPythonCommands(void)

   332

{

   333

    char msg = PYTHON_PENDING_COMMAND;

   334

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

   335

}

   336

   337

int TryLockPython(void)

   338

{

   339

    return AtomicCompareExchange(

   340

        &python_state,

   341

        PYTHON_FREE,

   342

        PYTHON_BUSY) == PYTHON_FREE;

   343

}

   344

   345

#define UNLOCK_PYTHON 1

   346

void LockPython(void)

   347

{

   348

    char cmd = UNLOCK_PYTHON;

   349

    if (PLC_shutdown) return;

   350

    while(AtomicCompareExchange(

   351

            &python_state,

   352

            PYTHON_FREE,

   353

            PYTHON_BUSY) != PYTHON_FREE &&

   354

            cmd == UNLOCK_PYTHON){

   355

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

   356

    }

   357

}

   358

   359

void UnLockPython(void)

   360

{

   361

    if(AtomicCompareExchange(

   362

            &python_state,

   363

            PYTHON_BUSY,

   364

            PYTHON_FREE) == PYTHON_BUSY){

   365

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

   366

           char cmd = UNLOCK_PYTHON;

   367

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

   368

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

   369

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

   370

}

   371

   372

int CheckRetainBuffer(void)

   373

{

   374

	return 1;

   375

}

   376

   377

void ValidateRetainBuffer(void)

   378

{

   379

}

   380

   381

void InValidateRetainBuffer(void)

   382

{

   383

}

   384

   385

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

   386

{

   387

}

   388

   389

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

   390

{

   391

}