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

    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 _startPLCLog(text) \

   163

    {\

   164

    	char mstr[] = text;\

   165

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

   166

        goto error;\

   167

    }

   168

   169

#define FO "Failed opening "

   170

   171

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

   172

int startPLC(int argc,char **argv)

   173

{

   174

    signal(SIGINT, catch_signal);

   175

   176

    /* no memory swapping for that process */

   177

    mlockall(MCL_CURRENT | MCL_FUTURE);

   178

   179

    PLC_shutdown = 0;

   180

   181

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

   182

    /* create Debug_pipe */

   183

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

   184

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

   185

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   186

   187

    /* open Debug_pipe*/

   188

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

   189

        _startPLCLog(FO DEBUG_PIPE_DEVICE);

   190

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   191

   192

    /* create Python_pipe */

   193

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

   194

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

   195

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   196

   197

    /* open Python_pipe*/

   198

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

   199

        _startPLCLog(FO PYTHON_PIPE_DEVICE);

   200

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   201

   202

    /* create WaitDebug_pipe */

   203

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

   204

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

   205

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   206

   207

    /* open WaitDebug_pipe*/

   208

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

   209

        _startPLCLog(FO WAITDEBUG_PIPE_DEVICE);

   210

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   211

   212

    /* create WaitPython_pipe */

   213

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

   214

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

   215

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   216

   217

    /* open WaitPython_pipe*/

   218

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

   219

        _startPLCLog(FO WAITPYTHON_PIPE_DEVICE);

   220

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   221

   222

    /*** create PLC task ***/

   223

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

   224

        _startPLCLog("Failed creating PLC task");

   225

    PLC_state |= PLC_STATE_TASK_CREATED;

   226

   227

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

   228

   229

    /* start PLC task */

   230

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

   231

        _startPLCLog("Failed starting PLC task");

   232

   233

    return 0;

   234

   235

error:

   236

   237

    PLC_cleanup_all();

   238

    return 1;

   239

}

   240

   241

#define DEBUG_FREE 0

   242

#define DEBUG_BUSY 1

   243

static long debug_state = DEBUG_FREE;

   244

   245

int TryEnterDebugSection(void)

   246

{

   247

    if(AtomicCompareExchange(

   248

        &debug_state,

   249

        DEBUG_FREE,

   250

        DEBUG_BUSY) == DEBUG_FREE){

   251

        if(__DEBUG){

   252

            return 1;

   253

        }

   254

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   255

    }

   256

    return 0;

   257

}

   258

   259

#define DEBUG_UNLOCK 1

   260

void LeaveDebugSection(void)

   261

{

   262

    if(AtomicCompareExchange( &debug_state, 

   263

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   264

        char msg = DEBUG_UNLOCK;

   265

        /* signal to NRT for wakeup */

   266

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

   267

    }

   268

}

   269

   270

extern unsigned long __tick;

   271

   272

#define DEBUG_PENDING_DATA 1

   273

int WaitDebugData(unsigned long *tick)

   274

{

   275

    char cmd;

   276

    int res;

   277

    if (PLC_shutdown) return -1;

   278

    /* Wait signal from PLC thread */

   279

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

   280

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

   281

        *tick = __debug_tick;

   282

        return 0;

   283

    }

   284

    return -1;

   285

}

   286

   287

/* Called by PLC thread when debug_publish finished

   288

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

   289

void InitiateDebugTransfer()

   290

{

   291

    char msg = DEBUG_PENDING_DATA;

   292

    /* remember tick */

   293

    __debug_tick = __tick;

   294

    /* signal debugger thread it can read data */

   295

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

   296

}

   297

   298

int suspendDebug(int disable)

   299

{

   300

    char cmd = DEBUG_UNLOCK;

   301

    if (PLC_shutdown) return -1;

   302

    while(AtomicCompareExchange(

   303

            &debug_state,

   304

            DEBUG_FREE,

   305

            DEBUG_BUSY) != DEBUG_FREE &&

   306

            cmd == DEBUG_UNLOCK){

   307

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

   308

           return -1;

   309

       }

   310

    }

   311

    __DEBUG = !disable;

   312

    if (disable)

   313

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   314

    return 0;

   315

}

   316

   317

void resumeDebug(void)

   318

{

   319

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   320

}

   321

   322

#define PYTHON_PENDING_COMMAND 1

   323

   324

#define PYTHON_FREE 0

   325

#define PYTHON_BUSY 1

   326

static long python_state = PYTHON_FREE;

   327

   328

int WaitPythonCommands(void)

   329

{ 

   330

    char cmd;

   331

    if (PLC_shutdown) return -1;

   332

    /* Wait signal from PLC thread */

   333

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

   334

        return 0;

   335

    }

   336

    return -1;

   337

}

   338

   339

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

   340

void UnBlockPythonCommands(void)

   341

{

   342

    char msg = PYTHON_PENDING_COMMAND;

   343

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

   344

}

   345

   346

int TryLockPython(void)

   347

{

   348

    return AtomicCompareExchange(

   349

        &python_state,

   350

        PYTHON_FREE,

   351

        PYTHON_BUSY) == PYTHON_FREE;

   352

}

   353

   354

#define UNLOCK_PYTHON 1

   355

void LockPython(void)

   356

{

   357

    char cmd = UNLOCK_PYTHON;

   358

    if (PLC_shutdown) return;

   359

    while(AtomicCompareExchange(

   360

            &python_state,

   361

            PYTHON_FREE,

   362

            PYTHON_BUSY) != PYTHON_FREE &&

   363

            cmd == UNLOCK_PYTHON){

   364

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

   365

    }

   366

}

   367

   368

void UnLockPython(void)

   369

{

   370

    if(AtomicCompareExchange(

   371

            &python_state,

   372

            PYTHON_BUSY,

   373

            PYTHON_FREE) == PYTHON_BUSY){

   374

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

   375

           char cmd = UNLOCK_PYTHON;

   376

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

   377

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

   378

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

   379

}

   380

   381

#ifndef HAVE_RETAIN

   382

int CheckRetainBuffer(void)

   383

{

   384

	return 1;

   385

}

   386

   387

void ValidateRetainBuffer(void)

   388

{

   389

}

   390

   391

void InValidateRetainBuffer(void)

   392

{

   393

}

   394

   395

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

   396

{

   397

}

   398

   399

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

   400

{

   401

}

   402

   403

void CleanupRetain(void)

   404

{

   405

}

   406

   407

void InitRetain(void)

   408

{

   409

}

   410

#endif // !HAVE_RETAIN