diff --git a/hal/src/main/native/athena/DIO.cpp b/hal/src/main/native/athena/DIO.cpp index 6608071f92..b99510574c 100644 --- a/hal/src/main/native/athena/DIO.cpp +++ b/hal/src/main/native/athena/DIO.cpp @@ -10,6 +10,7 @@ #include #include "DigitalInternal.h" +#include "HAL/cpp/UnsafeDIO.h" #include "HAL/handles/HandlesInternal.h" #include "HAL/handles/LimitedHandleResource.h" #include "PortsInternal.h" @@ -66,10 +67,18 @@ HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle, port->channel = static_cast(channel); + auto mask = ComputeDigitalMask(port->channel); + std::lock_guard lock(digitalDIOMutex); tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); + if (input) { + outputEnable.value |= mask; + } else { + outputEnable.value &= ~mask; + } + if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { if (!getPortHandleSPIEnable(portHandle)) { // if this flag is not set, we actually want DIO. @@ -78,38 +87,14 @@ HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle, uint16_t specialFunctions = spiSystem->readEnableDIO(status); // Set the field to enable SPI DIO spiSystem->writeEnableDIO(specialFunctions | bitToSet, status); - - if (input) { - outputEnable.SPIPort = - outputEnable.SPIPort & (~bitToSet); // clear the field for read - } else { - outputEnable.SPIPort = - outputEnable.SPIPort | bitToSet; // set the bits for write - } } - } else if (port->channel < kNumDigitalHeaders) { - uint32_t bitToSet = 1u << port->channel; - if (input) { - outputEnable.Headers = - outputEnable.Headers & (~bitToSet); // clear the bit for read - } else { - outputEnable.Headers = - outputEnable.Headers | bitToSet; // set the bit for write - } - } else { + } else if (port->channel >= kNumDigitalHeaders) { uint32_t bitToSet = 1u << remapMXPChannel(port->channel); uint16_t specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); - - if (input) { - outputEnable.MXP = - outputEnable.MXP & (~bitToSet); // clear the bit for read - } else { - outputEnable.MXP = outputEnable.MXP | bitToSet; // set the bit for write - } } digitalSystem->writeOutputEnable(outputEnable, status); @@ -271,36 +256,18 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value, *status = HAL_HANDLE_ERROR; return; } - if (value != 0 && value != 1) { - if (value != 0) value = 1; - } + + auto mask = ComputeDigitalMask(port->channel); { std::lock_guard lock(digitalDIOMutex); tDIO::tDO currentDIO = digitalSystem->readDO(status); - if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { - if (value == 0) { - currentDIO.SPIPort = - currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel)); - } else if (value == 1) { - currentDIO.SPIPort = - currentDIO.SPIPort | (1u << remapSPIChannel(port->channel)); - } - } else if (port->channel < kNumDigitalHeaders) { - if (value == 0) { - currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel); - } else if (value == 1) { - currentDIO.Headers = currentDIO.Headers | (1u << port->channel); - } + if (value) { + currentDIO.value &= ~mask; } else { - if (value == 0) { - currentDIO.MXP = - currentDIO.MXP & ~(1u << remapMXPChannel(port->channel)); - } else if (value == 1) { - currentDIO.MXP = - currentDIO.MXP | (1u << remapMXPChannel(port->channel)); - } + currentDIO.value |= mask; } + digitalSystem->writeDO(currentDIO, status); } } @@ -318,33 +285,17 @@ void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input, *status = HAL_HANDLE_ERROR; return; } + auto mask = ComputeDigitalMask(port->channel); { 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); - } + if (input) { + currentDIO.value |= mask; } else { - if (input) { - currentDIO.MXP = - currentDIO.MXP & ~(1u << remapMXPChannel(port->channel)); - } else { - currentDIO.MXP = - currentDIO.MXP | (1u << remapMXPChannel(port->channel)); - } + currentDIO.value &= ~mask; } + digitalSystem->writeOutputEnable(currentDIO, status); } } @@ -368,13 +319,9 @@ HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) { // if it == 0, then return false // else return true - if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { - return ((currentDIO.SPIPort >> remapSPIChannel(port->channel)) & 1) != 0; - } else if (port->channel < kNumDigitalHeaders) { - return ((currentDIO.Headers >> port->channel) & 1) != 0; - } else { - return ((currentDIO.MXP >> remapMXPChannel(port->channel)) & 1) != 0; - } + auto mask = ComputeDigitalMask(port->channel); + + return (currentDIO.value & mask) != 0; } /** @@ -397,15 +344,9 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) { // if it == 0, then return false // else return true - if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { - return ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) & - 1) != 0; - } else if (port->channel < kNumDigitalHeaders) { - return ((currentOutputEnable.Headers >> port->channel) & 1) != 0; - } else { - return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) != - 0; - } + auto mask = ComputeDigitalMask(port->channel); + + return (currentOutputEnable.value & mask) != 0; } /** @@ -425,13 +366,7 @@ void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength, } tDIO::tPulse pulse; - if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { - pulse.SPIPort = 1u << remapSPIChannel(port->channel); - } else if (port->channel < kNumDigitalHeaders) { - pulse.Headers = 1u << port->channel; - } else { - pulse.MXP = 1u << remapMXPChannel(port->channel); - } + pulse.value = ComputeDigitalMask(port->channel); digitalSystem->writePulseLength( static_cast(1.0e9 * pulseLength / @@ -453,13 +388,8 @@ HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) { } tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { - return (pulseRegister.SPIPort & (1 << remapSPIChannel(port->channel))) != 0; - } else if (port->channel < kNumDigitalHeaders) { - return (pulseRegister.Headers & (1 << port->channel)) != 0; - } else { - return (pulseRegister.MXP & (1 << remapMXPChannel(port->channel))) != 0; - } + auto mask = ComputeDigitalMask(port->channel); + return (pulseRegister.value & mask) != 0; } /** @@ -471,8 +401,8 @@ HAL_Bool HAL_IsAnyPulsing(int32_t* status) { initializeDigital(status); if (*status != 0) return false; tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 && - pulseRegister.SPIPort != 0; + pulseRegister.Reserved = 0; + return pulseRegister.value != 0; } /** diff --git a/hal/src/main/native/athena/DigitalInternal.cpp b/hal/src/main/native/athena/DigitalInternal.cpp index 2305ce2812..6632532f74 100644 --- a/hal/src/main/native/athena/DigitalInternal.cpp +++ b/hal/src/main/native/athena/DigitalInternal.cpp @@ -47,24 +47,20 @@ void InitializeDigitalInternal() { 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; - } +} // namespace detail + +int32_t ComputeDigitalMask(uint8_t channel) { 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); + if (channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { + output.SPIPort = (1u << remapSPIChannel(channel)); + } else if (channel < kNumDigitalHeaders) { + output.Headers = (1u << channel); } else { - output.MXP = (1u << remapMXPChannel(port->channel)); + output.MXP = (1u << remapMXPChannel(channel)); } return output.value; } -} // namespace detail /** * Initialize the digital system. diff --git a/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h b/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h index 5d951005fb..5f3aa7fcee 100644 --- a/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h +++ b/hal/src/main/native/include/HAL/cpp/UnsafeDIO.h @@ -10,6 +10,7 @@ #include #include "HAL/ChipObject.h" +#include "HAL/DIO.h" #include "HAL/Types.h" namespace hal { @@ -41,10 +42,10 @@ struct DIOSetProxy { tDIO::tDO m_unsetOutputStateReg; tDIO* m_dio; }; +int32_t ComputeDigitalMask(uint8_t channel); namespace detail { wpi::mutex& UnsafeGetDIOMutex(); tDIO* UnsafeGetDigialSystem(); -int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status); } // namespace detail /** @@ -65,7 +66,7 @@ void UnsafeManipulateDIO(HAL_DigitalHandle handle, int32_t* status, } wpi::mutex& dioMutex = detail::UnsafeGetDIOMutex(); tDIO* dSys = detail::UnsafeGetDigialSystem(); - auto mask = detail::ComputeDigitalMask(handle, status); + auto mask = ComputeDigitalMask(port->channel); if (status != 0) return; std::lock_guard lock(dioMutex);