1

/*

     2

 * DEBUGGER code

     3

 * 

     4

 * On "publish", when buffer is free, debugger stores arbitrary variables 

     5

 * content into, and mark this buffer as filled

     6

 * 

     7

 * 

     8

 * Buffer content is read asynchronously, (from non real time part), 

     9

 * and then buffer marked free again.

    10

 *  

    11

 * 

    12

 * */

    13

#include "iec_types_all.h"

    14

#include "POUS.h"

    15

/*for memcpy*/

    16

#include <string.h>

    17

#include <stdio.h>

    18

    19

#define BUFFER_SIZE %(buffer_size)d

    20

    21

/* Atomically accessed variable for buffer state */

    22

#define BUFFER_FREE 0

    23

#define BUFFER_BUSY 1

    24

static long buffer_state = BUFFER_FREE;

    25

    26

/* The buffer itself */

    27

char debug_buffer[BUFFER_SIZE];

    28

    29

/* Buffer's cursor*/

    30

static char* buffer_cursor = debug_buffer;

    31

static unsigned int retain_offset = 0;

    32

/***

    33

 * Declare programs 

    34

 **/

    35

%(programs_declarations)s

    36

    37

/***

    38

 * Declare global variables from resources and conf 

    39

 **/

    40

%(extern_variables_declarations)s

    41

    42

typedef void(*__for_each_variable_do_fp)(void*, __IEC_types_enum);

    43

void __for_each_variable_do(__for_each_variable_do_fp fp)

    44

{

    45

%(for_each_variable_do_code)s

    46

}

    47

    48

__IEC_types_enum __find_variable(unsigned int varindex, void ** varp)

    49

{

    50

    switch(varindex){

    51

%(find_variable_case_code)s

    52

     default:

    53

      *varp = NULL;

    54

      return UNKNOWN_ENUM;

    55

    }

    56

}

    57

    58

#define __Unpack_case_t(TYPENAME) \

    59

        case TYPENAME##_ENUM :\

    60

            *flags = ((__IEC_##TYPENAME##_t *)varp)->flags;\

    61

            forced_value_p = *real_value_p = &((__IEC_##TYPENAME##_t *)varp)->value;\

    62

            break;

    63

    64

#define __Unpack_case_p(TYPENAME)\

    65

        case TYPENAME##_O_ENUM :\

    66

            *flags = __IEC_OUTPUT_FLAG;\

    67

        case TYPENAME##_P_ENUM :\

    68

            *flags |= ((__IEC_##TYPENAME##_p *)varp)->flags;\

    69

            *real_value_p = ((__IEC_##TYPENAME##_p *)varp)->value;\

    70

            forced_value_p = &((__IEC_##TYPENAME##_p *)varp)->fvalue;\

    71

            break;

    72

    73

void* UnpackVar(void* varp, __IEC_types_enum vartype, void **real_value_p, char *flags)

    74

{

    75

    void *forced_value_p = NULL;

    76

    *flags = 0;

    77

    /* find data to copy*/

    78

    switch(vartype){

    79

        __ANY(__Unpack_case_t)

    80

        __ANY(__Unpack_case_p)

    81

    default:

    82

        break;

    83

    }

    84

    if (*flags & __IEC_FORCE_FLAG)

    85

        return forced_value_p;

    86

    return *real_value_p;

    87

}

    88

    89

void Remind(unsigned int offset, unsigned int count, void * p);

    90

    91

void RemindIterator(void* varp, __IEC_types_enum vartype)

    92

{

    93

    void *real_value_p = NULL;

    94

    char flags = 0;

    95

    UnpackVar(varp, vartype, &real_value_p, &flags);

    96

    97

    if(flags & __IEC_RETAIN_FLAG){

    98

        USINT size = __get_type_enum_size(vartype);

    99

        /* compute next cursor positon*/

   100

        unsigned int next_retain_offset = retain_offset + size;

   101

        /* if buffer not full */

   102

        Remind(retain_offset, size, real_value_p);

   103

        /* increment cursor according size*/

   104

        retain_offset = next_retain_offset;

   105

    }

   106

}

   107

   108

extern int CheckRetainBuffer(void);

   109

   110

void __init_debug(void)

   111

{

   112

    /* init local static vars */

   113

    buffer_cursor = debug_buffer;

   114

    retain_offset = 0;

   115

    buffer_state = BUFFER_FREE;

   116

    /* Iterate over all variables to fill debug buffer */

   117

    if(CheckRetainBuffer())

   118

    	__for_each_variable_do(RemindIterator);

   119

    retain_offset = 0;

   120

}

   121

   122

extern void InitiateDebugTransfer(void);

   123

   124

extern unsigned long __tick;

   125

   126

void __cleanup_debug(void)

   127

{

   128

    buffer_cursor = debug_buffer;

   129

    InitiateDebugTransfer();

   130

}

   131

   132

void __retrieve_debug(void)

   133

{

   134

}

   135

   136

   137

void Retain(unsigned int offset, unsigned int count, void * p);

   138

   139

inline void BufferIterator(void* varp, __IEC_types_enum vartype, int do_debug)

   140

{

   141

    void *real_value_p = NULL;

   142

    void *visible_value_p = NULL;

   143

    char flags = 0;

   144

   145

    visible_value_p = UnpackVar(varp, vartype, &real_value_p, &flags);

   146

   147

    if(flags & ( __IEC_DEBUG_FLAG | __IEC_RETAIN_FLAG)){

   148

        USINT size = __get_type_enum_size(vartype);

   149

        if(flags & __IEC_DEBUG_FLAG){

   150

            /* copy visible variable to buffer */;

   151

            if(do_debug){

   152

                /* compute next cursor positon.

   153

                   No need to check overflow, as BUFFER_SIZE

   154

                   is computed large enough */

   155

                char* next_cursor = buffer_cursor + size;

   156

                /* copy data to the buffer */

   157

                memcpy(buffer_cursor, visible_value_p, size);

   158

                /* increment cursor according size*/

   159

                buffer_cursor = next_cursor;

   160

            }

   161

            /* re-force real value of outputs (M and Q)*/

   162

            if((flags & __IEC_FORCE_FLAG) && (flags & __IEC_OUTPUT_FLAG)){

   163

                memcpy(real_value_p, visible_value_p, size);

   164

            }

   165

        }

   166

        if(flags & __IEC_RETAIN_FLAG){

   167

            /* compute next cursor positon*/

   168

            unsigned int next_retain_offset = retain_offset + size;

   169

            /* if buffer not full */

   170

            Retain(retain_offset, size, real_value_p);

   171

            /* increment cursor according size*/

   172

            retain_offset = next_retain_offset;

   173

        }

   174

    }

   175

}

   176

   177

void DebugIterator(void* varp, __IEC_types_enum vartype){

   178

    BufferIterator(varp, vartype, 1);

   179

}

   180

   181

void RetainIterator(void* varp, __IEC_types_enum vartype){

   182

    BufferIterator(varp, vartype, 0);

   183

}

   184

   185

extern int TryEnterDebugSection(void);

   186

extern long AtomicCompareExchange(long*, long, long);

   187

extern void LeaveDebugSection(void);

   188

extern void ValidateRetainBuffer(void);

   189

extern void InValidateRetainBuffer(void);

   190

   191

void __publish_debug(void)

   192

{

   193

    retain_offset = 0;

   194

    InValidateRetainBuffer();

   195

    /* Check there is no running debugger re-configuration */

   196

    if(TryEnterDebugSection()){

   197

        /* Lock buffer */

   198

        long latest_state = AtomicCompareExchange(

   199

            &buffer_state,

   200

            BUFFER_FREE,

   201

            BUFFER_BUSY);

   202

   203

        /* If buffer was free */

   204

        if(latest_state == BUFFER_FREE)

   205

        {

   206

            /* Reset buffer cursor */

   207

            buffer_cursor = debug_buffer;

   208

            /* Iterate over all variables to fill debug buffer */

   209

            __for_each_variable_do(DebugIterator);

   210

   211

            /* Leave debug section,

   212

             * Trigger asynchronous transmission 

   213

             * (returns immediately) */

   214

            InitiateDebugTransfer(); /* size */

   215

        }else{

   216

            /* when not debugging, do only retain */

   217

            __for_each_variable_do(RetainIterator);

   218

        }

   219

        LeaveDebugSection();

   220

    }else{

   221

        /* when not debugging, do only retain */

   222

        __for_each_variable_do(RetainIterator);

   223

    }

   224

    ValidateRetainBuffer();

   225

}

   226

   227

#define __RegisterDebugVariable_case_t(TYPENAME) \

   228

        case TYPENAME##_ENUM :\

   229

            ((__IEC_##TYPENAME##_t *)varp)->flags |= flags;\

   230

            if(force)\

   231

             ((__IEC_##TYPENAME##_t *)varp)->value = *((TYPENAME *)force);\

   232

            break;

   233

#define __RegisterDebugVariable_case_p(TYPENAME)\

   234

        case TYPENAME##_P_ENUM :\

   235

            ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\

   236

            if(force)\

   237

             ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\

   238

            break;\

   239

        case TYPENAME##_O_ENUM :\

   240

            ((__IEC_##TYPENAME##_p *)varp)->flags |= flags;\

   241

            if(force){\

   242

             ((__IEC_##TYPENAME##_p *)varp)->fvalue = *((TYPENAME *)force);\

   243

             *(((__IEC_##TYPENAME##_p *)varp)->value) = *((TYPENAME *)force);\

   244

            }\

   245

            break;

   246

void RegisterDebugVariable(int idx, void* force)

   247

{

   248

    void *varp = NULL;

   249

    unsigned char flags = force ? __IEC_DEBUG_FLAG | __IEC_FORCE_FLAG : __IEC_DEBUG_FLAG;

   250

    switch(__find_variable(idx, &varp)){

   251

        __ANY(__RegisterDebugVariable_case_t)

   252

        __ANY(__RegisterDebugVariable_case_p)

   253

    default:

   254

        break;

   255

    }

   256

}

   257

   258

#define __ResetDebugVariablesIterator_case_t(TYPENAME) \

   259

        case TYPENAME##_ENUM :\

   260

            ((__IEC_##TYPENAME##_t *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\

   261

            break;

   262

   263

#define __ResetDebugVariablesIterator_case_p(TYPENAME)\

   264

        case TYPENAME##_P_ENUM :\

   265

        case TYPENAME##_O_ENUM :\

   266

            ((__IEC_##TYPENAME##_p *)varp)->flags &= ~(__IEC_DEBUG_FLAG|__IEC_FORCE_FLAG);\

   267

            break;

   268

   269

void ResetDebugVariablesIterator(void* varp, __IEC_types_enum vartype)

   270

{

   271

    /* force debug flag to 0*/

   272

    switch(vartype){

   273

        __ANY(__ResetDebugVariablesIterator_case_t)

   274

        __ANY(__ResetDebugVariablesIterator_case_p)

   275

    default:

   276

        break;

   277

    }

   278

}

   279

   280

void ResetDebugVariables(void)

   281

{

   282

    __for_each_variable_do(ResetDebugVariablesIterator);

   283

}

   284

   285

void FreeDebugData(void)

   286

{

   287

    /* atomically mark buffer as free */

   288

    long latest_state;

   289

    latest_state = AtomicCompareExchange(

   290

        &buffer_state,

   291

        BUFFER_BUSY,

   292

        BUFFER_FREE);

   293

}

   294

int WaitDebugData(unsigned long *tick);

   295

/* Wait until debug data ready and return pointer to it */

   296

int GetDebugData(unsigned long *tick, unsigned long *size, void **buffer){

   297

    int wait_error = WaitDebugData(tick);

   298

    if(!wait_error){

   299

        *size = buffer_cursor - debug_buffer;

   300

        *buffer = debug_buffer;

   301

    }

   302

    return wait_error;

   303

}

   304

   305

   306

/* LOGGING

   307

*/

   308

   309

#define LOG_LEVELS 4

   310

#define LOG_CRITICAL 0

   311

#define LOG_WARNING 1

   312

#define LOG_INFO 2

   313

#define LOG_DEBUG 3

   314

   316

#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/

   318

#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

   319

static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE];

   320

void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){

   321

    if(buffpos + size < LOG_BUFFER_SIZE){

   322

        memcpy(&LogBuff[level][buffpos], buf, size);

   323

    }else{

   324

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 

   325

        memcpy(&LogBuff[level][buffpos], buf, remaining);

   326

        memcpy(LogBuff[level], buf + remaining, size - remaining);

   327

    }

   328

}

   329

void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){

   330

    if(buffpos + size < LOG_BUFFER_SIZE){

   331

        memcpy(buf, &LogBuff[level][buffpos], size);

   332

    }else{

   333

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

   334

        memcpy(buf, &LogBuff[level][buffpos], remaining);

   335

        memcpy(buf + remaining, LogBuff[level], size - remaining);

   336

    }

   337

}

   338

   339

/* Log buffer structure

   340

   341

 |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|...

   342

 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

   343

   344

*/

   345

typedef struct {

   346

    uint32_t msgidx;

   347

    uint32_t msgsize;

   348

    unsigned long tick;

   349

    IEC_TIME time;

   350

} mTail;

   351

   352

/* Log cursor : 64b

   353

   |63 ... 32|31 ... 0|

   354

   | Message | Buffer |

   355

   | counter | Index  | */

   356

static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0};

   357

   358

/* Store one log message of give size */

   359

int LogMessage(uint8_t level, char* buf, uint32_t size){

   360

    if(size < LOG_BUFFER_SIZE - sizeof(mTail)){

   361

        uint32_t buffpos;

   362

        uint64_t new_cursor, old_cursor;

   363

   364

        mTail tail;

   365

        tail.msgsize = size;

   366

        tail.tick = __tick;

   367

        PLC_GetTime(&tail.time);

   368

   369

        /* We cannot increment both msg index and string pointer 

   370

           in a single atomic operation but we can detect having been interrupted.

   371

           So we can try with atomic compare and swap in a loop until operation

   372

           succeeds non interrupted */

   373

        do{

   374

            old_cursor = LogCursor[level];

   375

            buffpos = (uint32_t)old_cursor;

   376

            tail.msgidx = (old_cursor >> 32); 

   377

            new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) 

   378

                         | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK);

   379

        }while(!__sync_bool_compare_and_swap(&LogCursor[level],old_cursor,new_cursor));

   380

   381

        copy_to_log(level, buffpos, buf, size);

   382

        copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail));

   383

   384

        return 1; /* Success */

   385

    }else{

   386

        char mstr[] = "Logging error : message too big";

   387

        LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));

   388

    }

   389

    return 0;

   390

}

   391

   392

uint32_t GetLogCount(uint8_t level){

   393

    return (uint64_t)LogCursor[level] >> 32;

   394

}

   395

   396

/* Return message size and content */

   397

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){

   398

    uint64_t cursor = LogCursor[level];

   399

    if(cursor){

   400

        /* seach cursor */

   401

        uint32_t stailpos = (uint32_t)cursor; 

   402

        uint32_t smsgidx;

   403

        mTail tail;

   404

        tail.msgidx = cursor >> 32;

   405

        tail.msgsize = 0;

   406

   407

        /* Message search loop */

   408

        do {

   409

            smsgidx = tail.msgidx;

   410

            stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK;

   411

            copy_from_log(level, stailpos, &tail, sizeof(mTail));

   412

        }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx));

   413

   414

        if(tail.msgidx == msgidx){

   415

            uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; 

   416

            uint32_t totalsize = tail.msgsize;

   417

            *tick = tail.tick; 

   418

            *tv_sec = tail.time.tv_sec; 

   419

            *tv_nsec = tail.time.tv_nsec; 

   420

            copy_from_log(level, sbuffpos, buf, 

   421

                          totalsize > max_size ? max_size : totalsize);

   422

            return totalsize;

   423

        }

   424

    }

   425

    return 0;

   426

}