alarmListener.cpp

Go to the documentation of this file.

 
 
#include "profinet/alarmListener.h"
 
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
 
#include <cstring>
 
#include "profinet/exceptions.h"
 
namespace profinet
{
 
AlarmListener::AlarmListener(AlarmEndpoint endpoint,
                             std::optional<MacAddress> controllerMac,
                             std::unique_ptr<IRawEthernetSocket> l2Socket)
    : endpoint(std::move(endpoint)),
      controllerMac(controllerMac.value_or(MacAddress{})),
      l2Sock(std::move(l2Socket))
{
}
 
AlarmListener::~AlarmListener()
{
  Stop();
}
 
void AlarmListener::AddCallback(std::function<void(const AlarmNotification&)> callback)
{
  if (!callback)
  {
    return;
  }
  const std::lock_guard<std::mutex> lock(callbacksMutex);
  callbacks.push_back(std::move(callback));
}
 
void AlarmListener::RemoveCallback(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(callbacks, [fnToMatch](const auto& cb)
  {
    auto target = cb.template target<void (*)(const AlarmNotification&)>();
    return target && *target == fnToMatch;
  });
}
 
void AlarmListener::Start()
{
  if (running)
  {
    return;
  }
 
  running = true;
 
  if (endpoint.transport == 0)
  {
    if (l2Sock == nullptr)
    {
      l2Sock = std::make_unique<EthernetSocket>(endpoint.interface, ETHERTYPE_PROFINET);
    }
    static constexpr uint16_t timeoutMs = 1000;
    l2Sock->SetTimeout(std::chrono::milliseconds(timeoutMs));
  }
  else
  {
    udpFd = ::socket(AF_INET, SOCK_DGRAM, 0);
    if (udpFd < 0)
    {
      throw SocketError("Failed to create UDP alarm socket");
    }
    int reuse = 1;
    ::setsockopt(udpFd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
 
    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port = htons(ALARM_UDP_PORT);
    addr.sin_addr.s_addr = INADDR_ANY;
    if (::bind(udpFd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
    {
      const int err = errno;
      ::close(udpFd);
      udpFd = -1;
      running = false;
      if (err == EACCES || err == EPERM)
      {
        throw PermissionDeniedError("Raw socket requires root/admin privileges");
      }
      throw SocketError(std::string("Failed to bind UDP alarm socket: ") + std::strerror(err));
    }
    timeval tv{};
    tv.tv_sec = 1;
    ::setsockopt(udpFd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
  }
 
  thread = std::thread([this]
  { ListenLoop(); });
}
 
void AlarmListener::Stop()
{
  if (!running)
  {
    return;
  }
 
  running = false;
 
  if (thread.joinable())
  {
    thread.join();
  }
 
  l2Sock.reset();
 
  if (udpFd >= 0)
  {
    ::close(udpFd);
    udpFd = -1;
  }
}
 
void AlarmListener::ListenLoop()
{
  while (running)
  {
    try
    {
      if (endpoint.transport == 0)
      {
        HandleLayer2Frame();
      }
      else
      {
        HandleUdpFrame();
      }
    }
    catch (const std::exception&)
    {
      if (!running)
      {
        break;
      }
      // Matches alarm_listener.py: log and keep the loop alive for
      // non-fatal errors (parse failures, transient socket hiccups).
      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;
  }
 
  auto ethertype = static_cast<std::uint16_t>((data[etherTypeOffset] << OneOctetShift) | data[etherTypeOffset + 1]);
 
  if (ethertype != ETHERTYPE_PROFINET)
  {
    return;
  }
  const std::size_t frameIdOffset = etherTypeOffset + etherTypeLength;
  auto frameId = static_cast<std::uint16_t>((data[frameIdOffset] << OneOctetShift) | data[frameIdOffset + 1]);
  // offset += 2;
  const std::size_t rtaOffset = frameIdOffset + frameIdLength;
  // offset = 1; //(so we are 1 based not 0 based, so 16!)
 
  if (frameId == FRAME_ID_ALARM_HIGH)
  {
    ProcessAlarm(Bytes(data.begin() + rtaOffset, data.end()), true, srcMac);
  }
  else if (frameId == FRAME_ID_ALARM_LOW)
  {
    ProcessAlarm(Bytes(data.begin() + rtaOffset, data.end()), false, srcMac);
  }
}
 
void AlarmListener::HandleUdpFrame()
{
  if (udpFd < 0)
  {
    return;
  }
  Bytes buf(RECEIVE_BUFFER_LENGTH);
  sockaddr_in from{};
  socklen_t fromLen = sizeof(from);
  const ssize_t n = ::recvfrom(udpFd, buf.data(), buf.size(), 0, reinterpret_cast<sockaddr*>(&from), &fromLen);
  if (n < 0)
  {
    if (errno == EAGAIN || errno == EWOULDBLOCK)
    {
      return;
    }
    if (running)
    {
      throw SocketError(std::string("Alarm listener socket error: ") + std::strerror(errno));
    }
    return;
  }
  buf.resize(static_cast<std::size_t>(n));
  if (buf.size() < 28)
  {
    return;
  }
 
  lastUdpSrc = from;
  ProcessAlarm(buf, std::nullopt, std::nullopt);
}
 
void AlarmListener::ProcessAlarm(const Bytes& payload, std::optional<bool> highPriority,
                                 std::optional<MacAddress> srcMac)
{
  Bytes alarmData;
 
  if (endpoint.transport == 0 && payload.size() >= 12)
  {
    PNRTAHeader rta;
    try
    {
      rta = PNRTAHeader::Parse(Bytes(payload.begin(), payload.begin() + 12));
    }
    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) == 0)
    {
      return;
    }
    // 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);
 
  std::vector<std::function<void(const AlarmNotification&)>> callbacksCopy;
 
  {
    std::scoped_lock lock(callbacksMutex);
    callbacksCopy = callbacks;
  }
 
  for (const auto& cb : callbacksCopy)
  {
    try
    {
      cb(alarm);
    }
    catch (...)
    {
      // A misbehaving callback must not take down the listener thread.
    }
  }
}
 
void AlarmListener::SendAck(const AlarmNotification& alarm, std::optional<MacAddress> srcMac)
{
  try
  {
    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 & 0x07FF) |
                                   (alarm.channelDiagnosis ? 0x0800 : 0) |
                                   (alarm.manufacturerSpecific ? 0x1000 : 0) |
                                   (alarm.submoduleDiagnosisState ? 0x2000 : 0) |
                                   (alarm.arDiagnosisState ? 0x4000 : 0));
 
    PNAlarmAckPDU ack;
    std::memcpy(ack.blockHeader.data(), header.ToBytes().data(), blockHeaderLenght);
    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 (lastUdpSrc)
    {
      SendUdpAck(ackData, *lastUdpSrc);
    }
  }
  catch (const std::exception&)
  {
    // Best-effort ack, matches alarm_listener.py's catch-and-log.
  }
}
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)
{
  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) | PNRTAHeader::RTA_TYPE_DATA);
  rta.addFlags = PNRTAHeader::ADD_FLAGS_WINDOW_1 | PNRTAHeader::ADD_FLAGS_TACK;
  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;
 
  frame.push_back(static_cast<std::uint8_t>(kVlanTpid >> OneOctetShift));
  frame.push_back(static_cast<std::uint8_t>(kVlanTpid & LowByteMask));
  frame.push_back(static_cast<std::uint8_t>(vlanTci >> OneOctetShift));
  frame.push_back(static_cast<std::uint8_t>(vlanTci & LowByteMask));
 
  frame.push_back(static_cast<std::uint8_t>(ETHERTYPE_PROFINET >> OneOctetShift));
  frame.push_back(static_cast<std::uint8_t>(ETHERTYPE_PROFINET & LowByteMask));
  const std::uint16_t frameId = highPriority ? FRAME_ID_ALARM_HIGH : FRAME_ID_ALARM_LOW;
  frame.push_back(static_cast<std::uint8_t>(frameId >> OneOctetShift));
  frame.push_back(static_cast<std::uint8_t>(frameId & LowByteMask));
  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&)
  {
    // matches alarm_listener.py's catch-and-log
  }
}
 
void AlarmListener::SendUdpAck(const Bytes& ackData, const sockaddr_in& dstAddr) const
{
  if (udpFd < 0)
  {
    return;
  }
  ::sendto(udpFd, ackData.data(), ackData.size(), 0, reinterpret_cast<const sockaddr*>(&dstAddr),
           sizeof(dstAddr));
}
 
} // namespace profinet