etisserant@209: /*
etisserant@209:  * DEBUGGER code
etisserant@209:  * 
etisserant@209:  * On "publish", when buffer is free, debugger stores arbitrary variables 
etisserant@209:  * content into, and mark this buffer as filled
etisserant@209:  * 
etisserant@209:  * 
etisserant@209:  * Buffer content is read asynchronously, (from non real time part), 
etisserant@209:  * and then buffer marked free again.
etisserant@209:  *  
etisserant@209:  * 
etisserant@209:  * */
etisserant@209: #include "iec_types_all.h"
etisserant@209: #include "POUS.h"
etisserant@209: /*for memcpy*/
etisserant@209: #include <string.h>
greg@335: #include <stdio.h>
etisserant@209: 
greg@335: #define BUFFER_SIZE %(buffer_size)d
etisserant@209: 
etisserant@209: /* Atomically accessed variable for buffer state */
etisserant@209: #define BUFFER_FREE 0
etisserant@209: #define BUFFER_BUSY 1
etisserant@209: static long buffer_state = BUFFER_FREE;
etisserant@209: 
etisserant@209: /* The buffer itself */
etisserant@209: char debug_buffer[BUFFER_SIZE];
etisserant@209: 
etisserant@209: /* Buffer's cursor*/
etisserant@209: static char* buffer_cursor = debug_buffer;
edouard@483: static unsigned int retain_offset = 0;
etisserant@209: /***
etisserant@209:  * Declare programs 
etisserant@209:  **/
etisserant@209: %(programs_declarations)s
etisserant@209: 
etisserant@209: /***
etisserant@209:  * Declare global variables from resources and conf 
etisserant@209:  **/
etisserant@209: %(extern_variables_declarations)s
etisserant@209: 
edouard@450: typedef void(*__for_each_variable_do_fp)(void*, __IEC_types_enum);
edouard@452: void __for_each_variable_do(__for_each_variable_do_fp fp)
edouard@450: {
edouard@450: %(for_each_variable_do_code)s
edouard@450: }
etisserant@209: 
edouard@450: __IEC_types_enum __find_variable(unsigned int varindex, void ** varp)
edouard@450: {
edouard@450:     switch(varindex){
edouard@450: %(find_variable_case_code)s
edouard@452:      default:
edouard@452:       *varp = NULL;
edouard@452:       return UNKNOWN_ENUM;
edouard@452:     }
edouard@450: }
etisserant@209: 
Edouard@605: #define __Unpack_case_t(TYPENAME) \
edouard@450:         case TYPENAME##_ENUM :\
edouard@483:             *flags = ((__IEC_##TYPENAME##_t *)varp)->flags;\
Edouard@605:             forced_value_p = *real_value_p = &((__IEC_##TYPENAME##_t *)varp)->value;\
Edouard@605:             break;
Edouard@605: 
Edouard@605: #define __Unpack_case_p(TYPENAME)\
Edouard@605:         case TYPENAME##_O_ENUM :\
Edouard@605:             *flags = __IEC_OUTPUT_FLAG;\
edouard@450:         case TYPENAME##_P_ENUM :\
Edouard@605:             *flags |= ((__IEC_##TYPENAME##_p *)varp)->flags;\
Edouard@605:             *real_value_p = ((__IEC_##TYPENAME##_p *)varp)->value;\
Edouard@605:             forced_value_p = &((__IEC_##TYPENAME##_p *)varp)->fvalue;\
Edouard@605:             break;
Edouard@605: 
Edouard@605: void* UnpackVar(void* varp, __IEC_types_enum vartype, void **real_value_p, char *flags)
Edouard@605: {
Edouard@605:     void *forced_value_p = NULL;
Edouard@605:     *flags = 0;
edouard@450:     /* find data to copy*/
edouard@450:     switch(vartype){
laurent@611:         __ANY(__Unpack_case_t)
laurent@611:         __ANY(__Unpack_case_p)
edouard@452:     default:
edouard@452:         break;
edouard@450:     }
Edouard@605:     if (*flags & __IEC_FORCE_FLAG)
Edouard@605:         return forced_value_p;
Edouard@605:     return *real_value_p;
edouard@483: }
edouard@483: 
edouard@483: void Remind(unsigned int offset, unsigned int count, void * p);
edouard@483: 
edouard@483: void RemindIterator(void* varp, __IEC_types_enum vartype)
edouard@483: {
Edouard@605:     void *real_value_p = NULL;
edouard@483:     char flags = 0;
Edouard@605:     UnpackVar(varp, vartype, &real_value_p, &flags);
edouard@483: 
laurent@507:     if(flags & __IEC_RETAIN_FLAG){
edouard@450:         USINT size = __get_type_enum_size(vartype);
edouard@450:         /* compute next cursor positon*/
edouard@483:         unsigned int next_retain_offset = retain_offset + size;
edouard@450:         /* if buffer not full */
Edouard@605:         Remind(retain_offset, size, real_value_p);
edouard@483:         /* increment cursor according size*/
edouard@483:         retain_offset = next_retain_offset;
edouard@483:     }
edouard@483: }
edouard@483: 
edouard@580: extern int CheckRetainBuffer(void);
edouard@521: 
edouard@483: void __init_debug(void)
edouard@483: {
edouard@483:     /* init local static vars */
edouard@483:     buffer_cursor = debug_buffer;
edouard@483:     retain_offset = 0;
edouard@483:     buffer_state = BUFFER_FREE;
edouard@483:     /* Iterate over all variables to fill debug buffer */
edouard@521:     if(CheckRetainBuffer())
edouard@521:     	__for_each_variable_do(RemindIterator);
edouard@483:     retain_offset = 0;
edouard@483: }
edouard@483: 
edouard@483: extern void InitiateDebugTransfer(void);
edouard@483: 
edouard@483: extern unsigned long __tick;
edouard@483: 
edouard@483: void __cleanup_debug(void)
edouard@483: {
edouard@483:     buffer_cursor = debug_buffer;
edouard@483:     InitiateDebugTransfer();
edouard@483: }
edouard@483: 
edouard@483: void __retrieve_debug(void)
edouard@483: {
edouard@483: }
edouard@483: 
edouard@483: 
edouard@483: void Retain(unsigned int offset, unsigned int count, void * p);
Edouard@605: 
Edouard@605: inline void BufferIterator(void* varp, __IEC_types_enum vartype, int do_debug)
Edouard@605: {
Edouard@605:     void *real_value_p = NULL;
Edouard@605:     void *visible_value_p = NULL;
edouard@483:     char flags = 0;
Edouard@605: 
Edouard@605:     visible_value_p = UnpackVar(varp, vartype, &real_value_p, &flags);
Edouard@605: 
Edouard@605:     if(flags & ( __IEC_DEBUG_FLAG | __IEC_RETAIN_FLAG)){
edouard@483:         USINT size = __get_type_enum_size(vartype);
edouard@483:         if(flags & __IEC_DEBUG_FLAG){
Edouard@605:             /* copy visible variable to buffer */;
Edouard@605:             if(do_debug){
Edouard@605:                 /* compute next cursor positon.
Edouard@605:                    No need to check overflow, as BUFFER_SIZE
Edouard@605:                    is computed large enough */
Edouard@605:                 char* next_cursor = buffer_cursor + size;
Edouard@605:                 /* copy data to the buffer */
Edouard@605:                 memcpy(buffer_cursor, visible_value_p, size);
Edouard@605:                 /* increment cursor according size*/
Edouard@605:                 buffer_cursor = next_cursor;
Edouard@605:             }
Edouard@605:             /* re-force real value of outputs (M and Q)*/
laurent@649:             if((flags & __IEC_FORCE_FLAG) && (flags & __IEC_OUTPUT_FLAG)){
Edouard@605:                 memcpy(real_value_p, visible_value_p, size);
Edouard@605:             }
edouard@450:         }
edouard@483:         if(flags & __IEC_RETAIN_FLAG){
Edouard@605:             /* compute next cursor positon*/
Edouard@605:             unsigned int next_retain_offset = retain_offset + size;
Edouard@605:             /* if buffer not full */
Edouard@605:             Retain(retain_offset, size, real_value_p);
Edouard@605:             /* increment cursor according size*/
Edouard@605:             retain_offset = next_retain_offset;
edouard@483:         }
edouard@483:     }
edouard@483: }
edouard@483: 
Edouard@605: void DebugIterator(void* varp, __IEC_types_enum vartype){
Edouard@605:     BufferIterator(varp, vartype, 1);
Edouard@605: }
Edouard@605: 
Edouard@605: void RetainIterator(void* varp, __IEC_types_enum vartype){
Edouard@605:     BufferIterator(varp, vartype, 0);
edouard@483: }
edouard@483: 
edouard@483: extern int TryEnterDebugSection(void);
edouard@483: extern long AtomicCompareExchange(long*, long, long);
edouard@483: extern void LeaveDebugSection(void);
edouard@580: extern void ValidateRetainBuffer(void);
edouard@580: extern void InValidateRetainBuffer(void);
edouard@450: 
greg@423: void __publish_debug(void)
etisserant@209: {
edouard@483:     retain_offset = 0;
edouard@580:     InValidateRetainBuffer();
etisserant@239:     /* Check there is no running debugger re-configuration */
etisserant@239:     if(TryEnterDebugSection()){
etisserant@239:         /* Lock buffer */
etisserant@239:         long latest_state = AtomicCompareExchange(
etisserant@239:             &buffer_state,
etisserant@239:             BUFFER_FREE,
etisserant@239:             BUFFER_BUSY);
etisserant@239:             
etisserant@239:         /* If buffer was free */
etisserant@239:         if(latest_state == BUFFER_FREE)
etisserant@209:         {
etisserant@239:             /* Reset buffer cursor */
etisserant@239:             buffer_cursor = debug_buffer;
edouard@450:             /* Iterate over all variables to fill debug buffer */
Edouard@605:             __for_each_variable_do(DebugIterator);
etisserant@239:             
etisserant@239:             /* Leave debug section,
etisserant@239:              * Trigger asynchronous transmission 
etisserant@239:              * (returns immediately) */
etisserant@239:             InitiateDebugTransfer(); /* size */
edouard@581:         }else{
edouard@581:             /* when not debugging, do only retain */
edouard@581:             __for_each_variable_do(RetainIterator);
etisserant@209:         }
etisserant@239:         LeaveDebugSection();
edouard@483:     }else{
edouard@483:         /* when not debugging, do only retain */
edouard@483:         __for_each_variable_do(RetainIterator);
etisserant@209:     }
edouard@580:     ValidateRetainBuffer();
etisserant@209: }
etisserant@209: 
edouard@450: #define __RegisterDebugVariable_case_t(TYPENAME) \
edouard@450:         case TYPENAME##_ENUM :\
edouard@477:             ((__IEC_##TYPENAME##_t *)varp)->flags |= flags;\
edouard@477:             if(force)\
edouard@477:              ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force);\
edouard@458:             break;
edouard@450: #define __RegisterDebugVariable_case_p(TYPENAME)\
edouard@450:         case TYPENAME##_P_ENUM :\
edouard@477:             ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\
edouard@477:             if(force)\
edouard@477:              ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\
laurent@511:             break;\
laurent@511:         case TYPENAME##_O_ENUM :\
laurent@511:             ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\
edouard@582:             if(force){\
laurent@511:              ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\
laurent@511:              *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\
edouard@582:             }\
edouard@477:             break;
edouard@477: void RegisterDebugVariable(int idx, void* force)
etisserant@209: {
edouard@452:     void *varp = NULL;
edouard@477:     unsigned char flags = force ? __IEC_DEBUG_FLAG | __IEC_FORCE_FLAG : __IEC_DEBUG_FLAG;
edouard@458:     switch(__find_variable(idx, &varp)){
laurent@611:         __ANY(__RegisterDebugVariable_case_t)
laurent@611:         __ANY(__RegisterDebugVariable_case_p)
edouard@452:     default:
edouard@452:         break;
edouard@450:     }
edouard@450: }
edouard@450: 
edouard@450: #define __ResetDebugVariablesIterator_case_t(TYPENAME) \
edouard@450:         case TYPENAME##_ENUM :\
edouard@477:             ((__IEC_##TYPENAME##_t *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\
edouard@458:             break;
edouard@450: 
edouard@450: #define __ResetDebugVariablesIterator_case_p(TYPENAME)\
edouard@450:         case TYPENAME##_P_ENUM :\
edouard@582:         case TYPENAME##_O_ENUM :\
edouard@477:             ((__IEC_##TYPENAME##_p *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\
edouard@458:             break;
edouard@450: 
edouard@450: void ResetDebugVariablesIterator(void* varp, __IEC_types_enum vartype)
edouard@450: {
edouard@450:     /* force debug flag to 0*/
edouard@450:     switch(vartype){
laurent@611:         __ANY(__ResetDebugVariablesIterator_case_t)
laurent@611:         __ANY(__ResetDebugVariablesIterator_case_p)
edouard@452:     default:
edouard@452:         break;
etisserant@209:     }
etisserant@209: }
etisserant@209: 
etisserant@209: void ResetDebugVariables(void)
etisserant@209: {
edouard@450:     __for_each_variable_do(ResetDebugVariablesIterator);
etisserant@209: }
etisserant@209: 
greg@423: void FreeDebugData(void)
etisserant@209: {
etisserant@209:     /* atomically mark buffer as free */
greg@423:     long latest_state;
greg@423:     latest_state = AtomicCompareExchange(
etisserant@209:         &buffer_state,
etisserant@209:         BUFFER_BUSY,
etisserant@209:         BUFFER_FREE);
etisserant@209: }
edouard@452: int WaitDebugData(unsigned long *tick);
edouard@450: /* Wait until debug data ready and return pointer to it */
edouard@450: int GetDebugData(unsigned long *tick, unsigned long *size, void **buffer){
edouard@504:     int wait_error = WaitDebugData(tick);
edouard@504:     if(!wait_error){
edouard@502:         *size = buffer_cursor - debug_buffer;
edouard@502:         *buffer = debug_buffer;
edouard@502:     }
edouard@504:     return wait_error;
edouard@504: }
edouard@504: 
Edouard@906: 
Edouard@910: /* LOGGING
Edouard@910: */
Edouard@910: 
Edouard@917: #define LOG_LEVELS 4
Edouard@917: #define LOG_CRITICAL 0
Edouard@917: #define LOG_WARNING 1
Edouard@917: #define LOG_INFO 2
Edouard@923: #define LOG_DEBUG 3
Edouard@917: 
Edouard@944: #ifndef LOG_BUFFER_SIZE
Edouard@910: #define LOG_BUFFER_SIZE (1<<14) /*16Ko*/
Edouard@944: #endif
Edouard@910: #define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)
Edouard@917: static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE];
Edouard@917: 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@910:         uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 
Edouard@917:         memcpy(&LogBuff[level][buffpos], buf, remaining);
Edouard@917:         memcpy(LogBuff[level], buf + remaining, size - remaining);
Edouard@917:     }
Edouard@917: }
Edouard@917: 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@917:         memcpy(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@917: static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0};
Edouard@910: 
Edouard@910: /* Store one log message of give size */
Edouard@917: 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@917:         }while(!__sync_bool_compare_and_swap(&LogCursor[level],old_cursor,new_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@910:         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: }