From 43cba675310e0cfda3e325092aed6555a370ed48 Mon Sep 17 00:00:00 2001
From: Thomas Basler <thomas@familie-basler.net>
Date: Thu, 3 Aug 2023 22:16:29 +0200
Subject: [PATCH] Fix: Virtual console scrambled output when the output came
 from different contexts

---
 include/MessageOutput.h |  7 ++++---
 src/MessageOutput.cpp   | 29 ++++++++++++++---------------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/include/MessageOutput.h b/include/MessageOutput.h
index a47b0278..7c56a6f4 100644
--- a/include/MessageOutput.h
+++ b/include/MessageOutput.h
@@ -4,14 +4,15 @@
 #include <AsyncWebSocket.h>
 #include <HardwareSerial.h>
 #include <Stream.h>
+#include <mutex>
 
 #define BUFFER_SIZE 500
 
 class MessageOutputClass : public Print {
 public:
-    MessageOutputClass();
     void loop();
-    size_t write(uint8_t c);
+    size_t write(uint8_t c) override;
+    size_t write(const uint8_t *buffer, size_t size) override;
     void register_ws_output(AsyncWebSocket* output);
 
 private:
@@ -21,7 +22,7 @@ private:
     uint32_t _lastSend = 0;
     bool _forceSend = false;
 
-    SemaphoreHandle_t _lock;
+    std::mutex _msgLock;
 };
 
 extern MessageOutputClass MessageOutput;
\ No newline at end of file
diff --git a/src/MessageOutput.cpp b/src/MessageOutput.cpp
index dc9c6715..23c644bc 100644
--- a/src/MessageOutput.cpp
+++ b/src/MessageOutput.cpp
@@ -8,17 +8,6 @@
 
 MessageOutputClass MessageOutput;
 
-#define MSG_LOCK() \
-    do {           \
-    } while (xSemaphoreTake(_lock, portMAX_DELAY) != pdPASS)
-#define MSG_UNLOCK() xSemaphoreGive(_lock)
-
-MessageOutputClass::MessageOutputClass()
-{
-    _lock = xSemaphoreCreateMutex();
-    MSG_UNLOCK();
-}
-
 void MessageOutputClass::register_ws_output(AsyncWebSocket* output)
 {
     _ws = output;
@@ -27,10 +16,9 @@ void MessageOutputClass::register_ws_output(AsyncWebSocket* output)
 size_t MessageOutputClass::write(uint8_t c)
 {
     if (_buff_pos < BUFFER_SIZE) {
-        MSG_LOCK();
+        std::lock_guard<std::mutex> lock(_msgLock);
         _buffer[_buff_pos] = c;
         _buff_pos++;
-        MSG_UNLOCK();
     } else {
         _forceSend = true;
     }
@@ -38,11 +26,23 @@ size_t MessageOutputClass::write(uint8_t c)
     return Serial.write(c);
 }
 
+size_t MessageOutputClass::write(const uint8_t* buffer, size_t size)
+{
+    std::lock_guard<std::mutex> lock(_msgLock);
+    if (_buff_pos + size < BUFFER_SIZE) {
+        memcpy(&_buffer[_buff_pos], buffer, size);
+        _buff_pos += size;
+    }
+    _forceSend = true;
+
+    return Serial.write(buffer, size);
+}
+
 void MessageOutputClass::loop()
 {
     // Send data via websocket if either time is over or buffer is full
     if (_forceSend || (millis() - _lastSend > 1000)) {
-        MSG_LOCK();
+        std::lock_guard<std::mutex> lock(_msgLock);
         if (_ws && _buff_pos > 0) {
             _ws->textAll(_buffer, _buff_pos);
             _buff_pos = 0;
@@ -50,7 +50,6 @@ void MessageOutputClass::loop()
         if (_forceSend) {
             _buff_pos = 0;
         }
-        MSG_UNLOCK();
         _forceSend = false;
     }
 }
\ No newline at end of file