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

unsigned int PLC_state = 0;

    21

#define PLC_STATE_TASK_CREATED                 1

    22

#define PLC_STATE_DEBUG_PIPE_CREATED           2

    23

#define PLC_STATE_PYTHON_PIPE_CREATED          8

    24

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       16

    25

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      32

    26

    27

#define PIPE_SIZE                    1

    28

    29

// rt-pipes commands

    30

    31

#define PYTHON_PENDING_COMMAND 1

    32

#define PYTHON_FINISH 2

    33

    34

#define DEBUG_FINISH 2

    35

    36

#define DEBUG_PENDING_DATA 1

    37

#define DEBUG_UNLOCK 1

    38

    39

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

    40

{

    41

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    42

}

    43

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

    44

{

    45

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    46

}

    47

    48

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    49

{

    50

    RTIME current_time = rt_timer_read();

    51

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    52

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    53

}

    54

    55

RT_TASK PLC_task;

    56

void *WaitDebug_handle;

    57

void *WaitPython_handle;

    58

void *Debug_handle;

    59

void *Python_handle;

    60

void *svghmi_handle;

    61

    62

struct RT_to_nRT_signal_s {

    63

    int used;

    64

    RT_PIPE pipe;

    65

    int pipe_fd;

    66

    char *name;

    67

};

    68

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

    69

    70

#define max_RT_to_nRT_signals 16

    71

    72

static RT_to_nRT_signal_t RT_to_nRT_signal_pool[max_RT_to_nRT_signals];

    73

    74

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

    75

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    76

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

    77

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

    78

}

    79

    80

int send_RT_to_nRT_signal(void* handle, char payload){

    81

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    82

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

    83

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

    84

}

    85

    86

    87

int PLC_shutdown = 0;

    88

    89

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

    90

{

    91

  RTIME current_time = rt_timer_read();

    92

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

    93

}

    94

    95

void PLC_task_proc(void *arg)

    96

{

    97

    PLC_SetTimer(common_ticktime__, common_ticktime__);

    98

    99

    while (!PLC_shutdown) {

   100

        PLC_GetTime(&__CURRENT_TIME);

   101

        __run();

   102

        if (PLC_shutdown) break;

   103

        rt_task_wait_period(NULL);

   104

    }

   105

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

   106

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

   107

    {

   108

        /* explicitely finish python thread */

   109

        char msg = PYTHON_FINISH;

   110

        send_RT_to_nRT_signal(WaitPython_handle, msg);

   111

    }

   112

    {

   113

        /* explicitely finish debug thread */

   114

        char msg = DEBUG_FINISH;

   115

        send_RT_to_nRT_signal(WaitDebug_handle, msg);

   116

    }

   117

}

   118

   119

static unsigned long __debug_tick;

   120

   121

#define _Log(text, err) \

   122

    {\

   123

        char mstr[256];\

   124

        snprintf(mstr, 255, text " for %s (%d)", name, err);\

   125

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

   126

    }

   127

   128

void *create_RT_to_nRT_signal(char* name){

   129

    int new_index = -1;

   130

    int ret;

   131

    RT_to_nRT_signal_t *sig;

   132

    char pipe_dev[64];

   133

   134

    /* find a free slot */

   135

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

   136

        sig = &RT_to_nRT_signal_pool[i];

   137

        if(!sig->used){

   138

            new_index = i;

   139

            break;

   140

        }

   141

    }

   142

   143

    /* fail if none found */

   144

    if(new_index == -1) {

   145

    	_Log("Maximum count of RT-PIPE reached while creating pipe", max_RT_to_nRT_signals);

   146

        return NULL;

   147

    }

   148

   149

    /* create rt pipe */

   150

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

   151

    	_Log("Failed opening real-time end of RT-PIPE", ret);

   152

        return NULL;

   153

    }

   154

   155

    /* open pipe's userland */

   156

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

   157

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

   158

        rt_pipe_delete(&sig->pipe);

   159

    	_Log("Failed opening non-real-time end of RT-PIPE", errno);

   160

        return NULL;

   161

    }

   162

   163

    sig->used = 1;

   164

    sig->name = name;

   165

   166

    return sig;

   167

}

   168

   169

void delete_RT_to_nRT_signal(void* handle){

   170

    int ret;

   171

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   172

    char *name = sig->name;

   173

   174

    if(!sig->used) return;

   175

   176

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

   177

    	_Log("Failed closing real-time end of RT-PIPE", ret);

   178

    }

   179

   180

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

   181

    	_Log("Failed closing non-real-time end of RT-PIPE", errno);

   182

    }

   183

   184

    sig->used = 0;

   185

}

   186

   187

int wait_RT_to_nRT_signal(void* handle){

   188

    char cmd;

   189

    int ret = recv_RT_to_nRT_signal(handle, &cmd);

   190

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

   191

}

   192

   193

int unblock_RT_to_nRT_signal(void* handle){

   194

    int ret = send_RT_to_nRT_signal(handle, 0);

   195

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

   196

}

   197

   198

void nRT_reschedule(void){

   199

    sched_yield();

   200

}

   201

   202

void PLC_cleanup_all(void)

   203

{

   204

    if (PLC_state & PLC_STATE_TASK_CREATED) {

   205

        rt_task_delete(&PLC_task);

   206

        PLC_state &= ~PLC_STATE_TASK_CREATED;

   207

    }

   208

   209

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   210

        delete_RT_to_nRT_signal(WaitDebug_handle);

   211

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   212

    }

   213

   214

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   215

        delete_RT_to_nRT_signal(WaitPython_handle);

   216

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   217

    }

   218

   219

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   220

        delete_RT_to_nRT_signal(Debug_handle);

   221

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   222

    }

   223

   224

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   225

        delete_RT_to_nRT_signal(Python_handle);

   226

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   227

    }

   228

}

   229

   230

int stopPLC()

   231

{

   232

    /* Stop the PLC */

   233

    PLC_shutdown = 1;

   234

   235

    /* Wait until PLC task stops */

   236

    rt_task_join(&PLC_task);

   237

   238

    PLC_cleanup_all();

   239

    __cleanup();

   240

    __debug_tick = -1;

   241

    return 0;

   242

}

   243

   244

//

   245

void catch_signal(int sig)

   246

{

   247

    stopPLC();

   248

//  signal(SIGTERM, catch_signal);

   249

    signal(SIGINT, catch_signal);

   250

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

   251

    exit(0);

   252

}

   253

   254

#define _startPLCLog(text) \

   255

    {\

   256

    	char mstr[] = text;\

   257

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

   258

        goto error;\

   259

    }

   260

   261

#define FO "Failed opening "

   262

   263

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

   264

int startPLC(int argc,char **argv)

   265

{

   266

    signal(SIGINT, catch_signal);

   267

   268

    /* no memory swapping for that process */

   269

    mlockall(MCL_CURRENT | MCL_FUTURE);

   270

   271

    /* memory initialization */

   272

    PLC_shutdown = 0;

   273

    bzero(RT_to_nRT_signal_pool, sizeof(RT_to_nRT_signal_pool));

   274

   275

    /*** RT Pipes ***/

   276

    /* create Debug_pipe */

   277

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

   278

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   279

   280

    /* create Python_pipe */

   281

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

   282

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   283

   284

    /* create WaitDebug_pipe */

   285

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

   286

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   287

   288

    /* create WaitPython_pipe */

   289

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

   290

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   291

   292

    /*** create PLC task ***/

   293

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

   294

        _startPLCLog("Failed creating PLC task");

   295

    PLC_state |= PLC_STATE_TASK_CREATED;

   296

   297

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

   298

   299

    /* start PLC task */

   300

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

   301

        _startPLCLog("Failed starting PLC task");

   302

   303

    return 0;

   304

   305

error:

   306

   307

    PLC_cleanup_all();

   308

    return 1;

   309

}

   310

   311

#define DEBUG_FREE 0

   312

#define DEBUG_BUSY 1

   313

static long debug_state = DEBUG_FREE;

   314

   315

int TryEnterDebugSection(void)

   316

{

   317

    if(AtomicCompareExchange(

   318

        &debug_state,

   319

        DEBUG_FREE,

   320

        DEBUG_BUSY) == DEBUG_FREE){

   321

        if(__DEBUG){

   322

            return 1;

   323

        }

   324

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   325

    }

   326

    return 0;

   327

}

   328

   329

void LeaveDebugSection(void)

   330

{

   331

    if(AtomicCompareExchange( &debug_state,

   332

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   333

        char msg = DEBUG_UNLOCK;

   334

        /* signal to NRT for wakeup */

   335

        send_RT_to_nRT_signal(Debug_handle, msg);

   336

    }

   337

}

   338

   339

extern unsigned long __tick;

   340

   341

int WaitDebugData(unsigned long *tick)

   342

{

   343

    char cmd;

   344

    int res;

   345

    if (PLC_shutdown) return -1;

   346

    /* Wait signal from PLC thread */

   347

    res = recv_RT_to_nRT_signal(WaitDebug_handle, &cmd);

   348

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

   349

        *tick = __debug_tick;

   350

        return 0;

   351

    }

   352

    return -1;

   353

}

   354

   355

/* Called by PLC thread when debug_publish finished

   356

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

   357

void InitiateDebugTransfer()

   358

{

   359

    char msg = DEBUG_PENDING_DATA;

   360

    /* remember tick */

   361

    __debug_tick = __tick;

   362

    /* signal debugger thread it can read data */

   363

    send_RT_to_nRT_signal(WaitDebug_handle, msg);

   364

}

   365

   366

int suspendDebug(int disable)

   367

{

   368

    char cmd = DEBUG_UNLOCK;

   369

    if (PLC_shutdown) return -1;

   370

    while(AtomicCompareExchange(

   371

            &debug_state,

   372

            DEBUG_FREE,

   373

            DEBUG_BUSY) != DEBUG_FREE &&

   374

            cmd == DEBUG_UNLOCK){

   375

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

   376

           return -1;

   377

       }

   378

    }

   379

    __DEBUG = !disable;

   380

    if (disable)

   381

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   382

    return 0;

   383

}

   384

   385

void resumeDebug(void)

   386

{

   387

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   388

}

   389

   390

#define PYTHON_FREE 0

   391

#define PYTHON_BUSY 1

   392

static long python_state = PYTHON_FREE;

   393

   394

int WaitPythonCommands(void)

   395

{

   396

    char cmd;

   397

    if (PLC_shutdown) return -1;

   398

    /* Wait signal from PLC thread */

   399

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

   400

        return 0;

   401

    }

   402

    return -1;

   403

}

   404

   405

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

   406

void UnBlockPythonCommands(void)

   407

{

   408

    char msg = PYTHON_PENDING_COMMAND;

   409

    send_RT_to_nRT_signal(WaitPython_handle, msg);

   410

}

   411

   412

int TryLockPython(void)

   413

{

   414

    return AtomicCompareExchange(

   415

        &python_state,

   416

        PYTHON_FREE,

   417

        PYTHON_BUSY) == PYTHON_FREE;

   418

}

   419

   420

#define UNLOCK_PYTHON 1

   421

void LockPython(void)

   422

{

   423

    char cmd = UNLOCK_PYTHON;

   424

    if (PLC_shutdown) return;

   425

    while(AtomicCompareExchange(

   426

            &python_state,

   427

            PYTHON_FREE,

   428

            PYTHON_BUSY) != PYTHON_FREE &&

   429

            cmd == UNLOCK_PYTHON){

   430

       recv_RT_to_nRT_signal(Python_handle, &cmd);

   431

    }

   432

}

   433

   434

void UnLockPython(void)

   435

{

   436

    if(AtomicCompareExchange(

   437

            &python_state,

   438

            PYTHON_BUSY,

   439

            PYTHON_FREE) == PYTHON_BUSY){

   440

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

   441

           char cmd = UNLOCK_PYTHON;

   442

           send_RT_to_nRT_signal(Python_handle, cmd);

   443

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

   444

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

   445

}

   446

   447

#ifndef HAVE_RETAIN

   448

int CheckRetainBuffer(void)

   449

{

   450

	return 1;

   451

}

   452

   453

void ValidateRetainBuffer(void)

   454

{

   455

}

   456

   457

void InValidateRetainBuffer(void)

   458

{

   459

}

   460

   461

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

   462

{

   463

}

   464

   465

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

   466

{

   467

}

   468

   469

void CleanupRetain(void)

   470

{

   471

}

   472

   473

void InitRetain(void)

   474

{

   475

}

   476

#endif // !HAVE_RETAIN