device.cpp

Go to the documentation of this file.

 
 
#include "profinet/device.h"
 
#include <algorithm>
#include <cctype>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <sstream>
 
#include "profinet/exceptions.h"
#include "profinet/indices.h"
#include "profinet/wire.h"
 
namespace profinet::device
{
 
namespace
{
 
std::string Trim(const std::string& s)
{
  const std::size_t start = s.find_first_not_of(" \t\r\n\0", 0, 4);
  if (start == std::string::npos)
  {
    return "";
  }
  const std::size_t end = s.find_last_not_of(" \t\r\n\0", std::string::npos, 4);
  return s.substr(start, end - start + 1);
}
 
Bytes BuildImBlockHeader(std::uint16_t blockType, std::uint16_t blockLength)
{
  Bytes out;
  wire::PutU16(out, blockType);
  wire::PutU16(out, blockLength);
  wire::PutU8(out, 0x01);
  wire::PutU8(out, 0x00);
  return out;
}
 
Bytes Latin1Pad(const std::string& s, std::size_t maxLen)
{
  Bytes out(s.begin(), s.begin() + std::min(s.size(), maxLen));
  out.resize(maxLen, static_cast<std::uint8_t>(' '));
  return out;
}
 
} // namespace
 
std::optional<MacAddress> ParseMac(const std::string& macStrIn)
{
  std::string s = macStrIn;
  std::ranges::transform(s, s.begin(), [](unsigned char c)
  { return std::tolower(c); });
 
  // Handle Cisco-style dotted MAC: xxxx.xxxx.xxxx
  // Cisco-style dotted MAC: aabb.ccdd.eeff
  if (s.find('.') != std::string::npos)
  {
    std::vector<std::string> groups;
    std::stringstream ss(s);
    std::string group;
 
    while (std::getline(ss, group, '.'))
    {
      groups.push_back(group);
    }
 
    // Support Cisco notation: aabb.ccdd.eeff
    if (groups.size() == 3)
    {
      if (!std::ranges::all_of(groups, [](const std::string& group)
      {
        return group.size() == 4 &&
               std::ranges::all_of(group, [](unsigned char c)
        {
          return std::isxdigit(c) != 0;
        });
      }))
      {
        return std::nullopt;
      }
 
      MacAddress mac{};
 
      for (std::size_t i = 0; i < 3; ++i)
      {
        const auto value =
            static_cast<std::uint16_t>(
                std::stoul(groups[i], nullptr, 16));
 
        mac[i * 2] =
            static_cast<std::uint8_t>(value >> 8);
 
        mac[i * 2 + 1] =
            static_cast<std::uint8_t>(value & 0xff);
      }
 
      return mac;
    }
 
    // Also support six-group dotted notation:
    // aa.bb.cc.dd.ee.ff
    if (groups.size() == 6)
    {
      if (!std::ranges::all_of(groups, [](const std::string& group)
      {
        return group.size() == 2 &&
               std::ranges::all_of(group, [](unsigned char c)
        {
          return std::isxdigit(c) != 0;
        });
      }))
      {
        return std::nullopt;
      }
 
      MacAddress mac{};
 
      for (std::size_t i = 0; i < 6; ++i)
      {
        mac[i] = static_cast<std::uint8_t>(
            std::stoul(groups[i], nullptr, 16));
      }
 
      return mac;
    }
 
    return std::nullopt;
  }
 
  std::ranges::replace(s, '-', ':');
 
  std::vector<std::string> parts;
  std::stringstream ss(s);
  std::string part;
  while (std::getline(ss, part, ':'))
  {
    parts.push_back(part);
  }
 
  if (parts.size() != macAddressLength)
  {
    return std::nullopt;
  }
 
  MacAddress mac{};
  for (int i = 0; i < macAddressLength; ++i)
  {
    if (parts[i].empty() || parts[i].size() > 2)
    {
      return std::nullopt;
    }
    for (const char c : parts[i])
    {
      if (std::isxdigit(static_cast<unsigned char>(c)) == 0)
      {
        return std::nullopt;
      }
    }
    mac[i] = static_cast<std::uint8_t>(std::stoul(parts[i], nullptr, 16));
  }
  return mac;
}
 
bool IsMacAddress(const std::string& identifier)
{
  return ParseMac(identifier).has_value();
}
 
// =============================================================================
// DeviceInfo
// =============================================================================
 
std::string DeviceInfo::SerialNumber() const
{
  return im0 ? Trim(im0->imSerialNumber) : "";
}
std::string DeviceInfo::OrderId() const
{
  return im0 ? Trim(im0->orderId) : "";
}
std::uint16_t DeviceInfo::HardwareRevision() const
{
  return im0 ? im0->imHardwareRevision : 0;
}
 
std::string DeviceInfo::SoftwareRevision() const
{
  if (!im0)
  {
    return "";
  }
  // Create the prefix string directly: either the character itself, or empty if it is 0U
  const std::string prefixStr = (im0->swRevisionPrefix != 0U)
                                    ? std::string(1, static_cast<char>(im0->swRevisionPrefix))
                                    : "";
 
  // Format everything directly into the final const string without any post-processing or erasing
  const std::string result = std::format("{}{}.{}.{}",
                                         prefixStr,
                                         im0->imSwRevisionFunctionalEnhancement,
                                         im0->imSwRevisionBugFix,
                                         im0->imSwRevisionInternalChange);
 
  return result;
}
 
// =============================================================================
// ProfinetDevice
// =============================================================================
 
ProfinetDevice::ProfinetDevice(dcp::DCPDeviceDescription info, std::string interface, MacAddress srcMac,
                               double timeoutSec, std::unique_ptr<rpc::IRPCCon> rpc, std::unique_ptr<IAlarmListener> alarmListener)
    : info(std::move(info)), interface(std::move(interface)), srcMac(srcMac), timeout(timeoutSec), rpc(std::move(rpc)), alarmListener(std::move(alarmListener))
{
}
 
ProfinetDevice::~ProfinetDevice()
{
  Close();
}
 
ProfinetDevice ProfinetDevice::Discover(const std::string& identifier, const std::string& interface,
                                        double timeoutSec)
{
  const MacAddress srcMac = GetMac(interface);
  const EthernetSocket sock(interface);
 
  auto targetMac = ParseMac(identifier);
  if (targetMac)
  {
    dcp::SendDiscover(sock, srcMac);
    auto responses = dcp::ReadResponse(sock, srcMac, static_cast<int>(timeoutSec));
    for (const auto& [Mac, Blocks] : responses)
    {
      if (Mac == *targetMac)
      {
        dcp::DCPDeviceDescription info(Mac, Blocks);
        // NOLINTNEXTLINE(modernize-return-braced-init-list)
        return ProfinetDevice(std::move(info), interface, srcMac, timeoutSec);
      }
    }
    throw DCPDeviceNotFoundError("Device with MAC '" + identifier + "' not found");
  }
 
  dcp::DCPDeviceDescription info =
      rpc::GetStationInfo(sock, srcMac, identifier, static_cast<int>(timeoutSec));
  std::cout << info.ToString() << "\n\n";
  auto device = ProfinetDevice(std::move(info), interface, srcMac, timeoutSec);
  std::cout << device.ToString() << "\n\n";
  return device;
}
 
ProfinetDevice ProfinetDevice::FromIp(const std::string& ip, const std::string& interface, double timeoutSec)
{
  const MacAddress srcMac = GetMac(interface);
  const EthernetSocket sock(interface);
 
  dcp::SendDiscover(sock, srcMac);
  auto responses = dcp::ReadResponse(sock, srcMac, static_cast<int>(timeoutSec));
  for (const auto& [Mac, Blocks] : responses)
  {
    dcp::DCPDeviceDescription info(Mac, Blocks);
    if (info.ip == ip)
    {
      // NOLINTNEXTLINE(modernize-return-braced-init-list)
      return ProfinetDevice(std::move(info), interface, srcMac, timeoutSec);
    }
  }
  throw DCPDeviceNotFoundError("No device found at IP " + ip);
}
 
ProfinetDevice ProfinetDevice::FromDcpInfo(dcp::DCPDeviceDescription info, const std::string& interface,
                                           double timeoutSec)
{
  const MacAddress srcMac = GetMac(interface);
  // NOLINTNEXTLINE(modernize-return-braced-init-list)
  return ProfinetDevice(std::move(info), interface, srcMac, timeoutSec);
}
 
void ProfinetDevice::Connect()
{
  if (connected && rpc)
  {
    return;
  }
  auto options = rpc::RPCConOptions{.timeoutSec = timeout};
 
  if (rpc == nullptr)
  {
    rpc = std::make_unique<rpc::RPCCon>(info, options);
  }
  try
  {
    rpc->Connect(srcMac);
    connected = true;
  }
  catch (const RPCError& e)
  {
    rpc->Close();
    rpc.reset();
    throw RPCConnectionError("Failed to connect to " + info.name + ": " + e.what());
  }
}
 
void ProfinetDevice::Disconnect()
{
  if (rpc)
  {
    rpc->Disconnect();
    connected = false;
  }
}
 
void ProfinetDevice::Close()
{
  if (alarmListener)
  {
    alarmListener->Stop();
    alarmListener.reset();
  }
  if (rpc)
  {
    rpc->Close();
    rpc.reset();
    connected = false;
  }
}
 
void ProfinetDevice::ReleaseAr(rpc::IRPCCon& releaseRpc)
{
  StopAlarmListener();
 
  try
  {
    releaseRpc.Disconnect();
  }
  catch (const std::exception& e)
  {
    // Best-effort teardown: continue closing the RPC connection even when
    // the PROFINET disconnect procedure fails.
  }
 
  releaseRpc.Close();
 
  if (this->rpc.get() == &releaseRpc)
  {
    this->rpc.reset(); // Safely destroys the RPCCon instance and resets pointer to nullptr
    connected = false;
  }
}
 
rpc::IRPCCon& ProfinetDevice::EnsureConnected()
{
  if (!connected || !rpc)
  {
    Connect();
  }
  return *rpc;
}
 
DeviceInfo ProfinetDevice::GetInfo(bool includeTopology)
{
  DeviceInfo returnInfo;
  returnInfo.name = info.name;
  returnInfo.ip = info.ip;
  returnInfo.mac = info.mac;
  returnInfo.vendorId = info.VendorId();
  returnInfo.deviceId = info.DeviceId();
  returnInfo.deviceType = info.deviceType;
  returnInfo.netmask = info.netmask;
  returnInfo.gateway = info.gateway;
  returnInfo.deviceRoles = info.deviceRoles;
  returnInfo.vendorName = info.VendorName();
 
  auto& conn = EnsureConnected();
 
  try
  {
    returnInfo.im0 = conn.ReadIm0();
  }
  catch (const RPCError&)
  {
    // PNIOError derives from RPCError, so it's already caught above.
  }
 
  try
  {
    auto endpoints = rpc::EpmLookup(rpc->IoContext(), returnInfo.ip);
    for (const auto& ep : endpoints)
    {
      if (!ep.annotation.empty())
      {
        returnInfo.annotation = ep.annotation;
        break;
      }
    }
  }
  catch (const std::exception&)
  {
  }
 
  if (includeTopology)
  {
    try
    {
      returnInfo.topology = conn.ReadPdRealData();
    }
    catch (const RPCError&)
    {
      // PNIOError derives from RPCError, so it's already caught above.
    }
  }
 
  return returnInfo;
}
 
Bytes ProfinetDevice::Read(std::uint16_t slot, std::uint16_t subslot, std::uint16_t index, std::uint32_t api)
{
  try
  {
    auto& conn = EnsureConnected();
    auto responseIOD = conn.Read(api, slot, subslot, index);
    return responseIOD.payload;
  }
  catch (const RPCError& e)
  {
    // Some device stacks (e.g. p-net) do not implement the Device
    // Access AR; the AR-less Read Implicit service still works there.
    // logger.info("AR connect failed, falling back to Read Implicit")
    return ReadImplicit(slot, subslot, index, api);
  }
}
 
Bytes ProfinetDevice::ReadImplicit(const std::uint16_t slot, const std::uint16_t subslot, const std::uint16_t index, const std::uint32_t api)
{
  if (connected && rpc)
  {
    return rpc->ReadImplicit(api, slot, subslot, index).payload;
  }
 
  rpc::RPCConOptions options{.timeoutSec = timeout};
  rpc::RPCCon tmpRpc(info, options);
  Bytes implicitRead;
 
  try
  {
    implicitRead = tmpRpc.ReadImplicit(api, slot, subslot, index).payload;
  }
  catch (...)
  {
    tmpRpc.Close();
    throw;
  }
  tmpRpc.Close();
 
  return implicitRead;
}
 
void ProfinetDevice::Write(std::uint16_t slot, std::uint16_t subslot, std::uint16_t index, const Bytes& data,
                           std::uint32_t api)
{
  auto& conn = EnsureConnected();
  conn.Write(api, slot, subslot, index, data);
}
 
std::vector<blocks::WriteMultipleResult> ProfinetDevice::WriteMultiple(const std::vector<WriteItem>& writes)
{
  auto& conn = EnsureConnected();
  std::vector<rpc::WriteItem> items;
  items.reserve(writes.size());
  for (const auto& w : writes)
  {
    items.push_back({.slot = w.slot, .subslot = w.subslot, .index = w.index, .data = w.data, .api = w.api});
  }
  return conn.WriteMultiple(items);
}
 
PNInM0 ProfinetDevice::ReadIm0(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm0(slot, subslot);
}
PNInM1 ProfinetDevice::ReadIm1(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm1(slot, subslot);
}
PNInM2 ProfinetDevice::ReadIm2(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm2(slot, subslot);
}
PNInM3 ProfinetDevice::ReadIm3(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm3(slot, subslot);
}
PNInM4 ProfinetDevice::ReadIm4(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm4(slot, subslot);
}
PNInM5 ProfinetDevice::ReadIm5(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadIm5(slot, subslot);
}
rpc::AllIM ProfinetDevice::ReadAllIm(std::uint16_t slot, std::uint16_t subslot)
{
  return EnsureConnected().ReadAllIm(slot, subslot);
}
 
void ProfinetDevice::WriteIm1(const std::string& tagFunction, const std::string& tagLocation, std::uint16_t slot, std::uint16_t subslot)
{
  if (tagFunction.size() > 32)
  {
    throw std::invalid_argument("tag_function exceeds 32 character limit");
  }
  if (tagLocation.size() > 22)
  {
    throw std::invalid_argument("tag_location exceeds 22 character limit");
  }
 
  Bytes data = BuildImBlockHeader(0x0021, 58);
  data.push_back(0x00);
  data.push_back(0x00);
  Bytes func = Latin1Pad(tagFunction, 32);
  Bytes loc = Latin1Pad(tagLocation, 22);
  data.insert(data.end(), func.begin(), func.end());
  data.insert(data.end(), loc.begin(), loc.end());
 
  Write(slot, subslot, IM1, data);
}
 
void ProfinetDevice::WriteIm2(const std::string& date, std::uint16_t slot, std::uint16_t subslot)
{
  if (date.size() > 16)
  {
    throw std::invalid_argument("date exceeds 16 character limit");
  }
 
  Bytes data = BuildImBlockHeader(0x0022, 20);
  data.push_back(0x00);
  data.push_back(0x00);
  Bytes dateBytes = Latin1Pad(date, 16);
  data.insert(data.end(), dateBytes.begin(), dateBytes.end());
 
  Write(slot, subslot, IM2, data);
}
 
void ProfinetDevice::WriteIm3(const std::string& descriptor, std::uint16_t slot, std::uint16_t subslot)
{
  if (descriptor.size() > 54)
  {
    throw std::invalid_argument("descriptor exceeds 54 character limit");
  }
 
  Bytes data = BuildImBlockHeader(0x0023, 58);
  data.push_back(0x00);
  data.push_back(0x00);
  Bytes descBytes = Latin1Pad(descriptor, 54);
  data.insert(data.end(), descBytes.begin(), descBytes.end());
 
  Write(slot, subslot, IM3, data);
}
 
blocks::ModuleDiffBlock ProfinetDevice::ReadModuleDiff()
{
  return EnsureConnected().ReadModuleDiff();
}
 
diagnosis::DiagnosisData ProfinetDevice::ReadDiagnosis(std::uint16_t slot, std::uint16_t subslot,
                                                       std::uint16_t index)
{
  return EnsureConnected().ReadDiagnosis(slot, subslot, index);
}
 
std::map<std::uint16_t, diagnosis::DiagnosisData> ProfinetDevice::ReadAllDiagnosis()
{
  return EnsureConnected().ReadAllDiagnosis();
}
 
std::vector<blocks::SlotInfo> ProfinetDevice::DiscoverSlots()
{
  return EnsureConnected().DiscoverSlots();
}
 
blocks::PDRealData ProfinetDevice::ReadTopology()
{
  return EnsureConnected().ReadPdRealData();
}
 
std::optional<AlarmNotification> ProfinetDevice::ReadAlarm(std::uint16_t slot, std::uint16_t subslot,
                                                           std::uint16_t index)
{
  try
  {
    const Bytes data = Read(slot, subslot, index);
    if (data.size() >= 26)
    {
      return ParseAlarmNotification(data);
    }
    return std::nullopt;
  }
  catch (const RPCError&)
  {
    // PNIOError derives from RPCError, so it's already caught above.
    return std::nullopt;
  }
}
 
void ProfinetDevice::OnAlarm(std::function<void(const AlarmNotification&)> callback)
{
  alarmCallbacks.push_back(callback);
  if (alarmListener)
  {
    alarmListener->AddCallback(callback);
  }
}
 
void ProfinetDevice::StartAlarmListener()
{
  if (!connected || !rpc)
  {
    throw std::runtime_error("Must be connected first");
  }
 
  if (!rpc->HasAlarmCr())
  {
    throw std::runtime_error("AlarmCR not established. Reconnect with with_alarm_cr=true via RPCCon directly");
  }
 
  AlarmEndpoint endpoint;
  endpoint.interface = interface;
  endpoint.controllerRef = rpc->LocalAlarmRef();
  endpoint.deviceRef = static_cast<std::uint16_t>(rpc->DeviceAlarmRef());
  endpoint.deviceMac = String2Mac(info.mac);
  endpoint.transport = 0; // Layer 2
 
  if (alarmListener == nullptr)
  {
    alarmListener = std::make_unique<AlarmListener>(endpoint, srcMac);
  }
  for (const auto& cb : alarmCallbacks)
  {
    alarmListener->AddCallback(cb);
  }
  alarmListener->Start();
}
 
void ProfinetDevice::StopAlarmListener()
{
  if (alarmListener)
  {
    alarmListener->Stop();
    alarmListener.reset();
  }
}
 
bool ProfinetDevice::AlarmListenerRunning() const
{
  return alarmListener != nullptr && alarmListener->IsRunning();
}
 
// hier ben je gebleven
std::unique_ptr<cyclic::ICyclicController> ProfinetDevice::StartCyclic(const rpc::IOCRSetup& iocrSetup,
                                                                       int maxConsecutiveTimeouts,
                                                                       std::unique_ptr<cyclic::ICyclicController> controller)
{
  auto& conn = EnsureConnected();
 
  auto result = conn.Connect(srcMac, /*WithAlarmCr=*/true, iocrSetup);
  if (!result || !result->HasCyclic())
  {
    throw std::runtime_error("Cyclic IO not established by device");
  }
 
  conn.PrmEnd();
  conn.ApplicationReady(30.0);
 
  auto [input_iocr, output_iocr] = rt::BuildIocrConfigs(
      iocrSetup.slots, result->inputFrameId, result->outputFrameId, iocrSetup.sendClockFactor,
      iocrSetup.reductionRatio, iocrSetup.watchdogFactor);
  if (controller == nullptr)
  {
    controller = std::make_unique<cyclic::CyclicController>(
        interface, srcMac, String2Mac(info.mac), input_iocr, output_iocr, maxConsecutiveTimeouts);
  }
  controller->Start();
  return controller;
}
 
std::map<std::uint16_t, rpc::IndexProbeResult> ProfinetDevice::EnumerateIndices(std::uint16_t slot,
                                                                                std::uint16_t subslot)
{
  return EnsureConnected().EnumerateIndices(slot, subslot);
}
 
std::string ProfinetDevice::ToString() const
{
  return "ProfinetDevice('" + info.name + "', " + info.ip + ", " + (connected ? "connected" : "disconnected") +
         ")";
}
 
// =============================================================================
// Scan
// =============================================================================
 
std::vector<ProfinetDevice> Scan(const std::string& interface, double timeoutSec)
{
  const MacAddress srcMac = GetMac(interface);
  const EthernetSocket sock(interface);
 
  dcp::SendDiscover(sock, srcMac);
  auto responses = dcp::ReadResponse(sock, srcMac, static_cast<int>(timeoutSec));
 
  std::vector<ProfinetDevice> devices;
  devices.reserve(responses.size());
  for (const auto& [Mac, Blocks] : responses)
  {
    dcp::DCPDeviceDescription info(Mac, Blocks);
    devices.emplace_back(std::move(info), interface, srcMac, 5.0);
  }
  return devices;
}
 
std::map<std::string, DeviceInfo> ScanDict(const std::string& interface, double timeoutSec)
{
  std::map<std::string, DeviceInfo> result;
  for (auto& dev : Scan(interface, timeoutSec))
  {
    DeviceInfo info;
    info.name = dev.Name();
    info.ip = dev.Ip();
    info.mac = dev.Mac();
    result[dev.Name()] = info;
  }
  return result;
}
 
} // namespace profinet::device