From 6f3d7ca4d21c1bd1f05751c1e2777910f20424c3 Mon Sep 17 00:00:00 2001
From: Jonathan Bennett <jbennett@incomsystems.biz>
Date: Fri, 28 Jun 2024 23:30:39 -0500
Subject: [PATCH] Trim extra vprintf and filter for unprintable characters

---
 src/RedirectablePrint.cpp | 43 +++++++++++++--------------------------
 src/RedirectablePrint.h   |  1 -
 src/SerialConsole.cpp     |  2 +-
 3 files changed, 15 insertions(+), 31 deletions(-)

diff --git a/src/RedirectablePrint.cpp b/src/RedirectablePrint.cpp
index ee819643e8..265bb42d66 100644
--- a/src/RedirectablePrint.cpp
+++ b/src/RedirectablePrint.cpp
@@ -50,27 +50,6 @@ size_t RedirectablePrint::write(uint8_t c)
               // serial port said (which could be zero)
 }
 
-size_t RedirectablePrint::vprintf(const char *format, va_list arg)
-{
-    va_list copy;
-    static char printBuf[160];
-
-    va_copy(copy, arg);
-    size_t len = vsnprintf(printBuf, sizeof(printBuf), format, copy);
-    va_end(copy);
-
-    // If the resulting string is longer than sizeof(printBuf)-1 characters, the remaining characters are still counted for the
-    // return value
-
-    if (len > sizeof(printBuf) - 1) {
-        len = sizeof(printBuf) - 1;
-        printBuf[sizeof(printBuf) - 2] = '\n';
-    }
-
-    len = Print::write(printBuf, len);
-    return len;
-}
-
 size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg)
 {
     va_list copy;
@@ -87,14 +66,20 @@ size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_l
         len = sizeof(printBuf) - 1;
         printBuf[sizeof(printBuf) - 2] = '\n';
     }
-    if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
-        Print::write("\u001b[34m", 6);
-    if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
-        Print::write("\u001b[32m", 6);
-    if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
-        Print::write("\u001b[33m", 6);
-    if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
-        Print::write("\u001b[31m", 6);
+    for (size_t f = 0; f < len; f++) {
+        if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n')
+            printBuf[f] = '#';
+    }
+    if (logLevel != nullptr) {
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
+            Print::write("\u001b[34m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
+            Print::write("\u001b[32m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
+            Print::write("\u001b[33m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
+            Print::write("\u001b[31m", 6);
+    }
     len = Print::write(printBuf, len);
     Print::write("\u001b[0m", 5);
     return len;
diff --git a/src/RedirectablePrint.h b/src/RedirectablePrint.h
index c997d3d4e3..a29ad9c749 100644
--- a/src/RedirectablePrint.h
+++ b/src/RedirectablePrint.h
@@ -44,7 +44,6 @@ class RedirectablePrint : public Print
     void log(const char *logLevel, const char *format, ...) __attribute__((format(printf, 3, 4)));
 
     /** like printf but va_list based */
-    size_t vprintf(const char *format, va_list arg);
     size_t vprintf(const char *logLevel, const char *format, va_list arg);
 
     void hexDump(const char *logLevel, unsigned char *buf, uint16_t len);
diff --git a/src/SerialConsole.cpp b/src/SerialConsole.cpp
index 53ece0fa3d..41064f2882 100644
--- a/src/SerialConsole.cpp
+++ b/src/SerialConsole.cpp
@@ -24,7 +24,7 @@ void consolePrintf(const char *format, ...)
 {
     va_list arg;
     va_start(arg, format);
-    console->vprintf(format, arg);
+    console->vprintf(nullptr, format, arg);
     va_end(arg);
     console->flush();
 }