From 53875419a1346ccdc04f02ac21ca760458ca11cc Mon Sep 17 00:00:00 2001
From: Dustin Spicuzza <dustin@virtualroadside.com>
Date: Wed, 30 Nov 2022 02:19:15 -0500
Subject: [PATCH] [hal] Allow overriding stderr printing by HAL_SendError
 (#4742)

---
 hal/src/main/native/athena/FRCDriverStation.cpp | 16 +++++++++++++++-
 hal/src/main/native/include/hal/DriverStation.h |  8 ++++++++
 hal/src/main/native/sim/DriverStation.cpp       | 17 ++++++++++++++++-
 3 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp
index 3d1e1c9199..0f0b734b76 100644
--- a/hal/src/main/native/athena/FRCDriverStation.cpp
+++ b/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -179,6 +179,15 @@ void InitializeFRCDriverStation() {
 }
 }  // namespace hal::init
 
+namespace hal {
+static void DefaultPrintErrorImpl(const char* line, size_t size) {
+  std::fwrite(line, size, 1, stderr);
+}
+}  // namespace hal
+
+static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
+    hal::DefaultPrintErrorImpl};
+
 extern "C" {
 
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
@@ -256,7 +265,8 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
       if (callStack && callStack[0] != '\0') {
         fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
       }
-      std::fwrite(buf.data(), buf.size(), 1, stderr);
+      auto printError = gPrintErrorImpl.load();
+      printError(buf.data(), buf.size());
     }
     if (i == KEEP_MSGS) {
       // replace the oldest one
@@ -275,6 +285,10 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   return retval;
 }
 
+void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
+  gPrintErrorImpl.store(func ? func : hal::DefaultPrintErrorImpl);
+}
+
 int32_t HAL_SendConsoleLine(const char* line) {
   std::string_view lineRef{line};
   if (lineRef.size() <= 65535) {
diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h
index fabc45f905..ae68b654ca 100644
--- a/hal/src/main/native/include/hal/DriverStation.h
+++ b/hal/src/main/native/include/hal/DriverStation.h
@@ -36,6 +36,14 @@ extern "C" {
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg);
+
+/**
+ * Set the print function used by HAL_SendError
+ *
+ * @param func Function called by HAL_SendError when stderr is printed
+ */
+void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size));
+
 /**
  * Sends a line to the driver station console.
  *
diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp
index a8630fa2ae..7e4831469b 100644
--- a/hal/src/main/native/sim/DriverStation.cpp
+++ b/hal/src/main/native/sim/DriverStation.cpp
@@ -8,6 +8,7 @@
 #include <pthread.h>
 #endif
 
+#include <atomic>
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
@@ -86,6 +87,15 @@ void InitializeDriverStation() {
 }
 }  // namespace hal::init
 
+namespace hal {
+static void DefaultPrintErrorImpl(const char* line, size_t size) {
+  std::fwrite(line, size, 1, stderr);
+}
+}  // namespace hal
+
+static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
+    hal::DefaultPrintErrorImpl};
+
 extern "C" {
 
 void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {
@@ -138,7 +148,8 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
       if (callStack && callStack[0] != '\0') {
         fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
       }
-      std::fwrite(buf.data(), buf.size(), 1, stderr);
+      auto printError = gPrintErrorImpl.load();
+      printError(buf.data(), buf.size());
     }
     if (i == KEEP_MSGS) {
       // replace the oldest one
@@ -157,6 +168,10 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   return retval;
 }
 
+void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
+  gPrintErrorImpl.store(func ? func : hal::DefaultPrintErrorImpl);
+}
+
 int32_t HAL_SendConsoleLine(const char* line) {
   auto handler = sendConsoleLineHandler.load();
   if (handler) {