soem.h

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <sstream>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>
#include <variant>
#include <vector>
 
#include <tl/expected.hpp>
 
#include "base.h"
#include "ethercat.h"
#include "loguru.h"
 
namespace mm::comm::soem {
 
// 3-second SDO read/write timeout — long enough for devices with slower mailbox
// implementations.
inline constexpr int kSdoTimeoutUs = 3'000'000;
 
// 10-second FoE write timeout — covers large firmware files over slow links.
inline constexpr int kFoeTimeoutUs = 10'000'000;
 
// 6-second FoE read timeout. A SOMANET bootloader can take several seconds to
// answer a read, and 6 s is the timeout known to work on such drives. A read
// given up too early fails on every attempt, because each attempt starts the
// read again after the abort (see abortFoeTransfer). The full wait is spent
// only when the device does not answer.
inline constexpr int kFoeReadTimeoutUs = 6'000'000;
 
// 100-millisecond wait for the answer to an FoE abort. The device's EtherCAT
// stack answers an abort itself, without involving the file system.
inline constexpr int kFoeAbortReplyTimeoutUs = 100'000;
 
// 10-second timeout for a file removal ("fs-remove=<name>" issued as an FoE
// read). Removing a file erases flash, which can take several seconds — much
// longer than reading one back.
inline constexpr int kFoeRemoveTimeoutUs = 10'000'000;
 
// 30-second timeout for removing the ESI file. It is large and its erase takes
// considerably longer than other files (measured at ~17 s on a SOMANET drive).
inline constexpr int kFoeRemoveEsiTimeoutUs = 30'000'000;

std::recursive_mutex& controlPlaneMutex();

std::string drainMailboxErrors(ecx_contextt* context);

int drainMailbox(ecx_contextt* context, uint16_t slave);

std::vector<uint8_t> makeFoeAbortRequest(uint8_t counter);

void abortFoeTransfer(ecx_contextt* context, uint16_t slave);

bool isTransientFoeFailure(int wkc);

std::string describeFoeFailure(int wkc);

std::map<std::string, std::string> mapMacAddressesToInterfaces();

struct IghMasterModule {
  bool loaded = false;
  std::vector<std::string> devices;

  [[nodiscard]] bool isConfiguredFor(const std::string& macAddress) const;
};

IghMasterModule readIghMasterModule(
    const std::string& moduleDirectory = "/sys/module/ec_master");

inline std::string uiConfigWithDefaultPdoMapping = R"(
{
  "pdoMapping": {
    "rx": {
      "0x1600": [
        "0x60400010",
        "0x60600008",
        "0x60710010",
        "0x607a0020",
        "0x60ff0020",
        "0x60b20010",
        "0x27010020"
      ],
      "0x1601": [
        "0x60fe0120",
        "0x60fe0220"
      ],
      "0x1602": [
        "0x27030020",
        "0x60b10020",
        "0x20120120"
      ]
    },
    "tx": {
      "0x1a00": [
        "0x60410010",
        "0x60610008",
        "0x60640020",
        "0x606c0020",
        "0x60770010",
        "0x60f40020",
        "0x21110220",
        "0x21130220"
      ],
      "0x1a01": [
        "0x24010010",
        "0x24020010",
        "0x24030010",
        "0x24040010",
        "0x27020020"
      ],
      "0x1a02": [
        "0x60fd0020"
      ],
      "0x1a03": [
        "0x27040020",
        "0x20f00020",
        "0x60fc0020",
        "0x606b0020",
        "0x60740010",
        "0x60790020"
      ]
    }
  }
})";

int setPdoMappingFromUiConfig(ecx_contextt* context, uint16 slave);

void checkPdoMapping(ecx_contextt* context, uint16 slave);

uint16_t getSlaveState(ecx_contextt* context, uint16_t slave,
                       int timeoutUs = EC_TIMEOUTRET);

bool isSlaveAnswering(ecx_contextt* context, uint16_t slave);

struct StationAddressReading {
  enum class Kind {
    kNoAnswer,
    kCleared,
    kChanged,
    kIntact,
  };
 
  Kind kind = Kind::kNoAnswer;
  uint16_t value = 0;
  uint16_t expected = 0;

  [[nodiscard]] std::string describe() const;
};

StationAddressReading readStationAddressByPosition(
    ecx_contextt* context, uint16_t slave, int timeoutUs = EC_TIMEOUTRET);

std::string convertSlaveStateToString(int state);

void updateMailboxSyncManagersOnNextState(ecx_contextt* context, uint16_t slave,
                                          int targetState);

void resetBootMailboxTracking();

bool resetMailboxCounter(ecx_contextt* context, uint16_t slave);

void configureDetectedSmmModule(ecx_contextt* context, uint16_t slave);

void transitionSlaves(ecx_contextt* context,
                      const std::vector<uint16_t>& positions,
                      uint16_t requiredState, uint16_t targetState,
                      const char* targetName,
                      std::chrono::steady_clock::duration timeout,
                      std::chrono::steady_clock::duration resendInterval =
                          std::chrono::seconds(2),
                      std::function<void()> tick = nullptr,
                      std::function<bool()> shouldAbort = nullptr);

bool setSlaveState(
    ecx_contextt* context, uint16_t slave, uint16_t targetState,
    std::chrono::steady_clock::duration timeout = std::chrono::seconds(5));

std::vector<uint8_t> readSii(ecx_contextt* context, uint16 position);

mm::comm::base::PdoMappings readPdoMappings(ecx_contextt* context,
                                            uint16_t slave);

void logPdoMappingEntries(
    const std::vector<mm::comm::base::PdoMappingEntry>& entries);

void logIoMap(uint8_t* ioMap, size_t totalBytes);

struct Fieldbus {
  ecx_contextt context;
  std::string iface;
  uint8 group;
  int roundtripTime;
 
  /* Used by the context */
  uint8 map[4096];
  ecx_portt port;
  ec_slavet slavelist[EC_MAXSLAVE];
  int slavecount;
  ec_groupt grouplist[EC_MAXGROUP];
  uint8 esibuf[EC_MAXEEPBUF];
  uint32 esimap[EC_MAXEEPBITMAP];
  ec_eringt elist;
  ec_idxstackT idxstack;
  boolean ecaterror;
  int64 DCtime;
  ec_SMcommtypet SMcommtype[EC_MAX_MAPT];
  ec_PDOassignt PDOassign[EC_MAX_MAPT];
  ec_PDOdesct PDOdesc[EC_MAX_MAPT];
  ec_eepromSMt eepSM;
  ec_eepromFMMUt eepFMMU;
};

struct SlaveInfoMailbox {
  uint16_t readLength;

  uint16_t readOffset;

  uint16_t supportedProtocols;

  uint16_t writeLength;

  uint16_t writeOffset;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(SlaveInfoMailbox, readLength, readOffset,
                                   supportedProtocols, writeLength, writeOffset)
 

struct SlaveInfoSyncManager {
  uint32_t flags;

  uint16_t length;

  uint16_t startAddress;

  uint8_t type;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(SlaveInfoSyncManager, flags, length,
                                   startAddress, type)
 

struct SlaveInfoFmmu {
  uint8_t active;

  uint8_t logicalEndBit;

  uint8_t logicalStartBit;

  uint32_t logicalStartAddress;

  uint16_t logicalLength;

  uint8_t physicalStartBit;

  uint16_t physicalStartAddress;

  uint8_t type;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(SlaveInfoFmmu, active, logicalEndBit,
                                   logicalStartBit, logicalStartAddress,
                                   logicalLength, physicalStartBit,
                                   physicalStartAddress, type)
 

struct SlaveInfo {
  uint16_t alStatusCode;

  uint16_t aliasAddress;

  std::vector<SlaveInfoFmmu> fmmus;

  SlaveInfoMailbox mailbox;

  uint32_t productCode;

  uint32_t position;

  uint32_t revisionNumber;

  std::vector<SlaveInfoSyncManager> syncManagers;

  std::string name;

  uint32_t vendorId;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(SlaveInfo, alStatusCode, aliasAddress, fmmus,
                                   mailbox, productCode, position,
                                   revisionNumber, syncManagers, name, vendorId)
 

SlaveInfo getSlaveInfo(ecx_contextt* context, uint16 position);

void logSlaveInfo(ecx_contextt* context, uint16 position);

void initFieldbus(std::unique_ptr<Fieldbus>& fieldbus,
                  const std::string& iface);

class Slave {
 public:
  explicit Slave(ecx_contextt& context, uint16_t position);

  uint16_t position() const;

  uint32_t vendorId() const;

  uint32_t productCode() const;

  uint32_t revisionNumber() const;

  int getState() const;

  bool setState(int targetState, int timeoutMs = 0, int intervalMs = 100);

  tl::expected<void, std::string> loadParameters(bool readValues = false);

  void logParameters() const;

  void logRxPdoMappedParameters();

  void logTxPdoMappedParameters();

  void clearParameters();

  std::unordered_map<mm::comm::base::Parameter::Address,
                     mm::comm::base::Parameter>&
  parametersMap();

  mm::comm::base::Parameter& findParameter(uint16_t index, uint8_t subindex);

  template <typename T = std::vector<uint8_t>>
  T upload(uint16_t index, uint8_t subindex, bool refresh = true) {
    std::lock_guard<std::recursive_mutex> lock(controlPlaneMutex());
 
    // Check if the device is in a valid state for uploading
    auto state = context_.slavelist[position_].state;
    if (state == EC_STATE_INIT || state == EC_STATE_BOOT) {
      throw std::runtime_error(
          "To upload object dictionary entries, the device must be in the (2) "
          "PRE-OPERATIONAL, (4) SAFE-OPERATIONAL, or (8) OPERATIONAL state. "
          "The current state is " +
          convertSlaveStateToString(state) + ".");
    }
 
    // Find the previously loaded parameter
    auto& parameter = findParameter(index, subindex);
 
    // Refresh data if needed (SDO read)
    if (refresh) {
      const std::unique_ptr<std::uint8_t[]> data =
          std::make_unique<std::uint8_t[]>(parameter.byteLength);
      auto wkc =
          ecx_SDOread(&context_, position_, parameter.index, parameter.subindex,
                      false, &parameter.byteLength, data.get(), kSdoTimeoutUs);
      if (wkc <= 0) {
        LOG_F(ERROR, "Device %d: SDO read failed for %#04x:%02x! WKC: %d",
              position_, parameter.index, parameter.subindex, wkc);
        auto id = mm::comm::base::makeParameterId(parameter.index,
                                                  parameter.subindex);
        throw std::runtime_error("Device " + std::to_string(position_) +
                                 ": SDO read failed for " + id +
                                 "! WKC: " + std::to_string(wkc));
      } else {
        std::vector<uint8_t> receivedData(data.get(),
                                          data.get() + parameter.byteLength);
        if (!parameter.trySetValue(receivedData)) {
          LOG_F(ERROR,
                "Device %d: Failed to set the value of parameter %#04x:%02x!",
                position_, parameter.index, parameter.subindex);
        }
      }
    }
 
    // Return the appropriate type
    if constexpr (std::is_same_v<T, std::vector<uint8_t>>) {
      return parameter.data;  // parameter.data should be std::vector<uint8_t>
    } else {
      // Otherwise, try to get the value as the requested type T
      auto value = parameter.getValue();
      if (auto ptr = std::get_if<T>(&value)) {
        return *ptr;
      } else {
        throw std::bad_variant_access();  // Type mismatch
      }
    }
  }

  template <typename T = std::vector<uint8_t>>
  bool download(uint16_t index, uint8_t subindex, const T& value) {
    std::lock_guard<std::recursive_mutex> lock(controlPlaneMutex());
 
    // Check if device is in valid state for download
    auto state = context_.slavelist[position_].state;
    if (state == EC_STATE_INIT || state == EC_STATE_BOOT) {
      throw std::runtime_error(
          "To download object dictionary entries, the device must be in the "
          "(2) PRE-OPERATIONAL, (4) SAFE-OPERATIONAL, or (8) OPERATIONAL "
          "state. The current state is " +
          convertSlaveStateToString(state) + ".");
    }
 
    auto& parameter = findParameter(index, subindex);
 
    // Convert value to raw byte data
    std::vector<uint8_t> data;
    if constexpr (std::is_same_v<T, std::vector<uint8_t>>) {
      data = value;
    } else {
      // Set the value in the parameter model
      if (!parameter.trySetValue(value)) {
        LOG_F(ERROR,
              "Device %d: Failed to set the value of parameter %#04x:%02x!",
              position_, parameter.index, parameter.subindex);
        return false;
      }
      data = parameter.data;  // Use the parameter's byte representation
    }
 
    // Check if data size matches the expected parameter size
    if (data.size() != parameter.byteLength) {
      LOG_F(ERROR, "Data size mismatch: expected %d, got %zu",
            parameter.byteLength, data.size());
      throw std::runtime_error("Data size does not match the parameter size!");
    }
 
    // Perform the SDO write
    auto wkc =
        ecx_SDOwrite(&context_, position_, parameter.index, parameter.subindex,
                     false, parameter.byteLength, data.data(), kSdoTimeoutUs);
 
    // Check write result
    if (wkc <= 0) {
      LOG_F(ERROR, "Device %d: SDO write failed for %#04x:%02x! WKC: %d",
            position_, parameter.index, parameter.subindex, wkc);
      return false;
    }
 
    return true;
  }

  mm::comm::base::PdoMappings readPdoMappings() const;

  void updatePdoMappings();

  void logPdoMappings() const;

  mm::comm::base::PdoMappings& getPdoMappings();

  void updateOutputs();

  void updateParametersFromInputs();

  tl::expected<std::vector<uint8_t>, std::string> readFile(
      const std::string& filename,
      size_t maxFileSize = 1 * 1024 * 1024,  // 1 MB default max file size
      int retries = 3,
      std::chrono::steady_clock::duration retryWait =
          std::chrono::milliseconds(100));

  tl::expected<std::vector<std::uint8_t>, std::string> readSdo(
      uint16_t index, uint8_t subindex);

  tl::expected<ec_ODlistt, std::string> readODList(
      int retries = 5, std::chrono::steady_clock::duration retryWait =
                           std::chrono::milliseconds(100));

  tl::expected<void, std::string> readODDescription(
      uint16_t item, ec_ODlistt& odList, int retries = 5,
      std::chrono::steady_clock::duration retryWait =
          std::chrono::milliseconds(100));

  tl::expected<ec_OElistt, std::string> readOE(
      uint16_t item, ec_ODlistt& odList, int retries = 5,
      std::chrono::steady_clock::duration retryWait =
          std::chrono::milliseconds(100));
 
 private:
  ecx_contextt& context_;    
  const uint16_t position_;  
  std::unordered_map<mm::comm::base::Parameter::Address,
                     mm::comm::base::Parameter>
      parametersMap_;  
  mm::comm::base::PdoMappings
      pdoMappings_;  
};
 
class Master {
 public:
  explicit Master();

  explicit Master(const std::string& iface);

  void init(const std::string& iface);

  void deinit();

  ~Master();

  Fieldbus& getFieldbus() const;

  std::string getInterfaceName() const;

  size_t getIoMapSize() const;

  tl::expected<int, std::string> initSlaves();

  const std::vector<std::unique_ptr<Slave>>& slaves() const;

  inline int expectedWkc() const {
    return fieldbus_->context.grouplist[fieldbus_->group].outputsWKC * 2 +
           fieldbus_->context.grouplist[fieldbus_->group].inputsWKC;
  }

  int roundtrip();

  void exchangeProcessDataAndUpdateParameters();

  bool start();
 
 private:
  std::unique_ptr<Fieldbus> fieldbus_;

  std::vector<std::unique_ptr<Slave>> slaves_;
};
};  // namespace mm::comm::soem