1

/****

     2

 * IEC 61131-3 standard function lib

     3

 */

     4

     5

#include <limits.h>

     6

#include <float.h>

     7

#include <math.h>

     8

#include <time.h>

     9

#include <stdint.h>

    10

    11

#include <stdio.h>

    12

#include <stdlib.h>

    13

#include <string.h>

    14

#include <stdarg.h>

    15

    16

#include "iec_std_lib_generated.h"

    17

    18

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

    19

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

    20

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

    21

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

    22

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

    23

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

    24

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

    25

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

    26

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

    27

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

    28

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

    29

#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);}

    30

#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);}

    31

#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);}

    32

#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);}

    33

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

    34

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

    35

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

    36

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

    37

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

    38

    39

#ifdef DEBUG_IEC

    40

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

    41

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

    42

#else

    43

#define DBG(...)

    44

#define DBG_TYPE(TYPENAME, name)

    45

#endif

    46

    47

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

    48

/*  IEC Types defs   */

    49

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

    50

    51

/* Include non windows.h clashing typedefs */

    52

#include "iec_types.h"

    53

    54

/* Those typdefs clash with windows.h */

    55

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

    56

typedef IEC_BOOL  BOOL;

    57

    58

#define TRUE 1

    59

#define FALSE 0

    60

    61

typedef IEC_SINT    SINT;

    62

typedef IEC_INT   INT;

    63

typedef IEC_DINT   DINT;

    64

typedef IEC_LINT   LINT;

    65

    66

typedef IEC_USINT    USINT;

    67

typedef IEC_UINT   UINT;

    68

typedef IEC_UDINT   UDINT;

    69

typedef IEC_ULINT   ULINT;

    70

    71

typedef IEC_BYTE    BYTE;

    72

typedef IEC_WORD   WORD;

    73

typedef IEC_DWORD   DWORD;

    74

typedef IEC_LWORD   LWORD;

    75

    76

typedef IEC_REAL    REAL;

    77

typedef IEC_LREAL   LREAL;

    78

    79

typedef IEC_TIME TIME;

    80

typedef IEC_DATE DATE;

    81

typedef IEC_DT DT;

    82

typedef IEC_TOD TOD;

    83

    84

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

    85

extern TIME __CURRENT_TIME;

    86

    87

typedef IEC_STRING STRING;

    88

    89

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

    90

    91

/* TODO

    92

typedef struct {

    93

    __strlen_t len;

    94

    u_int16_t body[STR_MAX_LEN];

    95

} WSTRING;

    96

*/

    97

    98

# if __WORDSIZE == 64

    99

#define __32b_sufix

   100

#define __64b_sufix L

   101

#else

   102

#define __32b_sufix L

   103

#define __64b_sufix LL 

   104

#endif 

   105

   106

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

   107

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

   108

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

   109

   110

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

   111

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

   112

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

   113

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

   114

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

   115

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

   116

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

   117

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

   118

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

   119

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

   120

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

   121

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

   122

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

   123

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

   124

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

   125

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

   126

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

   127

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

   128

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

   129

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

   130

   131

   132

   133

#define __INIT_REAL 0

   134

#define __INIT_LREAL 0

   135

#define __INIT_SINT 0

   136

#define __INIT_INT 0

   137

#define __INIT_DINT 0

   138

#define __INIT_LINT 0

   139

#define __INIT_USINT 0

   140

#define __INIT_UINT 0

   141

#define __INIT_UDINT 0

   142

#define __INIT_ULINT 0

   143

#define __INIT_TIME (TIME){0,0}

   144

#define __INIT_BOOL 0

   145

#define __INIT_BYTE 0

   146

#define __INIT_WORD 0

   147

#define __INIT_DWORD 0

   148

#define __INIT_LWORD 0

   149

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

   150

//#define __INIT_WSTRING

   151

#define __INIT_DATE (DATE){0,0}

   152

#define __INIT_TOD (TOD){0,0}

   153

#define __INIT_DT (DT){0,0}

   154

   155

typedef union __IL_DEFVAR_T {

   156

    BOOL    BOOLvar;

   157

   158

    SINT    SINTvar;

   159

    INT     INTvar;

   160

    DINT    DINTvar;

   161

    LINT    LINTvar;

   162

   163

    USINT   USINTvar;

   164

    UINT    UINTvar;

   165

    UDINT   UDINTvar;

   166

    ULINT   ULINTvar;

   167

   168

    BYTE    BYTEvar;

   169

    WORD    WORDvar;

   170

    DWORD   DWORDvar;

   171

    LWORD   LWORDvar;

   172

   173

    REAL    REALvar;

   174

    LREAL   LREALvar;

   175

   176

    TIME    TIMEvar;

   177

    TOD TODvar;

   178

    DT  DTvar;

   179

    DATE    DATEvar;

   180

} __IL_DEFVAR_T;

   181

   196

   197

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

   198

/* Misc internal */

   199

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

   200

   201

/* function that generates an IEC runtime error */

   202

static inline void IEC_error(void) {

   203

  /* TODO... */

   204

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

   205

  /*exit(1);*/

   206

}

   207

   208

   209

static inline void __normalize_timespec (struct timespec *ts) {

   210

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

   211

    ts->tv_sec--;

   212

    ts->tv_nsec += 1000000000;

   213

  }

   214

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

   215

    ts->tv_sec++;

   216

    ts->tv_nsec -= 1000000000;

   217

  }

   218

}

   219

   220

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

   221

  struct timespec ts;

   222

   223

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

   224

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

   225

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

   226

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

   227

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

   228

   229

  return ts;

   230

}

   231

   232

   233

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

   234

  struct timespec ts;

   235

   236

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

   237

  ts.tv_sec = (long int)total_sec;

   238

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

   239

   240

  return ts;

   241

}

   242

   243

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

   244

  struct timespec ts;

   245

  struct tm broken_down_time;

   246

   247

  broken_down_time.tm_sec = 0;

   248

  broken_down_time.tm_min = 0;

   249

  broken_down_time.tm_hour = 0;

   250

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

   251

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

   252

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

   253

   254

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

   255

   256

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

   257

    IEC_error();

   258

   259

  ts.tv_sec = epoch_seconds;

   260

  ts.tv_nsec = 0;

   261

   262

  return ts;

   263

}

   264

   265

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

   266

  struct timespec ts;

   267

   268

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

   269

  ts.tv_sec = (long int)total_sec;

   270

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

   271

   272

  struct tm broken_down_time;

   273

  broken_down_time.tm_sec = 0;

   274

  broken_down_time.tm_min = 0;

   275

  broken_down_time.tm_hour = 0;

   276

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

   277

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

   278

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

   279

   280

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

   281

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

   282

    IEC_error();

   283

   284

  ts.tv_sec += epoch_seconds;

   285

  if (ts.tv_sec < epoch_seconds)

   286

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

   287

    IEC_error();

   288

   289

  return ts;

   290

}

   291

   292

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

   293

/*   Time ops  */

   294

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

   295

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   296

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

   297

}

   298

static inline TIME __date_and_time_to_date(TIME IN){

   299

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

   300

}

   301

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

   302

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

   303

             IN1.tv_nsec + IN2.tv_nsec };

   304

  __normalize_timespec(&res);

   305

  return res;

   306

}

   307

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

   308

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

   309

             IN1.tv_nsec - IN2.tv_nsec };

   310

  __normalize_timespec(&res);

   311

  return res;

   312

}

   313

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

   314

  LREAL s_f = IN1.tv_sec * IN2;

   315

  time_t s = s_f;

   316

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

   317

  TIME res = {s + ns.quot,

   318

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

   319

  __normalize_timespec(&res);

   320

  return res;

   321

}

   322

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

   323

  LREAL s_f = IN1.tv_sec / IN2;

   324

  time_t s = s_f;

   325

  TIME res = {s,

   326

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

   327

  __normalize_timespec(&res);

   328

  return res;

   329

}

   330

   331

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

   332

/* String ops  */

   333

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

   334

static inline UINT __len(STRING IN){

   335

    return IN.len;

   336

}

   337

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

   338

    STRING res = __INIT_STRING;

   339

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

   340

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

   341

    res.len = L;

   342

    return res;

   343

}

   344

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

   345

    STRING res = __INIT_STRING;

   346

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

   347

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

   348

    res.len = L;

   349

    return res;

   350

}

   351

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

   352

    STRING res = __INIT_STRING;

   353

    if(P <= IN.len){

   354

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

   355

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

   356

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

   357

        res.len = L;

   358

    }

   359

    return res;

   360

}

   361

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

   362

  va_list ap;

   363

  UINT i;

   364

  __strlen_t charcount = 0;

   365

  STRING res = __INIT_STRING;

   366

   367

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

   368

   369

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

   370

  {

   371

    STRING tmp = va_arg(ap, STRING);

   372

    __strlen_t charrem = STR_MAX_LEN - charcount;

   373

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

   374

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

   375

    charcount += to_write;

   376

  }

   377

   378

  res.len = charcount;

   379

   380

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

   381

  return res;

   382

}

   383

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

   384

    STRING res = __INIT_STRING;

   385

    __strlen_t to_copy;

   386

   387

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

   388

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

   389

    P = res.len = to_copy;

   390

   391

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

   392

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

   393

    res.len += to_copy;

   394

   395

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

   396

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

   397

    res.len += to_copy;

   398

   399

    return res;

   400

}

   401

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

   402

    STRING res = __INIT_STRING;

   403

    __strlen_t to_copy;

   404

   405

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

   406

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

   407

    P = res.len = to_copy;

   408

   409

    if( IN.len > P + L ){

   410

        to_copy = IN.len - P - L;

   411

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

   412

        res.len += to_copy;

   413

    }

   414

   415

    return res;

   416

}

   417

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

   418

    STRING res = __INIT_STRING;

   419

    __strlen_t to_copy;

   420

   421

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

   422

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

   423

    P = res.len = to_copy;

   424

   425

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

   426

   427

    if( to_copy + res.len > STR_MAX_LEN ) 

   428

       to_copy = STR_MAX_LEN - res.len;   

   429

   430

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

   431

    res.len += to_copy;

   432

   433

    P += L;

   434

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

   435

    {

   436

        to_copy = IN1.len - P;

   437

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

   438

        res.len += to_copy;

   439

    }

   440

   441

    return res;

   442

}

   443

   444

   445

   446

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

   447

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

   448

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

   449

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

   450

    {

   451

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

   452

            count1 += count2;

   453

            count2 = 0;

   454

        }        

   455

    }

   456

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

   457

}

   458

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

   459

    return __pfind(&IN1, &IN2);

   460

}

   461

   462

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

   463

/* Convertions */

   464

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

   465

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

   466

    /*  TO_STRING  */

   467

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

   468

static inline STRING __bool_to_string(BOOL IN)

   469

{

   470

    if(IN)

   471

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

   472

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

   473

}

   474

static inline STRING __bit_to_string(LWORD IN){

   475

    STRING res = __INIT_STRING;

   476

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

   477

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

   478

    return res;

   479

}

   480

static inline STRING __real_to_string(LREAL IN){

   481

    STRING res = __INIT_STRING;

   482

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

   483

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

   484

    return res;

   485

}

   486

static inline STRING __sint_to_string(LINT IN){

   487

    STRING res = __INIT_STRING;

   488

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

   489

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

   490

    return res;

   491

}

   492

static inline STRING __uint_to_string(ULINT IN){

   493

    STRING res = __INIT_STRING;

   494

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

   495

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

   496

    return res;

   497

}

   498

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

   499

    /* FROM_STRING */

   500

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

   501

static inline BOOL __string_to_bool(STRING IN){

   502

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

   503

}

   504

   505

static inline LINT __pstring_to_sint(STRING* IN){

   506

    LINT res = 0;

   507

    char tmp[STR_MAX_LEN];

   508

    char tmp2[STR_MAX_LEN];

   509

    __strlen_t l;

   510

    unsigned int shift = 0;