[hal] Digital IO SystemCore implementation (#7621)

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

@@ -11,6 +11,7 @@
 #include "HALInitializer.h"
 #include "HALInternal.h"
 #include "PortsInternal.h"
+#include "SmartIo.h"
 #include "hal/Errors.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
@@ -29,15 +30,69 @@ HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
                                         const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
-  *status = HAL_HANDLE_ERROR;
-  return HAL_kInvalidHandle;
+
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumSmartIo) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                     kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port =
+      smartIoHandles->Allocate(channel, HAL_HandleEnum::DIO, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                       kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(input ? SmartIoMode::DigitalInput
+                                       : SmartIoMode::DigitalOutput);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::DIO);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
 }
 
 HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
-  return channel < kNumDigitalChannels && channel >= 0;
+  return channel < kNumSmartIo && channel >= 0;
 }
 
-void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {}
+void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    return;
+  }
+
+  smartIoHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
+
+  // Wait for no other object to hold this handle.
+  auto start = hal::fpga_clock::now();
+  while (port.use_count() != 1) {
+    auto current = hal::fpga_clock::now();
+    if (start + std::chrono::seconds(1) < current) {
+      std::puts("DIO handle free timeout");
+      std::fflush(stdout);
+      break;
+    }
+    std::this_thread::yield();
+  }
+}
 
 void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
 }
@@ -74,24 +129,54 @@ void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
 
 void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
                 int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SetDigitalOutput(value);
 }
 
 void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
                          int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SwitchDioDirection(input);
 }
 
 HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return false;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  bool ret = false;
+  *status = port->GetDigitalInput(&ret);
+  return ret;
 }
 
 HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return false;
+  auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  switch (port->currentMode) {
+    case SmartIoMode::DigitalInput:
+      return true;
+    case SmartIoMode::DigitalOutput:
+      return false;
+    default:
+      *status = INCOMPATIBLE_STATE;
+      return false;
+  }
 }
 
 void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,