feat(ServiceInterfacePrinter): ringbuffer printer

Previously the printer used a 2d array mechanism to queue up messages,
by selecting a free slot with help of a etl::bitset for tracking. A
message is then sent to the callback what slot is filled up and the
callback then drains the entire queue and prints out the messages.

This was prone to deadline issues, since the entire queue was always
flushed and often I noticed a deadline missed messages when developing
other stuff. Also memory is inefficiently used with the 2d array.

This new version fixes the above using a ring buffer datastructure. We
use a flat array and 2 integer pointers for storing and tracking the
bytes to print. So now there isn't any wasted space between messages.
Also with the design of the ring buffer messages can be written in and
read out at the same time, so we have minimal mutex use (just for
updating the integer pointers). To address the deadline issue, the
callback also only prints out a limited number of bytes per cycle.

(Also the printer code in general has been optimized a bit, since it was
quite needlessly big)

src/fsfw/serviceinterface/CMakeLists.txt

@@ -1,5 +1,4 @@
 target_sources(
   ${LIB_FSFW_NAME}
   PRIVATE ServiceInterfaceStream.cpp ServiceInterfaceBuffer.cpp
-          ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp
-          ServiceInterfacePrinterMessage.cpp)
+          ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp)

src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp

@@ -1,13 +1,12 @@
 #include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
 
+#include <algorithm>
 #include <cstdarg>
+#include <cstring>
 
-#include "ServiceInterfacePrinterMessage.h"
-#include "etl/bitset.h"
-#include "etl/queue.h"
 #include "fsfw/FSFW.h"
+#include "fsfw/ipc/MutexFactory.h"
 #include "fsfw/ipc/MutexGuard.h"
-#include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
 #include "fsfw/timemanager/Clock.h"
 
@@ -18,33 +17,32 @@ static bool consoleInitialized = false;
 
 #if FSFW_DISABLE_PRINTOUT == 0
 
-typedef etl::bitset<fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT> bitset;
+namespace {
 
-static bool addCrAtEnd = false;
-static bool initializedAndReady = false;
-static bool replaceLastCharWithNewline = false;
+// Formatted messages are staged in a ring buffer and drained by the print task
+// in bounded chunks, so producers never block on the slow debug UART.
+constexpr size_t RING_SIZE =
+    fsfwconfig::FSFW_PRINT_BUFFER_SIZE * fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT;
 
-bitset bufferState{};
-std::array<char[fsfwconfig::FSFW_PRINT_BUFFER_SIZE], fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT>
-    printBufferArray = {};
+constexpr size_t MAX_BYTES_PER_CYCLE = 512;
 
-uint32_t droppedMessagesCounter = 0;
-uint32_t droppedMessagesCounterContinuous = 0;
+// The print ring buffer
+char ring[RING_SIZE];
+MutexIF* ringMutex = nullptr;
 
-MutexIF* bufferMutex;
-MessageQueueIF* bufferQueue;
+size_t readIdx = 0;    // Start of data to print out
+size_t bytesUsed = 0;  // Pending data amount to print out
+uint32_t droppedMessages = 0;
+uint32_t droppedMessagesTotal = 0;
 
-size_t selectFreeBuffer() {
-  MutexGuard guard(bufferMutex, MutexIF::TimeoutType::WAITING, 10);
-  const size_t bufferIdx = bufferState.find_first(false);
-  if (bufferIdx == bitset::npos) {
-    return bitset::npos;
-  }
-  bufferState.set(bufferIdx, true);
-  return bufferIdx;
-}
+bool addCrAtEnd = false;
+bool replaceLastCharWithNewline = false;
 
-void fsfwPrint(const sif::PrintLevel printType, const char* fmt, va_list arg) {
+// False until the print task runs.
+// Prints go directly to stdout before that.
+bool taskRunning = false;
+
+void fsfwPrint(sif::PrintLevel printType, const char* fmt, va_list arg) {
 #if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
   if (not consoleInitialized) {
     HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
@@ -56,89 +54,73 @@ void fsfwPrint(const sif::PrintLevel printType, const char* fmt, va_list arg) {
   consoleInitialized = true;
 #endif
 
-  /* Check logger level */
   if (printType == sif::PrintLevel::NONE or printType > printLevel) {
     return;
   }
 
-  const bool isReady = initializedAndReady;
-  const size_t bufferIdx = isReady ? selectFreeBuffer() : 0;
-  if (bufferIdx == bitset::npos) {
-    // This will lose updates, but we are mostly concerned with if we are dropping messages
-    ++droppedMessagesCounter;
-    return;
-  }
-
-  size_t len = 0;
-  char* bufferPosition = printBufferArray[bufferIdx];
-
-  /* Log message to terminal */
-
+  static const char* const labels[] = {"", "ERROR  ", "WARNING", "INFO   ", "DEBUG  "};
 #if FSFW_COLORED_OUTPUT == 1
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_GREEN);
-  } else if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_CYAN);
-  } else if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_YELLOW);
-  } else if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_RED);
-  }
-#endif
-
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition + len, "INFO   ");
-  }
-  if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition + len, "DEBUG  ");
-  }
-  if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition + len, "WARNING");
-  }
-  if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition + len, "ERROR  ");
-  }
-
-#if FSFW_COLORED_OUTPUT == 1
-  len += sprintf(bufferPosition + len, sif::ANSI_COLOR_RESET);
+  static const char* const colors[] = {"", sif::ANSI_COLOR_RED, sif::ANSI_COLOR_YELLOW,
+                                       sif::ANSI_COLOR_GREEN, sif::ANSI_COLOR_CYAN};
+  const char* color = colors[printType];
+  const char* reset = sif::ANSI_COLOR_RESET;
+#else
+  const char* color = "";
+  const char* reset = "";
 #endif
 
   Clock::TimeOfDay_t now;
   Clock::getDateAndTime(&now);
-  /*
-   * Log current time to terminal if desired.
-   */
-  len += sprintf(bufferPosition + len, " | %02lu:%02lu:%02lu.%03lu | ", (unsigned long)now.hour,
-                 (unsigned long)now.minute, (unsigned long)now.second,
-                 (unsigned long)now.usecond / 1000);
 
-  len += vsnprintf(bufferPosition + len, sizeof(printBufferArray[bufferIdx]) - len, fmt, arg);
+  char buf[fsfwconfig::FSFW_PRINT_BUFFER_SIZE + 2];  // slack for the CR/newline
 
-  if (addCrAtEnd) {
-    len += sprintf(bufferPosition + len, "\r");
+  // Need to clamp, since snprintf returns WOULD be written length (not actual; could be higher than
+  // really)
+  const auto prefixLen =
+      std::min(static_cast<size_t>(snprintf(
+                   buf, fsfwconfig::FSFW_PRINT_BUFFER_SIZE, "%s%s%s | %02u:%02u:%02u.%03u | ",
+                   color, labels[printType], reset, static_cast<unsigned>(now.hour),
+                   static_cast<unsigned>(now.minute), static_cast<unsigned>(now.second),
+                   static_cast<unsigned>(now.usecond / 1000))),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1);
+
+  // Same here
+  const auto msgLen =
+      std::min(static_cast<size_t>(vsnprintf(
+                   buf + prefixLen, fsfwconfig::FSFW_PRINT_BUFFER_SIZE - prefixLen, fmt, arg)),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1 - prefixLen);
+
+  size_t textLen = prefixLen + msgLen;
+  if (addCrAtEnd and buf[textLen - 1] == '\n') {
+    buf[textLen++] = '\r';
+  }
+  if (replaceLastCharWithNewline and buf[textLen - 1] != '\n' and buf[textLen - 1] != '\r') {
+    buf[textLen++] = '\n';
   }
 
-  printBufferArray[bufferIdx][fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1] = 0;
-
-  if (replaceLastCharWithNewline) {
-    const size_t stringLength = strlen(bufferPosition);
-    const size_t lastCharPosition =
-        etl::min(stringLength - 1, fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 3);
-    const char lastChar = printBufferArray[bufferIdx][lastCharPosition];
-    if (!(lastChar == '\n' or lastChar == '\r')) {
-      printBufferArray[bufferIdx][lastCharPosition + 1] = '\n';
-      printBufferArray[bufferIdx][lastCharPosition + 2] = 0;
-    }
+  if (not taskRunning) {
+    fwrite(buf, 1, textLen, stdout);
+    return;
   }
 
-  if (isReady) {
-    ServiceInterfacePrinterMessage message(bufferIdx);
-    bufferQueue->sendToDefault(&message);
-  } else {
-    printf("%s", printBufferArray[bufferIdx]);
+  MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+
+  if (textLen > RING_SIZE - bytesUsed) {
+    ++droppedMessages;
+    return;
   }
+
+  const size_t writeIdx = (readIdx + bytesUsed) % RING_SIZE;
+  const size_t firstPart = std::min(textLen, RING_SIZE - writeIdx);
+
+  std::memcpy(ring + writeIdx, buf, firstPart);
+  std::memcpy(ring, buf + firstPart, textLen - firstPart);
+
+  bytesUsed += textLen;
 }
 
+}  // namespace
+
 void sif::setToAddCrAtEnd(const bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; }
 
 void sif::setReplaceLastCharWithNewline(const bool replace) {
@@ -174,35 +156,43 @@ void sif::printError(const char* fmt, ...) {
 }
 
 void sif::printCallback() {
-  if (!initializedAndReady) {
-    initializedAndReady = true;
-  }
-  uint32_t bitmask = 0;
-  ServiceInterfacePrinterMessage message;
-  while (bufferQueue->receiveMessage(&message) == returnvalue::OK) {
-    const size_t bufferIdx = message.getBufferIndex();
-    bitmask |= 1 << bufferIdx;
-    printf("%s", printBufferArray[bufferIdx]);
-  }
+  taskRunning = true;
+
+  size_t chunk;
   {
-    MutexGuard guard(bufferMutex, MutexIF::TimeoutType::BLOCKING);
-    bufferState &= ~bitmask;
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    chunk = std::min(bytesUsed, MAX_BYTES_PER_CYCLE);
   }
-  const uint32_t droppedMessages = droppedMessagesCounter;
-  droppedMessagesCounter = 0;
-  droppedMessagesCounterContinuous += droppedMessages;
-  if (droppedMessages != 0) {
-    sif::printError("ServiceInterfacePrinter: Dropped ca. %i messages\n", droppedMessages);
+
+  if (chunk > 0) {
+    // Safe without the mutex: producers only touch the ring beyond
+    // readIdx + bytesUsed and only the print task advances readIdx.
+    const size_t firstPart = std::min(chunk, RING_SIZE - readIdx);
+    fwrite(ring + readIdx, 1, firstPart, stdout);
+    fwrite(ring, 1, chunk - firstPart, stdout);
+    fflush(stdout);
+  }
+
+  uint32_t dropped;
+  {
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    readIdx = (readIdx + chunk) % RING_SIZE;
+    bytesUsed -= chunk;
+    dropped = droppedMessages;
+    droppedMessages = 0;
+  }
+
+  droppedMessagesTotal += dropped;
+
+  if (dropped != 0) {
+    sif::printError("ServiceInterfacePrinter: Dropped %lu messages\n",
+                    static_cast<unsigned long>(dropped));
   }
 }
 
-void sif::init() {
-  bufferMutex = MutexFactory::instance()->createMutex();
-  bufferQueue = QueueFactory::instance()->createMessageQueue(fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT);
-  bufferQueue->setDefaultDestination(bufferQueue->getId());
-}
+void sif::init() { ringMutex = MutexFactory::instance()->createMutex(); }
 
-uint32_t sif::getDroppedMessagesCount() { return droppedMessagesCounterContinuous; }
+uint32_t sif::getDroppedMessagesCount() { return droppedMessagesTotal; }
 
 #else
 

src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp

@@ -1,18 +0,0 @@
-#include "ServiceInterfacePrinterMessage.h"
-
-#include <cstring>
-
-ServiceInterfacePrinterMessage::ServiceInterfacePrinterMessage(const size_t bufferIdx) {
-  this->MessageQueueMessage::setMessageSize(sizeof(bufferIdx));
-  this->setBufferIndex(bufferIdx);
-}
-
-void ServiceInterfacePrinterMessage::setBufferIndex(const size_t bufferIdx) {
-  std::memcpy(this->getData(), &bufferIdx, sizeof(bufferIdx));
-}
-
-size_t ServiceInterfacePrinterMessage::getBufferIndex() {
-  size_t tempIdx;
-  std::memcpy(&tempIdx, this->getData(), sizeof(size_t));
-  return tempIdx;
-}

src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h

@@ -1,12 +0,0 @@
-#pragma once
-#include "fsfw/ipc/MessageQueueMessage.h"
-
-class ServiceInterfacePrinterMessage : public MessageQueueMessage {
- public:
-  ServiceInterfacePrinterMessage() = default;
-  explicit ServiceInterfacePrinterMessage(size_t bufferIdx);
-  size_t getBufferIndex();
-
- private:
-  void setBufferIndex(size_t bufferIdx);
-};