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

    16

/* Macro that expand to subtypes */

    17

#define ANY(DO) ANY_DERIVED(DO) ANY_ELEMENTARY(DO)

    18

#define ANY_DERIVED(DO)

    19

#define ANY_ELEMENTARY(DO) ANY_MAGNITUDE(DO) ANY_BIT(DO) ANY_STRING(DO) ANY_DATE(DO)

    20

#define ANY_MAGNITUDE(DO) ANY_NUM(DO) DO(TIME)

    21

#define ANY_BIT(DO) ANY_NBIT(DO) DO(BOOL)

    22

#define ANY_NBIT(DO) DO(BYTE) DO(WORD) DO(DWORD) DO(LWORD)

    23

#define ANY_STRING(DO) DO(STRING) /*DO(WSTRING) TODO */

    24

#define ANY_DATE(DO) DO(DATE) DO(TOD) DO(DT)

    25

#define ANY_NUM(DO) ANY_REAL(DO) ANY_INT(DO)

    26

#define ANY_REAL(DO) DO(REAL) DO(LREAL)

    27

#define ANY_INT(DO) ANY_SINT(DO) ANY_UINT(DO)

    28

#define ANY_SINT(DO) DO(SINT) DO(INT) DO(DINT) DO(LINT)

    29

#define ANY_UINT(DO) DO(USINT) DO(UINT) DO(UDINT) DO(ULINT)

    30

    31

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

    32

/*  Types defs   */

    33

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

    34

    35

typedef u_int8_t  BOOL;

    36

    37

#define TRUE 1

    38

#define FALSE 0

    39

    40

typedef int8_t    SINT;

    41

typedef int16_t   INT;

    42

typedef int32_t   DINT;

    43

typedef int64_t   LINT;

    44

    45

typedef u_int8_t    USINT;

    46

typedef u_int16_t   UINT;

    47

typedef u_int32_t   UDINT;

    48

typedef u_int64_t   ULINT;

    49

    50

typedef u_int8_t    BYTE;

    51

typedef u_int16_t   WORD;

    52

typedef u_int32_t   DWORD;

    53

typedef u_int64_t   LWORD;

    54

    55

typedef float    REAL;

    56

typedef double   LREAL;

    57

    58

typedef struct timespec TIME;

    59

typedef struct timespec DATE;

    60

typedef struct timespec DT;

    61

typedef struct timespec TOD;

    62

    65

#define STR_MAX_LEN 40

    66

typedef int8_t __strlen_t;

    67

typedef struct {

    68

    __strlen_t len;

    69

    u_int8_t body[STR_MAX_LEN];

    70

} STRING;

    71

    75

/* TODO

    76

typedef struct {

    77

    __strlen_t len;

    78

    u_int16_t body[STR_MAX_LEN];

    79

} WSTRING;

    80

*/

    81

    82

typedef union __IL_DEFVAR_T {

    83

    BOOL    BOOLvar;

    84

    85

    SINT    SINTvar;

    86

    INT     INTvar;

    87

    DINT    DINTvar;

    88

    LINT    LINTvar;

    89

    90

    USINT   USINTvar;

    91

    UINT    UINTvar;

    92

    UDINT   UDINTvar;

    93

    ULINT   ULINTvar;

    94

    95

    BYTE    BYTEvar;

    96

    WORD    WORDvar;

    97

    DWORD   DWORDvar;

    98

    LWORD   LWORDvar;

    99

   100

    REAL    REALvar;

   101

    LREAL   LREALvar;

   102

   103

    TIME    TIMEvar;

   104

    TOD TODvar;

   105

    DT  DTvar;

   106

    DATE    DATEvar;

   107

} __IL_DEFVAR_T;

   108

   109

   110

   111

   112

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

   113

/* Misc internal */

   114

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

   115

   116

/* function that generates an IEC runtime error */

   117

void IEC_error(void) {

   118

  /* TODO... */

   119

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

   120

  /*exit(1);*/

   121

}

   122

   123

   124

static inline void __normalize_timespec (struct timespec *ts) {

   125

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

   126

    ts->tv_sec--;

   127

    ts->tv_nsec += 1000000000;

   128

  }

   129

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

   130

    ts->tv_sec++;

   131

    ts->tv_nsec -= 1000000000;

   132

  }

   133

}

   134

   135

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

   136

  struct timespec ts;

   137

   138

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

   139

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

   140

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

   141

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

   142

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

   143

   144

  return ts;

   145

}

   146

   147

   148

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

   149

  struct timespec ts;

   150

   151

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

   152

  ts.tv_sec = (long int)total_sec;

   153

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

   154

   155

  return ts;

   156

}

   157

   158

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

   159

  struct timespec ts;

   160

  struct tm broken_down_time;

   161

   162

  broken_down_time.tm_sec = 0;

   163

  broken_down_time.tm_min = 0;

   164

  broken_down_time.tm_hour = 0;

   165

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

   166

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

   167

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

   168

   169

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

   170

   171

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

   172

    IEC_error();

   173

   174

  ts.tv_sec = epoch_seconds;

   175

  ts.tv_nsec = 0;

   176

   177

  return ts;

   178

}

   179

   180

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

   181

  struct timespec ts;

   182

   183

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

   184

  ts.tv_sec = (long int)total_sec;

   185

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

   186

   187

  struct tm broken_down_time;

   188

  broken_down_time.tm_sec = 0;

   189

  broken_down_time.tm_min = 0;

   190

  broken_down_time.tm_hour = 0;

   191

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

   192

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

   193

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

   194

   195

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

   196

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

   197

    IEC_error();

   198

   199

  ts.tv_sec += epoch_seconds;

   200

  if (ts.tv_sec < epoch_seconds)

   201

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

   202

    IEC_error();

   203

   204

  return ts;

   205

}

   206

   207

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

   208

/*   Time ops  */

   209

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

   210

inline TIME __date_and_time_to_time_of_day(TIME IN){

   211

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

   212

}

   213

inline TIME __date_and_time_to_date(TIME IN){

   214

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

   215

}

   216

inline TIME __time_add(TIME IN1, TIME IN2){

   217

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

   218

             IN1.tv_nsec + IN2.tv_nsec };

   219

  __normalize_timespec(&res);

   220

  return res;

   221

}

   222

inline TIME __time_sub(TIME IN1, TIME IN2){

   223

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

   224

             IN1.tv_nsec - IN2.tv_nsec };

   225

  __normalize_timespec(&res);

   226

  return res;

   227

}

   228

inline TIME __time_mul(TIME IN1, LREAL IN2){

   229

  LREAL s_f = IN1.tv_sec * IN2;

   230

  time_t s = s_f;

   231

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

   232

  TIME res = {s + ns.quot,

   233

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

   234

  __normalize_timespec(&res);

   235

  return res;

   236

}

   237

inline TIME __time_div(TIME IN1, LREAL IN2){

   238

  LREAL s_f = IN1.tv_sec / IN2;

   239

  time_t s = s_f;

   240

  TIME res = {s,

   241

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

   242

  __normalize_timespec(&res);

   243

  return res;

   244

}

   245

   246

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

   247

/* String ops  */

   248

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

   249

inline UINT __len(STRING IN){

   250

    return IN.len;

   251

}

   252

inline STRING __left(STRING IN, SINT L){

   253

    STRING res = {0,};

   254

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

   255

    return res;

   256

}

   257

inline STRING __right(STRING IN, SINT L){

   258

    STRING res = {0,};

   259

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

   260

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

   261

    res.len = L;

   262

    return res;

   263

}

   264

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

   265

    STRING res = {0,};

   266

    if(P <= IN.len){

   267

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

   268

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

   269

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

   270

        res.len = L;

   271

    }

   272

    return res;

   273

}

   274

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

   275

  va_list ap;

   276

  UINT i;

   277

  __strlen_t charcount = 0;

   278

  STRING res = {0,};

   279

   280

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

   281

   282

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

   283

  {

   284

    STRING tmp = va_arg(ap, STRING);

   285

    __strlen_t charrem = STR_MAX_LEN - charcount;

   286

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

   287

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

   288

    charcount += to_write;

   289

  }

   290

   291

  res.len = charcount;

   292

   293

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

   294

  return res;

   295

}

   296

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

   297

    STRING res = {0,};

   298

    __strlen_t to_copy;

   299

   300

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

   301

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

   302

    P = res.len = to_copy;

   303

   304

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

   305

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

   306

    res.len += to_copy;

   307

   308

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

   309

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

   310

    res.len += to_copy;

   311

   312

    return res;

   313

}

   314

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

   315

    STRING res = {0,};

   316

    __strlen_t to_copy;

   317

   318

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

   319

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

   320

    P = res.len = to_copy;

   321

   322

    to_copy = IN.len - P;

   323

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

   324

    res.len += to_copy;

   325

   326

    return res;

   327

}

   328

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

   329

    STRING res = {0,};

   330

    __strlen_t to_copy;

   331

   332

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

   333

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

   334

    P = res.len = to_copy;

   335

   336

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

   337

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

   338

    res.len += to_copy;

   339

   340

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

   341

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

   342

    res.len += to_copy;

   343

   344

    return res;

   345

}

   346

   347

   348

   349

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

   350

    UINT count1 = 0;

   351

    UINT count2 = 0;

   352

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

   353

    {

   354

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

   355

            count1 += count2;

   356

            count2 = 0;

   357

        }        

   358

    }

   359

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

   360

}

   361

inline UINT __find(STRING IN1, STRING IN2){

   362

    return __pfind(&IN1, &IN2);

   363

}

   364

   365

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

   366

/* Convertions */

   367

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

   368

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

   369

    /*  TO_STRING  */

   370

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

   371

inline STRING __bool_to_string(BOOL IN)

   372

{

   373

    if(IN)

   374

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

   375

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

   376

}

   377

inline STRING __bit_to_string(LWORD IN){

   378

    STRING res = {0,};

   379

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

   380

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

   381

    return res;

   382

}

   383

inline STRING __real_to_string(LREAL IN){

   384

    STRING res = {0,};

   385

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

   386

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

   387

    return res;

   388

}

   389

inline STRING __sint_to_string(LINT IN){

   390

    STRING res = {0,};

   391

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

   392

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

   393

    return res;

   394

}

   395

inline STRING __uint_to_string(ULINT IN){

   396

    STRING res = {0,};

   397

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

   398

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

   399

    return res;

   400

}

   401

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

   402

    /* FROM_STRING */

   403

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

   404

inline BOOL __string_to_bool(STRING IN){

   405

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

   406

}

   407

   408

inline LINT __pstring_to_sint(STRING* IN){

   409

    LINT res = 0;

   410

    char tmp[STR_MAX_LEN];

   411

    char tmp2[STR_MAX_LEN];

   412

    __strlen_t l;

   413

    unsigned int shift = 0;

   414

   415

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

   416

        /* 2#0101_1010_1011_1111 */

   417

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

   418

        {

   419

            char c = IN->body[l];

   420

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

   421

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

   422

                shift += 1;

   423

            }

   424

        }

   425

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

   426

        /* 8#1234_5665_4321 */

   427

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

   428

        {

   429

            char c = IN->body[l];

   430

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

   431

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

   432

                shift += 3;

   433

            }

   434

        }

   435

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

   436

        /* 16#1234_5678_9abc_DEFG */

   437

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

   438

        {

   439

            char c = IN->body[l];

   440

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

   441

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

   442

                shift += 4;

   443

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

   444

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

   445

                shift += 4;

   446

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

   447

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

   448

                shift += 4;

   449

            }

   450

        }

   451

    }else{

   452

        /* -123456789 */

   453

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

   454

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

   455

        {

   456

            char c = IN->body[l];

   457

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

   458

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

   459

                fac *= 10;

   460

                shift += 1;

   461

            }

   462

        }

   463

    }

   464

    return res;

   465

}

   466

   467

inline LINT __string_to_sint(STRING IN){

   468

    return (LWORD)__pstring_to_sint(&IN);

   469

}

   470

inline LWORD __string_to_bit(STRING IN){

   471

    return (LWORD)__pstring_to_sint(&IN);

   472

}

   473

inline ULINT __string_to_uint(STRING IN){

   474

    return (ULINT)__pstring_to_sint(&IN);

   475

}

   476

inline LREAL __string_to_real(STRING IN){

   477

    /* search the dot */

   478

    __strlen_t l = IN.len;

   479

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

   480

    if(l != 0){

   481

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

   482

    }else{

   483

        return (LREAL)__pstring_to_sint(&IN);

   484

    }    

   485

}

   486

   487

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

   488

    /*   TO_TIME   */

   489

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

   490

inline TIME __int_to_time(LINT IN){

   491

    return (TIME){IN, 0};

   492

}

   493

   494

inline TIME __real_to_time(LREAL IN){

   495

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

   496

}

   497

inline TIME __string_to_time(STRING IN){

   498

    /* TODO :

   499

     *

   500

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

   501

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

   502

     *                                        t#5d14h12m18s3.5ms

   503

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

   504

     *  Duration literals with underlines:

   505

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

   506

     *                long prefix             TIME#25h_15m

   507

     *                                        time#5d_14h_12m_18s_3.5ms

   508

     *

   509

     *  Long prefix notation                 Short prefix notation

   510

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

   511

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

   512

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

   513

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

   514

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

   515

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

   516

     *

   517

     */

   518

    /* Quick hack : only transform seconds */

   519

    /* search the dot */

   520

    __strlen_t l = IN.len;

   521

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

   522

    if(l != 0){

   523

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

   524

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

   525

    }else{

   526

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

   527

    }

   528

}

   529