util.cpp

Go to the documentation of this file.

 
 
#include "profinet/util.h"
 
#include <arpa/inet.h>
#include <fcntl.h>
#include <net/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
 
#include <algorithm>
#include <ranges>
 
#include "profinet/exceptions.h"
 
extern "C"
{
#include <linux/if_packet.h>
}
 
#include <cerrno>
#include <cstdio>
#include <cstring>
#include <sstream>
#include <utility>
 
namespace profinet
{
// =============================================================================
// Byte Conversion Utilities
// =============================================================================
std::string Hex4(std::uint16_t value)
{
  return std::format("0x{:04X}", value);
}
 
std::string Hex2(std::uint8_t value)
{
  return std::format("0x{:02X}", value);
}
 
std::vector<std::uint8_t> ToVec(const std::string& input)
{
  return {input.begin(), input.end()};
}
 
std::string HexFallback(const char* widthPrefix, std::uint32_t value, int digitWidth)
{
  // {}         -> The prefix string
  // {:0{}X}    -> Hexadecimal (X), zero-padded (0), with a dynamic width ({})
  return std::format("{}(0x{:0{}X})", widthPrefix, value, digitWidth);
}
 
// =============================================================================
// Address Conversion Utilities
// =============================================================================
 
std::size_t SkipVlanTags(std::span<const std::uint8_t> frame) noexcept
{
  std::size_t offset = 12;
 
  while (frame.size() >= offset + 4)
  {
    // Read 16-bit TPID in network byte order (big-endian)
    const std::uint16_t tpid = (static_cast<std::uint16_t>(frame[offset]) << 8) | frame[offset + 1];
 
    // 802.1Q (0x8100), 802.1ad (0x88A8), 802.1Q double-tagging legacy (0x9100)
    if (tpid == 0x8100 || tpid == 0x88A8 || tpid == 0x9100)
    {
      offset += 4;
    }
    else
    {
      break;
    }
  }
 
  return offset;
}
 
std::string ToHex(const std::uint8_t* data, std::size_t len)
{
  std::string out;
  out.reserve(len * 2);
  static const char* digits = "0123456789abcdef";
  for (std::size_t i = 0; i < len; ++i)
  {
    out.push_back(digits[data[i] >> 4]);
    out.push_back(digits[data[i] & 0x0F]);
  }
  return out;
}
 
std::string ToHex(const std::vector<std::uint8_t>& data)
{
  return ToHex(data.data(), data.size());
}
 
MacAddress String2Mac(const std::string& macStr)
{
  // Strict structural constants for a standard MAC address
  static constexpr std::size_t expectedStringLength = 17;
  static constexpr std::size_t singleByteHexLength = 2;
  static constexpr int hexBase = 16;
 
  // Explicit array index positions for delimiters
  static constexpr std::array<std::size_t, 5> delimiterIndices = {2, 5, 8, 11, 14};
 
  // Enforce strict total length check
  if (macStr.size() != expectedStringLength)
  {
    throw InvalidMACError(std::format("Invalid MAC address length: '{}'", macStr));
  }
 
  // Validate format
  // We loop through indices from 0 to 16
  bool isValidStructure = true;
  for (std::size_t i = 0; i < expectedStringLength; ++i)
  {
    // Check if current index 'i' matches one of our delimiter positions
    const bool isDelimiterIdx = std::ranges::any_of(delimiterIndices,
                                                    [i](std::size_t d)
    { return d == i; });
 
    if (isDelimiterIdx)
    {
      if (macStr[i] != ':')
      {
        isValidStructure = false;
        break;
      }
    }
    else
    {
      if (std::isxdigit(static_cast<unsigned char>(macStr[i])) == 0)
      {
        isValidStructure = false;
        break;
      }
    }
  }
 
  if (!isValidStructure)
  {
    throw InvalidMACError(std::format("Invalid MAC address characters or format: '{}'", macStr));
  }
 
  MacAddress mac{};
  std::string_view macView{macStr};
 
  // 3. Parse the hex pairs directly into the destination array using string_view
  static constexpr std::size_t hexPairStride = 3; // Length of "xx:" segment
  for (std::size_t i = 0; i < macAddressLength; ++i)
  {
    const std::size_t offset = i * hexPairStride;
    const std::string_view hexPair = macView.substr(offset, singleByteHexLength);
 
    // std::stoul is fully safe here because we pre-validated all digits above
    mac.at(i) = static_cast<std::uint8_t>(std::stoul(std::string(hexPair), nullptr, hexBase));
  }
 
  return mac;
}
 
std::string Mac2String(const MacAddress& mac)
{
  return Mac2String(mac.data(), mac.size());
}
 
std::string Mac2String(const std::uint8_t* macBytes, std::size_t len)
{
  if (len != 6)
  {
    throw InvalidMACError("MAC address must be 6 bytes, got " + std::to_string(len));
  }
  // Extract the formatted MAC address into a dedicated string variable
  const std::string macStr = std::format("{:02x}:{:02x}:{:02x}:{:02x}:{:02x}:{:02x}",
                                         macBytes[0], macBytes[1], macBytes[2],
                                         macBytes[3], macBytes[4], macBytes[5]);
  return macStr;
}
 
std::string String2Ip(std::span<const std::uint8_t> ipBytes)
{
  if (ipBytes.size() < 4)
  {
    throw InvalidIPError("IP address must be at least 4 bytes, got " + std::to_string(ipBytes.size()));
  }
 
  return std::to_string(ipBytes[0]) + "." + std::to_string(ipBytes[1]) + "." +
         std::to_string(ipBytes[2]) + "." + std::to_string(ipBytes[3]);
}
 
std::string String2Ip(const std::uint8_t* ipBytes, std::size_t len)
{
  return String2Ip(std::span<const std::uint8_t>(ipBytes, len));
}
 
std::array<std::uint8_t, 4> Ip2String(const std::string& ipStr)
{
  if (ipStr.empty())
  {
    throw InvalidIPError("IP address cannot be empty");
  }
 
  in_addr addr{};
  if (::inet_pton(AF_INET, ipStr.c_str(), &addr) != 1)
  {
    throw InvalidIPError("Invalid IP address: '" + ipStr + "'");
  }
  std::array<std::uint8_t, 4> out{};
  std::memcpy(out.data(), &addr.s_addr, 4);
  return out;
}
 
std::string DecodeBytes(const std::uint8_t* data, std::size_t len)
{
  std::size_t end = len;
  while (end > 0 && data[end - 1] == 0)
  {
    --end;
  }
  // NOLINTNEXTLINE(modernize-return-braced-init-list)
  return std::string(reinterpret_cast<const char*>(data), end);
}
 
std::string DecodeBytes(const std::vector<std::uint8_t>& data)
{
  return DecodeBytes(data.data(), data.size());
}
 
// =============================================================================
// Socket Utilities (Linux AF_PACKET)
// =============================================================================
 
MacAddress GetMac(const std::string& ifname)
{
  if (ifname.empty())
  {
    throw SocketError("Interface name cannot be empty");
  }
 
  const int s = ::socket(AF_INET, SOCK_DGRAM, 0);
  if (s < 0)
  {
    throw SocketError("Failed to get MAC address for '" + ifname + "': " + std::strerror(errno));
  }
 
  ifreq ifr{};
  std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
  if (::ioctl(s, SIOCGIFHWADDR, &ifr) < 0)
  {
    const int err = errno;
    ::close(s);
    throw SocketError("Failed to get MAC address for '" + ifname + "': " + std::strerror(err));
  }
  ::close(s);
 
  MacAddress mac{};
  std::memcpy(mac.data(), ifr.ifr_hwaddr.sa_data, 6);
  return mac;
}
 
EthernetSocket::EthernetSocket(const std::string& interface, std::uint16_t ethertype)
{
  if (interface.empty())
  {
    throw SocketError("Interface name cannot be empty");
  }
 
  const std::uint16_t proto = ethertype != 0 ? ethertype : static_cast<std::uint16_t>(ETH_P_ALL);
 
  fd = ::socket(AF_PACKET, SOCK_RAW, htons(proto));
  if (fd < 0)
  {
    if (errno == EPERM || errno == EACCES)
    {
      throw PermissionDeniedError("Root privileges required for raw socket access: " +
                                  std::string(std::strerror(errno)));
    }
    throw SocketError("Failed to create socket on '" + interface + "': " + std::strerror(errno));
  }
 
  const unsigned int ifindex = if_nametoindex(interface.c_str());
  if (ifindex == 0)
  {
    const int err = errno;
    ::close(fd);
    fd = -1;
    throw SocketError("Failed to create socket on '" + interface + "': " + std::strerror(err));
  }
 
  sockaddr_ll addr{};
  addr.sll_family = AF_PACKET;
  addr.sll_protocol = htons(proto);
  addr.sll_ifindex = static_cast<int>(ifindex);
 
  if (::bind(fd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
  {
    const int err = errno;
    ::close(fd);
    fd = -1;
    throw SocketError("Failed to create socket on '" + interface + "': " + std::strerror(err));
  }
}
 
EthernetSocket::~EthernetSocket()
{
  if (fd >= 0)
  {
    ::close(fd);
  }
}
 
EthernetSocket::EthernetSocket(EthernetSocket&& other) noexcept
    : fd(other.fd)
{
  other.fd = -1;
}
 
EthernetSocket& EthernetSocket::operator=(EthernetSocket&& other) noexcept
{
  if (this != &other)
  {
    if (fd >= 0)
    {
      ::close(fd);
    }
    fd = other.fd;
    other.fd = -1;
  }
  return *this;
}
 
void EthernetSocket::Send(const std::vector<std::uint8_t>& frame) const
{
  const ssize_t n = ::send(fd, frame.data(), frame.size(), 0);
  if (n < 0 || static_cast<std::size_t>(n) != frame.size())
  {
    throw SocketError(std::string("Failed to send frame: ") + std::strerror(errno));
  }
}
 
void EthernetSocket::SetTimeout(std::chrono::milliseconds timeout) const
{
  timeval tv{};
  tv.tv_sec = static_cast<time_t>(timeout.count() / 1000);
  tv.tv_usec = static_cast<suseconds_t>((timeout.count() % 1000) * 1000);
  if (::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
  {
    throw SocketError(std::string("Failed to set socket timeout: ") + std::strerror(errno));
  }
}
 
std::vector<std::uint8_t> EthernetSocket::Recv() const
{
  std::vector<std::uint8_t> buf(MAX_ETHERNET_FRAME);
  const ssize_t n = ::recv(fd, buf.data(), buf.size(), 0);
  if (n < 0)
  {
    if (errno == EAGAIN || errno == EWOULDBLOCK)
    {
      return {}; // timeout
    }
    throw SocketError(std::string("Socket error during receive: ") + std::strerror(errno));
  }
  buf.resize(static_cast<std::size_t>(n));
  return buf;
}
 
UdpSocket::UdpSocket(const std::string& host, std::uint16_t port,
                     std::chrono::duration<double> timeout)
{
  addrinfo hints{};
  hints.ai_family = AF_INET;
  hints.ai_socktype = SOCK_DGRAM;
  addrinfo* result = nullptr;
 
  const std::string portStr = std::to_string(port);
  if (::getaddrinfo(host.c_str(), portStr.c_str(), &hints, &result) != 0 || result == nullptr)
  {
    throw SocketError("Failed to create UDP socket to " + host + ":" + portStr);
  }
 
  fd = ::socket(result->ai_family, result->ai_socktype, result->ai_protocol);
  if (fd < 0)
  {
    ::freeaddrinfo(result);
    throw SocketError("Failed to create UDP socket to " + host + ":" + portStr + ": " +
                      std::strerror(errno));
  }
 
  timeval tv{};
  tv.tv_sec = static_cast<time_t>(timeout.count());
  static constexpr int microsecondsPerSecond = 1'000'000;
  tv.tv_usec = static_cast<suseconds_t>((timeout.count() - static_cast<double>(tv.tv_sec)) * microsecondsPerSecond);
  ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
 
  if (::connect(fd, result->ai_addr, result->ai_addrlen) < 0)
  {
    const int err = errno;
    ::freeaddrinfo(result);
    ::close(fd);
    fd = -1;
    throw SocketError("Failed to create UDP socket to " + host + ":" + portStr + ": " +
                      std::strerror(err));
  }
  ::freeaddrinfo(result);
}
 
UdpSocket::~UdpSocket()
{
  if (fd >= 0)
  {
    ::close(fd);
  }
}
 
UdpSocket::UdpSocket(UdpSocket&& other) noexcept
    : fd(other.fd)
{
  other.fd = -1;
}
 
UdpSocket& UdpSocket::operator=(UdpSocket&& other) noexcept
{
  if (this != &other)
  {
    if (fd >= 0)
    {
      ::close(fd);
    }
    fd = other.fd;
    other.fd = -1;
  }
  return *this;
}
 
void UdpSocket::Send(const std::vector<std::uint8_t>& data) const
{
  const ssize_t n = ::send(fd, data.data(), data.size(), 0);
  if (n < 0 || static_cast<std::size_t>(n) != data.size())
  {
    throw SocketError(std::string("Failed to send UDP datagram: ") + std::strerror(errno));
  }
}
 
std::vector<std::uint8_t> UdpSocket::Recv(std::size_t maxLen) const
{
  std::vector<std::uint8_t> buf(maxLen);
  const ssize_t n = ::recv(fd, buf.data(), buf.size(), 0);
  if (n < 0)
  {
    if (errno == EAGAIN || errno == EWOULDBLOCK)
    {
      return {};
    }
    throw SocketError(std::string("Socket error during UDP receive: ") + std::strerror(errno));
  }
  buf.resize(static_cast<std::size_t>(n));
  return buf;
}
std::filesystem::path GetExecutableDirectory()
{
  constexpr std::string_view processLink = "/proc/self/exe";
 
  try
  {
    // resolve a symlink to its absolute path
    if (std::filesystem::is_symlink(processLink) || std::filesystem::exists(processLink))
    {
      return std::filesystem::read_symlink(processLink).parent_path();
    }
  }
  catch (const std::filesystem::filesystem_error& e)
  {
    // std::cerr << "Filesystem error: " << e.what() << '\n';
    throw std::runtime_error("Filesystem error: " + std::string(e.what()));
  }
  throw std::runtime_error("Critical error: /proc/self/exe not available on this system");
}
} // namespace profinet