etisserant@107: /* msousa@350: * copyright 2008 Edouard TISSERANT msousa@350: * copyright 2011 Mario de Sousa (msousa@fe.up.pt) etisserant@107: * etisserant@107: * Offered to the public under the terms of the GNU Lesser General Public etisserant@107: * License as published by the Free Software Foundation; either version 2 etisserant@107: * of the License, or (at your option) any later version. etisserant@107: * etisserant@107: * This program is distributed in the hope that it will be useful, but etisserant@107: * WITHOUT ANY WARRANTY; without even the implied warranty of etisserant@107: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser etisserant@107: * General Public License for more details. etisserant@107: * etisserant@107: * This code is made available on the understanding that it will not be etisserant@107: * used in safety-critical situations without a full and competent review. etisserant@107: */ etisserant@107: etisserant@40: /**** etisserant@107: * IEC 61131-3 standard function library etisserant@40: */ etisserant@40: msousa@350: /* NOTE: This file is full of (what may seem at first) very strange macros. msousa@350: * If you want to know what all these strange macros are doing, msousa@350: * just parse this file through a C preprocessor (e.g. cpp), msousa@350: * and analyse the output! msousa@350: * $gcc -E iec_std_lib.h msousa@350: */ msousa@350: msousa@739: #ifndef _IEC_STD_LIB_H msousa@739: #define _IEC_STD_LIB_H msousa@739: msousa@739: etisserant@40: #include etisserant@40: #include etisserant@40: #include etisserant@57: #include Laurent@707: #include etisserant@40: etisserant@41: #include etisserant@40: #include etisserant@40: #include etisserant@40: #include etisserant@40: etisserant@43: #ifdef DEBUG_IEC etisserant@43: #define DBG(...) printf(__VA_ARGS__); etisserant@43: #define DBG_TYPE(TYPENAME, name) __print_##TYPENAME(name); etisserant@43: #else etisserant@43: #define DBG(...) etisserant@43: #define DBG_TYPE(TYPENAME, name) etisserant@43: #endif etisserant@43: etisserant@137: /* etisserant@137: * Include type defs. etisserant@137: */ etisserant@137: #include "iec_types_all.h" etisserant@40: etisserant@41: extern TIME __CURRENT_TIME; etisserant@140: extern BOOL __DEBUG; etisserant@40: etisserant@40: /* TODO etisserant@40: typedef struct { etisserant@40: __strlen_t len; etisserant@40: u_int16_t body[STR_MAX_LEN]; etisserant@40: } WSTRING; etisserant@40: */ msousa@350: /* etisserant@57: # if __WORDSIZE == 64 etisserant@57: #define __32b_sufix etisserant@57: #define __64b_sufix L etisserant@57: #else etisserant@57: #define __32b_sufix L greg@180: #define __64b_sufix LL greg@180: #endif msousa@350: */ msousa@350: msousa@350: # if __WORDSIZE == 64 msousa@350: #define __32b_sufix msousa@350: #define __64b_sufix L msousa@350: #else msousa@350: #define __32b_sufix L msousa@350: /* changed this from LL to L temporarily. It was causing a bug when compiling resulting code with gcc. msousa@350: * I have other things to worry about at the moment.. msousa@350: */ msousa@350: #define __64b_sufix L msousa@350: #endif msousa@350: etisserant@57: conti@582: #define __lit(type,value,...) (type)value##__VA_ARGS__ conti@582: // Keep this macro expention step to let sfx(__VA_ARGS__) change into L or LL Laurent@638: #define __literal(type,value,...) __lit(type,value,__VA_ARGS__) conti@582: conti@582: #define __BOOL_LITERAL(value) __literal(BOOL,value) conti@582: #define __SINT_LITERAL(value) __literal(SINT,value) conti@582: #define __INT_LITERAL(value) __literal(INT,value) etisserant@57: #define __DINT_LITERAL(value) __literal(DINT,value,__32b_sufix) etisserant@57: #define __LINT_LITERAL(value) __literal(LINT,value,__64b_sufix) conti@582: #define __USINT_LITERAL(value) __literal(USINT,value) conti@582: #define __UINT_LITERAL(value) __literal(UINT,value) etisserant@57: #define __UDINT_LITERAL(value) __literal(UDINT,value,__32b_sufix) etisserant@57: #define __ULINT_LITERAL(value) __literal(ULINT,value,__64b_sufix) etisserant@57: #define __REAL_LITERAL(value) __literal(REAL,value,__32b_sufix) etisserant@57: #define __LREAL_LITERAL(value) __literal(LREAL,value,__64b_sufix) conti@582: #define __TIME_LITERAL(value) __literal(TIME,value) conti@582: #define __DATE_LITERAL(value) __literal(DATE,value) conti@582: #define __TOD_LITERAL(value) __literal(TOD,value) conti@582: #define __DT_LITERAL(value) __literal(DT,value) Laurent@625: #define __STRING_LITERAL(count,value) (STRING){count,value} conti@582: #define __BYTE_LITERAL(value) __literal(BYTE,value) conti@582: #define __WORD_LITERAL(value) __literal(WORD,value) etisserant@57: #define __DWORD_LITERAL(value) __literal(DWORD,value,__32b_sufix) msousa@350: #define __LWORD_LITERAL(value) __literal(LWORD,value,__64b_sufix) etisserant@55: etisserant@55: etisserant@40: typedef union __IL_DEFVAR_T { etisserant@40: BOOL BOOLvar; etisserant@40: etisserant@40: SINT SINTvar; etisserant@40: INT INTvar; etisserant@40: DINT DINTvar; etisserant@40: LINT LINTvar; etisserant@40: etisserant@40: USINT USINTvar; etisserant@40: UINT UINTvar; etisserant@40: UDINT UDINTvar; etisserant@40: ULINT ULINTvar; etisserant@40: etisserant@40: BYTE BYTEvar; etisserant@40: WORD WORDvar; etisserant@40: DWORD DWORDvar; etisserant@40: LWORD LWORDvar; etisserant@40: etisserant@40: REAL REALvar; etisserant@40: LREAL LREALvar; etisserant@40: etisserant@40: TIME TIMEvar; etisserant@40: TOD TODvar; etisserant@40: DT DTvar; etisserant@40: DATE DATEvar; etisserant@40: } __IL_DEFVAR_T; etisserant@40: msousa@350: msousa@350: /**********************************************************************/ msousa@350: /**********************************************************************/ msousa@350: /***** *****/ msousa@350: /***** Some helper functions... *****/ msousa@350: /***** ...used later: *****/ msousa@350: /***** - when declaring the IEC 61131-3 standard functions *****/ msousa@350: /***** - in the C source code itself in SFC and ST expressions *****/ msousa@350: /***** *****/ msousa@350: /**********************************************************************/ msousa@350: /**********************************************************************/ msousa@350: msousa@350: msousa@350: /****************************/ msousa@350: /* Notify IEC runtime error */ msousa@350: /****************************/ etisserant@40: etisserant@40: /* function that generates an IEC runtime error */ msousa@350: static inline void __iec_error(void) { etisserant@40: /* TODO... */ etisserant@40: fprintf(stderr, "IEC 61131-3 runtime error.\n"); etisserant@40: /*exit(1);*/ etisserant@40: } etisserant@40: msousa@350: /*******************************/ msousa@350: /* Time normalization function */ msousa@350: /*******************************/ etisserant@40: Edouard@254: static inline void __normalize_timespec (IEC_TIMESPEC *ts) { etisserant@40: if( ts->tv_nsec < -1000000000 || (( ts->tv_sec > 0 ) && ( ts->tv_nsec < 0 ))){ etisserant@40: ts->tv_sec--; etisserant@40: ts->tv_nsec += 1000000000; etisserant@40: } etisserant@40: if( ts->tv_nsec > 1000000000 || (( ts->tv_sec < 0 ) && ( ts->tv_nsec > 0 ))){ etisserant@40: ts->tv_sec++; etisserant@40: ts->tv_nsec -= 1000000000; etisserant@40: } etisserant@40: } etisserant@40: msousa@350: /**********************************************/ msousa@350: /* Time conversion to/from timespec functions */ msousa@350: /**********************************************/ mjsousa@900: /* NOTE: The following function was turned into a macro, so it could be used to initialize the initial value of TIME variables. mjsousa@900: * Since each macro parameter is evaluated several times, the macro may result in multiple function invocations if an expression mjsousa@900: * containing a function invocation is passed as a parameter. However, currently matiec only uses this conversion macro with mjsousa@900: * constant literals, so it is safe to change it into a macro. mjsousa@900: */ mjsousa@900: /* NOTE: I (Mario - msousa@fe.up.pt) believe that the following function contains a bug when handling negative times. mjsousa@900: * The equivalent macro has this bug fixed. mjsousa@900: * e.g.; mjsousa@900: * T#3.8s mjsousa@900: * using the function, will result in a timespec of 3.8s !!!: mjsousa@900: * tv_sec = 4 <----- 1 * 3.8 is rounded up when converting a double to an int! mjsousa@900: * tv_nsec = -200 000 000 <----- 1 * (3.8 - 4)*1e9 mjsousa@900: * mjsousa@900: * -T#3.8s mjsousa@900: * using the function, will result in a timespec of -11.8s !!!: mjsousa@900: * tv_sec = -4 <----- -1 * 3.8 is rounded down when converting a double to an int! mjsousa@900: * tv_nsec = -7 800 000 000 <----- -1 * (3.8 - -4)*1e9 mjsousa@900: */ mjsousa@900: /* NOTE: Due to the fact that the C compiler may round a tv_sec number away from zero, mjsousa@900: * the following macro may result in a timespec that is not normalized, i.e. with a tv_sec > 0, and a tv_nsec < 0 !!!! mjsousa@900: * This is due to the rounding that C compiler applies when converting a (long double) to a (long int). mjsousa@900: * To produce normalized timespec's we need to use floor(), but we cannot call any library functions since we want this macro to be mjsousa@900: * useable as a variable initializer. mjsousa@900: * VAR x : TIME = T#3.5h; END_VAR ---> IEC_TIME x = __time_to_timespec(1, 0, 0, 0, 3.5, 0); mjsousa@900: */ mjsousa@900: /* Edouard@254: static inline IEC_TIMESPEC __time_to_timespec(int sign, double mseconds, double seconds, double minutes, double hours, double days) { Edouard@254: IEC_TIMESPEC ts; greg@180: mjsousa@900: // sign is 1 for positive values, -1 for negative time... etisserant@40: long double total_sec = ((days*24 + hours)*60 + minutes)*60 + seconds + mseconds/1e3; etisserant@40: if (sign >= 0) sign = 1; else sign = -1; etisserant@40: ts.tv_sec = sign * (long int)total_sec; etisserant@40: ts.tv_nsec = sign * (long int)((total_sec - ts.tv_sec)*1e9); etisserant@40: etisserant@40: return ts; etisserant@40: } mjsousa@900: */ mjsousa@900: /* NOTE: Unfortunately older versions of ANSI C (e.g. C99) do not allow explicit identification of elements in initializers mjsousa@900: * e.g. {tv_sec = 1, tv_nsec = 300} mjsousa@900: * They are therefore commented out. This however means that any change to the definition of IEC_TIMESPEC may require this mjsousa@900: * macro to be updated too! mjsousa@900: */ mjsousa@900: #define __time_to_timespec(sign,mseconds,seconds,minutes,hours,days) \ mjsousa@900: ((IEC_TIMESPEC){\ mjsousa@903: /*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))), \ mjsousa@900: /*tv_nsec =*/ ((long int)(( \ mjsousa@903: ((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))) - \ mjsousa@903: ((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))) \ mjsousa@900: )*1e9))\ mjsousa@900: }) mjsousa@900: mjsousa@900: mjsousa@900: mjsousa@900: mjsousa@900: /* 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 */ mjsousa@900: /* 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! mjsousa@900: /* Edouard@254: static inline IEC_TIMESPEC __tod_to_timespec(double seconds, double minutes, double hours) { Edouard@254: IEC_TIMESPEC ts; greg@180: etisserant@40: long double total_sec = (hours*60 + minutes)*60 + seconds; etisserant@40: ts.tv_sec = (long int)total_sec; etisserant@40: ts.tv_nsec = (long int)((total_sec - ts.tv_sec)*1e9); greg@180: etisserant@40: return ts; etisserant@40: } mjsousa@900: */ mjsousa@900: #define __tod_to_timespec(seconds,minutes,hours) \ mjsousa@900: ((IEC_TIMESPEC){\ mjsousa@903: /*tv_sec =*/ ((long int) ((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds)), \ mjsousa@900: /*tv_nsec =*/ ((long int)(( \ mjsousa@903: ((long double)((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds)) - \ mjsousa@903: ((long int) ((((long double)hours)*60 + (long double)minutes)*60 + (long double)seconds)) \ mjsousa@900: )*1e9))\ mjsousa@900: }) mjsousa@900: etisserant@40: Laurent@711: #define EPOCH_YEAR 1970 Laurent@714: #define SECONDS_PER_MINUTE 60 Laurent@714: #define SECONDS_PER_HOUR (60 * SECONDS_PER_MINUTE) Laurent@711: #define SECONDS_PER_DAY (24 * SECONDS_PER_HOUR) Laurent@711: #define __isleap(year) \ Laurent@711: ((year) % 4 == 0 && ((year) % 100 != 0 || (year) % 400 == 0)) Laurent@711: static const unsigned short int __mon_yday[2][13] = Laurent@711: { Laurent@711: /* Normal years. */ Laurent@711: { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365}, Laurent@711: /* Leap years. */ Laurent@711: { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366} Laurent@711: }; Laurent@711: Laurent@711: typedef struct { Laurent@711: int tm_sec; /* Seconds. [0-60] (1 leap second) */ Laurent@711: int tm_min; /* Minutes. [0-59] */ mjsousa@900: int tm_hour; /* Hours. [0-23] */ Laurent@711: int tm_day; /* Day. [1-31] */ Laurent@711: int tm_mon; /* Month. [0-11] */ mjsousa@900: int tm_year; /* Year */ Laurent@711: } tm; Laurent@711: Laurent@711: static inline tm convert_seconds_to_date_and_time(long int seconds) { Laurent@711: tm dt; Laurent@711: long int days, rem; Laurent@711: days = seconds / SECONDS_PER_DAY; Laurent@711: rem = seconds % SECONDS_PER_DAY; Laurent@711: if (rem < 0) { Laurent@711: rem += SECONDS_PER_DAY; Laurent@711: days--; Laurent@711: } Laurent@711: Laurent@711: // time of day Laurent@711: dt.tm_hour = rem / SECONDS_PER_HOUR; Laurent@711: rem %= SECONDS_PER_HOUR; Laurent@711: dt.tm_min = rem / 60; Laurent@711: dt.tm_sec = rem % 60; Laurent@711: Laurent@711: // date Laurent@711: dt.tm_year = EPOCH_YEAR; Laurent@711: while (days >= (rem = __isleap(dt.tm_year) ? 366 : 365)) { Laurent@711: dt.tm_year++; Laurent@711: days -= rem; Laurent@711: } Laurent@711: while (days < 0) { Laurent@711: dt.tm_year--; Laurent@711: days += __isleap(dt.tm_year) ? 366 : 365; Laurent@711: } Laurent@711: dt.tm_mon = 1; Laurent@711: while (days > __mon_yday[__isleap(dt.tm_year)][dt.tm_mon]) { Laurent@711: dt.tm_mon += 1; Laurent@711: } Laurent@711: dt.tm_day = days - __mon_yday[__isleap(dt.tm_year)][dt.tm_mon - 1] + 1; Laurent@711: Laurent@711: return dt; Laurent@711: } Edouard@709: Edouard@254: static inline IEC_TIMESPEC __date_to_timespec(int day, int month, int year) { Edouard@254: IEC_TIMESPEC ts; Laurent@711: int a4, b4, a100, b100, a400, b400; Laurent@711: int yday; Laurent@711: int intervening_leap_days; Laurent@711: Laurent@711: if (month < 1 || month > 12) Laurent@711: __iec_error(); Laurent@711: Laurent@711: yday = __mon_yday[__isleap(year)][month - 1] + day; Laurent@711: Laurent@711: if (yday > __mon_yday[__isleap(year)][month]) Laurent@711: __iec_error(); Laurent@711: Laurent@711: a4 = (year >> 2) - ! (year & 3); Laurent@711: b4 = (EPOCH_YEAR >> 2) - ! (EPOCH_YEAR & 3); Laurent@711: a100 = a4 / 25 - (a4 % 25 < 0); Laurent@711: b100 = b4 / 25 - (b4 % 25 < 0); Laurent@711: a400 = a100 >> 2; Laurent@711: b400 = b100 >> 2; Laurent@711: intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400); Laurent@705: Laurent@711: ts.tv_sec = ((year - EPOCH_YEAR) * 365 + intervening_leap_days + yday - 1) * 24 * 60 * 60; etisserant@40: ts.tv_nsec = 0; greg@180: etisserant@40: return ts; etisserant@40: } etisserant@40: Laurent@705: static inline IEC_TIMESPEC __dt_to_timespec(double seconds, double minutes, double hours, int day, int month, int year) { Laurent@705: IEC_TIMESPEC ts_date = __date_to_timespec(day, month, year); Laurent@705: IEC_TIMESPEC ts = __tod_to_timespec(seconds, minutes, hours); Laurent@705: Laurent@705: ts.tv_sec += ts_date.tv_sec; etisserant@40: etisserant@40: return ts; etisserant@40: } etisserant@40: msousa@350: /*******************/ msousa@350: /* Time operations */ msousa@350: /*******************/ msousa@350: msousa@350: #define __time_cmp(t1, t2) (t2.tv_sec == t1.tv_sec ? t1.tv_nsec - t2.tv_nsec : t1.tv_sec - t2.tv_sec) msousa@350: msousa@350: static inline TIME __time_add(TIME IN1, TIME IN2){ etisserant@40: TIME res ={IN1.tv_sec + IN2.tv_sec, etisserant@40: IN1.tv_nsec + IN2.tv_nsec }; etisserant@40: __normalize_timespec(&res); etisserant@40: return res; etisserant@40: } msousa@350: static inline TIME __time_sub(TIME IN1, TIME IN2){ etisserant@40: TIME res ={IN1.tv_sec - IN2.tv_sec, etisserant@40: IN1.tv_nsec - IN2.tv_nsec }; etisserant@40: __normalize_timespec(&res); etisserant@40: return res; etisserant@40: } msousa@350: static inline TIME __time_mul(TIME IN1, LREAL IN2){ etisserant@40: LREAL s_f = IN1.tv_sec * IN2; Edouard@639: time_t s = (time_t)s_f; conti@700: div_t ns = div((int)((LREAL)IN1.tv_nsec * IN2), 1000000000); Edouard@639: TIME res = {(long)s + ns.quot, Edouard@639: (long)ns.rem + (s_f - s) * 1000000000 }; etisserant@40: __normalize_timespec(&res); etisserant@40: return res; etisserant@40: } msousa@350: static inline TIME __time_div(TIME IN1, LREAL IN2){ etisserant@40: LREAL s_f = IN1.tv_sec / IN2; Edouard@639: time_t s = (time_t)s_f; Edouard@639: TIME res = {(long)s, Edouard@639: (long)(IN1.tv_nsec / IN2 + (s_f - s) * 1000000000) }; etisserant@40: __normalize_timespec(&res); etisserant@40: return res; etisserant@40: } etisserant@40: etisserant@40: etisserant@40: /***************/ etisserant@40: /* Convertions */ etisserant@40: /***************/ laurent@314: /*****************/ laurent@314: /* REAL_TO_INT */ laurent@314: /*****************/ msousa@350: static inline LINT __real_round(LREAL IN) { laurent@314: return fmod(IN, 1) == 0 ? ((LINT)IN / 2) * 2 : (LINT)IN; laurent@314: } msousa@350: static inline LINT __preal_to_sint(LREAL IN) { laurent@314: return IN >= 0 ? __real_round(IN + 0.5) : __real_round(IN - 0.5); laurent@314: } msousa@350: static inline LINT __preal_to_uint(LREAL IN) { laurent@314: return IN >= 0 ? __real_round(IN + 0.5) : 0; laurent@314: } msousa@350: static inline LINT __real_to_sint(LREAL IN) {return (LINT)__preal_to_sint(IN);} msousa@350: static inline LWORD __real_to_bit(LREAL IN) {return (LWORD)__preal_to_uint(IN);} msousa@350: static inline ULINT __real_to_uint(LREAL IN) {return (ULINT)__preal_to_uint(IN);} msousa@350: etisserant@40: /***************/ etisserant@40: /* TO_STRING */ etisserant@40: /***************/ msousa@350: static inline STRING __bool_to_string(BOOL IN) { msousa@350: if(IN) return (STRING){4, "TRUE"}; etisserant@40: return (STRING){5,"FALSE"}; etisserant@40: } msousa@350: static inline STRING __bit_to_string(LWORD IN) { laurent@200: STRING res; laurent@200: res = __INIT_STRING; Edouard@389: res.len = snprintf((char*)res.body, STR_MAX_LEN, "16#%llx",(long long unsigned int)IN); etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __real_to_string(LREAL IN) { laurent@200: STRING res; laurent@200: res = __INIT_STRING; etisserant@114: res.len = snprintf((char*)res.body, STR_MAX_LEN, "%.10g", IN); etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __sint_to_string(LINT IN) { laurent@200: STRING res; laurent@200: res = __INIT_STRING; Edouard@389: res.len = snprintf((char*)res.body, STR_MAX_LEN, "%lld", (long long int)IN); etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __uint_to_string(ULINT IN) { laurent@200: STRING res; laurent@200: res = __INIT_STRING; Edouard@389: res.len = snprintf((char*)res.body, STR_MAX_LEN, "%llu", (long long unsigned int)IN); etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } etisserant@40: /***************/ etisserant@40: /* FROM_STRING */ etisserant@40: /***************/ msousa@350: static inline BOOL __string_to_bool(STRING IN) { Laurent@705: int i; Laurent@705: if (IN.len == 1) return !memcmp(&IN.body,"1", IN.len); Laurent@705: for (i = 0; i < IN.len; i++) IN.body[i] = toupper(IN.body[i]); Laurent@705: return IN.len == 4 ? !memcmp(&IN.body,"TRUE", IN.len) : 0; etisserant@40: } etisserant@40: msousa@350: static inline LINT __pstring_to_sint(STRING* IN) { etisserant@40: LINT res = 0; etisserant@40: __strlen_t l; etisserant@40: unsigned int shift = 0; greg@180: etisserant@40: if(IN->body[0]=='2' && IN->body[1]=='#'){ etisserant@40: /* 2#0101_1010_1011_1111 */ etisserant@40: for(l = IN->len - 1; l >= 2 && shift < 64; l--) etisserant@40: { etisserant@40: char c = IN->body[l]; etisserant@40: if( c >= '0' && c <= '1'){ etisserant@40: res |= ( c - '0') << shift; etisserant@40: shift += 1; etisserant@40: } etisserant@40: } etisserant@40: }else if(IN->body[0]=='8' && IN->body[1]=='#'){ etisserant@40: /* 8#1234_5665_4321 */ etisserant@40: for(l = IN->len - 1; l >= 2 && shift < 64; l--) etisserant@40: { etisserant@40: char c = IN->body[l]; etisserant@40: if( c >= '0' && c <= '7'){ etisserant@40: res |= ( c - '0') << shift; etisserant@40: shift += 3; etisserant@40: } etisserant@40: } etisserant@43: }else if(IN->body[0]=='1' && IN->body[1]=='6' && IN->body[2]=='#'){ etisserant@40: /* 16#1234_5678_9abc_DEFG */ etisserant@40: for(l = IN->len - 1; l >= 3 && shift < 64; l--) etisserant@40: { etisserant@40: char c = IN->body[l]; etisserant@40: if( c >= '0' && c <= '9'){ etisserant@43: res |= (LWORD)( c - '0') << shift; etisserant@40: shift += 4; etisserant@40: }else if( c >= 'a' && c <= 'f'){ etisserant@43: res |= (LWORD)( c - 'a' + 10 ) << shift; etisserant@40: shift += 4; etisserant@40: }else if( c >= 'A' && c <= 'F'){ etisserant@43: res |= (LWORD)( c - 'A' + 10 ) << shift; etisserant@40: shift += 4; etisserant@40: } etisserant@40: } etisserant@40: }else{ etisserant@40: /* -123456789 */ etisserant@40: LINT fac = IN->body[0] == '-' ? -1 : 1; etisserant@40: for(l = IN->len - 1; l >= 0 && shift < 20; l--) etisserant@40: { etisserant@40: char c = IN->body[l]; etisserant@40: if( c >= '0' && c <= '9'){ etisserant@40: res += ( c - '0') * fac; etisserant@40: fac *= 10; etisserant@40: shift += 1; etisserant@41: }else if( c >= '.' ){ /* reset value */ etisserant@41: res = 0; etisserant@163: fac = IN->body[0] == '-' ? -1 : 1; etisserant@41: shift = 0; greg@180: } etisserant@40: } etisserant@40: } etisserant@40: return res; etisserant@40: } etisserant@40: msousa@350: static inline LINT __string_to_sint(STRING IN) {return (LINT)__pstring_to_sint(&IN);} msousa@350: static inline LWORD __string_to_bit (STRING IN) {return (LWORD)__pstring_to_sint(&IN);} msousa@350: static inline ULINT __string_to_uint(STRING IN) {return (ULINT)__pstring_to_sint(&IN);} msousa@350: static inline LREAL __string_to_real(STRING IN) { msousa@350: __strlen_t l; msousa@350: l = IN.len; etisserant@40: /* search the dot */ etisserant@40: while(--l > 0 && IN.body[l] != '.'); etisserant@40: if(l != 0){ etisserant@40: return atof((const char *)&IN.body); etisserant@40: }else{ etisserant@40: return (LREAL)__pstring_to_sint(&IN); greg@180: } etisserant@40: } etisserant@40: etisserant@40: /***************/ etisserant@40: /* TO_TIME */ etisserant@40: /***************/ msousa@350: static inline TIME __int_to_time(LINT IN) {return (TIME){IN, 0};} msousa@350: static inline TIME __real_to_time(LREAL IN) {return (TIME){IN, (IN - (LINT)IN) * 1000000000};} msousa@350: static inline TIME __string_to_time(STRING IN){ msousa@350: __strlen_t l; etisserant@40: /* TODO : etisserant@40: * etisserant@40: * Duration literals without underlines: T#14ms T#-14ms T#14.7s T#14.7m etisserant@40: * short prefix T#14.7h t#14.7d t#25h15m etisserant@40: * t#5d14h12m18s3.5ms etisserant@40: * long prefix TIME#14ms TIME#-14ms time#14.7s etisserant@40: * Duration literals with underlines: etisserant@40: * short prefix t#25h_15m t#5d_14h_12m_18s_3.5ms etisserant@40: * long prefix TIME#25h_15m etisserant@40: * time#5d_14h_12m_18s_3.5ms etisserant@40: * etisserant@40: * Long prefix notation Short prefix notation etisserant@40: * DATE#1984-06-25 D#1984-06-25 etisserant@40: * date#1984-06-25 d#1984-06-25 etisserant@40: * TIME_OF_DAY#15:36:55.36 TOD#15:36:55.36 etisserant@40: * time_of_day#15:36:55.36 tod#15:36:55.36 etisserant@40: * DATE_AND_TIME#1984-06-25-15:36:55.36 DT#1984-06-25-15:36:55.36 etisserant@40: * date_and_time#1984-06-25-15:36:55.36 dt#1984-06-25-15:36:55.36 etisserant@40: * etisserant@40: */ etisserant@40: /* Quick hack : only transform seconds */ etisserant@40: /* search the dot */ msousa@350: l = IN.len; etisserant@40: while(--l > 0 && IN.body[l] != '.'); etisserant@40: if(l != 0){ etisserant@40: LREAL IN_val = atof((const char *)&IN.body); Edouard@639: return (TIME){(long)IN_val, (long)(IN_val - (LINT)IN_val)*1000000000}; etisserant@40: }else{ Edouard@639: return (TIME){(long)__pstring_to_sint(&IN), 0}; etisserant@40: } etisserant@40: } etisserant@40: etisserant@40: /***************/ etisserant@40: /* FROM_TIME */ etisserant@40: /***************/ msousa@350: static inline LREAL __time_to_real(TIME IN){ etisserant@40: return (LREAL)IN.tv_sec + ((LREAL)IN.tv_nsec/1000000000); etisserant@40: } msousa@350: static inline LINT __time_to_int(TIME IN) {return IN.tv_sec;} msousa@350: static inline STRING __time_to_string(TIME IN){ laurent@200: STRING res; laurent@200: div_t days; etisserant@40: /*t#5d14h12m18s3.5ms*/ laurent@200: res = __INIT_STRING; Laurent@714: days = div(IN.tv_sec, SECONDS_PER_DAY); etisserant@40: if(!days.rem && IN.tv_nsec == 0){ etisserant@40: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd", days.quot); etisserant@40: }else{ Laurent@714: div_t hours = div(days.rem, SECONDS_PER_HOUR); etisserant@40: if(!hours.rem && IN.tv_nsec == 0){ etisserant@40: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh", days.quot, hours.quot); etisserant@40: }else{ Laurent@714: div_t minuts = div(hours.rem, SECONDS_PER_MINUTE); etisserant@40: if(!minuts.rem && IN.tv_nsec == 0){ etisserant@40: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh%dm", days.quot, hours.quot, minuts.quot); etisserant@40: }else{ etisserant@40: if(IN.tv_nsec == 0){ etisserant@40: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh%dm%ds", days.quot, hours.quot, minuts.quot, minuts.rem); etisserant@40: }else{ etisserant@42: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "T#%dd%dh%dm%ds%gms", days.quot, hours.quot, minuts.quot, minuts.rem, (LREAL)IN.tv_nsec / 1000000); etisserant@40: } etisserant@40: } etisserant@40: } etisserant@40: } etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __date_to_string(DATE IN){ laurent@200: STRING res; Laurent@711: tm broken_down_time; etisserant@40: /* D#1984-06-25 */ Laurent@711: broken_down_time = convert_seconds_to_date_and_time(IN.tv_sec); laurent@200: res = __INIT_STRING; Laurent@711: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "D#%d-%2.2d-%2.2d", Laurent@711: broken_down_time.tm_year, Laurent@711: broken_down_time.tm_mon, Laurent@711: broken_down_time.tm_day); etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __tod_to_string(TOD IN){ laurent@200: STRING res; Laurent@711: tm broken_down_time; laurent@200: time_t seconds; etisserant@40: /* TOD#15:36:55.36 */ Laurent@711: seconds = IN.tv_sec; Laurent@711: if (seconds >= SECONDS_PER_DAY){ Laurent@711: __iec_error(); Laurent@711: return (STRING){9,"TOD#ERROR"}; Laurent@711: } Laurent@711: broken_down_time = convert_seconds_to_date_and_time(seconds); laurent@200: res = __INIT_STRING; etisserant@40: if(IN.tv_nsec == 0){ Laurent@711: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "TOD#%2.2d:%2.2d:%2.2d", Laurent@711: broken_down_time.tm_hour, Laurent@711: broken_down_time.tm_min, Laurent@711: broken_down_time.tm_sec); etisserant@40: }else{ Laurent@803: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "TOD#%2.2d:%2.2d:%09.6f", Laurent@711: broken_down_time.tm_hour, Laurent@711: broken_down_time.tm_min, Laurent@711: (LREAL)broken_down_time.tm_sec + (LREAL)IN.tv_nsec / 1e9); etisserant@40: } etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: static inline STRING __dt_to_string(DT IN){ laurent@200: STRING res; Laurent@711: tm broken_down_time; etisserant@40: /* DT#1984-06-25-15:36:55.36 */ Laurent@711: broken_down_time = convert_seconds_to_date_and_time(IN.tv_sec); etisserant@40: if(IN.tv_nsec == 0){ Laurent@705: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "DT#%d-%2.2d-%2.2d-%2.2d:%2.2d:%2.2d", Laurent@711: broken_down_time.tm_year, Laurent@711: broken_down_time.tm_mon, Laurent@711: broken_down_time.tm_day, Laurent@711: broken_down_time.tm_hour, Laurent@711: broken_down_time.tm_min, Laurent@711: broken_down_time.tm_sec); etisserant@40: }else{ Laurent@803: res.len = snprintf((char*)&res.body, STR_MAX_LEN, "DT#%d-%2.2d-%2.2d-%2.2d:%2.2d:%09.6f", Laurent@711: broken_down_time.tm_year, Laurent@711: broken_down_time.tm_mon, Laurent@711: broken_down_time.tm_day, Laurent@711: broken_down_time.tm_hour, Laurent@711: broken_down_time.tm_min, Laurent@711: (LREAL)broken_down_time.tm_sec + ((LREAL)IN.tv_nsec / 1e9)); etisserant@40: } etisserant@40: if(res.len > STR_MAX_LEN) res.len = STR_MAX_LEN; etisserant@40: return res; etisserant@40: } msousa@350: msousa@350: /**********************************************/ msousa@350: /* [ANY_DATE | TIME] _TO_ [ANY_DATE | TIME] */ msousa@350: /**********************************************/ msousa@350: Laurent@714: static inline TOD __date_and_time_to_time_of_day(DT IN) { Laurent@714: return (TOD){ Laurent@714: IN.tv_sec % SECONDS_PER_DAY + (IN.tv_sec < 0 ? SECONDS_PER_DAY : 0), Laurent@714: IN.tv_nsec}; Laurent@714: } Laurent@714: static inline DATE __date_and_time_to_date(DT IN){ Laurent@714: return (DATE){ Laurent@714: IN.tv_sec - IN.tv_sec % SECONDS_PER_DAY - (IN.tv_sec < 0 ? SECONDS_PER_DAY : 0), Laurent@714: 0}; Laurent@714: } msousa@350: msousa@350: /*****************/ msousa@350: /* FROM/TO BCD */ msousa@350: /*****************/ Laurent@715: Laurent@715: static inline BOOL __test_bcd(LWORD IN) { Laurent@715: while (IN) { Laurent@715: if ((IN & 0xf) > 9) return 1; Laurent@715: IN >>= 4; Laurent@715: } Laurent@715: return 0; Laurent@715: } Laurent@715: msousa@350: static inline ULINT __bcd_to_uint(LWORD IN){ Laurent@715: ULINT res = IN & 0xf; Laurent@715: ULINT factor = 10ULL; Laurent@715: Laurent@715: while (IN >>= 4) { Laurent@715: res += (IN & 0xf) * factor; Laurent@715: factor *= 10; greg@212: } greg@212: return res; greg@212: } greg@212: msousa@350: static inline LWORD __uint_to_bcd(ULINT IN){ Laurent@715: LWORD res = IN % 10; Laurent@715: USINT shift = 4; Laurent@715: Laurent@715: while (IN /= 10) { Laurent@715: res |= (IN % 10) << shift; Laurent@715: shift += 4; greg@212: } greg@212: return res; etisserant@40: } etisserant@40: msousa@350: msousa@350: /************/ msousa@350: /* MOVE_* */ msousa@350: /************/ msousa@350: msousa@350: /* some helpful __move_[ANY] functions, used in the *_TO_** and MOVE standard functions */ msousa@350: /* e.g. __move_BOOL, __move_BYTE, __move_REAL, __move_TIME, ... */ laurent@393: #define __move_(TYPENAME)\ msousa@350: static inline TYPENAME __move_##TYPENAME(TYPENAME op1) {return op1;} laurent@393: __ANY(__move_) msousa@350: msousa@350: msousa@350: mjsousa@905: mjsousa@905: mjsousa@905: mjsousa@905: mjsousa@905: mjsousa@905: #include "iec_std_functions.h" mjsousa@905: #include "iec_std_FB.h" msousa@739: msousa@739: #endif /* _IEC_STD_LIB_H */