rt.cpp

Go to the documentation of this file.

 
 
#include "profinet/rt.h"
 
#include <algorithm>
#include <cstdio>
#include <ranges>
#include <stdexcept>
 
namespace profinet::rt
{
 
RTFrame RTFrame::FromBytes(const Bytes& data)
{
  static constexpr int minimumRtFrameSize = 6;
  if (data.size() < minimumRtFrameSize)
  {
    throw std::runtime_error("RT frame too short: " + std::to_string(data.size()) + " bytes");
  }
 
  RTFrame f;
  f.frameId = static_cast<std::uint16_t>((data[0] << OneOctetShift) | data[1]);
  const std::size_t n = data.size();
  f.cycleCounter = static_cast<std::uint16_t>((data[n - 4] << OneOctetShift) | data[n - 3]);
  f.dataStatus = data[n - 2];
  f.transferStatus = data[n - 1];
  f.payload = Bytes(data.begin() + 2, data.end() - 4);
  return f;
}
 
Bytes RTFrame::ToBytes() const
{
  Bytes out;
  out.push_back(static_cast<std::uint8_t>(frameId >> OneOctetShift));
  out.push_back(static_cast<std::uint8_t>(frameId & LowByteMask));
  out.insert(out.end(), payload.begin(), payload.end());
  out.push_back(static_cast<std::uint8_t>(cycleCounter >> OneOctetShift));
  out.push_back(static_cast<std::uint8_t>(cycleCounter & LowByteMask));
  out.push_back(dataStatus);
  out.push_back(transferStatus);
  return out;
}
 
std::string RTFrame::ToString() const
{
  std::vector<std::string> flags;
  if (IsValid())
  {
    flags.emplace_back("VALID");
  }
  if (IsRunning())
  {
    flags.emplace_back("RUN");
  }
  if (IsOk())
  {
    flags.emplace_back("OK");
  }
  if (IsPrimary())
  {
    flags.emplace_back("PRIMARY");
  }
  std::string flagsStr;
  for (std::size_t i = 0; i < flags.size(); ++i)
  {
    flagsStr += flags[i];
    if (i + 1 < flags.size())
    {
      flagsStr += "|";
    }
  }
  if (flagsStr.empty())
  {
    flagsStr = "NONE";
  }
 
  return std::format("RTFrame(id=0x{:04X}, cycle={}, status={}, payload={}B)",
                     frameId, cycleCounter, flagsStr, payload.size());
}
 
CyclicDataBuilder::CyclicDataBuilder(IOCRConfig config)
    : config(std::move(config)),
      writeBuffer(static_cast<std::size_t>(this->config.dataLength), 0),
      sendBuffer(static_cast<std::size_t>(this->config.dataLength), 0)
{
}
 
void CyclicDataBuilder::SetData(int slot, int subslot, const Bytes& data)
{
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      const std::size_t copyLen = std::min<std::size_t>(data.size(), static_cast<std::size_t>(obj.dataLength));
      const std::lock_guard<std::mutex> lock(writeLock);
      std::ranges::copy(
          data | std::views::take(copyLen),
          writeBuffer.begin() + obj.frameOffset);
      dirty = true;
      return;
    }
  }
  throw std::invalid_argument("Unknown slot/subslot: " + std::to_string(slot) + "/" + std::to_string(subslot));
}
 
Bytes CyclicDataBuilder::GetData(int slot, int subslot) const
{
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      const std::lock_guard<std::mutex> lock(writeLock);
      // NOLINTNEXTLINE(modernize-return-braced-init-list)
      return Bytes(writeBuffer.begin() + obj.frameOffset,
                   writeBuffer.begin() + obj.frameOffset + obj.dataLength);
    }
  }
  throw std::invalid_argument("Unknown slot/subslot: " + std::to_string(slot) + "/" + std::to_string(subslot));
}
 
void CyclicDataBuilder::SetIops(int slot, int subslot, std::uint8_t status)
{
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      if (obj.dataLength > 0)
      {
        const std::lock_guard<std::mutex> lock(writeLock);
        writeBuffer.at(static_cast<std::size_t>(obj.offsetIOPS)) = status;
        dirty = true;
      }
      return;
    }
  }
}
 
void CyclicDataBuilder::SetIocs(int slot, int subslot, std::uint8_t status)
{
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      if (obj.offsetIOCS > 0)
      {
        const std::lock_guard<std::mutex> lock(writeLock);
        writeBuffer.at(static_cast<std::size_t>(obj.offsetIOCS)) = status;
        dirty = true;
      }
      return;
    }
  }
}
 
void CyclicDataBuilder::SetAllIops(std::uint8_t status)
{
  const std::lock_guard<std::mutex> lock(writeLock);
  for (const auto& obj : config.objects)
  {
    if (obj.dataLength > 0)
    {
      writeBuffer.at(static_cast<std::size_t>(obj.offsetIOPS)) = status;
    }
  }
  dirty = true;
}
 
void CyclicDataBuilder::SetAllIocs(std::uint8_t status)
{
  const std::lock_guard<std::mutex> lock(writeLock);
  for (const auto& obj : config.objects)
  {
    if (obj.offsetIOCS > 0)
    {
      writeBuffer.at(static_cast<std::size_t>(obj.offsetIOCS)) = status;
    }
  }
  dirty = true;
}
 
void CyclicDataBuilder::Clear()
{
  const std::lock_guard<std::mutex> lock(writeLock);
  std::ranges::fill(writeBuffer, 0);
  dirty = true;
}
 
void CyclicDataBuilder::Swap()
{
  if (dirty)
  {
    const std::lock_guard<std::mutex> lock(writeLock);
    sendBuffer = writeBuffer;
    dirty = false;
  }
}
 
Bytes CyclicDataBuilder::Build() const
{
  return sendBuffer;
}
 
void CyclicDataBuilder::Load(const Bytes& payload)
{
  const std::size_t copyLen = std::min(payload.size(), writeBuffer.size());
  const std::lock_guard<std::mutex> lock(writeLock);
  std::ranges::copy(payload | std::views::take(copyLen), writeBuffer.begin());
  dirty = true;
}
 
Bytes BuildEthernetFrame(const MacAddress& dstMac, const MacAddress& srcMac, const RTFrame& rtFrame)
{
  Bytes out;
  out.insert(out.end(), dstMac.begin(), dstMac.end());
  out.insert(out.end(), srcMac.begin(), srcMac.end());
  // need to check for VLAN ?
  // out.push_back(static_cast<std::uint8_t>(VLAN_TAG_RT >> ThreeOctetsShift));
  // out.push_back(static_cast<std::uint8_t>(VLAN_TAG_RT >> TwoOctetsShift));
  // out.push_back(static_cast<std::uint8_t>(VLAN_TAG_RT >> OneOctetShift));
  // out.push_back(static_cast<std::uint8_t>(VLAN_TAG_RT & LowByteMask));
  out.push_back(static_cast<std::uint8_t>(ETHERTYPE_PROFINET >> OneOctetShift));
  out.push_back(static_cast<std::uint8_t>(ETHERTYPE_PROFINET & LowByteMask));
  Bytes frameBytes = rtFrame.ToBytes();
  out.insert(out.end(), frameBytes.begin(), frameBytes.end());
  return out;
}
 
std::optional<RTFrame> ParseEthernetFrame(const Bytes& data)
{
  if (data.size() < 18)
  {
    return std::nullopt; // 14 (eth) + 4 (min RT)
  }
 
  const auto ethertype = static_cast<std::uint16_t>((data[12] << OneOctetShift) | data[13]);
  if (ethertype != ETHERTYPE_PROFINET)
  {
    return std::nullopt;
  }
 
  try
  {
    return RTFrame::FromBytes(Bytes(data.begin() + 14, data.end()));
  }
  catch (const std::exception&)
  {
    return std::nullopt;
  }
}
 
} // namespace profinet::rt