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 long long AtomicCompareExchange64(long long* , long long , long long);

   188

extern void LeaveDebugSection(void);

   189

extern void ValidateRetainBuffer(void);

   190

extern void InValidateRetainBuffer(void);

   191

   192

void __publish_debug(void)

   193

{

   194

    retain_offset = 0;

   195

    InValidateRetainBuffer();

   196

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

   197

    if(TryEnterDebugSection()){

   198

        /* Lock buffer */

   199

        long latest_state = AtomicCompareExchange(

   200

            &buffer_state,

   201

            BUFFER_FREE,

   202

            BUFFER_BUSY);

   203

   204

        /* If buffer was free */

   205

        if(latest_state == BUFFER_FREE)

   206

        {

   207

            /* Reset buffer cursor */

   208

            buffer_cursor = debug_buffer;

   209

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

   210

            __for_each_variable_do(DebugIterator);

   211

   212

            /* Leave debug section,

   213

             * Trigger asynchronous transmission 

   214

             * (returns immediately) */

   215

            InitiateDebugTransfer(); /* size */

   216

        }else{

   217

            /* when not debugging, do only retain */

   218

            __for_each_variable_do(RetainIterator);

   219

        }

   220

        LeaveDebugSection();

   221

    }else{

   222

        /* when not debugging, do only retain */

   223

        __for_each_variable_do(RetainIterator);

   224

    }

   225

    ValidateRetainBuffer();

   226

}

   227

   228

#define __RegisterDebugVariable_case_t(TYPENAME) \

   229

        case TYPENAME##_ENUM :\

   230

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

   231

            if(force)\

   232

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

   233

            break;

   234

#define __RegisterDebugVariable_case_p(TYPENAME)\

   235

        case TYPENAME##_P_ENUM :\

   236

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

   237

            if(force)\

   238

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

   239

            break;\

   240

        case TYPENAME##_O_ENUM :\

   241

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

   242

            if(force){\

   243

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

   244

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

   245

            }\

   246

            break;

   247

void RegisterDebugVariable(int idx, void* force)

   248

{

   249

    void *varp = NULL;

   250

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

   251

    switch(__find_variable(idx, &varp)){

   252

        __ANY(__RegisterDebugVariable_case_t)

   253

        __ANY(__RegisterDebugVariable_case_p)

   254

    default:

   255

        break;

   256

    }

   257

}

   258

   259

#define __ResetDebugVariablesIterator_case_t(TYPENAME) \

   260

        case TYPENAME##_ENUM :\

   261

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

   262

            break;

   263

   264

#define __ResetDebugVariablesIterator_case_p(TYPENAME)\

   265

        case TYPENAME##_P_ENUM :\

   266

        case TYPENAME##_O_ENUM :\

   267

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

   268

            break;

   269

   270

void ResetDebugVariablesIterator(void* varp, __IEC_types_enum vartype)

   271

{

   272

    /* force debug flag to 0*/

   273

    switch(vartype){

   274

        __ANY(__ResetDebugVariablesIterator_case_t)

   275

        __ANY(__ResetDebugVariablesIterator_case_p)

   276

    default:

   277

        break;

   278

    }

   279

}

   280

   281

void ResetDebugVariables(void)

   282

{

   283

    __for_each_variable_do(ResetDebugVariablesIterator);

   284

}

   285

   286

void FreeDebugData(void)

   287

{

   288

    /* atomically mark buffer as free */

   289

    long latest_state;

   290

    latest_state = AtomicCompareExchange(

   291

        &buffer_state,

   292

        BUFFER_BUSY,

   293

        BUFFER_FREE);

   294

}

   295

int WaitDebugData(unsigned long *tick);

   296

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

   297

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

   298

    int wait_error = WaitDebugData(tick);

   299

    if(!wait_error){

   300

        *size = buffer_cursor - debug_buffer;

   301

        *buffer = debug_buffer;

   302

    }

   303

    return wait_error;

   304

}

   305

   306

   307

/* LOGGING

   308

*/

   309

   310

#define LOG_LEVELS 4

   311

#define LOG_CRITICAL 0

   312

#define LOG_WARNING 1

   313

#define LOG_INFO 2

   314

#define LOG_DEBUG 3

   315

   316

#ifndef LOG_BUFFER_SIZE

   317

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

   318

#endif

   319

#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

   320

static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE];

   321

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

   322

    if(buffpos + size < LOG_BUFFER_SIZE){

   323

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

   324

    }else{

   325

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 

   326

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

   327

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

   328

    }

   329

}

   330

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

   331

    if(buffpos + size < LOG_BUFFER_SIZE){

   332

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

   333

    }else{

   334

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

   335

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

   336

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

   337

    }

   338

}

   339

   340

/* Log buffer structure

   341

   342

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

   343

 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

   344

   345

*/

   346

typedef struct {

   347

    uint32_t msgidx;

   348

    uint32_t msgsize;

   349

    unsigned long tick;

   350

    IEC_TIME time;

   351

} mTail;

   352

   353

/* Log cursor : 64b

   354

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

   355

   | Message | Buffer |

   356

   | counter | Index  | */

   357

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

   358

   359

/* Store one log message of give size */

   360

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

   361

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

   362

        uint32_t buffpos;

   363

        uint64_t new_cursor, old_cursor;

   364

   365

        mTail tail;

   366

        tail.msgsize = size;

   367

        tail.tick = __tick;

   368

        PLC_GetTime(&tail.time);

   369

   370

        /* We cannot increment both msg index and string pointer 

   371

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

   372

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

   373

           succeeds non interrupted */

   374

        do{

   375

            old_cursor = LogCursor[level];

   376

            buffpos = (uint32_t)old_cursor;

   377

            tail.msgidx = (old_cursor >> 32); 

   378

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

   379

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

   380

        }while(AtomicCompareExchange64(

   381

            (long long*)&LogCursor[level],

   382

            (long long)old_cursor,

   383

            (long long)new_cursor)!=old_cursor);

   384

   385

        copy_to_log(level, buffpos, buf, size);

   386

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

   387

   388

        return 1; /* Success */

   389

    }else{

   390

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

   391

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

   392

    }

   393

    return 0;

   394

}

   395

   396

uint32_t GetLogCount(uint8_t level){

   397

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

   398

}

   399

   400

/* Return message size and content */

   401

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

   402

    uint64_t cursor = LogCursor[level];

   403

    if(cursor){

   404

        /* seach cursor */

   405

        uint32_t stailpos = (uint32_t)cursor; 

   406

        uint32_t smsgidx;

   407

        mTail tail;

   408

        tail.msgidx = cursor >> 32;

   409

        tail.msgsize = 0;

   410

   411

        /* Message search loop */

   412

        do {

   413

            smsgidx = tail.msgidx;

   414

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

   415

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

   416

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

   417

   418

        if(tail.msgidx == msgidx){

   419

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

   420

            uint32_t totalsize = tail.msgsize;

   421

            *tick = tail.tick; 

   422

            *tv_sec = tail.time.tv_sec; 

   423

            *tv_nsec = tail.time.tv_nsec; 

   424

            copy_from_log(level, sbuffpos, buf, 

   425

                          totalsize > max_size ? max_size : totalsize);

   426

            return totalsize;

   427

        }

   428

    }

   429

    return 0;

   430

}