[sdlog] update sdLog API for new zero-copy functions

This implementation makes the sdlog, and in particular the
flight_recorder, much more efficient (4 or 5 times) with less buffer
copies and less calls to tlsf_malloc

sw/airborne/modules/loggers/sdlog_chibios.c

@@ -30,6 +30,7 @@
 #include "modules/loggers/sdlog_chibios/sdLog.h"
 #include "modules/loggers/sdlog_chibios/usbStorage.h"
 #include "modules/loggers/sdlog_chibios.h"
+#include "modules/tlsf/tlsf_malloc.h"
 #include "mcu_periph/adc.h"
 #include "led.h"
 
@@ -84,22 +85,38 @@ FileDes flightRecorderLogFile = -1;
 
 
 // Functions for the generic device API
-static int sdlog_check_free_space(struct chibios_sdlog* p __attribute__((unused)), long *fd __attribute__((unused)), uint16_t len __attribute__((unused)))
+static int sdlog_check_free_space(struct chibios_sdlog* p __attribute__((unused)), long *fd, uint16_t len)
 {
+  SdLogBuffer *sdb;
+  SdioError status = sdLogAllocSDB(&sdb, len);
+  if (status != SDLOG_OK) {
+    return 0;
+  } else {
+    *fd = (long) sdb;
     return 1;
   }
-
-static void sdlog_transmit(struct chibios_sdlog* p, long fd __attribute__((unused)), uint8_t byte)
-{
-  sdLogWriteByte(*p->file, byte);
 }
 
-static void sdlog_transmit_buffer(struct chibios_sdlog* p, long fd __attribute__((unused)), uint8_t *data, uint16_t len)
+static void sdlog_transmit(struct chibios_sdlog* p __attribute__((unused)), long fd, uint8_t byte)
 {
-  sdLogWriteRaw(*p->file, data, len);
+  SdLogBuffer *sdb = (SdLogBuffer *) fd;
+  uint8_t *data = (uint8_t *) sdLogGetBufferFromSDB(sdb);
+  *data = byte;
+  sdLogSeekBufferFromSDB(sdb, 1);
 }
 
-static void sdlog_send(struct chibios_sdlog* p __attribute__((unused)), long fd __attribute__((unused))) { }
+static void sdlog_transmit_buffer(struct chibios_sdlog* p __attribute__((unused)), long fd, uint8_t *data, uint16_t len)
+{
+  SdLogBuffer *sdb = (SdLogBuffer *) fd;
+  memcpy(sdLogGetBufferFromSDB(sdb), data, len);
+  sdLogSeekBufferFromSDB(sdb, len);
+}
+
+static void sdlog_send(struct chibios_sdlog* p, long fd)
+{
+  SdLogBuffer *sdb = (SdLogBuffer *) fd;
+  sdLogWriteSDB(*(p->file), sdb);
+}
 
 static int null_function(struct chibios_sdlog *p __attribute__((unused))) { return 0; }
 

sw/airborne/modules/loggers/sdlog_chibios/msg_queue.c

@@ -43,7 +43,7 @@ void    msgqueue_init(MsgQueue *que, tlsf_memory_heap_t *heap,
                       msg_t *mb_buf, const cnt_t mb_size)
 {
   chMBObjectInit(&que->mb, mb_buf, mb_size);
-  memset(mb_buf, 0, mb_size * sizeof(msg_t *));
+  memset(mb_buf, 0, mb_size * sizeof(msg_t));
   que->heap = heap;
 }
 

sw/airborne/modules/loggers/sdlog_chibios/sdLog.c

@@ -127,6 +127,12 @@ typedef enum {
   FCNTL_EXIT =  0b11
 } FileFcntl;
 
+struct _SdLogBuffer {
+  LogMessage *lm;
+  size_t     len;
+  uint32_t   offset;
+};
+
 
 
 #define LOG_MESSAGE_PREBUF_LEN (SDLOG_MAX_MESSAGE_LEN+sizeof(LogMessage))
@@ -162,6 +168,12 @@ SdioError sdLogInit(uint32_t *freeSpaceInKo)
   DWORD clusters = 0;
   FATFS *fsp = NULL;
 
+  // if init is already done, return ERROR
+  if (sdLogThd != NULL) {
+    *freeSpaceInKo = 0;
+    return SDLOG_WAS_LAUNCHED;
+  }
+
 #ifdef SDLOG_NEED_QUEUE
   msgqueue_init(&messagesQueue, &HEAP_DEFAULT, queMbBuffer, SDLOG_QUEUE_BUCKETS);
 #endif
@@ -182,7 +194,7 @@ SdioError sdLogInit(uint32_t *freeSpaceInKo)
 #if _FATFS < 8000
   FRESULT rc = f_mount(0, &fatfs);
 #else
-  FRESULT rc = f_mount(&fatfs, "", 0);
+  FRESULT rc = f_mount(&fatfs, "/", 1);
 #endif
 
   if (rc != FR_OK) {
@@ -340,6 +352,7 @@ SdioError sdLogWriteLog(const FileDes fd, const char *fmt, ...)
 
   LogMessage *lm = tlsf_malloc_r(&HEAP_DEFAULT, LOG_MESSAGE_PREBUF_LEN);
   if (lm == NULL) {
+    va_end(ap);
     return SDLOG_QUEUEFULL;
   }
 
@@ -403,7 +416,7 @@ SdioError sdLogCloseLog(const FileDes fd)
   lm->op.fcntl = FCNTL_CLOSE;
   lm->op.fd = fd & 0x1f;
 
-  if (msgqueue_send(&messagesQueue, lm, sizeof(lm), MsgQueue_REGULAR) < 0) {
+  if (msgqueue_send(&messagesQueue, lm, sizeof(LogMessage), MsgQueue_REGULAR) < 0) {
     return SDLOG_QUEUEFULL;
   }
 
@@ -458,6 +471,78 @@ SdioError sdLogWriteRaw(const FileDes fd, const uint8_t *buffer, const size_t le
   return SDLOG_OK;
 }
 
+SdioError sdLogAllocSDB(SdLogBuffer **sdb, const size_t len)
+{
+  *sdb = tlsf_malloc_r(&HEAP_DEFAULT, logRawLen(len));
+  if (*sdb == NULL) {
+    return SDLOG_QUEUEFULL;
+  }
+
+  LogMessage *lm = tlsf_malloc_r(&HEAP_DEFAULT, logRawLen(len));
+  if (lm == NULL) {
+    tlsf_free_r(&HEAP_DEFAULT, *sdb);
+    return SDLOG_QUEUEFULL;
+  }
+
+  (*sdb)->lm = lm;
+  (*sdb)->len = len;
+  (*sdb)->offset = 0;
+  return SDLOG_OK;
+}
+
+char *sdLogGetBufferFromSDB(SdLogBuffer *sdb)
+{
+  return sdb->lm->mess + sdb->offset;
+}
+
+bool sdLogSeekBufferFromSDB(SdLogBuffer *sdb, uint32_t offset)
+{
+  if ((sdb->offset + offset) < sdb->len) {
+    sdb->offset += offset;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+size_t sdLogGetBufferLenFromSDB(SdLogBuffer *sdb)
+{
+  return sdb->len - sdb->offset;
+}
+
+SdioError sdLogWriteSDB(const FileDes fd, SdLogBuffer *sdb)
+{
+  SdioError status = SDLOG_OK;
+
+  if ((fd >= SDLOG_NUM_BUFFER) || (fileDes[fd].inUse == false)) {
+    status = SDLOG_FATFS_ERROR;
+    goto fail;
+  }
+
+  status = flushWriteByteBuffer(fd);
+  if (status != SDLOG_OK) {
+    goto fail;
+  }
+
+  sdb->lm->op.fcntl = FCNTL_WRITE;
+  sdb->lm->op.fd = fd & 0x1f;
+
+  if (msgqueue_send(&messagesQueue, sdb->lm, logRawLen(sdb->len), MsgQueue_REGULAR) < 0) {
+    // msgqueue_send take care of freeing lm memory even in case of failure
+    // just need to free sdb memory
+    status = SDLOG_QUEUEFULL;
+  }
+
+  goto exit;
+
+fail:
+  tlsf_free_r(&HEAP_DEFAULT, sdb->lm);
+
+exit:
+  tlsf_free_r(&HEAP_DEFAULT, sdb);
+  return status;
+}
+
 
 
 SdioError sdLogWriteByte(const FileDes fd, const uint8_t value)

sw/airborne/modules/loggers/sdlog_chibios/sdLog.h

@@ -59,7 +59,7 @@ extern "C" {
  sdLogInit (initialize peripheral,  verify sdCard availibility)
  sdLogOpenLog : open file
  sdLogWriteXXX
-r sdLogFlushLog : flush buffer (optional)
+ sdLogFlushLog : flush buffer (optional)
  sdLogCloseLog
  sdLogFinish
 
@@ -92,12 +92,15 @@ typedef enum {
   SDLOG_QUEUEFULL,
   SDLOG_NOTHREAD,
   SDLOG_INTERNAL_ERROR,
-  SDLOG_LOGNUM_ERROR
+  SDLOG_LOGNUM_ERROR,
+  SDLOG_WAS_LAUNCHED
 } SdioError;
 
-
+typedef struct _SdLogBuffer SdLogBuffer;
 typedef int8_t FileDes;
 
+
+
 /**
  * @brief initialise sdLog
  * @details init sdio peripheral, verify sdCard is inserted, check and mount filesystem,
@@ -105,6 +108,7 @@ typedef int8_t FileDes;
  *          This function is available even without thread login facility : even
  *          if SDLOG_XXX macro are zeroed
  * @param[out]  freeSpaceInKo : if pointer in nonnull, return free space on filesystem
+ * @return status (always check status)
  */
 SdioError sdLogInit(uint32_t *freeSpaceInKo);
 
@@ -120,6 +124,7 @@ SdioError sdLogInit(uint32_t *freeSpaceInKo);
  * @param[out]  nextFileName : file with path ready to be used for f_open system call
  * @param[in] nameLength : length of previous buffer
  * @param[in] indexOffset : use 0 to retrieve last existent filename, 1 for next filename
+ * @return status (always check status)
  */
 SdioError getFileName(const char *prefix, const char *directoryName,
                       char *nextFileName, const size_t nameLength, const int indexOffset);
@@ -136,6 +141,7 @@ SdioError getFileName(const char *prefix, const char *directoryName,
  * @param[in] prefix : the pattern for the file : example LOG_
  * @param[in] directoryName : root directory where to find file
  * @param[in] sizeConsideredEmpty : file whose size is less or equal to that value will be removed
+ * @return status (always check status)
  */
 SdioError removeEmptyLogs(const char *directoryName, const char *prefix,
                           const size_t sizeConsideredEmpty);
@@ -144,6 +150,7 @@ SdioError removeEmptyLogs(const char *directoryName, const char *prefix,
  * @details unmount filesystem, free sdio peripheral
  *          This function is available even without thread login facility : even
  *          if SDLOG_XXX macro are zeroed
+ * @return status (always check status)
  */
 SdioError sdLogFinish(void);
 
@@ -158,6 +165,7 @@ SdioError sdLogFinish(void);
  * @param[in] appendTagAtClose : at close, a marker will be added to prove that the file is complete
  *            and not corrupt. useful for text logging purpose, but probably not wanted fort binary
  *            files.
+ * @return status (always check status)
  */
 SdioError sdLogOpenLog(FileDes *fileObject, const char *directoryName, const char *fileName,
                        bool appendTagAtClose);
@@ -166,6 +174,7 @@ SdioError sdLogOpenLog(FileDes *fileObject, const char *directoryName, const cha
 /**
  * @brief flush ram buffer associated with file to sdCard
  * @param[in] fileObject : file descriptor returned by sdLogOpenLog
+ * @return status (always check status)
  */
 SdioError sdLogFlushLog(const FileDes fileObject);
 
@@ -173,6 +182,7 @@ SdioError sdLogFlushLog(const FileDes fileObject);
 /**
  * @brief flush ram buffer then close file.
  * @param[in] fileObject : file descriptor returned by sdLogOpenLog
+ * @return status (always check status)
  */
 SdioError sdLogCloseLog(const FileDes fileObject);
 
@@ -182,17 +192,16 @@ SdioError sdLogCloseLog(const FileDes fileObject);
  *      if false : close imediatly files without flushing buffers,
  *                 more chance to keep filesystem integrity in case of
  *                 emergency close after power outage is detected
+ * @return status (always check status)
  */
 SdioError sdLogCloseAllLogs(bool flush);
 
 
-
-
-
 /**
  * @brief log text
  * @param[in] fileObject : file descriptor returned by sdLogOpenLog
  * @param[in] fmt : format and args in printf convention
+ * @return status (always check status)
  */
 SdioError sdLogWriteLog(const FileDes fileObject, const char *fmt, ...);
 
@@ -202,14 +211,72 @@ SdioError sdLogWriteLog(const FileDes fileObject, const char *fmt, ...);
  * @param[in] fileObject : file descriptor returned by sdLogOpenLog
  * @param[in] buffer : memory pointer on buffer
  * @param[in] len : number of bytes to write
+ * @return status (always check status)
  */
 SdioError sdLogWriteRaw(const FileDes fileObject, const uint8_t *buffer, const size_t len);
 
+/**
+ * @brief log binary data limiting buffer copy by preallocating space
+ * @param[in] len : number of bytes to write
+ * @param[out] sdb : pointer to opaque object pointer containing buffer
+ *                   there is two accessor functions (below) to access
+ *                   buffer ptr and buffer len.
+ * @details usage of the set of 4 functions :
+ *          SdLogBuffer *sdb;
+ *          sdLogAllocSDB (&sdb, 100);
+ *          memcpy (getBufferFromSDB(sdb), SOURCE, offset);
+ *          sdLogSeekBufferFromSDB (sdb, offset);
+ *          sdLogWriteSDB (file, sdb);
+ * @return status (always check status)
+ */
+SdioError sdLogAllocSDB(SdLogBuffer **sdb, const size_t len);
+
+/**
+ * @brief return a pointer of the writable area of a preallocated
+ *        message + offset managed by sdLogSeekBufferFromSDB
+ * @param[in] sdb : pointer to opaque object containing buffer
+ *                  and previously filled by sdLogAllocSDB
+ * @return pointer to writable area
+ */
+char *sdLogGetBufferFromSDB(SdLogBuffer *sdb);
+
+
+/**
+ * @brief manage internal offset in user buffer
+ * @param[in] sdb : pointer to opaque object containing buffer
+ *            and previously filled by sdLogAllocSDB
+ *            offset : increment internal offset with this value
+ * @return true if offset is withing internal buffer boundary
+ *         false if offset is NOT withing internal buffer boundary, in this case,
+ *         no keek is done
+ */
+bool sdLogSeekBufferFromSDB(SdLogBuffer *sdb, uint32_t offset);
+
+
+/**
+ * @brief return len of the writable area of a preallocated message (this take into account
+ *        the offset)
+ * @param[in] sdb : pointer to opaque object containing buffer
+ *            and previously filled by sdLogAllocSDB
+ * @return len of writable area
+ */
+size_t sdLogGetBufferLenFromSDB(SdLogBuffer *sdb);
+
+/**
+ * @brief send a preallocted message after it has been filled
+ * @param[in] fileObject : file descriptor returned by sdLogOpenLog
+ * @param[in] sdb : pointer to opaque object containing buffer
+ *                  and previously filled by sdLogAllocSDB
+ * @return status (always check status)
+*/
+SdioError sdLogWriteSDB(const FileDes fd, SdLogBuffer *sdb);
+
 
 /**
  * @brief log one byte of binary data
  * @param[in] fileObject : file descriptor returned by sdLogOpenLog
  * @param[in] value : byte to log
+ * @return status (always check status)
  */
 SdioError sdLogWriteByte(const FileDes fileObject, const uint8_t value);
 #endif