From 2eb924929acbd3ea566f4bdd9b47a1c147800a5f Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sun, 22 Sep 2019 11:33:25 +0200
Subject: [PATCH] The "buffer full" marker functionality seems to be broken,
 fixed by rewriting part if it. Buffer full marker added after current message
 if possible. Incoming data discarded until current message is sent. Closes
 #471

---
 src/modules/src/console.c | 68 +++++++++++++++++++++++++++++++--------
 1 file changed, 54 insertions(+), 14 deletions(-)

diff --git a/src/modules/src/console.c b/src/modules/src/console.c
index e147742d82..5ebc2b25aa 100644
--- a/src/modules/src/console.c
+++ b/src/modules/src/console.c
@@ -25,6 +25,7 @@
  */
 
 #include <stdbool.h>
+#include <string.h>
 
 /*FreeRtos includes*/
 #include "FreeRTOS.h"
@@ -41,22 +42,26 @@
 #endif
 #endif
 
-CRTPPacket messageToPrint;
-xSemaphoreHandle synch = NULL;
+static CRTPPacket messageToPrint;
+static bool messageSendingIsPending = false;
+static xSemaphoreHandle synch = NULL;
 
-static const char fullMsg[] = "<F>\n";
+static const char bufferFullMsg[] = "<F>\n";
 static bool isInit;
 
+static void addBufferFullMarker();
+
+
 /**
  * Send the data to the client
  * returns TRUE if successful otherwise FALSE
  */
 static bool consoleSendMessage(void)
 {
-
   if (crtpSendPacket(&messageToPrint) == pdTRUE)
   {
     messageToPrint.size = 0;
+    messageSendingIsPending = false;
   }
   else
   {
@@ -74,6 +79,7 @@ void consoleInit()
   messageToPrint.size = 0;
   messageToPrint.header = CRTP_HEADER(CRTP_PORT_CONSOLE, 0);
   vSemaphoreCreateBinary(synch);
+  messageSendingIsPending = false;
 
   isInit = true;
 }
@@ -85,7 +91,6 @@ bool consoleTest(void)
 
 int consolePutchar(int ch)
 {
-  int i;
   bool isInInterrupt = (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
 
   if (!isInit) {
@@ -98,22 +103,29 @@ int consolePutchar(int ch)
 
   if (xSemaphoreTake(synch, portMAX_DELAY) == pdTRUE)
   {
-    if (messageToPrint.size < CRTP_MAX_DATA_SIZE)
+    // Try to send if we already have a pending message
+    if (messageSendingIsPending) 
     {
-      messageToPrint.data[messageToPrint.size] = (unsigned char)ch;
-      messageToPrint.size++;
+      consoleSendMessage();
     }
-    if (ch == '\n' || messageToPrint.size >= CRTP_MAX_DATA_SIZE)
+
+    if (! messageSendingIsPending) 
     {
-      if (crtpGetFreeTxQueuePackets() == 1)
+      if (messageToPrint.size < CRTP_MAX_DATA_SIZE)
+      {
+        messageToPrint.data[messageToPrint.size] = (unsigned char)ch;
+        messageToPrint.size++;
+      }
+
+      if (ch == '\n' || messageToPrint.size >= CRTP_MAX_DATA_SIZE)
       {
-        for (i = 0; i < sizeof(fullMsg) && (messageToPrint.size - i) > 0; i++)
+        if (crtpGetFreeTxQueuePackets() == 1)
         {
-          messageToPrint.data[messageToPrint.size - i] =
-              (uint8_t)fullMsg[sizeof(fullMsg) - i - 1];
+          addBufferFullMarker();
         }
+        messageSendingIsPending = true;
+        consoleSendMessage();
       }
-      consoleSendMessage();
     }
     xSemaphoreGive(synch);
   }
@@ -154,3 +166,31 @@ void consoleFlush(void)
     xSemaphoreGive(synch);
   }
 }
+
+
+static int findMarkerStart()
+{
+  int start = messageToPrint.size;
+  
+  // If last char is new line, rewind one char since the marker contains a new line.
+  if (start > 0 && messageToPrint.data[start - 1] == '\n')
+  {
+    start -= 1;
+  }
+
+  return start;
+}
+
+static void addBufferFullMarker()
+{
+  // Try to add the marker after the message if it fits in the buffer, otherwise overwrite the end of the message 
+  int endMarker = findMarkerStart() + sizeof(bufferFullMsg);
+  if (endMarker >= (CRTP_MAX_DATA_SIZE)) 
+  {
+    endMarker = CRTP_MAX_DATA_SIZE;
+  }
+
+  int startMarker = endMarker - sizeof(bufferFullMsg);
+  memcpy(&messageToPrint.data[startMarker], bufferFullMsg, sizeof(bufferFullMsg));
+  messageToPrint.size = startMarker + sizeof(bufferFullMsg);
+}