virtual_phoenix_plc.cpp

Go to the documentation of this file.

 
 
#include "profinet/sim/virtual_phoenix_plc.h"
 
#include <algorithm>
#include <bit>
#include <cfloat>
#include <cmath>
#include <cstring>
#include <limits>
#include <sstream>
#include <stdexcept>
#include <utility>
 
#include "profinet/sim/virtual_auma_valve.h"
 
namespace profinet::sim
{
 
namespace
{
 
const io::IoSignal* FindSignal(const io::IoSignalCatalog& catalog,
                               const std::string_view textId,
                               const io::IoSignalDirection direction)
{
  const auto& signals = direction == io::IoSignalDirection::Sensor
                            ? catalog.Sensors()
                            : catalog.Actuators();
  const io::IoSignal* result = nullptr;
  for (const auto& signal : signals)
  {
    if (signal.textId != textId)
    {
      continue;
    }
    if (result != nullptr)
    {
      throw std::invalid_argument("Signal is not unique: " + std::string(textId));
    }
    result = &signal;
  }
  return result;
}
 
void ValidateRange(const io::IoSignal& signal, const Bytes& data)
{
  if (signal.byteOffset > data.size() || signal.byteLength > data.size() - signal.byteOffset)
  {
    throw std::out_of_range("Signal lies outside the process-data object: " + signal.textId);
  }
}
 
std::uint64_t ReadUnsigned(const Bytes& data, const io::IoSignal& signal)
{
  ValidateRange(signal, data);
  if (signal.bitSignal)
  {
    if (signal.bitOffset >= 8U)
    {
      throw std::out_of_range("Signal bit offset exceeds one byte: " + signal.textId);
    }
    return (data[signal.byteOffset] & static_cast<std::uint8_t>(1U << signal.bitOffset)) != 0U ? 1U : 0U;
  }
 
  if (signal.byteLength > sizeof(std::uint64_t))
  {
    throw std::invalid_argument("Signal is wider than uint64_t: " + signal.textId);
  }
  std::uint64_t value = 0;
  for (std::size_t index = 0; index < signal.byteLength; ++index)
  {
    value = (value << 8U) | data[signal.byteOffset + index];
  }
  return value;
}
 
void WriteUnsigned(Bytes& data, const io::IoSignal& signal, const std::uint64_t value)
{
  ValidateRange(signal, data);
  if (signal.bitSignal)
  {
    if (signal.bitOffset >= 8U || value > 1U)
    {
      throw std::invalid_argument("Invalid Boolean value for signal: " + signal.textId);
    }
    const auto mask = static_cast<std::uint8_t>(1U << signal.bitOffset);
    if (value != 0U)
    {
      data[signal.byteOffset] |= mask;
    }
    else
    {
      data[signal.byteOffset] &= static_cast<std::uint8_t>(~mask);
    }
    return;
  }
 
  if (signal.byteLength > sizeof(std::uint64_t))
  {
    throw std::invalid_argument("Signal is wider than uint64_t: " + signal.textId);
  }
  for (std::size_t index = 0; index < signal.byteLength; ++index)
  {
    const std::size_t shift = (signal.byteLength - index - 1U) * 8U;
    data[signal.byteOffset + index] =
        static_cast<std::uint8_t>((value >> shift) & 0xFFU);
  }
}
 
} // namespace
 
VirtualPhoenixPlc::VirtualPhoenixPlc(std::string nameRef,
                                     const gsdml::GSDMLDevice& gsdmlRef,
                                     MacAddress controllerMacRef)
    : name(std::move(nameRef)),
      gsdml(gsdmlRef),
      controllerMac(controllerMacRef),
      catalog(io::IoSignalCatalog::FromGsdml(this->gsdml))
{
  const std::size_t imageSize = std::max<std::size_t>(1U, RequiredImageSize(catalog));
  ioImage.inputs.resize(imageSize, 0U);
  ioImage.outputs.resize(imageSize, 0U);
}
 
VirtualPhoenixPlc::~VirtualPhoenixPlc()
{
  Stop();
}
 
void VirtualPhoenixPlc::Connect(asio::io_context& ioContext, InMemoryEthernetBus& bus,
                                VirtualAumaValve& valve,
                                rt::IOCRConfig inputIocr,
                                rt::IOCRConfig outputIocr,
                                const int maxConsecutiveTimeouts)
{
  const std::string connectionName = valve.Name();
 
  std::lock_guard<std::mutex> lock(mutex);
  if (FindConnection(connectionName) != nullptr)
  {
    throw std::invalid_argument("Phoenix connection already exists: " + connectionName);
  }
 
  if (inputIocr.dataLength == 0U || outputIocr.dataLength == 0U)
  {
    throw std::invalid_argument("Phoenix connection requires non-zero IOCR data lengths");
  }
 
  auto connection = std::make_unique<Connection>();
  connection->name = connectionName;
  connection->valve = &valve;
 
  // Determine the local output image before moving the IOCRs into the controller.
  // The production controller takes ownership by value, so no moved-from IOCR is inspected.
  for (const auto& object : outputIocr.objects)
  {
    if (object.slot == 2U && object.subslot == 1U && object.dataLength > 0U)
    {
      connection->outputImage.assign(object.dataLength, 0U);
      break;
    }
  }
  if (connection->outputImage.empty())
  {
    throw std::invalid_argument("Phoenix connection has no slot 2/1 output object");
  }
 
  auto txSocket = bus.CreateSocket(controllerMac);
  txSocket->SetReceiveEnabled(false);
  auto rxSocket = bus.CreateSocket(controllerMac);
 
  connection->controller = std::make_unique<cyclic::CyclicController>(
      ioContext,
      "virtual",
      controllerMac,
      valve.DeviceMac(),
      std::move(inputIocr),
      std::move(outputIocr),
      maxConsecutiveTimeouts,
      rt::ETHERTYPE_PROFINET);
 
  connection->controller->Start(std::move(txSocket), std::move(rxSocket));
  connections.push_back(std::move(connection));
}
 
void VirtualPhoenixPlc::Disconnect(const std::string& valveName)
{
  std::unique_ptr<Connection> removed;
  {
    std::lock_guard<std::mutex> lock(mutex);
    const auto it = std::find_if(connections.begin(), connections.end(),
                                 [&valveName](const std::unique_ptr<Connection>& connection)
    {
      return connection != nullptr && connection->name == valveName;
    });
    if (it == connections.end())
    {
      return;
    }
    removed = std::move(*it);
    connections.erase(it);
  }
 
  if (removed != nullptr && removed->controller != nullptr)
  {
    removed->controller->Stop();
  }
}
 
void VirtualPhoenixPlc::Stop()
{
  std::vector<std::unique_ptr<Connection>> local;
  {
    std::lock_guard<std::mutex> lock(mutex);
    local.swap(connections);
  }
  for (auto& connection : local)
  {
    if (connection != nullptr && connection->controller != nullptr)
    {
      connection->controller->Stop();
    }
  }
}
 
void VirtualPhoenixPlc::SetOutput(const std::string& valveName,
                                  const std::string_view signalTextId,
                                  const double value)
{
  std::lock_guard<std::mutex> lock(mutex);
  Connection* connection = FindConnection(valveName);
  if (connection == nullptr || connection->controller == nullptr || connection->valve == nullptr)
  {
    throw std::invalid_argument("Unknown Phoenix connection: " + valveName);
  }
 
  const io::IoSignal* signal =
      FindDeviceSignal(*connection->valve, signalTextId, io::IoSignalDirection::Actuator);
  if (signal == nullptr)
  {
    throw std::invalid_argument("Unknown AUMA actuator signal: " + std::string(signalTextId));
  }
 
  const Bytes updated = EncodeSignalValue(*signal, connection->outputImage, value);
  connection->outputImage = updated;
  connection->controller->SetOutputData(connection->valve->OutputSlot(),
                                        connection->valve->OutputSubslot(),
                                        updated);
}
 
std::optional<double> VirtualPhoenixPlc::ReadInput(const std::string& valveName,
                                                   const std::string_view signalTextId) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const Connection* connection = FindConnection(valveName);
  if (connection == nullptr || connection->controller == nullptr || connection->valve == nullptr)
  {
    return std::nullopt;
  }
 
  const io::IoSignal* signal =
      FindDeviceSignal(*connection->valve, signalTextId, io::IoSignalDirection::Sensor);
  if (signal == nullptr)
  {
    return std::nullopt;
  }
 
  const auto [slot, subslot] = ResolveDeviceLocation(*signal,
                                                     connection->valve->InputSlot(),
                                                     connection->valve->InputSubslot());
  const auto data = connection->controller->GetInputData(slot, subslot);
  if (!data.has_value())
  {
    return std::nullopt;
  }
  return DecodeSignal(*signal, *data);
}
 
void VirtualPhoenixPlc::SetLocalInputBytes(const std::string_view signalTextId,
                                           const Bytes& data)
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalInputSignal(signalTextId);
  if (signal == nullptr)
  {
    throw std::invalid_argument("Unknown Phoenix input signal: " + std::string(signalTextId));
  }
  if (signal->byteLength != data.size())
  {
    throw std::invalid_argument("Phoenix input byte length mismatch for signal: " +
                                std::string(signalTextId));
  }
  ValidateRange(*signal, ioImage.inputs);
  std::copy(data.begin(), data.end(), ioImage.inputs.begin() + static_cast<std::ptrdiff_t>(signal->byteOffset));
}
 
std::optional<Bytes> VirtualPhoenixPlc::GetLocalInputBytes(const std::string_view signalTextId) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalInputSignal(signalTextId);
  if (signal == nullptr)
  {
    return std::nullopt;
  }
  ValidateRange(*signal, ioImage.inputs);
  return Bytes(ioImage.inputs.begin() + static_cast<std::ptrdiff_t>(signal->byteOffset),
               ioImage.inputs.begin() +
                   static_cast<std::ptrdiff_t>(signal->byteOffset + signal->byteLength));
}
 
void VirtualPhoenixPlc::SetLocalInputValue(const std::string_view signalTextId, const double value)
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalInputSignal(signalTextId);
  if (signal == nullptr)
  {
    throw std::invalid_argument("Unknown Phoenix input signal: " + std::string(signalTextId));
  }
  ioImage.inputs = EncodeSignalValue(*signal, ioImage.inputs, value);
}
 
std::optional<double> VirtualPhoenixPlc::GetLocalInputValue(const std::string_view signalTextId) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalInputSignal(signalTextId);
  if (signal == nullptr)
  {
    return std::nullopt;
  }
  return DecodeSignal(*signal, ioImage.inputs);
}
 
void VirtualPhoenixPlc::SetLocalOutputBytes(const std::string_view signalTextId, const Bytes& data)
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalOutputSignal(signalTextId);
  if (signal == nullptr)
  {
    throw std::invalid_argument("Unknown Phoenix output signal: " + std::string(signalTextId));
  }
  if (signal->byteLength != data.size())
  {
    throw std::invalid_argument("Phoenix output byte length mismatch for signal: " +
                                std::string(signalTextId));
  }
  ValidateRange(*signal, ioImage.outputs);
  std::copy(data.begin(), data.end(),
            ioImage.outputs.begin() + static_cast<std::ptrdiff_t>(signal->byteOffset));
}
 
std::optional<Bytes> VirtualPhoenixPlc::GetLocalOutputBytes(const std::string_view signalTextId) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalOutputSignal(signalTextId);
  if (signal == nullptr)
  {
    return std::nullopt;
  }
  ValidateRange(*signal, ioImage.outputs);
  return Bytes(ioImage.outputs.begin() + static_cast<std::ptrdiff_t>(signal->byteOffset),
               ioImage.outputs.begin() +
                   static_cast<std::ptrdiff_t>(signal->byteOffset + signal->byteLength));
}
 
void VirtualPhoenixPlc::SetLocalOutputValue(const std::string_view signalTextId, const double value)
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalOutputSignal(signalTextId);
  if (signal == nullptr)
  {
    throw std::invalid_argument("Unknown Phoenix output signal: " + std::string(signalTextId));
  }
  ioImage.outputs = EncodeSignalValue(*signal, ioImage.outputs, value);
}
 
std::optional<double> VirtualPhoenixPlc::GetLocalOutputValue(const std::string_view signalTextId) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const io::IoSignal* signal = FindLocalOutputSignal(signalTextId);
  if (signal == nullptr)
  {
    return std::nullopt;
  }
  return DecodeSignal(*signal, ioImage.outputs);
}
 
const io::IoSignalCatalog& VirtualPhoenixPlc::SignalCatalog() const noexcept
{
  return catalog;
}
 
const MacAddress& VirtualPhoenixPlc::ControllerMac() const noexcept
{
  return controllerMac;
}
 
const std::string& VirtualPhoenixPlc::Name() const noexcept
{
  return name;
}
 
std::size_t VirtualPhoenixPlc::ConnectionCount() const
{
  std::lock_guard<std::mutex> lock(mutex);
  return connections.size();
}
 
std::optional<cyclic::CyclicState>
VirtualPhoenixPlc::ConnectionState(const std::string& valveName) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const Connection* connection = FindConnection(valveName);
  if (connection == nullptr || connection->controller == nullptr)
  {
    return std::nullopt;
  }
  return connection->controller->State();
}
 
std::optional<cyclic::CyclicStatsSnapshot>
VirtualPhoenixPlc::ConnectionStats(const std::string& valveName) const
{
  std::lock_guard<std::mutex> lock(mutex);
  const Connection* connection = FindConnection(valveName);
  if (connection == nullptr || connection->controller == nullptr)
  {
    return std::nullopt;
  }
  return connection->controller->StatsSnapshot();
}
 
std::string VirtualPhoenixPlc::StatusReport() const
{
  std::lock_guard<std::mutex> lock(mutex);
  std::ostringstream out;
  out << "Phoenix PLC " << name << "  MAC " << Mac2String(controllerMac) << '\n';
  out << "  connections: " << connections.size() << '\n';
  for (const auto& connection : connections)
  {
    if (connection == nullptr || connection->controller == nullptr || connection->valve == nullptr)
    {
      continue;
    }
    const auto stats = connection->controller->StatsSnapshot();
    out << "  - " << connection->name
        << " state=" << cyclic::ToString(connection->controller->State())
        << " position=" << connection->valve->Position()
        << " target=" << connection->valve->TargetPosition()
        << " scenario=" << ToString(connection->valve->Scenario())
        << " rx=" << stats.framesReceived
        << " missed=" << stats.framesMissed
        << '\n';
  }
  return out.str();
}
 
VirtualPhoenixPlc::Connection* VirtualPhoenixPlc::FindConnection(const std::string& valveName)
{
  for (auto& connection : connections)
  {
    if (connection != nullptr && connection->name == valveName)
    {
      return connection.get();
    }
  }
  return nullptr;
}
 
const VirtualPhoenixPlc::Connection*
VirtualPhoenixPlc::FindConnection(const std::string& valveName) const
{
  for (const auto& connection : connections)
  {
    if (connection != nullptr && connection->name == valveName)
    {
      return connection.get();
    }
  }
  return nullptr;
}
 
const io::IoSignal* VirtualPhoenixPlc::FindDeviceSignal(const VirtualAumaValve& valve,
                                                        const std::string_view textId,
                                                        const io::IoSignalDirection direction)
{
  return FindSignal(valve.SignalCatalog(), textId, direction);
}
 
double VirtualPhoenixPlc::DecodeSignal(const io::IoSignal& signal, const Bytes& data) const
{
  const std::uint64_t raw = ReadUnsigned(data, signal);
  switch (signal.valueType)
  {
    case io::IoSignalValueType::Boolean:
      return raw == 0U ? 0.0 : 1.0;
    case io::IoSignalValueType::UnsignedInteger:
      return static_cast<double>(raw);
    case io::IoSignalValueType::SignedInteger:
    {
      const std::size_t bits = signal.byteLength * 8U;
      if (bits == 0U || bits > 64U)
      {
        throw std::invalid_argument("Invalid signed integer width: " + signal.textId);
      }
      if (bits == 64U)
      {
        const std::int64_t signedValue = std::bit_cast<std::int64_t>(raw);
        return static_cast<double>(signedValue);
      }
      const std::uint64_t signBit = 1ULL << (bits - 1U);
      const std::uint64_t mask = ~0ULL << bits;
      const std::int64_t signedValue = (raw & signBit) != 0U
                                           ? static_cast<std::int64_t>(raw | mask)
                                           : static_cast<std::int64_t>(raw);
      return static_cast<double>(signedValue);
    }
    case io::IoSignalValueType::FloatingPoint:
      if (signal.byteLength == sizeof(float))
      {
        const std::uint32_t bits = static_cast<std::uint32_t>(raw);
        float value = 0.0F;
        std::memcpy(&value, &bits, sizeof(value));
        return static_cast<double>(value);
      }
      if (signal.byteLength == sizeof(double))
      {
        double value = 0.0;
        std::memcpy(&value, &raw, sizeof(value));
        return value;
      }
      throw std::invalid_argument("Unsupported floating-point width: " + signal.textId);
    case io::IoSignalValueType::Unsupported:
      throw std::invalid_argument("Unsupported Phoenix simulator signal: " + signal.textId);
  }
  throw std::logic_error("Unknown IoSignalValueType");
}
 
Bytes VirtualPhoenixPlc::EncodeSignalValue(const io::IoSignal& signal,
                                           const Bytes& data,
                                           const double value) const
{
  Bytes result = data;
  if (!signal.IsDoubleCompatible())
  {
    throw std::invalid_argument("Signal is not double-compatible: " + signal.textId);
  }
  if (!std::isfinite(value))
  {
    throw std::invalid_argument("Signal value must be finite: " + signal.textId);
  }
 
  if (signal.valueType == io::IoSignalValueType::Boolean)
  {
    if (value != 0.0 && value != 1.0)
    {
      throw std::invalid_argument("Boolean signal requires value 0 or 1: " + signal.textId);
    }
    WriteUnsigned(result, signal, value == 0.0 ? 0U : 1U);
    return result;
  }
 
  if (signal.valueType == io::IoSignalValueType::FloatingPoint)
  {
    ValidateRange(signal, result);
    if (signal.byteLength == sizeof(float))
    {
      if (static_cast<long double>(value) > static_cast<long double>(FLT_MAX) ||
          static_cast<long double>(value) < -static_cast<long double>(FLT_MAX))
      {
        throw std::out_of_range("Float32 signal value is outside its range: " + signal.textId);
      }
      const float floatValue = static_cast<float>(value);
      const std::uint32_t bits = std::bit_cast<std::uint32_t>(floatValue);
      for (std::size_t index = 0; index < sizeof(bits); ++index)
      {
        const std::size_t shift = (sizeof(bits) - index - 1U) * 8U;
        result[signal.byteOffset + index] =
            static_cast<std::uint8_t>((bits >> shift) & 0xFFU);
      }
      return result;
    }
    if (signal.byteLength == sizeof(double))
    {
      const std::uint64_t bits = std::bit_cast<std::uint64_t>(value);
      for (std::size_t index = 0; index < sizeof(bits); ++index)
      {
        const std::size_t shift = (sizeof(bits) - index - 1U) * 8U;
        result[signal.byteOffset + index] =
            static_cast<std::uint8_t>((bits >> shift) & 0xFFU);
      }
      return result;
    }
    throw std::invalid_argument("Unsupported floating-point width: " + signal.textId);
  }
 
  const long double integerValue = static_cast<long double>(value);
  if (std::floor(integerValue) != integerValue)
  {
    throw std::invalid_argument("Integer signal requires an integral value: " + signal.textId);
  }
  if (signal.byteLength == 0U || signal.byteLength > sizeof(std::uint64_t))
  {
    throw std::invalid_argument("Integer signal has an invalid width: " + signal.textId);
  }
 
  const std::size_t bits = signal.byteLength * 8U;
  if (signal.valueType == io::IoSignalValueType::UnsignedInteger)
  {
    if (integerValue < 0.0L)
    {
      throw std::invalid_argument("Unsigned signal cannot be negative: " + signal.textId);
    }
    const long double maxValue = bits == 64U
                                     ? 18446744073709551615.0L
                                     : static_cast<long double>((1ULL << bits) - 1ULL);
    if (integerValue > maxValue)
    {
      throw std::out_of_range("Unsigned signal value exceeds its range: " + signal.textId);
    }
    WriteUnsigned(result, signal, static_cast<std::uint64_t>(value));
    return result;
  }
 
  if (signal.valueType == io::IoSignalValueType::SignedInteger)
  {
    const long double minValue = bits == 64U
                                     ? -9223372036854775808.0L
                                     : -static_cast<long double>(1ULL << (bits - 1U));
    const long double maxValue = bits == 64U
                                     ? 9223372036854775807.0L
                                     : static_cast<long double>((1ULL << (bits - 1U)) - 1ULL);
    if (integerValue < minValue || integerValue > maxValue)
    {
      throw std::out_of_range("Signed signal value exceeds its range: " + signal.textId);
    }
    const auto signedValue = static_cast<std::int64_t>(value);
    WriteUnsigned(result, signal, std::bit_cast<std::uint64_t>(signedValue));
    return result;
  }
 
  throw std::invalid_argument("Unsupported signal value type: " + signal.textId);
}
 
std::size_t VirtualPhoenixPlc::RequiredImageSize(const io::IoSignalCatalog& signalCatalog)
{
  std::size_t size = 0;
  const auto accumulate = [&size](const std::vector<io::IoSignal>& signals)
  {
    for (const auto& signal : signals)
    {
      size = std::max(size, signal.byteOffset + signal.byteLength);
    }
  };
  accumulate(signalCatalog.Sensors());
  accumulate(signalCatalog.Actuators());
  return size;
}
 
const io::IoSignal* VirtualPhoenixPlc::FindLocalInputSignal(const std::string_view textId) const
{
  return FindSignal(catalog, textId, io::IoSignalDirection::Sensor);
}
 
const io::IoSignal* VirtualPhoenixPlc::FindLocalOutputSignal(const std::string_view textId) const
{
  return FindSignal(catalog, textId, io::IoSignalDirection::Actuator);
}
 
std::pair<std::uint16_t, std::uint16_t> VirtualPhoenixPlc::ResolveDeviceLocation(
    const io::IoSignal& signal,
    const std::uint16_t defaultSlot,
    const std::uint16_t defaultSubslot)
{
  std::uint16_t slot = defaultSlot;
  std::uint16_t subslot = defaultSubslot;
  if (!signal.possibleSlots.empty())
  {
    if (signal.possibleSlots.size() != 1U || signal.possibleSlots.front() < 0 ||
        signal.possibleSlots.front() > std::numeric_limits<std::uint16_t>::max())
    {
      throw std::invalid_argument("AUMA signal has ambiguous slot placement: " + signal.textId);
    }
    slot = static_cast<std::uint16_t>(signal.possibleSlots.front());
  }
  if (!signal.possibleSubslots.empty())
  {
    if (signal.possibleSubslots.size() != 1U || signal.possibleSubslots.front() < 0 ||
        signal.possibleSubslots.front() > std::numeric_limits<std::uint16_t>::max())
    {
      throw std::invalid_argument("AUMA signal has ambiguous subslot placement: " + signal.textId);
    }
    subslot = static_cast<std::uint16_t>(signal.possibleSubslots.front());
  }
  return {slot, subslot};
}
 
} // namespace profinet::sim