virtual_profinet_rpc_device.cpp

Go to the documentation of this file.

 
 
#include "profinet/sim/virtual_profinet_rpc_device.h"
 
#include <algorithm>
#include <chrono>
#include <stdexcept>
#include <utility>
 
#include "profinet/wire.h"
 
namespace profinet::sim
{
 
namespace
{
 
using namespace std::chrono_literals;
 
constexpr std::uint16_t kBlockHeaderSize = 4U;
constexpr std::uint16_t kControlBlockSize = 32U;
 
bool IsControlPrmBegin(const std::uint16_t type)
{
  return type == static_cast<std::uint16_t>(BlockType::ParameterizationBeginRequest);
}
 
bool IsControlPrmEnd(const std::uint16_t type)
{
  return type == static_cast<std::uint16_t>(BlockType::ControlPrmEndRequest);
}
 
bool IsControlRelease(const std::uint16_t type)
{
  return type == static_cast<std::uint16_t>(BlockType::ReleaseRequest);
}
 
} // namespace
 
const char* ToString(const VirtualProfinetDeviceState state) noexcept
{
  switch (state)
  {
    case VirtualProfinetDeviceState::Idle:
      return "Idle";
    case VirtualProfinetDeviceState::Connected:
      return "Connected";
    case VirtualProfinetDeviceState::Parameterizing:
      return "Parameterizing";
    case VirtualProfinetDeviceState::Parameterized:
      return "Parameterized";
    case VirtualProfinetDeviceState::ApplicationReady:
      return "ApplicationReady";
    case VirtualProfinetDeviceState::Released:
      return "Released";
    case VirtualProfinetDeviceState::Fault:
      return "Fault";
  }
  return "Unknown";
}
 
VirtualProfinetRpcDevice::VirtualProfinetRpcDevice(
    VirtualProfinetDeviceConfiguration configRef)
    : config(std::move(configRef))
{
}
 
VirtualProfinetRpcDevice::~VirtualProfinetRpcDevice()
{
  Stop();
}
 
void VirtualProfinetRpcDevice::Start()
{
  if (running.exchange(true))
  {
    return;
  }
 
  ioContext = std::make_unique<asio::io_context>();
  socket = std::make_unique<asio::ip::udp::socket>(*ioContext);
 
  asio::error_code error;
  socket->open(asio::ip::udp::v4(), error);
  if (error)
  {
    running = false;
    throw std::system_error(error, "VirtualProfinetRpcDevice::open");
  }
 
  socket->set_option(asio::socket_base::reuse_address(true), error);
  if (error)
  {
    running = false;
    throw std::system_error(error, "VirtualProfinetRpcDevice::set_option");
  }
 
  socket->bind(
      asio::ip::udp::endpoint(asio::ip::udp::v4(), config.rpcPort),
      error);
  if (error)
  {
    running = false;
    throw std::system_error(error, "VirtualProfinetRpcDevice::bind");
  }
 
  worker = std::thread([this]()
  {
    Run();
  });
}
void VirtualProfinetRpcDevice::Stop() noexcept
{
  if (!running.exchange(false))
  {
    return;
  }
 
  // Wake both state waiters and the delayed ApplicationReady worker.
  stateCondition.notify_all();
 
  // The worker is blocked in synchronous receive_from(). Do not close the
  // socket yet. First inject a local datagram so receive_from() returns.
  WakeWorker();
 
  if (worker.joinable())
  {
    worker.join();
  }
 
  // The delayed ApplicationReady operation must also be joined before the
  // socket and io_context are destroyed.
  std::thread applicationReadyThread;
  {
    std::scoped_lock lock(mutex);
    applicationReadyThread = std::move(applicationReadyWorker);
  }
 
  if (applicationReadyThread.joinable())
  {
    applicationReadyThread.join();
  }
 
  asio::error_code error;
  if (socket != nullptr)
  {
    socket->cancel(error);
    socket->close(error);
  }
 
  socket.reset();
  ioContext.reset();
}
 
void VirtualProfinetRpcDevice::WakeWorker() noexcept
{
  if (socket == nullptr)
  {
    return;
  }
 
  try
  {
    asio::io_context wakeContext;
    asio::ip::udp::socket wakeSocket(wakeContext);
 
    asio::error_code error;
 
    wakeSocket.open(asio::ip::udp::v4(), error);
    if (error)
    {
      return;
    }
 
    constexpr std::uint8_t wakeByte = 0;
 
    const asio::ip::udp::endpoint endpoint(
        asio::ip::address_v4::loopback(),
        config.rpcPort);
 
    wakeSocket.send_to(
        asio::buffer(&wakeByte, sizeof(wakeByte)),
        endpoint,
        0,
        error);
  }
  catch (...)
  {
    // Stop() is noexcept. If the wake-up cannot be sent, the caller will
    // still proceed with shutdown.
  }
}
 
VirtualProfinetDeviceState VirtualProfinetRpcDevice::State() const noexcept
{
  std::scoped_lock lock(mutex);
  return state;
}
 
std::size_t VirtualProfinetRpcDevice::ConnectCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return connectCount;
}
 
std::size_t VirtualProfinetRpcDevice::PrmBeginCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return prmBeginCount;
}
 
std::size_t VirtualProfinetRpcDevice::PrmEndCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return prmEndCount;
}
 
std::size_t VirtualProfinetRpcDevice::ApplicationReadyRequestCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return applicationReadyRequestCount;
}
 
std::size_t VirtualProfinetRpcDevice::ApplicationReadyResponseCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return applicationReadyResponseCount;
}
 
std::size_t VirtualProfinetRpcDevice::ReleaseCount() const noexcept
{
  std::scoped_lock lock(mutex);
  return releaseCount;
}
 
bool VirtualProfinetRpcDevice::SawInputIocr() const noexcept
{
  std::scoped_lock lock(mutex);
  return sawInputIocr;
}
 
bool VirtualProfinetRpcDevice::SawOutputIocr() const noexcept
{
  std::scoped_lock lock(mutex);
  return sawOutputIocr;
}
 
bool VirtualProfinetRpcDevice::SawExpectedSubmodules() const noexcept
{
  std::scoped_lock lock(mutex);
  return sawExpectedSubmodules;
}
 
bool VirtualProfinetRpcDevice::SawAlarmCr() const noexcept
{
  std::scoped_lock lock(mutex);
  return sawAlarmCr;
}
 
std::string VirtualProfinetRpcDevice::LastError() const
{
  std::scoped_lock lock(mutex);
  return lastError;
}
 
bool VirtualProfinetRpcDevice::WaitForState(
    const VirtualProfinetDeviceState expected,
    const std::chrono::milliseconds timeout) const
{
  std::unique_lock lock(mutex);
  return stateCondition.wait_for(
      lock,
      timeout,
      [this, expected]()
  {
    return state == expected;
  });
}
 
std::uint16_t VirtualProfinetRpcDevice::RpcPort() const noexcept
{
  return config.rpcPort;
}
 
void VirtualProfinetRpcDevice::Run()
{
  while (running.load())
  {
    Bytes buffer(65535U);
    asio::ip::udp::endpoint sender;
    asio::error_code error;
 
    const std::size_t received = socket->receive_from(
        asio::buffer(buffer),
        sender,
        0,
        error);
 
    if (!running.load())
    {
      break;
    }
 
    if (error)
    {
      continue;
    }
 
    buffer.resize(received);
 
    try
    {
      HandlePacket(PNRPCHeader::Parse(buffer), sender);
    }
    catch (const std::exception& exception)
    {
      std::scoped_lock lock(mutex);
      state = VirtualProfinetDeviceState::Fault;
      lastError = exception.what();
      stateCondition.notify_all();
    }
  }
}
 
void VirtualProfinetRpcDevice::HandlePacket(
    const PNRPCHeader& request,
    const asio::ip::udp::endpoint& sender)
{
  if (request.packetType == PNRPCHeader::RESPONSE &&
      request.operationNumber == PNRPCHeader::CONTROL)
  {
    HandleApplicationReadyResponse(request);
    return;
  }
 
  if (request.packetType != PNRPCHeader::REQUEST)
  {
    return;
  }
 
  switch (request.operationNumber)
  {
    case PNRPCHeader::CONNECT:
      HandleConnect(request, sender);
      break;
 
    case PNRPCHeader::RELEASE:
    case PNRPCHeader::CONTROL:
      HandleControl(request, sender);
      break;
    default:
      // A lifecycle simulator deliberately does not implement acyclic record
      // access yet. The cyclic/record simulator can be layered on top later.
      break;
  }
}
 
void VirtualProfinetRpcDevice::ScheduleApplicationReady()
{
  std::scoped_lock lock(mutex);
 
  if (!running.load())
  {
    return;
  }
 
  if (applicationReadyWorker.joinable())
  {
    return;
  }
 
  applicationReadyWorker = std::thread([this]()
  {
    std::unique_lock lock(mutex);
 
    const bool stopped = stateCondition.wait_for(
        lock,
        config.applicationReadyDelay,
        [this]()
    {
      return !running.load();
    });
 
    if (stopped)
    {
      return;
    }
 
    lock.unlock();
 
    SendApplicationReady();
  });
}
 
void VirtualProfinetRpcDevice::HandleConnect(
    const PNRPCHeader& request,
    const asio::ip::udp::endpoint& sender)
{
  const PNNRDData nrd = PNNRDData::Parse(request.payload);
 
  if (!ValidateConnect(nrd.payload))
  {
    return;
  }
 
  ParseArIdentity(nrd.payload, arUuid, sessionKey);
 
  {
    std::scoped_lock lock(mutex);
    controllerEndpoint = sender;
    ++connectCount;
    state = VirtualProfinetDeviceState::Connected;
    lastError.clear();
  }
  stateCondition.notify_all();
 
  Bytes responsePayload;
 
  // Echo a valid IOCR configuration into IOCRBlockRes blocks. RPCCon only
  // needs the response frame IDs at this stage, so the response intentionally
  // contains no application-specific data.
  std::size_t offset = 0;
  while (offset + kBlockHeaderSize <= nrd.payload.size())
  {
    const auto header = rpc::PeekBlockHeader(nrd.payload, offset);
    const std::size_t blockSize =
        kBlockHeaderSize + static_cast<std::size_t>(header.blockLength);
 
    if (blockSize > nrd.payload.size() - offset)
    {
      break;
    }
 
    if (header.blockType == PNIOCRBlockReqHeader::BLOCK_TYPE)
    {
      const Bytes block(
          nrd.payload.begin() + static_cast<std::ptrdiff_t>(offset),
          nrd.payload.begin() + static_cast<std::ptrdiff_t>(offset + blockSize));
      const PNIOCRBlockReqHeader iocr = PNIOCRBlockReqHeader::Parse(block);
      const std::uint16_t frameId =
          iocr.iocrType == 1U ? config.inputFrameId : config.outputFrameId;
      const Bytes response = MakeIocrResponse(iocr, frameId);
      responsePayload.insert(responsePayload.end(), response.begin(), response.end());
    }
 
    offset += blockSize;
  }
 
  if (sawAlarmCr)
  {
    const Bytes alarm = MakeAlarmCrResponse(config.deviceAlarmReference);
    responsePayload.insert(responsePayload.end(), alarm.begin(), alarm.end());
  }
 
  SendResponse(request, MakeNrd(responsePayload), sender);
}
 
void VirtualProfinetRpcDevice::HandleControl(
    const PNRPCHeader& request,
    const asio::ip::udp::endpoint& sender)
{
  const PNNRDData nrd = PNNRDData::Parse(request.payload);
  if (nrd.payload.size() < kControlBlockSize)
  {
    return;
  }
 
  wire::Reader reader(nrd.payload);
  const std::uint16_t blockType = reader.U16();
  reader.SkipTo(PNIODHeader::kContolCommandOffset);
  const std::uint16_t command = reader.U16();
 
  if (IsControlPrmBegin(blockType) && command == rpc::CONTROL_CMD_PRM_BEGIN)
  {
    std::scoped_lock lock(mutex);
    if (state != VirtualProfinetDeviceState::Connected)
    {
      state = VirtualProfinetDeviceState::Fault;
      lastError = "PrmBegin received outside Connected state";
      return;
    }
    ++prmBeginCount;
    state = VirtualProfinetDeviceState::Parameterizing;
    stateCondition.notify_all();
    SendResponse(request, MakeNrd({}), sender);
    return;
  }
 
  if (IsControlPrmEnd(blockType) && command == rpc::CONTROL_CMD_PRM_END)
  {
    {
      std::scoped_lock lock(mutex);
      if (state != VirtualProfinetDeviceState::Parameterizing)
      {
        state = VirtualProfinetDeviceState::Fault;
        lastError = "PrmEnd received before PrmBegin";
        stateCondition.notify_all();
        return;
      }
      ++prmEndCount;
      state = VirtualProfinetDeviceState::Parameterized;
    }
    stateCondition.notify_all();
    SendResponse(request, MakeNrd({}), sender);
 
    if (config.automaticApplicationReady)
    {
      ScheduleApplicationReady();
 
      // std::thread([this]()
      // {
      //   std::this_thread::sleep_for(config.applicationReadyDelay);
      //   SendApplicationReady();
      // }).detach();
    }
    return;
  }
 
  if (IsControlRelease(blockType))
  {
    {
      std::scoped_lock lock(mutex);
      ++releaseCount;
      state = VirtualProfinetDeviceState::Released;
    }
    stateCondition.notify_all();
    SendResponse(request, MakeNrd({}), sender);
  }
}
 
void VirtualProfinetRpcDevice::HandleApplicationReadyResponse(
    const PNRPCHeader& response)
{
  std::scoped_lock lock(mutex);
 
  if (state != VirtualProfinetDeviceState::Parameterized)
  {
    state = VirtualProfinetDeviceState::Fault;
    lastError = "ApplicationReady response received outside Parameterized state";
    stateCondition.notify_all();
    return;
  }
 
  if (!lastApplicationReadyRequest.has_value())
  {
    state = VirtualProfinetDeviceState::Fault;
    lastError = "ApplicationReady response received without a request";
    stateCondition.notify_all();
    return;
  }
 
  if (response.activityUuid != lastApplicationReadyRequest->activityUuid ||
      response.objectUuid != lastApplicationReadyRequest->objectUuid ||
      response.interfaceUuid != lastApplicationReadyRequest->interfaceUuid ||
      response.sequenceNumber != lastApplicationReadyRequest->sequenceNumber)
  {
    state = VirtualProfinetDeviceState::Fault;
    lastError = "ApplicationReady response did not preserve RPC correlation";
    stateCondition.notify_all();
    return;
  }
 
  ++applicationReadyResponseCount;
  state = VirtualProfinetDeviceState::ApplicationReady;
  stateCondition.notify_all();
}
 
bool VirtualProfinetRpcDevice::ValidateConnect(const Bytes& nrdPayload)
{
  const bool hasAr = ContainsBlock(nrdPayload, BlockType::ApplicationRelationRequest);
  if (!hasAr)
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "CONNECT missing ARBlockReq";
    stateCondition.notify_all();
    return false;
  }
 
  bool input = false;
  bool output = false;
 
  std::size_t offset = 0;
  while (offset + kBlockHeaderSize <= nrdPayload.size())
  {
    const auto header = rpc::PeekBlockHeader(nrdPayload, offset);
    const std::size_t blockSize =
        kBlockHeaderSize + static_cast<std::size_t>(header.blockLength);
 
    if (blockSize > nrdPayload.size() - offset)
    {
      break;
    }
 
    if (header.blockType == PNIOCRBlockReqHeader::BLOCK_TYPE)
    {
      const Bytes block(
          nrdPayload.begin() + static_cast<std::ptrdiff_t>(offset),
          nrdPayload.begin() + static_cast<std::ptrdiff_t>(offset + blockSize));
      const PNIOCRBlockReqHeader iocr = PNIOCRBlockReqHeader::Parse(block);
      input = input || iocr.iocrType == 1U;
      output = output || iocr.iocrType == 2U;
    }
 
    offset += blockSize;
  }
 
  {
    std::scoped_lock lock(mutex);
    sawInputIocr = input;
    sawOutputIocr = output;
    sawExpectedSubmodules = ContainsBlock(
        nrdPayload,
        BlockType::ExpectedSubmoduleRequest);
    sawAlarmCr = ContainsBlock(
        nrdPayload,
        PNAlarmCRBlockReq::BLOCK_TYPE);
  }
 
  const bool cyclicConnect = input || output;
  if (cyclicConnect && config.requireBothIocrs && (!input || !output))
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "Cyclic CONNECT must contain both Input and Output IOCR";
    stateCondition.notify_all();
    return false;
  }
 
  if (cyclicConnect && config.requireExpectedSubmodules && !sawExpectedSubmodules)
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "Cyclic CONNECT missing ExpectedSubmoduleBlockReq";
    stateCondition.notify_all();
    return false;
  }
 
  if (cyclicConnect && config.requireAlarmCr && !sawAlarmCr)
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "Cyclic CONNECT missing AlarmCRBlockReq";
    stateCondition.notify_all();
    return false;
  }
 
  return true;
}
 
void VirtualProfinetRpcDevice::SendResponse(
    const PNRPCHeader& request,
    const Bytes& nrdPayload,
    const asio::ip::udp::endpoint& sender)
{
  const PNRPCHeader response = MakeResponse(request, nrdPayload);
  const Bytes bytes = response.ToBytes();
 
  asio::error_code error;
  socket->send_to(asio::buffer(bytes), sender, 0, error);
  if (error)
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "Failed to send RPC response: " + error.message();
    stateCondition.notify_all();
  }
}
 
void VirtualProfinetRpcDevice::SendApplicationReady()
{
  asio::ip::udp::endpoint controller;
 
  {
    std::scoped_lock lock(mutex);
    if (state != VirtualProfinetDeviceState::Parameterized ||
        !controllerEndpoint.has_value())
    {
      return;
    }
    controller = {
        asio::ip::make_address(config.controllerAddress),
        config.controllerCcontrolPort};
  }
 
  PNBlockHeader block;
  block.blockType = BlockType::ControlApplicationReadyRequest;
  block.blockLength = 28U;
  block.blockVersionHigh = 1U;
  block.blockVersionLow = 0U;
 
  PNIODReleaseBlock control;
  std::ranges::copy(
      block.ToBytes(),
      control.blockHeader.begin());
  control.padding1 = 0;
  control.arUuid = arUuid;
  control.sessionKey = sessionKey;
  control.padding2 = 0;
  control.controlCommand = ControlCommand::ApplicationReady;
  control.controlBlockProperties = 0;
 
  const Bytes nrd = MakeNrd(control.ToBytes());
 
  PNRPCHeader request;
  request.version = 4;
  request.packetType = PNRPCHeader::REQUEST;
  request.flags1 = static_cast<std::uint8_t>(
      rpc::raw::Flags1::Idempotent | rpc::raw::Flags1::LastFragment);
  request.flags2 = 0;
  request.dataRepresentation = {0, 0, 0};
  request.serialNrHigh = 0;
  request.objectUuid = PNRPCHeader::IfaceUuidDevice();
  request.interfaceUuid = PNRPCHeader::IfaceUuidDevice();
  request.activityUuid = arUuid;
  request.serverBootTime = 0;
  request.interfaceVersion = 1;
  request.sequenceNumber = 1;
  request.operationNumber = PNRPCHeader::CONTROL;
  request.interfaceHint = PNRPCHeader::defaultInterfaceHint;
  request.activityHint = PNRPCHeader::defaultActivityHint;
  request.lengthOfBody = static_cast<std::uint16_t>(nrd.size());
  request.fragmentNumber = 0;
  request.authenticationProtocol = 0;
  request.serialNrLow = 0;
  request.payload = nrd;
 
  {
    std::scoped_lock lock(mutex);
    lastApplicationReadyRequest = request;
    ++applicationReadyRequestCount;
  }
 
  const Bytes bytes = request.ToBytes();
  asio::error_code error;
  socket->send_to(asio::buffer(bytes), controller, 0, error);
 
  if (error)
  {
    std::scoped_lock lock(mutex);
    state = VirtualProfinetDeviceState::Fault;
    lastError = "Failed to send ApplicationReady: " + error.message();
    stateCondition.notify_all();
  }
}
 
Bytes VirtualProfinetRpcDevice::MakeNrd(const Bytes& payload)
{
  PNNRDData nrd;
  nrd.argsMaximumStatus = 0;
  nrd.argsLength = static_cast<std::uint32_t>(payload.size());
  nrd.maximumCount = static_cast<std::uint32_t>(payload.size());
  nrd.offset = 0;
  nrd.actualCount = static_cast<std::uint32_t>(payload.size());
  nrd.payload = payload;
  return nrd.ToBytes();
}
 
PNRPCHeader VirtualProfinetRpcDevice::MakeResponse(
    const PNRPCHeader& request,
    const Bytes& nrdPayload)
{
  PNRPCHeader response = request;
  response.packetType = PNRPCHeader::RESPONSE;
  response.flags1 = static_cast<std::uint8_t>(
      rpc::raw::Flags1::NoFack | rpc::raw::Flags1::LastFragment);
  response.flags2 = 0;
  response.lengthOfBody =
      static_cast<std::uint16_t>(nrdPayload.size());
  response.payload = nrdPayload;
  return response;
}
 
Bytes VirtualProfinetRpcDevice::MakeIocrResponse(
    const PNIOCRBlockReqHeader& request,
    const std::uint16_t frameId)
{
  Bytes bytes;
  wire::PutU16(bytes, PNIOCRBlockRes::BLOCK_TYPE);
  wire::PutU16(bytes, static_cast<std::uint16_t>(PNIOCRBlockRes::kSize - 4U));
  wire::PutU8(bytes, rpc::VERSION_HIGH);
  wire::PutU8(bytes, rpc::VERSION_LOW);
  wire::PutU16(bytes, request.iocrType);
  wire::PutU16(bytes, request.iocrReference);
  wire::PutU16(bytes, frameId);
  return bytes;
}
 
Bytes VirtualProfinetRpcDevice::MakeAlarmCrResponse(
    const std::uint16_t deviceAlarmReference)
{
  PNAlarmCRBlockRes response;
  PNBlockHeader header;
  header.blockType = BlockType::AlarmCrResponse;
  header.blockLength =
      static_cast<std::uint16_t>(PNAlarmCRBlockRes::kSize - 4U);
  header.blockVersionHigh = rpc::VERSION_HIGH;
  header.blockVersionLow = rpc::VERSION_LOW;
  std::ranges::copy(
      header.ToBytes(),
      response.blockHeader.begin());
  response.localAlarmReference = deviceAlarmReference;
  return response.ToBytes();
}
 
bool VirtualProfinetRpcDevice::ContainsBlock(
    const Bytes& data,
    const BlockType blockType)
{
  std::size_t offset = 0;
  while (offset + kBlockHeaderSize <= data.size())
  {
    const auto header = rpc::PeekBlockHeader(data, offset);
    const std::size_t blockSize =
        kBlockHeaderSize + static_cast<std::size_t>(header.blockLength);
 
    if (blockSize > data.size() - offset)
    {
      return false;
    }
 
    if (header.blockType == blockType)
    {
      return true;
    }
 
    offset += blockSize;
  }
  return false;
}
 
void VirtualProfinetRpcDevice::ParseArIdentity(
    const Bytes& connectPayload,
    std::array<std::uint8_t, uuidLenght>& arUuidOut,
    std::uint16_t& sessionKeyOut)
{
  if (connectPayload.size() < 8U + uuidLenght + sizeof(std::uint16_t))
  {
    throw std::invalid_argument("CONNECT ARBlockReq is truncated");
  }
 
  wire::Reader reader(connectPayload);
  reader.SkipTo(8U);
  arUuidOut = reader.Fixed<uuidLenght>();
  sessionKeyOut = reader.U16();
}
 
} // namespace profinet::sim