--- /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);
+ }
+ }
+}