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(LOG_CRITICAL, 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 long __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

        // Sleep until next PLC run

   121

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

   122

        if(res==EINTR){

   123

            continue;

   124

        }

   125

        if(res!=0){

   126

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

   127

            return;

   128

        }

   129

   130

#ifdef REALTIME_LINUX

   131

        // timer overrun detection

   132

        clock_gettime(CLOCK_MONOTONIC, &plc_start_time);

   133

        deadline_time=next_cycle_time;

   134

        inc_timespec(&deadline_time, MAX_JITTER);

   135

        if(timespec_gt(plc_start_time, deadline_time)){

   136

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

   137

        }

   138

#endif

   139

   140

        PLC_GetTime(&__CURRENT_TIME);

   141

        __run();

   142

   143

        // ensure next PLC cycle occurence is in the future

   144

        clock_gettime(CLOCK_MONOTONIC, &plc_end_time);

   145

        while(timespec_gt(plc_end_time, next_cycle_time)){

   146

            periods += 1;

   147

            inc_timespec(&next_cycle_time, period_ns);

   148

        }

   149

   150

        // plc execution time overrun detection

   151

        if(periods > 1) {

   152

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

   153

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

   154

            //  - test if next cycle occurs after minimal idle

   155

            //  - enforce minimum idle time if not

   156

   157

            struct timespec earliest_possible_time = plc_end_time;

   158

            inc_timespec(&earliest_possible_time, MIN_IDLE_TIME_NS);

   159

            while(timespec_gt(earliest_possible_time, next_cycle_time)){

   160

                periods += 1;

   161

                inc_timespec(&next_cycle_time, period_ns);

   162

            }

   163

   164

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

   165

            __tick+=periods-1;

   166

   167

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

   168

        }

   169

    }

   170

   171

    pthread_exit(0);

   172

}

   173

   174

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

   175

int startPLC(int argc,char **argv)

   176

{

   177

   178

    int ret;

   179

	pthread_attr_t *pattr = NULL;

   180

   181

#ifdef REALTIME_LINUX

   182

	struct sched_param param;

   183

	pthread_attr_t attr;

   184

   185

    /* Lock memory */

   186

    ret = mlockall(MCL_CURRENT|MCL_FUTURE);

   187

    if(ret == -1) {

   188

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

   189

		return ret;

   190

    }

   191

   192

	/* Initialize pthread attributes (default values) */

   193

	ret = pthread_attr_init(&attr);

   194

	if (ret) {

   195

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

   196

		return ret;

   197

	}

   198

   199

	/* Set scheduler policy and priority of pthread */

   200

	ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);

   201

	if (ret) {

   202

		_LogError("pthread setschedpolicy failed\n");

   203

		return ret;

   204

	}

   205

	param.sched_priority = PLC_THREAD_PRIORITY;

   206

	ret = pthread_attr_setschedparam(&attr, &param);

   207

	if (ret) {

   208

		_LogError("pthread setschedparam failed\n");

   209

		return ret;

   210

	}

   211

   212

	/* Use scheduling parameters of attr */

   213

	ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

   214

	if (ret) {

   215

		_LogError("pthread setinheritsched failed\n");

   216

		return ret;

   217

	}

   218

   219

	pattr = &attr;

   220

#endif

   221

   222

    PLC_shutdown = 0;

   223

   224

    pthread_mutex_init(&debug_wait_mutex, NULL);

   225

    pthread_mutex_init(&debug_mutex, NULL);

   226

    pthread_mutex_init(&python_wait_mutex, NULL);

   227

    pthread_mutex_init(&python_mutex, NULL);

   228

   229

    pthread_mutex_lock(&debug_wait_mutex);

   230

    pthread_mutex_lock(&python_wait_mutex);

   231

   232

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

   233

   234

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

   235

        signal(SIGUSR1, PLCThreadSignalHandler);

   236

        /* Signal to end PLC thread */

   237

        signal(SIGUSR2, PLCThreadSignalHandler);

   238

        /* install signal handler for manual break */

   239

        signal(SIGINT, catch_signal);

   240

   241

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

   242

		if (ret) {

   243

			_LogError("create pthread failed\n");

   244

			return ret;

   245

		}

   246

    }else{

   247

        return ret;

   248

    }

   249

    return 0;

   250

}

   251

   252

int TryEnterDebugSection(void)

   253

{

   254

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

   255

        /* Only enter if debug active */

   256

        if(__DEBUG){

   257

            return 1;

   258

        }

   259

        pthread_mutex_unlock(&debug_mutex);

   260

    }

   261

    return 0;

   262

}

   263

   264

void LeaveDebugSection(void)

   265

{

   266

    pthread_mutex_unlock(&debug_mutex);

   267

}

   268

   269

int stopPLC()

   270

{

   271

    /* Stop the PLC */

   272

    PLC_shutdown = 1;

   273

    /* Order PLCThread to exit */

   274

    pthread_kill(PLC_thread, SIGUSR2);

   275

    pthread_join(PLC_thread, NULL);

   276

    __cleanup();

   277

    pthread_mutex_destroy(&debug_wait_mutex);

   278

    pthread_mutex_destroy(&debug_mutex);

   279

    pthread_mutex_destroy(&python_wait_mutex);

   280

    pthread_mutex_destroy(&python_mutex);

   281

    return 0;

   282

}

   283

   284

extern unsigned long __tick;

   285

   286

int WaitDebugData(unsigned long *tick)

   287

{

   288

    int res;

   289

    if (PLC_shutdown) return 1;

   290

    /* Wait signal from PLC thread */

   291

    res = pthread_mutex_lock(&debug_wait_mutex);

   292

    *tick = __debug_tick;

   293

    return res;

   294

}

   295

   296

/* Called by PLC thread when debug_publish finished

   297

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

   298

void InitiateDebugTransfer()

   299

{

   300

    /* remember tick */

   301

    __debug_tick = __tick;

   302

    /* signal debugger thread it can read data */

   303

    pthread_mutex_unlock(&debug_wait_mutex);

   304

}

   305

   306

int suspendDebug(int disable)

   307

{

   308

    /* Prevent PLC to enter debug code */

   309

    pthread_mutex_lock(&debug_mutex);

   310

    /*__DEBUG is protected by this mutex */

   311

    __DEBUG = !disable;

   312

    if (disable)

   313

        pthread_mutex_unlock(&debug_mutex);

   314

    return 0;

   315

}

   316

   317

void resumeDebug(void)

   318

{

   319

    __DEBUG = 1;

   320

    /* Let PLC enter debug code */

   321

    pthread_mutex_unlock(&debug_mutex);

   322

}

   323

   324

/* from plc_python.c */

   325

int WaitPythonCommands(void)

   326

{

   327

    /* Wait signal from PLC thread */

   328

    return pthread_mutex_lock(&python_wait_mutex);

   329

}

   330

   331

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

   332

void UnBlockPythonCommands(void)

   333

{

   334

    /* signal python thread it can read data */

   335

    pthread_mutex_unlock(&python_wait_mutex);

   336

}

   337

   338

int TryLockPython(void)

   339

{

   340

    return pthread_mutex_trylock(&python_mutex) == 0;

   341

}

   342

   343

void UnLockPython(void)

   344

{

   345

    pthread_mutex_unlock(&python_mutex);

   346

}

   347

   348

void LockPython(void)

   349

{

   350

    pthread_mutex_lock(&python_mutex);

   351

}

   352

   353

struct RT_to_nRT_signal_s {

   354

    int used;

   355

    pthread_cond_t WakeCond;

   356

    pthread_mutex_t WakeCondLock;

   357

};

   358

   359

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

   360

   361

#define _LogAndReturnNull(text) \

   362

    {\

   363

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

   364

        strncat(mstr, name, 255);\

   365

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

   366

        return NULL;\

   367

    }

   368

   369

void *create_RT_to_nRT_signal(char* name){

   370

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

   371

   372

    if(!sig)

   373

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

   374

   375

    sig->used = 1;

   376

    pthread_cond_init(&sig->WakeCond, NULL);

   377

    pthread_mutex_init(&sig->WakeCondLock, NULL);

   378

   379

    return (void*)sig;

   380

}

   381

   382

void delete_RT_to_nRT_signal(void* handle){

   383

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   384

   385

    pthread_mutex_lock(&sig->WakeCondLock);

   386

    sig->used = 0;

   387

    pthread_cond_signal(&sig->WakeCond);

   388

    pthread_mutex_unlock(&sig->WakeCondLock);

   389

}

   390

   391

int wait_RT_to_nRT_signal(void* handle){

   392

    int ret;

   393

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   394

    pthread_mutex_lock(&sig->WakeCondLock);

   395

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

   396

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

   397

    pthread_mutex_unlock(&sig->WakeCondLock);

   398

   399

    if(!sig->used){

   400

        pthread_cond_destroy(&sig->WakeCond);

   401

        pthread_mutex_destroy(&sig->WakeCondLock);

   402

        free(sig);

   403

    }

   404

    return ret;

   405

}

   406

   407

int unblock_RT_to_nRT_signal(void* handle){

   408

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   409

    return pthread_cond_signal(&sig->WakeCond);

   410

}

   411

   412

void nRT_reschedule(void){

   413

    sched_yield();

   414

}