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,

hal/src/main/native/include/HAL/DIO.h

@@ -28,6 +28,8 @@ void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
                                     int32_t channel, int32_t* status);
 void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
                 int32_t* status);
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+                         int32_t* status);
 HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status);
 HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status);
 void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,

hal/src/main/native/include/HAL/cpp/UnsafeDIO.h

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <support/mutex.h>
+
+#include "HAL/ChipObject.h"
+#include "HAL/Types.h"
+
+namespace hal {
+struct DIOSetProxy {
+  DIOSetProxy(const DIOSetProxy&) = delete;
+  DIOSetProxy(DIOSetProxy&&) = delete;
+  DIOSetProxy& operator=(const DIOSetProxy&) = delete;
+  DIOSetProxy& operator=(DIOSetProxy&&) = delete;
+
+  void SetOutputMode(int32_t* status) {
+    m_dio->writeOutputEnable(m_setOutputDirReg, status);
+  }
+
+  void SetInputMode(int32_t* status) {
+    m_dio->writeOutputEnable(m_unsetOutputDirReg, status);
+  }
+
+  void SetOutputTrue(int32_t* status) {
+    m_dio->writeDO(m_setOutputStateReg, status);
+  }
+
+  void SetOutputFalse(int32_t* status) {
+    m_dio->writeDO(m_unsetOutputStateReg, status);
+  }
+
+  tDIO::tOutputEnable m_setOutputDirReg;
+  tDIO::tOutputEnable m_unsetOutputDirReg;
+  tDIO::tDO m_setOutputStateReg;
+  tDIO::tDO m_unsetOutputStateReg;
+  tDIO* m_dio;
+};
+namespace detail {
+wpi::mutex& UnsafeGetDIOMutex();
+tDIO* UnsafeGetDigialSystem();
+int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status);
+}  // namespace detail
+
+/**
+ * Unsafe digital output set function
+ * This function can be used to perform fast and determinstically set digital
+ * outputs. This function holds the DIO lock, so calling anyting other then
+ * functions on the Proxy object passed as a parameter can deadlock your
+ * program.
+ *
+ */
+template <typename Functor>
+void UnsafeManipulateDIO(HAL_DigitalHandle handle, int32_t* status,
+                         Functor func) {
+  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  wpi::mutex& dioMutex = detail::UnsafeGetDIOMutex();
+  tDIO* dSys = detail::UnsafeGetDigialSystem();
+  auto mask = detail::ComputeDigitalMask(handle, status);
+  if (status != 0) return;
+  std::lock_guard<wpi::mutex> lock(dioMutex);
+
+  tDIO::tOutputEnable enableOE = dSys->readOutputEnable(status);
+  enableOE.value |= mask;
+  auto disableOE = enableOE;
+  disableOE.value &= ~mask;
+  tDIO::tDO enableDO = dSys->readDO(status);
+  enableDO.value |= mask;
+  auto disableDO = enableDO;
+  disableDO.value &= ~mask;
+
+  DIOSetProxy dioData{enableOE, disableOE, enableDO, disableDO, dSys};
+  func(dioData);
+}
+
+}  // namespace hal