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

    44

    45

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

    46

extern TIME __CURRENT_TIME;

    47

    48

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

    49

    50

/* TODO

    51

typedef struct {

    52

    __strlen_t len;

    53

    u_int16_t body[STR_MAX_LEN];

    54

} WSTRING;

    55

*/

    56

    57

# if __WORDSIZE == 64

    58

#define __32b_sufix

    59

#define __64b_sufix L

    60

#else

    61

#define __32b_sufix L

    62

#define __64b_sufix LL 

    63

#endif 

    64

    65

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

    66

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

    67

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

    68

    69

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

    70

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

    71

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

    72

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

    73

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

    74

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

    75

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

    76

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

    77

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

    78

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

    79

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

    80

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

    81

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

    82

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

    83

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

    84

#define __STRING_LITERAL(value) __literal(STRING,value,)

    85

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

    86

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

    87

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

    88

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

    89

    90

    91

    92

#define __INIT_REAL 0

    93

#define __INIT_LREAL 0

    94

#define __INIT_SINT 0

    95

#define __INIT_INT 0

    96

#define __INIT_DINT 0

    97

#define __INIT_LINT 0

    98

#define __INIT_USINT 0

    99

#define __INIT_UINT 0

   100

#define __INIT_UDINT 0

   101

#define __INIT_ULINT 0

   102

#define __INIT_TIME (TIME){0,0}

   103

#define __INIT_BOOL 0

   104

#define __INIT_BYTE 0

   105

#define __INIT_WORD 0

   106

#define __INIT_DWORD 0

   107

#define __INIT_LWORD 0

   108

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

   109

//#define __INIT_WSTRING

   110

#define __INIT_DATE (DATE){0,0}

   111

#define __INIT_TOD (TOD){0,0}

   112

#define __INIT_DT (DT){0,0}

   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

/* Misc internal */

   143

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

   144

   145

/* function that generates an IEC runtime error */

   146

static inline void IEC_error(void) {

   147

  /* TODO... */

   148

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

   149

  /*exit(1);*/

   150

}

   151

   152

   153

static inline void __normalize_timespec (struct timespec *ts) {

   154

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

   155

    ts->tv_sec--;

   156

    ts->tv_nsec += 1000000000;

   157

  }

   158

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

   159

    ts->tv_sec++;

   160

    ts->tv_nsec -= 1000000000;

   161

  }

   162

}

   163

   164

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

   165

  struct timespec ts;

   166

   167

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

   168

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

   169

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

   170

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

   171

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

   172

   173

  return ts;

   174

}

   175

   176

   177

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

   178

  struct timespec ts;

   179

   180

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

   181

  ts.tv_sec = (long int)total_sec;

   182

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

   183

   184

  return ts;

   185

}

   186

   187

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

   188

  struct timespec ts;

   189

  struct tm broken_down_time;

   190

   191

  broken_down_time.tm_sec = 0;

   192

  broken_down_time.tm_min = 0;

   193

  broken_down_time.tm_hour = 0;

   194

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

   195

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

   196

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

   197

   198

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

   199

   200

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

   201

    IEC_error();

   202

   203

  ts.tv_sec = epoch_seconds;

   204

  ts.tv_nsec = 0;

   205

   206

  return ts;

   207

}

   208

   209

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

   210

  struct timespec ts;

   211

   212

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

   213

  ts.tv_sec = (long int)total_sec;

   214

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

   215

   216

  struct tm broken_down_time;

   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

  time_t 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

/*   Time ops  */

   238

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

   239

static inline TIME __date_and_time_to_time_of_day(TIME IN){

   240

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

   241

}

   242

static inline TIME __date_and_time_to_date(TIME IN){

   243

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

   244

}

   245

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

   246

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

   247

             IN1.tv_nsec + IN2.tv_nsec };

   248

  __normalize_timespec(&res);

   249

  return res;

   250

}

   251

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

   252

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

   253

             IN1.tv_nsec - IN2.tv_nsec };

   254

  __normalize_timespec(&res);

   255

  return res;

   256

}

   257

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

   258

  LREAL s_f = IN1.tv_sec * IN2;

   259

  time_t s = s_f;

   260

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

   261

  TIME res = {s + ns.quot,

   262

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

   263

  __normalize_timespec(&res);

   264

  return res;

   265

}

   266

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

   267

  LREAL s_f = IN1.tv_sec / IN2;

   268

  time_t s = s_f;

   269

  TIME res = {s,

   270

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

   271

  __normalize_timespec(&res);

   272

  return res;

   273

}

   274

   275

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

   276

/* String ops  */

   277

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

   278

static inline UINT __len(STRING IN){

   279

    return IN.len;

   280

}

   281

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

   282

    STRING res = __INIT_STRING;

   283

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

   284

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

   285

    res.len = L;

   286

    return res;

   287

}

   288

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

   289

    STRING res = __INIT_STRING;

   290

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

   291

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

   292

    res.len = L;

   293

    return res;

   294

}

   295

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

   296

    STRING res = __INIT_STRING;

   297

    if(P <= IN.len){

   298

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

   299

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

   300

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

   301

        res.len = L;

   302

    }

   303

    return res;

   304

}

   305

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

   306

  va_list ap;

   307

  UINT i;

   308

  __strlen_t charcount = 0;

   309

  STRING res = __INIT_STRING;

   310

   311

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

   312

   313

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

   314

  {

   315

    STRING tmp = va_arg(ap, STRING);

   316

    __strlen_t charrem = STR_MAX_LEN - charcount;

   317

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

   318

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

   319

    charcount += to_write;

   320

  }

   321

   322

  res.len = charcount;

   323

   324

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

   325

  return res;

   326

}

   327

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

   328

    STRING res = __INIT_STRING;

   329

    __strlen_t to_copy;

   330

   331

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

   332

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

   333

    P = res.len = to_copy;

   334

   335

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

   336

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

   337

    res.len += to_copy;

   338

   339

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

   340

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

   341

    res.len += to_copy;

   342

   343

    return res;

   344

}

   345

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

   346

    STRING res = __INIT_STRING;

   347

    __strlen_t to_copy;

   348

   349

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

   350

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

   351

    P = res.len = to_copy;

   352

   353

    if( IN.len > P + L ){

   354

        to_copy = IN.len - P - L;

   355

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

   356

        res.len += to_copy;

   357

    }

   358

   359

    return res;

   360

}

   361

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

   362

    STRING res = __INIT_STRING;

   363

    __strlen_t to_copy;

   364

   365

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

   366

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

   367

    P = res.len = to_copy;

   368

   369

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

   370

   371

    if( to_copy + res.len > STR_MAX_LEN ) 

   372

       to_copy = STR_MAX_LEN - res.len;   

   373

   374

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

   375

    res.len += to_copy;

   376

   377

    P += L;

   378

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

   379

    {

   380

        to_copy = IN1.len - P;

   381

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

   382

        res.len += to_copy;

   383

    }

   384

   385

    return res;

   386

}

   387

   388

   389

   390

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

   391

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

   392

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

   393

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

   394

    {

   395

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

   396

            count1 += count2;

   397

            count2 = 0;

   398

        }        

   399

    }

   400

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

   401

}

   402

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

   403

    return __pfind(&IN1, &IN2);

   404

}

   405

   406

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

   407

/* Convertions */

   408

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

   409

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

   410

    /*  TO_STRING  */

   411

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

   412

static inline STRING __bool_to_string(BOOL IN)

   413

{

   414

    if(IN)

   415

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

   416

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

   417

}

   418

static inline STRING __bit_to_string(LWORD IN){

   419

    STRING res = __INIT_STRING;

   420

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

   421

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

   422

    return res;

   423

}

   424

static inline STRING __real_to_string(LREAL IN){

   425

    STRING res = __INIT_STRING;

   426

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

   427

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

   428

    return res;

   429

}

   430

static inline STRING __sint_to_string(LINT IN){

   431

    STRING res = __INIT_STRING;

   432

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

   433

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

   434

    return res;

   435

}

   436

static inline STRING __uint_to_string(ULINT IN){

   437

    STRING res = __INIT_STRING;

   438

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

   439

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

   440

    return res;

   441

}

   442

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

   443

    /* FROM_STRING */

   444

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

   445

static inline BOOL __string_to_bool(STRING IN){

   446

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

   447

}

   448

   449

static inline LINT __pstring_to_sint(STRING* IN){

   450

    LINT res = 0;

   451

    char tmp[STR_MAX_LEN];

   452

    char tmp2[STR_MAX_LEN];

   453

    __strlen_t l;

   454

    unsigned int shift = 0;

   455

   456

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

   457

        /* 2#0101_1010_1011_1111 */

   458

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

   459

        {

   460

            char c = IN->body[l];

   461

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

   462

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

   463

                shift += 1;

   464

            }

   465

        }

   466

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

   467

        /* 8#1234_5665_4321 */

   468

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

   469

        {

   470

            char c = IN->body[l];

   471

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

   472

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

   473

                shift += 3;

   474

            }

   475

        }

   476

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

   477

        /* 16#1234_5678_9abc_DEFG */

   478

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

   479

        {

   480

            char c = IN->body[l];

   481

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

   482

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

   483

                shift += 4;

   484

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

   485

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

   486

                shift += 4;

   487

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

   488

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

   489

                shift += 4;

   490

            }

   491

        }

   492

    }else{

   493

        /* -123456789 */

   494

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

   495

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

   496

        {

   497

            char c = IN->body[l];

   498

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

   499

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

   500

                fac *= 10;

   501

                shift += 1;

   502

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

   503

                res = 0;

   504

                fac = 1;

   505

                shift = 0;

   506

            }            

   507

        }

   508

    }

   509

    return res;

   510

}

   511