base.h

Go to the documentation of this file.

#pragma once
 
#include <condition_variable>
#include <map>
#include <optional>
#include <set>
#include <sstream>
#include <string>
#include <typeindex>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <variant>
#include <vector>
 
#include <nlohmann/json.hpp>
 
#include "core/util.h"
 
namespace mm::comm::base {
struct UiPdoMapping {
  std::map<std::uint16_t, std::vector<std::uint32_t>> rx;
  std::map<std::uint16_t, std::vector<std::uint32_t>> tx;
};

struct UiConfigJson {
  UiPdoMapping pdoMapping;
};

void from_json(const nlohmann::json& j, UiPdoMapping& p);

void from_json(const nlohmann::json& j, UiConfigJson& r);

enum class EtherCATVendorID : uint32_t {
  SYNAPTICON = 0x000022d2,  
 
  SENSODRIVE = 0x0000063a  
};
 
inline constexpr std::size_t kDeviceFilePartSize =
    9000;  

inline bool operator==(uint32_t lhs, EtherCATVendorID rhs) {
  return lhs == static_cast<uint32_t>(rhs);
}

inline bool operator==(EtherCATVendorID lhs, uint32_t rhs) {
  return static_cast<uint32_t>(lhs) == rhs;
}

struct PdoMappingEntry {
  uint16_t pdoIndex;    
  uint8_t pdoSubindex;  
  uint16_t index;       
  uint8_t subindex;     
  uint8_t
      bitlength;  

  PdoMappingEntry() = default;

  PdoMappingEntry(uint16_t pdoIndex, uint8_t pdoSubindex, uint16_t index,
                  uint8_t subindex, uint8_t bitlength)
      : pdoIndex(pdoIndex),
        pdoSubindex(pdoSubindex),
        index(index),
        subindex(subindex),
        bitlength(bitlength) {}
};

struct PdoMappings {
  std::vector<PdoMappingEntry> rxPdos;  
  std::vector<PdoMappingEntry> txPdos;  

  [[nodiscard]] size_t rxSizeBits() const {
    size_t total = 0;
    for (const auto& entry : rxPdos) {
      total += entry.bitlength;
    }
    return total;
  }

  [[nodiscard]] size_t txSizeBits() const {
    size_t total = 0;
    for (const auto& entry : txPdos) {
      total += entry.bitlength;
    }
    return total;
  }

  [[nodiscard]] size_t rxSizeBytes() const { return (rxSizeBits() + 7) / 8; }

  [[nodiscard]] size_t txSizeBytes() const { return (txSizeBits() + 7) / 8; }
};

enum class ObjectFlags : uint16_t {
  None = 0x0000,
 
  // Read access
  PO_RD = 0x0001,
  SO_RD = 0x0002,
  OP_RD = 0x0004,
  ALL_RD = PO_RD | SO_RD | OP_RD,
 
  // Write access
  PO_WR = 0x0008,
  SO_WR = 0x0010,
  OP_WR = 0x0020,
  ALL_WR = PO_WR | SO_WR | OP_WR,
 
  // Read/Write combinations
  PO_RDWR = PO_RD | PO_WR,
  SO_RDWR = SO_RD | SO_WR,
  OP_RDWR = OP_RD | OP_WR,
  ALL_RDWR = PO_RDWR | SO_RDWR | OP_RDWR,
 
  // Mapping
  RXPDO_MAP = 0x0040,
  TXPDO_MAP = 0x0080,
  RXTXPDO_MAP = 0x00C0,
 
  // Other flags
  BACKUP = 0x0100,
  STARTUP = 0x0200,
 
  ALL_LIST_FLAGS = RXPDO_MAP | TXPDO_MAP | BACKUP | STARTUP
};

inline ObjectFlags operator|(ObjectFlags lhs, ObjectFlags rhs) {
  return static_cast<ObjectFlags>(static_cast<uint16_t>(lhs) |
                                  static_cast<uint16_t>(rhs));
}

inline ObjectFlags operator&(ObjectFlags lhs, ObjectFlags rhs) {
  return static_cast<ObjectFlags>(static_cast<uint16_t>(lhs) &
                                  static_cast<uint16_t>(rhs));
}

inline ObjectFlags& operator|=(ObjectFlags& lhs, ObjectFlags rhs) {
  lhs = lhs | rhs;
  return lhs;
}

inline ObjectFlags SetObjectFlags(ObjectFlags b, ObjectFlags s, ObjectFlags p,
                                  ObjectFlags a) {
  return b | s | p | a;
}

enum class ObjectDataType : uint16_t {
  UNSPECIFIED = 0x0000,  
 
  BOOLEAN = 0x0001,  
  BYTE = 0x001E,     
  WORD = 0x001F,     
  DWORD = 0x0020,    
 
  BIT1 = 0x0030,   
  BIT2 = 0x0031,   
  BIT3 = 0x0032,   
  BIT4 = 0x0033,   
  BIT5 = 0x0034,   
  BIT6 = 0x0035,   
  BIT7 = 0x0036,   
  BIT8 = 0x0037,   
  BIT9 = 0x0038,   
  BIT10 = 0x0039,  
  BIT11 = 0x003A,  
  BIT12 = 0x003B,  
  BIT13 = 0x003C,  
  BIT14 = 0x003D,  
  BIT15 = 0x003E,  
  BIT16 = 0x003F,  
 
  BITARR8 = 0x002D,   
  BITARR16 = 0x002E,  
  BITARR32 = 0x002F,  
 
  INTEGER8 = 0x0002,   
  INTEGER16 = 0x0003,  
  INTEGER24 = 0x0010,  
  INTEGER32 = 0x0004,  
  INTEGER40 = 0x0012,  
  INTEGER48 = 0x0013,  
  INTEGER56 = 0x0014,  
  INTEGER64 = 0x0015,  
 
  UNSIGNED8 = 0x0005,   
  UNSIGNED16 = 0x0006,  
  UNSIGNED24 = 0x0016,  
  UNSIGNED32 = 0x0007,  
  UNSIGNED40 = 0x0018,  
  UNSIGNED48 = 0x0019,  
  UNSIGNED56 = 0x001A,  
  UNSIGNED64 = 0x001B,  
 
  REAL32 = 0x0008,  
  REAL64 = 0x0011,  
 
  GUID = 0x001D,  
 
  VISIBLE_STRING = 0x0009,  
  OCTET_STRING = 0x000A,    
  UNICODE_STRING = 0x000B,  
  ARRAY_OF_INT = 0x0260,    
  ARRAY_OF_SINT = 0x0261,   
  ARRAY_OF_DINT = 0x0262,   
  ARRAY_OF_UDINT = 0x0263,  
 
  PDO_MAPPING = 0x0021,    
  IDENTITY = 0x0023,       
  COMMAND_PAR = 0x0025,    
  PDO_PARAMETER = 0x0027,  
  ENUM = 0x0028,           
  SM_SYNCHRONIZATION =
      0x0029,       
  RECORD = 0x002A,  
  BACKUP_PARAMETER = 0x002B,  
  MODULAR_DEVICE_PARAMETER = 0x002C,  
  ERROR_SETTING = 0x0281,  
  DIAGNOSIS_HISTORY =
      0x0282,  
  EXTERNAL_SYNC_STATUS = 0x0283,    
  EXTERNAL_SYNC_SETTINGS = 0x0284,  
  DEFTYPE_FSOEFRAME = 0x0285,       
  DEFTYPE_FSOECOMMPAR =
      0x0286,  
 
  TIME_OF_DAY = 0x000C,  
  TIME_DIFFERENCE =
      0x000D,            
  UTYPE_START = 0x0800,  
  UTYPE_END = 0x0FFF     
};

enum class ObjectCode : uint16_t {
  DEFTYPE = 0x0005,    
  DEFSTRUCT = 0x0006,  
  VAR = 0x0007,        
  ARRAY = 0x0008,      
  RECORD = 0x0009,     
};

using ParameterValue =
    std::variant<bool, std::int8_t, std::int16_t, std::int32_t, std::int64_t,
                 std::uint8_t, std::uint16_t, std::uint32_t, std::uint64_t,
                 float, double, std::string,
                 std::vector<std::uint8_t>>;  

struct MappedParameterValue {
  uint16_t index;
  uint8_t subindex;
  ParameterValue value;
};

struct MappedParameterValues {
  std::vector<MappedParameterValue> rx;
  std::vector<MappedParameterValue> tx;
};

enum class PdoMappingStatus {
  NOT_MAPPED,     
  MAPPED_RX_PDO,  
  MAPPED_TX_PDO   
};

class Parameter {
 public:
  using Address = std::pair<uint16_t, uint8_t>;

  std::string group;

  std::string name;

  std::uint16_t index;

  std::uint8_t subindex;

  std::uint16_t bitLength;

  int byteLength;

  ObjectDataType dataType;

  ObjectCode code;

  ObjectFlags flags;

  ObjectFlags access;

  PdoMappingStatus pdoMappingStatus;

  std::vector<std::uint8_t> data;

  ParameterValue getValue() const;

  template <typename T>
  T getValue() const {
    // Get the value as a ParameterValue
    const ParameterValue& val = getValue();
 
    // Try to get the value of type T from the variant
    if (auto ptr = std::get_if<T>(&val)) {
      return *ptr;  // Return the value of type T
    } else {
      throw std::bad_variant_access();  // If type T is not found in the variant
    }
  }

  template <typename T>
  std::optional<T> tryGetValue() const {
    const ParameterValue& val = getValue();
    if (auto ptr = std::get_if<T>(&val)) {
      return *ptr;
    } else {
      return std::nullopt;
    }
  }

  template <typename T>
  T getIntValue() {
    return mm::core::util::toInteger<T>(data, 0,
                                        mm::core::util::Endianness::LITTLE);
  }

  void setValue(const ParameterValue& value);

  template <typename T>
  void setValue(const T& value) {
    // Convert the input value to ParameterValue (variant)
    ParameterValue parameterValue(value);
 
    // Reuse the existing setValue function to handle the assignment
    setValue(parameterValue);
  }

  template <typename T>
  bool trySetValue(const T& value) {
    auto expectedType = [this]() -> std::type_index {
      switch (dataType) {
        case ObjectDataType::BOOLEAN: {
          return typeid(bool);
        }
        case ObjectDataType::INTEGER8: {
          return typeid(std::int8_t);
        }
        case ObjectDataType::INTEGER16: {
          return typeid(std::int16_t);
        }
        case ObjectDataType::INTEGER24:
        case ObjectDataType::INTEGER32: {
          return typeid(std::int32_t);
        }
        case ObjectDataType::INTEGER64: {
          return typeid(std::int64_t);
        }
        case ObjectDataType::UNSIGNED8:
        case ObjectDataType::PDO_MAPPING:
        case ObjectDataType::IDENTITY:
        case ObjectDataType::COMMAND_PAR:
        case ObjectDataType::RECORD: {
          return typeid(std::uint8_t);
        }
        case ObjectDataType::UNSIGNED16: {
          return typeid(std::uint16_t);
        }
        case ObjectDataType::UNSIGNED24:
        case ObjectDataType::UNSIGNED32: {
          return typeid(std::uint32_t);
        }
        case ObjectDataType::UNSIGNED64: {
          return typeid(std::uint64_t);
        }
        case ObjectDataType::REAL32: {
          return typeid(float);
        }
        case ObjectDataType::REAL64: {
          return typeid(double);
        }
        case ObjectDataType::VISIBLE_STRING:
        case ObjectDataType::OCTET_STRING:
        case ObjectDataType::UNICODE_STRING: {
          return typeid(std::string);
        }
        default: {
          return typeid(void);
        }
      }
    };
 
    const std::type_index expected = expectedType();
    if (expected != typeid(T) &&
        typeid(T) != typeid(std::vector<std::uint8_t>)) {
      return false;
    }
 
    setValue(ParameterValue(value));
    return true;
  }

  bool hasAllRead() const;

  bool hasAllWrite() const;

  bool operator<(const Parameter& other) const {
    if (index != other.index) {
      return index < other.index;  // Compare by index first
    }
    return subindex <
           other.subindex;  // Compare by subindex if indices are equal
  }

  bool operator>(const Parameter& other) const {
    if (index != other.index) {
      return index > other.index;  // Compare by index first
    }
    return subindex >
           other.subindex;  // Compare by subindex if indices are equal
  }

  bool operator==(const Parameter& other) const {
    return index == other.index && subindex == other.subindex;
  }

  static void to_json(nlohmann::json& j, const Parameter& p);

  static void from_json(const nlohmann::json& j, Parameter& p);
};

std::string convertParameterValueToString(const ParameterValue& value);

void assignGroupsToParameters(std::vector<Parameter>& parameters);

void logParametersMap(
    const std::unordered_map<Parameter::Address, Parameter>& parametersMap,
    bool sortParameters = true);

const std::unordered_set<std::string> kFirmwareInstallationFilenameWhitelist = {
    "stack_image.svg.zip", "SOMANET_CiA_402.xml.zip"};

inline bool isAppBinFile(const std::string& filename) {
  return filename.rfind("app_", 0) == 0 && filename.size() >= 4 &&
         filename.compare(filename.size() - 4, 4, ".bin") == 0;
}

inline bool isComBinFile(const std::string& filename) {
  return filename.rfind("com_", 0) == 0 && filename.size() >= 4 &&
         filename.compare(filename.size() - 4, 4, ".bin") == 0;
}

inline bool isFirmwareBinFile(const std::string& filename) {
  return isAppBinFile(filename) || isComBinFile(filename);
}

enum class DeviceFileError : uint8_t {
  None = 0x00,
  NotFound = 0x01,
  AccessDenied = 0x02,
  StorageFull = 0x03,
  IllegalRequest = 0x04,
  PacketNumber = 0x05,
  AlreadyExists = 0x06,
  NoUser = 0x07,
  BootstrapOnly = 0x08,
  NotBootstrap = 0x09,
  NoRights = 0x0A,
  ProgramError = 0x0B,
  Busy = 0x0C,
  FilenameLength = 0x0D,
  Timeout = 0x0E,
  FlashBusy = 0x28,
  SuccessAcknowledgement = 0x58,
  CommunicationBridgeError = 0x63,
  EmptyContent = 0xD0,
  UnexpectedNullTerminator = 0xD1,
  ZipFailure = 0xD2,
  UnzipFailure = 0xD3,
  FlashFailure = 0xD4,
  WriteFailure = 0xD5,
  RemoveFailure = 0xD6,
  Unknown = 0xFF,
};

inline std::string deviceFileErrorToString(DeviceFileError error) {
  switch (error) {
    case DeviceFileError::None:
      return "No error";
    case DeviceFileError::NotFound:
      return "File not found";
    case DeviceFileError::EmptyContent:
      return "File is empty";
    case DeviceFileError::UnexpectedNullTerminator:
      return "File starts with a null terminator";
    case DeviceFileError::UnzipFailure:
      return "Failed to unzip file";
    case DeviceFileError::FlashFailure:
      return "Failed to flash device";
    case DeviceFileError::WriteFailure:
      return "Failed to write to device";
    case DeviceFileError::Unknown:
      return "An unknown error occurred";
    default:
      return "Unrecognized error";
  }
}

class Device {
 public:
  virtual ~Device() = default;

  virtual uint16_t getPosition() const = 0;

  virtual uint8_t getState(
      bool refresh = false,
      const std::chrono::steady_clock::duration expiryTime =
          std::chrono::milliseconds(5000)) = 0;

  virtual bool setState(uint8_t state,
                        const std::chrono::steady_clock::duration expiryTime =
                            std::chrono::milliseconds(5000)) = 0;

  virtual std::vector<uint8_t> readFile(
      const std::string& filename,
      const std::chrono::steady_clock::duration expiryTime =
          std::chrono::milliseconds(5000)) = 0;

  virtual std::vector<std::string> readFileList(
      const bool stripSizeSuffix,
      const std::chrono::steady_clock::duration expiryTime =
          std::chrono::milliseconds(5000)) = 0;

  virtual void writeFile(const std::string& filename,
                         const std::vector<uint8_t>& data,
                         const std::chrono::steady_clock::duration expiryTime =
                             std::chrono::milliseconds(5000)) = 0;

  virtual void removeFile(const std::string& filename,
                          const std::chrono::steady_clock::duration expiryTime =
                              std::chrono::milliseconds(5000)) = 0;

  virtual void updateFirmware(
      const std::vector<std::uint8_t>& data,
      const std::vector<std::string>& skipFiles = {},
      std::function<void(uint8_t, std::string)> progressCallback = nullptr,
      const std::chrono::steady_clock::duration expiryTime =
          std::chrono::milliseconds(60000)) = 0;

  virtual size_t loadParameters(
      bool readValues, const std::chrono::steady_clock::duration expiryTime =
                           std::chrono::milliseconds(9000)) = 0;

  virtual void clearParameters() = 0;

  virtual std::vector<std::reference_wrapper<Parameter>> parameters() = 0;

  virtual Parameter& findParameter(uint16_t index, uint8_t subindex) = 0;

  virtual Parameter& upload(
      uint16_t index, uint8_t subindex,
      const std::chrono::steady_clock::duration expiryTime =
          std::chrono::milliseconds(5000)) = 0;

  virtual void download(uint16_t index, uint8_t subindex,
                        const std::chrono::steady_clock::duration expiryTime =
                            std::chrono::milliseconds(5000)) = 0;

  virtual void download(uint16_t index, uint8_t subindex,
                        const ParameterValue& value,
                        const std::chrono::steady_clock::duration expiryTime =
                            std::chrono::milliseconds(5000)) = 0;

  virtual void exchangeProcessDataAndUpdateParameters(
      uint64_t missedCycles = 0) = 0;

  virtual PdoMappingStatus getPdoMappingStatus(uint16_t index,
                                               uint8_t subindex) const = 0;

  virtual bool isOnline() = 0;
};

class DeviceManager {
 public:
  virtual bool setState(
      uint8_t state, const std::chrono::steady_clock::duration expiryTime) = 0;

  virtual std::vector<uint8_t> exchangeProcessData(
      const std::vector<uint8_t>& data,
      const std::chrono::steady_clock::duration expiryTime) = 0;

  virtual void exchangeProcessDataAndUpdateParameters(
      uint64_t missedCycles,
      const std::chrono::steady_clock::duration expiryTime) = 0;
  virtual std::vector<Device>& devices() = 0;
 
 private:
  // Container holding all managed devices.
  std::vector<Device> devices_;
};

class DeviceResponseException : public std::runtime_error {
 public:
  DeviceResponseException(const std::string& what_arg, uint16_t position,
                          int code);

  uint16_t position() const noexcept;

  int code() const noexcept;
 
 private:
  uint16_t position_;
  int code_;
};

extern std::set<std::string> kNonPartedFileNames;

std::vector<std::uint8_t> readCompleteDeviceFile(
    Device& device, const std::string& filename,
    const std::chrono::steady_clock::duration expiryTime =
        std::chrono::milliseconds(20000));

void writeDeviceFileInOptionalParts(
    Device& device, const std::string& filename,
    const std::vector<std::uint8_t>& data,
    std::function<void(uint8_t, std::string)> progressCallback = nullptr,
    const std::chrono::steady_clock::duration expiryTime =
        std::chrono::milliseconds(30000));

void removeDeviceFileOrParts(Device& device, const std::string& filename,
                             const std::chrono::steady_clock::duration
                                 expiryTime = std::chrono::milliseconds(20000));

void writeFirmwarePackage(
    Device& device, const std::vector<std::uint8_t>& data,
    const std::vector<std::string>& skipFiles = {},
    std::function<void(uint8_t, std::string)> progressCallback = nullptr,
    const std::chrono::steady_clock::duration expiryTime =
        std::chrono::milliseconds(60000));

inline std::string makeParameterId(int index, int subindex) {
  std::stringstream oss;
  // Format index and subindex into "0xINDEX:SUBINDEX"
  oss << "0x" << std::setw(4) << std::setfill('0') << std::hex << std::uppercase
      << index << ":" << std::setw(2) << std::setfill('0') << std::hex
      << std::uppercase << subindex;
 
  return oss.str();
}

class DeviceParameterRefresher {
 public:
  DeviceParameterRefresher(Device& device,
                           std::chrono::milliseconds intervalBetweenUploads);

  ~DeviceParameterRefresher();

  void add(uint16_t index, uint8_t subindex);

  void remove(uint16_t index, uint8_t subindex);

  void clear();

  void start();

  void stop();

  void triggerManualRefresh(bool async = false);
 
 protected:
  virtual void refresh(const std::vector<Parameter::Address>& addresses) = 0;
 
  std::chrono::milliseconds
      interval_;  
  std::condition_variable
      cv_;            
  std::mutex mutex_;  
  std::vector<Parameter::Address>
      addresses_;   
  Device& device_;  
  bool running_;  
  std::thread worker_;  
 
 private:
  void run();
};

mm::comm::base::PdoMappingEntry parsePdoMapping(uint32_t mapping,
                                                uint16_t pdoIndex,
                                                uint8_t pdoSubindex);

std::vector<mm::comm::base::Parameter::Address> makeParameterAddressIndexRange(
    uint16_t start, uint16_t end, uint8_t maxSubIndex);

std::vector<mm::comm::base::Parameter::Address> makeAllPdoAddresses();
 
}  // namespace mm::comm::base
 
namespace nlohmann {
template <>
struct adl_serializer<mm::comm::base::ParameterValue> {
  static void to_json(json& j, const mm::comm::base::ParameterValue& value) {
    std::visit([&j](const auto& val) { j = val; }, value);
  }
 
  static void from_json(const json& j, mm::comm::base::ParameterValue& value) {
    if (j.is_boolean()) {
      value = j.get<bool>();
    } else if (j.is_number_integer()) {
      int64_t num = j.get<int64_t>();
      if (num >= -128 && num <= 127) {
        value = static_cast<int8_t>(num);
      } else if (num >= -32768 && num <= 32767) {
        value = static_cast<int16_t>(num);
      } else if (num >= -2147483648LL && num <= 2147483647LL) {
        value = static_cast<int32_t>(num);
      } else {
        value = num;  // int64_t
      }
    } else if (j.is_number_unsigned()) {
      uint64_t num = j.get<uint64_t>();
      if (num <= 255) {
        value = static_cast<uint8_t>(num);
      } else if (num <= 65535) {
        value = static_cast<uint16_t>(num);
      } else if (num <= 4294967295ULL) {
        value = static_cast<uint32_t>(num);
      } else {
        value = num;  // uint64_t
      }
    } else if (j.is_number_float()) {
      double f = j.get<double>();
      // float range: approx ±3.4e38
      if (f >= -3.4e38 && f <= 3.4e38) {
        value = static_cast<float>(f);
      } else {
        value = f;  // double
      }
    } else if (j.is_string()) {
      value = j.get<std::string>();
    } else if (j.is_array()) {
      value = j.get<std::vector<uint8_t>>();
    } else {
      throw std::runtime_error("Unsupported JSON type for ParameterValue");
    }
  }
};

inline void to_json(nlohmann::json& j,
                    const mm::comm::base::MappedParameterValue& p) {
  j = nlohmann::json{
      {"index", p.index}, {"subindex", p.subindex}, {"value", p.value}};
}

inline void from_json(const nlohmann::json& j,
                      mm::comm::base::MappedParameterValue& p) {
  j.at("index").get_to(p.index);
  j.at("subindex").get_to(p.subindex);
  j.at("value").get_to(p.value);
}

inline void to_json(json& j, const mm::comm::base::MappedParameterValues& p) {
  j = json{{"rx", p.rx}, {"tx", p.tx}};
}

inline void from_json(const json& j, mm::comm::base::MappedParameterValues& p) {
  j.at("rx").get_to(p.rx);
  j.at("tx").get_to(p.tx);
}
}  // namespace nlohmann