1

/**

     2

 * 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 <native/task.h>

    15

#include <native/timer.h>

    16

#include <native/mutex.h>

    17

#include <native/sem.h>

    18

#include <native/pipe.h>

    19

    20

unsigned int PLC_state = 0;

    21

#define PLC_STATE_TASK_CREATED                 1

    22

#define PLC_STATE_DEBUG_FILE_OPENED            2 

    23

#define PLC_STATE_DEBUG_PIPE_CREATED           4 

    24

#define PLC_STATE_PYTHON_FILE_OPENED           8 

    25

#define PLC_STATE_PYTHON_PIPE_CREATED          16   

    26

#define PLC_STATE_WAITDEBUG_FILE_OPENED        32   

    27

#define PLC_STATE_WAITDEBUG_PIPE_CREATED       64

    28

#define PLC_STATE_WAITPYTHON_FILE_OPENED       128

    29

#define PLC_STATE_WAITPYTHON_PIPE_CREATED      256

    30

    31

#define WAITDEBUG_PIPE_DEVICE        "/dev/rtp0"

    32

#define WAITDEBUG_PIPE_MINOR         0

    33

#define DEBUG_PIPE_DEVICE            "/dev/rtp1"

    34

#define DEBUG_PIPE_MINOR             1

    35

#define WAITPYTHON_PIPE_DEVICE       "/dev/rtp2"

    36

#define WAITPYTHON_PIPE_MINOR        2

    37

#define PYTHON_PIPE_DEVICE           "/dev/rtp3"

    38

#define PYTHON_PIPE_MINOR            3

    39

#define PIPE_SIZE                    1 

    40

    41

/* provided by POUS.C */

    42

extern unsigned long common_ticktime__;

    43

    44

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

    45

{

    46

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    47

}

    48

    49

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    50

{

    51

    RTIME current_time = rt_timer_read();

    52

    CURRENT_TIME->tv_sec = current_time / 1000000000;

    53

    CURRENT_TIME->tv_nsec = current_time % 1000000000;

    54

}

    55

    56

RT_TASK PLC_task;

    57

RT_PIPE WaitDebug_pipe;

    58

RT_PIPE WaitPython_pipe;

    59

RT_PIPE Debug_pipe;

    60

RT_PIPE Python_pipe;

    61

int WaitDebug_pipe_fd;

    62

int WaitPython_pipe_fd;

    63

int Debug_pipe_fd;

    64

int Python_pipe_fd;

    65

    66

int PLC_shutdown = 0;

    67

    68

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

    69

{

    70

  RTIME current_time = rt_timer_read();

    71

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

    72

}

    73

    74

void PLC_task_proc(void *arg)

    75

{

    76

    PLC_SetTimer(Ttick, Ttick);

    77

    78

    while (1) {

    79

        PLC_GetTime(&__CURRENT_TIME);

    80

        __run();

    81

        if (PLC_shutdown) break;

    82

        rt_task_wait_period(NULL);

    83

    }

    84

}

    85

    86

static unsigned long __debug_tick;

    87

    88

void PLC_cleanup_all(void)

    89

{

    90

    if (PLC_state & PLC_STATE_TASK_CREATED) {

    91

        rt_task_delete(&PLC_task);

    92

        PLC_state &= ~PLC_STATE_TASK_CREATED;

    93

    }

    94

    95

    if (PLC_state & PLC_STATE_WAITDEBUG_PIPE_CREATED) {

    96

        rt_pipe_delete(&WaitDebug_pipe);

    97

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

    98

    }

    99

   100

    if (PLC_state & PLC_STATE_WAITDEBUG_FILE_OPENED) {

   101

        close(WaitDebug_pipe_fd);

   102

        PLC_state &= ~PLC_STATE_WAITDEBUG_FILE_OPENED;

   103

    }

   104

   105

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   106

        rt_pipe_delete(&WaitPython_pipe);

   107

        PLC_state &= ~PLC_STATE_WAITDEBUG_PIPE_CREATED;

   108

    }

   109

   110

    if (PLC_state & PLC_STATE_WAITPYTHON_PIPE_CREATED) {

   111

        close(WaitPython_pipe_fd);

   112

        PLC_state &= ~PLC_STATE_WAITPYTHON_FILE_OPENED;

   113

    }

   114

   115

    if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) {

   116

        rt_pipe_delete(&Debug_pipe);

   117

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   118

    }

   119

   120

    if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) {

   121

        close(Debug_pipe_fd);

   122

        PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED;

   123

    }

   124

   125

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   126

        rt_pipe_delete(&Python_pipe);

   127

        PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED;

   128

    }

   129

   130

    if (PLC_state & PLC_STATE_PYTHON_PIPE_CREATED) {

   131

        close(Python_pipe_fd);

   132

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   133

    }

   134

   135

}

   136

   137

int stopPLC()

   138

{

   139

    PLC_shutdown = 1;

   140

    /* Stop the PLC */

   141

    PLC_SetTimer(0, 0);

   142

    __cleanup();

   143

    PLC_cleanup_all();

   144

    __debug_tick = -1;

   145

    return 0;

   146

}

   147

   148

//

   149

void catch_signal(int sig)

   150

{

   151

    stopPLC();

   152

//  signal(SIGTERM, catch_signal);

   153

    signal(SIGINT, catch_signal);

   154

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

   155

    exit(0);

   156

}

   157

   158

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

   159

int startPLC(int argc,char **argv)

   160

{

   161

    signal(SIGINT, catch_signal);

   162

   163

    /* no memory swapping for that process */

   164

    mlockall(MCL_CURRENT | MCL_FUTURE);

   165

   166

    /* Define Ttick to 1ms if common_ticktime not defined */

   167

    Ttick = common_ticktime__?common_ticktime__:1000000;

   168

   169

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

   170

    /* create Debug_pipe */

   171

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

   172

        goto error;

   173

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   174

   175

    /* open Debug_pipe*/

   176

    if((Debug_pipe_fd = open(DEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   177

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   178

   179

    /* create Python_pipe */

   180

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

   181

        goto error;

   182

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   183

   184

    /* open Python_pipe*/

   185

    if((Python_pipe_fd = open(PYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   186

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   187

   188

    /* create WaitDebug_pipe */

   189

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

   190

        goto error;

   191

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   192

   193

    /* open WaitDebug_pipe*/

   194

    if((WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   195

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   196

   197

    /* create WaitPython_pipe */

   198

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

   199

        goto error;

   200

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   201

   202

    /* open WaitPython_pipe*/

   203

    if((WaitPython_pipe_fd = open(WAITPYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;

   204

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   205

   206

    /*** create PLC task ***/

   207

    if(rt_task_create(&PLC_task, "PLC_task", 0, 50, 0)) goto error;

   208

    PLC_state |= PLC_STATE_TASK_CREATED;

   209

   210

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

   211

   212

    /* start PLC task */

   213

    if(rt_task_start(&PLC_task, &PLC_task_proc, NULL)) goto error;

   214

   215

    return 0;

   216

   217

error:

   218

   219

    PLC_cleanup_all();

   220

    return 1;

   221

}

   222

   223

#define DEBUG_FREE 0

   224

#define DEBUG_BUSY 1

   225

static long debug_state = DEBUG_FREE;

   226

   227

int TryEnterDebugSection(void)

   228

{

   229

    if(AtomicCompareExchange(

   230

        &debug_state,

   231

        DEBUG_FREE,

   232

        DEBUG_BUSY) == DEBUG_FREE){

   233

        if(__DEBUG){

   234

            return 1;

   235

        }

   236

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   237

    }

   238

    return 0;

   239

}

   240

   241

#define DEBUG_UNLOCK 1

   242

void LeaveDebugSection(void)

   243

{

   244

    if(AtomicCompareExchange( &debug_state, 

   245

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   246

        char msg = DEBUG_UNLOCK;

   247

        /* signal to NRT for wakeup */

   248

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

   249

    }

   250

}

   251

   252

extern unsigned long __tick;

   253

   254

#define DEBUG_PENDING_DATA 1

   255

int WaitDebugData(unsigned long *tick)

   256

{

   257

    char cmd;

   258

    int res;

   259

    /* Wait signal from PLC thread */

   260

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

   261

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

   262

        *tick = __debug_tick;

   263

        return 0;

   264

    }

   265

    return -1;

   266

}

   267

   268

/* Called by PLC thread when debug_publish finished

   269

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

   270

void InitiateDebugTransfer()

   271

{

   272

    char msg = DEBUG_PENDING_DATA;

   273

    /* remember tick */

   274

    __debug_tick = __tick;

   275

    /* signal debugger thread it can read data */

   276

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

   277

}

   278

   279

int suspendDebug(int disable)

   280

{

   281

    char cmd = DEBUG_UNLOCK;

   282

    while(AtomicCompareExchange(

   283

            &debug_state,

   284

            DEBUG_FREE,

   285

            DEBUG_BUSY) != DEBUG_FREE &&

   286

            cmd == DEBUG_UNLOCK){

   287

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

   288

           return -1;

   289

       }

   290

    }

   291

    __DEBUG = !disable;

   292

    if (disable)

   293

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   294

    return 0;

   295

}

   296

   297

void resumeDebug(void)

   298

{

   299

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   300

}

   301

   302

#define PYTHON_PENDING_COMMAND 1

   303

   304

#define PYTHON_FREE 0

   305

#define PYTHON_BUSY 1

   306

static long python_state = PYTHON_FREE;

   307

   308

int WaitPythonCommands(void)

   309

{ 

   310

    char cmd;

   311

    /* Wait signal from PLC thread */

   312

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

   313

        return 0;

   314

    }

   315

    return -1;

   316

}

   317

   318

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

   319

void UnBlockPythonCommands(void)

   320

{

   321

    char msg = PYTHON_PENDING_COMMAND;

   322

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

   323

}

   324

   325

int TryLockPython(void)

   326

{

   327

    return AtomicCompareExchange(

   328

        &python_state,

   329

        PYTHON_FREE,

   330

        PYTHON_BUSY) == PYTHON_FREE;

   331

}

   332

   333

#define UNLOCK_PYTHON 1

   334

void LockPython(void)

   335

{

   336

    char cmd = UNLOCK_PYTHON;

   337

    while(AtomicCompareExchange(

   338

            &python_state,

   339

            PYTHON_FREE,

   340

            PYTHON_BUSY) != PYTHON_FREE &&

   341

            cmd == UNLOCK_PYTHON){

   342

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

   343

    }

   344

}

   345

   346

void UnLockPython(void)

   347

{

   348

    if(AtomicCompareExchange(

   349

            &python_state,

   350

            PYTHON_BUSY,

   351

            PYTHON_FREE) == PYTHON_BUSY){

   352

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

   353

           char cmd = UNLOCK_PYTHON;

   354

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

   355

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

   356

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

   357

}

   358

   359

int CheckRetainBuffer(void)

   360

{

   361

	return 1;

   362

}

   363

   364

void ValidateRetainBuffer(void)

   365

{

   366

}

   367

   368

void InValidateRetainBuffer(void)

   369

{

   370

}

   371

   372

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

   373

{

   374

}

   375

   376

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

   377

{

   378

}

   379