[hal] Radio LED: Properly close files and improve error messages (#7181)

Author: rzblue

hal/src/main/native/athena/LEDs.cpp

@@ -6,10 +6,14 @@
 
 #include <unistd.h>
 
+#include <cstring>
 #include <fstream>
 
+#include <fmt/format.h>
+#include <fmt/std.h>
 #include <wpi/fs.h>
 
+#include "HALInternal.h"
 #include "hal/Errors.h"
 
 namespace hal::init {
@@ -28,19 +32,56 @@ static const fs::path radioLEDRedFilePath =
 static const char* onStr = "1";
 static const char* offStr = "0";
 
+static bool ReadStateFromFile(fs::path path, int32_t* status) {
+  std::error_code ec;
+  fs::file_t file = fs::OpenFileForRead(path, ec, fs::OF_Text);
+  if (ec) {
+    hal::SetLastError(status, fmt::format("Could not open '{}' for read: {}",
+                                          path, ec.message()));
+    *status = INCOMPATIBLE_STATE;
+    return false;
+  }
+  // We only need to read one byte because the file won't have leading zeros.
+  char buf[1]{};
+  ssize_t count = read(file, buf, 1);
+  // save errno, always close file.
+  int err = errno;
+  fs::CloseFile(file);
+  if (count <= 0) {
+    *status = INCOMPATIBLE_STATE;
+    if (count == 0) {
+      hal::SetLastError(status,
+                        fmt::format("Read from '{}' returned no data.", path));
+    } else {
+      hal::SetLastError(status, fmt::format("Failed to read from '{}': {}",
+                                            path, std::strerror(err)));
+    }
+    return false;
+  }
+  // If the brightness is not zero, the LED is on.
+  return buf[0] != '0';
+}
+
 extern "C" {
 void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
   std::error_code ec;
   fs::file_t greenFile = fs::OpenFileForWrite(radioLEDGreenFilePath, ec,
                                               fs::CD_OpenExisting, fs::OF_Text);
   if (ec) {
+    // not opened, nothing to clean up
     *status = INCOMPATIBLE_STATE;
+    hal::SetLastError(status, fmt::format("Could not open '{}' for write: {}",
+                                          greenFile, ec.message()));
     return;
   }
   fs::file_t redFile = fs::OpenFileForWrite(radioLEDRedFilePath, ec,
                                             fs::CD_OpenExisting, fs::OF_Text);
   if (ec) {
+    // green file opened successfully, need to close it
+    fs::CloseFile(greenFile);
     *status = INCOMPATIBLE_STATE;
+    hal::SetLastError(status, fmt::format("Could not open '{}' for write: {}",
+                                          greenFile, ec.message()));
     return;
   }
 
@@ -51,24 +92,6 @@ void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
   fs::CloseFile(redFile);
 }
 
-bool ReadStateFromFile(fs::path path, int32_t* status) {
-  std::error_code ec;
-  fs::file_t file = fs::OpenFileForRead(path, ec, fs::OF_Text);
-  if (ec) {
-    *status = INCOMPATIBLE_STATE;
-    return false;
-  }
-  // We only need to read one byte because the file won't have leading zeros.
-  char buf[1]{};
-  size_t count = read(file, buf, 1);
-  if (count == 0) {
-    *status = INCOMPATIBLE_STATE;
-    return false;
-  }
-  // If the brightness is not zero, the LED is on.
-  return buf[0] != '0';
-}
-
 HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
   bool green = ReadStateFromFile(radioLEDGreenFilePath, status);
   bool red = ReadStateFromFile(radioLEDRedFilePath, status);