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");
  }
  wire::Reader r(data);
 
  RTFrame f;
  f.frameId = r.U16();
 
  // trailing bytes are cyclecounter (2), a dataStatus (1) and transferStatus (1), in between is the payload
  constexpr std::size_t trailerLength = sizeof(std::uint16_t) + (2 * sizeof(std::uint8_t));
  const std::size_t payloadLength = r.Remaining() - trailerLength;
  f.payload = r.ReadBytes(payloadLength);
 
  f.cycleCounter = r.U16();
  f.dataStatus = r.U8();
  f.transferStatus = r.U8();
  return f;
}
 
Bytes RTFrame::ToBytes() const
{
  Bytes out;
  wire::PutBigEndian(out, frameId);
  out.insert(out.end(), payload.begin(), payload.end());
  wire::PutBigEndian(out, cycleCounter);
  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());
}
 
void IODataObject::Validate(std::size_t frameSize) const
{
  const auto offset = static_cast<std::size_t>(frameOffset);
  const auto length = static_cast<std::size_t>(dataLength);
  if (offset > frameSize || length > frameSize - offset)
  {
    throw std::invalid_argument("IODataObject data range exceeds frame");
  }
 
  if (offsetIOPS.has_value() && *offsetIOPS >= frameSize)
  {
    throw std::invalid_argument("IOPS offset exceeds frame");
  }
 
  if (offsetIOCS.has_value() && *offsetIOCS >= frameSize)
  {
    throw std::invalid_argument("IOCS offset exceeds frame");
  }
}
 
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)
{
  for (const auto& object : this->config.objects)
  {
    object.Validate(this->config.dataLength);
  }
}
 
void CyclicDataBuilder::SetData(int slot, int subslot, const Bytes& data)
{
  const std::lock_guard<std::mutex> lock(writeLock);
 
  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));
      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
{
  const std::lock_guard<std::mutex> lock(writeLock);
 
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      // 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)
{
  const std::lock_guard<std::mutex> lock(writeLock);
 
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      if (obj.dataLength > 0 && obj.offsetIOPS.has_value())
      {
        writeBuffer.at(static_cast<std::size_t>(*obj.offsetIOPS)) = status;
        dirty = true;
      }
      return;
    }
  }
}
 
void CyclicDataBuilder::SetIocs(int slot, int subslot, std::uint8_t status)
{
  const std::lock_guard<std::mutex> lock(writeLock);
 
  for (const auto& obj : config.objects)
  {
    if (obj.slot == slot && obj.subslot == subslot)
    {
      if (obj.offsetIOCS.has_value())
      {
        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 && obj.offsetIOPS.has_value())
    {
      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.has_value())
    {
      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()
{
  const std::lock_guard<std::mutex> lock(writeLock);
  if (dirty)
  {
    sendBuffer = writeBuffer;
    dirty = false;
  }
}
 
Bytes CyclicDataBuilder::Build() const
{
  const std::lock_guard<std::mutex> lock(writeLock);
  return sendBuffer;
}
 
void CyclicDataBuilder::Load(const Bytes& payload)
{
  const std::lock_guard<std::mutex> lock(writeLock);
  const std::size_t copyLen = std::min(payload.size(), writeBuffer.size());
  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));
  wire::PutBigEndian(out, ETHERTYPE_PROFINET);
 
  Bytes frameBytes = rtFrame.ToBytes();
  out.insert(out.end(), frameBytes.begin(), frameBytes.end());
  return out;
}
 
std::optional<RTFrame> ParseEthernetFrame(const Bytes& data)
{
  constexpr std::size_t ethernetHeaderSize = 14U;
  constexpr std::size_t ethernetTypeOffset = 12U;
  constexpr std::size_t vlanTagSize = 4U;
  constexpr std::size_t minimumRtFrameSize = 6U;
 
  if (data.size() < ethernetHeaderSize + minimumRtFrameSize)
  {
    return std::nullopt; // 14 (eth) + 4 (min RT)
  }
  std::size_t rtOffset = ethernetHeaderSize;
 
  wire::Reader r(data);
  r.SkipTo(ethernetTypeOffset);
  auto ethertype = r.U16();
 
  if (ethertype == ETHERTYPE_VLAN)
  {
    if (data.size() < ethernetHeaderSize + vlanTagSize + minimumRtFrameSize)
    {
      return std::nullopt;
    }
 
    rtOffset += vlanTagSize;
    r.Skip(sizeof(std::uint16_t));
 
    ethertype = r.U16();
  }
 
  if (ethertype != ETHERTYPE_PROFINET)
  {
    return std::nullopt;
  }
 
  try
  {
    return RTFrame::FromBytes(Bytes(data.begin() + rtOffset, data.end()));
  }
  catch (const std::exception&)
  {
    return std::nullopt;
  }
}
 
} // namespace profinet::rt