Fixes and improvements for Linux

score/time_daemon/src/application/svt/BUILD

@@ -41,15 +41,15 @@ load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER
             "svt_handler",
             False,
             [
-                "//score/time_daemon/src/ptp_machine/shm:gptp_shm_machine",
+                "//score/time_daemon/src/ptp_machine:shm_ptp_machine",
                 "//score/time_daemon/src/verification_machine/svt:svt_verification_machine",
             ],
         ),
         (
             "svt_handler_for_utests",
             True,
             [
-                "//score/time_daemon/src/ptp_machine/shm:gptp_shm_machine",
+                "//score/time_daemon/src/ptp_machine:shm_ptp_machine",
                 "//score/time_daemon/src/verification_machine/svt:svt_verification_machine_for_utests",
             ],
         ),

score/time_daemon/src/ptp_machine/BUILD

@@ -19,6 +19,12 @@ alias(
     visibility = ["//score/time_daemon:__subpackages__"],
 )
 
+alias(
+    name = "shm_ptp_machine",
+    actual = "//score/time_daemon/src/ptp_machine/shm:gptp_shm_machine",
+    visibility = ["//score/time_daemon:__subpackages__"],
+)
+
 cc_unit_test_suites_for_host_and_qnx(
     name = "unit_test_suite",
     test_suites_from_sub_packages = [

score/time_slave/src/application/time_slave.cpp

@@ -15,6 +15,7 @@
 #include "score/mw/log/logging.h"
 #include "score/time_slave/src/common/logging_contexts.h"
 
+#include <iostream>
 #include <thread>
 
 namespace score
@@ -28,12 +29,25 @@ namespace
 constexpr std::int32_t kInitSuccess = 0;
 constexpr std::int32_t kInitFailure = -1;
 
+void PrintUsage(const std::string& app_name)
+{
+    std::cout << "Usage: " << app_name
+              << " [options]\n"
+                 "Options:\n"
+                 "  -h, --help Print this message and exit.\n"
+                 "  -i, --interface <iface> Define the ethernet interface to be used (default is \""
+              << details::GptpEngineOptions().iface_name << "\").\n";
+}
+
 }  // namespace
 
 TimeSlave::TimeSlave() = default;
 
-std::int32_t TimeSlave::Initialize(const score::mw::lifecycle::ApplicationContext& /*context*/)
+std::int32_t TimeSlave::Initialize(const score::mw::lifecycle::ApplicationContext& context)
 {
+    if (!ParseAndApplyCmdLineArgs(context))
+        return kInitFailure;
+
     engine_ = std::make_unique<details::GptpEngine>(opts_);
 
     if (!engine_->Initialize())
@@ -52,6 +66,39 @@ std::int32_t TimeSlave::Initialize(const score::mw::lifecycle::ApplicationContex
     return kInitSuccess;
 }
 
+bool TimeSlave::ParseAndApplyCmdLineArgs(const score::mw::lifecycle::ApplicationContext& context)
+{
+    const auto& args = context.get_arguments();
+    auto iter = args.cbegin();
+    const auto& app_name = *iter;
+
+    for (++iter; iter != args.cend(); ++iter)
+    {
+        if (*iter == "-h" || *iter == "--help")
+        {
+            PrintUsage(app_name);
+            return false;
+        }
+        else if (*iter == "-i" || *iter == "--interface")
+        {
+            ++iter;
+            if (iter == args.cend())
+            {
+                PrintUsage(app_name);
+                return false;
+            }
+            opts_.iface_name = *iter;
+        }
+        else
+        {
+            std::cerr << "Unknown argument \"" << *iter << "\"." << std::endl;
+            PrintUsage(app_name);
+            return false;
+        }
+    }
+    return true;
+}
+
 std::int32_t TimeSlave::Run(const score::cpp::stop_token& token)
 {
     constexpr auto kPublishInterval = std::chrono::milliseconds{50};

score/time_slave/src/application/time_slave.h

@@ -48,6 +48,8 @@ class TimeSlave final : public score::mw::lifecycle::Application
     std::int32_t Run(const score::cpp::stop_token& token) override;
 
   private:
+    bool ParseAndApplyCmdLineArgs(const score::mw::lifecycle::ApplicationContext& context);
+
     details::GptpEngineOptions opts_;
     std::unique_ptr<details::GptpEngine> engine_;
     details::GptpIpcPublisher publisher_;

score/time_slave/src/gptp/details/BUILD

@@ -67,6 +67,8 @@ cc_library(
         ":os_syscalls",
         ":ptp_types",
         ":raw_socket",
+        "//score/time_slave/src/common:logging_contexts",
+        "@score_baselibs//score/mw/log:frontend",
     ],
 )
 

score/time_slave/src/gptp/gptp_engine.cpp

@@ -32,7 +32,7 @@ namespace details
 namespace
 {
 
-constexpr int kRxTimeoutMs = 100;  // poll timeout; keeps RxLoop responsive to shutdown
+constexpr int kRxTimeoutMs = 500;  // poll timeout; keeps RxLoop responsive to shutdown
 constexpr int kRxBufferSize = 2048;
 
 }  // namespace
@@ -70,13 +70,17 @@ GptpEngine::~GptpEngine() noexcept
 
 bool GptpEngine::Initialize()
 {
+    mw::log::LogInfo(kTimeSlaveAppContext) << "GptpEngine: Initializing on " << opts_.iface_name;
     if (running_.load(std::memory_order_acquire))
+    {
+        mw::log::LogWarn(kTimeSlaveAppContext) << "GptpEngine::Initialize: Engine is already running";
         return true;
+    }
 
     if (!identity_->Resolve(opts_.iface_name))
     {
         score::mw::log::LogError(kTimeSlaveAppContext)
-            << "GptpEngine: failed to resolve ClockIdentity for " << opts_.iface_name;
+            << "GptpEngine::Initialize: Failed to resolve ClockIdentity for " << opts_.iface_name;
         return false;
     }
 
@@ -85,14 +89,14 @@ bool GptpEngine::Initialize()
     if (!socket_->Open(opts_.iface_name))
     {
         score::mw::log::LogError(kTimeSlaveAppContext)
-            << "GptpEngine: failed to open raw socket on " << opts_.iface_name;
+            << "GptpEngine::Initialize: Failed to open raw socket on " << opts_.iface_name;
         return false;
     }
 
     if (!socket_->EnableHwTimestamping())
     {
-        score::mw::log::LogWarn(kTimeSlaveAppContext)
-            << "GptpEngine: HW timestamping not available on " << opts_.iface_name << ", falling back to SW timestamps";
+        score::mw::log::LogWarn(kTimeSlaveAppContext) << "GptpEngine::Initialize: HW timestamping not available on "
+                                                      << opts_.iface_name << ", falling back to SW timestamps";
     }
 
     running_.store(true, std::memory_order_release);
@@ -108,7 +112,7 @@ bool GptpEngine::Initialize()
     catch (const std::system_error& e)
     {
         score::mw::log::LogError(kTimeSlaveAppContext)
-            << "GptpEngine: failed to create RxThread: " << std::string_view{e.what()};
+            << "GptpEngine::Initialize: Failed to create RxThread: " << std::string_view{e.what()};
         running_.store(false, std::memory_order_release);
         socket_->Close();
         return false;
@@ -123,7 +127,7 @@ bool GptpEngine::Initialize()
     catch (const std::system_error& e)
     {
         score::mw::log::LogError(kTimeSlaveAppContext)
-            << "GptpEngine: failed to create PdelayThread: " << std::string_view{e.what()};
+            << "GptpEngine::Initialize: Failed to create PdelayThread: " << std::string_view{e.what()};
         Deinitialize();
         return false;
     }
@@ -258,11 +262,13 @@ void GptpEngine::HandlePacket(const std::uint8_t* frame, int len, const ::timesp
     switch (msg.msgtype)
     {
         case kPtpMsgtypePdelayReq:
+            mw::log::LogDebug(kGPtpMachineContext) << "PdelayReq message received, hw_ts=" << hw_ts.ns << " ns";
             if (msg.ptpHdr.domainNumber == opts_.domain_number)
                 SendPDelayResponseAndFollowUp(msg, hw_ts);
             break;
 
         case kPtpMsgtypeSync:
+            mw::log::LogDebug(kGPtpMachineContext) << "Sync message received, hw_ts=" << hw_ts.ns << " ns";
             if (msg.ptpHdr.domainNumber != opts_.domain_number)
                 break;
             msg.recvHardwareTS = hw_ts;
@@ -271,6 +277,7 @@ void GptpEngine::HandlePacket(const std::uint8_t* frame, int len, const ::timesp
             break;
 
         case kPtpMsgtypeFollowUp:
+            mw::log::LogDebug(kGPtpMachineContext) << "FollowUp message received, hw_ts=" << hw_ts.ns << " ns";
             if (msg.ptpHdr.domainNumber != opts_.domain_number)
                 break;
             msg.parseMessageTs = TimestampToTmv(msg.follow_up.preciseOriginTimestamp);
@@ -295,19 +302,24 @@ void GptpEngine::HandlePacket(const std::uint8_t* frame, int len, const ::timesp
             break;
 
         case kPtpMsgtypePdelayResp:
+            mw::log::LogDebug(kGPtpMachineContext) << "PdelayResp message received, hw_ts=" << hw_ts.ns << " ns";
             msg.recvHardwareTS = hw_ts;
             msg.parseMessageTs = TimestampToTmv(msg.pdelay_resp.requestReceiptTimestamp);
             if (pdelay_)
                 pdelay_->OnResponse(msg);
             break;
 
         case kPtpMsgtypePdelayRespFollowUp:
+            mw::log::LogDebug(kGPtpMachineContext)
+                << "PdelayRespFollowUp message received, hw_ts=" << hw_ts.ns << " ns";
             msg.parseMessageTs = TimestampToTmv(msg.pdelay_resp_fup.responseOriginReceiptTimestamp);
             if (pdelay_)
                 pdelay_->OnResponseFollowUp(msg);
             break;
 
         default:
+            mw::log::LogDebug(kGPtpMachineContext)
+                << "Unhandled message received: " << static_cast<int>(msg.msgtype) << ", hw_ts=" << hw_ts.ns << " ns";
             break;
     }
 }

score/time_slave/src/gptp/platform/linux/raw_socket.cpp

@@ -12,7 +12,11 @@
  ********************************************************************************/
 #include "score/time_slave/src/gptp/details/raw_socket_impl.h"
 
+#include "score/mw/log/logging.h"
+#include "score/time_slave/src/common/logging_contexts.h"
+
 #include <arpa/inet.h>
+#include <linux/filter.h>
 #include <linux/if_ether.h>
 #include <linux/net_tstamp.h>
 #include <linux/sockios.h>
@@ -64,32 +68,80 @@ bool RawSocketImpl::Open(const std::string& iface)
 {
     Close();
 
-    const int fd = sys_->socket_call(AF_PACKET, SOCK_RAW, htons(ETH_P_1588));
+    const int fd = sys_->socket_call(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
     if (fd < 0)
+    {
+        score::mw::log::LogError(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to create raw socket endpoint (errno=" << errno << ")";
         return false;
+    }
 
     ::ifreq ifr{};
     std::strncpy(ifr.ifr_name, iface.c_str(), IFNAMSIZ - 1);
     ifr.ifr_name[IFNAMSIZ - 1] = '\0';
     if (sys_->ioctl_call(fd, SIOCGIFINDEX, &ifr) < 0)
     {
         sys_->close_call(fd);
+        score::mw::log::LogError(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to manipulate raw socket endpoint (errno=" << errno << ")";
         return false;
     }
 
     ::sockaddr_ll sa{};
     sa.sll_family = AF_PACKET;
-    sa.sll_protocol = htons(ETH_P_1588);
+    sa.sll_protocol = htons(ETH_P_ALL);
     sa.sll_ifindex = ifr.ifr_ifindex;
     if (sys_->bind_call(fd, reinterpret_cast<::sockaddr*>(&sa), sizeof(sa)) < 0)
     {
         sys_->close_call(fd);
+        score::mw::log::LogError(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to bind raw socket endpoint to interface " << iface << " (errno=" << errno
+            << ")";
         return false;
     }
 
     // SO_BINDTODEVICE: best-effort, don't fail if it doesn't work
     (void)sys_->setsockopt_call(fd, SOL_SOCKET, SO_BINDTODEVICE, iface.c_str(), static_cast<socklen_t>(iface.size()));
 
+    // Enable promiscuous mode so the NIC passes all frames (including PTP multicast) to the kernel.
+    ::packet_mreq mr{};
+    mr.mr_ifindex = ifr.ifr_ifindex;
+    mr.mr_type = PACKET_MR_PROMISC;
+    if (sys_->setsockopt_call(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) < 0)
+    {
+        score::mw::log::LogWarn(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to set promiscuous mode (errno=" << errno << ")";
+    }
+
+    // BPF filter: pass only frames with EtherType 0x88F7 (PTP/gPTP). This is fine ss gPTP frames
+    // are n9ot VLAN (nor priority) tagged. The BPF filter runs in-kernel before delivery
+    // to userspace, so non-PTP frames are dropped with zero overhead in the application.
+    static const ::sock_filter kPtpBpfCode[] = {
+        BPF_STMT(BPF_LD | BPF_H | BPF_ABS, 12),                    // load EtherType
+        BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ETH_P_1588, 1, 0),     // == 0x88F7 → PASS
+        BPF_STMT(BPF_RET | BPF_K, 0U),                             // FAIL: drop
+        BPF_STMT(BPF_RET | BPF_K, static_cast<unsigned int>(-1)),  // PASS: accept
+    };
+    ::sock_fprog prog{};
+    prog.len = static_cast<unsigned short>(sizeof(kPtpBpfCode) / sizeof(kPtpBpfCode[0]));
+    prog.filter = const_cast<::sock_filter*>(kPtpBpfCode);
+    if (sys_->setsockopt_call(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)) < 0)
+    {
+        score::mw::log::LogWarn(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to set BPF filter (errno=" << errno << ")";
+    }
+
+    // Enable kernel layer software timestamps as a baseline, so we get at least some timestamp
+    // even if hw timestamping isn't available or fails to configure. The HW timestamp will come
+    // in the same control message but different field, so the caller can use it if present and
+    // fall back to SW timestamp if not.
+    const int enable = 1;
+    if (sys_->setsockopt_call(fd, SOL_SOCKET, SO_TIMESTAMPNS, &enable, sizeof(enable)) < 0)
+    {
+        score::mw::log::LogWarn(kTimeSlaveAppContext)
+            << "RawSocket::Open: Failed to enable SO_TIMESTAMPNS (errno=" << errno << ")";
+    }
+
     fd_.store(fd, std::memory_order_release);
     iface_ = iface;
     return true;
@@ -168,7 +220,14 @@ int RawSocketImpl::Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts
                 continue;
             const auto* ts = reinterpret_cast<const ::timespec*>(CMSG_DATA(cm));
             if (ts[2].tv_sec != 0 || ts[2].tv_nsec != 0)
+            {
                 hwts = ts[2];
+                break;  // Prefer raw HW timestamp if available
+            }
+        }
+        else if (cm->cmsg_type == SO_TIMESTAMPNS)
+        {
+            memcpy(&hwts, CMSG_DATA(cm), sizeof(hwts));
         }
     }
     return len;