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 <errno.h>

    12

#include <sys/mman.h>

    13

#include <sys/fcntl.h>

    14

    15

#include <alchemy/task.h>

    16

#include <alchemy/timer.h>

    17

#include <alchemy/sem.h>

    18

#include <alchemy/pipe.h>

    19

    20

#define _Log(level,text,...) \

    21

    {\

    22

        char mstr[256];\

    23

        snprintf(mstr, 255, text, ##__VA_ARGS__);\

    24

        LogMessage(level, mstr, strlen(mstr));\

    25

    }

    26

    27

#define _LogError(text,...) _Log(LOG_CRITICAL, text, ##__VA_ARGS__)

    28

#define _LogWarning(text,...) _Log(LOG_WARNING, text, ##__VA_ARGS__)

    29

    30

unsigned int PLC_state = 0;

    31

#define PLC_STATE_TASK_CREATED                 1

    32

#define PLC_STATE_DEBUG_PIPE_CREATED           2

    33

#define PLC_STATE_PYTHON_PIPE_CREATED          8

    34

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       16

    35

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      32

    36

    37

#define PIPE_SIZE                    1

    38

    39

// rt-pipes commands

    40

    41

#define PYTHON_PENDING_COMMAND 1

    42

#define PYTHON_FINISH 2

    43

    44

#define DEBUG_FINISH 2

    45

    46

#define DEBUG_PENDING_DATA 1

    47

#define DEBUG_UNLOCK 1

    48

    49

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

    50

{

    51

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    52

}

    53

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

    54

{

    55

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    56

}

    57

    58

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    59

{

    60

    RTIME current_time = rt_timer_read();

    61

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    62

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    63

}

    64

    65

RT_TASK PLC_task;

    66

void *WaitDebug_handle;

    67

void *WaitPython_handle;

    68

void *Debug_handle;

    69

void *Python_handle;

    70

void *svghmi_handle;

    71

    72

struct RT_to_nRT_signal_s {

    73

    int used;

    74

    RT_PIPE pipe;

    75

    int pipe_fd;

    76

    char *name;

    77

};

    78

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

    79

    80

#define max_RT_to_nRT_signals 16

    81

    82

static RT_to_nRT_signal_t RT_to_nRT_signal_pool[max_RT_to_nRT_signals];

    83

    84

int recv_RT_to_nRT_signal(void* handle, char* payload){

    85

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    86

    if(!sig->used) return -EINVAL;

    87

    return read(sig->pipe_fd, payload, 1);

    88

}

    89

    90

int send_RT_to_nRT_signal(void* handle, char payload){

    91

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    92

    if(!sig->used) return -EINVAL;

    93

    return rt_pipe_write(&sig->pipe, &payload, 1, P_NORMAL);

    94

}

    95

    96

    97

int PLC_shutdown = 0;

    98

    99

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

   100

{

   101

  RTIME current_time = rt_timer_read();

   102

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

   103

}

   104

   105

void PLC_task_proc(void *arg)

   106

{

   107

    unsigned long overruns = 0;

   108

    PLC_SetTimer(common_ticktime__, common_ticktime__);

   109

   110

    while (!PLC_shutdown) {

   111

        PLC_GetTime(&__CURRENT_TIME);

   112

        if(overruns == 0){

   113

            __run();

   114

        } else {

   115

            // in case of overrun, don't run PLC on next cycle, to prevent CPU hogging.

   116

            _LogWarning("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns);

   117

            // rt_printf("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", overruns);

   118

            // increment tick count anyhow, so that task scheduling keeps consistent

   119

            __tick += overruns;

   120

        }

   121

        if (PLC_shutdown) break;

   122

        rt_task_wait_period(&overruns);

   123

    }

   124

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

   125

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

   126

    {

   127

        /* explicitely finish python thread */

   128

        char msg = PYTHON_FINISH;

   129

        send_RT_to_nRT_signal(WaitPython_handle, msg);

   130

    }

   131

    {

   132

        /* explicitely finish debug thread */

   133

        char msg = DEBUG_FINISH;

   134

        send_RT_to_nRT_signal(WaitDebug_handle, msg);

   135

    }

   136

}

   137

   138

static unsigned long __debug_tick;

   139

   140

void *create_RT_to_nRT_signal(char* name){

   141

    int new_index = -1;

   142

    int ret;

   143

    RT_to_nRT_signal_t *sig;

   144

    char pipe_dev[64];

   145

   146

    /* find a free slot */

   147

    for(int i=0; i < max_RT_to_nRT_signals; i++){

   148

        sig = &RT_to_nRT_signal_pool[i];

   149

        if(!sig->used){

   150

            new_index = i;

   151

            break;

   152

        }

   153

    }

   154

   155

    /* fail if none found */

   156

    if(new_index == -1) {

   157

    	_LogError("Maximum count of RT-PIPE reached while creating pipe for %s (%d)", name, max_RT_to_nRT_signals);

   158

        return NULL;

   159

    }

   160

   161

    /* create rt pipe */

   162

    if(ret = rt_pipe_create(&sig->pipe, name, new_index, PIPE_SIZE) < 0){

   163

    	_LogError("Failed opening real-time end of RT-PIPE for %s (%d)", name, ret);

   164

        return NULL;

   165

    }

   166

   167

    /* open pipe's userland */

   168

    snprintf(pipe_dev, 63, "/dev/rtp%d", new_index);

   169

    if((sig->pipe_fd = open(pipe_dev, O_RDWR)) == -1){

   170

        rt_pipe_delete(&sig->pipe);

   171

    	_LogError("Failed opening non-real-time end of RT-PIPE for %s (%d)", name, errno);

   172

        return NULL;

   173

    }

   174

   175

    sig->used = 1;

   176

    sig->name = name;

   177

   178

    return sig;

   179

}

   180

   181

void delete_RT_to_nRT_signal(void* handle){

   182

    int ret;

   183

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   184

    char *name = sig->name;

   185

   186

    if(!sig->used) return;

   187

   188

    if(ret = rt_pipe_delete(&sig->pipe) != 0){

   189

    	_LogError("Failed closing real-time end of RT-PIPE for %s (%d)", name, ret);

   190

    }

   191

   192

    if(close(sig->pipe_fd) != 0){

   193

    	_LogError("Failed closing non-real-time end of RT-PIPE for %s (%d)", name, errno);

   194

    }

   195

   196

    sig->used = 0;

   197

}

   198

   199

int wait_RT_to_nRT_signal(void* handle){

   200

    char cmd;

   201

    int ret = recv_RT_to_nRT_signal(handle, &cmd);

   202

    return (ret == 1) ? 0 : ((ret == 0) ? ENODATA : -ret);

   203

}

   204

   205

int unblock_RT_to_nRT_signal(void* handle){

   206

    int ret = send_RT_to_nRT_signal(handle, 0);

   207

    return (ret == 1) ? 0 : ((ret == 0) ? EINVAL : -ret);

   208

}

   209

   210

void nRT_reschedule(void){

   211

    sched_yield();

   212

}

   213

   214

void PLC_cleanup_all(void)

   215

{

   216

    if (PLC_state & PLC_STATE_TASK_CREATED) {

   217

        rt_task_delete(&PLC_task);

   218

        PLC_state &= ~PLC_STATE_TASK_CREATED;

   219

    }

   220

   221

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   222

        delete_RT_to_nRT_signal(WaitDebug_handle);

   223

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   224

    }

   225

   226

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   227

        delete_RT_to_nRT_signal(WaitPython_handle);

   228

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   229

    }

   230

   231

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   232

        delete_RT_to_nRT_signal(Debug_handle);

   233

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   234

    }

   235

   236

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   237

        delete_RT_to_nRT_signal(Python_handle);

   238

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   239

    }

   240

}

   241

   242

int stopPLC()

   243

{

   244

    /* Stop the PLC */

   245

    PLC_shutdown = 1;

   246

   247

    /* Wait until PLC task stops */

   248

    rt_task_join(&PLC_task);

   249

   250

    PLC_cleanup_all();

   251

    __cleanup();

   252

    __debug_tick = -1;

   253

    return 0;

   254

}

   255

   256

//

   257

void catch_signal(int sig)

   258

{

   259

    stopPLC();

   260

//  signal(SIGTERM, catch_signal);

   261

    signal(SIGINT, catch_signal);

   262

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

   263

    exit(0);

   264

}

   265

   266

#define _startPLCLog(text) \

   267

    {\

   268

    	char mstr[] = text;\

   269

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

   270

        goto error;\

   271

    }

   272

   273

#define FO "Failed opening "

   274

   275

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

   276

int startPLC(int argc,char **argv)

   277

{

   278

    signal(SIGINT, catch_signal);

   279

   280

    /* no memory swapping for that process */

   281

    mlockall(MCL_CURRENT | MCL_FUTURE);

   282

   283

    /* memory initialization */

   284

    PLC_shutdown = 0;

   285

    bzero(RT_to_nRT_signal_pool, sizeof(RT_to_nRT_signal_pool));

   286

   287

    /*** RT Pipes ***/

   288

    /* create Debug_pipe */

   289

    if(!(Debug_handle = create_RT_to_nRT_signal("Debug_pipe"))) goto error;

   290

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   291

   292

    /* create Python_pipe */

   293

    if(!(Python_handle = create_RT_to_nRT_signal("Python_pipe"))) goto error;

   294

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   295

   296

    /* create WaitDebug_pipe */

   297

    if(!(WaitDebug_handle = create_RT_to_nRT_signal("WaitDebug_pipe"))) goto error;

   298

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   299

   300

    /* create WaitPython_pipe */

   301

    if(!(WaitPython_handle = create_RT_to_nRT_signal("WaitPython_pipe"))) goto error;

   302

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   303

   304

    /*** create PLC task ***/

   305

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

   306

        _startPLCLog("Failed creating PLC task");

   307

    PLC_state |= PLC_STATE_TASK_CREATED;

   308

   309

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

   310

   311

    /* start PLC task */

   312

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

   313

        _startPLCLog("Failed starting PLC task");

   314

   315

    return 0;

   316

   317

error:

   318

   319

    PLC_cleanup_all();

   320

    return 1;

   321

}

   322

   323

#define DEBUG_FREE 0

   324

#define DEBUG_BUSY 1

   325

static long debug_state = DEBUG_FREE;

   326

   327

int TryEnterDebugSection(void)

   328

{

   329

    if(AtomicCompareExchange(

   330

        &debug_state,

   331

        DEBUG_FREE,

   332

        DEBUG_BUSY) == DEBUG_FREE){

   333

        if(__DEBUG){

   334

            return 1;

   335

        }

   336

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   337

    }

   338

    return 0;

   339

}

   340

   341

void LeaveDebugSection(void)

   342

{

   343

    if(AtomicCompareExchange( &debug_state,

   344

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   345

        char msg = DEBUG_UNLOCK;

   346

        /* signal to NRT for wakeup */

   347

        send_RT_to_nRT_signal(Debug_handle, msg);

   348

    }

   349

}

   350

   351

extern unsigned int __tick;

   352

   353

int WaitDebugData(unsigned int *tick)

   354

{

   355

    char cmd;

   356

    int res;

   357

    if (PLC_shutdown) return -1;

   358

    /* Wait signal from PLC thread */

   359

    res = recv_RT_to_nRT_signal(WaitDebug_handle, &cmd);

   360

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

   361

        *tick = __debug_tick;

   362

        return 0;

   363

    }

   364

    return -1;

   365

}

   366

   367

/* Called by PLC thread when debug_publish finished

   368

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

   369

void InitiateDebugTransfer()

   370

{

   371

    char msg = DEBUG_PENDING_DATA;

   372

    /* remember tick */

   373

    __debug_tick = __tick;

   374

    /* signal debugger thread it can read data */

   375

    send_RT_to_nRT_signal(WaitDebug_handle, msg);

   376

}

   377

   378

int suspendDebug(int disable)

   379

{

   380

    char cmd = DEBUG_UNLOCK;

   381

    if (PLC_shutdown) return -1;

   382

    while(AtomicCompareExchange(

   383

            &debug_state,

   384

            DEBUG_FREE,

   385

            DEBUG_BUSY) != DEBUG_FREE &&

   386

            cmd == DEBUG_UNLOCK){

   387

       if(recv_RT_to_nRT_signal(Debug_handle, &cmd) != 1){

   388

           return -1;

   389

       }

   390

    }

   391

    __DEBUG = !disable;

   392

    if (disable)

   393

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   394

    return 0;

   395

}

   396

   397

void resumeDebug(void)

   398

{

   399

    __DEBUG = 1;

   400

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   401

}

   402

   403

#define PYTHON_FREE 0

   404

#define PYTHON_BUSY 1

   405

static long python_state = PYTHON_FREE;

   406

   407

int WaitPythonCommands(void)

   408

{

   409

    char cmd;

   410

    if (PLC_shutdown) return -1;

   411

    /* Wait signal from PLC thread */

   412

    if(recv_RT_to_nRT_signal(WaitPython_handle, &cmd) == 1 && cmd==PYTHON_PENDING_COMMAND){

   413

        return 0;

   414

    }

   415

    return -1;

   416

}

   417

   418

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

   419

void UnBlockPythonCommands(void)

   420

{

   421

    char msg = PYTHON_PENDING_COMMAND;

   422

    send_RT_to_nRT_signal(WaitPython_handle, msg);

   423

}

   424

   425

int TryLockPython(void)

   426

{

   427

    return AtomicCompareExchange(

   428

        &python_state,

   429

        PYTHON_FREE,

   430

        PYTHON_BUSY) == PYTHON_FREE;

   431

}

   432

   433

#define UNLOCK_PYTHON 1

   434

void LockPython(void)

   435

{

   436

    char cmd = UNLOCK_PYTHON;

   437

    if (PLC_shutdown) return;

   438

    while(AtomicCompareExchange(

   439

            &python_state,

   440

            PYTHON_FREE,

   441

            PYTHON_BUSY) != PYTHON_FREE &&

   442

            cmd == UNLOCK_PYTHON){

   443

       recv_RT_to_nRT_signal(Python_handle, &cmd);

   444

    }

   445

}

   446

   447

void UnLockPython(void)

   448

{

   449

    if(AtomicCompareExchange(

   450

            &python_state,

   451

            PYTHON_BUSY,

   452

            PYTHON_FREE) == PYTHON_BUSY){

   453

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

   454

           char cmd = UNLOCK_PYTHON;

   455

           send_RT_to_nRT_signal(Python_handle, cmd);

   456

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

   457

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

   458

}

   459

   460

#ifndef HAVE_RETAIN

   461

int CheckRetainBuffer(void)

   462

{

   463

	return 1;

   464

}

   465

   466

void ValidateRetainBuffer(void)

   467

{

   468

}

   469

   470

void InValidateRetainBuffer(void)

   471

{

   472

}

   473

   474

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

   475

{

   476

}

   477

   478

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

   479

{

   480

}

   481

   482

void CleanupRetain(void)

   483

{

   484

}

   485

   486

void InitRetain(void)

   487

{

   488

}

   489

#endif // !HAVE_RETAIN