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 <pthread.h>

    11

#include <locale.h>

    12

#include <semaphore.h>

    13

    14

static sem_t Run_PLC;

    15

    16

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

    17

{

    18

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    19

}

    20

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

    21

{

    22

    return __sync_val_compare_and_swap(atomicvar, compared, exchange);

    23

}

    24

    25

void PLC_GetTime(IEC_TIME *CURRENT_TIME)

    26

{

    27

    struct timespec tmp;

    28

    clock_gettime(CLOCK_REALTIME, &tmp);

    29

    CURRENT_TIME->tv_sec = tmp.tv_sec;

    30

    CURRENT_TIME->tv_nsec = tmp.tv_nsec;

    31

}

    32

    33

void PLC_timer_notify(sigval_t val)

    34

{

    35

    PLC_GetTime(&__CURRENT_TIME);

    36

    sem_post(&Run_PLC);

    37

}

    38

    39

timer_t PLC_timer;

    40

    41

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

    42

{

    43

    struct itimerspec timerValues;

    44

	/*

    45

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

    46

	*/

    47

    memset (&timerValues, 0, sizeof (struct itimerspec));

    48

	{

    49

#ifdef __lldiv_t_defined

    50

		lldiv_t nxt_div = lldiv(next, 1000000000);

    51

		lldiv_t period_div = lldiv(period, 1000000000);

    52

	    timerValues.it_value.tv_sec = nxt_div.quot;

    53

	    timerValues.it_value.tv_nsec = nxt_div.rem;

    54

	    timerValues.it_interval.tv_sec = period_div.quot;

    55

	    timerValues.it_interval.tv_nsec = period_div.rem;

    56

#else

    57

	    timerValues.it_value.tv_sec = next / 1000000000;

    58

	    timerValues.it_value.tv_nsec = next % 1000000000;

    59

	    timerValues.it_interval.tv_sec = period / 1000000000;

    60

	    timerValues.it_interval.tv_nsec = period % 1000000000;

    61

#endif

    62

	}

    63

    timer_settime (PLC_timer, 0, &timerValues, NULL);

    64

}

    65

//

    66

void catch_signal(int sig)

    67

{

    68

//  signal(SIGTERM, catch_signal);

    69

  signal(SIGINT, catch_signal);

    70

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

    71

  exit(0);

    72

}

    73

    74

    75

static unsigned long __debug_tick;

    76

    77

pthread_t PLC_thread;

    78

static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    79

static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER;

    80

static pthread_mutex_t debug_wait_mutex = PTHREAD_MUTEX_INITIALIZER;

    81

static pthread_mutex_t debug_mutex = PTHREAD_MUTEX_INITIALIZER;

    82

    83

int PLC_shutdown = 0;

    84

    85

int ForceSaveRetainReq(void) {

    86

    return PLC_shutdown;

    87

}

    88

    89

void PLC_thread_proc(void *arg)

    90

{

    91

    while (!PLC_shutdown) {

    92

        sem_wait(&Run_PLC);

    93

        __run();

    94

    }

    95

    pthread_exit(0);

    96

}

    97

    98

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

    99

int startPLC(int argc,char **argv)

   100

{

   101

    struct sigevent sigev;

   102

    setlocale(LC_NUMERIC, "C");

   103

   104

    PLC_shutdown = 0;

   105

   106

    sem_init(&Run_PLC, 0, 0);

   107

   108

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

   109

   110

    memset (&sigev, 0, sizeof (struct sigevent));

   111

    sigev.sigev_value.sival_int = 0;

   112

    sigev.sigev_notify = SIGEV_THREAD;

   113

    sigev.sigev_notify_attributes = NULL;

   114

    sigev.sigev_notify_function = PLC_timer_notify;

   115

   116

    pthread_mutex_init(&debug_wait_mutex, NULL);

   117

    pthread_mutex_init(&debug_mutex, NULL);

   118

    pthread_mutex_init(&python_wait_mutex, NULL);

   119

    pthread_mutex_init(&python_mutex, NULL);

   120

   121

    pthread_mutex_lock(&debug_wait_mutex);

   122

    pthread_mutex_lock(&python_wait_mutex);

   123

   124

    timer_create (CLOCK_MONOTONIC, &sigev, &PLC_timer);

   125

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

   126

        PLC_SetTimer(common_ticktime__,common_ticktime__);

   127

   128

        /* install signal handler for manual break */

   129

        signal(SIGINT, catch_signal);

   130

    }else{

   131

        return 1;

   132

    }

   133

    return 0;

   134

}

   135

   136

int TryEnterDebugSection(void)

   137

{

   138

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

   139

        /* Only enter if debug active */

   140

        if(__DEBUG){

   141

            return 1;

   142

        }

   143

        pthread_mutex_unlock(&debug_mutex);

   144

    }

   145

    return 0;

   146

}

   147

   148

void LeaveDebugSection(void)

   149

{

   150

    pthread_mutex_unlock(&debug_mutex);

   151

}

   152

   153

int stopPLC()

   154

{

   155

    /* Stop the PLC */

   156

    PLC_shutdown = 1;

   157

    sem_post(&Run_PLC);

   158

    PLC_SetTimer(0,0);

   159

	pthread_join(PLC_thread, NULL);

   160

	sem_destroy(&Run_PLC);

   161

    timer_delete (PLC_timer);

   162

    __cleanup();

   163

    pthread_mutex_destroy(&debug_wait_mutex);

   164

    pthread_mutex_destroy(&debug_mutex);

   165

    pthread_mutex_destroy(&python_wait_mutex);

   166

    pthread_mutex_destroy(&python_mutex);

   167

    return 0;

   168

}

   169

   170

extern unsigned long __tick;

   171

   172

int WaitDebugData(unsigned long *tick)

   173

{

   174

    int res;

   175

    if (PLC_shutdown) return 1;

   176

    /* Wait signal from PLC thread */

   177

    res = pthread_mutex_lock(&debug_wait_mutex);

   178

    *tick = __debug_tick;

   179

    return res;

   180

}

   181

   182

/* Called by PLC thread when debug_publish finished

   183

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

   184

void InitiateDebugTransfer()

   185

{

   186

    /* remember tick */

   187

    __debug_tick = __tick;

   188

    /* signal debugger thread it can read data */

   189

    pthread_mutex_unlock(&debug_wait_mutex);

   190

}

   191

   192

int suspendDebug(int disable)

   193

{

   194

    /* Prevent PLC to enter debug code */

   195

    pthread_mutex_lock(&debug_mutex);

   196

    /*__DEBUG is protected by this mutex */

   197

    __DEBUG = !disable;

   198

    if (disable)

   199

    	pthread_mutex_unlock(&debug_mutex);

   200

    return 0;

   201

}

   202

   203

void resumeDebug(void)

   204

{

   205

    __DEBUG = 1;

   206

    /* Let PLC enter debug code */

   207

    pthread_mutex_unlock(&debug_mutex);

   208

}

   209

   210

/* from plc_python.c */

   211

int WaitPythonCommands(void)

   212

{

   213

    /* Wait signal from PLC thread */

   214

    return pthread_mutex_lock(&python_wait_mutex);

   215

}

   216

   217

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

   218

void UnBlockPythonCommands(void)

   219

{

   220

    /* signal python thread it can read data */

   221

    pthread_mutex_unlock(&python_wait_mutex);

   222

}

   223

   224

int TryLockPython(void)

   225

{

   226

    return pthread_mutex_trylock(&python_mutex) == 0;

   227

}

   228

   229

void UnLockPython(void)

   230

{

   231

    pthread_mutex_unlock(&python_mutex);

   232

}

   233

   234

void LockPython(void)

   235

{

   236

    pthread_mutex_lock(&python_mutex);

   237

}

   238

   239

struct RT_to_nRT_signal_s {

   240

    pthread_cond_t WakeCond;

   241

    pthread_mutex_t WakeCondLock;

   242

};

   243

   244

typedef struct RT_to_nRT_signal_s RT_to_nRT_signal_t;

   245

   246

#define _LogAndReturnNull(text) \

   247

    {\

   248

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

   249

        strncat(mstr, name, 255);\

   250

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

   251

        return NULL;\

   252

    }

   253

   254

void *create_RT_to_nRT_signal(char* name){

   255

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

   256

   257

    if(!sig) 

   258

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

   259

   260

    pthread_cond_init(&sig->WakeCond, NULL);

   261

    pthread_mutex_init(&sig->WakeCondLock, NULL);

   262

   263

    return (void*)sig;

   264

}

   265

   266

void delete_RT_to_nRT_signal(void* handle){

   267

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   268

   269

    pthread_cond_destroy(&sig->WakeCond);

   270

    pthread_mutex_destroy(&sig->WakeCondLock);

   271

   272

    free(sig);

   273

}

   274

   275

int wait_RT_to_nRT_signal(void* handle){

   276

    int ret;

   277

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   278

    pthread_mutex_lock(&sig->WakeCondLock);

   279

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

   280

    pthread_mutex_unlock(&sig->WakeCondLock);

   281

    return ret;

   282

}

   283

   284

int unblock_RT_to_nRT_signal(void* handle){

   285

    RT_to_nRT_signal_t *sig = (RT_to_nRT_signal_t*)handle;

   286

    return pthread_cond_signal(&sig->WakeCond);

   287

}

   288

   289

void nRT_reschedule(void){

   290

    sched_yield();

   291

}