1 /*
2 * CANopen trace interface.
3 *
4 * @file CO_trace.c
5 * @author Janez Paternoster
6 * @copyright 2016 - 2020 Janez Paternoster
7 *
8 * This file is part of CANopenNode, an opensource CANopen Stack.
9 * Project home page is <https://github.com/CANopenNode/CANopenNode>.
10 * For more information on CANopen see <http://www.can-cia.org/>.
11 *
12 * Licensed under the Apache License, Version 2.0 (the "License");
13 * you may not use this file except in compliance with the License.
14 * You may obtain a copy of the License at
15 *
16 * http://www.apache.org/licenses/LICENSE-2.0
17 *
18 * Unless required by applicable law or agreed to in writing, software
19 * distributed under the License is distributed on an "AS IS" BASIS,
20 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
21 * See the License for the specific language governing permissions and
22 * limitations under the License.
23 */
24
25
26 #include "CO_trace.h"
27 #include <stdio.h>
28 #include <inttypes.h>
29
30 /* Different functions for processing value for different data types. */
getValueI8(void * OD_variable)31 static int32_t getValueI8 (void *OD_variable) { return (int32_t) *((int8_t*) OD_variable);}
getValueI16(void * OD_variable)32 static int32_t getValueI16(void *OD_variable) { return (int32_t) *((int16_t*) OD_variable);}
getValueI32(void * OD_variable)33 static int32_t getValueI32(void *OD_variable) { return *((int32_t*) OD_variable);}
getValueU8(void * OD_variable)34 static int32_t getValueU8 (void *OD_variable) { return (int32_t) *((uint8_t*) OD_variable);}
getValueU16(void * OD_variable)35 static int32_t getValueU16(void *OD_variable) { return (int32_t) *((uint16_t*) OD_variable);}
getValueU32(void * OD_variable)36 static int32_t getValueU32(void *OD_variable) { return *((int32_t*) OD_variable);}
37
38
39 /* Different functions for printing points for different data types. */
printPointCsv(char * s,uint32_t size,uint32_t timeStamp,int32_t value)40 static uint32_t printPointCsv(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
41 return snprintf(s, size, "%" PRIu32 ";%" PRId32 "\n", timeStamp, value);
42 }
printPointCsvUnsigned(char * s,uint32_t size,uint32_t timeStamp,int32_t value)43 static uint32_t printPointCsvUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
44 return snprintf(s, size, "%" PRIu32 ";%" PRIu32 "\n", timeStamp, (uint32_t) value);
45 }
printPointBinary(char * s,uint32_t size,uint32_t timeStamp,int32_t value)46 static uint32_t printPointBinary(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
47 if(size < 8) return 0;
48 CO_memcpySwap4(s, &timeStamp);
49 CO_memcpySwap4(s+4, &value);
50 return 8;
51 }
printPointSvgStart(char * s,uint32_t size,uint32_t timeStamp,int32_t value)52 static uint32_t printPointSvgStart(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
53 return snprintf(s, size, "M%" PRIu32 ",%" PRId32, timeStamp, value);
54 }
printPointSvgStartUnsigned(char * s,uint32_t size,uint32_t timeStamp,int32_t value)55 static uint32_t printPointSvgStartUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
56 return snprintf(s, size, "M%" PRIu32 ",%" PRIu32, timeStamp, (uint32_t) value);
57 }
printPointSvg(char * s,uint32_t size,uint32_t timeStamp,int32_t value)58 static uint32_t printPointSvg(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
59 return snprintf(s, size, "H%" PRIu32 "V%" PRId32, timeStamp, value);
60 }
printPointSvgUnsigned(char * s,uint32_t size,uint32_t timeStamp,int32_t value)61 static uint32_t printPointSvgUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
62 return snprintf(s, size, "H%" PRIu32 "V%" PRIu32, timeStamp, (uint32_t) value);
63 }
64
65
66 /* Collection of function pointers for fast processing based on specific data type. */
67 /* Rules for the array: There must be groups of six members (I8, I16, I32, U8, U16, U32)
68 * in correct order and sequence, so findVariable() finds correct member. */
69 static const CO_trace_dataType_t dataTypes[] = {
70 {getValueI8, printPointCsv, printPointCsv, printPointCsv},
71 {getValueI16, printPointCsv, printPointCsv, printPointCsv},
72 {getValueI32, printPointCsv, printPointCsv, printPointCsv},
73 {getValueU8, printPointCsvUnsigned, printPointCsvUnsigned, printPointCsvUnsigned},
74 {getValueU16, printPointCsvUnsigned, printPointCsvUnsigned, printPointCsvUnsigned},
75 {getValueU32, printPointCsvUnsigned, printPointCsvUnsigned, printPointCsvUnsigned},
76 {getValueI8, printPointBinary, printPointBinary, printPointBinary},
77 {getValueI16, printPointBinary, printPointBinary, printPointBinary},
78 {getValueI32, printPointBinary, printPointBinary, printPointBinary},
79 {getValueU8, printPointBinary, printPointBinary, printPointBinary},
80 {getValueU16, printPointBinary, printPointBinary, printPointBinary},
81 {getValueU32, printPointBinary, printPointBinary, printPointBinary},
82 {getValueI8, printPointSvgStart, printPointSvg, printPointSvg},
83 {getValueI16, printPointSvgStart, printPointSvg, printPointSvg},
84 {getValueI32, printPointSvgStart, printPointSvg, printPointSvg},
85 {getValueU8, printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned},
86 {getValueU16, printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned},
87 {getValueU32, printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned}
88 };
89
90
91 /* Find variable in Object Dictionary *****************************************/
findVariable(CO_trace_t * trace)92 static void findVariable(CO_trace_t *trace) {
93 bool_t err = false;
94 uint16_t index;
95 uint8_t subIndex;
96 uint8_t dataLen;
97 void *OdDataPtr = NULL;
98 unsigned dtIndex = 0;
99
100 /* parse mapping */
101 index = (uint16_t) ((*trace->map) >> 16);
102 subIndex = (uint8_t) ((*trace->map) >> 8);
103 dataLen = (uint8_t) (*trace->map);
104 if((dataLen & 0x07) != 0) { /* data length must be byte aligned */
105 err = true;
106 }
107 dataLen >>= 3; /* in bytes now */
108 if(dataLen == 0) {
109 dataLen = 4;
110 }
111
112 /* find mapped variable, if map available */
113 if(!err && (index != 0 || subIndex != 0)) {
114 uint16_t entryNo = CO_OD_find(trace->SDO, index);
115
116 if(index >= 0x1000 && entryNo != 0xFFFF && subIndex <= trace->SDO->OD[entryNo].maxSubIndex) {
117 OdDataPtr = CO_OD_getDataPointer(trace->SDO, entryNo, subIndex);
118 }
119
120 if(OdDataPtr != NULL) {
121 uint16_t len = CO_OD_getLength(trace->SDO, entryNo, subIndex);
122
123 if(len < dataLen) {
124 dataLen = len;
125 }
126 }
127 else {
128 err = true;
129 }
130 }
131
132 /* Get function pointers for correct data type */
133 if(!err) {
134 /* first sequence: data length */
135 switch(dataLen) {
136 case 1: dtIndex = 0; break;
137 case 2: dtIndex = 1; break;
138 case 4: dtIndex = 2; break;
139 default: err = true; break;
140 }
141 /* second sequence: signed or unsigned */
142 if(((*trace->format) & 1) == 1) {
143 dtIndex += 3;
144 }
145 /* third sequence: Output type */
146 dtIndex += ((*trace->format) >> 1) * 6;
147
148 if(dtIndex > (sizeof(dataTypes) / sizeof(CO_trace_dataType_t))) {
149 err = true;
150 }
151 }
152
153 /* set output variables */
154 if(!err) {
155 if(OdDataPtr != NULL) {
156 trace->OD_variable = OdDataPtr;
157 }
158 else {
159 trace->OD_variable = trace->value;
160 }
161 trace->dt = &dataTypes[dtIndex];
162 }
163 else {
164 trace->OD_variable = NULL;
165 trace->dt = NULL;
166 }
167 }
168
169
170 /* OD function for accessing _OD_traceConfig_ (index 0x2300+) from SDO server.
171 * For more information see file CO_SDO.h. */
CO_ODF_traceConfig(CO_ODF_arg_t * ODF_arg)172 static CO_SDO_abortCode_t CO_ODF_traceConfig(CO_ODF_arg_t *ODF_arg) {
173 CO_trace_t *trace;
174 CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
175
176 trace = (CO_trace_t*) ODF_arg->object;
177
178 switch(ODF_arg->subIndex) {
179 case 1: /* size */
180 if(ODF_arg->reading) {
181 CO_setUint32(ODF_arg->data, trace->bufferSize);
182 }
183 break;
184
185 case 2: /* axisNo (trace enabled if nonzero) */
186 if(ODF_arg->reading) {
187 uint8_t *value = (uint8_t*) ODF_arg->data;
188 if(!trace->enabled) {
189 *value = 0;
190 }
191 }
192 else {
193 uint8_t *value = (uint8_t*) ODF_arg->data;
194
195 if(*value == 0) {
196 trace->enabled = false;
197 }
198 else if(!trace->enabled) {
199 if(trace->bufferSize == 0) {
200 ret = CO_SDO_AB_OUT_OF_MEM;
201 }
202 else {
203 /* set trace->OD_variable and trace->dt, based on 'map' and 'format' */
204 findVariable(trace);
205
206 if(trace->OD_variable != NULL) {
207 *trace->value = 0;
208 *trace->minValue = 0;
209 *trace->maxValue = 0;
210 *trace->triggerTime = 0;
211 trace->valuePrev = 0;
212 trace->readPtr = 0;
213 trace->writePtr = 0;
214 trace->enabled = true;
215 }
216 else {
217 ret = CO_SDO_AB_NO_MAP;
218 }
219 }
220 }
221 }
222 break;
223
224 case 5: /* map */
225 case 6: /* format */
226 if(!ODF_arg->reading) {
227 if(trace->enabled) {
228 ret = CO_SDO_AB_INVALID_VALUE;
229 }
230 }
231 break;
232 }
233
234 return ret;
235 }
236
237
238 /* OD function for accessing _OD_trace_ (index 0x2400+) from SDO server.
239 * For more information see file CO_SDO.h. */
CO_ODF_trace(CO_ODF_arg_t * ODF_arg)240 static CO_SDO_abortCode_t CO_ODF_trace(CO_ODF_arg_t *ODF_arg) {
241 CO_trace_t *trace;
242 CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
243
244 trace = (CO_trace_t*) ODF_arg->object;
245
246 switch(ODF_arg->subIndex) {
247 case 1: /* size */
248 if(ODF_arg->reading) {
249 uint32_t size = trace->bufferSize;
250 uint32_t wp = trace->writePtr;
251 uint32_t rp = trace->readPtr;
252
253 if(wp >= rp) {
254 CO_setUint32(ODF_arg->data, wp - rp);
255 }
256 else {
257 CO_setUint32(ODF_arg->data, size - rp + wp);
258 }
259 }
260 else {
261 if(CO_getUint32(ODF_arg->data) == 0) {
262 /* clear buffer, handle race conditions */
263 while(trace->readPtr != 0 || trace->writePtr != 0) {
264 trace->readPtr = 0;
265 trace->writePtr = 0;
266 *trace->triggerTime = 0;
267 }
268 }
269 else {
270 ret = CO_SDO_AB_INVALID_VALUE;
271 }
272 }
273 break;
274
275 case 5: /* plot */
276 if(ODF_arg->reading) {
277 /* This plot will be transmitted as domain data type. String data
278 * will be printed directly to SDO buffer. If there is more data
279 * to print, than is the size of SDO buffer, then this function
280 * will be called multiple times until internal trace buffer is
281 * empty. Internal trace buffer is circular buffer. It is accessed
282 * by this function and by higher priority thread. If this buffer
283 * is full, there is a danger for race condition. First records
284 * from trace buffer may be overwritten somewhere between. If this
285 * is detected, then do{}while() loop tries printing again. */
286 if(trace->bufferSize == 0 || ODF_arg->dataLength < 100) {
287 ret = CO_SDO_AB_OUT_OF_MEM;
288 }
289 else if(trace->readPtr == trace->writePtr) {
290 ret = CO_SDO_AB_NO_DATA;
291 }
292 else {
293 uint32_t rp, t, v, len, freeLen;
294 char *s;
295 bool_t readPtrOverflowed; /* for handling race conditions */
296
297 /* repeat everything, if trace->readPtr was overflowed in CO_trace_process */
298 do {
299 readPtrOverflowed = false;
300 s = (char*) ODF_arg->data;
301 freeLen = ODF_arg->dataLength;
302
303 rp = trace->readPtr;
304
305 /* start plot, increment variables, verify overflow */
306 if(ODF_arg->firstSegment) {
307 t = trace->timeBuffer[rp];
308 v = trace->valueBuffer[rp];
309 rp ++;
310 if(++trace->readPtr == trace->bufferSize) {
311 trace->readPtr = 0;
312 if(rp != trace->bufferSize) {
313 readPtrOverflowed = true;
314 continue;
315 }
316 rp = 0;
317 }
318 if(rp != trace->readPtr) {
319 readPtrOverflowed = true;
320 continue;
321 }
322 len = trace->dt->printPointStart(s, freeLen, t, v);
323 s += len;
324 freeLen -= len;
325 }
326
327 /* print other points */
328 if(rp != trace->writePtr) {
329 for(;;) {
330 t = trace->timeBuffer[rp];
331 v = trace->valueBuffer[rp];
332 rp ++;
333 if(++trace->readPtr == trace->bufferSize) {
334 trace->readPtr = 0;
335 if(rp != trace->bufferSize && ODF_arg->firstSegment) {
336 readPtrOverflowed = true;
337 break;
338 }
339 rp = 0;
340 }
341 if(rp != trace->readPtr && ODF_arg->firstSegment) {
342 readPtrOverflowed = true;
343 break;
344 }
345
346 /* If internal buffer is empty, end transfer */
347 if(rp == trace->writePtr) {
348 /* If there is last time stamp, point will be printed at the end */
349 if(t != trace->lastTimeStamp) {
350 len = trace->dt->printPoint(s, freeLen, t, v);
351 s += len;
352 freeLen -= len;
353 }
354 ODF_arg->lastSegment = true;
355 break;
356 }
357 len = trace->dt->printPoint(s, freeLen, t, v);
358 s += len;
359 freeLen -= len;
360
361 /* if output buffer is full, next data will be sent later */
362 if(freeLen < 50) {
363 ODF_arg->lastSegment = false;
364 break;
365 }
366 }
367 }
368
369 /* print last point */
370 if(!readPtrOverflowed && ODF_arg->lastSegment) {
371 v = trace->valuePrev;
372 t = trace->lastTimeStamp;
373 len = trace->dt->printPointEnd(s, freeLen, t, v);
374 s += len;
375 freeLen -= len;
376 }
377 } while(readPtrOverflowed);
378
379 ODF_arg->dataLength -= freeLen;
380 }
381 }
382 break;
383 }
384
385 return ret;
386 }
387
388
389 /******************************************************************************/
CO_trace_init(CO_trace_t * trace,CO_SDO_t * SDO,uint8_t enabled,uint32_t * timeBuffer,int32_t * valueBuffer,uint32_t bufferSize,uint32_t * map,uint8_t * format,uint8_t * trigger,int32_t * threshold,int32_t * value,int32_t * minValue,int32_t * maxValue,uint32_t * triggerTime,uint16_t idx_OD_traceConfig,uint16_t idx_OD_trace)390 void CO_trace_init(
391 CO_trace_t *trace,
392 CO_SDO_t *SDO,
393 uint8_t enabled,
394 uint32_t *timeBuffer,
395 int32_t *valueBuffer,
396 uint32_t bufferSize,
397 uint32_t *map,
398 uint8_t *format,
399 uint8_t *trigger,
400 int32_t *threshold,
401 int32_t *value,
402 int32_t *minValue,
403 int32_t *maxValue,
404 uint32_t *triggerTime,
405 uint16_t idx_OD_traceConfig,
406 uint16_t idx_OD_trace)
407 {
408 trace->SDO = SDO;
409 trace->enabled = (enabled != 0) ? true : false;
410 trace->timeBuffer = timeBuffer;
411 trace->valueBuffer = valueBuffer;
412 trace->bufferSize = bufferSize;
413 trace->writePtr = 0;
414 trace->readPtr = 0;
415 trace->lastTimeStamp = 0;
416 trace->map = map;
417 trace->format = format;
418 trace->trigger = trigger;
419 trace->threshold = threshold;
420 trace->value = value;
421 trace->minValue = minValue;
422 trace->maxValue = maxValue;
423 trace->triggerTime = triggerTime;
424 *trace->value = 0;
425 *trace->minValue = 0;
426 *trace->maxValue = 0;
427 *trace->triggerTime = 0;
428 trace->valuePrev = 0;
429
430 /* set trace->OD_variable and trace->dt, based on 'map' and 'format' */
431 findVariable(trace);
432
433 if(timeBuffer == NULL || valueBuffer == NULL) {
434 trace->bufferSize = 0;
435 }
436
437 if( trace->bufferSize == 0 || trace->OD_variable == NULL) {
438 trace->enabled = false;
439 }
440
441 CO_OD_configure(SDO, idx_OD_traceConfig, CO_ODF_traceConfig, (void*)trace, 0, 0);
442 CO_OD_configure(SDO, idx_OD_trace, CO_ODF_trace, (void*)trace, 0, 0);
443 }
444
445
446 /******************************************************************************/
CO_trace_process(CO_trace_t * trace,uint32_t timestamp)447 void CO_trace_process(CO_trace_t *trace, uint32_t timestamp) {
448 if(trace->enabled) {
449
450 int32_t val = trace->dt->pGetValue(trace->OD_variable);
451
452 if(val != trace->valuePrev) {
453 /* Verify, if value passed threshold */
454 if((*trace->trigger & 1) != 0 && trace->valuePrev < *trace->threshold && val >= *trace->threshold) {
455 *trace->triggerTime = timestamp;
456 }
457 if((*trace->trigger & 2) != 0 && trace->valuePrev < *trace->threshold && val >= *trace->threshold) {
458 *trace->triggerTime = timestamp;
459 }
460
461 /* Write value and verify min/max */
462 if(trace->value != trace->OD_variable) {
463 *trace->value = val;
464 }
465 trace->valuePrev = val;
466 if(*trace->minValue > val) {
467 *trace->minValue = val;
468 }
469 if(*trace->maxValue < val) {
470 *trace->maxValue = val;
471 }
472
473 /* write buffers and update pointers */
474 trace->timeBuffer[trace->writePtr] = timestamp;
475 trace->valueBuffer[trace->writePtr] = val;
476 if(++trace->writePtr == trace->bufferSize) {
477 trace->writePtr = 0;
478 }
479 if(trace->writePtr == trace->readPtr) {
480 if(++trace->readPtr == trace->bufferSize) {
481 trace->readPtr = 0;
482 }
483 }
484 }
485 else {
486 /* if buffer is empty, make first record */
487 if(trace->writePtr == trace->readPtr) {
488 /* write buffers and update pointers */
489 trace->timeBuffer[trace->writePtr] = timestamp;
490 trace->valueBuffer[trace->writePtr] = val;
491 if(++trace->writePtr == trace->bufferSize) {
492 trace->writePtr = 0;
493 }
494 }
495 }
496 trace->lastTimeStamp = timestamp;
497 }
498 }
499
