23 #include "POUS.h" |
23 #include "POUS.h" |
24 /*for memcpy*/ |
24 /*for memcpy*/ |
25 #include <string.h> |
25 #include <string.h> |
26 #include <stdio.h> |
26 #include <stdio.h> |
27 |
27 |
|
28 typedef unsigned int dbgvardsc_index_t; |
|
29 typedef unsigned short trace_buf_offset_t; |
|
30 |
|
31 #define BUFFER_EMPTY 0 |
|
32 #define BUFFER_FULL 1 |
|
33 |
28 #ifndef TARGET_ONLINE_DEBUG_DISABLE |
34 #ifndef TARGET_ONLINE_DEBUG_DISABLE |
|
35 |
29 #define TRACE_BUFFER_SIZE 4096 |
36 #define TRACE_BUFFER_SIZE 4096 |
30 #define TRACE_LIST_SIZE 1024 |
37 #define TRACE_LIST_SIZE 1024 |
31 |
38 |
32 /* Atomically accessed variable for buffer state */ |
39 /* Atomically accessed variable for buffer state */ |
33 #define BUFFER_FREE 0 |
40 static long trace_buffer_state = BUFFER_EMPTY; |
34 #define BUFFER_BUSY 1 |
|
35 static long trace_buffer_state = BUFFER_FREE; |
|
36 |
|
37 typedef unsigned int dbgvardsc_index_t; |
|
38 typedef unsigned short trace_buf_offset_t; |
|
39 |
41 |
40 typedef struct trace_item_s { |
42 typedef struct trace_item_s { |
41 dbgvardsc_index_t dbgvardsc_index; |
43 dbgvardsc_index_t dbgvardsc_index; |
42 } trace_item_t; |
44 } trace_item_t; |
43 |
45 |
44 trace_item_t trace_list[TRACE_LIST_SIZE]; |
46 trace_item_t trace_list[TRACE_LIST_SIZE]; |
45 char trace_buffer[TRACE_BUFFER_SIZE]; |
47 char trace_buffer[TRACE_BUFFER_SIZE]; |
46 |
48 |
47 /* Buffer's cursor*/ |
49 /* Trace's cursor*/ |
48 static trace_item_t *trace_list_collect_cursor = trace_list; |
50 static trace_item_t *trace_list_collect_cursor = trace_list; |
49 static trace_item_t *trace_list_addvar_cursor = trace_list; |
51 static trace_item_t *trace_list_addvar_cursor = trace_list; |
50 static const trace_item_t *trace_list_end = |
52 static const trace_item_t *trace_list_end = |
51 &trace_list[TRACE_LIST_SIZE-1]; |
53 &trace_list[TRACE_LIST_SIZE-1]; |
52 static char *trace_buffer_cursor = trace_buffer; |
54 static char *trace_buffer_cursor = trace_buffer; |
53 static const char *trace_buffer_end = trace_buffer + TRACE_BUFFER_SIZE; |
55 static const char *trace_buffer_end = trace_buffer + TRACE_BUFFER_SIZE; |
|
56 |
|
57 |
|
58 |
|
59 #define FORCE_BUFFER_SIZE 1024 |
|
60 #define FORCE_LIST_SIZE 256 |
|
61 |
|
62 typedef struct force_item_s { |
|
63 dbgvardsc_index_t dbgvardsc_index; |
|
64 void *value_pointer_backup; |
|
65 } force_item_t; |
|
66 |
|
67 force_item_t force_list[FORCE_LIST_SIZE]; |
|
68 char force_buffer[FORCE_BUFFER_SIZE]; |
|
69 |
|
70 /* Force's cursor*/ |
|
71 static force_item_t *force_list_apply_cursor = force_list; |
|
72 static force_item_t *force_list_addvar_cursor = force_list; |
|
73 static const force_item_t *force_list_end = |
|
74 &force_list[FORCE_LIST_SIZE-1]; |
|
75 static char *force_buffer_cursor = force_buffer; |
|
76 static const char *force_buffer_end = force_buffer + FORCE_BUFFER_SIZE; |
|
77 |
|
78 |
54 #endif |
79 #endif |
55 |
80 |
56 static unsigned int retain_offset = 0; |
81 static unsigned int retain_offset = 0; |
57 /*** |
82 /*** |
58 * Declare programs |
83 * Declare programs |
154 |
183 |
155 void Retain(unsigned int offset, unsigned int count, void * p); |
184 void Retain(unsigned int offset, unsigned int count, void * p); |
156 |
185 |
157 static inline void BufferIterator(dbgvardsc_t *dsc, int do_debug) |
186 static inline void BufferIterator(dbgvardsc_t *dsc, int do_debug) |
158 { |
187 { |
159 void *real_value_p = NULL; |
188 void *value_p = NULL; |
160 void *visible_value_p = NULL; |
189 char flags; |
161 char flags = 0; |
190 size_t size; |
162 |
191 |
163 visible_value_p = UnpackVar(dsc, &real_value_p, &flags); |
192 UnpackVar(dsc, &value_p, &flags, &size); |
164 |
193 |
165 if(flags & __IEC_RETAIN_FLAG || |
194 if(flags & __IEC_RETAIN_FLAG){ |
166 ((flags & __IEC_FORCE_FLAG) && (flags & __IEC_OUTPUT_FLAG))){ |
195 |
167 USINT size = __get_type_enum_size(dsc->type); |
|
168 |
|
169 #ifndef TARGET_ONLINE_DEBUG_DISABLE |
|
170 if((flags & __IEC_FORCE_FLAG) && (flags & __IEC_OUTPUT_FLAG)){ |
|
171 if(__Is_a_string(dsc)){ |
|
172 /* optimization for strings */ |
|
173 size = ((STRING*)visible_value_p)->len + 1; |
|
174 } |
|
175 /* re-force real value of outputs (M and Q)*/ |
|
176 memcpy(real_value_p, visible_value_p, size); |
|
177 } |
|
178 #endif |
|
179 |
|
180 if(flags & __IEC_RETAIN_FLAG){ |
|
181 /* compute next cursor positon*/ |
196 /* compute next cursor positon*/ |
182 unsigned int next_retain_offset = retain_offset + size; |
197 unsigned int next_retain_offset = retain_offset + size; |
183 /* if buffer not full */ |
198 /* if buffer not full */ |
184 Retain(retain_offset, size, real_value_p); |
199 Retain(retain_offset, size, value_p); |
185 /* increment cursor according size*/ |
200 /* increment cursor according size*/ |
186 retain_offset = next_retain_offset; |
201 retain_offset = next_retain_offset; |
187 } |
202 } |
188 } |
|
189 } |
|
190 |
|
191 void DebugIterator(dbgvardsc_t *dsc){ |
|
192 BufferIterator(dsc, 1); |
|
193 } |
203 } |
194 |
204 |
195 void RetainIterator(dbgvardsc_t *dsc){ |
205 void RetainIterator(dbgvardsc_t *dsc){ |
196 BufferIterator(dsc, 0); |
206 BufferIterator(dsc, 0); |
197 } |
207 } |
238 /* Check there is no running debugger re-configuration */ |
265 /* Check there is no running debugger re-configuration */ |
239 if(TryEnterDebugSection()){ |
266 if(TryEnterDebugSection()){ |
240 /* Lock buffer */ |
267 /* Lock buffer */ |
241 long latest_state = AtomicCompareExchange( |
268 long latest_state = AtomicCompareExchange( |
242 &trace_buffer_state, |
269 &trace_buffer_state, |
243 BUFFER_FREE, |
270 BUFFER_EMPTY, |
244 BUFFER_BUSY); |
271 BUFFER_FULL); |
245 |
272 |
246 /* If buffer was free */ |
273 /* If buffer was free */ |
247 if(latest_state == BUFFER_FREE) |
274 if(latest_state == BUFFER_EMPTY) |
248 { |
275 { |
|
276 int stop = 0; |
|
277 /* Reset force list cursor */ |
|
278 force_list_apply_cursor = force_list; |
|
279 |
|
280 /* iterate over force list */ |
|
281 while(!stop && force_list_apply_cursor < force_list_addvar_cursor){ |
|
282 dbgvardsc_t *dsc = &dbgvardsc[ |
|
283 force_list_apply_cursor->dbgvardsc_index]; |
|
284 void *varp = dsc->ptr; |
|
285 __IEC_types_enum vartype = dsc->type; |
|
286 switch(vartype){ |
|
287 __ANY(__ReForceOutput_case_p) |
|
288 default: |
|
289 break; |
|
290 } |
|
291 force_list_apply_cursor++; \ |
|
292 } |
|
293 |
249 /* Reset buffer cursor */ |
294 /* Reset buffer cursor */ |
250 trace_buffer_cursor = trace_buffer; |
295 trace_buffer_cursor = trace_buffer; |
251 /* Reset trace list cursor */ |
296 /* Reset trace list cursor */ |
252 trace_list_collect_cursor = trace_list; |
297 trace_list_collect_cursor = trace_list; |
253 /* Iterate over all variables to fill debug buffer */ |
298 |
254 __for_each_variable_do(DebugIterator); |
299 /* iterate over trace list */ |
255 while(trace_list_collect_cursor < trace_list_addvar_cursor){ |
300 while(trace_list_collect_cursor < trace_list_addvar_cursor){ |
256 void *real_value_p = NULL; |
301 void *value_p = NULL; |
257 void *visible_value_p = NULL; |
302 size_t size; |
258 char flags = 0; |
|
259 USINT size; |
|
260 char* next_cursor; |
303 char* next_cursor; |
261 |
304 |
262 dbgvardsc_t *dsc = &dbgvardsc[ |
305 dbgvardsc_t *dsc = &dbgvardsc[ |
263 trace_list_collect_cursor->dbgvardsc_index]; |
306 trace_list_collect_cursor->dbgvardsc_index]; |
264 |
307 |
265 visible_value_p = UnpackVar(dsc, &real_value_p, &flags); |
308 UnpackVar(dsc, &value_p, NULL, &size); |
266 |
309 |
267 /* copy visible variable to buffer */; |
310 /* copy visible variable to buffer */; |
268 if(__Is_a_string(dsc)){ |
311 if(__Is_a_string(dsc)){ |
269 /* optimization for strings */ |
312 /* optimization for strings */ |
270 /* assume NULL terminated strings */ |
313 /* assume NULL terminated strings */ |
271 size = ((STRING*)visible_value_p)->len + 1; |
314 size = ((STRING*)value_p)->len + 1; |
272 }else{ |
|
273 size = __get_type_enum_size(dsc->type); |
|
274 } |
315 } |
275 |
316 |
276 /* compute next cursor positon.*/ |
317 /* compute next cursor positon.*/ |
277 next_cursor = trace_buffer_cursor + size; |
318 next_cursor = trace_buffer_cursor + size; |
278 /* check for buffer overflow */ |
319 /* check for buffer overflow */ |
279 if(next_cursor < trace_buffer_end) |
320 if(next_cursor < trace_buffer_end) |
280 /* copy data to the buffer */ |
321 /* copy data to the buffer */ |
281 memcpy(trace_buffer_cursor, visible_value_p, size); |
322 memcpy(trace_buffer_cursor, value_p, size); |
282 else |
323 else |
283 /* stop looping in case of overflow */ |
324 /* stop looping in case of overflow */ |
284 break; |
325 break; |
285 /* increment cursor according size*/ |
326 /* increment cursor according size*/ |
286 trace_buffer_cursor = next_cursor; |
327 trace_buffer_cursor = next_cursor; |
299 __for_each_variable_do(RetainIterator); |
340 __for_each_variable_do(RetainIterator); |
300 ValidateRetainBuffer(); |
341 ValidateRetainBuffer(); |
301 } |
342 } |
302 |
343 |
303 #ifndef TARGET_ONLINE_DEBUG_DISABLE |
344 #ifndef TARGET_ONLINE_DEBUG_DISABLE |
304 #define __RegisterDebugVariable_case_t(TYPENAME) \ |
345 |
305 case TYPENAME##_ENUM :\ |
346 #define TRACE_LIST_OVERFLOW 1 |
306 ((__IEC_##TYPENAME##_t *)varp)->flags |= __IEC_FORCE_FLAG;\ |
347 #define FORCE_LIST_OVERFLOW 2 |
307 ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force);\ |
348 #define FORCE_BUFFER_OVERFLOW 3 |
308 break; |
349 |
309 #define __RegisterDebugVariable_case_p(TYPENAME)\ |
350 #define __ForceVariable_case_t(TYPENAME) \ |
310 case TYPENAME##_P_ENUM :\ |
351 case TYPENAME##_ENUM : \ |
311 ((__IEC_##TYPENAME##_p *)varp)->flags |= __IEC_FORCE_FLAG;\ |
352 /* add to force_list*/ \ |
312 ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\ |
353 force_list_addvar_cursor->dbgvardsc_index = idx; \ |
313 break;\ |
354 ((__IEC_##TYPENAME##_t *)varp)->flags |= __IEC_FORCE_FLAG; \ |
314 case TYPENAME##_O_ENUM :\ |
355 ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force); \ |
315 ((__IEC_##TYPENAME##_p *)varp)->flags |= __IEC_FORCE_FLAG;\ |
356 break; |
316 ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\ |
357 #define __ForceVariable_case_p(TYPENAME) \ |
317 *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\ |
358 case TYPENAME##_P_ENUM : \ |
318 break; |
359 case TYPENAME##_O_ENUM : \ |
319 void RegisterDebugVariable(dbgvardsc_index_t idx, void* force) |
360 { \ |
320 { |
361 char *next_cursor = force_buffer_cursor + sizeof(TYPENAME); \ |
|
362 if(next_cursor <= force_buffer_end ){ \ |
|
363 /* add to force_list*/ \ |
|
364 force_list_addvar_cursor->dbgvardsc_index = idx; \ |
|
365 /* save pointer to backup */ \ |
|
366 force_list_addvar_cursor->value_pointer_backup = \ |
|
367 ((__IEC_##TYPENAME##_p *)varp)->value; \ |
|
368 /* store forced value in force_buffer */ \ |
|
369 *((TYPENAME *)force_buffer_cursor) = *((TYPENAME *)force); \ |
|
370 /* replace pointer with pointer to force_buffer */ \ |
|
371 ((__IEC_##TYPENAME##_p *)varp)->value = \ |
|
372 (TYPENAME *)force_buffer_cursor; \ |
|
373 /* mark variable as forced */ \ |
|
374 ((__IEC_##TYPENAME##_p *)varp)->flags |= __IEC_FORCE_FLAG; \ |
|
375 /* inc force_buffer cursor */ \ |
|
376 force_buffer_cursor = next_cursor; \ |
|
377 /* outputs real value must be systematically forced */ \ |
|
378 if(vartype == TYPENAME##_O_ENUM) \ |
|
379 *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\ |
|
380 } else { \ |
|
381 error_code = FORCE_BUFFER_OVERFLOW; \ |
|
382 goto error_cleanup; \ |
|
383 } \ |
|
384 } \ |
|
385 break; |
|
386 |
|
387 |
|
388 void ResetDebugVariables(void); |
|
389 |
|
390 int RegisterDebugVariable(dbgvardsc_index_t idx, void* force) |
|
391 { |
|
392 int error_code = 0; |
321 if(idx < sizeof(dbgvardsc)/sizeof(dbgvardsc_t)){ |
393 if(idx < sizeof(dbgvardsc)/sizeof(dbgvardsc_t)){ |
322 /* add to trace_list, inc tracelist_addvar_cursor*/ |
394 /* add to trace_list, inc trace_list_addvar_cursor*/ |
323 if(trace_list_addvar_cursor <= trace_list_end){ |
395 if(trace_list_addvar_cursor <= trace_list_end){ |
324 trace_list_addvar_cursor->dbgvardsc_index = idx; |
396 trace_list_addvar_cursor->dbgvardsc_index = idx; |
325 trace_list_addvar_cursor++; |
397 trace_list_addvar_cursor++; |
|
398 } else { |
|
399 error_code = TRACE_LIST_OVERFLOW; |
|
400 goto error_cleanup; |
326 } |
401 } |
327 if(force){ |
402 if(force){ |
328 dbgvardsc_t *dsc = &dbgvardsc[idx]; |
403 if(force_list_addvar_cursor <= force_list_end){ |
329 void *varp = dsc->ptr; |
404 dbgvardsc_t *dsc = &dbgvardsc[idx]; |
330 switch(dsc->type){ |
405 void *varp = dsc->ptr; |
331 __ANY(__RegisterDebugVariable_case_t) |
406 __IEC_types_enum vartype = dsc->type; |
332 __ANY(__RegisterDebugVariable_case_p) |
407 |
333 default: |
408 switch(vartype){ |
334 break; |
409 __ANY(__ForceVariable_case_t) |
|
410 __ANY(__ForceVariable_case_p) |
|
411 default: |
|
412 break; |
|
413 } |
|
414 /* inc force_list cursor */ |
|
415 force_list_addvar_cursor++; |
|
416 } else { |
|
417 error_code = FORCE_LIST_OVERFLOW; |
|
418 goto error_cleanup; |
335 } |
419 } |
336 } |
420 } |
337 } |
421 } |
338 } |
422 return 0; |
339 |
423 |
340 #define __ResetDebugVariablesIterator_case_t(TYPENAME) \ |
424 error_cleanup: |
341 case TYPENAME##_ENUM :\ |
425 ResetDebugVariables(); |
342 ((__IEC_##TYPENAME##_t *)varp)->flags &= ~(__IEC_FORCE_FLAG);\ |
426 trace_buffer_state = BUFFER_EMPTY; |
343 break; |
427 return error_code; |
344 |
428 |
345 #define __ResetDebugVariablesIterator_case_p(TYPENAME)\ |
429 } |
346 case TYPENAME##_P_ENUM :\ |
430 |
347 case TYPENAME##_O_ENUM :\ |
431 #define ResetForcedVariable_case_t(TYPENAME) \ |
348 ((__IEC_##TYPENAME##_p *)varp)->flags &= ~(__IEC_FORCE_FLAG);\ |
432 case TYPENAME##_ENUM : \ |
349 break; |
433 ((__IEC_##TYPENAME##_t *)varp)->flags &= ~__IEC_FORCE_FLAG; \ |
350 |
434 /* for local variable we don't restore original value */ \ |
351 void ResetDebugVariablesIterator(dbgvardsc_t *dsc) |
435 /* that can be added if needed, but it was like that since ever */ \ |
352 { |
436 break; |
353 /* force debug flag to 0*/ |
437 |
354 void *varp = dsc->ptr; |
438 #define ResetForcedVariable_case_p(TYPENAME) \ |
355 switch(dsc->type){ |
439 case TYPENAME##_P_ENUM : \ |
356 __ANY(__ResetDebugVariablesIterator_case_t) |
440 case TYPENAME##_O_ENUM : \ |
357 __ANY(__ResetDebugVariablesIterator_case_p) |
441 ((__IEC_##TYPENAME##_p *)varp)->flags &= ~__IEC_FORCE_FLAG; \ |
358 default: |
442 /* restore backup to pointer */ \ |
359 break; |
443 ((__IEC_##TYPENAME##_p *)varp)->value = \ |
360 } |
444 force_list_apply_cursor->value_pointer_backup; \ |
361 } |
445 break; |
362 |
446 |
363 void ResetDebugVariables(void) |
447 void ResetDebugVariables(void) |
364 { |
448 { |
|
449 /* Reset trace list */ |
365 trace_list_addvar_cursor = trace_list; |
450 trace_list_addvar_cursor = trace_list; |
366 __for_each_variable_do(ResetDebugVariablesIterator); |
451 |
|
452 force_list_apply_cursor = force_list; |
|
453 /* Restore forced variables */ |
|
454 while(force_list_apply_cursor < force_list_addvar_cursor){ |
|
455 dbgvardsc_t *dsc = &dbgvardsc[ |
|
456 force_list_apply_cursor->dbgvardsc_index]; |
|
457 void *varp = dsc->ptr; |
|
458 switch(dsc->type){ |
|
459 __ANY(ResetForcedVariable_case_t) |
|
460 __ANY(ResetForcedVariable_case_p) |
|
461 default: |
|
462 break; |
|
463 } |
|
464 /* inc force_list cursor */ |
|
465 force_list_apply_cursor++; |
|
466 } /* else TODO: warn user about failure to force */ |
|
467 |
|
468 /* Reset force list */ |
|
469 force_list_addvar_cursor = force_list; |
|
470 /* Reset force buffer */ |
|
471 force_buffer_cursor = force_buffer; |
367 } |
472 } |
368 |
473 |
369 void FreeDebugData(void) |
474 void FreeDebugData(void) |
370 { |
475 { |
371 /* atomically mark buffer as free */ |
476 /* atomically mark buffer as free */ |
372 AtomicCompareExchange( |
477 AtomicCompareExchange( |
373 &trace_buffer_state, |
478 &trace_buffer_state, |
374 BUFFER_BUSY, |
479 BUFFER_FULL, |
375 BUFFER_FREE); |
480 BUFFER_EMPTY); |
376 } |
481 } |
377 int WaitDebugData(unsigned long *tick); |
482 int WaitDebugData(unsigned long *tick); |
378 /* Wait until debug data ready and return pointer to it */ |
483 /* Wait until debug data ready and return pointer to it */ |
379 int GetDebugData(unsigned long *tick, unsigned long *size, void **buffer){ |
484 int GetDebugData(unsigned long *tick, unsigned long *size, void **buffer){ |
380 int wait_error = WaitDebugData(tick); |
485 int wait_error = WaitDebugData(tick); |