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

    12

#include <string.h>

    13

#include <stdarg.h>

    14

    15

#include "iec_std_lib_generated.h"

    16

    17

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

    18

/*  Types defs   */

    19

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

    20

    21

typedef u_int8_t  BOOL;

    22

    23

#define TRUE 1

    24

#define FALSE 0

    25

    26

typedef int8_t    SINT;

    27

typedef int16_t   INT;

    28

typedef int32_t   DINT;

    29

typedef int64_t   LINT;

    30

    31

typedef u_int8_t    USINT;

    32

typedef u_int16_t   UINT;

    33

typedef u_int32_t   UDINT;

    34

typedef u_int64_t   ULINT;

    35

    36

typedef u_int8_t    BYTE;

    37

typedef u_int16_t   WORD;

    38

typedef u_int32_t   DWORD;

    39

typedef u_int64_t   LWORD;

    40

    41

typedef float    REAL;

    42

typedef double   LREAL;

    43

    44

typedef struct timespec TIME;

    45

typedef struct timespec DATE;

    46

typedef struct timespec DT;

    47

typedef struct timespec TOD;

    48

    49

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

    50

    51

#define STR_MAX_LEN 40

    52

typedef int8_t __strlen_t;

    53

typedef struct {

    54

    __strlen_t len;

    55

    u_int8_t body[STR_MAX_LEN];

    56

} STRING;

    57

    58

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

    59

    60

    61

/* TODO

    62

typedef struct {

    63

    __strlen_t len;

    64

    u_int16_t body[STR_MAX_LEN];

    65

} WSTRING;

    66

*/

    67

    68

typedef union __IL_DEFVAR_T {

    69

    BOOL    BOOLvar;

    70

    71

    SINT    SINTvar;

    72

    INT     INTvar;

    73

    DINT    DINTvar;

    74

    LINT    LINTvar;

    75

    76

    USINT   USINTvar;

    77

    UINT    UINTvar;

    78

    UDINT   UDINTvar;

    79

    ULINT   ULINTvar;

    80

    81

    BYTE    BYTEvar;

    82

    WORD    WORDvar;

    83

    DWORD   DWORDvar;

    84

    LWORD   LWORDvar;

    85

    86

    REAL    REALvar;

    87

    LREAL   LREALvar;

    88

    89

    TIME    TIMEvar;

    90

    TOD TODvar;

    91

    DT  DTvar;

    92

    DATE    DATEvar;

    93

} __IL_DEFVAR_T;

    94

    95

    96

    97

    98

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

    99

/* Misc internal */

   100

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

   101

   102

/* function that generates an IEC runtime error */

   103

void IEC_error(void) {

   104

  /* TODO... */

   105

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

   106

  /*exit(1);*/

   107

}

   108

   109

   110

static inline void __normalize_timespec (struct timespec *ts) {

   111

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

   112

    ts->tv_sec--;

   113

    ts->tv_nsec += 1000000000;

   114

  }

   115

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

   116

    ts->tv_sec++;

   117

    ts->tv_nsec -= 1000000000;

   118

  }

   119

}

   120

   121

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

   122

  struct timespec ts;

   123

   124

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

   125

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

   126

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

   127

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

   128

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

   129

   130

  return ts;

   131

}

   132

   133

   134

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

   135

  struct timespec ts;

   136

   137

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

   138

  ts.tv_sec = (long int)total_sec;

   139

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

   140

   141

  return ts;

   142

}

   143

   144

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

   145

  struct timespec ts;

   146

  struct tm broken_down_time;

   147

   148

  broken_down_time.tm_sec = 0;

   149

  broken_down_time.tm_min = 0;

   150

  broken_down_time.tm_hour = 0;

   151

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

   152

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

   153

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

   154

   155

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

   156

   157

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

   158

    IEC_error();

   159

   160

  ts.tv_sec = epoch_seconds;

   161

  ts.tv_nsec = 0;

   162

   163

  return ts;

   164

}

   165

   166

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

   167

  struct timespec ts;

   168

   169

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

   170

  ts.tv_sec = (long int)total_sec;

   171

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

   172

   173

  struct tm broken_down_time;

   174

  broken_down_time.tm_sec = 0;

   175

  broken_down_time.tm_min = 0;

   176

  broken_down_time.tm_hour = 0;

   177

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

   178

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

   179

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

   180

   181

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

   182

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

   183

    IEC_error();

   184

   185

  ts.tv_sec += epoch_seconds;

   186

  if (ts.tv_sec < epoch_seconds)

   187

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

   188

    IEC_error();

   189

   190

  return ts;

   191

}

   192

   193

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

   194

/*   Time ops  */

   195

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

   196

inline TIME __date_and_time_to_time_of_day(TIME IN){

   197

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

   198

}

   199

inline TIME __date_and_time_to_date(TIME IN){

   200

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

   201

}

   202

inline TIME __time_add(TIME IN1, TIME IN2){

   203

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

   204

             IN1.tv_nsec + IN2.tv_nsec };

   205

  __normalize_timespec(&res);

   206

  return res;

   207

}

   208

inline TIME __time_sub(TIME IN1, TIME IN2){

   209

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

   210

             IN1.tv_nsec - IN2.tv_nsec };

   211

  __normalize_timespec(&res);

   212

  return res;

   213

}

   214

inline TIME __time_mul(TIME IN1, LREAL IN2){

   215

  LREAL s_f = IN1.tv_sec * IN2;

   216

  time_t s = s_f;

   217

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

   218

  TIME res = {s + ns.quot,

   219

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

   220

  __normalize_timespec(&res);

   221

  return res;

   222

}

   223

inline TIME __time_div(TIME IN1, LREAL IN2){

   224

  LREAL s_f = IN1.tv_sec / IN2;

   225

  time_t s = s_f;

   226

  TIME res = {s,

   227

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

   228

  __normalize_timespec(&res);

   229

  return res;

   230

}

   231

   232

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

   233

/* String ops  */

   234

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

   235

inline UINT __len(STRING IN){

   236

    return IN.len;

   237

}

   238

inline STRING __left(STRING IN, SINT L){

   239

    STRING res = {0,};

   240

    memcpy(&res.body, &IN.body, L < res.len ? L : res.len);

   241

    return res;

   242

}

   243

inline STRING __right(STRING IN, SINT L){

   244

    STRING res = {0,};

   245

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

   246

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

   247

    res.len = L;

   248

    return res;

   249

}

   250

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

   251

    STRING res = {0,};

   252

    if(P <= IN.len){

   253

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

   254

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

   255

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

   256

        res.len = L;

   257

    }

   258

    return res;

   259

}

   260

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

   261

  va_list ap;

   262

  UINT i;

   263

  __strlen_t charcount = 0;

   264

  STRING res = {0,};

   265

   266

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

   267

   268

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

   269

  {

   270

    STRING tmp = va_arg(ap, STRING);

   271

    __strlen_t charrem = STR_MAX_LEN - charcount;

   272

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

   273

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

   274

    charcount += to_write;

   275

  }

   276

   277

  res.len = charcount;

   278

   279

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

   280

  return res;

   281

}

   282

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

   283

    STRING res = {0,};

   284

    __strlen_t to_copy;

   285

   286

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

   287

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

   288

    P = res.len = to_copy;

   289

   290

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

   291

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

   292

    res.len += to_copy;

   293

   294

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

   295

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

   296

    res.len += to_copy;

   297

   298

    return res;

   299

}

   300

inline STRING __delete(STRING IN, SINT L_value, SINT P){

   301

    STRING res = {0,};

   302

    __strlen_t to_copy;

   303

   304

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

   305

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

   306

    P = res.len = to_copy;

   307

   308

    to_copy = IN.len - P;

   309

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

   310

    res.len += to_copy;

   311

   312

    return res;

   313

}

   314

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

   315

    STRING res = {0,};

   316

    __strlen_t to_copy;

   317

   318

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

   319

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

   320

    P = res.len = to_copy;

   321

   322

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

   323

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

   324

    res.len += to_copy;

   325

   326

    to_copy = res.len < IN1.len ? IN1.len - res.len : 0;

   327

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

   328

    res.len += to_copy;

   329

   330

    return res;

   331

}

   332

   333

   334

   335

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

   336

    UINT count1 = 0;

   337

    UINT count2 = 0;

   338

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

   339

    {

   340

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

   341

            count1 += count2;

   342

            count2 = 0;

   343

        }        

   344

    }

   345

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

   346

}

   347

inline UINT __find(STRING IN1, STRING IN2){

   348

    return __pfind(&IN1, &IN2);

   349

}

   350

   351

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

   352

/* Convertions */

   353

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

   354

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

   355

    /*  TO_STRING  */

   356

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

   357

inline STRING __bool_to_string(BOOL IN)

   358

{

   359

    if(IN)

   360

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

   361

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

   362

}

   363

inline STRING __bit_to_string(LWORD IN){

   364

    STRING res = {0,};

   365

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

   366

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

   367

    return res;

   368

}

   369

inline STRING __real_to_string(LREAL IN){

   370

    STRING res = {0,};

   371

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

   372

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

   373

    return res;

   374

}

   375

inline STRING __sint_to_string(LINT IN){

   376

    STRING res = {0,};

   377

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

   378

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

   379

    return res;

   380

}

   381

inline STRING __uint_to_string(ULINT IN){

   382

    STRING res = {0,};

   383

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

   384

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

   385

    return res;

   386

}

   387

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

   388

    /* FROM_STRING */

   389

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

   390

inline BOOL __string_to_bool(STRING IN){

   391

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

   392

}

   393

   394

inline LINT __pstring_to_sint(STRING* IN){

   395

    LINT res = 0;

   396

    char tmp[STR_MAX_LEN];

   397

    char tmp2[STR_MAX_LEN];

   398

    __strlen_t l;

   399

    unsigned int shift = 0;

   400

   401

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

   402

        /* 2#0101_1010_1011_1111 */

   403

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

   404

        {

   405

            char c = IN->body[l];

   406

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

   407

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

   408

                shift += 1;

   409

            }

   410

        }

   411

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

   412

        /* 8#1234_5665_4321 */

   413

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

   414

        {

   415

            char c = IN->body[l];

   416

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

   417

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

   418

                shift += 3;

   419

            }

   420

        }

   421

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

   422

        /* 16#1234_5678_9abc_DEFG */

   423

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

   424

        {

   425

            char c = IN->body[l];

   426

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

   427

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

   428

                shift += 4;

   429

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

   430

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

   431

                shift += 4;

   432

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

   433

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

   434

                shift += 4;

   435

            }

   436

        }

   437

    }else{

   438

        /* -123456789 */

   439

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

   440

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

   441

        {

   442

            char c = IN->body[l];

   443

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

   444

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

   445

                fac *= 10;

   446

                shift += 1;

   447

            }

   448

        }

   449

    }

   450

    return res;

   451

}

   452

   453

inline LINT __string_to_sint(STRING IN){

   454

    return (LWORD)__pstring_to_sint(&IN);

   455

}

   456

inline LWORD __string_to_bit(STRING IN){

   457

    return (LWORD)__pstring_to_sint(&IN);

   458

}

   459

inline ULINT __string_to_uint(STRING IN){

   460

    return (ULINT)__pstring_to_sint(&IN);

   461

}

   462

inline LREAL __string_to_real(STRING IN){

   463

    /* search the dot */

   464

    __strlen_t l = IN.len;

   465

    while(--l > 0 && IN.body[l] != '.');

   466

    if(l != 0){

   467

        return atof((const char *)&IN.body);

   468

    }else{

   469

        return (LREAL)__pstring_to_sint(&IN);

   470

    }    

   471

}

   472

   473

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

   474

    /*   TO_TIME   */

   475

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

   476

inline TIME __int_to_time(LINT IN){

   477

    return (TIME){IN, 0};

   478

}

   479

   480

inline TIME __real_to_time(LREAL IN){

   481

    return (TIME){IN, (IN - (LINT)IN) * 1000000000};

   482

}

   483

inline TIME __string_to_time(STRING IN){

   484

    /* TODO :

   485

     *

   486

     *  Duration literals without underlines: T#14ms    T#-14ms   T#14.7s   T#14.7m

   487

     *                short prefix            T#14.7h    t#14.7d   t#25h15m

   488

     *                                        t#5d14h12m18s3.5ms

   489

     *                long prefix             TIME#14ms    TIME#-14ms   time#14.7s

   490

     *  Duration literals with underlines:

   491

     *                short prefix            t#25h_15m t#5d_14h_12m_18s_3.5ms

   492

     *                long prefix             TIME#25h_15m

   493

     *                                        time#5d_14h_12m_18s_3.5ms

   494

     *

   495

     *  Long prefix notation                 Short prefix notation

   496

     *  DATE#1984-06-25                      D#1984-06-25

   497

     *  date#1984-06-25                      d#1984-06-25

   498

     *  TIME_OF_DAY#15:36:55.36              TOD#15:36:55.36

   499

     *  time_of_day#15:36:55.36              tod#15:36:55.36

   500

     *  DATE_AND_TIME#1984-06-25-15:36:55.36 DT#1984-06-25-15:36:55.36

   501

     *  date_and_time#1984-06-25-15:36:55.36 dt#1984-06-25-15:36:55.36

   502

     *

   503

     */

   504

    /* Quick hack : only transform seconds */

   505

    /* search the dot */

   506

    __strlen_t l = IN.len;

   507

    while(--l > 0 && IN.body[l] != '.');

   508

    if(l != 0){

   509

        LREAL IN_val = atof((const char *)&IN.body);

   510

        return  (TIME){IN_val, (IN_val - (LINT)IN_val)*1000000000};

   511

    }else{

   512

        return  (TIME){__pstring_to_sint(&IN), 0};

   513

    }

   514

}

   515

   516

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

   517

    /*  FROM_TIME  */

   518

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

   519

inline LREAL __time_to_real(TIME IN){

   520

    return (LREAL)IN.tv_sec + ((LREAL)IN.tv_nsec/1000000000);

   521

}

   522

inline LINT __time_to_int(TIME IN){

   523

    return IN.tv_sec;

   524

}

   525

inline STRING __time_to_string(TIME IN){

   526

    /*t#5d14h12m18s3.5ms*/

   527

    STRING res = {0,};

   528

    div_t days = div(IN.tv_sec ,86400);

   529

    if(!days.rem && IN.tv_nsec == 0){

   530

        res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd", days.quot);