[hal] Report previous allocation location for indexed resource duplicates (#3322)

hal/src/main/native/sim/AnalogGyro.cpp

@@ -4,7 +4,10 @@
 
 #include "hal/AnalogGyro.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/Errors.h"
@@ -15,6 +18,7 @@ namespace {
 struct AnalogGyro {
   HAL_AnalogInputHandle handle;
   uint8_t index;
+  std::string previousAllocation;
 };
 }  // namespace
 
@@ -34,36 +38,41 @@ void InitializeAnalogGyro() {
 
 extern "C" {
 HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
+  // Handle will be type checked by HAL_IsAccumulatorChannel
+  int16_t channel = getHandleIndex(analogHandle);
   if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
     if (*status == 0) {
       *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
     }
     return HAL_kInvalidHandle;
   }
 
-  // handle known to be correct, so no need to type check
-  int16_t channel = getHandleIndex(analogHandle);
-
-  auto handle = analogGyroHandles->Allocate(channel, status);
+  HAL_GyroHandle handle;
+  auto gyro = analogGyroHandles->Allocate(channel, &handle, status);
 
   if (*status != 0) {
+    if (gyro) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Gyro", channel,
+                                           gyro->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  // Initialize port structure
-  auto gyro = analogGyroHandles->Get(handle);
-  if (gyro == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   gyro->handle = analogHandle;
   gyro->index = channel;
 
   SimAnalogGyroData[channel].initialized = true;
 
+  gyro->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 

hal/src/main/native/sim/AnalogInput.cpp

@@ -6,6 +6,7 @@
 
 #include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/handles/HandlesInternal.h"
@@ -18,28 +19,32 @@ void InitializeAnalogInput() {}
 }  // namespace hal::init
 
 extern "C" {
-HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
-                                                    int32_t* status) {
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogInputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                     0, kNumAnalogInputs, channel);
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogInputHandle handle = analogInputHandles->Allocate(channel, status);
+  HAL_AnalogInputHandle handle;
+  auto analog_port = analogInputHandles->Allocate(channel, &handle, status);
 
   if (*status != 0) {
+    if (analog_port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Input", channel,
+                                           analog_port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                       0, kNumAnalogInputs, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  // Initialize port structure
-  auto analog_port = analogInputHandles->Get(handle);
-  if (analog_port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   analog_port->channel = static_cast<uint8_t>(channel);
   if (HAL_IsAccumulatorChannel(handle, status)) {
     analog_port->isAccumulator = true;
@@ -51,6 +56,9 @@ HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
   SimAnalogInData[channel].accumulatorInitialized = false;
   SimAnalogInData[channel].simDevice = 0;
 
+  analog_port->previousAllocation =
+      allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {

hal/src/main/native/sim/AnalogInternal.h

@@ -6,6 +6,8 @@
 
 #include <stdint.h>
 
+#include <string>
+
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
@@ -20,6 +22,7 @@ static constexpr uint32_t kAccumulatorChannels[] = {0, 1};
 struct AnalogPort {
   uint8_t channel;
   bool isAccumulator;
+  std::string previousAllocation;
 };
 
 extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,

hal/src/main/native/sim/AnalogOutput.cpp

@@ -4,7 +4,10 @@
 
 #include "hal/AnalogOutput.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
@@ -16,6 +19,7 @@ using namespace hal;
 namespace {
 struct AnalogOutput {
   uint8_t channel;
+  std::string previousAllocation;
 };
 }  // namespace
 
@@ -33,32 +37,39 @@ void InitializeAnalogOutput() {
 }  // namespace hal::init
 
 extern "C" {
-HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
-                                                      int32_t* status) {
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogOutputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Output",
+                                     0, kNumAnalogOutputs, channel);
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogOutputHandle handle =
-      analogOutputHandles->Allocate(channel, status);
+  HAL_AnalogOutputHandle handle;
+  auto port = analogOutputHandles->Allocate(channel, &handle, status);
 
   if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Output", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status,
+                                       "Invalid Index for Analog Output", 0,
+                                       kNumAnalogOutputs, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  auto port = analogOutputHandles->Get(handle);
-  if (port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   port->channel = static_cast<uint8_t>(channel);
 
   // Initialize sim analog input
   SimAnalogOutData[channel].initialized = true;
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
   return handle;
 }
 

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

@@ -6,6 +6,7 @@
 
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
@@ -31,36 +32,41 @@ void InitializeDIO() {
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
-                                        HAL_Bool input, int32_t* status) {
+                                        HAL_Bool input,
+                                        const char* allocationLocation,
+                                        int32_t* status) {
   hal::init::CheckInit();
-  if (*status != 0) {
-    return HAL_kInvalidHandle;
-  }
 
   int16_t channel = getPortHandleChannel(portHandle);
   if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                     kNumDigitalChannels, channel);
     return HAL_kInvalidHandle;
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO, status);
+  HAL_DigitalHandle handle;
+
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO,
+                                              &handle, status);
 
   if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                       kNumDigitalChannels, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   port->channel = static_cast<uint8_t>(channel);
 
   SimDIOData[channel].initialized = true;
   SimDIOData[channel].isInput = input;
   SimDIOData[channel].simDevice = 0;
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
 
   return handle;
 }

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

@@ -6,6 +6,8 @@
 
 #include <stdint.h>
 
+#include <string>
+
 #include "PortsInternal.h"
 #include "hal/AnalogTrigger.h"
 #include "hal/handles/DigitalHandleResource.h"
@@ -56,6 +58,7 @@ struct DigitalPort {
   int32_t centerPwm = 0;
   int32_t deadbandMinPwm = 0;
   int32_t minPwm = 0;
+  std::string previousAllocation;
 };
 
 extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,

hal/src/main/native/sim/HALInternal.h

@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <wpi/Twine.h>
+
+namespace hal {
+void SetLastError(int32_t* status, const wpi::Twine& value);
+void SetLastErrorIndexOutOfRange(int32_t* status, const wpi::Twine& message,
+                                 int32_t minimum, int32_t maximum,
+                                 int32_t channel);
+void SetLastErrorPreviouslyAllocated(int32_t* status, const wpi::Twine& message,
+                                     int32_t channel,
+                                     const wpi::Twine& previousAllocation);
+}  // namespace hal

hal/src/main/native/sim/PWM.cpp

@@ -7,6 +7,7 @@
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "mockdata/PWMDataInternal.h"
@@ -20,15 +21,15 @@ void InitializePWM() {}
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
-  if (*status != 0) {
-    return HAL_kInvalidHandle;
-  }
 
   int16_t channel = getPortHandleChannel(portHandle);
   if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                     kNumPWMChannels, channel);
     return HAL_kInvalidHandle;
   }
 
@@ -40,19 +41,22 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
     channel = remapMXPPWMChannel(channel) + 10;  // remap MXP to proper channel
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM, status);
+  HAL_DigitalHandle handle;
+
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM,
+                                              &handle, status);
 
   if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                       kNumPWMChannels, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::PWM);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   port->channel = origChannel;
 
   SimPWMData[origChannel].initialized = true;
@@ -60,6 +64,8 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
   // Defaults to allow an always valid config.
   HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
 
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {

hal/src/main/native/sim/Relay.cpp

@@ -4,7 +4,10 @@
 
 #include "hal/Relay.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
 #include "mockdata/RelayDataInternal.h"
@@ -15,6 +18,7 @@ namespace {
 struct Relay {
   uint8_t channel;
   bool fwd;
+  std::string previousAllocation;
 };
 }  // namespace
 
@@ -32,6 +36,7 @@ void InitializeRelay() {
 
 extern "C" {
 HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
   if (*status != 0) {
@@ -39,8 +44,10 @@ HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
   }
 
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumRelayChannels) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                     kNumRelayChannels, channel);
     return HAL_kInvalidHandle;
   }
 
@@ -48,18 +55,20 @@ HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
     channel += kNumRelayHeaders;  // add 4 to reverse channels
   }
 
-  auto handle = relayHandles->Allocate(channel, status);
+  HAL_RelayHandle handle;
+  auto port = relayHandles->Allocate(channel, &handle, status);
 
   if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Relay", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                       kNumRelayChannels, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }
 
-  auto port = relayHandles->Get(handle);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   if (!fwd) {
     // Subtract number of headers to put channel in range
     channel -= kNumRelayHeaders;
@@ -73,6 +82,7 @@ HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
   }
 
   port->channel = static_cast<uint8_t>(channel);
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
 
   return handle;
 }

hal/src/main/native/sim/Solenoid.cpp

@@ -55,17 +55,13 @@ HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
     return HAL_kInvalidHandle;
   }
 
-  auto handle = solenoidHandles->Allocate(
-      module * kNumSolenoidChannels + channel, status);
+  HAL_SolenoidHandle handle;
+  auto solenoidPort = solenoidHandles->Allocate(
+      module * kNumSolenoidChannels + channel, &handle, status);
   if (handle == HAL_kInvalidHandle) {  // out of resources
     *status = NO_AVAILABLE_RESOURCES;
     return HAL_kInvalidHandle;
   }
-  auto solenoidPort = solenoidHandles->Get(handle);
-  if (solenoidPort == nullptr) {  // would only occur on thread issues
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
   solenoidPort->module = static_cast<uint8_t>(module);
   solenoidPort->channel = static_cast<uint8_t>(channel);