1

/*

     2

 * (c) 2008 Edouard TISSERANT

     3

 *

     4

 * Offered to the public under the terms of the GNU Lesser General Public

     5

 * License as published by the Free Software Foundation; either version 2

     6

 * of the License, or (at your option) any later version.

     7

 *

     8

 * This program is distributed in the hope that it will be useful, but

     9

 * WITHOUT ANY WARRANTY; without even the implied warranty of

    10

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser

    11

 * General Public License for more details.

    12

 *

    13

 * This code is made available on the understanding that it will not be

    14

 * used in safety-critical situations without a full and competent review.

    15

 */

    16

    17

/****

    18

 * IEC 61131-3 standard function library

    19

 */

    20

    21

#include <limits.h>

    22

#include <float.h>

    23

#include <math.h>

    24

#include <time.h>

    25

#include <stdint.h>

    26

    27

#include <stdio.h>

    28

#include <stdlib.h>

    29

#include <string.h>

    30

#include <stdarg.h>

    31

    32

#include "iec_std_lib_generated.h"

    33

    34

#define __print_BOOL(name) printf("  %s = (BOOL) %s\n",#name, name?"TRUE":"FALSE");

    35

#define __print_SINT(name) printf("  %s = (SINT) %d\n",#name, name);

    36

#define __print_INT(name) printf("  %s = (INT) %d\n",#name, name);

    37

#define __print_DINT(name) printf("  %s = (DINT) %d\n",#name, name);

    38

#define __print_LINT(name) printf("  %s = (LINT) %d\n",#name, name);

    39

#define __print_USINT(name) printf("  %s = (USINT) %u\n",#name, name);

    40

#define __print_UINT(name) printf("  %s = (UINT) %u\n",#name, name);

    41

#define __print_UDINT(name) printf("  %s = (UDINT) %u\n",#name, name);

    42

#define __print_ULINT(name) printf("  %s = (ULINT) %lu\n",#name, name);

    43

#define __print_REAL(name) printf("  %s = (REAL) %f\n",#name, (double)name);

    44

#define __print_LREAL(name) printf("  %s = (LREAL) %f\n",#name, (double)name);

    45

#define __print_TIME(name) {STRING __tmp = __time_to_string(name);__tmp.body[__tmp.len] = 0; printf("  %s = (TIME) %*s\n",#name, __tmp.len, &__tmp.body);}

    46

#define __print_DATE(name) {STRING __tmp = __date_to_string(name);__tmp.body[__tmp.len] = 0; printf("  %s = (DATE) %*s\n",#name, __tmp.len, &__tmp.body);}

    47

#define __print_TOD(name) {STRING __tmp = __tod_to_string(name);__tmp.body[__tmp.len] = 0; printf("  %s = (TOD) %*s\n",#name, __tmp.len, &__tmp.body);}

    48

#define __print_DT(name) {STRING __tmp = __dt_to_string(name);__tmp.body[__tmp.len] = 0; printf("  %s = (DT) %*s\n",#name, __tmp.len, &__tmp.body);}

    49

#define __print_STRING(name) printf("  %s = (STRING) {%d, \"%*s\"}\n",#name, name.len, name.len, &name.body);

    50

#define __print_BYTE(name) printf("  %s = (BYTE) 0x%2.2x\n",#name, name);

    51

#define __print_WORD(name) printf("  %s = (WORD) 0x%4.4x\n",#name, name);

    52

#define __print_DWORD(name) printf("  %s = (DWORD) 0x%8.8x\n",#name, name);

    53

#define __print_LWORD(name) printf("  %s = (LWORD) 0x%16.16lx\n",#name, name);

    54

    55

#ifdef DEBUG_IEC

    56

#define DBG(...) printf(__VA_ARGS__);

    57

#define DBG_TYPE(TYPENAME, name) __print_##TYPENAME(name);

    58

#else

    59

#define DBG(...)

    60

#define DBG_TYPE(TYPENAME, name)

    61

#endif

    62

    63

/*********************/

    64

/*  IEC Types defs   */

    65

/*********************/

    66

    67

/* Include non windows.h clashing typedefs */

    68

#include "iec_types.h"

    69

    70

/* Those typdefs clash with windows.h */

    71

/* i.e. this file cannot be included aside windows.h */

    72

typedef IEC_BOOL  BOOL;

    73

    74

#define TRUE 1

    75

#define FALSE 0

    76

    77

typedef IEC_SINT    SINT;

    78

typedef IEC_INT   INT;

    79

typedef IEC_DINT   DINT;

    80

typedef IEC_LINT   LINT;

    81

    82

typedef IEC_USINT    USINT;

    83

typedef IEC_UINT   UINT;

    84

typedef IEC_UDINT   UDINT;

    85

typedef IEC_ULINT   ULINT;

    86

    87

typedef IEC_BYTE    BYTE;

    88

typedef IEC_WORD   WORD;

    89

typedef IEC_DWORD   DWORD;

    90

typedef IEC_LWORD   LWORD;

    91

    92

typedef IEC_REAL    REAL;

    93

typedef IEC_LREAL   LREAL;

    94

    95

typedef IEC_TIME TIME;

    96

typedef IEC_DATE DATE;

    97

typedef IEC_DT DT;

    98

typedef IEC_TOD TOD;

    99

   100

#define __TIME_CMP(t1, t2) (t2.tv_sec == t1.tv_sec ? t1.tv_nsec - t2.tv_nsec : t1.tv_sec - t2.tv_sec) 

   101

extern TIME __CURRENT_TIME;

   102

   103

typedef IEC_STRING STRING;

   104

   105

#define __STR_CMP(str1, str2) memcmp((char*)&str1.body,(char*)&str2.body, str1.len < str2.len ? str1.len : str2.len)

   106

   107

/* TODO

   108

typedef struct {

   109

    __strlen_t len;

   110

    u_int16_t body[STR_MAX_LEN];

   111

} WSTRING;

   112

*/

   113

   114

# if __WORDSIZE == 64

   115

#define __32b_sufix

   116

#define __64b_sufix L

   117

#else

   118

#define __32b_sufix L

   119

#define __64b_sufix LL 

   120

#endif 

   121

   122

#define __lit(type,value,sfx) (type)value##sfx

   123

// Keep this macro expention step to let sfx change into L or LL 

   124

#define __literal(type,value,sfx) __lit(type,value,sfx)

   125

   126

#define __BOOL_LITERAL(value) __literal(BOOL,value,)

   127

#define __SINT_LITERAL(value) __literal(SINT,value,)

   128

#define __INT_LITERAL(value) __literal(INT,value,)

   129

#define __DINT_LITERAL(value) __literal(DINT,value,__32b_sufix)

   130

#define __LINT_LITERAL(value) __literal(LINT,value,__64b_sufix)

   131

#define __USINT_LITERAL(value) __literal(USINT,value,)

   132

#define __UINT_LITERAL(value) __literal(UINT,value,)

   133

#define __UDINT_LITERAL(value) __literal(UDINT,value,__32b_sufix)

   134

#define __ULINT_LITERAL(value) __literal(ULINT,value,__64b_sufix)

   135

#define __REAL_LITERAL(value) __literal(REAL,value,__32b_sufix)

   136

#define __LREAL_LITERAL(value) __literal(LREAL,value,__64b_sufix)

   137

#define __TIME_LITERAL(value) __literal(TIME,value,)

   138

#define __DATE_LITERAL(value) __literal(DATE,value,)

   139

#define __TOD_LITERAL(value) __literal(TOD,value,)

   140

#define __DT_LITERAL(value) __literal(DT,value,)

   141

#define __STRING_LITERAL(value) __literal(STRING,value,)

   142

#define __BYTE_LITERAL(value) __literal(BYTE,value,)

   143

#define __WORD_LITERAL(value) __literal(WORD,value,)

   144

#define __DWORD_LITERAL(value) __literal(DWORD,value,__32b_sufix)

   145

#define __LWORD_LITERAL(value) __literal(LWORD,value,__32b_sufix)

   146

   147

   148

   149

#define __INIT_REAL 0

   150

#define __INIT_LREAL 0

   151

#define __INIT_SINT 0

   152

#define __INIT_INT 0

   153

#define __INIT_DINT 0

   154

#define __INIT_LINT 0

   155

#define __INIT_USINT 0

   156

#define __INIT_UINT 0

   157

#define __INIT_UDINT 0

   158

#define __INIT_ULINT 0

   159

#define __INIT_TIME (TIME){0,0}

   160

#define __INIT_BOOL 0

   161

#define __INIT_BYTE 0

   162

#define __INIT_WORD 0

   163

#define __INIT_DWORD 0

   164

#define __INIT_LWORD 0

   165

#define __INIT_STRING (STRING){0,""}

   166

//#define __INIT_WSTRING

   167

#define __INIT_DATE (DATE){0,0}

   168

#define __INIT_TOD (TOD){0,0}

   169

#define __INIT_DT (DT){0,0}

   170

   171

typedef union __IL_DEFVAR_T {

   172

    BOOL    BOOLvar;

   173

   174

    SINT    SINTvar;

   175

    INT     INTvar;

   176

    DINT    DINTvar;

   177

    LINT    LINTvar;

   178

   179

    USINT   USINTvar;

   180

    UINT    UINTvar;

   181

    UDINT   UDINTvar;

   182

    ULINT   ULINTvar;

   183

   184

    BYTE    BYTEvar;

   185

    WORD    WORDvar;

   186

    DWORD   DWORDvar;

   187

    LWORD   LWORDvar;

   188

   189

    REAL    REALvar;

   190

    LREAL   LREALvar;

   191

   192

    TIME    TIMEvar;

   193

    TOD TODvar;

   194

    DT  DTvar;

   195

    DATE    DATEvar;

   196

} __IL_DEFVAR_T;

   197

   198

typedef struct {

   199

  BOOL state;     // current step state. 0 : inative, 1: active

   200

  BOOL prev_state;  // previous step state. 0 : inative, 1: active

   201

  TIME elapsed_time;  // time since step is active

   202

} STEP;

   203

   204

typedef struct {

   205

  BOOL stored;  // action storing state. 0 : not stored, 1: stored

   206

  BOOL state; // current action state. 0 : inative, 1: active

   207

  BOOL set;   // set have been requested (reset each time the body is evaluated)

   208

  BOOL reset; // reset have been requested (reset each time the body is evaluated)

   209

  TIME set_remaining_time;    // time before set will be requested

   210

  TIME reset_remaining_time;  // time before reset will be requested

   211

} ACTION;

   212

   213

/*****************/

   214

/* Misc internal */

   215

/*****************/

   216

   217

/* function that generates an IEC runtime error */

   218

static inline void IEC_error(void) {

   219

  /* TODO... */

   220

  fprintf(stderr, "IEC 61131-3 runtime error.\n");

   221

  /*exit(1);*/

   222

}

   223

   224

   225

static inline void __normalize_timespec (struct timespec *ts) {

   226

  if( ts->tv_nsec < -1000000000 || (( ts->tv_sec > 0 ) && ( ts->tv_nsec < 0 ))){

   227

    ts->tv_sec--;

   228

    ts->tv_nsec += 1000000000;

   229

  }

   230

  if( ts->tv_nsec > 1000000000 || (( ts->tv_sec < 0 ) && ( ts->tv_nsec > 0 ))){

   231

    ts->tv_sec++;

   232

    ts->tv_nsec -= 1000000000;

   233

  }

   234

}

   235

   236

static inline struct timespec __time_to_timespec(int sign, double mseconds, double seconds, double minutes, double hours, double days) {

   237

  struct timespec ts;

   238

   239

  /* sign is 1 for positive values, -1 for negative time... */

   240

  long double total_sec = ((days*24 + hours)*60 + minutes)*60 + seconds + mseconds/1e3;

   241

  if (sign >= 0) sign = 1; else sign = -1;

   242

  ts.tv_sec = sign * (long int)total_sec;

   243

  ts.tv_nsec = sign * (long int)((total_sec - ts.tv_sec)*1e9);

   244

   245

  return ts;

   246

}

   247

   248

   249

static inline struct timespec __tod_to_timespec(double seconds, double minutes, double hours) {

   250

  struct timespec ts;

   251

   252

  long double total_sec = (hours*60 + minutes)*60 + seconds;

   253

  ts.tv_sec = (long int)total_sec;

   254

  ts.tv_nsec = (long int)((total_sec - ts.tv_sec)*1e9);

   255

   256

  return ts;

   257

}

   258

   259

static inline struct timespec __date_to_timespec(int day, int month, int year) {

   260

  struct timespec ts;

   261

  struct tm broken_down_time;

   262

   263

  broken_down_time.tm_sec = 0;

   264

  broken_down_time.tm_min = 0;

   265

  broken_down_time.tm_hour = 0;

   266

  broken_down_time.tm_mday = day;  /* day of month, from 1 to 31 */

   267

  broken_down_time.tm_mon = month - 1;   /* month since January, in the range 0 to 11 */

   268

  broken_down_time.tm_year = year - 1900;  /* number of years since 1900 */

   269

   270

  time_t epoch_seconds = mktime(&broken_down_time); /* determine number of seconds since the epoch, i.e. Jan 1st 1970 */

   271

   272

  if ((time_t)(-1) == epoch_seconds)

   273

    IEC_error();

   274

   275

  ts.tv_sec = epoch_seconds;

   276

  ts.tv_nsec = 0;

   277

   278

  return ts;

   279

}

   280

   281

static inline struct timespec __dt_to_timespec(double seconds,  double minutes, double hours, int day, int month, int year) {

   282

  struct timespec ts;

   283

   284

  long double total_sec = (hours*60 + minutes)*60 + seconds;

   285

  ts.tv_sec = (long int)total_sec;

   286

  ts.tv_nsec = (long int)((total_sec - ts.tv_sec)*1e9);

   287

   288

  struct tm broken_down_time;

   289

  broken_down_time.tm_sec = 0;

   290

  broken_down_time.tm_min = 0;

   291

  broken_down_time.tm_hour = 0;

   292

  broken_down_time.tm_mday = day;  /* day of month, from 1 to 31 */

   293

  broken_down_time.tm_mon = month - 1;   /* month since January, in the range 0 to 11 */

   294

  broken_down_time.tm_year = year - 1900;  /* number of years since 1900 */

   295

   296

  time_t epoch_seconds = mktime(&broken_down_time); /* determine number of seconds since the epoch, i.e. Jan 1st 1970 */

   297

  if ((time_t)(-1) == epoch_seconds)

   298

    IEC_error();

   299

   300

  ts.tv_sec += epoch_seconds;

   301

  if (ts.tv_sec < epoch_seconds)

   302

    /* since the TOD is always positive, if the above happens then we had an overflow */

   303

    IEC_error();

   304

   305

  return ts;

   306

}

   307

   308

/***************/

   309

/*   Time ops  */

   310

/***************/

   311

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   312

  return (TIME){IN.tv_sec % 86400, IN.tv_nsec};

   313

}

   314

static inline TIME __date_and_time_to_date(TIME IN){

   315

  return (TIME){IN.tv_sec - (IN.tv_sec % (24*60*60)), 0};

   316

}

   317

static inline TIME __time_add(TIME IN1, TIME IN2){

   318

  TIME res ={IN1.tv_sec + IN2.tv_sec,

   319

             IN1.tv_nsec + IN2.tv_nsec };

   320

  __normalize_timespec(&res);

   321

  return res;

   322

}

   323

static inline TIME __time_sub(TIME IN1, TIME IN2){

   324

  TIME res ={IN1.tv_sec - IN2.tv_sec,

   325

             IN1.tv_nsec - IN2.tv_nsec };

   326

  __normalize_timespec(&res);

   327

  return res;

   328

}

   329

static inline TIME __time_mul(TIME IN1, LREAL IN2){

   330

  LREAL s_f = IN1.tv_sec * IN2;

   331

  time_t s = s_f;

   332

  div_t ns = div((LREAL)IN1.tv_nsec * IN2, 1000000000);

   333

  TIME res = {s + ns.quot,

   334

              ns.rem + (s_f - s) * 1000000000 };

   335

  __normalize_timespec(&res);

   336

  return res;

   337

}

   338

static inline TIME __time_div(TIME IN1, LREAL IN2){

   339

  LREAL s_f = IN1.tv_sec / IN2;

   340

  time_t s = s_f;

   341

  TIME res = {s,

   342

              IN1.tv_nsec / IN2 + (s_f - s) * 1000000000 };

   343

  __normalize_timespec(&res);

   344

  return res;

   345

}

   346

   347

/***************/

   348

/* String ops  */

   349

/***************/

   350

static inline UINT __len(STRING IN){

   351

    return IN.len;

   352

}

   353

static inline STRING __left(STRING IN, SINT L){

   354

    STRING res = __INIT_STRING;

   355

    L = L < IN.len ? L : IN.len;

   356

    memcpy(&res.body, &IN.body, L);

   357

    res.len = L;

   358

    return res;

   359

}

   360

static inline STRING __right(STRING IN, SINT L){

   361

    STRING res = __INIT_STRING;

   362

    L = L < IN.len ? L : IN.len;

   363

    memcpy(&res.body, &IN.body[IN.len - L], L);

   364

    res.len = L;

   365

    return res;

   366

}

   367

static inline STRING __mid(STRING IN, SINT L, SINT P){

   368

    STRING res = __INIT_STRING;

   369

    if(P <= IN.len){

   370

	    P -= 1; /* now can be used as [index]*/

   371

	    L = L + P <= IN.len ? L : IN.len - P;

   372

	    memcpy(&res.body, &IN.body[P] , L);

   373

        res.len = L;

   374

    }

   375

    return res;

   376

}

   377

static inline STRING __concat(SINT param_count, ...){

   378

  va_list ap;

   379

  UINT i;

   380

  __strlen_t charcount = 0;

   381

  STRING res = __INIT_STRING;

   382

   383

  va_start (ap, param_count);         /* Initialize the argument list.  */

   384

   385

  for (i = 0; i < param_count && charcount < STR_MAX_LEN; i++)

   386

  {

   387

    STRING tmp = va_arg(ap, STRING);

   388

    __strlen_t charrem = STR_MAX_LEN - charcount;

   389

    __strlen_t to_write = tmp.len > charrem ? charrem : tmp.len;

   390

    memcpy(&res.body[charcount], &tmp.body , to_write);

   391

    charcount += to_write;

   392

  }

   393

   394

  res.len = charcount;

   395

   396

  va_end (ap);                  /* Clean up.  */

   397

  return res;

   398

}

   399

static inline STRING __insert(STRING IN1, STRING IN2, SINT P){

   400

    STRING res = __INIT_STRING;

   401

    __strlen_t to_copy;

   402

   403

    to_copy = P > IN1.len ? IN1.len : P - 1;

   404

    memcpy(&res.body, &IN1.body , to_copy);

   405

    P = res.len = to_copy;

   406

   407

    to_copy = IN2.len + res.len > STR_MAX_LEN ? STR_MAX_LEN - res.len : IN2.len;

   408

    memcpy(&res.body[res.len], &IN2.body , to_copy);

   409

    res.len += to_copy;

   410

   411

    to_copy = IN1.len - P < STR_MAX_LEN - res.len ? IN1.len - P : STR_MAX_LEN - res.len ;

   412

    memcpy(&res.body[res.len], &IN1.body[P] , to_copy);

   413

    res.len += to_copy;

   414

   415

    return res;

   416

}

   417

static inline STRING __delete(STRING IN, SINT L, SINT P){

   418

    STRING res = __INIT_STRING;

   419

    __strlen_t to_copy;

   420

   421

    to_copy = P > IN.len ? IN.len : P-1;

   422

    memcpy(&res.body, &IN.body , to_copy);

   423

    P = res.len = to_copy;

   424

   425

    if( IN.len > P + L ){

   426

        to_copy = IN.len - P - L;

   427

        memcpy(&res.body[res.len], &IN.body[P + L], to_copy);

   428

        res.len += to_copy;

   429

    }

   430

   431

    return res;

   432

}

   433

static inline STRING __replace(STRING IN1, STRING IN2, SINT L, SINT P){

   434

    STRING res = __INIT_STRING;

   435

    __strlen_t to_copy;

   436

   437

    to_copy = P > IN1.len ? IN1.len : P-1;

   438

    memcpy(&res.body, &IN1.body , to_copy);

   439

    P = res.len = to_copy;

   440

   441

    to_copy = IN2.len < L ? IN2.len : L;

   442

   443

    if( to_copy + res.len > STR_MAX_LEN ) 

   444

       to_copy = STR_MAX_LEN - res.len;   

   445

   446

    memcpy(&res.body[res.len], &IN2.body , to_copy);

   447

    res.len += to_copy;

   448

   449

    P += L;

   450

    if( res.len <  STR_MAX_LEN && P < IN1.len)

   451

    {

   452

        to_copy = IN1.len - P;

   453

        memcpy(&res.body[res.len], &IN1.body[P] , to_copy);

   454

        res.len += to_copy;

   455

    }

   456

   457

    return res;

   458

}

   459

   460

   461

   462

static inline UINT __pfind(STRING* IN1, STRING* IN2){

   463

    UINT count1 = 0; /* offset of first matching char in IN1 */

   464

    UINT count2 = 0; /* count of matching char */

   465

    while(count1 + count2 < IN1->len && count2 < IN2->len)

   466

    {

   467

        if(IN1->body[count1 + count2] != IN2->body[count2++]){

   468

            count1 += count2;

   469

            count2 = 0;

   470

        }        

   471

    }

   472

    return count2 == IN2->len -1 ? 0 : count1 + 1;

   473

}

   474

static inline UINT __find(STRING IN1, STRING IN2){

   475

    return __pfind(&IN1, &IN2);

   476

}

   477

   478

/***************/

   479

/* Convertions */

   480

/***************/

   481

    /***************/

   482

    /*  TO_STRING  */

   483

    /***************/

   484

static inline STRING __bool_to_string(BOOL IN)

   485

{

   486

    if(IN)

   487

        return (STRING){4, "TRUE"};

   488

    return (STRING){5,"FALSE"};

   489

}

   490

static inline STRING __bit_to_string(LWORD IN){

   491

    STRING res = __INIT_STRING;

   492

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "16#%llx", IN);

   493

    if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN;

   494

    return res;

   495

}

   496

static inline STRING __real_to_string(LREAL IN){

   497

    STRING res = __INIT_STRING;

   498

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "%.10g", IN);

   499

    if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN;

   500

    return res;

   501

}

   502

static inline STRING __sint_to_string(LINT IN){

   503

    STRING res = __INIT_STRING;

   504

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "%lld", IN);

   505

    if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN;

   506

    return res;

   507

}

   508

static inline STRING __uint_to_string(ULINT IN){

   509