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

#ifdef DEBUG_IEC

    35

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

    36

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

    37

#else

    38

#define DBG(...)

    39

#define DBG_TYPE(TYPENAME, name)

    40

#endif

    41

    42

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

    43

/*  IEC Types defs   */

    44

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

    45

    46

/* Include non windows.h clashing typedefs */

    47

#include "iec_types.h"

    48

    49

/* Those typdefs clash with windows.h */

    50

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

    51

typedef IEC_BOOL  BOOL;

    52

    53

#define TRUE 1

    54

#define FALSE 0

    55

    56

typedef IEC_SINT    SINT;

    57

typedef IEC_INT   INT;

    58

typedef IEC_DINT   DINT;

    59

typedef IEC_LINT   LINT;

    60

    61

typedef IEC_USINT    USINT;

    62

typedef IEC_UINT   UINT;

    63

typedef IEC_UDINT   UDINT;

    64

typedef IEC_ULINT   ULINT;

    65

    66

typedef IEC_BYTE    BYTE;

    67

typedef IEC_WORD   WORD;

    68

typedef IEC_DWORD   DWORD;

    69

typedef IEC_LWORD   LWORD;

    70

    71

typedef IEC_REAL    REAL;

    72

typedef IEC_LREAL   LREAL;

    73

    74

typedef IEC_TIME TIME;

    75

typedef IEC_DATE DATE;

    76

typedef IEC_DT DT;

    77

typedef IEC_TOD TOD;

    78

    79

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

    80

extern TIME __CURRENT_TIME;

    81

    82

typedef IEC_STRING STRING;

    83

    84

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

    85

    86

/* TODO

    87

typedef struct {

    88

    __strlen_t len;

    89

    u_int16_t body[STR_MAX_LEN];

    90

} WSTRING;

    91

*/

    92

    93

# if __WORDSIZE == 64

    94

#define __32b_sufix

    95

#define __64b_sufix L

    96

#else

    97

#define __32b_sufix L

    98

#define __64b_sufix LL 

    99

#endif 

   100

   101

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

   102

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

   103

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

   104

   105

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

   106

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

   107

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

   108

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

   109

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

   110

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

   111

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

   112

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

   113

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

   114

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

   115

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

   116

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

   117

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

   118

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

   119

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

   120

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

   121

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

   122

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

   123

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

   124

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

   125

   126

   127

   128

#define __INIT_REAL 0

   129

#define __INIT_LREAL 0

   130

#define __INIT_SINT 0

   131

#define __INIT_INT 0

   132

#define __INIT_DINT 0

   133

#define __INIT_LINT 0

   134

#define __INIT_USINT 0

   135

#define __INIT_UINT 0

   136

#define __INIT_UDINT 0

   137

#define __INIT_ULINT 0

   138

#define __INIT_TIME (TIME){0,0}

   139

#define __INIT_BOOL 0

   140

#define __INIT_BYTE 0

   141

#define __INIT_WORD 0

   142

#define __INIT_DWORD 0

   143

#define __INIT_LWORD 0

   144

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

   145

//#define __INIT_WSTRING

   146

#define __INIT_DATE (DATE){0,0}

   147

#define __INIT_TOD (TOD){0,0}

   148

#define __INIT_DT (DT){0,0}

   149

   150

typedef union __IL_DEFVAR_T {

   151

    BOOL    BOOLvar;

   152

   153

    SINT    SINTvar;

   154

    INT     INTvar;

   155

    DINT    DINTvar;

   156

    LINT    LINTvar;

   157

   158

    USINT   USINTvar;

   159

    UINT    UINTvar;

   160

    UDINT   UDINTvar;

   161

    ULINT   ULINTvar;

   162

   163

    BYTE    BYTEvar;

   164

    WORD    WORDvar;

   165

    DWORD   DWORDvar;

   166

    LWORD   LWORDvar;

   167

   168

    REAL    REALvar;

   169

    LREAL   LREALvar;

   170

   171

    TIME    TIMEvar;

   172

    TOD TODvar;

   173

    DT  DTvar;

   174

    DATE    DATEvar;

   175

} __IL_DEFVAR_T;

   176

   177

typedef struct {

   178

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

   179

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

   180

  TIME elapsed_time;  // time since step is active

   181

} STEP;

   182

   183

typedef struct {

   184

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

   185

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

   186

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

   187

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

   188

  TIME set_remaining_time;    // time before set will be requested

   189

  TIME reset_remaining_time;  // time before reset will be requested

   190

} ACTION;

   191

   192

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

   193

/* Misc internal */

   194

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

   195

   196

/* function that generates an IEC runtime error */

   197

static inline void IEC_error(void) {

   198

  /* TODO... */

   199

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

   200

  /*exit(1);*/

   201

}

   202

   203

   204

static inline void __normalize_timespec (struct timespec *ts) {

   205

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

   206

    ts->tv_sec--;

   207

    ts->tv_nsec += 1000000000;

   208

  }

   209

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

   210

    ts->tv_sec++;

   211

    ts->tv_nsec -= 1000000000;

   212

  }

   213

}

   214

   215

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

   216

  struct timespec ts;

   217

   218

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

   219

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

   220

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

   221

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

   222

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

   223

   224

  return ts;

   225

}

   226

   227

   228

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

   229

  struct timespec ts;

   230

   231

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

   232

  ts.tv_sec = (long int)total_sec;

   233

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

   234

   235

  return ts;

   236

}

   237

   238

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

   239

  struct timespec ts;

   240

  struct tm broken_down_time;

   241

   242

  broken_down_time.tm_sec = 0;

   243

  broken_down_time.tm_min = 0;

   244

  broken_down_time.tm_hour = 0;

   245

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

   246

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

   247

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

   248

   249

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

   250

   251

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

   252

    IEC_error();

   253

   254

  ts.tv_sec = epoch_seconds;

   255

  ts.tv_nsec = 0;

   256

   257

  return ts;

   258

}

   259

   260

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

   261

  struct timespec ts;

   262

   263

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

   264

  ts.tv_sec = (long int)total_sec;

   265

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

   266

   267

  struct tm broken_down_time;

   268

  broken_down_time.tm_sec = 0;

   269

  broken_down_time.tm_min = 0;

   270

  broken_down_time.tm_hour = 0;

   271

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

   272

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

   273

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

   274

   275

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

   276

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

   277

    IEC_error();

   278

   279

  ts.tv_sec += epoch_seconds;

   280

  if (ts.tv_sec < epoch_seconds)

   281

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

   282

    IEC_error();

   283

   284

  return ts;

   285

}

   286

   287

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

   288

/*   Time ops  */

   289

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

   290

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   291

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

   292

}

   293

static inline TIME __date_and_time_to_date(TIME IN){

   294

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

   295

}

   296

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

   297

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

   298

             IN1.tv_nsec + IN2.tv_nsec };

   299

  __normalize_timespec(&res);

   300

  return res;

   301

}

   302

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

   303

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

   304

             IN1.tv_nsec - IN2.tv_nsec };

   305

  __normalize_timespec(&res);

   306

  return res;

   307

}

   308

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

   309

  LREAL s_f = IN1.tv_sec * IN2;

   310

  time_t s = s_f;

   311

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

   312

  TIME res = {s + ns.quot,

   313

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

   314

  __normalize_timespec(&res);

   315

  return res;

   316

}

   317

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

   318

  LREAL s_f = IN1.tv_sec / IN2;

   319

  time_t s = s_f;

   320

  TIME res = {s,

   321

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

   322

  __normalize_timespec(&res);

   323

  return res;

   324

}

   325

   326

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

   327

/* String ops  */

   328

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

   329

static inline UINT __len(STRING IN){

   330

    return IN.len;

   331

}

   332

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

   333

    STRING res = __INIT_STRING;

   334

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

   335

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

   336

    res.len = L;

   337

    return res;

   338

}

   339

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

   340

    STRING res = __INIT_STRING;

   341

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

   342

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

   343

    res.len = L;

   344

    return res;

   345

}

   346

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

   347

    STRING res = __INIT_STRING;

   348

    if(P <= IN.len){

   349

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

   350

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

   351

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

   352

        res.len = L;

   353

    }

   354

    return res;

   355

}

   356

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

   357

  va_list ap;

   358

  UINT i;

   359

  __strlen_t charcount = 0;

   360

  STRING res = __INIT_STRING;

   361

   362

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

   363

   364

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

   365

  {

   366

    STRING tmp = va_arg(ap, STRING);

   367

    __strlen_t charrem = STR_MAX_LEN - charcount;

   368

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

   369

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

   370

    charcount += to_write;

   371

  }

   372

   373

  res.len = charcount;

   374

   375

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

   376

  return res;

   377

}

   378

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

   379

    STRING res = __INIT_STRING;

   380

    __strlen_t to_copy;

   381

   382

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

   383

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

   384

    P = res.len = to_copy;

   385

   386

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

   387

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

   388

    res.len += to_copy;

   389

   390

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

   391

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

   392

    res.len += to_copy;

   393

   394

    return res;

   395

}

   396

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

   397

    STRING res = __INIT_STRING;

   398

    __strlen_t to_copy;

   399

   400

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

   401

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

   402

    P = res.len = to_copy;

   403

   404

    if( IN.len > P + L ){

   405

        to_copy = IN.len - P - L;

   406

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

   407

        res.len += to_copy;

   408

    }

   409

   410

    return res;

   411

}

   412

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

   413

    STRING res = __INIT_STRING;

   414

    __strlen_t to_copy;

   415

   416

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

   417

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

   418

    P = res.len = to_copy;

   419

   420

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

   421

   422

    if( to_copy + res.len > STR_MAX_LEN ) 

   423

       to_copy = STR_MAX_LEN - res.len;   

   424

   425

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

   426

    res.len += to_copy;

   427

   428

    P += L;

   429

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

   430

    {

   431

        to_copy = IN1.len - P;

   432

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

   433

        res.len += to_copy;

   434

    }

   435

   436

    return res;

   437

}

   438

   439

   440

   441

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

   442

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

   443

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

   444

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

   445

    {

   446

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

   447

            count1 += count2;

   448

            count2 = 0;

   449

        }        

   450

    }

   451

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

   452

}

   453

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

   454

    return __pfind(&IN1, &IN2);

   455

}

   456

   457

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

   458

/* Convertions */

   459

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

   460

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

   461

    /*  TO_STRING  */

   462

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

   463

static inline STRING __bool_to_string(BOOL IN)

   464

{

   465

    if(IN)

   466

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

   467

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

   468

}

   469

static inline STRING __bit_to_string(LWORD IN){

   470

    STRING res = __INIT_STRING;

   471

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

   472

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

   473

    return res;

   474

}

   475

static inline STRING __real_to_string(LREAL IN){

   476

    STRING res = __INIT_STRING;

   477

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

   478

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

   479

    return res;

   480

}

   481

static inline STRING __sint_to_string(LINT IN){

   482

    STRING res = __INIT_STRING;

   483

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

   484

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

   485

    return res;

   486

}

   487

static inline STRING __uint_to_string(ULINT IN){

   488

    STRING res = __INIT_STRING;

   489

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

   490

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

   491

    return res;

   492

}

   493

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

   494

    /* FROM_STRING */

   495

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

   496

static inline BOOL __string_to_bool(STRING IN){

   497

    return IN.len == 5 ? !memcmp(&IN.body,"TRUE", IN.len) : 0;

   498

}

   499

   500

static inline LINT __pstring_to_sint(STRING* IN){

   501

    LINT res = 0;

   502

    char tmp[STR_MAX_LEN];

   503

    char tmp2[STR_MAX_LEN];

   504

    __strlen_t l;

   505

    unsigned int shift = 0;

   506

   507

    if(IN->body[0]=='2' && IN->body[1]=='#'){

   508

        /* 2#0101_1010_1011_1111 */

   509

        for(l = IN->len - 1; l >= 2 && shift < 64; l--)