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 <alchemy/task.h>

    15

#include <alchemy/timer.h>

    16

#include <alchemy/sem.h>

    17

#include <alchemy/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

    31

    32

#define WAITDEBUG_PIPE_DEVICE        "/dev/rtp0"

    33

#define WAITDEBUG_PIPE_MINOR         0

    34

#define DEBUG_PIPE_DEVICE            "/dev/rtp1"

    35

#define DEBUG_PIPE_MINOR             1

    36

#define WAITPYTHON_PIPE_DEVICE       "/dev/rtp2"

    37

#define WAITPYTHON_PIPE_MINOR        2

    38

#define PYTHON_PIPE_DEVICE           "/dev/rtp3"

    39

#define PYTHON_PIPE_MINOR            3

    42

#define PIPE_SIZE                    1 

    43

    44

// rt-pipes commands

    45

    46

#define PYTHON_PENDING_COMMAND 1

    47

#define PYTHON_FINISH 2

    48

    49

#define DEBUG_FINISH 2

    50

    51

#define DEBUG_PENDING_DATA 1

    52

#define DEBUG_UNLOCK 1

    53

    54

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

    55

{

    56

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    57

}

    58

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

    59

{

    60

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    61

}

    62

    63

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    64

{

    65

    RTIME current_time = rt_timer_read();

    66

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    67

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    68

}

    69

    70

RT_TASK PLC_task;

    71

RT_PIPE WaitDebug_pipe;

    72

RT_PIPE WaitPython_pipe;

    73

RT_PIPE Debug_pipe;

    74

RT_PIPE Python_pipe;

    76

int WaitDebug_pipe_fd;

    77

int WaitPython_pipe_fd;

    78

int Debug_pipe_fd;

    79

int Python_pipe_fd;

    81

    82

int PLC_shutdown = 0;

    83

    84

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

    85

{

    86

  RTIME current_time = rt_timer_read();

    87

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

    88

}

    89

    90

void PLC_task_proc(void *arg)

    91

{

    92

    PLC_SetTimer(common_ticktime__, common_ticktime__);

    93

    94

    while (!PLC_shutdown) {

    95

        PLC_GetTime(&__CURRENT_TIME);

    96

        __run();

    97

        if (PLC_shutdown) break;

    98

        rt_task_wait_period(NULL);

    99

    }

   100

    /* since xenomai 3 it is not enough to close() 

   101

       file descriptor to unblock read()... */

   102

    {

   103

        /* explicitely finish python thread */

   104

        char msg = PYTHON_FINISH;

   105

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

   106

    }

   107

    {

   108

        /* explicitely finish debug thread */

   109

        char msg = DEBUG_FINISH;

   110

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

   111

    }

   112

}

   113

   114

static unsigned long __debug_tick;

   115

   116

void PLC_cleanup_all(void)

   117

{

   118

    if (PLC_state & PLC_STATE_TASK_CREATED) {

   119

        rt_task_delete(&PLC_task);

   120

        PLC_state &= ~PLC_STATE_TASK_CREATED;

   121

    }

   122

   133

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   134

        rt_pipe_delete(&WaitDebug_pipe);

   135

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   136

    }

   137

   138

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   139

        close(WaitDebug_pipe_fd);

   140

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   141

    }

   142

   143

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   144

        rt_pipe_delete(&WaitPython_pipe);

   145

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   146

    }

   147

   148

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   149

        close(WaitPython_pipe_fd);

   150

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   151

    }

   152

   153

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   154

        rt_pipe_delete(&Debug_pipe);

   155

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   156

    }

   157

   158

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   159

        close(Debug_pipe_fd);

   160

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   161

    }

   162

   163

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   164

        rt_pipe_delete(&Python_pipe);

   165

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   166

    }

   167

   168

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   169

        close(Python_pipe_fd);

   170

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   171

    }

   172

   173

}

   174

   175

int stopPLC()

   176

{

   177

    /* Stop the PLC */

   178

    PLC_shutdown = 1;

   179

   180

    /* Wait until PLC task stops */

   181

    rt_task_join(&PLC_task);

   182

   183

    PLC_cleanup_all();

   184

    __cleanup();

   185

    __debug_tick = -1;

   186

    return 0;

   187

}

   188

   189

//

   190

void catch_signal(int sig)

   191

{

   192

    stopPLC();

   193

//  signal(SIGTERM, catch_signal);

   194

    signal(SIGINT, catch_signal);

   195

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

   196

    exit(0);

   197

}

   198

   199

#define _startPLCLog(text) \

   200

    {\

   201

    	char mstr[] = text;\

   202

        LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));\

   203

        goto error;\

   204

    }

   205

   206

#define FO "Failed opening "

   207

   208

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

   209

int startPLC(int argc,char **argv)

   210

{

   211

    signal(SIGINT, catch_signal);

   212

   213

    /* no memory swapping for that process */

   214

    mlockall(MCL_CURRENT | MCL_FUTURE);

   215

   216

    PLC_shutdown = 0;

   217

   218

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

   219

    /* create Debug_pipe */

   220

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

   221

        _startPLCLog(FO "Debug_pipe real-time end");

   222

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   223

   224

    /* open Debug_pipe*/

   225

    if((Debug_pipe_fd = open(DEBUG_PIPE_DEVICE, O_RDWR)) == -1)

   226

        _startPLCLog(FO DEBUG_PIPE_DEVICE);

   227

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   228

   229

    /* create Python_pipe */

   230

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

   231

        _startPLCLog(FO "Python_pipe real-time end");

   232

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   233

   234

    /* open Python_pipe*/

   235

    if((Python_pipe_fd = open(PYTHON_PIPE_DEVICE, O_RDWR)) == -1)

   236

        _startPLCLog(FO PYTHON_PIPE_DEVICE);

   237

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   238

   239

    /* create WaitDebug_pipe */

   240

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

   241

        _startPLCLog(FO "WaitDebug_pipe real-time end");

   242

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   243

   244

    /* open WaitDebug_pipe*/

   245

    if((WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR)) == -1)

   246

        _startPLCLog(FO WAITDEBUG_PIPE_DEVICE);

   247

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   248

   249

    /* create WaitPython_pipe */

   250

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

   251

        _startPLCLog(FO "WaitPython_pipe real-time end");

   252

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   253

   254

    /* open WaitPython_pipe*/

   255

    if((WaitPython_pipe_fd = open(WAITPYTHON_PIPE_DEVICE, O_RDWR)) == -1)

   256

        _startPLCLog(FO WAITPYTHON_PIPE_DEVICE);

   257

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   258

   269

    /*** create PLC task ***/

   270

    if(rt_task_create(&PLC_task, "PLC_task", 0, 50, T_JOINABLE))

   271

        _startPLCLog("Failed creating PLC task");

   272

    PLC_state |= PLC_STATE_TASK_CREATED;

   273

   274

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

   275

   276

    /* start PLC task */

   277

    if(rt_task_start(&PLC_task, &PLC_task_proc, NULL))

   278

        _startPLCLog("Failed starting PLC task");

   279

   280

    return 0;

   281

   282

error:

   283

   284

    PLC_cleanup_all();

   285

    return 1;

   286

}

   287

   288

#define DEBUG_FREE 0

   289

#define DEBUG_BUSY 1

   290

static long debug_state = DEBUG_FREE;

   291

   292

int TryEnterDebugSection(void)

   293

{

   294

    if(AtomicCompareExchange(

   295

        &debug_state,

   296

        DEBUG_FREE,

   297

        DEBUG_BUSY) == DEBUG_FREE){

   298

        if(__DEBUG){

   299

            return 1;

   300

        }

   301

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   302

    }

   303

    return 0;

   304

}

   305

   306

void LeaveDebugSection(void)

   307

{

   308

    if(AtomicCompareExchange( &debug_state, 

   309

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   310

        char msg = DEBUG_UNLOCK;

   311

        /* signal to NRT for wakeup */

   312

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

   313

    }

   314

}

   315

   316

extern unsigned long __tick;

   317

   318

int WaitDebugData(unsigned long *tick)

   319

{

   320

    char cmd;

   321

    int res;

   322

    if (PLC_shutdown) return -1;

   323

    /* Wait signal from PLC thread */

   324

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

   325

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

   326

        *tick = __debug_tick;

   327

        return 0;

   328

    }

   329

    return -1;

   330

}

   331

   332

/* Called by PLC thread when debug_publish finished

   333

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

   334

void InitiateDebugTransfer()

   335

{

   336

    char msg = DEBUG_PENDING_DATA;

   337

    /* remember tick */

   338

    __debug_tick = __tick;

   339

    /* signal debugger thread it can read data */

   340

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

   341

}

   342

   343

int suspendDebug(int disable)

   344

{

   345

    char cmd = DEBUG_UNLOCK;

   346

    if (PLC_shutdown) return -1;

   347

    while(AtomicCompareExchange(

   348

            &debug_state,

   349

            DEBUG_FREE,

   350

            DEBUG_BUSY) != DEBUG_FREE &&

   351

            cmd == DEBUG_UNLOCK){

   352

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

   353

           return -1;

   354

       }

   355

    }

   356

    __DEBUG = !disable;

   357

    if (disable)

   358

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   359

    return 0;

   360

}

   361

   362

void resumeDebug(void)

   363

{

   364

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   365

}

   366

   367

#define PYTHON_FREE 0

   368

#define PYTHON_BUSY 1

   369

static long python_state = PYTHON_FREE;

   370

   371

int WaitPythonCommands(void)

   372

{ 

   373

    char cmd;

   374

    if (PLC_shutdown) return -1;

   375

    /* Wait signal from PLC thread */

   376

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

   377

        return 0;

   378

    }

   379

    return -1;

   380

}

   381

   382

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

   383

void UnBlockPythonCommands(void)

   384

{

   385

    char msg = PYTHON_PENDING_COMMAND;

   386

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

   387

}

   388

   389

int TryLockPython(void)

   390

{

   391

    return AtomicCompareExchange(

   392

        &python_state,

   393

        PYTHON_FREE,

   394

        PYTHON_BUSY) == PYTHON_FREE;

   395

}

   396

   397

#define UNLOCK_PYTHON 1

   398

void LockPython(void)

   399

{

   400

    char cmd = UNLOCK_PYTHON;

   401

    if (PLC_shutdown) return;

   402

    while(AtomicCompareExchange(

   403

            &python_state,

   404

            PYTHON_FREE,

   405

            PYTHON_BUSY) != PYTHON_FREE &&

   406

            cmd == UNLOCK_PYTHON){

   407

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

   408

    }

   409

}

   410

   411

void UnLockPython(void)

   412

{

   413

    if(AtomicCompareExchange(

   414

            &python_state,

   415

            PYTHON_BUSY,

   416

            PYTHON_FREE) == PYTHON_BUSY){

   417

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

   418

           char cmd = UNLOCK_PYTHON;

   419

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

   420

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

   421

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

   422

}

   423

   436

#ifndef HAVE_RETAIN

   437

int CheckRetainBuffer(void)

   438

{

   439

	return 1;

   440

}

   441

   442

void ValidateRetainBuffer(void)

   443

{

   444

}

   445

   446

void InValidateRetainBuffer(void)

   447

{

   448

}

   449

   450

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

   451

{

   452

}

   453

   454

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

   455

{

   456

}

   457

   458

void CleanupRetain(void)

   459

{

   460

}

   461

   462

void InitRetain(void)

   463

{

   464

}

   465

#endif // !HAVE_RETAIN