diff -r 2d03056993f6 -r cd8dadcef426 targets/plc_main_tail.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/targets/plc_main_tail.c Fri Mar 15 17:47:53 2013 +0900 @@ -0,0 +1,220 @@ +/** + * Tail of code common to all C targets + **/ + +/** + * LOGGING + **/ + +#define LOG_LEVELS 4 +#define LOG_CRITICAL 0 +#define LOG_WARNING 1 +#define LOG_INFO 2 +#define LOG_DEBUG 3 + +#ifndef LOG_BUFFER_SIZE +#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/ +#endif +#ifndef LOG_BUFFER_ATTRS +#define LOG_BUFFER_ATTRS +#endif + +#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1) + +static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS; +void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ + if(buffpos + size < LOG_BUFFER_SIZE){ + memcpy(&LogBuff[level][buffpos], buf, size); + }else{ + uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; + memcpy(&LogBuff[level][buffpos], buf, remaining); + memcpy(LogBuff[level], buf + remaining, size - remaining); + } +} +void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ + if(buffpos + size < LOG_BUFFER_SIZE){ + memcpy(buf, &LogBuff[level][buffpos], size); + }else{ + uint32_t remaining = LOG_BUFFER_SIZE - buffpos; + memcpy(buf, &LogBuff[level][buffpos], remaining); + memcpy(buf + remaining, LogBuff[level], size - remaining); + } +} + +/* Log buffer structure + + |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|... + | Message1 Body | Tail1 | Message2 Body | Tail2 | + +*/ +typedef struct { + uint32_t msgidx; + uint32_t msgsize; + unsigned long tick; + IEC_TIME time; +} mTail; + +/* Log cursor : 64b + |63 ... 32|31 ... 0| + | Message | Buffer | + | counter | Index | */ +static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0}; + +/* Store one log message of give size */ +int LogMessage(uint8_t level, uint8_t* buf, uint32_t size){ + if(size < LOG_BUFFER_SIZE - sizeof(mTail)){ + uint32_t buffpos; + uint64_t new_cursor, old_cursor; + + mTail tail; + tail.msgsize = size; + tail.tick = __tick; + PLC_GetTime(&tail.time); + + /* We cannot increment both msg index and string pointer + in a single atomic operation but we can detect having been interrupted. + So we can try with atomic compare and swap in a loop until operation + succeeds non interrupted */ + do{ + old_cursor = LogCursor[level]; + buffpos = (uint32_t)old_cursor; + tail.msgidx = (old_cursor >> 32); + new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) + | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK); + }while(AtomicCompareExchange64( + (long long*)&LogCursor[level], + (long long)old_cursor, + (long long)new_cursor)!=old_cursor); + + copy_to_log(level, buffpos, buf, size); + copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail)); + + return 1; /* Success */ + }else{ + uint8_t mstr[] = "Logging error : message too big"; + LogMessage(LOG_CRITICAL, mstr, sizeof(mstr)); + } + return 0; +} + +uint32_t GetLogCount(uint8_t level){ + return (uint64_t)LogCursor[level] >> 32; +} + +/* Return message size and content */ +uint32_t GetLogMessage(uint8_t level, uint32_t msgidx, char* buf, uint32_t max_size, uint32_t* tick, uint32_t* tv_sec, uint32_t* tv_nsec){ + uint64_t cursor = LogCursor[level]; + if(cursor){ + /* seach cursor */ + uint32_t stailpos = (uint32_t)cursor; + uint32_t smsgidx; + mTail tail; + tail.msgidx = cursor >> 32; + tail.msgsize = 0; + + /* Message search loop */ + do { + smsgidx = tail.msgidx; + stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK; + copy_from_log(level, stailpos, &tail, sizeof(mTail)); + }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx)); + + if(tail.msgidx == msgidx){ + uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; + uint32_t totalsize = tail.msgsize; + *tick = tail.tick; + *tv_sec = tail.time.tv_sec; + *tv_nsec = tail.time.tv_nsec; + copy_from_log(level, sbuffpos, buf, + totalsize > max_size ? max_size : totalsize); + return totalsize; + } + } + return 0; +} + +#define CALIBRATED -2 +#define NOT_CALIBRATED -1 +static int calibration_count = NOT_CALIBRATED; +static IEC_TIME cal_begin; +static long long Tsync = 0; +static long long FreqCorr = 0; +static int Nticks = 0; +static unsigned long last_tick = 0; + +/* + * Called on each external periodic sync event + * make PLC tick synchronous with external sync + * ratio defines when PLC tick occurs between two external sync + * @param sync_align_ratio + * 0->100 : align ratio + * < 0 : no align, calibrate period + **/ +void align_tick(int sync_align_ratio) +{ + /* + printf("align_tick(%d)\n", calibrate); + */ + if(sync_align_ratio < 0){ /* Calibration */ + if(calibration_count == CALIBRATED) + /* Re-calibration*/ + calibration_count = NOT_CALIBRATED; + if(calibration_count == NOT_CALIBRATED) + /* Calibration start, get time*/ + PLC_GetTime(&cal_begin); + calibration_count++; + }else{ /* do alignment (if possible) */ + if(calibration_count >= 0){ + /* End of calibration */ + /* Get final time */ + IEC_TIME cal_end; + PLC_GetTime(&cal_end); + /*adjust calibration_count*/ + calibration_count++; + /* compute mean of Tsync, over calibration period */ + Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + + (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; + if( (Nticks = (Tsync / Ttick)) > 0){ + FreqCorr = (Tsync % Ttick); /* to be divided by Nticks */ + }else{ + FreqCorr = Tsync - (Ttick % Tsync); + } + /* + printf("Tsync = %ld\n", Tsync); + printf("calibration_count = %d\n", calibration_count); + printf("Nticks = %d\n", Nticks); + */ + calibration_count = CALIBRATED; + } + if(calibration_count == CALIBRATED){ + /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ + IEC_TIME now; + long long elapsed; + long long Tcorr; + long long PhaseCorr; + long long PeriodicTcorr; + PLC_GetTime(&now); + elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; + if(Nticks > 0){ + PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ + Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; + if(Nticks < 2){ + /* When Sync source period is near Tick time */ + /* PhaseCorr may not be applied to Periodic time given to timer */ + PeriodicTcorr = Ttick + FreqCorr / Nticks; + }else{ + PeriodicTcorr = Tcorr; + } + }else if(__tick > last_tick){ + last_tick = __tick; + PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); + PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; + }else{ + /*PLC did not run meanwhile. Nothing to do*/ + return; + } + /* DO ALIGNEMENT */ + PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); + } + } +}