1

/**

     2

 * Linux specific code

     3

 **/

     4

     5

#include <stdio.h>

     6

#include <string.h>

     7

#include <time.h>

     8

#include <signal.h>

     9

#include <stdlib.h>

    10

#include <errno.h>

    11

#include <pthread.h>

    12

#include <locale.h>

    13

#include <semaphore.h>

    14

#ifdef REALTIME_LINUX

    15

#include <sys/mman.h>

    16

#endif

    17

    18

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

    19

    {\

    20

        char mstr[256];\

    21

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

    22

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

    23

    }

    24

    25

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

    26

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

    27

    28

static unsigned int __debug_tick;

    29

    30

static pthread_t PLC_thread;

    31

static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    32

static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;

    33

static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    34

static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;

    35

    36

static int PLC_shutdown = 0;

    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

    struct timespec tmp;

    50

    clock_gettime(CLOCK_REALTIME, &tmp);

    51

    CURRENT_TIME->tv_sec = tmp.tv_sec;

    52

    CURRENT_TIME->tv_nsec = tmp.tv_nsec;

    53

}

    54

    55

static long long period_ns = 0;

    56

struct timespec next_cycle_time;

    57

    58

static void inc_timespec(struct timespec *ts, unsigned long long value_ns)

    59

{

    60

    long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns;

    61

#ifdef __lldiv_t_defined

    62

    lldiv_t next_div = lldiv(next_ns, 1000000000);

    63

    ts->tv_sec = next_div.quot;

    64

    ts->tv_nsec = next_div.rem;

    65

#else

    66

    ts->tv_sec = next_ns / 1000000000;

    67

    ts->tv_nsec = next_ns % 1000000000;

    68

#endif

    69

}

    70

    71

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

    72

{

    73

    /*

    74

    printf("SetTimer(%lld,%lld)\n",next, period);

    75

    */

    76

    period_ns = period;

    77

    clock_gettime(CLOCK_MONOTONIC, &next_cycle_time);

    78

    inc_timespec(&next_cycle_time, next);

    79

    // interrupt clock_nanpsleep

    80

    pthread_kill(PLC_thread, SIGUSR1);

    81

}

    82

    83

void catch_signal(int sig)

    84

{

    85

//  signal(SIGTERM, catch_signal);

    86

  signal(SIGINT, catch_signal);

    87

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

    88

  exit(0);

    89

}

    90

    91

void PLCThreadSignalHandler(int sig)

    92

{

    93

    if (sig == SIGUSR2)

    94

        pthread_exit(NULL);

    95

}

    96

    97

int ForceSaveRetainReq(void) {

    98

    return PLC_shutdown;

    99

}

   100

   101

#define MAX_JITTER period_ns/10

   102

#define MIN_IDLE_TIME_NS 1000000 /* 1ms */

   103

/* Macro to compare timespec, evaluate to True if a is past b */

   104

#define timespec_gt(a,b) (a.tv_sec > b.tv_sec || (a.tv_sec == b.tv_sec && a.tv_nsec > b.tv_nsec))

   105

void PLC_thread_proc(void *arg)

   106

{

   107

    /* initialize next occurence and period */

   108

    period_ns = common_ticktime__;

   109

    clock_gettime(CLOCK_MONOTONIC, &next_cycle_time);

   110

   111

    while (!PLC_shutdown) {

   112

        int res;

   113

        struct timespec plc_end_time;

   114

        int periods = 0;

   115

#ifdef REALTIME_LINUX

   116

        struct timespec deadline_time;

   117

        struct timespec plc_start_time;

   118

#endif

   119

   120

// BEREMIZ_TEST_CYCLES is defined in tests that need to emulate time:

   121

// - all BEREMIZ_TEST_CYCLES cycles are executed in a row with no pause

   122

// - __CURRENT_TIME is incremented each cycle according to emulated cycle period

   123

   124

#ifndef BEREMIZ_TEST_CYCLES

   125

        // Sleep until next PLC run

   126

        res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_cycle_time, NULL);

   127

        if(res==EINTR){

   128

            continue;

   129

        }

   130

        if(res!=0){

   131

            _LogError("PLC thread timer returned error %d \n", res);

   132

            return;

   133

        }

   134

#endif // BEREMIZ_TEST_CYCLES

   135

   136

#ifdef REALTIME_LINUX

   137

        // timer overrun detection

   138

        clock_gettime(CLOCK_MONOTONIC, &plc_start_time);

   139

        deadline_time=next_cycle_time;

   140

        inc_timespec(&deadline_time, MAX_JITTER);

   141

        if(timespec_gt(plc_start_time, deadline_time)){

   142

            _LogWarning("PLC thread woken up too late. PLC cyclic task interval is too small.\n");

   143

        }

   144

#endif

   145

   146

#ifdef BEREMIZ_TEST_CYCLES

   147

#define xstr(s) str(s)

   148

#define str(arg) #arg

   149

        // fake current time

   150

        __CURRENT_TIME.tv_sec = next_cycle_time.tv_sec;

   151

        __CURRENT_TIME.tv_nsec = next_cycle_time.tv_nsec;

   152

        // exit loop when enough cycles

   153

        if(__tick >= BEREMIZ_TEST_CYCLES) {

   154

            _LogWarning("TEST PLC thread ended after "xstr(BEREMIZ_TEST_CYCLES)" cycles.\n");

   155

            // After pre-defined test cycles count, PLC thread exits.

   156

            // Remaining PLC runtime is expected to be cleaned-up/killed by test script

   157

            return;

   158

        }

   159

#else

   160

        PLC_GetTime(&__CURRENT_TIME);

   161

#endif

   162

        __run();

   163

   164

#ifndef BEREMIZ_TEST_CYCLES

   165

        // ensure next PLC cycle occurence is in the future

   166

        clock_gettime(CLOCK_MONOTONIC, &plc_end_time);

   167

        while(timespec_gt(plc_end_time, next_cycle_time))

   168

#endif

   169

        {

   170

            periods += 1;

   171

            inc_timespec(&next_cycle_time, period_ns);

   172

        }

   173

   174

        // plc execution time overrun detection

   175

        if(periods > 1) {

   176

            // Mitigate CPU hogging, in case of too small cyclic task interval:

   177

            //  - since cycle deadline already missed, better keep system responsive

   178

            //  - test if next cycle occurs after minimal idle

   179

            //  - enforce minimum idle time if not

   180

   181

            struct timespec earliest_possible_time = plc_end_time;

   182

            inc_timespec(&earliest_possible_time, MIN_IDLE_TIME_NS);

   183

            while(timespec_gt(earliest_possible_time, next_cycle_time)){

   184

                periods += 1;

   185

                inc_timespec(&next_cycle_time, period_ns);

   186

            }

   187

   188

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

   189

            __tick+=periods-1;

   190

   191

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

   192

        }

   193

    }

   194

   195

    pthread_exit(0);

   196

}

   197

   198

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

   199

int startPLC(int argc,char **argv)

   200

{

   201

   202

    int ret;

   203

	pthread_attr_t *pattr = NULL;

   204

   205

#ifdef REALTIME_LINUX

   206

	struct sched_param param;

   207

	pthread_attr_t attr;

   208

   209

    /* Lock memory */

   210

    ret = mlockall(MCL_CURRENT|MCL_FUTURE);

   211

    if(ret == -1) {

   212

		_LogError("mlockall failed: %m\n");

   213

		return ret;

   214

    }

   215

   216

	/* Initialize pthread attributes (default values) */

   217

	ret = pthread_attr_init(&attr);

   218

	if (ret) {

   219

		_LogError("init pthread attributes failed\n");

   220

		return ret;

   221

	}

   222

   223

	/* Set scheduler policy and priority of pthread */

   224

	ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);

   225

	if (ret) {

   226

		_LogError("pthread setschedpolicy failed\n");

   227

		return ret;

   228

	}

   229

	param.sched_priority = PLC_THREAD_PRIORITY;

   230

	ret = pthread_attr_setschedparam(&attr, &param);

   231

	if (ret) {

   232

		_LogError("pthread setschedparam failed\n");

   233

		return ret;

   234

	}

   235

   236

	/* Use scheduling parameters of attr */

   237

	ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

   238

	if (ret) {

   239

		_LogError("pthread setinheritsched failed\n");

   240

		return ret;

   241

	}

   242

   243

	pattr = &attr;

   244

#endif

   245

   246

    PLC_shutdown = 0;

   247

   248

    pthread_mutex_init(&debug_wait_mutex, NULL);

   249

    pthread_mutex_init(&debug_mutex, NULL);

   250

    pthread_mutex_init(&python_wait_mutex, NULL);

   251

    pthread_mutex_init(&python_mutex, NULL);

   252

   253

    pthread_mutex_lock(&debug_wait_mutex);

   254

    pthread_mutex_lock(&python_wait_mutex);

   255

   256

    if((ret = __init(argc,argv)) == 0 ){

   257

   258

        /* Signal to wakeup PLC thread when period changes */

   259

        signal(SIGUSR1, PLCThreadSignalHandler);

   260

        /* Signal to end PLC thread */

   261

        signal(SIGUSR2, PLCThreadSignalHandler);

   262

        /* install signal handler for manual break */

   263

        signal(SIGINT, catch_signal);

   264

   265

        ret = pthread_create(&PLC_thread, pattr, (void*) &PLC_thread_proc, NULL);

   266

		if (ret) {

   267

			_LogError("create pthread failed\n");

   268

			return ret;

   269

		}

   270

    }else{

   271

        return ret;

   272

    }

   273

    return 0;

   274

}

   275

   276

int TryEnterDebugSection(void)

   277

{

   278

    if (pthread_mutex_trylock(&debug_mutex) == 0){

   279

        /* Only enter if debug active */

   280

        if(__DEBUG){

   281

            return 1;

   282

        }

   283

        pthread_mutex_unlock(&debug_mutex);

   284

    }

   285

    return 0;

   286

}

   287

   288

void LeaveDebugSection(void)

   289

{

   290

    pthread_mutex_unlock(&debug_mutex);

   291

}

   292

   293

int stopPLC()

   294

{

   295

    /* Stop the PLC */

   296

    PLC_shutdown = 1;

   297

    /* Order PLCThread to exit */

   298

    pthread_kill(PLC_thread, SIGUSR2);

   299

    pthread_join(PLC_thread, NULL);

   300

    __cleanup();

   301

    pthread_mutex_destroy(&debug_wait_mutex);

   302

    pthread_mutex_destroy(&debug_mutex);

   303

    pthread_mutex_destroy(&python_wait_mutex);

   304

    pthread_mutex_destroy(&python_mutex);

   305

    return 0;

   306

}

   307

   308

extern unsigned int __tick;

   309

   310

int WaitDebugData(unsigned int *tick)

   311

{

   312

    int res;

   313

    if (PLC_shutdown) return 1;

   314

    /* Wait signal from PLC thread */

   315

    res = pthread_mutex_lock(&debug_wait_mutex);

   316

    *tick = __debug_tick;

   317

    return res;

   318

}

   319

   320

/* Called by PLC thread when debug_publish finished

   321

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

   322

void InitiateDebugTransfer()

   323

{

   324

    /* remember tick */

   325

    __debug_tick = __tick;

   326

    /* signal debugger thread it can read data */

   327

    pthread_mutex_unlock(&debug_wait_mutex);

   328

}

   329

   330

int suspendDebug(int disable)

   331

{

   332

    /* Prevent PLC to enter debug code */

   333

    pthread_mutex_lock(&debug_mutex);

   334

    /*__DEBUG is protected by this mutex */

   335

    __DEBUG = !disable;

   336

    if (disable)

   337

        pthread_mutex_unlock(&debug_mutex);

   338

    return 0;

   339

}

   340

   341

void resumeDebug(void)

   342

{

   343

    __DEBUG = 1;

   344

    /* Let PLC enter debug code */

   345

    pthread_mutex_unlock(&debug_mutex);

   346

}

   347

   348

/* from plc_python.c */

   349

int WaitPythonCommands(void)

   350

{

   351

    /* Wait signal from PLC thread */

   352

    return pthread_mutex_lock(&python_wait_mutex);

   353

}

   354

   355

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

   356

void UnBlockPythonCommands(void)

   357

{

   358

    /* signal python thread it can read data */

   359

    pthread_mutex_unlock(&python_wait_mutex);

   360

}

   361

   362

int TryLockPython(void)

   363

{

   364

    return pthread_mutex_trylock(&python_mutex) == 0;

   365

}

   366

   367

void UnLockPython(void)

   368

{

   369

    pthread_mutex_unlock(&python_mutex);

   370

}

   371

   372

void LockPython(void)

   373

{

   374

    pthread_mutex_lock(&python_mutex);

   375

}

   376

   377

struct RT_to_nRT_signal_s {

   378

    int used;

   379

    pthread_cond_t WakeCond;

   380

    pthread_mutex_t WakeCondLock;

   381

};

   382

   383

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

   384

   385

#define _LogAndReturnNull(text) \

   386

    {\

   387

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

   388

        strncat(mstr, name, 255);\

   389

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

   390

        return NULL;\

   391

    }

   392

   393

void *create_RT_to_nRT_signal(char* name){

   394

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)malloc(sizeof(RT_to_nRT_signal_t));

   395

   396

    if(!sig)

   397

        _LogAndReturnNull("Failed allocating memory for RT_to_nRT signal");

   398

   399

    sig->used = 1;

   400

    pthread_cond_init(&sig->WakeCond, NULL);

   401

    pthread_mutex_init(&sig->WakeCondLock, NULL);

   402

   403

    return (void*)sig;

   404

}

   405

   406

void delete_RT_to_nRT_signal(void* handle){

   407

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   408

   409

    pthread_mutex_lock(&sig->WakeCondLock);

   410

    sig->used = 0;

   411

    pthread_cond_signal(&sig->WakeCond);

   412

    pthread_mutex_unlock(&sig->WakeCondLock);

   413

}

   414

   415

int wait_RT_to_nRT_signal(void* handle){

   416

    int ret;

   417

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   418

    pthread_mutex_lock(&sig->WakeCondLock);

   419

    ret = pthread_cond_wait(&sig->WakeCond, &sig->WakeCondLock);

   420

    if(!sig->used) ret = -EINVAL;

   421

    pthread_mutex_unlock(&sig->WakeCondLock);

   422

   423

    if(!sig->used){

   424

        pthread_cond_destroy(&sig->WakeCond);

   425

        pthread_mutex_destroy(&sig->WakeCondLock);

   426

        free(sig);

   427

    }

   428

    return ret;

   429

}

   430

   431

int unblock_RT_to_nRT_signal(void* handle){

   432

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   433

    return pthread_cond_signal(&sig->WakeCond);

   434

}

   435

   436

void nRT_reschedule(void){

   437

    sched_yield();

   438

}