From 9c60e7d6f97eae8a53d882bb2fdffb8e913ecb9b Mon Sep 17 00:00:00 2001
From: Petri Mattila <petri.t.j.mattila@gmail.com>
Date: Fri, 28 Jun 2024 17:44:57 +0100
Subject: [PATCH] Add dprintf for debug prints on a serial port

---
 make/source.mk           |  1 +
 src/main/build/dprintf.c | 62 ++++++++++++++++++++++++++++++++++++++++
 src/main/build/dprintf.h | 39 +++++++++++++++++++++++++
 src/main/io/serial.h     |  1 +
 4 files changed, 103 insertions(+)
 create mode 100644 src/main/build/dprintf.c
 create mode 100644 src/main/build/dprintf.h

diff --git a/make/source.mk b/make/source.mk
index d73d333f9e..bd8f7f0ddc 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -3,6 +3,7 @@ COMMON_SRC = \
             build/build_config.c \
             build/debug.c \
             build/debug_pin.c \
+            build/dprintf.c \
             build/version.c \
             $(TARGET_DIR_SRC) \
             $(addprefix pg/, $(notdir $(wildcard $(SRC_DIR)/pg/*.c))) \
diff --git a/src/main/build/dprintf.c b/src/main/build/dprintf.c
new file mode 100644
index 0000000000..492a4958e6
--- /dev/null
+++ b/src/main/build/dprintf.c
@@ -0,0 +1,62 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "types.h"
+
+#include "dprintf.h"
+
+#ifdef USE_SERIAL_DPRINTF
+
+#include "drivers/serial.h"
+
+static serialPort_t * debugSerialPort = NULL;
+
+/*
+ * In the code to be debugged, call this init function with suitable
+ * parameters, for example:
+ *
+ *      initDebugSerial(SERIAL_PORT_USART6, 921600);
+ *
+ * The selected serial port should not be used elsewhere.
+ */
+
+void initDebugSerial(serialPortIdentifier_e port, uint32_t baudRate)
+{
+    debugSerialPort = openSerialPort(port, FUNCTION_DPRINTF, NULL, NULL, baudRate, MODE_TX, 0);
+    dprintf("\r\nDebug port ready\r\n");
+}
+
+/*
+ * This works exactly the same as printf, but into the selected serial port.
+ * Nearly all common printf formats are supported.
+ * For end-of-line, use "\r\n"
+ */
+
+int dprintf(const char *fmt, ...)
+{
+    if (debugSerialPort) {
+        va_list va;
+        va_start(va, fmt);
+        int written = tfp_format(debugSerialPort, (putcf)serialWrite, fmt, va);
+        va_end(va);
+        return written;
+    }
+
+    return 0;
+}
+
+#endif
diff --git a/src/main/build/dprintf.h b/src/main/build/dprintf.h
new file mode 100644
index 0000000000..4b7d8ea27a
--- /dev/null
+++ b/src/main/build/dprintf.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#include "io/serial.h"
+#include "common/printf.h"
+
+/*
+ * When debugging code, define this here, or on the command line.
+ *
+ * The dprintfs should be used only in debugging, and must _NOT_
+ * be left in the final code!
+ */
+//#define USE_SERIAL_DPRINTF
+
+#ifdef USE_SERIAL_DPRINTF
+
+void initDebugSerial(serialPortIdentifier_e port, uint32_t baudRate);
+
+int dprintf(const char *fmt, ...);
+
+#endif /* USE_SERIAL_DPRINTF */
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 4e77ce9e9a..7b09c6593a 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -50,6 +50,7 @@ typedef enum {
     FUNCTION_RCDEVICE            = (1 << 14), // 16384
     FUNCTION_LIDAR_TF            = (1 << 15), // 32768
     FUNCTION_FRSKY_OSD           = (1 << 16), // 65536
+    FUNCTION_DPRINTF             = (1 << 17), // 131072
 } serialPortFunction_e;
 
 #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)