Adds TriState DIO functionality to the HAL (#835)

Adds a function to raw set the DIO direction. Also adds a C++ unsafe
function to enable fast setting of DIO direction.

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

@@ -16,9 +16,6 @@
 
 using namespace hal;
 
-// Create a mutex to protect changes to the digital output values
-static wpi::mutex digitalDIOMutex;
-
 // Create a mutex to protect changes to the DO PWM config
 static wpi::mutex digitalPwmMutex;
 
@@ -308,6 +305,50 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
   }
 }
 
+/**
+ * Set direction of a DIO channel.
+ *
+ * @param channel The Digital I/O channel
+ * @param input true to set input, false for output
+ */
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+                         int32_t* status) {
+  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  {
+    std::lock_guard<wpi::mutex> lock(digitalDIOMutex);
+    tDIO::tOutputEnable currentDIO = digitalSystem->readOutputEnable(status);
+
+    if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+      if (input) {
+        currentDIO.SPIPort =
+            currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
+      } else {
+        currentDIO.SPIPort =
+            currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
+      }
+    } else if (port->channel < kNumDigitalHeaders) {
+      if (input) {
+        currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
+      } else {
+        currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
+      }
+    } else {
+      if (input) {
+        currentDIO.MXP =
+            currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
+      } else {
+        currentDIO.MXP =
+            currentDIO.MXP | (1u << remapMXPChannel(port->channel));
+      }
+    }
+    digitalSystem->writeOutputEnable(currentDIO, status);
+  }
+}
+
 /**
  * Read a digital I/O bit from the FPGA.
  * Get a single value from a digital I/O channel.

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

@@ -18,6 +18,7 @@
 #include "HAL/ChipObject.h"
 #include "HAL/HAL.h"
 #include "HAL/Ports.h"
+#include "HAL/cpp/UnsafeDIO.h"
 #include "PortsInternal.h"
 
 namespace hal {
@@ -27,6 +28,9 @@ std::unique_ptr<tRelay> relaySystem;
 std::unique_ptr<tPWM> pwmSystem;
 std::unique_ptr<tSPI> spiSystem;
 
+// Create a mutex to protect changes to the digital output values
+wpi::mutex digitalDIOMutex;
+
 DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
                       kNumDigitalChannels + kNumPWMHeaders>*
     digitalChannelHandles;
@@ -40,6 +44,28 @@ void InitializeDigitalInternal() {
 }
 }  // namespace init
 
+namespace detail {
+wpi::mutex& UnsafeGetDIOMutex() { return digitalDIOMutex; }
+tDIO* UnsafeGetDigialSystem() { return digitalSystem.get(); }
+int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status) {
+  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  tDIO::tDO output;
+  output.value = 0;
+  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+    output.SPIPort = (1u << remapSPIChannel(port->channel));
+  } else if (port->channel < kNumDigitalHeaders) {
+    output.Headers = (1u << port->channel);
+  } else {
+    output.MXP = (1u << remapMXPChannel(port->channel));
+  }
+  return output.value;
+}
+}  // namespace detail
+
 /**
  * Initialize the digital system.
  */
@@ -165,3 +191,8 @@ bool remapDigitalSource(HAL_Handle digitalSourceHandle,
 }
 
 }  // namespace hal
+
+// Unused function here to test template compile.
+__attribute__((unused)) static void CompileFunctorTest() {
+  hal::UnsafeManipulateDIO(0, nullptr, [](hal::DIOSetProxy& proxy) {});
+}

hal/src/main/native/athena/DigitalInternal.h

@@ -79,6 +79,8 @@ extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
                              kNumDigitalChannels + kNumPWMHeaders>*
     digitalChannelHandles;
 
+extern wpi::mutex digitalDIOMutex;
+
 void initializeDigital(int32_t* status);
 bool remapDigitalSource(HAL_Handle digitalSourceHandle,
                         HAL_AnalogTriggerType analogTriggerType,