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

#ifndef _IEC_STD_LIB_H

    30

#define _IEC_STD_LIB_H

    31

    32

    33

#include <limits.h>

    34

#include <float.h>

    35

#include <math.h>

    36

#include <stdint.h>

    37

#include <ctype.h>

    38

#include <time.h>

    39

    40

#include <stdio.h>

    41

#include <stdlib.h>

    42

#include <string.h>

    43

#include <stdarg.h>

    44

    45

#ifdef DEBUG_IEC

    46

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

    47

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

    48

#else

    49

#define DBG(...)

    50

#define DBG_TYPE(TYPENAME, name)

    51

#endif

    52

    53

/*

    54

 * Include type defs.

    55

 */

    56

#include "iec_types_all.h"

    57

    58

extern TIME __CURRENT_TIME;

    59

extern BOOL __DEBUG;

    60

    61

/* TODO

    62

typedef struct {

    63

    __strlen_t len;

    64

    u_int16_t body[STR_MAX_LEN];

    65

} WSTRING;

    66

*/

    67

/*

    68

# if __WORDSIZE == 64

    69

#define __32b_sufix

    70

#define __64b_sufix L

    71

#else

    72

#define __32b_sufix L

    73

#define __64b_sufix LL

    74

#endif

    75

*/

    76

    77

# if __WORDSIZE == 64

    78

#define __32b_sufix

    79

#define __64b_sufix L

    80

#else

    81

#define __32b_sufix L

    82

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

    83

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

    84

 */

    85

#define __64b_sufix L   

    86

#endif

    87

    88

    89

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

    90

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

    91

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

    92

    93

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

    94

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

    95

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

    96

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

    97

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

    98

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

    99

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

   100

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

   101

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

   102

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

   103

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

   104

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

   105

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

   106

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

   107

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

   108

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

   109

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

   110

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

   111

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

   112

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

   113

   114

   115

typedef union __IL_DEFVAR_T {

   116

    BOOL    BOOLvar;

   117

   118

    SINT    SINTvar;

   119

    INT     INTvar;

   120

    DINT    DINTvar;

   121

    LINT    LINTvar;

   122

   123

    USINT   USINTvar;

   124

    UINT    UINTvar;

   125

    UDINT   UDINTvar;

   126

    ULINT   ULINTvar;

   127

   128

    BYTE    BYTEvar;

   129

    WORD    WORDvar;

   130

    DWORD   DWORDvar;

   131

    LWORD   LWORDvar;

   132

   133

    REAL    REALvar;

   134

    LREAL   LREALvar;

   135

   136

    TIME    TIMEvar;

   137

    TOD TODvar;

   138

    DT  DTvar;

   139

    DATE    DATEvar;

   140

} __IL_DEFVAR_T;

   141

   142

   143

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

   144

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

   145

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

   146

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

   147

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

   148

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

   149

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

   150

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

   151

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

   152

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

   153

   154

   155

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

   156

/* Notify IEC runtime error */

   157

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

   158

   159

/* function that generates an IEC runtime error */

   160

static inline void __iec_error(void) {

   161

  /* TODO... */

   162

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

   163

  /*exit(1);*/

   164

}

   165

   166

   167

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

   168

/* Math Operations */

   169

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

   170

   171

static inline double __expt(double in1, double in2) {

   172

  return pow(in1, in2);

   173

}

   174

   175

   176

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

   177

/* Time normalization function */

   178

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

   179

   180

static inline void __normalize_timespec (IEC_TIMESPEC *ts) {

   181

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

   182

    ts->tv_sec--;

   183

    ts->tv_nsec += 1000000000;

   184

  }

   185

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

   186

    ts->tv_sec++;

   187

    ts->tv_nsec -= 1000000000;

   188

  }

   189

}

   190

   191

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

   192

/* Time conversion to/from timespec functions */

   193

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

   194

/* NOTE: The following function was turned into a macro, so it could be used to initialize the initial value of TIME variables.

   195

 *       Since each macro parameter is evaluated several times, the macro may result in multiple function invocations if an expression

   196

 *       containing a function invocation is passed as a parameter. However, currently matiec only uses this conversion macro with 

   197

 *       constant literals, so it is safe to change it into a macro.

   198

 */

   199

/* NOTE: I (Mario - msousa@fe.up.pt) believe that the following function contains a bug when handling negative times.

   200

 *       The equivalent macro has this bug fixed.

   201

 *       e.g.;

   202

 *          T#3.8s

   203

 *       using the function, will result in a timespec of 3.8s !!!: 

   204

 *          tv_sec  =  4               <-----  1 *  3.8           is rounded up when converting a double to an int!

   205

 *          tv_nsec = -200 000 000     <-----  1 * (3.8 - 4)*1e9

   206

 * 

   207

 *         -T#3.8s

   208

 *       using the function, will result in a timespec of -11.8s !!!: 

   209

 *          tv_sec  = -4                 <-----  -1 *  3.8 is rounded down when converting a double to an int!

   210

 *          tv_nsec = -7 800 000 000     <-----  -1 * (3.8 - -4)*1e9

   211

 */

   212

/* NOTE: Due to the fact that the C compiler may round a tv_sec number away from zero, 

   213

 *       the following macro may result in a timespec that is not normalized, i.e. with a tv_sec > 0, and a tv_nsec < 0 !!!!

   214

 *       This is due to the rounding that C compiler applies when converting a (long double) to a (long int).

   215

 *       To produce normalized timespec's we need to use floor(), but we cannot call any library functions since we want this macro to be 

   216

 *       useable as a variable initializer.

   217

 *       VAR x : TIME = T#3.5h; END_VAR --->  IEC_TIME x = __time_to_timespec(1, 0, 0, 0, 3.5, 0);

   218

 */

   219

/*

   220

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

   221

  IEC_TIMESPEC ts;

   222

   223

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

   224

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

   225

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

   226

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

   227

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

   228

   229

  return ts;

   230

}

   231

*/

   232

/* NOTE: Unfortunately older versions of ANSI C (e.g. C99) do not allow explicit identification of elements in initializers

   233

 *         e.g.  {tv_sec = 1, tv_nsec = 300}

   234

 *       They are therefore commented out. This however means that any change to the definition of IEC_TIMESPEC may require this

   235

 *       macro to be updated too!

   236

 */

   237

#define __time_to_timespec(sign,mseconds,seconds,minutes,hours,days) \

   238

          ((IEC_TIMESPEC){\

   239

              /*tv_sec  =*/ ((long int)   (((sign>=0)?1:-1)*((((long double)days*24 + (long double)hours)*60 + (long double)minutes)*60 + (long double)seconds + (long double)mseconds/1e3))), \

   240

              /*tv_nsec =*/ ((long int)(( \

   241

                            ((long double)(((sign>=0)?1:-1)*((((long double)days*24 + (long double)hours)*60 + (long double)minutes)*60 + (long double)seconds + (long double)mseconds/1e3))) - \

   242

                            ((long int)   (((sign>=0)?1:-1)*((((long double)days*24 + (long double)hours)*60 + (long double)minutes)*60 + (long double)seconds + (long double)mseconds/1e3)))   \

   243

                            )*1e9))\

   244

        })

   245

   246

   247

   248

   249

/* NOTE: The following function was turned into a macro, so it could be used to initialize the initial value of TOD (TIME_OF_DAY) variables */

   250

/* NOTE: many (but not all) of the same comments made regarding __time_to_timespec() are also valid here, so go and read those comments too!*/

   251

/*

   252

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

   253

  IEC_TIMESPEC ts;

   254

   255

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

   256

  ts.tv_sec = (long int)total_sec;

   257

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

   258

   259

  return ts;

   260

}

   261

*/

   262

#define __tod_to_timespec(seconds,minutes,hours) \

   263

          ((IEC_TIMESPEC){\

   264

              /*tv_sec  =*/ ((long int)   ((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds)), \

   265

              /*tv_nsec =*/ ((long int)(( \

   266

                            ((long double)((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds)) - \

   267

                            ((long int)   ((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds))   \

   268

                            )*1e9))\

   269

        })

   270

   271

   272

#define EPOCH_YEAR 1970

   273

#define SECONDS_PER_MINUTE 60

   274

#define SECONDS_PER_HOUR (60 * SECONDS_PER_MINUTE)

   275

#define SECONDS_PER_DAY (24 * SECONDS_PER_HOUR)

   276

#define __isleap(year) \

   277

  ((year) % 4 == 0 && ((year) % 100 != 0 || (year) % 400 == 0))

   278

static const unsigned short int __mon_yday[2][13] =

   279

{

   280

  /* Normal years.  */

   281

  { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365},

   282

  /* Leap years.  */

   283

  { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366}

   284

};

   285

   286

typedef struct {

   287

	int tm_sec;			/* Seconds.	[0-60] (1 leap second) */

   288

	int tm_min;			/* Minutes.	[0-59] */

   289

	int tm_hour;			/* Hours.	[0-23] */

   290

	int tm_day;			/* Day.		[1-31] */

   291

	int tm_mon;			/* Month.	[0-11] */

   292

	int tm_year;			/* Year	*/

   293

} tm;

   294

   295

static inline tm convert_seconds_to_date_and_time(long int seconds) {

   296

  tm dt;

   297

  long int days, rem;

   298

  days = seconds / SECONDS_PER_DAY;

   299

  rem = seconds % SECONDS_PER_DAY;

   300

  if (rem < 0) {

   301

	  rem += SECONDS_PER_DAY;

   302

	  days--;

   303

  }

   304

   305

  // time of day

   306

  dt.tm_hour = rem / SECONDS_PER_HOUR;

   307

  rem %= SECONDS_PER_HOUR;

   308

  dt.tm_min = rem / 60;

   309

  dt.tm_sec = rem % 60;

   310

   311

  // date

   312

  dt.tm_year = EPOCH_YEAR;

   313

  while (days >= (rem = __isleap(dt.tm_year) ? 366 : 365)) {

   314

	  dt.tm_year++;

   315

	  days -= rem;

   316

  }

   317

  while (days < 0) {

   318

	  dt.tm_year--;

   319

	  days += __isleap(dt.tm_year) ? 366 : 365;

   320

  }

   321

  dt.tm_mon = 1;

   322

  while (days >= __mon_yday[__isleap(dt.tm_year)][dt.tm_mon]) {

   323

	  dt.tm_mon += 1;

   324

  }

   325

  dt.tm_day = days - __mon_yday[__isleap(dt.tm_year)][dt.tm_mon - 1] + 1;

   326

   327

  return dt;

   328

}

   329

   330

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

   331

  IEC_TIMESPEC ts;

   332

  int a4, b4, a100, b100, a400, b400;

   333

  int yday;

   334

  int intervening_leap_days;

   335

   336

  if (month < 1 || month > 12)

   337

	 __iec_error();

   338

   339

  yday = __mon_yday[__isleap(year)][month - 1] + day;

   340

   341

  if (yday > __mon_yday[__isleap(year)][month])

   342

	  __iec_error();

   343

   344

  a4 = (year >> 2) - ! (year & 3);

   345

  b4 = (EPOCH_YEAR >> 2) - ! (EPOCH_YEAR & 3);

   346

  a100 = a4 / 25 - (a4 % 25 < 0);

   347

  b100 = b4 / 25 - (b4 % 25 < 0);

   348

  a400 = a100 >> 2;

   349

  b400 = b100 >> 2;

   350

  intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400);

   351

   352

  ts.tv_sec = ((year - EPOCH_YEAR) * 365 + intervening_leap_days + yday - 1) * 24 * 60 * 60;

   353

  ts.tv_nsec = 0;

   354

   355

  return ts;

   356

}

   357

   358

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

   359

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

   360

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

   361

   362

  ts.tv_sec += ts_date.tv_sec;

   363

   364

  return ts;

   365

}

   366

   367

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

   368

/* Time operations */

   369

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

   370

   371

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

   372

   373

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

   374

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

   375

             IN1.tv_nsec + IN2.tv_nsec };

   376

  __normalize_timespec(&res);

   377

  return res;

   378

}

   379

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

   380

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

   381

             IN1.tv_nsec - IN2.tv_nsec };

   382

  __normalize_timespec(&res);

   383

  return res;

   384

}

   385

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

   386

  LREAL s_f = IN1.tv_sec * IN2;

   387

  time_t s = (time_t)s_f;

   388

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

   389

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

   390

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

   391

  __normalize_timespec(&res);

   392

  return res;

   393

}

   394

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

   395

  LREAL s_f = IN1.tv_sec / IN2;

   396

  time_t s = (time_t)s_f;

   397

  TIME res = {(long)s,

   398

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

   399

  __normalize_timespec(&res);

   400

  return res;

   401

}

   402

   403

   404

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

   405

/* Convertions */

   406

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

   407

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

   408

    /*  REAL_TO_INT  */

   409

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

   410

static inline LINT __real_round(LREAL IN) {

   411

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

   412

}

   413

static inline LINT __preal_to_sint(LREAL IN) {

   414

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

   415

}

   416

static inline LINT __preal_to_uint(LREAL IN) {

   417

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

   418

}

   419

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

   420

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

   421

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

   422

   423

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

   424

    /*  TO_STRING  */

   425

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

   426

static inline STRING __bool_to_string(BOOL IN) {

   427

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

   428

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

   429

}

   430

static inline STRING __bit_to_string(LWORD IN) {

   431

    STRING res;

   432

    res = __INIT_STRING;

   433

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

   434

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

   435

    return res;

   436

}

   437

static inline STRING __real_to_string(LREAL IN) {

   438

    STRING res;

   439

    res = __INIT_STRING;

   440

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

   441

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

   442

    return res;

   443

}

   444

static inline STRING __sint_to_string(LINT IN) {

   445

    STRING res;

   446

    res = __INIT_STRING;

   447

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

   448

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

   449

    return res;

   450

}

   451

static inline STRING __uint_to_string(ULINT IN) {

   452

    STRING res;

   453

    res = __INIT_STRING;

   454

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

   455

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

   456

    return res;

   457

}

   458

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

   459

    /* FROM_STRING */

   460

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

   461

static inline BOOL __string_to_bool(STRING IN) {

   462

    int i;

   463

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

   464

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

   465

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

   466

}

   467

   468

static inline LINT __pstring_to_sint(STRING* IN) {

   469

    LINT res = 0;

   470

    __strlen_t l;

   471

    unsigned int shift = 0;

   472

   473

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

   474

        /* 2#0101_1010_1011_1111 */

   475

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

   476

        {

   477

            char c = IN->body[l];

   478

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

   479

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

   480

                shift += 1;

   481

            }

   482

        }

   483

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

   484

        /* 8#1234_5665_4321 */

   485

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

   486

        {

   487

            char c = IN->body[l];

   488

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

   489

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

   490

                shift += 3;

   491

            }

   492

        }

   493

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

   494

        /* 16#1234_5678_9abc_DEFG */

   495

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

   496

        {

   497

            char c = IN->body[l];

   498

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