306 uint32_t LogMessageCount = 0; 

   306 /* LOGGING

   307 */

   308 

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

   310 #define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

   311 static char LogBuff[LOG_BUFFER_SIZE];

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

   313     if(buffpos + size < LOG_BUFFER_SIZE){

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

   315     }else{

   316         uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 

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

   318         memcpy(LogBuff, buf + remaining, size - remaining);

   319     }

   320 }

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

   322     if(buffpos + size < LOG_BUFFER_SIZE){

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

   324     }else{

   325         uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 

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

   327         memcpy(buf + remaining, LogBuff, size - remaining);

   328     }

   329 }

   330 

   331 /* Log buffer structure

   332 

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

   334  |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

   335 

   336 */

   337 typedef struct {

   338     uint32_t msgidx;

   339     uint32_t msgsize;

   340     /*XXX tick*/

   341     /*XXX RTC*/

   342 } mTail;

   343 

   344 /* Log cursor : 64b

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

   346    | Message | Buffer |

   347    | counter | Index  | */

   348 static uint64_t LogCursor = 0x0;

   349 

   350 /* Store one log message of give size */

   351 int LogMessage(char* buf, uint32_t size){

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

   353         uint32_t buffpos;

   354         mTail tail;

   355         /* We cannot increment both msg index and string pointer 

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

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

   358            succeeds non interrupted */

   359         uint64_t new_cursor, old_cursor;

   360         do{

   361             old_cursor = LogCursor;

   362             buffpos = (uint32_t)old_cursor;

   363             tail.msgidx = (old_cursor >> 32); 

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

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

   366         }while(!__sync_bool_compare_and_swap(&LogCursor,old_cursor,new_cursor));

   367 

   368         copy_to_log(buffpos, buf, size);

   369         tail.msgsize = size;

   370         /*XXX tick*/

   371         /*XXX RTC*/

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

   373 

   374         return 1; /* Success */

   375     }else{

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

   377         LogMessage(mstr, sizeof(mstr));

   378     }

   379     return 0;

   380 }

   309     return LogMessageCount;

   383     return (uint64_t)LogCursor >> 32;

   310 }

   384 }

   311 

   385 

   312 int LogMessage(char* Message){

   386 /* Return message size and content */

   313     LogMessageCount = __sync_add_and_fetch(&LogMessageCount, 1);

   387 uint32_t GetLogMessage(uint32_t msgidx, char* buf, uint32_t max_size){

   314     return 1; // Success

   388     uint64_t cursor = LogCursor;

   315 }

   389     if(cursor){

   316 

   390         /* feeding cursor values */

   391         uint32_t curbuffpos = (uint32_t)cursor;

   392         uint32_t curmsgidx = (cursor >> 32);

   393 

   394         /* seach cursor */

   395         uint32_t stailpos = (curbuffpos - sizeof(mTail)) & LOG_BUFFER_MASK; 

   396         uint32_t smsgidx;

   397         mTail tail;

   398         tail.msgidx = curmsgidx;

   399 

   400         /* Message search loop */

   401         do {

   402             smsgidx = tail.msgidx;

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

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

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

   406 

   407         if(tail.msgidx == msgidx){

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

   409             uint32_t totalsize = tail.msgsize + sizeof(mTail);

   410             copy_from_log(stailpos, &tail, totalsize > max_size ? max_size : totalsize);

   411             return totalsize;

   412         }

   413     }

   414     return 0;

   415 }