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_PIPE_CREATED           2

    22

#define PLC_STATE_PYTHON_PIPE_CREATED          8

    23

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       16

    24

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      32

    25

    26

#define PIPE_SIZE                    1

    27

    28

// rt-pipes commands

    29

    30

#define PYTHON_PENDING_COMMAND 1

    31

#define PYTHON_FINISH 2

    32

    33

#define DEBUG_FINISH 2

    34

    35

#define DEBUG_PENDING_DATA 1

    36

#define DEBUG_UNLOCK 1

    37

    38

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

    39

{

    40

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    41

}

    42

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

    43

{

    44

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    45

}

    46

    47

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    48

{

    49

    RTIME current_time = rt_timer_read();

    50

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    51

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    52

}

    53

    54

RT_TASK PLC_task;

    55

void *WaitDebug_handle;

    56

void *WaitPython_handle;

    57

void *Debug_handle;

    58

void *Python_handle;

    59

void *svghmi_handle;

    60

    61

struct RT_to_nRT_signal_s {

    62

    int used;

    63

    RT_PIPE pipe;

    64

    int pipe_fd;

    65

    char *name;

    66

};

    67

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

    68

    69

#define max_RT_to_nRT_signals 16

    70

    71

static RT_to_nRT_signal_t RT_to_nRT_signal_pool[max_RT_to_nRT_signals];

    72

    73

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

    74

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    75

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

    76

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

    77

}

    78

    79

int send_RT_to_nRT_signal(void* handle, char payload){

    80

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

    81

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

    82

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

    83

}

    84

    85

    86

int PLC_shutdown = 0;

    87

    88

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

    89

{

    90

  RTIME current_time = rt_timer_read();

    91

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

    92

}

    93

    94

void PLC_task_proc(void *arg)

    95

{

    96

    PLC_SetTimer(common_ticktime__, common_ticktime__);

    97

    98

    while (!PLC_shutdown) {

    99

        PLC_GetTime(&__CURRENT_TIME);

   100

        __run();

   101

        if (PLC_shutdown) break;

   102

        rt_task_wait_period(NULL);

   103

    }

   104

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

   105

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

   106

    {

   107

        /* explicitely finish python thread */

   108

        char msg = PYTHON_FINISH;

   109

        send_RT_to_nRT_signal(WaitPython_handle, msg);

   110

    }

   111

    {

   112

        /* explicitely finish debug thread */

   113

        char msg = DEBUG_FINISH;

   114

        send_RT_to_nRT_signal(WaitDebug_handle, msg);

   115

    }

   116

}

   117

   118

static unsigned long __debug_tick;

   119

   120

#define _LogAndReturnNull(text) \

   121

    {\

   122

    	char mstr[256] = text " for ";\

   123

        strncat(mstr, name, 255);\

   124

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

   125

        return NULL;\

   126

    }

   127

   128

void *create_RT_to_nRT_signal(char* name){

   129

    int new_index = -1;

   130

    RT_to_nRT_signal_t *sig;

   131

    char pipe_dev[64];

   132

   133

    /* find a free slot */

   134

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

   135

        sig = &RT_to_nRT_signal_pool[i];

   136

        if(!sig->used){

   137

            new_index = i;

   138

            break;

   139

        }

   140

    }

   141

   142

    /* fail if none found */

   143

    if(new_index == -1) {

   144

    	_LogAndReturnNull("Maximum count of RT-PIPE reached while creating pipe");

   145

    }

   146

   147

    /* create rt pipe */

   148

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

   149

    	_LogAndReturnNull("Failed opening real-time end of RT-PIPE");

   150

    }

   151

   152

    /* open pipe's userland */

   153

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

   154

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

   155

        rt_pipe_delete(&sig->pipe);

   156

    	_LogAndReturnNull("Failed opening non-real-time end of RT-PIPE");

   157

    }

   158

   159

    sig->used = 1;

   160

    sig->name = name;

   161

   162

    return sig;

   163

}

   164

   165

void delete_RT_to_nRT_signal(void* handle){

   166

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   167

   168

    if(!sig->used) return;

   169

   170

    rt_pipe_delete(&sig->pipe);

   171

   172

    close(sig->pipe_fd);

   173

   174

    sig->used = 0;

   175

}

   176

   177

int wait_RT_to_nRT_signal(void* handle){

   178

    char cmd;

   179

    int ret = recv_RT_to_nRT_signal(handle, &cmd);

   180

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

   181

}

   182

   183

int unblock_RT_to_nRT_signal(void* handle){

   184

    int ret = send_RT_to_nRT_signal(handle, 0);

   185

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

   186

}

   187

   188

void nRT_reschedule(void){

   189

    sched_yield();

   190

}

   191

   192

void PLC_cleanup_all(void)

   193

{

   194

    if (PLC_state & PLC_STATE_TASK_CREATED) {

   195

        rt_task_delete(&PLC_task);

   196

        PLC_state &= ~PLC_STATE_TASK_CREATED;

   197

    }

   198

   199

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

   200

        delete_RT_to_nRT_signal(WaitDebug_handle);

   201

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   202

    }

   203

   204

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   205

        delete_RT_to_nRT_signal(WaitPython_handle);

   206

        PLC_state &= ~PLC_STATE_WAITPYTHON_PIPE_CREATED;

   207

    }

   208

   209

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   210

        delete_RT_to_nRT_signal(Debug_handle);

   211

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   212

    }

   213

   214

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   215

        delete_RT_to_nRT_signal(Python_handle);

   216

        PLC_state &= ~PLC_STATE_PYTHON_PIPE_CREATED;

   217

    }

   218

}

   219

   220

int stopPLC()

   221

{

   222

    /* Stop the PLC */

   223

    PLC_shutdown = 1;

   224

   225

    /* Wait until PLC task stops */

   226

    rt_task_join(&PLC_task);

   227

   228

    PLC_cleanup_all();

   229

    __cleanup();

   230

    __debug_tick = -1;

   231

    return 0;

   232

}

   233

   234

//

   235

void catch_signal(int sig)

   236

{

   237

    stopPLC();

   238

//  signal(SIGTERM, catch_signal);

   239

    signal(SIGINT, catch_signal);

   240

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

   241

    exit(0);

   242

}

   243

   244

#define _startPLCLog(text) \

   245

    {\

   246

    	char mstr[] = text;\

   247

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

   248

        goto error;\

   249

    }

   250

   251

#define FO "Failed opening "

   252

   253

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

   254

int startPLC(int argc,char **argv)

   255

{

   256

    signal(SIGINT, catch_signal);

   257

   258

    /* no memory swapping for that process */

   259

    mlockall(MCL_CURRENT | MCL_FUTURE);

   260

   261

    /* memory initialization */

   262

    PLC_shutdown = 0;

   263

    bzero(RT_to_nRT_signal_pool, sizeof(RT_to_nRT_signal_pool));

   264

   265

    /*** RT Pipes ***/

   266

    /* create Debug_pipe */

   267

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

   268

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   269

   270

    /* create Python_pipe */

   271

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

   272

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   273

   274

    /* create WaitDebug_pipe */

   275

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

   276

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   277

   278

    /* create WaitPython_pipe */

   279

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

   280

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   281

   282

    /*** create PLC task ***/

   283

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

   284

        _startPLCLog("Failed creating PLC task");

   285

    PLC_state |= PLC_STATE_TASK_CREATED;

   286

   287

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

   288

   289

    /* start PLC task */

   290

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

   291

        _startPLCLog("Failed starting PLC task");

   292

   293

    return 0;

   294

   295

error:

   296

   297

    PLC_cleanup_all();

   298

    return 1;

   299

}

   300

   301

#define DEBUG_FREE 0

   302

#define DEBUG_BUSY 1

   303

static long debug_state = DEBUG_FREE;

   304

   305

int TryEnterDebugSection(void)

   306

{

   307

    if(AtomicCompareExchange(

   308

        &debug_state,

   309

        DEBUG_FREE,

   310

        DEBUG_BUSY) == DEBUG_FREE){

   311

        if(__DEBUG){

   312

            return 1;

   313

        }

   314

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   315

    }

   316

    return 0;

   317

}

   318

   319

void LeaveDebugSection(void)

   320

{

   321

    if(AtomicCompareExchange( &debug_state,

   322

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   323

        char msg = DEBUG_UNLOCK;

   324

        /* signal to NRT for wakeup */

   325

        send_RT_to_nRT_signal(Debug_handle, msg);

   326

    }

   327

}

   328

   329

extern unsigned long __tick;

   330

   331

int WaitDebugData(unsigned long *tick)

   332

{

   333

    char cmd;

   334

    int res;

   335

    if (PLC_shutdown) return -1;

   336

    /* Wait signal from PLC thread */

   337

    res = recv_RT_to_nRT_signal(WaitDebug_handle, &cmd);

   338

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

   339

        *tick = __debug_tick;

   340

        return 0;

   341

    }

   342

    return -1;

   343

}

   344

   345

/* Called by PLC thread when debug_publish finished

   346

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

   347

void InitiateDebugTransfer()

   348

{

   349

    char msg = DEBUG_PENDING_DATA;

   350

    /* remember tick */

   351

    __debug_tick = __tick;

   352

    /* signal debugger thread it can read data */

   353

    send_RT_to_nRT_signal(WaitDebug_handle, msg);

   354

}

   355

   356

int suspendDebug(int disable)

   357

{

   358

    char cmd = DEBUG_UNLOCK;

   359

    if (PLC_shutdown) return -1;

   360

    while(AtomicCompareExchange(

   361

            &debug_state,

   362

            DEBUG_FREE,

   363

            DEBUG_BUSY) != DEBUG_FREE &&

   364

            cmd == DEBUG_UNLOCK){

   365

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

   366

           return -1;

   367

       }

   368

    }

   369

    __DEBUG = !disable;

   370

    if (disable)

   371

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   372

    return 0;

   373

}

   374

   375

void resumeDebug(void)

   376

{

   377

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   378

}

   379

   380

#define PYTHON_FREE 0

   381

#define PYTHON_BUSY 1

   382

static long python_state = PYTHON_FREE;

   383

   384

int WaitPythonCommands(void)

   385

{

   386

    char cmd;

   387

    if (PLC_shutdown) return -1;

   388

    /* Wait signal from PLC thread */

   389

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

   390

        return 0;

   391

    }

   392

    return -1;

   393

}

   394

   395

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

   396

void UnBlockPythonCommands(void)

   397

{

   398

    char msg = PYTHON_PENDING_COMMAND;

   399

    send_RT_to_nRT_signal(WaitPython_handle, msg);

   400

}

   401

   402

int TryLockPython(void)

   403

{

   404

    return AtomicCompareExchange(

   405

        &python_state,

   406

        PYTHON_FREE,

   407

        PYTHON_BUSY) == PYTHON_FREE;

   408

}

   409

   410

#define UNLOCK_PYTHON 1

   411

void LockPython(void)

   412

{

   413

    char cmd = UNLOCK_PYTHON;

   414

    if (PLC_shutdown) return;

   415

    while(AtomicCompareExchange(

   416

            &python_state,

   417

            PYTHON_FREE,

   418

            PYTHON_BUSY) != PYTHON_FREE &&

   419

            cmd == UNLOCK_PYTHON){

   420

       recv_RT_to_nRT_signal(Python_handle, &cmd);

   421

    }

   422

}

   423

   424

void UnLockPython(void)

   425

{

   426

    if(AtomicCompareExchange(

   427

            &python_state,

   428

            PYTHON_BUSY,

   429

            PYTHON_FREE) == PYTHON_BUSY){

   430

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

   431

           char cmd = UNLOCK_PYTHON;

   432

           send_RT_to_nRT_signal(Python_handle, cmd);

   433

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

   434

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

   435

}

   436

   437

#ifndef HAVE_RETAIN

   438

int CheckRetainBuffer(void)

   439

{

   440

	return 1;

   441

}

   442

   443

void ValidateRetainBuffer(void)

   444

{

   445

}

   446

   447

void InValidateRetainBuffer(void)

   448

{

   449

}

   450

   451

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

   452

{

   453

}

   454

   455

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

   456

{

   457

}

   458

   459

void CleanupRetain(void)

   460

{

   461

}

   462

   463

void InitRetain(void)

   464

{

   465

}

   466

#endif // !HAVE_RETAIN