diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 44ae9fee..16ff7b9f 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1268,7 +1268,7 @@ ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message) void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen, DeviceCommandId_t replyId, bool forceDirectTm) { - SerialBufferAdapter bufferWrapper(rawData, rawDataLen); + SerialBufferAdapter bufferWrapper(rawData, rawDataLen); handleDeviceTm(bufferWrapper, replyId, forceDirectTm); } diff --git a/src/fsfw/serviceinterface/CMakeLists.txt b/src/fsfw/serviceinterface/CMakeLists.txt index 586fdadb..c7c0b1fa 100644 --- a/src/fsfw/serviceinterface/CMakeLists.txt +++ b/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) diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp b/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp index 373aa7d7..8d758344 100644 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp +++ b/src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp @@ -1,13 +1,12 @@ #include "fsfw/serviceinterface/ServiceInterfacePrinter.h" +#include #include +#include -#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 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 - 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(snprintf( + buf, fsfwconfig::FSFW_PRINT_BUFFER_SIZE, "%s%s%s | %02u:%02u:%02u.%03u | ", + color, labels[printType], reset, static_cast(now.hour), + static_cast(now.minute), static_cast(now.second), + static_cast(now.usecond / 1000))), + fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1); + + // Same here + const auto msgLen = + std::min(static_cast(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(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 diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp deleted file mode 100644 index 8753f14e..00000000 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "ServiceInterfacePrinterMessage.h" - -#include - -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; -} diff --git a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h b/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h deleted file mode 100644 index f013c665..00000000 --- a/src/fsfw/serviceinterface/ServiceInterfacePrinterMessage.h +++ /dev/null @@ -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); -};