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

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

    19

/*  Types defs   */

    20

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

    21

    22

typedef u_int8_t  BOOL;

    23

    24

#define TRUE 1

    25

#define FALSE 0

    26

    27

typedef int8_t    SINT;

    28

typedef int16_t   INT;

    29

typedef int32_t   DINT;

    30

typedef int64_t   LINT;

    31

    32

typedef u_int8_t    USINT;

    33

typedef u_int16_t   UINT;

    34

typedef u_int32_t   UDINT;

    35

typedef u_int64_t   ULINT;

    36

    37

typedef u_int8_t    BYTE;

    38

typedef u_int16_t   WORD;

    39

typedef u_int32_t   DWORD;

    40

typedef u_int64_t   LWORD;

    41

    42

typedef float    REAL;

    43

typedef double   LREAL;

    44

    45

typedef struct timespec TIME;

    46

typedef struct timespec DATE;

    47

typedef struct timespec DT;

    48

typedef struct timespec TOD;

    49

    50

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

    51

extern TIME __CURRENT_TIME;

    52

    53

#define STR_MAX_LEN 40

    54

typedef int8_t __strlen_t;

    55

typedef struct {

    56

    __strlen_t len;

    57

    u_int8_t body[STR_MAX_LEN];

    58

} STRING;

    59

    60

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

    61

    62

/* TODO

    63

typedef struct {

    64

    __strlen_t len;

    65

    u_int16_t body[STR_MAX_LEN];

    66

} WSTRING;

    67

*/

    68

    69

#define __INIT_REAL 0

    70

#define __INIT_LREAL 0

    71

#define __INIT_SINT 0

    72

#define __INIT_INT 0

    73

#define __INIT_DINT 0

    74

#define __INIT_LINT 0

    75

#define __INIT_USINT 0

    76

#define __INIT_UINT 0

    77

#define __INIT_UDINT 0

    78

#define __INIT_ULINT 0

    79

#define __INIT_TIME (TIME){0,0}

    80

#define __INIT_BOOL 0

    81

#define __INIT_BYTE 0

    82

#define __INIT_WORD 0

    83

#define __INIT_DWORD 0

    84

#define __INIT_LWORD 0

    85

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

    86

//#define __INIT_WSTRING

    87

#define __INIT_DATE (DATE){0,0}

    88

#define __INIT_TOD (TOD){0,0}

    89

#define __INIT_DT (DT){0,0}

    90

    91

typedef union __IL_DEFVAR_T {

    92

    BOOL    BOOLvar;

    93

    94

    SINT    SINTvar;

    95

    INT     INTvar;

    96

    DINT    DINTvar;

    97

    LINT    LINTvar;

    98

    99

    USINT   USINTvar;

   100

    UINT    UINTvar;

   101

    UDINT   UDINTvar;

   102

    ULINT   ULINTvar;

   103

   104

    BYTE    BYTEvar;

   105

    WORD    WORDvar;

   106

    DWORD   DWORDvar;

   107

    LWORD   LWORDvar;

   108

   109

    REAL    REALvar;

   110

    LREAL   LREALvar;

   111

   112

    TIME    TIMEvar;

   113

    TOD TODvar;

   114

    DT  DTvar;

   115

    DATE    DATEvar;

   116

} __IL_DEFVAR_T;

   117

   118

   119

   120

   121

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

   122

/* Misc internal */

   123

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

   124

   125

/* function that generates an IEC runtime error */

   126

static inline void IEC_error(void) {

   127

  /* TODO... */

   128

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

   129

  /*exit(1);*/

   130

}

   131

   132

   133

static inline void __normalize_timespec (struct timespec *ts) {

   134

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

   135

    ts->tv_sec--;

   136

    ts->tv_nsec += 1000000000;

   137

  }

   138

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

   139

    ts->tv_sec++;

   140

    ts->tv_nsec -= 1000000000;

   141

  }

   142

}

   143

   144

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

   145

  struct timespec ts;

   146

   147

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

   148

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

   149

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

   150

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

   151

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

   152

   153

  return ts;

   154

}

   155

   156

   157

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

   158

  struct timespec ts;

   159

   160

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

   161

  ts.tv_sec = (long int)total_sec;

   162

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

   163

   164

  return ts;

   165

}

   166

   167

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

   168

  struct timespec ts;

   169

  struct tm broken_down_time;

   170

   171

  broken_down_time.tm_sec = 0;

   172

  broken_down_time.tm_min = 0;

   173

  broken_down_time.tm_hour = 0;

   174

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

   175

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

   176

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

   177

   178

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

   179

   180

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

   181

    IEC_error();

   182

   183

  ts.tv_sec = epoch_seconds;

   184

  ts.tv_nsec = 0;

   185

   186

  return ts;

   187

}

   188

   189

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

   190

  struct timespec ts;

   191

   192

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

   193

  ts.tv_sec = (long int)total_sec;

   194

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

   195

   196

  struct tm broken_down_time;

   197

  broken_down_time.tm_sec = 0;

   198

  broken_down_time.tm_min = 0;

   199

  broken_down_time.tm_hour = 0;

   200

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

   201

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

   202

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

   203

   204

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

   205

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

   206

    IEC_error();

   207

   208

  ts.tv_sec += epoch_seconds;

   209

  if (ts.tv_sec < epoch_seconds)

   210

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

   211

    IEC_error();

   212

   213

  return ts;

   214

}

   215

   216

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

   217

/*   Time ops  */

   218

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

   219

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   220

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

   221

}

   222

static inline TIME __date_and_time_to_date(TIME IN){

   223

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

   224

}

   225

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

   226

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

   227

             IN1.tv_nsec + IN2.tv_nsec };

   228

  __normalize_timespec(&res);

   229

  return res;

   230

}

   231

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

   232

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

   233

             IN1.tv_nsec - IN2.tv_nsec };

   234

  __normalize_timespec(&res);

   235

  return res;

   236

}

   237

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

   238

  LREAL s_f = IN1.tv_sec * IN2;

   239

  time_t s = s_f;

   240

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

   241

  TIME res = {s + ns.quot,

   242

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

   243

  __normalize_timespec(&res);

   244

  return res;

   245

}

   246

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

   247

  LREAL s_f = IN1.tv_sec / IN2;

   248

  time_t s = s_f;

   249

  TIME res = {s,

   250

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

   251

  __normalize_timespec(&res);

   252

  return res;

   253

}

   254

   255

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

   256

/* String ops  */

   257

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

   258

static inline UINT __len(STRING IN){

   259

    return IN.len;

   260

}

   261

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

   262

    STRING res = __INIT_STRING;

   263

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

   264

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

   265

    res.len = L;

   266

    return res;

   267

}

   268

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

   269

    STRING res = __INIT_STRING;

   270

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

   271

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

   272

    res.len = L;

   273

    return res;

   274

}

   275

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

   276

    STRING res = __INIT_STRING;

   277

    if(P <= IN.len){

   278

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

   279

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

   280

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

   281

        res.len = L;

   282

    }

   283

    return res;

   284

}

   285

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

   286

  va_list ap;

   287

  UINT i;

   288

  __strlen_t charcount = 0;

   289

  STRING res = __INIT_STRING;

   290

   291

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

   292

   293

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

   294

  {

   295

    STRING tmp = va_arg(ap, STRING);

   296

    __strlen_t charrem = STR_MAX_LEN - charcount;

   297

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

   298

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

   299

    charcount += to_write;

   300

  }

   301

   302

  res.len = charcount;

   303

   304

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

   305

  return res;

   306

}

   307

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

   308

    STRING res = __INIT_STRING;

   309

    __strlen_t to_copy;

   310

   311

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

   312

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

   313

    P = res.len = to_copy;

   314

   315

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

   316

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

   317

    res.len += to_copy;

   318

   319

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

   320

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

   321

    res.len += to_copy;

   322

   323

    return res;

   324

}

   325

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

   326

    STRING res = __INIT_STRING;

   327

    __strlen_t to_copy;

   328

   329

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

   330

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

   331

    P = res.len = to_copy;

   332

   333

    if( IN.len > P + L ){

   334

        to_copy = IN.len - P - L;

   335

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

   336

        res.len += to_copy;

   337

    }

   338

   339

    return res;

   340

}

   341

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

   342

    STRING res = __INIT_STRING;

   343

    __strlen_t to_copy;

   344

   345

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

   346

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

   347

    P = res.len = to_copy;

   348

   349

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

   350

   351

    if( to_copy + res.len > STR_MAX_LEN ) 

   352

       to_copy = STR_MAX_LEN - res.len;   

   353

   354

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

   355

    res.len += to_copy;

   356

   357

    P += L;

   358

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

   359

    {

   360

        to_copy = IN1.len - P;

   361

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

   362

        res.len += to_copy;

   363

    }

   364

   365

    return res;

   366

}

   367

   368

   369

   370

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

   371

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

   372

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

   373

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

   374

    {

   375

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

   376

            count1 += count2;

   377

            count2 = 0;

   378

        }        

   379

    }

   380

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

   381

}

   382

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

   383

    return __pfind(&IN1, &IN2);

   384

}

   385

   386

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

   387

/* Convertions */

   388

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

   389

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

   390

    /*  TO_STRING  */

   391

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

   392

static inline STRING __bool_to_string(BOOL IN)

   393

{

   394

    if(IN)

   395

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

   396

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

   397

}

   398

static inline STRING __bit_to_string(LWORD IN){

   399

    STRING res = __INIT_STRING;

   400

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

   401

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

   402

    return res;

   403

}

   404

static inline STRING __real_to_string(LREAL IN){

   405

    STRING res = __INIT_STRING;

   406

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

   407

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

   408

    return res;

   409

}

   410

static inline STRING __sint_to_string(LINT IN){

   411

    STRING res = __INIT_STRING;

   412

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

   413

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

   414

    return res;

   415

}

   416

static inline STRING __uint_to_string(ULINT IN){

   417

    STRING res = __INIT_STRING;

   419

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

   420

    return res;

   421

}

   422

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

   423

    /* FROM_STRING */

   424

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

   425

static inline BOOL __string_to_bool(STRING IN){

   426

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

   427

}

   428

   429

static inline LINT __pstring_to_sint(STRING* IN){

   430

    LINT res = 0;

   431

    char tmp[STR_MAX_LEN];

   432

    char tmp2[STR_MAX_LEN];

   433

    __strlen_t l;

   434

    unsigned int shift = 0;

   435

   436

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

   437

        /* 2#0101_1010_1011_1111 */

   438

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

   439

        {

   440

            char c = IN->body[l];

   441

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

   442

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

   443

                shift += 1;

   444

            }

   445

        }

   446

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

   447

        /* 8#1234_5665_4321 */

   448

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

   449

        {

   450

            char c = IN->body[l];

   451

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

   452

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

   453

                shift += 3;

   454

            }

   455

        }

   456

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

   457

        /* 16#1234_5678_9abc_DEFG */

   458

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

   459

        {

   460

            char c = IN->body[l];

   461

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

   462

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

   463

                shift += 4;

   464

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

   465

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

   466

                shift += 4;

   467

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

   468

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

   469

                shift += 4;

   470

            }

   471

        }

   472

    }else{

   473

        /* -123456789 */

   474

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

   475

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

   476

        {

   477

            char c = IN->body[l];

   478

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

   479

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

   480

                fac *= 10;

   481

                shift += 1;

   482

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

   483

                res = 0;

   484

                fac = 1;

   485

                shift = 0;

   486

            }            

   487

        }

   488

    }

   489

    return res;

   490

}

   491

   492

static inline LINT __string_to_sint(STRING IN){

   493

    return (LWORD)__pstring_to_sint(&IN);

   494

}

   495

static inline LWORD __string_to_bit(STRING IN){

   496

    return (LWORD)__pstring_to_sint(&IN);

   497

}

   498

static inline ULINT __string_to_uint(STRING IN){

   499

    return (ULINT)__pstring_to_sint(&IN);

   500

}

   501

static inline LREAL __string_to_real(STRING IN){

   502

    /* search the dot */

   503

    __strlen_t l = IN.len;

   504

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

   505

    if(l != 0){

   506

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

   507

    }else{

   508

        return (LREAL)__pstring_to_sint(&IN);

   509

    }    

   510

}

   511

   512

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

   513

    /*   TO_TIME   */

   514

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

   515

static inline TIME __int_to_time(LINT IN){

   516

    return (TIME){IN, 0};

   517

}