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

    39

#include <stdio.h>

    40

#include <stdlib.h>

    41

#include <string.h>

    42

#include <stdarg.h>

    43

    44

#ifdef DEBUG_IEC

    45

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

    46

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

    47

#else

    48

#define DBG(...)

    49

#define DBG_TYPE(TYPENAME, name)

    50

#endif

    51

    52

/*

    53

 * Include type defs.

    54

 */

    55

#include "iec_types_all.h"

    56

    57

extern TIME __CURRENT_TIME;

    58

extern BOOL __DEBUG;

    59

    60

/* TODO

    61

typedef struct {

    62

    __strlen_t len;

    63

    u_int16_t body[STR_MAX_LEN];

    64

} WSTRING;

    65

*/

    66

/*

    67

# if __WORDSIZE == 64

    68

#define __32b_sufix

    69

#define __64b_sufix L

    70

#else

    71

#define __32b_sufix L

    72

#define __64b_sufix LL

    73

#endif

    74

*/

    75

    76

# if __WORDSIZE == 64

    77

#define __32b_sufix

    78

#define __64b_sufix L

    79

#else

    80

#define __32b_sufix L

    81

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

    82

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

    83

 */

    84

#define __64b_sufix L   

    85

#endif

    86

    87

    88

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

    89

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

    90

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

    91

    92

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

    93

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

    94

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

    95

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

    96

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

    97

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

    98

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

    99

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

   100

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

   101

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

   102

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

   103

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

   104

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

   105

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

   106

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

   107

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

   108

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

   109

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

   110

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

   111

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

   112

   113

   114

typedef union __IL_DEFVAR_T {

   115

    BOOL    BOOLvar;

   116

   117

    SINT    SINTvar;

   118

    INT     INTvar;

   119

    DINT    DINTvar;

   120

    LINT    LINTvar;

   121

   122

    USINT   USINTvar;

   123

    UINT    UINTvar;

   124

    UDINT   UDINTvar;

   125

    ULINT   ULINTvar;

   126

   127

    BYTE    BYTEvar;

   128

    WORD    WORDvar;

   129

    DWORD   DWORDvar;

   130

    LWORD   LWORDvar;

   131

   132

    REAL    REALvar;

   133

    LREAL   LREALvar;

   134

   135

    TIME    TIMEvar;

   136

    TOD TODvar;

   137

    DT  DTvar;

   138

    DATE    DATEvar;

   139

} __IL_DEFVAR_T;

   140

   141

   142

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

   143

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

   144

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

   145

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

   146

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

   147

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

   148

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

   149

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

   150

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

   151

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

   152

   153

   154

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

   155

/* Notify IEC runtime error */

   156

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

   157

   158

/* function that generates an IEC runtime error */

   159

static inline void __iec_error(void) {

   160

  /* TODO... */

   161

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

   162

  /*exit(1);*/

   163

}

   164

   165

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

   166

/* Time normalization function */

   167

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

   168

   169

static inline void __normalize_timespec (IEC_TIMESPEC *ts) {

   170

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

   171

    ts->tv_sec--;

   172

    ts->tv_nsec += 1000000000;

   173

  }

   174

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

   175

    ts->tv_sec++;

   176

    ts->tv_nsec -= 1000000000;

   177

  }

   178

}

   179

   180

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

   181

/* Time conversion to/from timespec functions */

   182

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

   183

   184

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

   185

  IEC_TIMESPEC ts;

   186

   187

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

   188

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

   189

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

   190

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

   191

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

   192

   193

  return ts;

   194

}

   195

   196

   197

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

   198

  IEC_TIMESPEC ts;

   199

   200

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

   201

  ts.tv_sec = (long int)total_sec;

   202

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

   203

   204

  return ts;

   205

}

   206

   207

#define EPOCH_YEAR 1970

   208

#define SECONDS_PER_MINUTE 60

   209

#define SECONDS_PER_HOUR (60 * SECONDS_PER_MINUTE)

   210

#define SECONDS_PER_DAY (24 * SECONDS_PER_HOUR)

   211

#define __isleap(year) \

   212

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

   213

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

   214

{

   215

  /* Normal years.  */

   216

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

   217

  /* Leap years.  */

   218

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

   219

};

   220

   221

typedef struct {

   222

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

   223

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

   224

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

   225

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

   226

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

   227

	int tm_year;		/* Year	*/

   228

} tm;

   229

   230

static inline tm convert_seconds_to_date_and_time(long int seconds) {

   231

  tm dt;

   232

  long int days, rem;

   233

  days = seconds / SECONDS_PER_DAY;

   234

  rem = seconds % SECONDS_PER_DAY;

   235

  if (rem < 0) {

   236

	  rem += SECONDS_PER_DAY;

   237

	  days--;

   238

  }

   239

   240

  // time of day

   241

  dt.tm_hour = rem / SECONDS_PER_HOUR;

   242

  rem %= SECONDS_PER_HOUR;

   243

  dt.tm_min = rem / 60;

   244

  dt.tm_sec = rem % 60;

   245

   246

  // date

   247

  dt.tm_year = EPOCH_YEAR;

   248

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

   249

	  dt.tm_year++;

   250

	  days -= rem;

   251

  }

   252

  while (days < 0) {

   253

	  dt.tm_year--;

   254

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

   255

  }

   256

  dt.tm_mon = 1;

   257

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

   258

	  dt.tm_mon += 1;

   259

  }

   260

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

   261

   262

  return dt;

   263

}

   264

   265

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

   266

  IEC_TIMESPEC ts;

   267

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

   268

  int yday;

   269

  int intervening_leap_days;

   270

   271

  if (month < 1 || month > 12)

   272

	 __iec_error();

   273

   274

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

   275

   276

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

   277

	  __iec_error();

   278

   279

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

   280

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

   281

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

   282

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

   283

  a400 = a100 >> 2;

   284

  b400 = b100 >> 2;

   285

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

   286

   287

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

   288

  ts.tv_nsec = 0;

   289

   290

  return ts;

   291

}

   292

   293

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

   294

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

   295

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

   296

   297

  ts.tv_sec += ts_date.tv_sec;

   298

   299

  return ts;

   300

}

   301

   302

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

   303

/* Time operations */

   304

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

   305

   306

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

   307

   308

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

   309

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

   310

             IN1.tv_nsec + IN2.tv_nsec };

   311

  __normalize_timespec(&res);

   312

  return res;

   313

}

   314

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

   315

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

   316

             IN1.tv_nsec - IN2.tv_nsec };

   317

  __normalize_timespec(&res);

   318

  return res;

   319

}

   320

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

   321

  LREAL s_f = IN1.tv_sec * IN2;

   322

  time_t s = (time_t)s_f;

   323

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

   324

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

   325

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

   326

  __normalize_timespec(&res);

   327

  return res;

   328

}

   329

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

   330

  LREAL s_f = IN1.tv_sec / IN2;

   331

  time_t s = (time_t)s_f;

   332

  TIME res = {(long)s,

   333

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

   334

  __normalize_timespec(&res);

   335

  return res;

   336

}

   337

   338

   339

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

   340

/* Convertions */

   341

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

   342

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

   343

    /*  REAL_TO_INT  */

   344

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

   345

static inline LINT __real_round(LREAL IN) {

   346

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

   347

}

   348

static inline LINT __preal_to_sint(LREAL IN) {

   349

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

   350

}

   351

static inline LINT __preal_to_uint(LREAL IN) {

   352

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

   353

}

   354

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

   355

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

   356

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

   357

   358

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

   359

    /*  TO_STRING  */

   360

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

   361

static inline STRING __bool_to_string(BOOL IN) {

   362

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

   363

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

   364

}

   365

static inline STRING __bit_to_string(LWORD IN) {

   366

    STRING res;

   367

    res = __INIT_STRING;

   368

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

   369

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

   370

    return res;

   371

}

   372

static inline STRING __real_to_string(LREAL IN) {

   373

    STRING res;

   374

    res = __INIT_STRING;

   375

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

   376

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

   377

    return res;

   378

}

   379

static inline STRING __sint_to_string(LINT IN) {

   380

    STRING res;

   381

    res = __INIT_STRING;

   382

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

   383

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

   384

    return res;

   385

}

   386

static inline STRING __uint_to_string(ULINT IN) {

   387

    STRING res;

   388

    res = __INIT_STRING;

   389

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

   390

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

   391

    return res;

   392

}

   393

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

   394

    /* FROM_STRING */

   395

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

   396

static inline BOOL __string_to_bool(STRING IN) {

   397

    int i;

   398

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

   399

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

   400

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

   401

}

   402

   403

static inline LINT __pstring_to_sint(STRING* IN) {

   404

    LINT res = 0;

   405

    __strlen_t l;

   406

    unsigned int shift = 0;

   407

   408

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

   409

        /* 2#0101_1010_1011_1111 */

   410

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

   411

        {

   412

            char c = IN->body[l];

   413

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

   414

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

   415

                shift += 1;

   416

            }

   417

        }

   418

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

   419

        /* 8#1234_5665_4321 */

   420

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

   421

        {

   422

            char c = IN->body[l];

   423

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

   424

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

   425

                shift += 3;

   426

            }

   427

        }

   428

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

   429

        /* 16#1234_5678_9abc_DEFG */

   430

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

   431

        {

   432

            char c = IN->body[l];

   433

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

   434

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

   435

                shift += 4;

   436

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

   437

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

   438

                shift += 4;

   439

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

   440

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

   441

                shift += 4;

   442

            }

   443

        }

   444

    }else{

   445

        /* -123456789 */

   446

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

   447

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

   448

        {

   449

            char c = IN->body[l];

   450

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

   451

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

   452

                fac *= 10;

   453

                shift += 1;

   454

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

   455

                res = 0;

   456

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

   457

                shift = 0;

   458

            }

   459

        }

   460

    }

   461

    return res;

   462

}

   463

   464

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

   465

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

   466

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

   467

static inline LREAL __string_to_real(STRING IN) {

   468

    __strlen_t l;

   469

    l = IN.len;

   470

    /* search the dot */

   471

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

   472

    if(l != 0){

   473

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

   474

    }else{

   475

        return (LREAL)__pstring_to_sint(&IN);

   476

    }

   477

}

   478

   479

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

   480

    /*   TO_TIME   */

   481

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

   482

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

   483

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

   484

static inline TIME __string_to_time(STRING IN){

   485

    __strlen_t l;

   486

    /* TODO :

   487

     *

   488

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

   489

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

   490

     *                                        t#5d14h12m18s3.5ms

   491

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

   492

     *  Duration literals with underlines:

   493

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

   494

     *                long prefix             TIME#25h_15m

   495

     *                                        time#5d_14h_12m_18s_3.5ms

   496

     *

   497

     *  Long prefix notation                 Short prefix notation

   498

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

   499

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

   500

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

   501

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