|
1 /** |
|
2 * Tail of code common to all C targets |
|
3 **/ |
|
4 |
|
5 /** |
|
6 * LOGGING |
|
7 **/ |
|
8 |
|
9 #define LOG_LEVELS 4 |
|
10 #define LOG_CRITICAL 0 |
|
11 #define LOG_WARNING 1 |
|
12 #define LOG_INFO 2 |
|
13 #define LOG_DEBUG 3 |
|
14 |
|
15 #ifndef LOG_BUFFER_SIZE |
|
16 #define LOG_BUFFER_SIZE (1<<14) /*16Ko*/ |
|
17 #endif |
|
18 #ifndef LOG_BUFFER_ATTRS |
|
19 #define LOG_BUFFER_ATTRS |
|
20 #endif |
|
21 |
|
22 #define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1) |
|
23 |
|
24 static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS; |
|
25 void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ |
|
26 if(buffpos + size < LOG_BUFFER_SIZE){ |
|
27 memcpy(&LogBuff[level][buffpos], buf, size); |
|
28 }else{ |
|
29 uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; |
|
30 memcpy(&LogBuff[level][buffpos], buf, remaining); |
|
31 memcpy(LogBuff[level], buf + remaining, size - remaining); |
|
32 } |
|
33 } |
|
34 void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ |
|
35 if(buffpos + size < LOG_BUFFER_SIZE){ |
|
36 memcpy(buf, &LogBuff[level][buffpos], size); |
|
37 }else{ |
|
38 uint32_t remaining = LOG_BUFFER_SIZE - buffpos; |
|
39 memcpy(buf, &LogBuff[level][buffpos], remaining); |
|
40 memcpy(buf + remaining, LogBuff[level], size - remaining); |
|
41 } |
|
42 } |
|
43 |
|
44 /* Log buffer structure |
|
45 |
|
46 |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|... |
|
47 | Message1 Body | Tail1 | Message2 Body | Tail2 | |
|
48 |
|
49 */ |
|
50 typedef struct { |
|
51 uint32_t msgidx; |
|
52 uint32_t msgsize; |
|
53 unsigned long tick; |
|
54 IEC_TIME time; |
|
55 } mTail; |
|
56 |
|
57 /* Log cursor : 64b |
|
58 |63 ... 32|31 ... 0| |
|
59 | Message | Buffer | |
|
60 | counter | Index | */ |
|
61 static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0}; |
|
62 |
|
63 /* Store one log message of give size */ |
|
64 int LogMessage(uint8_t level, uint8_t* buf, uint32_t size){ |
|
65 if(size < LOG_BUFFER_SIZE - sizeof(mTail)){ |
|
66 uint32_t buffpos; |
|
67 uint64_t new_cursor, old_cursor; |
|
68 |
|
69 mTail tail; |
|
70 tail.msgsize = size; |
|
71 tail.tick = __tick; |
|
72 PLC_GetTime(&tail.time); |
|
73 |
|
74 /* We cannot increment both msg index and string pointer |
|
75 in a single atomic operation but we can detect having been interrupted. |
|
76 So we can try with atomic compare and swap in a loop until operation |
|
77 succeeds non interrupted */ |
|
78 do{ |
|
79 old_cursor = LogCursor[level]; |
|
80 buffpos = (uint32_t)old_cursor; |
|
81 tail.msgidx = (old_cursor >> 32); |
|
82 new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) |
|
83 | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK); |
|
84 }while(AtomicCompareExchange64( |
|
85 (long long*)&LogCursor[level], |
|
86 (long long)old_cursor, |
|
87 (long long)new_cursor)!=old_cursor); |
|
88 |
|
89 copy_to_log(level, buffpos, buf, size); |
|
90 copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail)); |
|
91 |
|
92 return 1; /* Success */ |
|
93 }else{ |
|
94 uint8_t mstr[] = "Logging error : message too big"; |
|
95 LogMessage(LOG_CRITICAL, mstr, sizeof(mstr)); |
|
96 } |
|
97 return 0; |
|
98 } |
|
99 |
|
100 uint32_t GetLogCount(uint8_t level){ |
|
101 return (uint64_t)LogCursor[level] >> 32; |
|
102 } |
|
103 |
|
104 /* Return message size and content */ |
|
105 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){ |
|
106 uint64_t cursor = LogCursor[level]; |
|
107 if(cursor){ |
|
108 /* seach cursor */ |
|
109 uint32_t stailpos = (uint32_t)cursor; |
|
110 uint32_t smsgidx; |
|
111 mTail tail; |
|
112 tail.msgidx = cursor >> 32; |
|
113 tail.msgsize = 0; |
|
114 |
|
115 /* Message search loop */ |
|
116 do { |
|
117 smsgidx = tail.msgidx; |
|
118 stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK; |
|
119 copy_from_log(level, stailpos, &tail, sizeof(mTail)); |
|
120 }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx)); |
|
121 |
|
122 if(tail.msgidx == msgidx){ |
|
123 uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; |
|
124 uint32_t totalsize = tail.msgsize; |
|
125 *tick = tail.tick; |
|
126 *tv_sec = tail.time.tv_sec; |
|
127 *tv_nsec = tail.time.tv_nsec; |
|
128 copy_from_log(level, sbuffpos, buf, |
|
129 totalsize > max_size ? max_size : totalsize); |
|
130 return totalsize; |
|
131 } |
|
132 } |
|
133 return 0; |
|
134 } |
|
135 |
|
136 #define CALIBRATED -2 |
|
137 #define NOT_CALIBRATED -1 |
|
138 static int calibration_count = NOT_CALIBRATED; |
|
139 static IEC_TIME cal_begin; |
|
140 static long long Tsync = 0; |
|
141 static long long FreqCorr = 0; |
|
142 static int Nticks = 0; |
|
143 static unsigned long last_tick = 0; |
|
144 |
|
145 /* |
|
146 * Called on each external periodic sync event |
|
147 * make PLC tick synchronous with external sync |
|
148 * ratio defines when PLC tick occurs between two external sync |
|
149 * @param sync_align_ratio |
|
150 * 0->100 : align ratio |
|
151 * < 0 : no align, calibrate period |
|
152 **/ |
|
153 void align_tick(int sync_align_ratio) |
|
154 { |
|
155 /* |
|
156 printf("align_tick(%d)\n", calibrate); |
|
157 */ |
|
158 if(sync_align_ratio < 0){ /* Calibration */ |
|
159 if(calibration_count == CALIBRATED) |
|
160 /* Re-calibration*/ |
|
161 calibration_count = NOT_CALIBRATED; |
|
162 if(calibration_count == NOT_CALIBRATED) |
|
163 /* Calibration start, get time*/ |
|
164 PLC_GetTime(&cal_begin); |
|
165 calibration_count++; |
|
166 }else{ /* do alignment (if possible) */ |
|
167 if(calibration_count >= 0){ |
|
168 /* End of calibration */ |
|
169 /* Get final time */ |
|
170 IEC_TIME cal_end; |
|
171 PLC_GetTime(&cal_end); |
|
172 /*adjust calibration_count*/ |
|
173 calibration_count++; |
|
174 /* compute mean of Tsync, over calibration period */ |
|
175 Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + |
|
176 (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; |
|
177 if( (Nticks = (Tsync / Ttick)) > 0){ |
|
178 FreqCorr = (Tsync % Ttick); /* to be divided by Nticks */ |
|
179 }else{ |
|
180 FreqCorr = Tsync - (Ttick % Tsync); |
|
181 } |
|
182 /* |
|
183 printf("Tsync = %ld\n", Tsync); |
|
184 printf("calibration_count = %d\n", calibration_count); |
|
185 printf("Nticks = %d\n", Nticks); |
|
186 */ |
|
187 calibration_count = CALIBRATED; |
|
188 } |
|
189 if(calibration_count == CALIBRATED){ |
|
190 /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ |
|
191 IEC_TIME now; |
|
192 long long elapsed; |
|
193 long long Tcorr; |
|
194 long long PhaseCorr; |
|
195 long long PeriodicTcorr; |
|
196 PLC_GetTime(&now); |
|
197 elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; |
|
198 if(Nticks > 0){ |
|
199 PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ |
|
200 Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; |
|
201 if(Nticks < 2){ |
|
202 /* When Sync source period is near Tick time */ |
|
203 /* PhaseCorr may not be applied to Periodic time given to timer */ |
|
204 PeriodicTcorr = Ttick + FreqCorr / Nticks; |
|
205 }else{ |
|
206 PeriodicTcorr = Tcorr; |
|
207 } |
|
208 }else if(__tick > last_tick){ |
|
209 last_tick = __tick; |
|
210 PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); |
|
211 PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; |
|
212 }else{ |
|
213 /*PLC did not run meanwhile. Nothing to do*/ |
|
214 return; |
|
215 } |
|
216 /* DO ALIGNEMENT */ |
|
217 PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); |
|
218 } |
|
219 } |
|
220 } |