1 /****

     2  * IEC 61131-3 standard function lib

     3  */

     4 

     5 #include <limits.h>

     6 #include <float.h>

     7 #include <math.h>

     8 #include <time.h>

     9 #include <sys/types.h>

    10 

    11 #include <stdlib.h>

    12 #include <string.h>

    13 #include <stdarg.h>

    14 

    15 

    16 /* Macro that expand to subtypes */

    17 #define ANY(DO) ANY_DERIVED(DO) ANY_ELEMENTARY(DO)

    18 #define ANY_DERIVED(DO)

    19 #define ANY_ELEMENTARY(DO) ANY_MAGNITUDE(DO) ANY_BIT(DO) ANY_STRING(DO) ANY_DATE(DO)

    20 #define ANY_MAGNITUDE(DO) ANY_NUM(DO) DO(TIME)

    21 #define ANY_BIT(DO) ANY_NBIT(DO) DO(BOOL)

    22 #define ANY_NBIT(DO) DO(BYTE) DO(WORD) DO(DWORD) DO(LWORD)

    23 #define ANY_STRING(DO) DO(STRING) /*DO(WSTRING) TODO */

    24 #define ANY_DATE(DO) DO(DATE) DO(TOD) DO(DT)

    25 #define ANY_NUM(DO) ANY_REAL(DO) ANY_INT(DO)

    26 #define ANY_REAL(DO) DO(REAL) DO(LREAL)

    27 #define ANY_INT(DO) ANY_SINT(DO) ANY_UINT(DO)

    28 #define ANY_SINT(DO) DO(SINT) DO(INT) DO(DINT) DO(LINT)

    29 #define ANY_UINT(DO) DO(USINT) DO(UINT) DO(UDINT) DO(ULINT)

    30 

    31 /*****************/

    32 /*  Types defs   */

    33 /*****************/

    34 

    35 typedef u_int8_t  BOOL;

    36 

    37 #define TRUE 1

    38 #define FALSE 0

    39 

    40 typedef int8_t    SINT;

    41 typedef int16_t   INT;

    42 typedef int32_t   DINT;

    43 typedef int64_t   LINT;

    44 

    45 typedef u_int8_t    USINT;

    46 typedef u_int16_t   UINT;

    47 typedef u_int32_t   UDINT;

    48 typedef u_int64_t   ULINT;

    49 

    50 typedef u_int8_t    BYTE;

    51 typedef u_int16_t   WORD;

    52 typedef u_int32_t   DWORD;

    53 typedef u_int64_t   LWORD;

    54 

    55 typedef float    REAL;

    56 typedef double   LREAL;

    57 

    58 typedef struct timespec TIME;

    59 typedef struct timespec DATE;

    60 typedef struct timespec DT;

    61 typedef struct timespec TOD;

    62 

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

    64 

    65 #define STR_MAX_LEN 40

    66 typedef int8_t __strlen_t;

    67 typedef struct {

    68     __strlen_t len;

    69     u_int8_t body[STR_MAX_LEN];

    70 } STRING;

    71 

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

    73 

    74 

    75 /* TODO

    76 typedef struct {

    77     __strlen_t len;

    78     u_int16_t body[STR_MAX_LEN];

    79 } WSTRING;

    80 */

    81 

    82 typedef union __IL_DEFVAR_T {

    83     BOOL    BOOLvar;

    84 

    85     SINT    SINTvar;

    86     INT     INTvar;

    87     DINT    DINTvar;

    88     LINT    LINTvar;

    89 

    90     USINT   USINTvar;

    91     UINT    UINTvar;

    92     UDINT   UDINTvar;

    93     ULINT   ULINTvar;

    94 

    95     BYTE    BYTEvar;

    96     WORD    WORDvar;

    97     DWORD   DWORDvar;

    98     LWORD   LWORDvar;

    99 

   100     REAL    REALvar;

   101     LREAL   LREALvar;

   102 

   103     TIME    TIMEvar;

   104     TOD TODvar;

   105     DT  DTvar;

   106     DATE    DATEvar;

   107 } __IL_DEFVAR_T;

   108 

   109 

   110 

   111 

   112 /*****************/

   113 /* Misc internal */

   114 /*****************/

   115 

   116 /* function that generates an IEC runtime error */

   117 void IEC_error(void) {

   118   /* TODO... */

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

   120   /*exit(1);*/

   121 }

   122 

   123 

   124 static inline void __normalize_timespec (struct timespec *ts) {

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

   126     ts->tv_sec--;

   127     ts->tv_nsec += 1000000000;

   128   }

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

   130     ts->tv_sec++;

   131     ts->tv_nsec -= 1000000000;

   132   }

   133 }

   134 

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

   136   struct timespec ts;

   137   

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

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

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

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

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

   143 

   144   return ts;

   145 }

   146 

   147 

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

   149   struct timespec ts;

   150   

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

   152   ts.tv_sec = (long int)total_sec;

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

   154   

   155   return ts;

   156 }

   157 

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

   159   struct timespec ts;

   160   struct tm broken_down_time;

   161 

   162   broken_down_time.tm_sec = 0;

   163   broken_down_time.tm_min = 0;

   164   broken_down_time.tm_hour = 0;

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

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

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

   168 

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

   170 

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

   172     IEC_error();

   173 

   174   ts.tv_sec = epoch_seconds;

   175   ts.tv_nsec = 0;

   176   

   177   return ts;

   178 }

   179 

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

   181   struct timespec ts;

   182   

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

   184   ts.tv_sec = (long int)total_sec;

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

   186 

   187   struct tm broken_down_time;

   188   broken_down_time.tm_sec = 0;

   189   broken_down_time.tm_min = 0;

   190   broken_down_time.tm_hour = 0;

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

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

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

   194 

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

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

   197     IEC_error();

   198 

   199   ts.tv_sec += epoch_seconds;

   200   if (ts.tv_sec < epoch_seconds)

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

   202     IEC_error();

   203 

   204   return ts;

   205 }

   206 

   207 /***************/

   208 /*   Time ops  */

   209 /***************/

   210 inline TIME __date_and_time_to_time_of_day(TIME IN){

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

   212 }

   213 inline TIME __date_and_time_to_date(TIME IN){

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

   215 }

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

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

   218              IN1.tv_nsec + IN2.tv_nsec };

   219   __normalize_timespec(&res);

   220   return res;

   221 }

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

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

   224              IN1.tv_nsec - IN2.tv_nsec };

   225   __normalize_timespec(&res);

   226   return res;

   227 }

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

   229   LREAL s_f = IN1.tv_sec * IN2;

   230   time_t s = s_f;

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

   232   TIME res = {s + ns.quot,

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

   234   __normalize_timespec(&res);

   235   return res;

   236 }

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

   238   LREAL s_f = IN1.tv_sec / IN2;

   239   time_t s = s_f;

   240   TIME res = {s,

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

   242   __normalize_timespec(&res);

   243   return res;

   244 }

   245 

   246 /***************/

   247 /* String ops  */

   248 /***************/

   249 inline UINT __len(STRING IN){

   250     return IN.len;

   251 }

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

   253     STRING res = {0,};

   254     memcpy(&res.body, &IN.body, L < res.len ? L : res.len);

   255     return res;

   256 }

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

   258     STRING res = {0,};

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

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

   261     res.len = L;

   262     return res;

   263 }

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

   265     STRING res = {0,};

   266     if(P <= IN.len){

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

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

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

   270         res.len = L;

   271     }

   272     return res;

   273 }

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

   275   va_list ap;

   276   UINT i;

   277   __strlen_t charcount = 0;

   278   STRING res = {0,};

   279 

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

   281 

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

   283   {

   284     STRING tmp = va_arg(ap, STRING);

   285     __strlen_t charrem = STR_MAX_LEN - charcount;

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

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

   288     charcount += to_write;

   289   }

   290 

   291   res.len = charcount;

   292 

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

   294   return res;

   295 }

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

   297     STRING res = {0,};

   298     __strlen_t to_copy;

   299     

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

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

   302     P = res.len = to_copy;

   303     

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

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

   306     res.len += to_copy;

   307 

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

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

   310     res.len += to_copy;

   311 

   312     return res;

   313 }

   314 inline STRING __delete(STRING IN, SINT L_value, SINT P){

   315     STRING res = {0,};

   316     __strlen_t to_copy;

   317     

   318     to_copy = P > IN.len ? IN.len : P;

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

   320     P = res.len = to_copy;

   321 

   322     to_copy = IN.len - P;

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

   324     res.len += to_copy;

   325 

   326     return res;

   327 }

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

   329     STRING res = {0,};

   330     __strlen_t to_copy;

   331     

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

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

   334     P = res.len = to_copy;

   335     

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

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

   338     res.len += to_copy;

   339 

   340     to_copy = res.len < IN1.len ? IN1.len - res.len : 0;

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

   342     res.len += to_copy;

   343 

   344     return res;

   345 }

   346 

   347 

   348 

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

   350     UINT count1 = 0;

   351     UINT count2 = 0;

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

   353     {

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

   355             count1 += count2;

   356             count2 = 0;

   357         }        

   358     }

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

   360 }

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

   362     return __pfind(&IN1, &IN2);

   363 }

   364 

   365 /***************/

   366 /* Convertions */

   367 /***************/

   368     /***************/

   369     /*  TO_STRING  */

   370     /***************/

   371 inline STRING __bool_to_string(BOOL IN)

   372 {

   373     if(IN)

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

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

   376 }

   377 inline STRING __bit_to_string(LWORD IN){

   378     STRING res = {0,};

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

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

   381     return res;

   382 }

   383 inline STRING __real_to_string(LREAL IN){

   384     STRING res = {0,};

   385     res.len = snprintf(res.body, STR_MAX_LEN, "%g", IN);

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

   387     return res;

   388 }

   389 inline STRING __sint_to_string(LINT IN){

   390     STRING res = {0,};

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

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

   393     return res;

   394 }

   395 inline STRING __uint_to_string(ULINT IN){

   396     STRING res = {0,};

   397     res.len = snprintf(res.body, STR_MAX_LEN, "16#%llu", IN);

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

   399     return res;

   400 }

   401     /***************/

   402     /* FROM_STRING */

   403     /***************/

   404 inline BOOL __string_to_bool(STRING IN){

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

   406 }

   407 

   408 inline LINT __pstring_to_sint(STRING* IN){

   409     LINT res = 0;

   410     char tmp[STR_MAX_LEN];

   411     char tmp2[STR_MAX_LEN];

   412     __strlen_t l;

   413     unsigned int shift = 0;

   414     

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

   416         /* 2#0101_1010_1011_1111 */

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

   418         {

   419             char c = IN->body[l];

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

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

   422                 shift += 1;

   423             }

   424         }

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

   426         /* 8#1234_5665_4321 */

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

   428         {

   429             char c = IN->body[l];

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

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

   432                 shift += 3;

   433             }

   434         }

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

   436         /* 16#1234_5678_9abc_DEFG */

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

   438         {

   439             char c = IN->body[l];

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

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

   442                 shift += 4;

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

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

   445                 shift += 4;

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

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

   448                 shift += 4;

   449             }

   450         }

   451     }else{

   452         /* -123456789 */

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

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

   455         {

   456             char c = IN->body[l];

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

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

   459                 fac *= 10;

   460                 shift += 1;

   461             }

   462         }

   463     }

   464     return res;

   465 }

   466 

   467 inline LINT __string_to_sint(STRING IN){

   468     return (LWORD)__pstring_to_sint(&IN);

   469 }

   470 inline LWORD __string_to_bit(STRING IN){

   471     return (LWORD)__pstring_to_sint(&IN);

   472 }

   473 inline ULINT __string_to_uint(STRING IN){

   474     return (ULINT)__pstring_to_sint(&IN);

   475 }

   476 inline LREAL __string_to_real(STRING IN){

   477     /* search the dot */

   478     __strlen_t l = IN.len;

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

   480     if(l != 0){

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

   482     }else{

   483         return (LREAL)__pstring_to_sint(&IN);

   484     }    

   485 }

   486 

   487     /***************/

   488     /*   TO_TIME   */

   489     /***************/

   490 inline TIME __int_to_time(LINT IN){

   491     return (TIME){IN, 0};

   492 }

   493 

   494 inline TIME __real_to_time(LREAL IN){

   495     return (TIME){IN, (IN - (LINT)IN) * 1000000000};

   496 }

   497 inline TIME __string_to_time(STRING IN){

   498     /* TODO :

   499      *

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

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

   502      *                                        t#5d14h12m18s3.5ms

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

   504      *  Duration literals with underlines:

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

   506      *                long prefix             TIME#25h_15m

   507      *                                        time#5d_14h_12m_18s_3.5ms

   508      *

   509      *  Long prefix notation                 Short prefix notation

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

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

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

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

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

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

   516      *

   517      */

   518     /* Quick hack : only transform seconds */

   519     /* search the dot */

   520     __strlen_t l = IN.len;

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

   522     if(l != 0){

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

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

   525     }else{

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

   527     }

   528 }

   529 

   530     /***************/

   531     /*  FROM_TIME  */

   532     /***************/

   533 inline LREAL __time_to_real(TIME IN){

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

   535 }

   536 inline LINT __time_to_int(TIME IN){

   537     return IN.tv_sec;

   538 }

   539 inline STRING __time_to_string(TIME IN){

   540     /*t#5d14h12m18s3.5ms*/

   541     STRING res = {0,};

   542     div_t days = div(IN.tv_sec ,86400);

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

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

   545     }else{

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

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

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

   549         }else{

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

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

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

   553             }else{

   554                 if(IN.tv_nsec == 0){

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

   556                 }else{

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

   558                 }

   559             }

   560         }

   561     }

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

   563     return res;

   564 }

   565 inline STRING __date_to_string(DATE IN){

   566     /* D#1984-06-25 */

   567     STRING res = {0,};

   568     struct tm broken_down_time;

   569     time_t seconds = IN.tv_sec;

   570     if (NULL == gmtime_r(&seconds, &broken_down_time)){ /* get the UTC (GMT) broken down time */

   571         IEC_error();

   572         return (STRING){7,"D#ERROR"};

   573     }

   574     res.len = snprintf((char*)&res.body, STR_MAX_LEN, "D#%d-%2.2d-%2.2d", broken_down_time.tm_year, broken_down_time.tm_mon, broken_down_time.tm_mday);

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

   576     return res;

   577 }

   578 inline STRING __tod_to_string(TOD IN){

   579     /* TOD#15:36:55.36 */

   580     STRING res = {0,};

   581     struct tm broken_down_time;

   582     time_t seconds = IN.tv_sec;

   583     if (NULL == gmtime_r(&seconds, &broken_down_time)){ /* get the UTC (GMT) broken down time */

   584         IEC_error();

   585         return (STRING){9,"TOD#ERROR"};

   586     }

   587     if(IN.tv_nsec == 0){

   588         res.len = snprintf((char*)&res.body, STR_MAX_LEN, "TOD#%2.2d:%2.2d:%d", broken_down_time.tm_hour, broken_down_time.tm_min, broken_down_time.tm_sec);

   589     }else{

   590         res.len = snprintf((char*)&res.body, STR_MAX_LEN, "TOD#%2.2d:%2.2d:%g", broken_down_time.tm_hour, broken_down_time.tm_min, (LREAL)broken_down_time.tm_sec + (LREAL)IN.tv_nsec / 1000000);

   591     }

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

   593     return res;

   594 }

   595 inline STRING __dt_to_string(DT IN){

   596     /* DT#1984-06-25-15:36:55.36 */

   597     STRING res;

   598     struct tm broken_down_time;

   599     time_t seconds = IN.tv_sec;

   600     if (NULL == gmtime_r(&seconds, &broken_down_time)){ /* get the UTC (GMT) broken down time */

   601         IEC_error();

   602         return (STRING){8,"DT#ERROR"};

   603     }

   604     if(IN.tv_nsec == 0){

   605         res.len = snprintf((char*)&res.body, STR_MAX_LEN, "DT#%d-%2.2d-%2.2d-%2.2d:%2.2d:%d",

   606                  broken_down_time.tm_year,

   607                  broken_down_time.tm_mon,

   608                  broken_down_time.tm_mday,

   609                  broken_down_time.tm_hour,

   610                  broken_down_time.tm_min,

   611                  broken_down_time.tm_sec);

   612     }else{

   613         res.len = snprintf((char*)&res.body, STR_MAX_LEN, "DT#%d-%2.2d-%2.2d-%2.2d:%2.2d:%d",

   614                  broken_down_time.tm_year,

   615                  broken_down_time.tm_mon,

   616                  broken_down_time.tm_mday,

   617                  broken_down_time.tm_hour,

   618                  broken_down_time.tm_min,

   619                  (LREAL)broken_down_time.tm_sec + ((LREAL)IN.tv_nsec / 1000000));

   620     }

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

   622     return res;

   623 }

   624     /* BCD */

   625 inline ULINT __bcd_to_uint(LWORD IN){

   626     return IN & 0xf +

   627            ((IN >>= 4) & 0xf) * 10 + 

   628            ((IN >>= 4) & 0xf) * 100 + 

   629            ((IN >>= 4) & 0xf) * 1000 + 

   630            ((IN >>= 4) & 0xf) * 10000 + 

   631            ((IN >>= 4) & 0xf) * 100000 + 

   632            ((IN >>= 4) & 0xf) * 1000000 + 

   633            ((IN >>= 4) & 0xf) * 10000000 + 

   634            ((IN >>= 4) & 0xf) * 100000000 + 

   635            ((IN >>= 4) & 0xf) * 1000000000 + 

   636            ((IN >>= 4) & 0xf) * 10000000000 + 

   637            ((IN >>= 4) & 0xf) * 100000000000 + 

   638            ((IN >>= 4) & 0xf) * 1000000000000 + 

   639            ((IN >>= 4) & 0xf) * 10000000000000 + 

   640            ((IN >>= 4) & 0xf) * 100000000000000 + 

   641            ((IN >>= 4) & 0xf) * 1000000000000000;

   642 }

   643 inline LWORD __uint_to_bcd(ULINT IN){

   644     return (IN - (IN /= 10))|

   645            (IN - (IN /= 10)) << 4 |

   646            (IN - (IN /= 10)) << 8 |

   647            (IN - (IN /= 10)) << 12 |

   648            (IN - (IN /= 10)) << 16 |

   649            (IN - (IN /= 10)) << 20 |

   650            (IN - (IN /= 10)) << 24 |

   651            (IN - (IN /= 10)) << 28 |

   652            (IN - (IN /= 10)) << 32 |

   653            (IN - (IN /= 10)) << 36 |

   654            (IN - (IN /= 10)) << 40 |

   655            (IN - (IN /= 10)) << 44 |

   656            (IN - (IN /= 10)) << 48 |

   657            (IN - (IN /= 10)) << 52 |

   658            (IN - (IN /= 10)) << 56 |

   659            (IN - (IN /= 10)) << 60;

   660 }

   661 

   662 /**************/

   663 /* Binary ops */

   664 /**************/

   665 #define __ror_(TYPENAME)\

   666 inline TYPENAME __ror_##TYPENAME( TYPENAME IN, USINT N){\

   667  N %= 8*sizeof(TYPENAME);\

   668  return (IN >> N) | (IN << 8*sizeof(TYPENAME)-N);\

   669 }

   670 /* Call previously defined macro for each ANY_NBIT */

   671 ANY_NBIT(__ror_)

   672 

   673 #define __rol_(TYPENAME)\

   674 inline TYPENAME __rol_##TYPENAME( TYPENAME IN, USINT N){\

   675  N %= 8*sizeof(TYPENAME);\

   676  return (IN << N) | (IN >> 8*sizeof(TYPENAME)-N);\

   677 }

   678 /* Call previously defined macro for each ANY_NBIT */

   679 ANY_NBIT(__rol_)

   680 

   681 /**************/

   682 /* Selection  */

   683 /**************/

   684 	/**************/

   685 	/*   limit    */

   686 	/**************/

   687 

   688 #define __limit_(TYPENAME)\

   689 inline TYPENAME __limit_##TYPENAME( TYPENAME MN, TYPENAME IN, TYPENAME MX){\

   690  return IN > MN ? IN < MX ? IN : MX : MN;\

   691 }

   692 

   693 /* Call previously defined macro for each concerned type */

   694 ANY_NBIT(__limit_)

   695 ANY_NUM(__limit_)

   696 

   697 #define __limit_time(TYPENAME)\

   698 inline TIME __limit_##TYPENAME( TYPENAME MN, TYPENAME IN, TYPENAME MX){\

   699     return __TIME_CMP(IN, MN) > 0 ? /* IN>MN ?*/\

   700            __TIME_CMP(IN, MX) < 0 ? /* IN<MX ?*/\

   701            IN : MX : MN;\

   702 }

   703 

   704 /* Call previously defined macro for each concerned type */

   705 ANY_DATE(__limit_time)

   706 __limit_time(TIME)

   707 

   708 inline STRING __limit_STRING( STRING MN, STRING IN, STRING MX){

   709     return __STR_CMP(IN, MN) > 0 ? __STR_CMP(IN, MX) < 0 ? IN : MX : MN;

   710 }

   711 

   712     /**************/

   713     /*     MAX    */

   714     /**************/

   715     

   716 /* workaround for va-atgs limitation on shorter that int params */    

   717 #define VA_ARGS_REAL LREAL

   718 #define VA_ARGS_LREAL LREAL

   719 #define VA_ARGS_SINT DINT

   720 #define VA_ARGS_INT DINT

   721 #define VA_ARGS_DINT DINT

   722 #define VA_ARGS_LINT LINT

   723 #define VA_ARGS_USINT UDINT

   724 #define VA_ARGS_UINT UDINT

   725 #define VA_ARGS_UDINT UDINT

   726 #define VA_ARGS_ULINT ULINT

   727 #define VA_ARGS_TIME TIME

   728 #define VA_ARGS_BOOL DWORD

   729 #define VA_ARGS_BYTE DWORD

   730 #define VA_ARGS_WORD DWORD

   731 #define VA_ARGS_DWORD DWORD

   732 #define VA_ARGS_LWORD LWORD

   733 #define VA_ARGS_STRING STRING

   734 #define VA_ARGS_WSTRING WSTRING

   735 #define VA_ARGS_DATE DATE

   736 #define VA_ARGS_TOD TOD

   737 #define VA_ARGS_DT DT

   738 

   739 #define __extrem_(fname,TYPENAME, COND) \

   740 inline TYPENAME fname##TYPENAME( UINT param_count, TYPENAME op1, ...){\

   741   va_list ap;\

   742   UINT i;\

   743   \

   744   va_start (ap, op1);         /* Initialize the argument list.  */\

   745   \

   746   for (i = 0; i < param_count; i++){\

   747     TYPENAME tmp = va_arg (ap, VA_ARGS_##TYPENAME);\

   748     op1 = COND ? tmp : op1;\

   749   }\

   750   \

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

   752   return op1;\

   753 }

   754 

   755 #define __max_num(TYPENAME) __extrem_(__max_,TYPENAME, op1 < tmp)

   756 ANY_NBIT(__max_num)

   757 ANY_NUM(__max_num)

   758 

   759 __extrem_(__max_, STRING, __STR_CMP(op1,tmp) < 0)

   760 #define __max_time(TYPENAME) __extrem_(__max_, TYPENAME, __TIME_CMP(op1, tmp) < 0)

   761 

   762 /* Call previously defined macro for each concerned type */

   763 ANY_DATE(__max_time)

   764 __max_time(TIME)

   765 

   766     /**************/

   767     /*     MIN    */

   768     /**************/

   769 #define __min_num(TYPENAME) __extrem_(__min, TYPENAME, op1 > tmp)

   770 ANY_NBIT(__min_num)

   771 ANY_NUM(__min_num)

   772 

   773 __extrem_(__min, STRING, __STR_CMP(op1,tmp) > 0)

   774 

   775 #define __min_time(TYPENAME) __extrem_(__min_, TYPENAME, __TIME_CMP(op1, tmp) > 0)

   776 

   777 /* Call previously defined macro for each concerned type */

   778 ANY_DATE(__min_time)

   779 __min_time(TIME)

   780 

   781     /**************/

   782     /*     MUX    */

   783     /**************/

   784 #define __mux_(TYPENAME) \

   785 inline TYPENAME __mux_##TYPENAME( UINT param_count, UINT K, TYPENAME op1, ...){\

   786   va_list ap;\

   787   UINT i;\

   788   \

   789   va_start (ap, op1);         /* Initialize the argument list.  */\

   790   \

   791   for (i = 0; i < param_count; i++){\

   792     if(K == i){\

   793         TYPENAME tmp = va_arg (ap, VA_ARGS_##TYPENAME);\

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

   795         return tmp;\

   796     }else{\

   797         va_arg (ap, VA_ARGS_##TYPENAME);\

   798     }\

   799   }\

   800   \

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

   802   return op1;\

   803 }

   804 

   805 ANY(__mux_)

   806 

   807 /**************/

   808 /* Comparison */

   809 /**************/

   810 

   811 #define __compare_(fname,TYPENAME, COND) \

   812 inline BOOL fname##TYPENAME( UINT param_count, TYPENAME op1, ...){\

   813   va_list ap;\

   814   UINT i;\

   815   \

   816   va_start (ap, op1);         /* Initialize the argument list.  */\

   817   \

   818   for (i = 0; i < param_count; i++){\

   819     TYPENAME tmp = va_arg (ap, VA_ARGS_##TYPENAME);\

   820     if(COND){\

   821         op1 = tmp;\

   822     }else{\

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

   824         return 0;\

   825     }\

   826   }\

   827   \

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

   829   return 1;\

   830 }

   831 

   832 #define __compare_num(fname, TYPENAME, TEST) __compare_(fname, TYPENAME, op1 TEST tmp )

   833 #define __compare_time(fname, TYPENAME, TEST) __compare_(fname, TYPENAME, __TIME_CMP(op1, tmp) TEST 0)

   834 #define __compare_string(fname, TEST) __compare_(fname, STRING, __STR_CMP(op1, tmp) TEST 0 )

   835 

   836     /**************/

   837     /*     GT     */

   838     /**************/

   839 

   840 #define __gt_num(TYPENAME) __compare_num(__gt_, TYPENAME, > )

   841 ANY_NBIT(__gt_num)

   842 ANY_NUM(__gt_num)

   843 

   844 #define __gt_time(TYPENAME) __compare_time(__gt_, TYPENAME, > )

   845 ANY_DATE(__gt_time)

   846 __gt_time(TIME)

   847 

   848 __compare_string(__gt_, > )

   849 

   850     /**************/

   851     /*     GE     */

   852     /**************/

   853 

   854 #define __ge_num(TYPENAME) __compare_num(__ge_, TYPENAME, >= )

   855 ANY_NBIT(__ge_num)

   856 ANY_NUM(__ge_num)

   857 

   858 #define __ge_time(TYPENAME) __compare_time(__ge_, TYPENAME, >= )

   859 ANY_DATE(__ge_time)

   860 __ge_time(TIME)

   861 

   862 __compare_string(__ge_, >=)

   863 

   864     /**************/

   865     /*     EQ     */

   866     /**************/

   867 

   868 #define __eq_num(TYPENAME) __compare_num(__eq_, TYPENAME, == )

   869 ANY_NBIT(__eq_num)

   870 ANY_NUM(__eq_num)

   871 

   872 #define __eq_time(TYPENAME) __compare_time(__eq_, TYPENAME, == )

   873 ANY_DATE(__eq_time)

   874 __eq_time(TIME)

   875 

   876 __compare_string(__eq_, == )

   877 

   878     /**************/

   879     /*     LT     */

   880     /**************/

   881 

   882 #define __lt_num(TYPENAME) __compare_num(__lt_, TYPENAME, < )

   883 ANY_NBIT(__lt_num)

   884 ANY_NUM(__lt_num)

   885 

   886 #define __lt_time(TYPENAME) __compare_time(__lt_, TYPENAME, < )

   887 ANY_DATE(__lt_time)

   888 __lt_time(TIME)

   889 

   890 __compare_string(__lt_, < )

   891 

   892     /**************/

   893     /*     LE     */

   894     /**************/

   895 

   896 #define __le_num(TYPENAME) __compare_num(__le_, TYPENAME, <= )

   897 ANY_NBIT(__le_num)

   898 ANY_NUM(__le_num)

   899 

   900 #define __le_time(TYPENAME) __compare_time(__le_, TYPENAME, <= )

   901 ANY_DATE(__le_time)

   902 __le_time(TIME)

   903 

   904 __compare_string(__le_, <= )

   905 

   906     /**************/

   907     /*     NE     */

   908     /**************/

   909 

   910 #define __ne_num(TYPENAME) __compare_num(__ne_, TYPENAME, != )

   911 ANY_NBIT(__ne_num)

   912 ANY_NUM(__ne_num)

   913 

   914 #define __ne_time(TYPENAME) __compare_time(__ne_, TYPENAME, != )

   915 ANY_DATE(__ne_time)

   916 __ne_time(TIME)

   917 

   918 __compare_string(__ne_, != )

   919 

   920