author | Edouard Tisserant |
Mon, 28 Oct 2019 19:52:43 +0100 | |
branch | svghmi |
changeset 2808 | dc78ffa5253d |
parent 1800 | 1711339585ce |
permissions | -rw-r--r-- |
/** * Tail of code common to all C targets **/ /** * LOGGING **/ #ifndef TARGET_LOGGING_DISABLE #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; static 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; memcpy(&LogBuff[level][buffpos], buf, remaining); memcpy(LogBuff[level], (char*)buf + remaining, size - remaining); } } static 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((char*)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] LOG_BUFFER_ATTRS = {0x0,0x0,0x0,0x0}; void ResetLogCount(void) { uint8_t level; for(level=0;level<LOG_LEVELS;level++){ LogCursor[level] = 0; } } /* Store one log message of give size */ int LogMessage(uint8_t level, char* 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)!=(long long)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{ char 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; } #endif #ifndef TARGET_EXT_SYNC_DISABLE #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 / common_ticktime__)) > 0){ FreqCorr = (Tsync % common_ticktime__); /* to be divided by Nticks */ }else{ FreqCorr = Tsync - (common_ticktime__ % 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 - (common_ticktime__ + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ Tcorr = common_ticktime__ + (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 = common_ticktime__ + FreqCorr / Nticks; }else{ PeriodicTcorr = Tcorr; } }else if(__tick > last_tick){ last_tick = __tick; PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); PeriodicTcorr = Tcorr = common_ticktime__ + PhaseCorr + FreqCorr; }else{ /*PLC did not run meanwhile. Nothing to do*/ return; } /* DO ALIGNEMENT */ PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); } } } #endif