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