alarmListener.cpp

Go to the documentation of this file.

 
 
#include "profinet/alarmListener.h"
 
#include <cstring>
#include <iostream>
#include <ranges>
 
#include "profinet/asioEthernetSocket.h"
#include "profinet/exceptions.h"
#include "profinet/udpSocket.h"
 
namespace profinet
{
 
AlarmListener::AlarmListener(asio::io_context& ioContext, AlarmEndpoint endpoint, std::optional<MacAddress> controllerMac)
    : ioContext(ioContext),
      endpoint(std::move(endpoint)),
      controllerMac(controllerMac.value_or(MacAddress{}))
{
}
 
AlarmListener::~AlarmListener()
{
  Stop();
}
 
void AlarmListener::AddAlarmNotificationCallback(std::function<void(const AlarmNotification&)> callback)
{
  if (!callback)
  {
    return;
  }
  const std::lock_guard<std::mutex> lock(callbacksMutex);
 
  alarmNotificationCallbacks.push_back(std::move(callback));
}
 
void AlarmListener::RemoveAlarmNotificationCallback(std::function<void(const AlarmNotification&)> callback)
{
  if (!callback)
  {
    return;
  }
  // Extract the raw function pointer from the passed callback
  auto targetPtr = callback.target<void (*)(const AlarmNotification&)>();
  if (!targetPtr || !*targetPtr)
  {
    return; // Passed callback is not a plain function pointer (e.g., a capturing lambda)
  }
 
  auto fnToMatch = *targetPtr;
 
  // Erase matching callbacks
  const std::lock_guard<std::mutex> lock(callbacksMutex);
  std::erase_if(alarmNotificationCallbacks, [fnToMatch](const auto& cb)
  {
    auto target = cb.template target<void (*)(const AlarmNotification&)>();
    return target && *target == fnToMatch;
  });
}
 
void AlarmListener::AddRtaAlarmCallback(std::function<void(const RtaError&)> callback)
{
  if (!callback)
  {
    return;
  }
  const std::lock_guard<std::mutex> lock(callbacksMutex);
  rtaAlarmCallbacks.push_back(std::move(callback));
}
 
void AlarmListener::RemoveRtaAlarmCallback(std::function<void(const RtaError&)> callback)
{
  if (!callback)
  {
    return;
  }
  // Extract the raw function pointer from the passed callback
  auto targetPtr = callback.target<void (*)(const RtaError&)>();
  if (!targetPtr || !*targetPtr)
  {
    return; // Passed callback is not a plain function pointer (e.g., a capturing lambda)
  }
 
  auto fnToMatch = *targetPtr;
 
  // Erase matching callbacks
  const std::lock_guard<std::mutex> lock(callbacksMutex);
  std::erase_if(rtaAlarmCallbacks, [fnToMatch](const auto& cb)
  {
    auto target = cb.template target<void (*)(const RtaError&)>();
    return target && *target == fnToMatch;
  });
}
 
void AlarmListener::Start(std::unique_ptr<IRawEthernetSocket> eSocket, std::unique_ptr<IUdpSocket> uSocket)
{
  std::lock_guard lock(lifecycleMutex);
 
  if (running)
  {
    return;
  }
 
  std::jthread worker;
  try
  {
    if (endpoint.transport == 0)
    {
      if (eSocket == nullptr)
      {
        eSocket = std::make_unique<AsioEthernetSocket>(ioContext, endpoint.interface, ETHERTYPE_PROFINET);
      }
      static constexpr uint16_t timeoutMs = 1000;
      eSocket->SetTimeout(std::chrono::milliseconds(timeoutMs));
    }
    else
    {
      if (uSocket == nullptr)
      {
        uSocket = std::make_unique<UdpSocket>(ALARM_UDP_PORT, std::chrono::seconds(1));
      }
    }
  }
  catch (...)
  {
    running = false;
    eSocket.reset();
    uSocket.reset();
    throw;
  }
  l2Sock = std::move(eSocket);
 
  udpSock = std::move(uSocket);
 
  running = true;
  worker = std::jthread([this]
  { ListenLoop(); });
  thread = std::move(worker);
}
 
void AlarmListener::Stop()
{
  std::scoped_lock lock(lifecycleMutex);
 
  if (!running && !thread.joinable())
  {
    return;
  }
 
  running = false;
 
  if (thread.joinable())
  {
    thread.join();
  }
 
  l2Sock.reset();
  udpSock.reset();
}
 
void AlarmListener::ListenLoop()
{
  while (running)
  {
    try
    {
      if (endpoint.transport == 0)
      {
        HandleLayer2Frame();
      }
      else
      {
        HandleUdpFrame();
      }
    }
    catch (const std::exception& e)
    {
      if (!running)
      {
        break;
      }
      // Keep the loop alive for non-fatal errors
      // (parse failures, transient socket hiccups).
      // euhm do i throw when udp or eth is failing...
      std::cerr
          << "Error: "
          << e.what()
          << "\n";
      continue;
    }
  }
}
 
void AlarmListener::HandleLayer2Frame()
{
  if (l2Sock == nullptr)
  {
    return;
  }
  Bytes data = l2Sock->Recv();
  constexpr std::size_t macHeaderLength = 12U;
  constexpr std::size_t etherTypeLength = 2U;
  constexpr std::size_t frameIdLength = 2U;
 
  if (data.size() < macHeaderLength + etherTypeLength + frameIdLength)
  {
    return;
  }
 
  MacAddress dstMac{};
  MacAddress srcMac{};
  std::size_t offset = 0;
  std::ranges::copy(data.begin() + offset, data.begin() + offset + macAddressLength, dstMac.begin());
  offset += macAddressLength;
  std::ranges::copy(data.begin() + offset, data.begin() + offset + macAddressLength, srcMac.begin());
  offset += macAddressLength;
  if (srcMac != endpoint.deviceMac)
  {
    return;
  }
 
  // Returns the offset at which the EtherType starts.
  const std::size_t etherTypeOffset = SkipVlanTags(data);
 
  if (data.size() < etherTypeOffset + etherTypeLength + frameIdLength)
  {
    return;
  }
  wire::Reader r(data);
  r.SkipTo(etherTypeOffset);
  const auto ethertype = r.U16();
  offset += sizeof(std::uint16_t);
 
  if (ethertype != ETHERTYPE_PROFINET)
  {
    return;
  }
 
  const std::size_t frameIdOffset = etherTypeOffset + etherTypeLength;
  r.SkipTo(frameIdOffset);
  const auto frameId = r.U16();
 
  const std::size_t rtaOffset = frameIdOffset + frameIdLength;
  if (frameId == FRAME_ID_ALARM_HIGH)
  {
    const auto alarmData = Bytes(data.begin() + rtaOffset, data.end());
    ProcessAlarm(alarmData, true, srcMac, std::nullopt);
  }
  else if (frameId == FRAME_ID_ALARM_LOW)
  {
    const auto alarmData = Bytes(data.begin() + rtaOffset, data.end());
    ProcessAlarm(alarmData, false, srcMac, std::nullopt);
  }
}
 
void AlarmListener::HandleUdpFrame()
{
  if (!udpSock)
  {
    return;
  }
  const UdpDatagram datagram = udpSock->RecvFrom(RECEIVE_BUFFER_LENGTH);
 
  if (datagram.data.empty() || datagram.data.size() < 28)
  {
    return;
  }
 
  ProcessAlarm(datagram.data, std::nullopt, std::nullopt, datagram.source);
}
 
void AlarmListener::ProcessAlarm(const Bytes& payload, std::optional<bool> highPriority,
                                 std::optional<MacAddress> srcMac, std::optional<UdpEndpoint> udpSource)
{
  Bytes alarmData;
 
  if (endpoint.transport == 0 && payload.size() >= 12)
  {
    // First check if it is a RTA ERR PDU (protocol error), then for a (error) message
    PNRTAHeader rta;
    try
    {
      rta = PNRTAHeader::Parse(Bytes(payload.begin(), payload.end()));
    }
    catch (const std::exception&)
    {
      return;
    }
    if (rta.alarmDstEndpoint != endpoint.controllerRef)
    {
      return;
    }
    recvSeqNum = rta.sendSeqNum;
    alarmData = Bytes(payload.begin() + 12, payload.end());
 
    auto version = (rta.pduType >> 4) & 0xFF;
    if (version != PNRTAHeader::VERSION_1)
    {
      return;
    }
 
    if (!(rta.addFlags & PNRTAHeader::ADD_FLAGS_TACK))
    {
      return;
    }
    try
    {
      auto error = ParseRtaError(rta);
      if (error.IsRtaProtocolError())
      {
        std::vector<std::function<void(const RtaError&)>> callbacksCopy;
 
        {
          std::scoped_lock lock(callbacksMutex);
          callbacksCopy = rtaAlarmCallbacks;
        }
 
        for (const auto& cb : callbacksCopy)
        {
          try
          {
            cb(error);
          }
          catch (...)
          {
            // A misbehaving callback must not take down the listener thread.
          }
        }
        SendTransportAck(srcMac, highPriority.has_value() ? *highPriority : false);
        return;
      }
    }
    catch (...)
    {
      // An Alarm notification is not a RTA alarm, continue.
    }
    // expected seqnr & SendNack maken
    // ook test:DuplicateNotificationIsNotDispatchedTwice
    // if rta_header.send_seq_num == self._exp_seq_num_o:
    //                 # Retransmission of an already-accepted PDU: re-ack only
    //                 self._send_transport_ack(src_mac, bool(high_priority))
    //                 return
  }
  else
  {
    alarmData = payload;
  }
 
  AlarmNotification alarm;
  try
  {
    alarm = ParseAlarmNotification(alarmData);
  }
  catch (const std::exception&)
  {
    return;
  }
  (void)highPriority; // frame-ID priority is informational only; block_type is authoritative (matches upstream).
 
  SendAck(alarm, srcMac, udpSource);
 
  std::vector<std::function<void(const AlarmNotification&)>> callbacksCopy;
 
  {
    std::scoped_lock lock(callbacksMutex);
    callbacksCopy = alarmNotificationCallbacks;
  }
 
  for (const auto& cb : callbacksCopy)
  {
    try
    {
      cb(alarm);
    }
    catch (...)
    {
      // A misbehaving callback must not take down the listener thread.
    }
  }
}
 
void AlarmListener::SendTransportAck(std::optional<MacAddress> srcMac, bool highPriority)
{
  SendRtaFrame(PNRTAHeader::RTA_TYPE_ACK,
               PNRTAHeader::ADD_FLAGS_WINDOW_1,
               {}, srcMac.value_or(endpoint.deviceMac), highPriority);
}
 
void AlarmListener::SendNack(std::optional<MacAddress> srcMac, bool highPriority)
{
  SendRtaFrame(PNRTAHeader::RTA_TYPE_NACK,
               PNRTAHeader::ADD_FLAGS_WINDOW_1,
               {}, srcMac.value_or(endpoint.deviceMac), highPriority);
}
 
void AlarmListener::SendAck(const AlarmNotification& alarm, std::optional<MacAddress> srcMac, std::optional<UdpEndpoint> udpSource)
{
  const BlockType blockType = alarm.IsHighPriority() ? BlockType::AlarmAcknowledgementHigh : BlockType::AlarmAcknowledgementLow;
 
  PNBlockHeader header;
  header.blockType = blockType;
  header.blockLength = static_cast<std::uint16_t>(PNAlarmAckPDU::kSize - 4);
  header.blockVersionHigh = 0x01;
  header.blockVersionLow = 0x00;
 
  auto alarmSpecifier =
      static_cast<std::uint16_t>((alarm.alarmSequenceNumber & AlarmNotificationBitmask::AlarmSequenceNumber) |
                                 (alarm.channelDiagnosis ? AlarmNotificationBitmask::ChannelDiagnosis : AlarmNotificationBitmask::None) |
                                 (alarm.manufacturerSpecific ? AlarmNotificationBitmask::ManufacturerSpecific : AlarmNotificationBitmask::None) |
                                 (alarm.submoduleDiagnosisState ? AlarmNotificationBitmask::SubmoduleDiagnosisState : AlarmNotificationBitmask::None) |
                                 (alarm.arDiagnosisState ? AlarmNotificationBitmask::ArDiagnosisState : AlarmNotificationBitmask::None));
 
  PNAlarmAckPDU ack;
  std::ranges::copy(header.ToBytes() | std::views::take(blockHeaderLength), ack.blockHeader.data());
  ack.alarmType = alarm.alarmType;
  ack.api = alarm.api;
  ack.slotNumber = alarm.slotNumber;
  ack.subslotNumber = alarm.subslotNumber;
  ack.alarmSpecifier = alarmSpecifier;
  ack.statusPNIO = 0;
 
  const Bytes ackData = ack.ToBytes();
 
  if (endpoint.transport == 0)
  {
    SendLayer2Ack(ackData, srcMac.value_or(endpoint.deviceMac), alarm.IsHighPriority());
  }
  else if (udpSource && udpSource.has_value())
  {
    SendUdpAck(ackData, *udpSource);
  }
}
namespace
{
constexpr std::uint16_t kVlanTpid = 0x8100;
constexpr std::uint16_t kVlanPcp6Vid0 = 0xC000;
constexpr std::uint16_t kVlanPcp5Vid0 = 0xA000;
} // namespace
void AlarmListener::SendLayer2Ack(const Bytes& ackData, const MacAddress& dstMac, bool highPriority)
{
  SendRtaFrame(PNRTAHeader::RTA_TYPE_DATA,
               PNRTAHeader::ADD_FLAGS_WINDOW_1 | PNRTAHeader::ADD_FLAGS_TACK,
               ackData, dstMac, highPriority);
}
 
void AlarmListener::SendRtaFrame(const std::uint8_t rtaType,
                                 const std::uint8_t addFlags,
                                 const Bytes& ackData, const MacAddress& dstMac, bool highPriority)
{
  if (l2Sock == nullptr)
  {
    return;
  }
 
  sendSeqNum = static_cast<std::uint16_t>((sendSeqNum + 1) & 0xFFFF);
 
  PNRTAHeader rta;
  rta.alarmDstEndpoint = endpoint.deviceRef;
  rta.alarmSrcEndpoint = endpoint.controllerRef;
  // rta.pduType = static_cast<std::uint8_t>((PNRTAHeader::RTA_TYPE_DATA << 4) | PNRTAHeader::VERSION_1);
  rta.pduType = static_cast<std::uint8_t>((PNRTAHeader::VERSION_1 << 4) | rtaType);
  rta.addFlags = addFlags;
  rta.sendSeqNum = sendSeqNum;
  rta.ackSeqNum = recvSeqNum;
  rta.variablePartLenght = static_cast<std::uint16_t>(ackData.size());
 
  Bytes frame;
  frame.insert(frame.end(), dstMac.begin(), dstMac.end());
  frame.insert(frame.end(), controllerMac.begin(), controllerMac.end());
  // add VLAN tag?
  const std::uint16_t vlanTci = highPriority ? kVlanPcp6Vid0 : kVlanPcp5Vid0;
 
  wire::PutBigEndian(frame, kVlanTpid);
  wire::PutBigEndian(frame, vlanTci);
  wire::PutBigEndian(frame, ETHERTYPE_PROFINET);
 
  const std::uint16_t frameId = highPriority ? FRAME_ID_ALARM_HIGH : FRAME_ID_ALARM_LOW;
  wire::PutBigEndian(frame, frameId);
 
  Bytes rtaBytes = rta.ToBytes();
  frame.insert(frame.end(), rtaBytes.begin(), rtaBytes.end());
  frame.insert(frame.end(), ackData.begin(), ackData.end());
 
  try
  {
    l2Sock->Send(frame);
  }
  catch (const std::exception&)
  {
    // log?
  }
}
 
void AlarmListener::SendUdpAck(const Bytes& ackData, const UdpEndpoint& dstAddr)
{
  if (udpSock == nullptr)
  {
    return;
  }
  udpSock->SendTo(ackData, dstAddr);
}
 
} // namespace profinet