diff --git a/hal/src/main/native/athena/DIO.cpp b/hal/src/main/native/athena/DIO.cpp index a7deaa1e04..e5a9d400ab 100644 --- a/hal/src/main/native/athena/DIO.cpp +++ b/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 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. diff --git a/hal/src/main/native/athena/DigitalInternal.cpp b/hal/src/main/native/athena/DigitalInternal.cpp index 94fecdb33f..01fa7c1430 100644 --- a/hal/src/main/native/athena/DigitalInternal.cpp +++ b/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 relaySystem; std::unique_ptr pwmSystem; std::unique_ptr spiSystem; +// Create a mutex to protect changes to the digital output values +wpi::mutex digitalDIOMutex; + DigitalHandleResource* 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) {}); +} diff --git a/hal/src/main/native/athena/DigitalInternal.h b/hal/src/main/native/athena/DigitalInternal.h index b7259e23f8..e40f2aaa2d 100644 --- a/hal/src/main/native/athena/DigitalInternal.h +++ b/hal/src/main/native/athena/DigitalInternal.h @@ -79,6 +79,8 @@ extern DigitalHandleResource* digitalChannelHandles; +extern wpi::mutex digitalDIOMutex; + void initializeDigital(int32_t* status); bool remapDigitalSource(HAL_Handle digitalSourceHandle, HAL_AnalogTriggerType analogTriggerType, diff --git a/hal/src/main/native/include/HAL/DIO.h b/hal/src/main/native/include/HAL/DIO.h index 21d8223f4e..0ca9b38b96 100644 --- a/hal/src/main/native/include/HAL/DIO.h +++ b/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, diff --git a/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h b/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h new file mode 100644 index 0000000000..bb443739e6 --- /dev/null +++ b/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 + +#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 +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 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