Diff: RtosLog.cpp

Revision:

34:fc366bab393f

Parent:

2:887c6b45e7fa

--- a/RtosLog.cpp	Thu Feb 19 14:41:14 2015 +0100
+++ b/RtosLog.cpp	Mon Mar 09 11:15:56 2015 +0100
@@ -58,6 +58,12 @@
  * Public Functions
  *****************************************************************************/
 
+#if defined(DM_BOARD_USE_USBSERIAL_IN_RTOSLOG)
+    RtosLog::RtosLog() :
+        _sem(NumMessages), _serial(), _thr(NULL)
+    {
+    }
+#else
 RtosLog::RtosLog() :
     _sem(NumMessages), _serial(USBTX, USBRX), _thr(NULL)
 {
@@ -68,6 +74,7 @@
     _serial.baud(115200);
 #endif
 }
+#endif
 
 RtosLog::~RtosLog()
 {
@@ -124,3 +131,46 @@
     return ret;
 }
 
+int RtosLog::isr_printf(const char* format, ...)
+{
+    // The pool has no "wait for free message" so we use a Sempahore
+    // to keep track of the number of free messages and, if needed,
+    // block the caller until a message is free
+    int available = _sem.wait(0);
+    if (available <= 0) {
+      // no free messages and it is not good to wait in an ISR so
+      // we discard the message
+      return 0;
+    }
+    
+    // Allocate a null terminated message. Will always succeed due to
+    // the semaphore above
+    message_t *message = _mpool.calloc();
+    
+    // Write the callers formatted message 
+    std::va_list args;
+    va_start(args, format);
+    int ret = vsnprintf(message->msg, MessageLen, format, args);
+    va_end(args);
+    
+    // If the entire message could not fit in the preallocated buffer
+    // then allocate a new one and try again.
+    if (ret > MessageLen) {
+        message->ptr = (char*)malloc(ret + 1);
+        if (message->ptr != NULL) {
+            va_start(args, format);
+            ret = vsnprintf(message->ptr, ret + 1, format, args);
+            va_end(args);
+        }
+    }
+    
+    // Send message
+    _queue.put(message);
+    
+    // Note that the Semaphore is not released here, that is done after
+    // the message has been processed and released into the pool by
+    // logTask()
+    //_sem.release();
+    
+    return ret;
+}