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

    15

static unsigned long __debug_tick;

    16

    17

static pthread_t PLC_thread;

    18

static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    19

static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;

    20

static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    21

static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;

    22

    23

static int PLC_shutdown = 0;

    24

    25

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

    26

{

    27

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    28

}

    29

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

    30

{

    31

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    32

}

    33

    34

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    35

{

    36

    struct timespec tmp;

    37

    clock_gettime(CLOCK_REALTIME, &tmp);

    38

    CURRENT_TIME->tv_sec = tmp.tv_sec;

    39

    CURRENT_TIME->tv_nsec = tmp.tv_nsec;

    40

}

    41

    42

static long long period_ns = 0;

    43

struct timespec next_abs_time;

    44

    45

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

    46

{

    47

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

    48

#ifdef __lldiv_t_defined

    49

    lldiv_t next_div = lldiv(next_ns, 1000000000);

    50

    ts->tv_sec = next_div.quot;

    51

    ts->tv_nsec = next_div.rem;

    52

#else

    53

    ts->tv_sec = next_ns / 1000000000;

    54

    ts->tv_nsec = next_ns % 1000000000;

    55

#endif

    56

}

    57

    58

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

    59

{

    60

    /*

    61

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

    62

    */

    63

    period_ns = period;

    64

    clock_gettime(CLOCK_MONOTONIC, &next_abs_time);

    65

    inc_timespec(&next_abs_time, next);

    66

    // interrupt clock_nanpsleep

    67

    pthread_kill(PLC_thread, SIGUSR1);

    68

}

    69

    70

void catch_signal(int sig)

    71

{

    72

//  signal(SIGTERM, catch_signal);

    73

  signal(SIGINT, catch_signal);

    74

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

    75

  exit(0);

    76

}

    77

    78

void PLCThreadSignalHandler(int sig)

    79

{

    80

    if (sig == SIGUSR2)

    81

        pthread_exit(NULL);

    82

}

    83

    84

int ForceSaveRetainReq(void) {

    85

    return PLC_shutdown;

    86

}

    87

    88

void PLC_thread_proc(void *arg)

    89

{

    90

    while (!PLC_shutdown) {

    91

        // Sleep until next PLC run

    92

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

    93

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

    94

        if(res==EINTR){

    95

            continue;

    96

        }

    97

        if(res!=0){

    98

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

    99

            return;

   100

        }

   101

        PLC_GetTime(&__CURRENT_TIME);

   102

        __run();

   103

        inc_timespec(&next_abs_time, period_ns);

   104

    }

   105

    pthread_exit(0);

   106

}

   107

   108

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

   109

int startPLC(int argc,char **argv)

   110

{

   111

    setlocale(LC_NUMERIC, "C");

   112

   113

    PLC_shutdown = 0;

   114

   115

    pthread_mutex_init(&debug_wait_mutex, NULL);

   116

    pthread_mutex_init(&debug_mutex, NULL);

   117

    pthread_mutex_init(&python_wait_mutex, NULL);

   118

    pthread_mutex_init(&python_mutex, NULL);

   119

   120

    pthread_mutex_lock(&debug_wait_mutex);

   121

    pthread_mutex_lock(&python_wait_mutex);

   122

   123

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

   124

   125

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

   126

        signal(SIGUSR1, PLCThreadSignalHandler);

   127

        /* Signal to end PLC thread */

   128

        signal(SIGUSR2, PLCThreadSignalHandler);

   129

        /* install signal handler for manual break */

   130

        signal(SIGINT, catch_signal);

   131

   132

        /* initialize next occurence and period */

   133

        period_ns = common_ticktime__;

   134

        clock_gettime(CLOCK_MONOTONIC, &next_abs_time);

   135

   136

        pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL);

   137

    }else{

   138

        return 1;

   139

    }

   140

    return 0;

   141

}

   142

   143

int TryEnterDebugSection(void)

   144

{

   145

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

   146

        /* Only enter if debug active */

   147

        if(__DEBUG){

   148

            return 1;

   149

        }

   150

        pthread_mutex_unlock(&debug_mutex);

   151

    }

   152

    return 0;

   153

}

   154

   155

void LeaveDebugSection(void)

   156

{

   157

    pthread_mutex_unlock(&debug_mutex);

   158

}

   159

   160

int stopPLC()

   161

{

   162

    /* Stop the PLC */

   163

    PLC_shutdown = 1;

   164

    /* Order PLCThread to exit */

   165

    pthread_kill(PLC_thread, SIGUSR2);

   166

    pthread_join(PLC_thread, NULL);

   167

    __cleanup();

   168

    pthread_mutex_destroy(&debug_wait_mutex);

   169

    pthread_mutex_destroy(&debug_mutex);

   170

    pthread_mutex_destroy(&python_wait_mutex);

   171

    pthread_mutex_destroy(&python_mutex);

   172

    return 0;

   173

}

   174

   175

extern unsigned long __tick;

   176

   177

int WaitDebugData(unsigned long *tick)

   178

{

   179

    int res;

   180

    if (PLC_shutdown) return 1;

   181

    /* Wait signal from PLC thread */

   182

    res = pthread_mutex_lock(&debug_wait_mutex);

   183

    *tick = __debug_tick;

   184

    return res;

   185

}

   186

   187

/* Called by PLC thread when debug_publish finished

   188

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

   189

void InitiateDebugTransfer()

   190

{

   191

    /* remember tick */

   192

    __debug_tick = __tick;

   193

    /* signal debugger thread it can read data */

   194

    pthread_mutex_unlock(&debug_wait_mutex);

   195

}

   196

   197

int suspendDebug(int disable)

   198

{

   199

    /* Prevent PLC to enter debug code */

   200

    pthread_mutex_lock(&debug_mutex);

   201

    /*__DEBUG is protected by this mutex */

   202

    __DEBUG = !disable;

   203

    if (disable)

   204

        pthread_mutex_unlock(&debug_mutex);

   205

    return 0;

   206

}

   207

   208

void resumeDebug(void)

   209

{

   210

    __DEBUG = 1;

   211

    /* Let PLC enter debug code */

   212

    pthread_mutex_unlock(&debug_mutex);

   213

}

   214

   215

/* from plc_python.c */

   216

int WaitPythonCommands(void)

   217

{

   218

    /* Wait signal from PLC thread */

   219

    return pthread_mutex_lock(&python_wait_mutex);

   220

}

   221

   222

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

   223

void UnBlockPythonCommands(void)

   224

{

   225

    /* signal python thread it can read data */

   226

    pthread_mutex_unlock(&python_wait_mutex);

   227

}

   228

   229

int TryLockPython(void)

   230

{

   231

    return pthread_mutex_trylock(&python_mutex) == 0;

   232

}

   233

   234

void UnLockPython(void)

   235

{

   236

    pthread_mutex_unlock(&python_mutex);

   237

}

   238

   239

void LockPython(void)

   240

{

   241

    pthread_mutex_lock(&python_mutex);

   242

}

   243

   244

struct RT_to_nRT_signal_s {

   245

    int used;

   246

    pthread_cond_t WakeCond;

   247

    pthread_mutex_t WakeCondLock;

   248

};

   249

   250

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

   251

   252

#define _LogAndReturnNull(text) \

   253

    {\

   254

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

   255

        strncat(mstr, name, 255);\

   256

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

   257

        return NULL;\

   258

    }

   259

   260

void *create_RT_to_nRT_signal(char* name){

   261

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

   262

   263

    if(!sig)

   264

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

   265

   266

    sig->used = 1;

   267

    pthread_cond_init(&sig->WakeCond, NULL);

   268

    pthread_mutex_init(&sig->WakeCondLock, NULL);

   269

   270

    return (void*)sig;

   271

}

   272

   273

void delete_RT_to_nRT_signal(void* handle){

   274

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   275

   276

    pthread_mutex_lock(&sig->WakeCondLock);

   277

    sig->used = 0;

   278

    pthread_cond_signal(&sig->WakeCond);

   279

    pthread_mutex_unlock(&sig->WakeCondLock);

   280

}

   281

   282

int wait_RT_to_nRT_signal(void* handle){

   283

    int ret;

   284

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   285

    pthread_mutex_lock(&sig->WakeCondLock);

   286

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

   287

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

   288

    pthread_mutex_unlock(&sig->WakeCondLock);

   289

   290

    if(!sig->used){

   291

        pthread_cond_destroy(&sig->WakeCond);

   292

        pthread_mutex_destroy(&sig->WakeCondLock);

   293

        free(sig);

   294

    }

   295

    return ret;

   296

}

   297

   298

int unblock_RT_to_nRT_signal(void* handle){

   299

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   300

    return pthread_cond_signal(&sig->WakeCond);

   301

}

   302

   303

void nRT_reschedule(void){

   304

    sched_yield();

   305

}