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 (!PLC_shutdown) {

    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_WAITPYTHON_PIPE_CREATED;

   108

    }

   109

   110

    if (PLC_state & PLC_STATE_WAITPYTHON_FILE_OPENED) {

   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_PYTHON_PIPE_CREATED;

   128

    }

   129

   130

    if (PLC_state & PLC_STATE_PYTHON_FILE_OPENED) {

   131

        close(Python_pipe_fd);

   132

        PLC_state &= ~PLC_STATE_PYTHON_FILE_OPENED;

   133

    }

   134

   135

}

   136

   137

int stopPLC()

   138

{

   139

    /* Stop the PLC */

   140

    PLC_shutdown = 1;

   141

   142

    /* Wait until PLC task stops */

   143

    rt_task_join(&PLC_task);

   144

   145

    PLC_cleanup_all();

   146

    __cleanup();

   147

    __debug_tick = -1;

   148

    return 0;

   149

}

   150

   151

//

   152

void catch_signal(int sig)

   153

{

   154

    stopPLC();

   155

//  signal(SIGTERM, catch_signal);

   156

    signal(SIGINT, catch_signal);

   157

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

   158

    exit(0);

   159

}

   160

   161

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

   162

int startPLC(int argc,char **argv)

   163

{

   164

    signal(SIGINT, catch_signal);

   165

   166

    /* no memory swapping for that process */

   167

    mlockall(MCL_CURRENT | MCL_FUTURE);

   168

   169

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

   170

    Ttick = common_ticktime__?common_ticktime__:1000000;

   171

   172

    PLC_shutdown = 0;

   173

   174

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

   175

    /* create Debug_pipe */

   176

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

   177

        goto error;

   178

    PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;

   179

   180

    /* open Debug_pipe*/

   181

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

   182

    PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;

   183

   184

    /* create Python_pipe */

   185

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

   186

        goto error;

   187

    PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;

   188

   189

    /* open Python_pipe*/

   190

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

   191

    PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;

   192

   193

    /* create WaitDebug_pipe */

   194

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

   195

        goto error;

   196

    PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;

   197

   198

    /* open WaitDebug_pipe*/

   199

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

   200

    PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;

   201

   202

    /* create WaitPython_pipe */

   203

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

   204

        goto error;

   205

    PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;

   206

   207

    /* open WaitPython_pipe*/

   208

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

   209

    PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;

   210

   211

    /*** create PLC task ***/

   212

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

   213

    PLC_state |= PLC_STATE_TASK_CREATED;

   214

   215

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

   216

   217

    /* start PLC task */

   218

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

   219

   220

    return 0;

   221

   222

error:

   223

   224

    PLC_cleanup_all();

   225

    return 1;

   226

}

   227

   228

#define DEBUG_FREE 0

   229

#define DEBUG_BUSY 1

   230

static long debug_state = DEBUG_FREE;

   231

   232

int TryEnterDebugSection(void)

   233

{

   234

    if(AtomicCompareExchange(

   235

        &debug_state,

   236

        DEBUG_FREE,

   237

        DEBUG_BUSY) == DEBUG_FREE){

   238

        if(__DEBUG){

   239

            return 1;

   240

        }

   241

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   242

    }

   243

    return 0;

   244

}

   245

   246

#define DEBUG_UNLOCK 1

   247

void LeaveDebugSection(void)

   248

{

   249

    if(AtomicCompareExchange( &debug_state, 

   250

        DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){

   251

        char msg = DEBUG_UNLOCK;

   252

        /* signal to NRT for wakeup */

   253

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

   254

    }

   255

}

   256

   257

extern unsigned long __tick;

   258

   259

#define DEBUG_PENDING_DATA 1

   260

int WaitDebugData(unsigned long *tick)

   261

{

   262

    char cmd;

   263

    int res;

   264

    if (PLC_shutdown) return -1;

   265

    /* Wait signal from PLC thread */

   266

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

   267

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

   268

        *tick = __debug_tick;

   269

        return 0;

   270

    }

   271

    return -1;

   272

}

   273

   274

/* Called by PLC thread when debug_publish finished

   275

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

   276

void InitiateDebugTransfer()

   277

{

   278

    char msg = DEBUG_PENDING_DATA;

   279

    /* remember tick */

   280

    __debug_tick = __tick;

   281

    /* signal debugger thread it can read data */

   282

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

   283

}

   284

   285

int suspendDebug(int disable)

   286

{

   287

    char cmd = DEBUG_UNLOCK;

   288

    if (PLC_shutdown) return -1;

   289

    while(AtomicCompareExchange(

   290

            &debug_state,

   291

            DEBUG_FREE,

   292

            DEBUG_BUSY) != DEBUG_FREE &&

   293

            cmd == DEBUG_UNLOCK){

   294

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

   295

           return -1;

   296

       }

   297

    }

   298

    __DEBUG = !disable;

   299

    if (disable)

   300

        AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   301

    return 0;

   302

}

   303

   304

void resumeDebug(void)

   305

{

   306

    AtomicCompareExchange( &debug_state, DEBUG_BUSY, DEBUG_FREE);

   307

}

   308

   309

#define PYTHON_PENDING_COMMAND 1

   310

   311

#define PYTHON_FREE 0

   312

#define PYTHON_BUSY 1

   313

static long python_state = PYTHON_FREE;

   314

   315

int WaitPythonCommands(void)

   316

{ 

   317

    char cmd;

   318

    if (PLC_shutdown) return -1;

   319

    /* Wait signal from PLC thread */

   320

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

   321

        return 0;

   322

    }

   323

    return -1;

   324

}

   325

   326

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

   327

void UnBlockPythonCommands(void)

   328

{

   329

    char msg = PYTHON_PENDING_COMMAND;

   330

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

   331

}

   332

   333

int TryLockPython(void)

   334

{

   335

    return AtomicCompareExchange(

   336

        &python_state,

   337

        PYTHON_FREE,

   338

        PYTHON_BUSY) == PYTHON_FREE;

   339

}

   340

   341

#define UNLOCK_PYTHON 1

   342

void LockPython(void)

   343

{

   344

    char cmd = UNLOCK_PYTHON;

   345

    if (PLC_shutdown) return;

   346

    while(AtomicCompareExchange(

   347

            &python_state,

   348

            PYTHON_FREE,

   349

            PYTHON_BUSY) != PYTHON_FREE &&

   350

            cmd == UNLOCK_PYTHON){

   351

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

   352

    }

   353

}

   354

   355

void UnLockPython(void)

   356

{

   357

    if(AtomicCompareExchange(

   358

            &python_state,

   359

            PYTHON_BUSY,

   360

            PYTHON_FREE) == PYTHON_BUSY){

   361

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

   362

           char cmd = UNLOCK_PYTHON;

   363

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

   364

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

   365

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

   366

}

   367

   368

int CheckRetainBuffer(void)

   369

{

   370

	return 1;

   371

}

   372

   373

void ValidateRetainBuffer(void)

   374

{

   375

}

   376

   377

void InValidateRetainBuffer(void)

   378

{

   379

}

   380

   381

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

   382

{

   383

}

   384

   385

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

   386

{

   387

}