Add simulation generic device/value support

This allows high-level library classes to implement enhanced simulation
support even if no low-level corresponding simulation library exists, and
avoids the need for bit-banging complex interfaces like SPI or CAN.

hal/src/main/native/sim/mockdata/SimDeviceData.cpp

@@ -0,0 +1,413 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "mockdata/SimDeviceData.h"  // NOLINT(build/include_order)
+
+#include <algorithm>
+
+#include "SimDeviceDataInternal.h"
+
+using namespace hal;
+
+namespace hal {
+namespace init {
+void InitializeSimDeviceData() {
+  static SimDeviceData sdd;
+  ::hal::SimSimDeviceData = &sdd;
+}
+}  // namespace init
+}  // namespace hal
+
+SimDeviceData* hal::SimSimDeviceData;
+
+SimDeviceData::Device* SimDeviceData::LookupDevice(HAL_SimDeviceHandle handle) {
+  if (handle <= 0) return nullptr;
+  --handle;
+  if (static_cast<uint32_t>(handle) >= m_devices.size() || !m_devices[handle])
+    return nullptr;
+  return m_devices[handle].get();
+}
+
+SimDeviceData::Value* SimDeviceData::LookupValue(HAL_SimValueHandle handle) {
+  if (handle <= 0) return nullptr;
+
+  // look up device
+  Device* deviceImpl = LookupDevice(handle >> 16);
+
+  // look up value
+  handle &= 0xffff;
+  --handle;
+  if (static_cast<uint32_t>(handle) >= deviceImpl->values.size() ||
+      !deviceImpl->values[handle])
+    return nullptr;
+
+  return deviceImpl->values[handle].get();
+}
+
+HAL_SimDeviceHandle SimDeviceData::CreateDevice(const char* name) {
+  std::scoped_lock lock(m_mutex);
+
+  // check for duplicates and don't overwrite them
+  auto it = m_deviceMap.find(name);
+  if (it != m_deviceMap.end()) return 0;
+
+  // don't allow more than 4096 devices (limit driven by 12-bit allocation in
+  // value changed callback uid)
+  if (m_devices.size() >= 4095) return 0;
+
+  // create and save
+  auto deviceImpl = std::make_shared<Device>(name);
+  HAL_SimDeviceHandle deviceHandle = m_devices.emplace_back(deviceImpl) + 1;
+  deviceImpl->handle = deviceHandle;
+  m_deviceMap[name] = deviceImpl;
+
+  // notify callbacks
+  m_deviceCreated(name, deviceHandle);
+
+  return deviceHandle;
+}
+
+void SimDeviceData::FreeDevice(HAL_SimDeviceHandle handle) {
+  std::scoped_lock lock(m_mutex);
+  --handle;
+
+  // see if it exists
+  if (handle < 0 || static_cast<uint32_t>(handle) >= m_devices.size()) return;
+  auto deviceImpl = std::move(m_devices[handle]);
+  if (!deviceImpl) return;
+
+  // remove from map
+  m_deviceMap.erase(deviceImpl->name);
+
+  // remove from vector
+  m_devices.erase(handle);
+
+  // notify callbacks
+  m_deviceFreed(deviceImpl->name.c_str(), handle + 1);
+}
+
+HAL_SimValueHandle SimDeviceData::CreateValue(HAL_SimDeviceHandle device,
+                                              const char* name, bool readonly,
+                                              int32_t numOptions,
+                                              const char** options,
+                                              const HAL_Value& initialValue) {
+  std::scoped_lock lock(m_mutex);
+
+  // look up device
+  Device* deviceImpl = LookupDevice(device);
+  if (!deviceImpl) return 0;
+
+  // check for duplicates and don't overwrite them
+  auto it = deviceImpl->valueMap.find(name);
+  if (it != deviceImpl->valueMap.end()) return 0;
+
+  // don't allow more than 4096 values per device (limit driven by 12-bit
+  // allocation in value changed callback uid)
+  if (deviceImpl->values.size() >= 4095) return 0;
+
+  // create and save; encode device into handle
+  auto valueImplPtr = std::make_unique<Value>(name, readonly, initialValue);
+  Value* valueImpl = valueImplPtr.get();
+  HAL_SimValueHandle valueHandle =
+      (device << 16) |
+      (deviceImpl->values.emplace_back(std::move(valueImplPtr)) + 1);
+  valueImpl->handle = valueHandle;
+  // copy options (if any provided)
+  if (numOptions > 0 && options) {
+    valueImpl->enumOptions.reserve(numOptions);
+    valueImpl->cstrEnumOptions.reserve(numOptions);
+    for (int32_t i = 0; i < numOptions; ++i) {
+      valueImpl->enumOptions.emplace_back(options[i]);
+      // point to our copy of the string, not the passed-in one
+      valueImpl->cstrEnumOptions.emplace_back(
+          valueImpl->enumOptions.back().c_str());
+    }
+  }
+  deviceImpl->valueMap[name] = valueImpl;
+
+  // notify callbacks
+  deviceImpl->valueCreated(name, valueHandle, readonly, &initialValue);
+
+  return valueHandle;
+}
+
+HAL_Value SimDeviceData::GetValue(HAL_SimValueHandle handle) {
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+
+  if (!valueImpl) {
+    HAL_Value value;
+    value.data.v_int = 0;
+    value.type = HAL_UNASSIGNED;
+    return value;
+  }
+
+  return valueImpl->value;
+}
+
+void SimDeviceData::SetValue(HAL_SimValueHandle handle,
+                             const HAL_Value& value) {
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) return;
+
+  valueImpl->value = value;
+
+  // notify callbacks
+  valueImpl->changed(valueImpl->name.c_str(), valueImpl->handle,
+                     valueImpl->readonly, &value);
+}
+
+int32_t SimDeviceData::RegisterDeviceCreatedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+    bool initialNotify) {
+  std::scoped_lock lock(m_mutex);
+
+  // register callback
+  int32_t index = m_deviceCreated.Register(prefix, param, callback);
+
+  // initial notifications
+  if (initialNotify) {
+    for (auto&& device : m_devices)
+      callback(device->name.c_str(), param, device->handle);
+  }
+
+  return index;
+}
+
+void SimDeviceData::CancelDeviceCreatedCallback(int32_t uid) {
+  if (uid <= 0) return;
+  std::scoped_lock lock(m_mutex);
+  m_deviceCreated.Cancel(uid);
+}
+
+int32_t SimDeviceData::RegisterDeviceFreedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+  std::scoped_lock lock(m_mutex);
+  return m_deviceFreed.Register(prefix, param, callback);
+}
+
+void SimDeviceData::CancelDeviceFreedCallback(int32_t uid) {
+  if (uid <= 0) return;
+  std::scoped_lock lock(m_mutex);
+  m_deviceFreed.Cancel(uid);
+}
+
+HAL_SimDeviceHandle SimDeviceData::GetDeviceHandle(const char* name) {
+  std::scoped_lock lock(m_mutex);
+  auto it = m_deviceMap.find(name);
+  if (it == m_deviceMap.end()) return 0;
+  if (auto deviceImpl = it->getValue().lock())
+    return deviceImpl->handle;
+  else
+    return 0;
+}
+
+const char* SimDeviceData::GetDeviceName(HAL_SimDeviceHandle handle) {
+  std::scoped_lock lock(m_mutex);
+
+  // look up device
+  Device* deviceImpl = LookupDevice(handle);
+  if (!deviceImpl) return nullptr;
+
+  return deviceImpl->name.c_str();
+}
+
+void SimDeviceData::EnumerateDevices(const char* prefix, void* param,
+                                     HALSIM_SimDeviceCallback callback) {
+  std::scoped_lock lock(m_mutex);
+  for (auto&& device : m_devices) {
+    if (wpi::StringRef{device->name}.startswith(prefix))
+      callback(device->name.c_str(), param, device->handle);
+  }
+}
+
+int32_t SimDeviceData::RegisterValueCreatedCallback(
+    HAL_SimDeviceHandle device, void* param, HALSIM_SimValueCallback callback,
+    bool initialNotify) {
+  std::scoped_lock lock(m_mutex);
+  Device* deviceImpl = LookupDevice(device);
+  if (!deviceImpl) return -1;
+
+  // register callback
+  int32_t index = deviceImpl->valueCreated.Register(callback, param);
+
+  // initial notifications
+  if (initialNotify) {
+    for (auto&& value : deviceImpl->values)
+      callback(value->name.c_str(), param, value->handle, value->readonly,
+               &value->value);
+  }
+
+  // encode device into uid
+  return (device << 16) | (index & 0xffff);
+}
+
+void SimDeviceData::CancelValueCreatedCallback(int32_t uid) {
+  if (uid <= 0) return;
+  std::scoped_lock lock(m_mutex);
+  Device* deviceImpl = LookupDevice(uid >> 16);
+  if (!deviceImpl) return;
+  deviceImpl->valueCreated.Cancel(uid & 0xffff);
+}
+
+int32_t SimDeviceData::RegisterValueChangedCallback(
+    HAL_SimValueHandle handle, void* param, HALSIM_SimValueCallback callback,
+    bool initialNotify) {
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) return -1;
+
+  // register callback
+  int32_t index = valueImpl->changed.Register(callback, param);
+
+  // initial notification
+  if (initialNotify)
+    callback(valueImpl->name.c_str(), param, valueImpl->handle,
+             valueImpl->readonly, &valueImpl->value);
+
+  // encode device and value into uid
+  return (((handle >> 16) & 0xfff) << 19) | ((handle & 0xfff) << 7) |
+         (index & 0x7f);
+}
+
+void SimDeviceData::CancelValueChangedCallback(int32_t uid) {
+  if (uid <= 0) return;
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(((uid >> 19) << 16) | ((uid >> 7) & 0xfff));
+  if (!valueImpl) return;
+  valueImpl->changed.Cancel(uid & 0x7f);
+}
+
+HAL_SimValueHandle SimDeviceData::GetValueHandle(HAL_SimDeviceHandle device,
+                                                 const char* name) {
+  std::scoped_lock lock(m_mutex);
+  Device* deviceImpl = LookupDevice(device);
+  if (!deviceImpl) return 0;
+
+  // lookup value
+  auto it = deviceImpl->valueMap.find(name);
+  if (it == deviceImpl->valueMap.end()) return 0;
+  if (!it->getValue()) return 0;
+  return it->getValue()->handle;
+}
+
+void SimDeviceData::EnumerateValues(HAL_SimDeviceHandle device, void* param,
+                                    HALSIM_SimValueCallback callback) {
+  std::scoped_lock lock(m_mutex);
+  Device* deviceImpl = LookupDevice(device);
+  if (!deviceImpl) return;
+
+  for (auto&& value : deviceImpl->values)
+    callback(value->name.c_str(), param, value->handle, value->readonly,
+             &value->value);
+}
+
+const char** SimDeviceData::GetValueEnumOptions(HAL_SimValueHandle handle,
+                                                int32_t* numOptions) {
+  *numOptions = 0;
+
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) return nullptr;
+
+  // get list of options (safe to return as they never change)
+  auto& options = valueImpl->cstrEnumOptions;
+  *numOptions = options.size();
+  return options.data();
+}
+
+void SimDeviceData::ResetData() {
+  std::scoped_lock lock(m_mutex);
+  m_devices.clear();
+  m_deviceMap.clear();
+  m_deviceCreated.Reset();
+  m_deviceFreed.Reset();
+}
+
+extern "C" {
+
+int32_t HALSIM_RegisterSimDeviceCreatedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+    HAL_Bool initialNotify) {
+  return SimSimDeviceData->RegisterDeviceCreatedCallback(
+      prefix, param, callback, initialNotify);
+}
+
+void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {
+  SimSimDeviceData->CancelDeviceCreatedCallback(uid);
+}
+
+int32_t HALSIM_RegisterSimDeviceFreedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+  return SimSimDeviceData->RegisterDeviceFreedCallback(prefix, param, callback);
+}
+
+void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {
+  SimSimDeviceData->CancelDeviceFreedCallback(uid);
+}
+
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) {
+  return SimSimDeviceData->GetDeviceHandle(name);
+}
+
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) {
+  return SimSimDeviceData->GetDeviceName(handle);
+}
+
+HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
+  if (handle <= 0) return 0;
+  return handle >> 16;
+}
+
+void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
+                                HALSIM_SimDeviceCallback callback) {
+  SimSimDeviceData->EnumerateDevices(prefix, param, callback);
+}
+
+int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify) {
+  return SimSimDeviceData->RegisterValueCreatedCallback(device, param, callback,
+                                                        initialNotify);
+}
+
+void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {
+  SimSimDeviceData->CancelValueCreatedCallback(uid);
+}
+
+int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify) {
+  return SimSimDeviceData->RegisterValueChangedCallback(handle, param, callback,
+                                                        initialNotify);
+}
+
+void HALSIM_CancelSimValueChangedCallback(int32_t uid) {
+  SimSimDeviceData->CancelValueChangedCallback(uid);
+}
+
+HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
+                                            const char* name) {
+  return SimSimDeviceData->GetValueHandle(device, name);
+}
+
+void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
+                               HALSIM_SimValueCallback callback) {
+  SimSimDeviceData->EnumerateValues(device, param, callback);
+}
+
+const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
+                                           int32_t* numOptions) {
+  return SimSimDeviceData->GetValueEnumOptions(handle, numOptions);
+}
+
+void HALSIM_ResetSimDeviceData(void) { SimSimDeviceData->ResetData(); }
+
+}  // extern "C"

hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h

@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 <stdint.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <wpi/StringMap.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "hal/Value.h"
+#include "mockdata/SimCallbackRegistry.h"
+#include "mockdata/SimDeviceData.h"
+
+namespace hal {
+
+namespace impl {
+
+template <typename CallbackFunction>
+class SimUnnamedCallbackRegistry {
+ public:
+  using RawFunctor = void (*)();
+
+ protected:
+  using CallbackVector = wpi::UidVector<HalCallbackListener<RawFunctor>, 4>;
+
+ public:
+  void Cancel(int32_t uid) {
+    if (m_callbacks) m_callbacks->erase(uid - 1);
+  }
+
+  void Reset() {
+    if (m_callbacks) m_callbacks->clear();
+  }
+
+  int32_t Register(CallbackFunction callback, void* param) {
+    // Must return -1 on a null callback for error handling
+    if (callback == nullptr) return -1;
+    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    return m_callbacks->emplace_back(param,
+                                     reinterpret_cast<RawFunctor>(callback)) +
+           1;
+  }
+
+  template <typename... U>
+  void Invoke(const char* name, U&&... u) const {
+    if (m_callbacks) {
+      for (auto&& cb : *m_callbacks) {
+        reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
+                                                        std::forward<U>(u)...);
+      }
+    }
+  }
+
+  template <typename... U>
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+    return Invoke(std::forward<U>(u)...);
+  }
+
+ private:
+  std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+template <typename CallbackFunction>
+class SimPrefixCallbackRegistry {
+  struct CallbackData {
+    CallbackData() = default;
+    CallbackData(const char* prefix_, void* param_, CallbackFunction callback_)
+        : prefix{prefix_}, callback{callback_}, param{param_} {}
+    std::string prefix;
+    CallbackFunction callback;
+    void* param;
+
+    explicit operator bool() const { return callback != nullptr; }
+  };
+  using CallbackVector = wpi::UidVector<CallbackData, 4>;
+
+ public:
+  void Cancel(int32_t uid) {
+    if (m_callbacks) m_callbacks->erase(uid - 1);
+  }
+
+  void Reset() {
+    if (m_callbacks) m_callbacks->clear();
+  }
+
+  int32_t Register(const char* prefix, void* param, CallbackFunction callback) {
+    // Must return -1 on a null callback for error handling
+    if (callback == nullptr) return -1;
+    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    return m_callbacks->emplace_back(prefix, param, callback) + 1;
+  }
+
+  template <typename... U>
+  void Invoke(const char* name, U&&... u) const {
+    if (m_callbacks) {
+      for (auto&& cb : *m_callbacks) {
+        if (wpi::StringRef{name}.startswith(cb.prefix)) {
+          cb.callback(name, cb.param, std::forward<U>(u)...);
+        }
+      }
+    }
+  }
+
+  template <typename... U>
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+    return Invoke(std::forward<U>(u)...);
+  }
+
+ private:
+  std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+}  // namespace impl
+
+class SimDeviceData {
+ private:
+  struct Value {
+    Value(const char* name_, bool readonly_, const HAL_Value& value_)
+        : name{name_}, readonly{readonly_}, value{value_} {}
+
+    HAL_SimValueHandle handle{0};
+    std::string name;
+    bool readonly;
+    HAL_Value value;
+    std::vector<std::string> enumOptions;
+    std::vector<const char*> cstrEnumOptions;
+    impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> changed;
+  };
+
+  struct Device {
+    explicit Device(const char* name_) : name{name_} {}
+
+    HAL_SimDeviceHandle handle{0};
+    std::string name;
+    wpi::UidVector<std::unique_ptr<Value>, 16> values;
+    wpi::StringMap<Value*> valueMap;
+    impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> valueCreated;
+  };
+
+  wpi::UidVector<std::shared_ptr<Device>, 4> m_devices;
+  wpi::StringMap<std::weak_ptr<Device>> m_deviceMap;
+
+  wpi::recursive_spinlock m_mutex;
+
+  impl::SimPrefixCallbackRegistry<HALSIM_SimDeviceCallback> m_deviceCreated;
+  impl::SimPrefixCallbackRegistry<HALSIM_SimDeviceCallback> m_deviceFreed;
+
+  // call with lock held, returns null if does not exist
+  Device* LookupDevice(HAL_SimDeviceHandle handle);
+  Value* LookupValue(HAL_SimValueHandle handle);
+
+ public:
+  HAL_SimDeviceHandle CreateDevice(const char* name);
+  void FreeDevice(HAL_SimDeviceHandle handle);
+  HAL_SimValueHandle CreateValue(HAL_SimDeviceHandle device, const char* name,
+                                 bool readonly, int32_t numOptions,
+                                 const char** options,
+                                 const HAL_Value& initialValue);
+  HAL_Value GetValue(HAL_SimValueHandle handle);
+  void SetValue(HAL_SimValueHandle handle, const HAL_Value& value);
+
+  int32_t RegisterDeviceCreatedCallback(const char* prefix, void* param,
+                                        HALSIM_SimDeviceCallback callback,
+                                        bool initialNotify);
+
+  void CancelDeviceCreatedCallback(int32_t uid);
+
+  int32_t RegisterDeviceFreedCallback(const char* prefix, void* param,
+                                      HALSIM_SimDeviceCallback callback);
+
+  void CancelDeviceFreedCallback(int32_t uid);
+
+  HAL_SimDeviceHandle GetDeviceHandle(const char* name);
+  const char* GetDeviceName(HAL_SimDeviceHandle handle);
+
+  void EnumerateDevices(const char* prefix, void* param,
+                        HALSIM_SimDeviceCallback callback);
+
+  int32_t RegisterValueCreatedCallback(HAL_SimDeviceHandle device, void* param,
+                                       HALSIM_SimValueCallback callback,
+                                       bool initialNotify);
+
+  void CancelValueCreatedCallback(int32_t uid);
+
+  int32_t RegisterValueChangedCallback(HAL_SimValueHandle handle, void* param,
+                                       HALSIM_SimValueCallback callback,
+                                       bool initialNotify);
+
+  void CancelValueChangedCallback(int32_t uid);
+
+  HAL_SimValueHandle GetValueHandle(HAL_SimDeviceHandle device,
+                                    const char* name);
+
+  void EnumerateValues(HAL_SimDeviceHandle device, void* param,
+                       HALSIM_SimValueCallback callback);
+
+  const char** GetValueEnumOptions(HAL_SimValueHandle handle,
+                                   int32_t* numOptions);
+
+  void ResetData();
+};
+extern SimDeviceData* SimSimDeviceData;
+}  // namespace hal