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

    63

#define STR_MAX_LEN 40

    64

typedef int8_t __strlen_t;

    65

typedef struct {

    66

    __strlen_t len;

    67

    u_int8_t body[STR_MAX_LEN];

    68

} STRING;

    69

    70

/* TODO

    71

typedef struct {

    72

    __strlen_t len;

    73

    u_int16_t body[STR_MAX_LEN];

    74

} WSTRING;

    75

*/

    76

    77

typedef union __IL_DEFVAR_T {

    78

    BOOL    BOOLvar;

    79

    80

    SINT    SINTvar;

    81

    INT     INTvar;

    82

    DINT    DINTvar;

    83

    LINT    LINTvar;

    84

    85

    USINT   USINTvar;

    86

    UINT    UINTvar;

    87

    UDINT   UDINTvar;

    88

    ULINT   ULINTvar;

    89

    90

    BYTE    BYTEvar;

    91

    WORD    WORDvar;

    92

    DWORD   DWORDvar;

    93

    LWORD   LWORDvar;

    94

    95

    REAL    REALvar;

    96

    LREAL   LREALvar;

    97

    98

    TIME    TIMEvar;

    99

    TOD TODvar;

   100

    DT  DTvar;

   101

    DATE    DATEvar;

   102

} __IL_DEFVAR_T;

   103

   104

   105

   106

   107

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

   108

/* Misc internal */

   109

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

   110

   111

/* function that generates an IEC runtime error */

   112

void IEC_error(void) {

   113

  /* TODO... */

   114

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

   115

  /*exit(1);*/

   116

}

   117

   118

   119

static inline void __normalize_timespec (struct timespec *ts) {

   120

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

   121

    ts->tv_sec--;

   122

    ts->tv_nsec += 1000000000;

   123

  }

   124

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

   125

    ts->tv_sec++;

   126

    ts->tv_nsec -= 1000000000;

   127

  }

   128

}

   129

   130

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

   131

  struct timespec ts;

   132

   133

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

   134

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

   135

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

   136

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

   137

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

   138

   139

  return ts;

   140

}

   141

   142

   143

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

   144

  struct timespec ts;

   145

   146

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

   147

  ts.tv_sec = (long int)total_sec;

   148

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

   149

   150

  return ts;

   151

}

   152

   153

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

   154

  struct timespec ts;

   155

  struct tm broken_down_time;

   156

   157

  broken_down_time.tm_sec = 0;

   158

  broken_down_time.tm_min = 0;

   159

  broken_down_time.tm_hour = 0;

   160

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

   161

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

   162

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

   163

   164

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

   165

   166

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

   167

    IEC_error();

   168

   169

  ts.tv_sec = epoch_seconds;

   170

  ts.tv_nsec = 0;

   171

   172

  return ts;

   173

}

   174

   175

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

   176

  struct timespec ts;

   177

   178

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

   179

  ts.tv_sec = (long int)total_sec;

   180

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

   181

   182

  struct tm broken_down_time;

   183

  broken_down_time.tm_sec = 0;

   184

  broken_down_time.tm_min = 0;

   185

  broken_down_time.tm_hour = 0;

   186

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

   187

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

   188

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

   189

   190

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

   191

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

   192

    IEC_error();

   193

   194

  ts.tv_sec += epoch_seconds;

   195

  if (ts.tv_sec < epoch_seconds)

   196

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

   197

    IEC_error();

   198

   199

  return ts;

   200

}

   201

   202

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

   203

/*   Time ops  */

   204

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

   205

inline TIME __date_and_time_to_time_of_day(TIME IN){

   206

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

   207

}

   208

inline TIME __date_and_time_to_date(TIME IN){

   209

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

   210

}

   211

inline TIME __time_add(TIME IN1, TIME IN2){

   212

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

   213

             IN1.tv_nsec + IN2.tv_nsec };

   214

  __normalize_timespec(&res);

   215

  return res;

   216

}

   217

inline TIME __time_sub(TIME IN1, TIME IN2){

   218

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

   219

             IN1.tv_nsec - IN2.tv_nsec };

   220

  __normalize_timespec(&res);

   221

  return res;

   222

}

   223

inline TIME __time_mul(TIME IN1, LREAL IN2){

   224

  LREAL s_f = IN1.tv_sec * IN2;

   225

  time_t s = s_f;

   226

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

   227

  TIME res = {s + ns.quot,

   228

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

   229

  __normalize_timespec(&res);

   230

  return res;

   231

}

   232

inline TIME __time_div(TIME IN1, LREAL IN2){

   233

  LREAL s_f = IN1.tv_sec / IN2;

   234

  time_t s = s_f;

   235

  TIME res = {s,

   236

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

   237

  __normalize_timespec(&res);

   238

  return res;

   239

}

   240

   241

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

   242

/* String ops  */

   243

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

   244

inline UINT __len(STRING IN){

   245

    return IN.len;

   246

}

   247

inline STRING __left(STRING IN, SINT L){

   248

    STRING res = {0,};

   249

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

   250

    return res;

   251

}

   252

inline STRING __right(STRING IN, SINT L){

   253

    STRING res = {0,};

   254

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

   255

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

   256

    res.len = L;

   257

    return res;

   258

}

   259

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

   260

    STRING res = {0,};

   261

    if(P <= IN.len){

   262

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

   263

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

   264

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

   265

        res.len = L;

   266

    }

   267

    return res;

   268

}

   269

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

   270

  va_list ap;

   271

  UINT i;

   272

  __strlen_t charcount = 0;

   273

  STRING res = {0,};

   274

   275

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

   276

   277

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

   278

  {

   279

    STRING tmp = va_arg(ap, STRING);

   280

    __strlen_t charrem = STR_MAX_LEN - charcount;

   281

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

   282

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

   283

    charcount += to_write;

   284

  }

   285

   286

  res.len = charcount;

   287

   288

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

   289

  return res;

   290

}

   291

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

   292

    STRING res = {0,};

   293

    __strlen_t to_copy;

   294

   295

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

   296

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

   297

    P = res.len = to_copy;

   298

   299

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

   300

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

   301

    res.len += to_copy;

   302

   303

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

   304

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

   305

    res.len += to_copy;

   306

   307

    return res;

   308

}

   309

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

   310

    STRING res = {0,};

   311

    __strlen_t to_copy;

   312

   313

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

   314

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

   315

    P = res.len = to_copy;

   316

   317

    to_copy = IN.len - P;

   318

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

   319

    res.len += to_copy;

   320

   321

    return res;

   322

}

   323

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

   324

    STRING res = {0,};

   325

    __strlen_t to_copy;

   326

   327

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

   328

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

   329

    P = res.len = to_copy;

   330

   331

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

   332

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

   333

    res.len += to_copy;

   334

   335

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

   336

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

   337

    res.len += to_copy;

   338

   339

    return res;

   340

}

   341

   342

   343

   344

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

   345

    UINT count1 = 0;

   346

    UINT count2 = 0;

   347

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

   348

    {

   349

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

   350

            count1 += count2;

   351

            count2 = 0;

   352

        }        

   353

    }

   354

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

   355

}

   356

inline UINT __find(STRING IN1, STRING IN2){

   357

    return __pfind(&IN1, &IN2);

   358

}

   359

   360

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

   361

/* Convertions */

   362

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

   363

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

   364

    /*  TO_STRING  */

   365

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

   366

inline STRING __bool_to_string(BOOL IN)

   367

{

   368

    if(IN)

   369

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

   370

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

   371

}

   372

inline STRING __bit_to_string(LWORD IN){

   373

    STRING res = {0,};

   374

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

   375

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

   376

    return res;

   377

}

   378

inline STRING __real_to_string(LREAL IN){

   379

    STRING res = {0,};

   380

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

   381

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

   382

    return res;

   383

}

   384

inline STRING __sint_to_string(LINT IN){

   385

    STRING res = {0,};

   386

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

   387

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

   388

    return res;

   389

}

   390

inline STRING __uint_to_string(ULINT IN){

   391

    STRING res = {0,};

   392

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

   393

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

   394

    return res;

   395

}

   396

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

   397

    /* FROM_STRING */

   398

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

   399

inline BOOL __string_to_bool(STRING IN){

   400

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

   401

}

   402

   403

inline LINT __pstring_to_sint(STRING* IN){

   404

    LINT res = 0;

   405

    char tmp[STR_MAX_LEN];

   406

    char tmp2[STR_MAX_LEN];

   407

    __strlen_t l;

   408

    unsigned int shift = 0;

   409

   410

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

   411

        /* 2#0101_1010_1011_1111 */

   412

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

   413

        {

   414

            char c = IN->body[l];

   415

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

   416

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

   417

                shift += 1;

   418

            }

   419

        }

   420

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

   421

        /* 8#1234_5665_4321 */

   422

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

   423

        {

   424

            char c = IN->body[l];

   425

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

   426

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

   427

                shift += 3;

   428

            }

   429

        }

   430

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

   431

        /* 16#1234_5678_9abc_DEFG */

   432

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

   433

        {

   434

            char c = IN->body[l];

   435

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

   436

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

   437

                shift += 4;

   438

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

   439

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

   440

                shift += 4;

   441

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

   442

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

   443

                shift += 4;

   444

            }

   445

        }

   446

    }else{

   447

        /* -123456789 */

   448

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

   449

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

   450

        {

   451

            char c = IN->body[l];

   452

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

   453

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

   454

                fac *= 10;

   455

                shift += 1;

   456

            }

   457

        }

   458

    }

   459

    return res;

   460

}

   461

   462

inline LINT __string_to_sint(STRING IN){

   463

    return (LWORD)__pstring_to_sint(&IN);

   464

}

   465

inline LWORD __string_to_bit(STRING IN){

   466

    return (LWORD)__pstring_to_sint(&IN);

   467

}

   468

inline ULINT __string_to_uint(STRING IN){

   469

    return (ULINT)__pstring_to_sint(&IN);

   470

}

   471

inline LREAL __string_to_real(STRING IN){

   472

    /* search the dot */

   473

    __strlen_t l = IN.len;

   474

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

   475

    if(l != 0){

   476

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

   477

    }else{

   478

        return (LREAL)__pstring_to_sint(&IN);

   479

    }    

   480

}

   481

   482

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

   483

    /*   TO_TIME   */

   484

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

   485

inline TIME __int_to_time(LINT IN){

   486

    return (TIME){IN, 0};

   487

}

   488

   489

inline TIME __real_to_time(LREAL IN){

   490

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

   491

}

   492

inline TIME __string_to_time(STRING IN){

   493

    /* TODO :

   494

     *

   495

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

   496

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

   497

     *                                        t#5d14h12m18s3.5ms

   498

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

   499

     *  Duration literals with underlines:

   500

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

   501

     *                long prefix             TIME#25h_15m

   502

     *                                        time#5d_14h_12m_18s_3.5ms

   503

     *

   504

     *  Long prefix notation                 Short prefix notation

   505

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

   506

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

   507

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

   508

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

   509

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

   510

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

   511

     *

   512

     */

   513

    /* Quick hack : only transform seconds */

   514

    /* search the dot */

   515

    __strlen_t l = IN.len;

   516

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

   517

    if(l != 0){

   518

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

   519

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

   520

    }else{

   521

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

   522

    }

   523

}

   524

   525

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

   526

    /*  FROM_TIME  */

   527

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

   528

inline LREAL __time_to_real(TIME IN){

   529

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

   530

}