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

    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

// rt-pipes commands

    41

    42

#define PYTHON_PENDING_COMMAND 1

    43

#define PYTHON_FINISH 2

    44

    45

#define DEBUG_FINISH 2

    46

    47

#define DEBUG_PENDING_DATA 1

    48

#define DEBUG_UNLOCK 1

    49

    50

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

    51

{

    52

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    53

}

    54

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

    55

{

    56

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    57

}

    58

    59

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    60

{

    61

    RTIME current_time = rt_timer_read();

    62

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    63

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    64

}

    65

    66

RT_TASK PLC_task;

    67

RT_PIPE WaitDebug_pipe;

    68

RT_PIPE WaitPython_pipe;

    69

RT_PIPE Debug_pipe;

    70

RT_PIPE Python_pipe;

    71

int WaitDebug_pipe_fd;

    72

int WaitPython_pipe_fd;

    73

int Debug_pipe_fd;

    74

int Python_pipe_fd;

    75

    76

int PLC_shutdown = 0;

    77

    78

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

    79

{

    80

  RTIME current_time = rt_timer_read();

    81

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

    82

}

    83

    84

void PLC_task_proc(void *arg)

    85

{

    86

    PLC_SetTimer(common_ticktime__, common_ticktime__);

    87

    88

    while (!PLC_shutdown) {

    89

        PLC_GetTime(&__CURRENT_TIME);

    90

        __run();

    91

        if (PLC_shutdown) break;

    92

        rt_task_wait_period(NULL);

    93

    }

    94

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

    95

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

    96

    {

    97

        /* explicitely finish python thread */

    98

        char msg = PYTHON_FINISH;

    99

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

   100

    }

   101

    {

   102

        /* explicitely finish debug thread */

   103

        char msg = DEBUG_FINISH;

   104

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

   105

    }

   106

}

   107

   108

static unsigned long __debug_tick;

   109

   110

void PLC_cleanup_all(void)

   111

{

   112

    if (PLC_state & PLC_STATE_TASK_CREATED) {

   113

        rt_task_delete(&PLC_task);

   114

        PLC_state &= ~PLC_STATE_TASK_CREATED;

   115

    }

   116

   117

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   118

        rt_pipe_delete(&WaitDebug_pipe);

   119

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   120

    }

   121

   122

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   123

        close(WaitDebug_pipe_fd);

   124

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   125

    }

   126

   127

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   128

        rt_pipe_delete(&WaitPython_pipe);

   129

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   130

    }

   131

   132

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   133

        close(WaitPython_pipe_fd);

   134

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   135

    }

   136

   137

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   138

        rt_pipe_delete(&Debug_pipe);

   139

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   140

    }

   141

   142

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   143

        close(Debug_pipe_fd);

   144

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   145

    }

   146

   147

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   148

        rt_pipe_delete(&Python_pipe);

   149

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   150

    }

   151

   152

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   153

        close(Python_pipe_fd);

   154

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   155

    }

   156

   157

}

   158

   159

int stopPLC()

   160

{

   161

    /* Stop the PLC */

   162

    PLC_shutdown = 1;

   163

   164

    /* Wait until PLC task stops */

   165

    rt_task_join(&PLC_task);

   166

   167

    PLC_cleanup_all();

   168

    __cleanup();

   169

    __debug_tick = -1;

   170

    return 0;

   171

}

   172

   173

//

   174

void catch_signal(int sig)

   175

{

   176

    stopPLC();

   177

//  signal(SIGTERM, catch_signal);

   178

    signal(SIGINT, catch_signal);

   179

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

   180

    exit(0);

   181

}

   182

   183

#define _startPLCLog(text) \

   184

    {\

   185

    	char mstr[] = text;\

   186

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

   187

        goto error;\

   188

    }

   189

   190

#define FO "Failed opening "

   191

   192

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

   193

int startPLC(int argc,char **argv)

   194

{

   195

    signal(SIGINT, catch_signal);

   196

   197

    /* no memory swapping for that process */

   198

    mlockall(MCL_CURRENT | MCL_FUTURE);

   199

   200

    PLC_shutdown = 0;

   201

   202

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

   203

    /* create Debug_pipe */

   204

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

   205

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

   206

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   207

   208

    /* open Debug_pipe*/

   209

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

   210

        _startPLCLog(FO DEBUG_PIPE_DEVICE);

   211

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   212

   213

    /* create Python_pipe */

   214

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

   215

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

   216

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   217

   218

    /* open Python_pipe*/

   219

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

   220

        _startPLCLog(FO PYTHON_PIPE_DEVICE);

   221

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   222

   223

    /* create WaitDebug_pipe */

   224

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

   225

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

   226

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   227

   228

    /* open WaitDebug_pipe*/

   229

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

   230

        _startPLCLog(FO WAITDEBUG_PIPE_DEVICE);

   231

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   232

   233

    /* create WaitPython_pipe */

   234

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

   235

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

   236

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   237

   238

    /* open WaitPython_pipe*/

   239

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

   240

        _startPLCLog(FO WAITPYTHON_PIPE_DEVICE);

   241

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   242

   243

    /*** create PLC task ***/

   244

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

   245

        _startPLCLog("Failed creating PLC task");

   246

    PLC_state |= PLC_STATE_TASK_CREATED;

   247

   248

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

   249

   250

    /* start PLC task */

   251

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

   252

        _startPLCLog("Failed starting PLC task");

   253

   254

    return 0;

   255

   256

error:

   257

   258

    PLC_cleanup_all();

   259

    return 1;

   260

}

   261

   262

#define DEBUG_FREE 0

   263

#define DEBUG_BUSY 1

   264

static long debug_state = DEBUG_FREE;

   265

   266

int TryEnterDebugSection(void)

   267

{

   268

    if(AtomicCompareExchange(

   269

        &debug_state,

   270

        DEBUG_FREE,

   271

        DEBUG_BUSY) == DEBUG_FREE){

   272

        if(__DEBUG){

   273

            return 1;

   274

        }

   275

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   276

    }

   277

    return 0;

   278

}

   279

   280

void LeaveDebugSection(void)

   281

{

   282

    if(AtomicCompareExchange( &debug_state, 

   283

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   284

        char msg = DEBUG_UNLOCK;

   285

        /* signal to NRT for wakeup */

   286

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

   287

    }

   288

}

   289

   290

extern unsigned long __tick;

   291

   292

int WaitDebugData(unsigned long *tick)

   293

{

   294

    char cmd;

   295

    int res;

   296

    if (PLC_shutdown) return -1;

   297

    /* Wait signal from PLC thread */

   298

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

   299

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

   300

        *tick = __debug_tick;

   301

        return 0;

   302

    }

   303

    return -1;

   304

}

   305

   306

/* Called by PLC thread when debug_publish finished

   307

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

   308

void InitiateDebugTransfer()

   309

{

   310

    char msg = DEBUG_PENDING_DATA;

   311

    /* remember tick */

   312

    __debug_tick = __tick;

   313

    /* signal debugger thread it can read data */

   314

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

   315

}

   316

   317

int suspendDebug(int disable)

   318

{

   319

    char cmd = DEBUG_UNLOCK;

   320

    if (PLC_shutdown) return -1;

   321

    while(AtomicCompareExchange(

   322

            &debug_state,

   323

            DEBUG_FREE,

   324

            DEBUG_BUSY) != DEBUG_FREE &&

   325

            cmd == DEBUG_UNLOCK){

   326

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

   327

           return -1;

   328

       }

   329

    }

   330

    __DEBUG = !disable;

   331

    if (disable)

   332

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   333

    return 0;

   334

}

   335

   336

void resumeDebug(void)

   337

{

   338

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   339

}

   340

   341

#define PYTHON_FREE 0

   342

#define PYTHON_BUSY 1

   343

static long python_state = PYTHON_FREE;

   344

   345

int WaitPythonCommands(void)

   346

{ 

   347

    char cmd;

   348

    if (PLC_shutdown) return -1;

   349

    /* Wait signal from PLC thread */

   350

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

   351

        return 0;

   352

    }

   353

    return -1;

   354

}

   355

   356

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

   357

void UnBlockPythonCommands(void)

   358

{

   359

    char msg = PYTHON_PENDING_COMMAND;

   360

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

   361

}

   362

   363

int TryLockPython(void)

   364

{

   365

    return AtomicCompareExchange(

   366

        &python_state,

   367

        PYTHON_FREE,

   368

        PYTHON_BUSY) == PYTHON_FREE;

   369

}

   370

   371

#define UNLOCK_PYTHON 1

   372

void LockPython(void)

   373

{

   374

    char cmd = UNLOCK_PYTHON;

   375

    if (PLC_shutdown) return;

   376

    while(AtomicCompareExchange(

   377

            &python_state,

   378

            PYTHON_FREE,

   379

            PYTHON_BUSY) != PYTHON_FREE &&

   380

            cmd == UNLOCK_PYTHON){

   381

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

   382

    }

   383

}

   384

   385

void UnLockPython(void)

   386

{

   387

    if(AtomicCompareExchange(

   388

            &python_state,

   389

            PYTHON_BUSY,

   390

            PYTHON_FREE) == PYTHON_BUSY){

   391

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

   392

           char cmd = UNLOCK_PYTHON;

   393

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

   394

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

   395

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

   396

}

   397

   398

#ifndef HAVE_RETAIN

   399

int CheckRetainBuffer(void)

   400

{

   401

	return 1;

   402

}

   403

   404

void ValidateRetainBuffer(void)

   405

{

   406

}

   407

   408

void InValidateRetainBuffer(void)

   409

{

   410

}

   411

   412

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

   413

{

   414

}

   415

   416

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

   417

{

   418

}

   419

   420

void CleanupRetain(void)

   421

{

   422

}

   423

   424

void InitRetain(void)

   425

{

   426

}

   427

#endif // !HAVE_RETAIN