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 <sys/types.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

/*  Types defs   */

    49

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

    50

    51

typedef u_int8_t  BOOL;

    52

    53

#define TRUE 1

    54

#define FALSE 0

    55

    56

typedef int8_t    SINT;

    57

typedef int16_t   INT;

    58

typedef int32_t   DINT;

    59

typedef int64_t   LINT;

    60

    61

typedef u_int8_t    USINT;

    62

typedef u_int16_t   UINT;

    63

typedef u_int32_t   UDINT;

    64

typedef u_int64_t   ULINT;

    65

    66

typedef u_int8_t    BYTE;

    67

typedef u_int16_t   WORD;

    68

typedef u_int32_t   DWORD;

    69

typedef u_int64_t   LWORD;

    70

    71

typedef float    REAL;

    72

typedef double   LREAL;

    73

    74

typedef struct timespec TIME;

    75

typedef struct timespec DATE;

    76

typedef struct timespec DT;

    77

typedef struct timespec TOD;

    78

    79

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

    80

extern TIME __CURRENT_TIME;

    81

    82

#define STR_MAX_LEN 40

    83

typedef int8_t __strlen_t;

    84

typedef struct {

    85

    __strlen_t len;

    86

    u_int8_t body[STR_MAX_LEN];

    87

} 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

#define __INIT_REAL 0

    99

#define __INIT_LREAL 0

   100

#define __INIT_SINT 0

   101

#define __INIT_INT 0

   102

#define __INIT_DINT 0

   103

#define __INIT_LINT 0

   104

#define __INIT_USINT 0

   105

#define __INIT_UINT 0

   106

#define __INIT_UDINT 0

   107

#define __INIT_ULINT 0

   108

#define __INIT_TIME (TIME){0,0}

   109

#define __INIT_BOOL 0

   110

#define __INIT_BYTE 0

   111

#define __INIT_WORD 0

   112

#define __INIT_DWORD 0

   113

#define __INIT_LWORD 0

   114

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

   115

//#define __INIT_WSTRING

   116

#define __INIT_DATE (DATE){0,0}

   117

#define __INIT_TOD (TOD){0,0}

   118

#define __INIT_DT (DT){0,0}

   119

   120

typedef union __IL_DEFVAR_T {

   121

    BOOL    BOOLvar;

   122

   123

    SINT    SINTvar;

   124

    INT     INTvar;

   125

    DINT    DINTvar;

   126

    LINT    LINTvar;

   127

   128

    USINT   USINTvar;

   129

    UINT    UINTvar;

   130

    UDINT   UDINTvar;

   131

    ULINT   ULINTvar;

   132

   133

    BYTE    BYTEvar;

   134

    WORD    WORDvar;

   135

    DWORD   DWORDvar;

   136

    LWORD   LWORDvar;

   137

   138

    REAL    REALvar;

   139

    LREAL   LREALvar;

   140

   141

    TIME    TIMEvar;

   142

    TOD TODvar;

   143

    DT  DTvar;

   144

    DATE    DATEvar;

   145

} __IL_DEFVAR_T;

   146

   147

   148

   149

   150

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

   151

/* Misc internal */

   152

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

   153

   154

/* function that generates an IEC runtime error */

   155

static inline void IEC_error(void) {

   156

  /* TODO... */

   157

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

   158

  /*exit(1);*/

   159

}

   160

   161

   162

static inline void __normalize_timespec (struct timespec *ts) {

   163

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

   164

    ts->tv_sec--;

   165

    ts->tv_nsec += 1000000000;

   166

  }

   167

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

   168

    ts->tv_sec++;

   169

    ts->tv_nsec -= 1000000000;

   170

  }

   171

}

   172

   173

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

   174

  struct timespec ts;

   175

   176

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

   177

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

   178

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

   179

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

   180

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

   181

   182

  return ts;

   183

}

   184

   185

   186

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

   187

  struct timespec ts;

   188

   189

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

   190

  ts.tv_sec = (long int)total_sec;

   191

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

   192

   193

  return ts;

   194

}

   195

   196

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

   197

  struct timespec ts;

   198

  struct tm broken_down_time;

   199

   200

  broken_down_time.tm_sec = 0;

   201

  broken_down_time.tm_min = 0;

   202

  broken_down_time.tm_hour = 0;

   203

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

   204

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

   205

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

   206

   207

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

   208

   209

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

   210

    IEC_error();

   211

   212

  ts.tv_sec = epoch_seconds;

   213

  ts.tv_nsec = 0;

   214

   215

  return ts;

   216

}

   217

   218

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

   219

  struct timespec ts;

   220

   221

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

   222

  ts.tv_sec = (long int)total_sec;

   223

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

   224

   225

  struct tm broken_down_time;

   226

  broken_down_time.tm_sec = 0;

   227

  broken_down_time.tm_min = 0;

   228

  broken_down_time.tm_hour = 0;

   229

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

   230

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

   231

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

   232

   233

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

   234

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

   235

    IEC_error();

   236

   237

  ts.tv_sec += epoch_seconds;

   238

  if (ts.tv_sec < epoch_seconds)

   239

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

   240

    IEC_error();

   241

   242

  return ts;

   243

}

   244

   245

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

   246

/*   Time ops  */

   247

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

   248

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   249

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

   250

}

   251

static inline TIME __date_and_time_to_date(TIME IN){

   252

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

   253

}

   254

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

   255

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

   256

             IN1.tv_nsec + IN2.tv_nsec };

   257

  __normalize_timespec(&res);

   258

  return res;

   259

}

   260

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

   261

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

   262

             IN1.tv_nsec - IN2.tv_nsec };

   263

  __normalize_timespec(&res);

   264

  return res;

   265

}

   266

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

   267

  LREAL s_f = IN1.tv_sec * IN2;

   268

  time_t s = s_f;

   269

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

   270

  TIME res = {s + ns.quot,

   271

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

   272

  __normalize_timespec(&res);

   273

  return res;

   274

}

   275

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

   276

  LREAL s_f = IN1.tv_sec / IN2;

   277

  time_t s = s_f;

   278

  TIME res = {s,

   279

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

   280

  __normalize_timespec(&res);

   281

  return res;

   282

}

   283

   284

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

   285

/* String ops  */

   286

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

   287

static inline UINT __len(STRING IN){

   288

    return IN.len;

   289

}

   290

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

   291

    STRING res = __INIT_STRING;

   292

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

   293

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

   294

    res.len = L;

   295

    return res;

   296

}

   297

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

   298

    STRING res = __INIT_STRING;

   299

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

   300

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

   301

    res.len = L;

   302

    return res;

   303

}

   304

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

   305

    STRING res = __INIT_STRING;

   306

    if(P <= IN.len){

   307

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

   308

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

   309

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

   310

        res.len = L;

   311

    }

   312

    return res;

   313

}

   314

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

   315

  va_list ap;

   316

  UINT i;

   317

  __strlen_t charcount = 0;

   318

  STRING res = __INIT_STRING;

   319

   320

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

   321

   322

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

   323

  {

   324

    STRING tmp = va_arg(ap, STRING);

   325

    __strlen_t charrem = STR_MAX_LEN - charcount;

   326

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

   327

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

   328

    charcount += to_write;

   329

  }

   330

   331

  res.len = charcount;

   332

   333

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

   334

  return res;

   335

}

   336

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

   337

    STRING res = __INIT_STRING;

   338

    __strlen_t to_copy;

   339

   340

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

   341

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

   342

    P = res.len = to_copy;

   343

   344

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

   345

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

   346

    res.len += to_copy;

   347

   348

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

   349

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

   350

    res.len += to_copy;

   351

   352

    return res;

   353

}

   354

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

   355

    STRING res = __INIT_STRING;

   356

    __strlen_t to_copy;

   357

   358

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

   359

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

   360

    P = res.len = to_copy;

   361

   362

    if( IN.len > P + L ){

   363

        to_copy = IN.len - P - L;

   364

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

   365

        res.len += to_copy;

   366

    }

   367

   368

    return res;

   369

}

   370

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

   371

    STRING res = __INIT_STRING;

   372

    __strlen_t to_copy;

   373

   374

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

   375

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

   376

    P = res.len = to_copy;

   377

   378

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

   379

   380

    if( to_copy + res.len > STR_MAX_LEN ) 

   381

       to_copy = STR_MAX_LEN - res.len;   

   382

   383

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

   384

    res.len += to_copy;

   385

   386

    P += L;

   387

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

   388

    {

   389

        to_copy = IN1.len - P;

   390

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

   391

        res.len += to_copy;

   392

    }

   393

   394

    return res;

   395

}

   396

   397

   398

   399

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

   400

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

   401

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

   402

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

   403

    {

   404

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

   405

            count1 += count2;

   406

            count2 = 0;

   407

        }        

   408

    }

   409

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

   410

}

   411

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

   412

    return __pfind(&IN1, &IN2);

   413

}

   414

   415

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

   416

/* Convertions */

   417

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

   418

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

   419

    /*  TO_STRING  */

   420

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

   421

static inline STRING __bool_to_string(BOOL IN)

   422

{

   423

    if(IN)

   424

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

   425

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

   426

}

   427

static inline STRING __bit_to_string(LWORD IN){

   428

    STRING res = __INIT_STRING;

   429

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

   430

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

   431

    return res;

   432

}

   433

static inline STRING __real_to_string(LREAL IN){

   434

    STRING res = __INIT_STRING;

   435

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

   436

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

   437

    return res;

   438

}

   439

static inline STRING __sint_to_string(LINT IN){

   440

    STRING res = __INIT_STRING;

   441

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

   442

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

   443

    return res;

   444

}

   445

static inline STRING __uint_to_string(ULINT IN){

   446

    STRING res = __INIT_STRING;

   447

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

   448

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

   449

    return res;

   450

}

   451

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

   452

    /* FROM_STRING */

   453

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

   454

static inline BOOL __string_to_bool(STRING IN){

   455

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

   456

}

   457

   458

static inline LINT __pstring_to_sint(STRING* IN){

   459

    LINT res = 0;

   460

    char tmp[STR_MAX_LEN];

   461

    char tmp2[STR_MAX_LEN];

   462

    __strlen_t l;

   463

    unsigned int shift = 0;

   464

   465

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

   466

        /* 2#0101_1010_1011_1111 */

   467

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

   468

        {

   469

            char c = IN->body[l];

   470

            if( c >= '0' && c <= '1'){

   471

                res |= ( c - '0') << shift;

   472

                shift += 1;

   473

            }

   474

        }

   475

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

   476

        /* 8#1234_5665_4321 */

   477

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

   478

        {

   479

            char c = IN->body[l];

   480

            if( c >= '0' && c <= '7'){

   481

                res |= ( c - '0') << shift;

   482

                shift += 3;

   483

            }

   484

        }

   485

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

   486

        /* 16#1234_5678_9abc_DEFG */

   487

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

   488

        {

   489

            char c = IN->body[l];

   490

            if( c >= '0' && c <= '9'){

   491

                res |= (LWORD)( c - '0') << shift;

   492

                shift += 4;

   493

            }else if( c >= 'a' && c <= 'f'){

   494

                res |= (LWORD)( c - 'a' + 10 ) << shift;

   495

                shift += 4;

   496

            }else if( c >= 'A' && c <= 'F'){

   497

                res |= (LWORD)( c - 'A' + 10 ) << shift;

   498

                shift += 4;

   499

            }

   500

        }

   501

    }else{

   502

        /* -123456789 */

   503

        LINT fac = IN->body[0] == '-' ? -1 : 1;

   504

        for(l = IN->len - 1; l >= 0 && shift < 20; l--)

   505

        {

   506

            char c = IN->body[l];

   507

            if( c >= '0' && c <= '9'){

   508

                res += ( c - '0') * fac;

   509

                fac *= 10;

   510

                shift += 1;

   511

            }else if( c >= '.' ){ /* reset value */

   512

                res = 0;

   513

                fac = 1;

   514

                shift = 0;

   515

            }