cyclic.cpp

Go to the documentation of this file.

 
 
#include "profinet/cyclic.h"
 
#include <algorithm>
#include <cstdio>
#include <iostream>
#include <stdexcept>
 
#include "profinet/exceptions.h"
 
namespace profinet::cyclic
{
 
std::string ToString(CyclicState state)
{
  switch (state)
  {
    case CyclicState::Idle:
      return "idle";
    case CyclicState::Starting:
      return "starting";
    case CyclicState::Running:
      return "running";
    case CyclicState::Stopping:
      return "stopping";
    case CyclicState::Stopped:
      return "stopped";
    case CyclicState::Fault:
      return "fault";
  }
  return "unknown";
}
 
void CyclicStats::Reset()
{
  framesSent = 0;
  framesReceived = 0;
  framesMissed = 0;
  framesInvalid = 0;
  framesDuplicate = 0;
  framesOutOfOrder = 0;
  lastCycleTimeUs = 0;
  maxJitterUs = 0;
  minCycleTimeUs = 0x7FFFFFFF;
  maxCycleTimeUs = 0;
  lastReceiveTime = std::chrono::steady_clock::now();
  consecutiveTimeouts = 0;
  cycleTimeSumUs = 0;
  cycleCount = 0;
}
 
CyclicController::CyclicController(std::string interface, MacAddress srcMac, MacAddress dstMac,
                                   rt::IOCRConfig inputIocr, rt::IOCRConfig outputIocr,
                                   int maxConsecutiveTimeouts, std::uint16_t etherType)
    : interface(std::move(interface)),
      srcMac(srcMac),
      dstMac(dstMac),
      inputIocr(std::move(inputIocr)),
      outputIocr(std::move(outputIocr)),
      maxConsecutiveTimeouts(maxConsecutiveTimeouts),
      etherType(etherType),
      outputBuilder(this->outputIocr),
      iocsGood(true)
{
  if (this->maxConsecutiveTimeouts < 0)
  {
    throw std::invalid_argument("max_consecutive_timeouts must be >= 0, got " +
                                std::to_string(this->maxConsecutiveTimeouts));
  }
 
  rxCounterStep = static_cast<std::uint32_t>(this->inputIocr.sendClockFactor) * this->inputIocr.reductionRatio;
  txCounterStep = static_cast<std::uint32_t>(this->outputIocr.sendClockFactor) * this->outputIocr.reductionRatio;
 
  CheckCycleTime();
 
  // Initialize provider and consumer status to good before the first
  // transmitted frame. Some IO devices do not start their input provider
  // until the controller reports that it is consuming input data.
  outputBuilder.SetAllIops(rt::IOXS_GOOD);
 
  outputBuilder.SetAllIocs(rt::IOXS_GOOD);
}
 
CyclicController::~CyclicController()
{
  if (running)
  {
    Stop();
  }
}
 
void CyclicController::CheckCycleTime()
{
  const double cycleMs = outputIocr.CycleTimeMs();
  if (cycleMs < 1)
  {
    // Extract the formatted error message into a message
    const std::string message = std::format(
        "Cycle time {:.2f}ms is below 1ms - not achievable. Use a hardware-based controller for "
        "sub-millisecond cycles.",
        cycleMs);
 
    throw std::invalid_argument(message);
  }
  // Cycle times below PYTHON_MIN_CYCLE_MS were flagged in the Python
  // original due to GIL/interpreter overhead; kept as a soft warning
  // here for parity, though this port has no equivalent hard limit.
}
 
void CyclicController::Transition(CyclicState newState)
{
  const CyclicState old = state.exchange(newState);
  if (old == newState)
  {
    return;
  }
  if (onStateChange)
  {
    try
    {
      onStateChange(old, newState);
    }
    catch (...)
    {
    }
  }
}
 
void CyclicController::SetOutputData(int slot, int subslot, const Bytes& data)
{
  const CyclicState s = state;
  if (s == CyclicState::Fault || s == CyclicState::Stopped)
  {
    throw std::runtime_error("Cannot set output data in " + ToString(s) + " state");
  }
  outputBuilder.SetData(slot, subslot, data);
  outputBuilder.SetIops(slot, subslot, rt::IOXS_GOOD);
}
 
std::optional<Bytes> CyclicController::GetInputData(int slot, int subslot, bool allowBad) const
{
  const std::lock_guard<std::mutex> lock(inputLock);
  auto it = inputData.find({slot, subslot});
  if (it == inputData.end() || (!allowBad && !LocalIsInputGood(slot, subslot)))
  {
    return std::nullopt;
  }
  return it->second;
}
 
std::optional<std::uint8_t> CyclicController::GetInputStatus(const std::uint16_t slot, const std::uint16_t subslot) const
{
  const std::lock_guard<std::mutex> lock(inputLock);
 
  const auto it = inputStatus.find({slot, subslot});
 
  if (it == inputStatus.end())
  {
    return std::nullopt;
  }
 
  return it->second;
}
 
bool CyclicController::IsInputGood(const std::uint16_t slot, const std::uint16_t subslot) const
{
  const std::lock_guard<std::mutex> lock(inputLock);
 
  return LocalIsInputGood(slot, subslot);
}
 
bool CyclicController::LocalIsInputGood(const std::uint16_t slot, const std::uint16_t subslot) const
{
  const auto it = inputStatus.find({slot, subslot});
 
  if (it == inputStatus.end())
  {
    return false;
  }
 
  return (it->second & rt::IOXS_DATA_STATE_GOOD) != 0;
}
 
void CyclicController::OnInputStatus(std::function<void(const std::uint16_t, const std::uint16_t, const std::uint8_t)> callback)
{
  onInputStatus = std::move(callback);
}
 
void CyclicController::OnInput(std::function<void(int, int, const Bytes&)> callback)
{
  onInputData = std::move(callback);
}
void CyclicController::OnTimeout(std::function<void()> callback)
{
  onTimeout = std::move(callback);
}
void CyclicController::OnError(std::function<void(const std::string&)> callback)
{
  onError = std::move(callback);
}
void CyclicController::OnStateChange(std::function<void(CyclicState, CyclicState)> callback)
{
  onStateChange = std::move(callback);
}
 
void CyclicController::Start()
{
  const CyclicState s = state;
  if (s != CyclicState::Idle && s != CyclicState::Stopped && s != CyclicState::Fault)
  {
    throw std::runtime_error("Cannot start from " + ToString(s) + " state");
  }
 
  Transition(CyclicState::Starting);
  running = true;
  stats.Reset();
  lastRxCycleCounter.reset();
 
  txSock.emplace(interface, etherType);
  // The RX socket binds ETH_P_ALL:
  // a socket bound to 0x8892 never sees VLAN-tagged frames unless the
  // NIC strips the tag, and devices commonly send priority-tagged RT.
  rxSock.emplace(interface, 0); // ETH_P_ALL
  rxSock->SetTimeout(std::chrono::milliseconds(1));
 
  outputBuilder.Swap();
 
  txThread = std::thread([this]
  { TxLoop(); });
  rxThread = std::thread([this]
  { RxLoop(); });
 
  Transition(CyclicState::Running);
}
 
void CyclicController::Stop()
{
  if (!running)
  {
    return;
  }
 
  Transition(CyclicState::Stopping);
 
  // 1. Signal threads to exit before sending stop frames.
  running = false;
 
  // 2. Wait for TX thread first -- no concurrent socket access during stop frames.
  if (txThread.joinable())
  {
    txThread.join();
  }
 
  // 3. Send STOP frames after TX thread has exited.
  SendStopFrames();
 
  // 4. Close RX socket to unblock recv, then wait for RX thread.
  rxSock.reset();
  if (rxThread.joinable())
  {
    rxThread.join();
  }
 
  // 5. Close TX socket after stop frames sent.
  txSock.reset();
 
  Transition(CyclicState::Stopped);
}
 
CyclicStatsSnapshot CyclicController::StatsSnapshot() const
{
  CyclicStatsSnapshot snapshot;
 
  snapshot.framesSent = stats.framesSent.load(std::memory_order_relaxed);
 
  snapshot.framesReceived = stats.framesReceived.load(std::memory_order_relaxed);
 
  snapshot.framesMissed = stats.framesMissed.load(std::memory_order_relaxed);
 
  snapshot.framesInvalid = stats.framesInvalid.load(std::memory_order_relaxed);
 
  snapshot.framesDuplicate = stats.framesDuplicate.load(std::memory_order_relaxed);
 
  snapshot.framesOutOfOrder = stats.framesOutOfOrder.load(std::memory_order_relaxed);
 
  snapshot.lastCycleTimeUs = stats.lastCycleTimeUs.load(std::memory_order_relaxed);
 
  snapshot.maxJitterUs = stats.maxJitterUs.load(std::memory_order_relaxed);
 
  snapshot.minCycleTimeUs = stats.minCycleTimeUs.load(std::memory_order_relaxed);
 
  snapshot.maxCycleTimeUs = stats.maxCycleTimeUs.load(std::memory_order_relaxed);
 
  snapshot.lastReceiveTime = stats.lastReceiveTime.load(std::memory_order_relaxed);
 
  snapshot.consecutiveTimeouts = stats.consecutiveTimeouts.load(std::memory_order_relaxed);
 
  snapshot.cycleTimeSumUs = stats.cycleTimeSumUs.load(std::memory_order_relaxed);
 
  snapshot.cycleCount = stats.cycleCount.load(std::memory_order_relaxed);
 
  return snapshot;
}
 
void CyclicController::TxCycle()
{
  outputBuilder.Swap();
 
  SendOutputFrame();
 
  stats.framesSent.fetch_add(1, std::memory_order_relaxed);
}
 
void CyclicController::TxLoop()
{
  const double cycleTimeS = outputIocr.CycleTimeUs() / 1'000'000.0;
  auto nextSend = std::chrono::steady_clock::now();
  auto lastSend = nextSend;
  bool firstFrame = true;
 
  while (running)
  {
    auto now = std::chrono::steady_clock::now();
 
    if (now >= nextSend)
    {
      /*
      if (state != CyclicState::Fault)
      {
        outputBuilder.Swap();
        SendOutputFrame();
        stats.framesSent.fetch_add(1, std::memory_order_relaxed);
      }
      */
      TxCycle();
 
      if (firstFrame)
      {
        firstFrame = false;
      }
      else
      {
        auto actualUs =
            static_cast<std::uint32_t>(std::chrono::duration_cast<std::chrono::microseconds>(now - lastSend)
                                           .count());
        stats.lastCycleTimeUs = actualUs;
        const std::uint32_t target = outputIocr.CycleTimeUs();
        const std::uint32_t jitter = actualUs > target ? actualUs - target : target - actualUs;
        const std::uint32_t prevMax = stats.maxJitterUs.load();
        if (jitter > prevMax)
        {
          stats.maxJitterUs = jitter;
        }
        const std::uint32_t prevMin = stats.minCycleTimeUs.load();
        if (actualUs < prevMin)
        {
          stats.minCycleTimeUs = actualUs;
        }
        const std::uint32_t prevMaxCycle = stats.maxCycleTimeUs.load();
        if (actualUs > prevMaxCycle)
        {
          stats.maxCycleTimeUs = actualUs;
        }
        stats.cycleTimeSumUs += actualUs;
        stats.cycleCount += 1;
      }
 
      lastSend = now;
 
      auto cycleDuration = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
          std::chrono::duration<double>(cycleTimeS));
      nextSend += cycleDuration;
 
      if (nextSend < now)
      {
        nextSend = now + cycleDuration;
      }
    }
 
    auto sleepTime = nextSend - std::chrono::steady_clock::now() - std::chrono::microseconds(100);
    if (sleepTime > std::chrono::steady_clock::duration::zero())
    {
      std::this_thread::sleep_for(sleepTime);
    }
  }
}
 
void CyclicController::SendOutputFrame(std::optional<std::uint8_t> dataStatusOverride)
{
  cycleCounter = static_cast<std::uint16_t>((cycleCounter + txCounterStep) & 0xFFFF);
 
  const Bytes payload = outputBuilder.Build();
 
  const std::uint8_t dataStatus = dataStatusOverride.value_or(
      static_cast<std::uint8_t>(rt::DATA_STATUS_VALID | rt::DATA_STATUS_PROVIDER_RUN | rt::DATA_STATUS_STATION_OK |
                                rt::DATA_STATUS_STATE));
  // const std::uint8_t dataStatus = 0xc0;
 
  rt::RTFrame frame;
  frame.frameId = outputIocr.frameId;
  frame.cycleCounter = cycleCounter;
  frame.dataStatus = dataStatus;
  // frame.dataStatus = 0x80; // Set the PROFINET DataState byte to GOOD (0x80)
  frame.transferStatus = 0x00;
  // frame.transferStatus = 0x80; // Set the TranferStatus byte to GOOD (0x80)
  frame.payload = payload;
 
  const Bytes ethFrame = rt::BuildEthernetFrame(dstMac, srcMac, frame);
 
  try
  {
    if (txSock)
    {
      txSock->Send(ethFrame);
    }
  }
  catch (const std::exception& e)
  {
    if (onError)
    {
      onError(std::string("TX error: ") + e.what());
    }
  }
}
 
void CyclicController::SendStopFrames()
{
  if (!txSock)
  {
    return;
  }
 
  auto stopStatus = static_cast<std::uint8_t>(rt::DATA_STATUS_VALID | rt::DATA_STATUS_STATION_OK |
                                              rt::DATA_STATUS_STATE);
  const double cycleTimeS = outputIocr.CycleTimeUs() / 1'000'000.0;
 
  for (int i = 0; i < STOP_FRAME_COUNT; ++i)
  {
    try
    {
      outputBuilder.Swap();
      SendOutputFrame(stopStatus);
      stats.framesSent.fetch_add(1, std::memory_order_relaxed);
    }
    catch (const std::exception&)
    {
      break;
    }
    if (i < STOP_FRAME_COUNT - 1)
    {
      std::this_thread::sleep_for(std::chrono::duration<double>(cycleTimeS));
    }
  }
}
 
void CyclicController::RxLoop()
{
  const double watchdogSeconds = inputIocr.WatchdogTimeUs() / 1'000'000.0;
  stats.lastReceiveTime = std::chrono::steady_clock::now();
 
  while (running)
  {
    if (!rxSock)
    {
      break;
    }
    Bytes data;
    try
    {
      data = rxSock->Recv();
    }
    catch (const std::exception& e)
    {
      if (running && onError)
      {
        onError(std::string("RX error: ") + e.what());
      }
      continue;
    }
 
    if (data.empty())
    {
      // Timeout (or socket closed returning 0 bytes) - check watchdog.
      auto elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - stats.lastReceiveTime.load(std::memory_order::relaxed)).count();
      if (elapsed > watchdogSeconds)
      {
        HandleWatchdogTimeout();
        stats.lastReceiveTime = std::chrono::steady_clock::now();
      }
      continue;
    }
 
    ProcessInputFrame(data);
  }
}
 
void CyclicController::HandleWatchdogTimeout()
{
  stats.framesMissed.fetch_add(1, std::memory_order_relaxed);
  const int consecutive = stats.consecutiveTimeouts.fetch_add(1, std::memory_order_relaxed) + 1;
 
  if (maxConsecutiveTimeouts > 0 && iocsGood)
  {
    // Set IOCS to BAD only when watchdog faulting is enabled. With
    // max_consecutive_timeouts=0 the watchdog is monitoring-only; keep
    // consumer status GOOD so transient RX gaps do not make the device drop
    // an otherwise active output relationship.
    outputBuilder.SetAllIocs(rt::IOXS_BAD);
    iocsGood = false;
  }
 
  if (onTimeout)
  {
    try
    {
      onTimeout();
    }
    // NOLINTNEXTLINE(bugprone-empty-catch)
    catch (...)
    {
      // proceed
    }
  }
 
  if (maxConsecutiveTimeouts > 0 && consecutive >= maxConsecutiveTimeouts && state == CyclicState::Running)
  {
    Transition(CyclicState::Fault);
    if (onError)
    {
      onError("Communication lost: " + std::to_string(consecutive) + " consecutive watchdog timeouts");
    }
  }
}
 
void CyclicController::ProcessInputFrame(const Bytes& data)
{
  // std::cout << "RX raw frame: "
  //           << data.size()
  //           << " bytes\n";
 
  if (data.size() < 18)
  {
    // std::cout << "  DROP: frame too short\n";
    return;
  }
  // Parse Ethernet header; devices may send 802.1Q priority-tagged frames
  MacAddress srcMacData{};
  std::copy(data.begin() + 6, data.begin() + 12, srcMacData.begin());
  size_t ethOffset = SkipVlanTags(data);
  if (ethOffset + 2 > data.size())
  {
    // std::cout << "  DROP: invalid Ethernet offset\n";
    return;
  }
  const auto ethertype = static_cast<std::uint16_t>((data[ethOffset] << OneOctetShift) | data[ethOffset + 1]);
  const bool fromDevice = srcMacData == dstMac;
 
  // std::cout
  //     << "Ethernet frame: "
  //     << data.size()
  //     << " bytes"
  //     << " src="
  //     << profinet::Mac2String(srcMacData)
  //     << " dst="
  //     << profinet::Mac2String(dstMac)
  //     << (fromDevice ? " [AUMA]" : " [local]")
  //     << '\n';
 
  if (ethertype != rt::ETHERTYPE_PROFINET)
  {
    // std::cout << "  DROP: not PROFINET RT\n";
    return;
  }
 
  if (srcMacData != dstMac)
  {
    // std::cout << "  DROP: unexpected source MAC, expected "
    //           << Mac2String(dstMac)
    //           << '\n';
    return;
  }
 
  std::optional<rt::RTFrame> frameOpt;
  try
  {
    // frameOpt = rt::RTFrame::FromBytes(Bytes(data.begin() + ethOffset + 3, data.end())); // ethOffset + 3 kan ook 14 of 13 zijn..
    frameOpt = rt::RTFrame::FromBytes(Bytes(data.begin() + ethOffset + 2, data.end())); // ethOffset + 3 kan ook 14 of 13 zijn..
  }
  catch (const std::exception&)
  {
    return;
  }
  rt::RTFrame& frame = *frameOpt;
  // std::cout
  //     << "  RT FrameID=0x"
  //     << std::uppercase
  //     << std::hex
  //     << frame.frameId
  //     << " expected=0x"
  //     << inputIocr.frameId
  //     << " cycleCounter=0x"
  //     << frame.cycleCounter
  //     << std::dec
  //     << '\n';
  if (frame.frameId != inputIocr.frameId)
  {
    // std::cout << "  DROP: FrameID mismatch\n";
    return;
  }
 
  stats.lastReceiveTime = std::chrono::steady_clock::now();
  stats.framesReceived.fetch_add(1, std::memory_order_relaxed);
  stats.consecutiveTimeouts = 0;
 
  if (state == CyclicState::Fault)
  {
    Transition(CyclicState::Running);
  }
 
  TrackCycleCounter(frame.cycleCounter);
  // Check validity. TransferStatus != 0 means the provider flagged a
  // transfer problem; conformant consumers discard such frames.
  if (!frame.IsValid() || frame.transferStatus != 0)
  {
    stats.framesInvalid.fetch_add(1, std::memory_order_relaxed);
    return;
  }
 
  // Set IOCS to GOOD - we received valid input data. Only rewrite the
  // buffer on a BAD->GOOD transition; set_all_iocs dirties the whole
  // send buffer and this runs for every received frame.
  if (!iocsGood)
  {
    outputBuilder.SetAllIocs(rt::IOXS_GOOD);
    iocsGood = true;
  }
 
  std::lock_guard<std::mutex> lock(inputLock);
  // Extract data per IO object. Each submodule's payload is followed by
  // its IOPS byte; the device sets it BAD to disown the data it is still
  // sending, so the payload is only usable while IOPS reports GOOD.
  std::map<std::pair<std::uint16_t, std::uint16_t>, std::uint8_t> statusEvents;
  std::map<std::pair<std::uint16_t, std::uint16_t>, Bytes> dataEvents;
 
  for (const auto& obj : inputIocr.objects)
  {
    if (static_cast<std::size_t>(obj.offsetIOPS) > frame.payload.size())
    {
      continue;
    }
    if (static_cast<std::size_t>(obj.frameOffset + obj.dataLength) > frame.payload.size())
    {
      continue;
    }
 
    std::uint8_t iops = frame.payload[obj.offsetIOPS];
    auto key = std::make_pair(obj.slot, obj.subslot);
    bool wasGood = LocalIsInputGood(obj.slot, obj.subslot);
    bool isGood = (iops & rt::IOXS_DATA_STATE_GOOD) != 0;
    inputStatus[key] = iops;
    const Bytes objData(frame.payload.begin() + obj.frameOffset,
                        frame.payload.begin() + obj.frameOffset + obj.dataLength);
    // inputData[{obj.slot, obj.subslot}] = objData;
    inputData[key] = objData;
 
    if (isGood != wasGood)
    {
      statusEvents[key] = iops;
    }
    if (isGood)
    {
      dataEvents[key] = objData;
    }
  }
 
  // Callbacks run outside the lock: they are application code and must not
  // be able to stall the RX thread's next frame while holding it.
  for (const auto& [key, iops] : statusEvents)
    if (auto [slot, subslot] = key; true)
    {
      if (!(iops & rt::IOXS_DATA_STATE_GOOD))
      {
        /*
        logger.warning(
                    f"Slot {slot}/{subslot}: device reports IOPS BAD (0x{iops:02X}), "
                    f"input data is no longer valid"
                )
        */
      }
      if (onInputStatus)
      {
        try
        {
          onInputStatus(slot, subslot, iops);
        }
        // NOLINTNEXTLINE(bugprone-empty-catch)
        catch (...)
        {
          // continue to next -> logger.error(f"Input status callback error: {e}")
        }
      }
    }
  if (onInputData)
  {
    for (const auto& [key, objData] : dataEvents)
      if (auto [slot, subslot] = key; true)
      {
        if (!objData.empty())
        {
          try
          {
            onInputData(slot, subslot, objData);
          }
          // NOLINTNEXTLINE(bugprone-empty-catch)
          catch (...)
          {
            // continue to next //-->logger.error(f "Input callback error: {e}")
          }
        }
      }
  }
}
 
void CyclicController::TrackCycleCounter(std::uint16_t rxCounter)
{
  if (!lastRxCycleCounter)
  {
    lastRxCycleCounter = rxCounter;
    return;
  }
 
  const std::uint32_t step = rxCounterStep;
  const auto expected = static_cast<std::uint16_t>((*lastRxCycleCounter + step) & 0xFFFF);
 
  if (rxCounter == *lastRxCycleCounter)
  {
    stats.framesDuplicate.fetch_add(1, std::memory_order_relaxed);
  }
  else if (rxCounter != expected)
  {
    const auto forward = static_cast<std::uint16_t>((rxCounter - *lastRxCycleCounter) & 0xFFFF);
    if (forward > 0x8000)
    {
      stats.framesOutOfOrder.fetch_add(1, std::memory_order_relaxed);
    }
    else if (step > 0)
    {
      const std::uint32_t gap = (forward / step) - 1;
      if (gap > 0)
      {
        stats.framesMissed.fetch_add(gap, std::memory_order_relaxed);
      }
    }
    lastRxCycleCounter = rxCounter;
  }
  else
  {
    lastRxCycleCounter = rxCounter;
  }
}
 
} // namespace profinet::cyclic