diff --git a/hal/include/HAL/handles/HandlesInternal.h b/hal/include/HAL/handles/HandlesInternal.h index a1d11635b1..06f3cd7f3b 100644 --- a/hal/include/HAL/handles/HandlesInternal.h +++ b/hal/include/HAL/handles/HandlesInternal.h @@ -84,7 +84,15 @@ static inline int16_t getPortHandleModule(HAL_PortHandle handle) { return static_cast((handle >> 8) & 0xff); } +// using a 16 bit value so we can store 0-255 and still report error +static inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) { + if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex; + return static_cast((handle >> 16) & 0xff); +} + HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module); +HAL_PortHandle createPortHandleForSPI(uint8_t channel); + HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType); } // namespace hal diff --git a/hal/lib/athena/DIO.cpp b/hal/lib/athena/DIO.cpp index 18517f4a8d..39ef6ba958 100644 --- a/hal/lib/athena/DIO.cpp +++ b/hal/lib/athena/DIO.cpp @@ -58,7 +58,24 @@ HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle, tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); - if (port->channel < kNumDigitalHeaders) { + if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { + if (!getPortHandleSPIEnable(portHandle)) { + // if this flag is not set, we actually want DIO. + uint32_t bitToSet = 1u << remapSPIChannel(port->channel); + + 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 = @@ -93,8 +110,26 @@ HAL_Bool HAL_CheckDIOChannel(int32_t channel) { } void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) { + auto port = digitalChannelHandles.Get(dioPortHandle, HAL_HandleEnum::DIO); // no status, so no need to check for a proper free. digitalChannelHandles.Free(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) return; + int32_t status = 0; + std::lock_guard sync(digitalDIOMutex); + if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { + // Unset the SPI flag + int32_t bitToUnset = 1 << remapSPIChannel(port->channel); + uint16_t specialFunctions = spiSystem->readEnableDIO(&status); + spiSystem->writeEnableDIO(specialFunctions & ~bitToUnset, &status); + } else if (port->channel >= kNumDigitalHeaders) { + // Unset the MXP flag + uint32_t bitToUnset = 1u << remapMXPChannel(port->channel); + + uint16_t specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(&status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToUnset, + &status); + } } /** @@ -200,7 +235,9 @@ void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator, return; } int32_t id = *port; - if (channel >= kNumDigitalHeaders) { // If it is on the MXP + if (channel >= kNumDigitalHeaders && + channel < + kNumDigitalHeaders + kNumDigitalMXPChannels) { // If it is on the MXP /* Then to write as a digital PWM channel an offset is needed to write on * the correct channel */ @@ -231,7 +268,15 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value, std::lock_guard sync(digitalDIOMutex); tDIO::tDO currentDIO = digitalSystem->readDO(status); - if (port->channel < kNumDigitalHeaders) { + 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) { @@ -245,12 +290,6 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value, currentDIO.MXP = currentDIO.MXP | (1u << remapMXPChannel(port->channel)); } - - int32_t bitToSet = 1 << remapMXPChannel(port->channel); - uint16_t specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, - status); } digitalSystem->writeDO(currentDIO, status); } @@ -275,16 +314,11 @@ HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) { // if it == 0, then return false // else return true - if (port->channel < kNumDigitalHeaders) { + 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 { - // Disable special functions - int32_t bitToSet = 1 << remapMXPChannel(port->channel); - uint16_t specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, - status); - return ((currentDIO.MXP >> remapMXPChannel(port->channel)) & 1) != 0; } } @@ -309,7 +343,10 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) { // if it == 0, then return false // else return true - if (port->channel < kNumDigitalHeaders) { + 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) != @@ -334,7 +371,9 @@ void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength, } tDIO::tPulse pulse; - if (port->channel < kNumDigitalHeaders) { + 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); @@ -360,7 +399,9 @@ HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) { } tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - if (port->channel < kNumDigitalHeaders) { + 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; @@ -376,7 +417,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; + return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 && + pulseRegister.SPIPort != 0; } /** @@ -396,7 +438,11 @@ void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex, } std::lock_guard sync(digitalDIOMutex); - if (port->channel < kNumDigitalHeaders) { + if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { + // Channels 10-15 are SPI channels, so subtract our MXP channels + digitalSystem->writeFilterSelectHdr(port->channel - kNumDigitalMXPChannels, + filterIndex, status); + } else if (port->channel < kNumDigitalHeaders) { digitalSystem->writeFilterSelectHdr(port->channel, filterIndex, status); } else { digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->channel), @@ -420,7 +466,11 @@ int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) { } std::lock_guard sync(digitalDIOMutex); - if (port->channel < kNumDigitalHeaders) { + if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) { + // Channels 10-15 are SPI channels, so subtract our MXP channels + return digitalSystem->readFilterSelectHdr( + port->channel - kNumDigitalMXPChannels, status); + } else if (port->channel < kNumDigitalHeaders) { return digitalSystem->readFilterSelectHdr(port->channel, status); } else { return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->channel), diff --git a/hal/lib/athena/DigitalInternal.cpp b/hal/lib/athena/DigitalInternal.cpp index 457901b436..645cfbf833 100644 --- a/hal/lib/athena/DigitalInternal.cpp +++ b/hal/lib/athena/DigitalInternal.cpp @@ -27,6 +27,7 @@ priority_recursive_mutex digitalPwmMutex; std::unique_ptr digitalSystem; std::unique_ptr relaySystem; std::unique_ptr pwmSystem; +std::unique_ptr spiSystem; static std::atomic digitalSystemsInitialized{false}; static priority_mutex initializeMutex; @@ -94,9 +95,18 @@ void initializeDigital(int32_t* status) { } } + // SPI setup + spiSystem.reset(tSPI::create(status)); + digitalSystemsInitialized = true; } +/** + * Map SPI channel numbers from their physical number (27 to 31) to their + * position in the bit field. + */ +int32_t remapSPIChannel(int32_t channel) { return channel - 26; } + /** * Map DIO channel numbers from their physical number (10 to 26) to their * position in the bit field. @@ -130,7 +140,11 @@ bool remapDigitalSource(HAL_Handle digitalSourceHandle, return true; } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) { int32_t index = getHandleIndex(digitalSourceHandle); - if (index >= kNumDigitalHeaders) { + if (index > kNumDigitalHeaders + kNumDigitalMXPChannels) { + // channels 10-15, so need to add headers to remap index + channel = remapSPIChannel(index) + kNumDigitalHeaders; + module = 0; + } else if (index >= kNumDigitalHeaders) { channel = remapMXPChannel(index); module = 1; } else { diff --git a/hal/lib/athena/DigitalInternal.h b/hal/lib/athena/DigitalInternal.h index e0f791fcdf..1b1544dd01 100644 --- a/hal/lib/athena/DigitalInternal.h +++ b/hal/lib/athena/DigitalInternal.h @@ -62,6 +62,7 @@ extern priority_recursive_mutex digitalPwmMutex; extern std::unique_ptr digitalSystem; extern std::unique_ptr relaySystem; extern std::unique_ptr pwmSystem; +extern std::unique_ptr spiSystem; struct DigitalPort { uint8_t channel; @@ -82,6 +83,7 @@ void initializeDigital(int32_t* status); bool remapDigitalSource(HAL_Handle digitalSourceHandle, HAL_AnalogTriggerType analogTriggerType, uint8_t& channel, uint8_t& module, bool& analogTrigger); +int32_t remapSPIChannel(int32_t channel); int32_t remapMXPPWMChannel(int32_t channel); int32_t remapMXPChannel(int32_t channel); } // namespace hal diff --git a/hal/lib/athena/PortsInternal.h b/hal/lib/athena/PortsInternal.h index 185030b0da..851f9d97af 100644 --- a/hal/lib/athena/PortsInternal.h +++ b/hal/lib/athena/PortsInternal.h @@ -18,8 +18,11 @@ constexpr int32_t kNumAnalogInputs = 8; constexpr int32_t kNumAnalogOutputs = tAO::kNumMXPRegisters; constexpr int32_t kNumCounters = tCounter::kNumSystems; constexpr int32_t kNumDigitalHeaders = 10; +constexpr int32_t kNumDigitalMXPChannels = 16; +constexpr int32_t kNumDigitalSPIPortChannels = 5; constexpr int32_t kNumPWMHeaders = tPWM::kNumHdrRegisters; -constexpr int32_t kNumDigitalChannels = 26; +constexpr int32_t kNumDigitalChannels = + kNumDigitalHeaders + kNumDigitalMXPChannels + kNumDigitalSPIPortChannels; constexpr int32_t kNumPWMChannels = tPWM::kNumMXPRegisters + kNumPWMHeaders; constexpr int32_t kNumDigitalPWMOutputs = tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements; diff --git a/hal/lib/athena/SPI.cpp b/hal/lib/athena/SPI.cpp index df41135355..7e46c27bd7 100644 --- a/hal/lib/athena/SPI.cpp +++ b/hal/lib/athena/SPI.cpp @@ -15,6 +15,7 @@ #include "HAL/HAL.h" #include "HAL/cpp/make_unique.h" #include "HAL/cpp/priority_mutex.h" +#include "HAL/handles/HandlesInternal.h" #include "spilib/spi-lib.h" using namespace hal; @@ -26,12 +27,11 @@ static int32_t m_spiCS3Handle = 0; static int32_t m_spiMXPHandle = 0; static priority_recursive_mutex spiOnboardMutex; static priority_recursive_mutex spiMXPMutex; -static std::unique_ptr spiSystem; -static HAL_DigitalHandle spiMXPDigitalHandle1 = HAL_kInvalidHandle; -static HAL_DigitalHandle spiMXPDigitalHandle2 = HAL_kInvalidHandle; -static HAL_DigitalHandle spiMXPDigitalHandle3 = HAL_kInvalidHandle; -static HAL_DigitalHandle spiMXPDigitalHandle4 = HAL_kInvalidHandle; +// MXP SPI does not count towards this +std::atomic spiPortCount{0}; + +static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle}; /** * Get the semaphore for a SPI port @@ -73,56 +73,115 @@ struct SPIAccumulator { }; std::unique_ptr spiAccumulators[5]; +static void CommonSPIPortInit(int32_t* status) { + // All false cases will set + if (spiPortCount.fetch_add(1) == 0) { + // Have not been initialized yet + initializeDigital(status); + if (*status != 0) return; + // MISO + if ((digitalHandles[3] = HAL_InitializeDIOPort(createPortHandleForSPI(29), + false, status)) == + HAL_kInvalidHandle) { + std::printf("Failed to allocate DIO 29 (MISO)\n"); + return; + } + // MOSI + if ((digitalHandles[4] = HAL_InitializeDIOPort(createPortHandleForSPI(30), + false, status)) == + HAL_kInvalidHandle) { + std::printf("Failed to allocate DIO 30 (MOSI)\n"); + HAL_FreeDIOPort(digitalHandles[3]); // free the first port allocated + return; + } + } +} + +static void CommonSPIPortFree() { + if (spiPortCount.fetch_sub(1) == 1) { + // Clean up SPI Handles + HAL_FreeDIOPort(digitalHandles[3]); + HAL_FreeDIOPort(digitalHandles[4]); + } +} + /* * Initialize the spi port. Opens the port if necessary and saves the handle. * If opening the MXP port, also sets up the channel functions appropriately - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param port The number of the port to use. 0-3 for Onboard CS0-CS3, 4 for MXP */ void HAL_InitializeSPI(int32_t port, int32_t* status) { - if (spiSystem == nullptr) spiSystem.reset(tSPI::create(status)); if (HAL_GetSPIHandle(port) != 0) return; switch (port) { case 0: + CommonSPIPortInit(status); + if (*status != 0) return; + // CS0 is not a DIO port, so nothing to allocate HAL_SetSPIHandle(0, spilib_open("/dev/spidev0.0")); break; case 1: + CommonSPIPortInit(status); + if (*status != 0) return; + // CS1, Allocate + if ((digitalHandles[0] = HAL_InitializeDIOPort( + HAL_GetPort(26), false, status)) == HAL_kInvalidHandle) { + std::printf("Failed to allocate DIO 26 (CS1)\n"); + CommonSPIPortFree(); + return; + } HAL_SetSPIHandle(1, spilib_open("/dev/spidev0.1")); break; case 2: + CommonSPIPortInit(status); + if (*status != 0) return; + // CS2, Allocate + if ((digitalHandles[1] = HAL_InitializeDIOPort( + HAL_GetPort(27), false, status)) == HAL_kInvalidHandle) { + std::printf("Failed to allocate DIO 27 (CS2)\n"); + CommonSPIPortFree(); + return; + } HAL_SetSPIHandle(2, spilib_open("/dev/spidev0.2")); break; case 3: + CommonSPIPortInit(status); + if (*status != 0) return; + // CS3, Allocate + if ((digitalHandles[2] = HAL_InitializeDIOPort( + HAL_GetPort(28), false, status)) == HAL_kInvalidHandle) { + std::printf("Failed to allocate DIO 28 (CS3)\n"); + CommonSPIPortFree(); + return; + } HAL_SetSPIHandle(3, spilib_open("/dev/spidev0.3")); break; case 4: initializeDigital(status); if (*status != 0) return; - if ((spiMXPDigitalHandle1 = HAL_InitializeDIOPort( + if ((digitalHandles[5] = HAL_InitializeDIOPort( HAL_GetPort(14), false, status)) == HAL_kInvalidHandle) { std::printf("Failed to allocate DIO 14\n"); return; } - if ((spiMXPDigitalHandle2 = HAL_InitializeDIOPort( + if ((digitalHandles[6] = HAL_InitializeDIOPort( HAL_GetPort(15), false, status)) == HAL_kInvalidHandle) { std::printf("Failed to allocate DIO 15\n"); - HAL_FreeDIOPort(spiMXPDigitalHandle1); // free the first port allocated + HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated return; } - if ((spiMXPDigitalHandle3 = HAL_InitializeDIOPort( + if ((digitalHandles[7] = HAL_InitializeDIOPort( HAL_GetPort(16), false, status)) == HAL_kInvalidHandle) { std::printf("Failed to allocate DIO 16\n"); - HAL_FreeDIOPort(spiMXPDigitalHandle1); // free the first port allocated - HAL_FreeDIOPort( - spiMXPDigitalHandle2); // free the second port allocated + HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated + HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated return; } - if ((spiMXPDigitalHandle4 = HAL_InitializeDIOPort( + if ((digitalHandles[8] = HAL_InitializeDIOPort( HAL_GetPort(17), false, status)) == HAL_kInvalidHandle) { std::printf("Failed to allocate DIO 17\n"); - HAL_FreeDIOPort(spiMXPDigitalHandle1); // free the first port allocated - HAL_FreeDIOPort( - spiMXPDigitalHandle2); // free the second port allocated - HAL_FreeDIOPort(spiMXPDigitalHandle3); // free the third port allocated + HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated + HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated + HAL_FreeDIOPort(digitalHandles[7]); // free the third port allocated return; } digitalSystem->writeEnableMXPSpecialFunction( @@ -205,11 +264,28 @@ void HAL_CloseSPI(int32_t port) { } spilib_close(HAL_GetSPIHandle(port)); HAL_SetSPIHandle(port, 0); - if (port == 4) { - HAL_FreeDIOPort(spiMXPDigitalHandle1); - HAL_FreeDIOPort(spiMXPDigitalHandle2); - HAL_FreeDIOPort(spiMXPDigitalHandle3); - HAL_FreeDIOPort(spiMXPDigitalHandle4); + if (port < 4) { + CommonSPIPortFree(); + } + switch (port) { + // Case 0 does not need to do anything + case 1: + HAL_FreeDIOPort(digitalHandles[0]); + break; + case 2: + HAL_FreeDIOPort(digitalHandles[1]); + break; + case 3: + HAL_FreeDIOPort(digitalHandles[2]); + break; + case 4: + HAL_FreeDIOPort(digitalHandles[5]); + HAL_FreeDIOPort(digitalHandles[6]); + HAL_FreeDIOPort(digitalHandles[7]); + HAL_FreeDIOPort(digitalHandles[8]); + break; + default: + break; } return; } diff --git a/hal/lib/shared/handles/HandlesInternal.cpp b/hal/lib/shared/handles/HandlesInternal.cpp index acbae7ef48..b07dfa4145 100644 --- a/hal/lib/shared/handles/HandlesInternal.cpp +++ b/hal/lib/shared/handles/HandlesInternal.cpp @@ -21,6 +21,21 @@ HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module) { return handle; } +HAL_PortHandle createPortHandleForSPI(uint8_t channel) { + // set last 8 bits, then shift to first 8 bits + HAL_PortHandle handle = static_cast(HAL_HandleEnum::Port); + handle = handle << 16; + // set second set up bits to 1 + int32_t temp = 1; + temp = (temp << 8) & 0xff00; + handle += temp; + // shift to last set of bits + handle = handle << 8; + // add channel to last 8 bits + handle += channel; + return handle; +} + HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType) { if (index < 0) return HAL_kInvalidHandle; uint8_t hType = static_cast(handleType); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java index b9e6316c9a..4a975e5c60 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java @@ -33,7 +33,7 @@ public class ConstantsPortsTest extends AbstractComsSetup { */ @Test public void testDigitalChannels() { - assertEquals(26, SensorBase.kDigitalChannels); + assertEquals(31, SensorBase.kDigitalChannels); } /**