1

/*

     2

 * (c) 2008 Edouard TISSERANT

     3

 *

     4

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

     5

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

     6

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

     7

 *

     8

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

     9

 * WITHOUT ANY WARRANTY; without even the implied warranty of

    10

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

    11

 * General Public License for more details.

    12

 *

    13

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

    14

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

    15

 */

    16

    17

/****

    18

 * IEC 61131-3 standard function library

    19

 */

    20

    21

#include <limits.h>

    22

#include <float.h>

    23

#include <math.h>

    24

#include <time.h>

    25

#include <stdint.h>

    26

    27

#include <stdio.h>

    28

#include <stdlib.h>

    29

#include <string.h>

    30

#include <stdarg.h>

    31

    32

#ifdef DEBUG_IEC

    33

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

    34

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

    35

#else

    36

#define DBG(...)

    37

#define DBG_TYPE(TYPENAME, name)

    38

#endif

    39

    40

/*

    41

 * Include type defs.

    42

 */

    43

#include "iec_types_all.h"

    44

    45

extern TIME __CURRENT_TIME;

    46

extern BOOL __DEBUG;

    47

    48

/* TODO

    49

typedef struct {

    50

    __strlen_t len;

    51

    u_int16_t body[STR_MAX_LEN];

    52

} WSTRING;

    53

*/

    54

    55

# if __WORDSIZE == 64

    56

#define __32b_sufix

    57

#define __64b_sufix L

    58

#else

    59

#define __32b_sufix L

    60

#define __64b_sufix LL

    61

#endif

    62

    63

#define __lit(type,value,sfx) (type)value##sfx

    64

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

    65

#define __literal(type,value,sfx) __lit(type,value,sfx)

    66

    67

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

    68

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

    69

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

    70

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

    71

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

    72

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

    73

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

    74

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

    75

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

    76

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

    77

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

    78

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

    79

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

    80

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

    81

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

    82

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

    83

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

    84

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

    85

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

    86

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

    87

    88

    89

    90

#define __INIT_REAL 0

    91

#define __INIT_LREAL 0

    92

#define __INIT_SINT 0

    93

#define __INIT_INT 0

    94

#define __INIT_DINT 0

    95

#define __INIT_LINT 0

    96

#define __INIT_USINT 0

    97

#define __INIT_UINT 0

    98

#define __INIT_UDINT 0

    99

#define __INIT_ULINT 0

   100

#define __INIT_TIME (TIME){0,0}

   101

#define __INIT_BOOL 0

   102

#define __INIT_BYTE 0

   103

#define __INIT_WORD 0

   104

#define __INIT_DWORD 0

   105

#define __INIT_LWORD 0

   106

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

   107

//#define __INIT_WSTRING

   108

#define __INIT_DATE (DATE){0,0}

   109

#define __INIT_TOD (TOD){0,0}

   110

#define __INIT_DT (DT){0,0}

   111

   112

typedef union __IL_DEFVAR_T {

   113

    BOOL    BOOLvar;

   114

   115

    SINT    SINTvar;

   116

    INT     INTvar;

   117

    DINT    DINTvar;

   118

    LINT    LINTvar;

   119

   120

    USINT   USINTvar;

   121

    UINT    UINTvar;

   122

    UDINT   UDINTvar;

   123

    ULINT   ULINTvar;

   124

   125

    BYTE    BYTEvar;

   126

    WORD    WORDvar;

   127

    DWORD   DWORDvar;

   128

    LWORD   LWORDvar;

   129

   130

    REAL    REALvar;

   131

    LREAL   LREALvar;

   132

   133

    TIME    TIMEvar;

   134

    TOD TODvar;

   135

    DT  DTvar;

   136

    DATE    DATEvar;

   137

} __IL_DEFVAR_T;

   138

   139

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

   140

/* Misc internal */

   141

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

   142

   143

/* function that generates an IEC runtime error */

   144

static inline void IEC_error(void) {

   145

  /* TODO... */

   146

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

   147

  /*exit(1);*/

   148

}

   149

   150

   151

static inline void __normalize_timespec (IEC_TIMESPEC *ts) {

   152

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

   153

    ts->tv_sec--;

   154

    ts->tv_nsec += 1000000000;

   155

  }

   156

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

   157

    ts->tv_sec++;

   158

    ts->tv_nsec -= 1000000000;

   159

  }

   160

}

   161

   162

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

   163

  IEC_TIMESPEC ts;

   164

   165

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

   166

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

   167

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

   168

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

   169

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

   170

   171

  return ts;

   172

}

   173

   174

   175

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

   176

  IEC_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

  return ts;

   183

}

   184

   185

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

   186

  IEC_TIMESPEC ts;

   187

  struct tm broken_down_time;

   188

  time_t epoch_seconds;

   189

   190

  broken_down_time.tm_sec = 0;

   191

  broken_down_time.tm_min = 0;

   192

  broken_down_time.tm_hour = 0;

   193

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

   194

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

   195

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

   196

   197

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

   198

   199

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

   200

    IEC_error();

   201

   202

  ts.tv_sec = epoch_seconds;

   203

  ts.tv_nsec = 0;

   204

   205

  return ts;

   206

}

   207

   208

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

   209

  IEC_TIMESPEC ts;

   210

  struct tm broken_down_time;

   211

  time_t epoch_seconds;

   212

   213

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

   214

  ts.tv_sec = (long int)total_sec;

   215

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

   216

   217

  broken_down_time.tm_sec = 0;

   218

  broken_down_time.tm_min = 0;

   219

  broken_down_time.tm_hour = 0;

   220

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

   221

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

   222

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

   223

   224

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

   225

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

   226

    IEC_error();

   227

   228

  ts.tv_sec += epoch_seconds;

   229

  if (ts.tv_sec < epoch_seconds)

   230

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

   231

    IEC_error();

   232

   233

  return ts;

   234

}

   235

   236

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

   237

/*   EN/ENO PARAMS  */

   238

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

   239

   240

#define EN_ENO_PARAMS BOOL EN, BOOL *ENO

   241

#define TEST_EN(TYPENAME)\

   242

  if (!EN) {\

   243

    if (ENO != NULL)\

   244

      *ENO = __BOOL_LITERAL(FALSE);\

   245

    return __INIT_##TYPENAME;\

   246

  }\

   247

  else if (ENO != NULL)\

   248

    *ENO = __BOOL_LITERAL(TRUE);

   249

#define TEST_EN_COND(TYPENAME, COND)\

   250

  if (!EN || (COND)) {\

   251

    if (ENO != NULL)\

   252

      *ENO = __BOOL_LITERAL(FALSE);\

   253

    return __INIT_##TYPENAME;\

   254

  }\

   255

  else if (ENO != NULL)\

   256

    *ENO = __BOOL_LITERAL(TRUE);

   257

   258

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

   259

/*   Time ops  */

   260

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

   261

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

   262

   263

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

   264

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

   265

             IN1.tv_nsec + IN2.tv_nsec };

   266

  __normalize_timespec(&res);

   267

  return res;

   268

}

   269

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

   270

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

   271

             IN1.tv_nsec - IN2.tv_nsec };

   272

  __normalize_timespec(&res);

   273

  return res;

   274

}

   275

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

   276

  LREAL s_f = IN1.tv_sec * IN2;

   277

  time_t s = s_f;

   278

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

   279

  TIME res = {s + ns.quot,

   280

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

   281

  __normalize_timespec(&res);

   282

  return res;

   283

}

   284

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

   285

  LREAL s_f = IN1.tv_sec / IN2;

   286

  time_t s = s_f;

   287

  TIME res = {s,

   288

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

   289

  __normalize_timespec(&res);

   290

  return res;

   291

}

   292

   293

static inline TIME __date_and_time_to_time_of_day(EN_ENO_PARAMS, TIME IN){

   294

  TEST_EN(TIME)

   295

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

   296

}

   297

static inline TIME __date_and_time_to_date(EN_ENO_PARAMS, TIME IN){

   298

  TEST_EN(TIME)

   299

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

   300

}

   301

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

   302

  TEST_EN(TIME)

   303

  return __TIME_ADD(IN1, IN2);

   304

}

   305

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

   306

  TEST_EN(TIME)

   307

  return __TIME_SUB(IN1, IN2);

   308

}

   309

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

   310

  TEST_EN(TIME)

   311

  return __TIME_MUL(IN1, IN2);

   312

}

   313

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

   314

  TEST_EN(TIME)

   315

  return __TIME_DIV(IN1, IN2);

   316

}

   317

   318

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

   319

/* String ops  */

   320

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

   321

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

   322

   323

static inline __strlen_t __len(EN_ENO_PARAMS, STRING IN){

   324

    TEST_EN(UINT)

   325

    return IN.len;

   326

}

   327

static inline STRING __left(EN_ENO_PARAMS, STRING IN, __strlen_t L){

   328

    STRING res;

   329

    TEST_EN_COND(STRING, L < 0)

   330

    res = __INIT_STRING;

   331

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

   332

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

   333

    res.len = L;

   334

    return res;

   335

}

   336

static inline STRING __right(EN_ENO_PARAMS, STRING IN, __strlen_t L){

   337

    STRING res;

   338

    TEST_EN_COND(STRING, L < 0)

   339

    res = __INIT_STRING;

   340

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

   341

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

   342

    res.len = L;

   343

    return res;

   344

}

   345

static inline STRING __mid(EN_ENO_PARAMS, STRING IN, __strlen_t L, __strlen_t P){

   346

    STRING res;

   347

    TEST_EN_COND(STRING, L < 0 || P < 0)

   348

    res = __INIT_STRING;

   349

    if(P <= IN.len){

   350

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

   351

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

   352

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

   353

        res.len = L;

   354

    }

   355

    return res;

   356

}

   357

static inline STRING __concat(EN_ENO_PARAMS, UINT param_count, ...){

   358

  UINT i;

   359

  STRING res;

   360

  va_list ap;

   361

  __strlen_t charcount;

   362

  TEST_EN(STRING)

   363

  charcount = 0;

   364

  res = __INIT_STRING;

   365

   366

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

   367

   368

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

   369

  {

   370

    STRING tmp = va_arg(ap, STRING);

   371

    __strlen_t charrem = STR_MAX_LEN - charcount;

   372

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

   373

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

   374

    charcount += to_write;

   375

  }

   376

   377

  res.len = charcount;

   378

   379

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

   380

  return res;

   381

}

   382

static inline STRING __insert(EN_ENO_PARAMS, STRING IN1, STRING IN2, __strlen_t P){

   383

    STRING res;

   384

    __strlen_t to_copy;

   385

    TEST_EN_COND(STRING, P < 0)

   386

    res = __INIT_STRING;

   387

   388

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

   389

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

   390

    P = res.len = to_copy;

   391

   392

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

   393

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

   394

    res.len += to_copy;

   395

   396

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

   397

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

   398

    res.len += to_copy;

   399

   400

    return res;

   401

}

   402

static inline STRING __delete(EN_ENO_PARAMS, STRING IN, __strlen_t L, __strlen_t P){

   403

    STRING res;

   404

    __strlen_t to_copy;

   405

    TEST_EN_COND(STRING, L < 0 || P < 0)

   406

    res = __INIT_STRING;

   407

   408

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

   409

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

   410

    P = res.len = to_copy;

   411

   412

    if( IN.len > P + L ){

   413

        to_copy = IN.len - P - L;

   414

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

   415

        res.len += to_copy;

   416

    }

   417

   418

    return res;

   419

}

   420

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

   421

    STRING res;

   422

    __strlen_t to_copy;

   423

    TEST_EN_COND(STRING, L < 0 || P < 0)

   424

    res = __INIT_STRING;

   425

   426

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

   427

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

   428

    P = res.len = to_copy;

   429

   430

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

   431

   432

    if( to_copy + res.len > STR_MAX_LEN )

   433

       to_copy = STR_MAX_LEN - res.len;

   434

   435

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

   436

    res.len += to_copy;

   437

   438

    P += L;

   439

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

   440

    {

   441

        to_copy = IN1.len - P;

   442

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

   443

        res.len += to_copy;

   444

    }

   445

   446

    return res;

   447

}

   448

   449

   450

   451

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

   452

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

   453

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

   454

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

   455

    {

   456

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

   457

            count1 += count2 + 1;

   458

            count2 = 0;

   459

        }

   460

        else {

   461

            count2++;

   462

        }

   463

    }

   464

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

   465

}

   466

static inline __strlen_t __find(EN_ENO_PARAMS, STRING IN1, STRING IN2){

   467

    TEST_EN(UINT)

   468

    return __pfind(&IN1, &IN2);

   469

}

   470

   471

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

   472

/* Convertions */

   473

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

   474

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

   475

    /*  REAL_TO_INT  */

   476

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

   477

static inline LINT __real_round(LREAL IN)

   478

{

   479

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

   480

}

   481

static inline LINT __preal_to_sint(LREAL IN)

   482

{

   483

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

   484

}

   485

static inline LINT __preal_to_uint(LREAL IN)

   486

{

   487

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

   488

}

   489

static inline LINT __real_to_sint(EN_ENO_PARAMS, LREAL IN){

   490

    TEST_EN(LINT)

   491

    return (LINT)__preal_to_sint(IN);

   492

}

   493

static inline LWORD __real_to_bit(EN_ENO_PARAMS, LREAL IN){

   494

    TEST_EN(LWORD)

   495

    return (LWORD)__preal_to_uint(IN);

   496

}

   497

static inline ULINT __real_to_uint(EN_ENO_PARAMS, LREAL IN){

   498

    TEST_EN(ULINT)

   499

    return (ULINT)__preal_to_uint(IN);

   500

}

   501

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