1

/*

     2

 * copyright 2008 Edouard TISSERANT

     3

 * copyright 2011 Mario de Sousa (msousa@fe.up.pt)

     4

 *

     5

 * Offered to the public under the terms of the GNU Lesser General Public

     6

 * License as published by the Free Software Foundation; either version 2

     7

 * of the License, or (at your option) any later version.

     8

 *

     9

 * This program is distributed in the hope that it will be useful, but

    10

 * WITHOUT ANY WARRANTY; without even the implied warranty of

    11

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser

    12

 * General Public License for more details.

    13

 *

    14

 * This code is made available on the understanding that it will not be

    15

 * used in safety-critical situations without a full and competent review.

    16

 */

    17

    18

/****

    19

 * IEC 61131-3 standard function library

    20

 */

    21

    22

/* NOTE: This file is full of (what may seem at first) very strange macros.

    23

 *       If you want to know what all these strange macros are doing,

    24

 *       just parse this file through a C preprocessor (e.g. cpp), 

    25

 *       and analyse the output!

    26

 *       $gcc -E iec_std_lib.h 

    27

 */

    28

    29

#include <limits.h>

    30

#include <float.h>

    31

#include <math.h>

    32

#include <time.h>

    33

#include <stdint.h>

    34

#include <ctype.h>

    35

    36

#include <stdio.h>

    37

#include <stdlib.h>

    38

#include <string.h>

    39

#include <stdarg.h>

    40

    41

#ifdef DEBUG_IEC

    42

#define DBG(...) printf(__VA_ARGS__);

    43

#define DBG_TYPE(TYPENAME, name) __print_##TYPENAME(name);

    44

#else

    45

#define DBG(...)

    46

#define DBG_TYPE(TYPENAME, name)

    47

#endif

    48

    49

/*

    50

 * Include type defs.

    51

 */

    52

#include "iec_types_all.h"

    53

    54

extern TIME __CURRENT_TIME;

    55

extern BOOL __DEBUG;

    56

    57

/* TODO

    58

typedef struct {

    59

    __strlen_t len;

    60

    u_int16_t body[STR_MAX_LEN];

    61

} WSTRING;

    62

*/

    63

/*

    64

# if __WORDSIZE == 64

    65

#define __32b_sufix

    66

#define __64b_sufix L

    67

#else

    68

#define __32b_sufix L

    69

#define __64b_sufix LL

    70

#endif

    71

*/

    72

    73

# if __WORDSIZE == 64

    74

#define __32b_sufix

    75

#define __64b_sufix L

    76

#else

    77

#define __32b_sufix L

    78

/* changed this from LL to L temporarily. It was causing a bug when compiling resulting code with gcc.

    79

 * I have other things to worry about at the moment.. 

    80

 */

    81

#define __64b_sufix L   

    82

#endif

    83

    84

    85

#define __lit(type,value,...) (type)value##__VA_ARGS__

    86

// Keep this macro expention step to let sfx(__VA_ARGS__) change into L or LL

    87

#define __literal(type,value,...) __lit(type,value,__VA_ARGS__)

    88

    89

#define __BOOL_LITERAL(value) __literal(BOOL,value)

    90

#define __SINT_LITERAL(value) __literal(SINT,value)

    91

#define __INT_LITERAL(value) __literal(INT,value)

    92

#define __DINT_LITERAL(value) __literal(DINT,value,__32b_sufix)

    93

#define __LINT_LITERAL(value) __literal(LINT,value,__64b_sufix)

    94

#define __USINT_LITERAL(value) __literal(USINT,value)

    95

#define __UINT_LITERAL(value) __literal(UINT,value)

    96

#define __UDINT_LITERAL(value) __literal(UDINT,value,__32b_sufix)

    97

#define __ULINT_LITERAL(value) __literal(ULINT,value,__64b_sufix)

    98

#define __REAL_LITERAL(value) __literal(REAL,value,__32b_sufix)

    99

#define __LREAL_LITERAL(value) __literal(LREAL,value,__64b_sufix)

   100

#define __TIME_LITERAL(value) __literal(TIME,value)

   101

#define __DATE_LITERAL(value) __literal(DATE,value)

   102

#define __TOD_LITERAL(value) __literal(TOD,value)

   103

#define __DT_LITERAL(value) __literal(DT,value)

   104

#define __STRING_LITERAL(count,value) (STRING){count,value}

   105

#define __BYTE_LITERAL(value) __literal(BYTE,value)

   106

#define __WORD_LITERAL(value) __literal(WORD,value)

   107

#define __DWORD_LITERAL(value) __literal(DWORD,value,__32b_sufix)

   108

#define __LWORD_LITERAL(value) __literal(LWORD,value,__64b_sufix)

   109

   110

   111

   112

#define __INIT_REAL 0

   113

#define __INIT_LREAL 0

   114

#define __INIT_SINT 0

   115

#define __INIT_INT 0

   116

#define __INIT_DINT 0

   117

#define __INIT_LINT 0

   118

#define __INIT_USINT 0

   119

#define __INIT_UINT 0

   120

#define __INIT_UDINT 0

   121

#define __INIT_ULINT 0

   122

#define __INIT_TIME (TIME){0,0}

   123

#define __INIT_BOOL 0

   124

#define __INIT_BYTE 0

   125

#define __INIT_WORD 0

   126

#define __INIT_DWORD 0

   127

#define __INIT_LWORD 0

   128

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

   129

//#define __INIT_WSTRING

   130

#define __INIT_DATE (DATE){0,0}

   131

#define __INIT_TOD (TOD){0,0}

   132

#define __INIT_DT (DT){0,0}

   133

   134

typedef union __IL_DEFVAR_T {

   135

    BOOL    BOOLvar;

   136

   137

    SINT    SINTvar;

   138

    INT     INTvar;

   139

    DINT    DINTvar;

   140

    LINT    LINTvar;

   141

   142

    USINT   USINTvar;

   143

    UINT    UINTvar;

   144

    UDINT   UDINTvar;

   145

    ULINT   ULINTvar;

   146

   147

    BYTE    BYTEvar;

   148

    WORD    WORDvar;

   149

    DWORD   DWORDvar;

   150

    LWORD   LWORDvar;

   151

   152

    REAL    REALvar;

   153

    LREAL   LREALvar;

   154

   155

    TIME    TIMEvar;

   156

    TOD TODvar;

   157

    DT  DTvar;

   158

    DATE    DATEvar;

   159

} __IL_DEFVAR_T;

   160

   161

   162

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

   163

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

   164

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

   165

/*****      Some helper functions...                              *****/

   166

/*****                     ...used later:                         *****/

   167

/*****    - when declaring the IEC 61131-3 standard functions     *****/

   168

/*****    - in the C source code itself in SFC and ST expressions *****/

   169

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

   170

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

   171

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

   172

   173

   174

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

   175

/* Notify IEC runtime error */

   176

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

   177

   178

/* function that generates an IEC runtime error */

   179

static inline void __iec_error(void) {

   180

  /* TODO... */

   181

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

   182

  /*exit(1);*/

   183

}

   184

   185

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

   186

/* Time normalization function */

   187

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

   188

   189

static inline void __normalize_timespec (IEC_TIMESPEC *ts) {

   190

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

   191

    ts->tv_sec--;

   192

    ts->tv_nsec += 1000000000;

   193

  }

   194

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

   195

    ts->tv_sec++;

   196

    ts->tv_nsec -= 1000000000;

   197

  }

   198

}

   199

   200

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

   201

/* Time conversion to/from timespec functions */

   202

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

   203

   204

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

   205

  IEC_TIMESPEC ts;

   206

   207

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

   208

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

   209

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

   210

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

   211

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

   212

   213

  return ts;

   214

}

   215

   216

   217

static inline IEC_TIMESPEC __tod_to_timespec(double seconds, double minutes, double hours) {

   218

  IEC_TIMESPEC ts;

   219

   220

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

   221

  ts.tv_sec = (long int)total_sec;

   222

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

   223

   224

  return ts;

   225

}

   226

   227

static inline IEC_TIMESPEC __date_to_timespec(int day, int month, int year) {

   228

  IEC_TIMESPEC ts;

   229

  struct tm broken_down_time;

   230

  time_t epoch_seconds;

   231

   232

  broken_down_time.tm_sec = 0;

   233

  broken_down_time.tm_min = 0;

   234

  broken_down_time.tm_hour = 0;

   235

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

   236

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

   237

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

   238

  broken_down_time.tm_isdst = -1; /* disable daylight savings time */

   239

   240

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

   241

   242

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

   243

    __iec_error();

   244

   245

  ts.tv_sec = epoch_seconds;

   246

  ts.tv_nsec = 0;

   247

   248

  return ts;

   249

}

   250

   251

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

   252

  IEC_TIMESPEC ts_date = __date_to_timespec(day, month, year);

   253

  IEC_TIMESPEC ts = __tod_to_timespec(seconds, minutes, hours);

   254

   255

  ts.tv_sec += ts_date.tv_sec;

   256

   257

  return ts;

   258

}

   259

   260

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

   261

/* Time operations */

   262

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

   263

   264

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

   265

   266

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

   267

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

   268

             IN1.tv_nsec + IN2.tv_nsec };

   269

  __normalize_timespec(&res);

   270

  return res;

   271

}

   272

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

   273

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

   274

             IN1.tv_nsec - IN2.tv_nsec };

   275

  __normalize_timespec(&res);

   276

  return res;

   277

}

   278

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

   279

  LREAL s_f = IN1.tv_sec * IN2;

   280

  time_t s = (time_t)s_f;

   281

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

   282

  TIME res = {(long)s + ns.quot,

   283

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

   284

  __normalize_timespec(&res);

   285

  return res;

   286

}

   287

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

   288

  LREAL s_f = IN1.tv_sec / IN2;

   289

  time_t s = (time_t)s_f;

   290

  TIME res = {(long)s,

   291

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

   292

  __normalize_timespec(&res);

   293

  return res;

   294

}

   295

   296

   297

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

   298

/* Convertions */

   299

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

   300

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

   301

    /*  REAL_TO_INT  */

   302

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

   303

static inline LINT __real_round(LREAL IN) {

   304

	return fmod(IN, 1) == 0 ? ((LINT)IN / 2) * 2 : (LINT)IN;

   305

}

   306

static inline LINT __preal_to_sint(LREAL IN) {

   307

   return IN >= 0 ? __real_round(IN + 0.5) : __real_round(IN - 0.5);

   308

}

   309

static inline LINT __preal_to_uint(LREAL IN) {

   310

   return IN >= 0 ? __real_round(IN + 0.5) : 0;

   311

}

   312

static inline LINT __real_to_sint(LREAL IN)  {return (LINT)__preal_to_sint(IN);}

   313

static inline LWORD __real_to_bit(LREAL IN)  {return (LWORD)__preal_to_uint(IN);}

   314

static inline ULINT __real_to_uint(LREAL IN) {return (ULINT)__preal_to_uint(IN);}

   315

   316

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

   317

    /*  TO_STRING  */

   318

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

   319

static inline STRING __bool_to_string(BOOL IN) {

   320

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

   321

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

   322

}

   323

static inline STRING __bit_to_string(LWORD IN) {

   324

    STRING res;

   325

    res = __INIT_STRING;

   326

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "16#%llx",(long long unsigned int)IN);

   327

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

   328

    return res;

   329

}

   330

static inline STRING __real_to_string(LREAL IN) {

   331

    STRING res;

   332

    res = __INIT_STRING;

   333

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

   334

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

   335

    return res;

   336

}

   337

static inline STRING __sint_to_string(LINT IN) {

   338

    STRING res;

   339

    res = __INIT_STRING;

   340

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "%lld", (long long int)IN);

   341

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

   342

    return res;

   343

}

   344

static inline STRING __uint_to_string(ULINT IN) {

   345

    STRING res;

   346

    res = __INIT_STRING;

   347

    res.len = snprintf((char*)res.body, STR_MAX_LEN, "%llu", (long long unsigned int)IN);

   348

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

   349

    return res;

   350

}

   351

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

   352

    /* FROM_STRING */

   353

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

   354

static inline BOOL __string_to_bool(STRING IN) {

   355

    int i;

   356

    if (IN.len == 1) return !memcmp(&IN.body,"1", IN.len);

   357

    for (i = 0; i < IN.len; i++) IN.body[i] = toupper(IN.body[i]);

   358

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

   359

}

   360

   361

static inline LINT __pstring_to_sint(STRING* IN) {

   362

    LINT res = 0;

   363

    __strlen_t l;

   364

    unsigned int shift = 0;

   365

   366

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

   367

        /* 2#0101_1010_1011_1111 */

   368

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

   369

        {

   370

            char c = IN->body[l];

   371

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

   372

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

   373

                shift += 1;

   374

            }

   375

        }

   376

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

   377

        /* 8#1234_5665_4321 */

   378

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

   379

        {

   380

            char c = IN->body[l];

   381

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

   382

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

   383

                shift += 3;

   384

            }

   385

        }

   386

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

   387

        /* 16#1234_5678_9abc_DEFG */

   388

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

   389

        {

   390

            char c = IN->body[l];

   391

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

   392

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

   393

                shift += 4;

   394

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

   395

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

   396

                shift += 4;

   397

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

   398

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

   399

                shift += 4;

   400

            }

   401

        }

   402

    }else{

   403

        /* -123456789 */

   404

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

   405

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

   406

        {

   407

            char c = IN->body[l];

   408

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

   409

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

   410

                fac *= 10;

   411

                shift += 1;

   412

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

   413

                res = 0;

   414

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

   415

                shift = 0;

   416

            }

   417

        }

   418

    }

   419

    return res;

   420

}

   421

   422

static inline LINT  __string_to_sint(STRING IN) {return (LINT)__pstring_to_sint(&IN);}

   423

static inline LWORD __string_to_bit (STRING IN) {return (LWORD)__pstring_to_sint(&IN);}

   424

static inline ULINT __string_to_uint(STRING IN) {return (ULINT)__pstring_to_sint(&IN);}

   425

static inline LREAL __string_to_real(STRING IN) {

   426

    __strlen_t l;

   427

    l = IN.len;

   428

    /* search the dot */

   429

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

   430

    if(l != 0){

   431

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

   432

    }else{

   433

        return (LREAL)__pstring_to_sint(&IN);

   434

    }

   435

}

   436

   437

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

   438

    /*   TO_TIME   */

   439

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

   440

static inline TIME    __int_to_time(LINT IN)  {return (TIME){IN, 0};}

   441

static inline TIME   __real_to_time(LREAL IN) {return (TIME){IN, (IN - (LINT)IN) * 1000000000};}

   442

static inline TIME __string_to_time(STRING IN){

   443

    __strlen_t l;

   444

    /* TODO :

   445

     *

   446

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

   447

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

   448

     *                                        t#5d14h12m18s3.5ms

   449

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

   450

     *  Duration literals with underlines:

   451

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

   452

     *                long prefix             TIME#25h_15m

   453

     *                                        time#5d_14h_12m_18s_3.5ms

   454

     *

   455

     *  Long prefix notation                 Short prefix notation

   456

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

   457

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

   458

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

   459

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

   460

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

   461

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

   462

     *

   463

     */

   464

    /* Quick hack : only transform seconds */

   465

    /* search the dot */

   466

    l = IN.len;

   467

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

   468

    if(l != 0){

   469

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

   470

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

   471

    }else{

   472

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

   473

    }

   474

}

   475

   476

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

   477

    /*  FROM_TIME  */

   478

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

   479

static inline LREAL __time_to_real(TIME IN){

   480

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

   481

}

   482

static inline LINT __time_to_int(TIME IN) {return IN.tv_sec;}

   483

static inline STRING __time_to_string(TIME IN){

   484

    STRING res;

   485

    div_t days;

   486

    /*t#5d14h12m18s3.5ms*/

   487

    res = __INIT_STRING;

   488

    days = div(IN.tv_sec ,86400);

   489

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

   490

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

   491

    }else{

   492

        div_t hours = div(days.rem, 3600);

   493

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

   494

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

   495

        }else{

   496

            div_t minuts = div(hours.rem, 60);

   497

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

   498

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

   499

            }else{

   500

                if(IN.tv_nsec == 0){

   501

                    res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh%dm%ds", days.quot, hours.quot, minuts.quot, minuts.rem);

   502

                }else{

   503

                    res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh%dm%ds%gms", days.quot, hours.quot, minuts.quot, minuts.rem, (LREAL)IN.tv_nsec / 1000000);