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

static unsigned long __debug_tick;

    19

    20

static pthread_t PLC_thread;

    21

static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    22

static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;

    23

static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    24

static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;

    25

    26

static int PLC_shutdown = 0;

    27

    28

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

    29

{

    30

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    31

}

    32

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

    33

{

    34

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    35

}

    36

    37

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    38

{

    39

    struct timespec tmp;

    40

    clock_gettime(CLOCK_REALTIME, &tmp);

    41

    CURRENT_TIME->tv_sec = tmp.tv_sec;

    42

    CURRENT_TIME->tv_nsec = tmp.tv_nsec;

    43

}

    44

    45

static long long period_ns = 0;

    46

struct timespec next_abs_time;

    47

    48

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

    49

{

    50

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

    51

#ifdef __lldiv_t_defined

    52

    lldiv_t next_div = lldiv(next_ns, 1000000000);

    53

    ts->tv_sec = next_div.quot;

    54

    ts->tv_nsec = next_div.rem;

    55

#else

    56

    ts->tv_sec = next_ns / 1000000000;

    57

    ts->tv_nsec = next_ns % 1000000000;

    58

#endif

    59

}

    60

    61

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

    62

{

    63

    /*

    64

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

    65

    */

    66

    period_ns = period;

    67

    clock_gettime(CLOCK_MONOTONIC, &next_abs_time);

    68

    inc_timespec(&next_abs_time, next);

    69

    // interrupt clock_nanpsleep

    70

    pthread_kill(PLC_thread, SIGUSR1);

    71

}

    72

    73

void catch_signal(int sig)

    74

{

    75

//  signal(SIGTERM, catch_signal);

    76

  signal(SIGINT, catch_signal);

    77

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

    78

  exit(0);

    79

}

    80

    81

void PLCThreadSignalHandler(int sig)

    82

{

    83

    if (sig == SIGUSR2)

    84

        pthread_exit(NULL);

    85

}

    86

    87

int ForceSaveRetainReq(void) {

    88

    return PLC_shutdown;

    89

}

    90

    91

void PLC_thread_proc(void *arg)

    92

{

    93

    while (!PLC_shutdown) {

    94

        // Sleep until next PLC run

    95

        // TODO check result of clock_nanosleep and wait again or exit eventually

    96

        int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL);

    97

        if(res==EINTR){

    98

            continue;

    99

        }

   100

        if(res!=0){

   101

            printf("PLC thread died with error %d \n", res);

   102

            return;

   103

        }

   104

        PLC_GetTime(&__CURRENT_TIME);

   105

        __run();

   106

        inc_timespec(&next_abs_time, period_ns);

   107

    }

   108

    pthread_exit(0);

   109

}

   110

   111

#define _LogError(text,...) \

   112

    {\

   113

        char mstr[256];\

   114

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

   115

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

   116

    }

   117

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

   118

int startPLC(int argc,char **argv)

   119

{

   120

   121

    int ret;

   122

	pthread_attr_t *pattr = NULL;

   123

   124

#ifdef REALTIME_LINUX

   125

	struct sched_param param;

   126

	pthread_attr_t attr;

   127

   128

    /* Lock memory */

   129

    ret = mlockall(MCL_CURRENT|MCL_FUTURE);

   130

    if(ret == -1) {

   131

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

   132

		return ret;

   133

    }

   134

   135

	/* Initialize pthread attributes (default values) */

   136

	ret = pthread_attr_init(&attr);

   137

	if (ret) {

   138

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

   139

		return ret;

   140

	}

   141

   142

	/* Set scheduler policy and priority of pthread */

   143

	ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);

   144

	if (ret) {

   145

		_LogError("pthread setschedpolicy failed\n");

   146

		return ret;

   147

	}

   148

	param.sched_priority = PLC_THREAD_PRIORITY;

   149

	ret = pthread_attr_setschedparam(&attr, &param);

   150

	if (ret) {

   151

		_LogError("pthread setschedparam failed\n");

   152

		return ret;

   153

	}

   154

   155

	/* Use scheduling parameters of attr */

   156

	ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

   157

	if (ret) {

   158

		_LogError("pthread setinheritsched failed\n");

   159

		return ret;

   160

	}

   161

   162

	pattr = &attr;

   163

#endif

   164

   165

    PLC_shutdown = 0;

   166

   167

    pthread_mutex_init(&debug_wait_mutex, NULL);

   168

    pthread_mutex_init(&debug_mutex, NULL);

   169

    pthread_mutex_init(&python_wait_mutex, NULL);

   170

    pthread_mutex_init(&python_mutex, NULL);

   171

   172

    pthread_mutex_lock(&debug_wait_mutex);

   173

    pthread_mutex_lock(&python_wait_mutex);

   174

   175

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

   176

   177

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

   178

        signal(SIGUSR1, PLCThreadSignalHandler);

   179

        /* Signal to end PLC thread */

   180

        signal(SIGUSR2, PLCThreadSignalHandler);

   181

        /* install signal handler for manual break */

   182

        signal(SIGINT, catch_signal);

   183

   184

        /* initialize next occurence and period */

   185

        period_ns = common_ticktime__;

   186

        clock_gettime(CLOCK_MONOTONIC, &next_abs_time);

   187

   188

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

   189

		if (ret) {

   190

			_LogError("create pthread failed\n");

   191

			return ret;

   192

		}

   193

    }else{

   194

        return ret;

   195

    }

   196

    return 0;

   197

}

   198

   199

int TryEnterDebugSection(void)

   200

{

   201

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

   202

        /* Only enter if debug active */

   203

        if(__DEBUG){

   204

            return 1;

   205

        }

   206

        pthread_mutex_unlock(&debug_mutex);

   207

    }

   208

    return 0;

   209

}

   210

   211

void LeaveDebugSection(void)

   212

{

   213

    pthread_mutex_unlock(&debug_mutex);

   214

}

   215

   216

int stopPLC()

   217

{

   218

    /* Stop the PLC */

   219

    PLC_shutdown = 1;

   220

    /* Order PLCThread to exit */

   221

    pthread_kill(PLC_thread, SIGUSR2);

   222

    pthread_join(PLC_thread, NULL);

   223

    __cleanup();

   224

    pthread_mutex_destroy(&debug_wait_mutex);

   225

    pthread_mutex_destroy(&debug_mutex);

   226

    pthread_mutex_destroy(&python_wait_mutex);

   227

    pthread_mutex_destroy(&python_mutex);

   228

    return 0;

   229

}

   230

   231

extern unsigned long __tick;

   232

   233

int WaitDebugData(unsigned long *tick)

   234

{

   235

    int res;

   236

    if (PLC_shutdown) return 1;

   237

    /* Wait signal from PLC thread */

   238

    res = pthread_mutex_lock(&debug_wait_mutex);

   239

    *tick = __debug_tick;

   240

    return res;

   241

}

   242

   243

/* Called by PLC thread when debug_publish finished

   244

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

   245

void InitiateDebugTransfer()

   246

{

   247

    /* remember tick */

   248

    __debug_tick = __tick;

   249

    /* signal debugger thread it can read data */

   250

    pthread_mutex_unlock(&debug_wait_mutex);

   251

}

   252

   253

int suspendDebug(int disable)

   254

{

   255

    /* Prevent PLC to enter debug code */

   256

    pthread_mutex_lock(&debug_mutex);

   257

    /*__DEBUG is protected by this mutex */

   258

    __DEBUG = !disable;

   259

    if (disable)

   260

        pthread_mutex_unlock(&debug_mutex);

   261

    return 0;

   262

}

   263

   264

void resumeDebug(void)

   265

{

   266

    __DEBUG = 1;

   267

    /* Let PLC enter debug code */

   268

    pthread_mutex_unlock(&debug_mutex);

   269

}

   270

   271

/* from plc_python.c */

   272

int WaitPythonCommands(void)

   273

{

   274

    /* Wait signal from PLC thread */

   275

    return pthread_mutex_lock(&python_wait_mutex);

   276

}

   277

   278

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

   279

void UnBlockPythonCommands(void)

   280

{

   281

    /* signal python thread it can read data */

   282

    pthread_mutex_unlock(&python_wait_mutex);

   283

}

   284

   285

int TryLockPython(void)

   286

{

   287

    return pthread_mutex_trylock(&python_mutex) == 0;

   288

}

   289

   290

void UnLockPython(void)

   291

{

   292

    pthread_mutex_unlock(&python_mutex);

   293

}

   294

   295

void LockPython(void)

   296

{

   297

    pthread_mutex_lock(&python_mutex);

   298

}

   299

   300

struct RT_to_nRT_signal_s {

   301

    int used;

   302

    pthread_cond_t WakeCond;

   303

    pthread_mutex_t WakeCondLock;

   304

};

   305

   306

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

   307

   308

#define _LogAndReturnNull(text) \

   309

    {\

   310

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

   311

        strncat(mstr, name, 255);\

   312

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

   313

        return NULL;\

   314

    }

   315

   316

void *create_RT_to_nRT_signal(char* name){

   317

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

   318

   319

    if(!sig)

   320

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

   321

   322

    sig->used = 1;

   323

    pthread_cond_init(&sig->WakeCond, NULL);

   324

    pthread_mutex_init(&sig->WakeCondLock, NULL);

   325

   326

    return (void*)sig;

   327

}

   328

   329

void delete_RT_to_nRT_signal(void* handle){

   330

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   331

   332

    pthread_mutex_lock(&sig->WakeCondLock);

   333

    sig->used = 0;

   334

    pthread_cond_signal(&sig->WakeCond);

   335

    pthread_mutex_unlock(&sig->WakeCondLock);

   336

}

   337

   338

int wait_RT_to_nRT_signal(void* handle){

   339

    int ret;

   340

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   341

    pthread_mutex_lock(&sig->WakeCondLock);

   342

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

   343

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

   344

    pthread_mutex_unlock(&sig->WakeCondLock);

   345

   346

    if(!sig->used){

   347

        pthread_cond_destroy(&sig->WakeCond);

   348

        pthread_mutex_destroy(&sig->WakeCondLock);

   349

        free(sig);

   350

    }

   351

    return ret;

   352

}

   353

   354

int unblock_RT_to_nRT_signal(void* handle){

   355

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   356

    return pthread_cond_signal(&sig->WakeCond);

   357

}

   358

   359

void nRT_reschedule(void){

   360

    sched_yield();

   361

}