diff --git a/docs/TimeSlave/_assets/gptp_engine_class.puml b/docs/TimeSlave/_assets/gptp_engine_class.puml new file mode 100644 index 0000000..c29842d --- /dev/null +++ b/docs/TimeSlave/_assets/gptp_engine_class.puml @@ -0,0 +1,104 @@ +@startuml +!theme plain + +title gPTP Engine Internal Class Diagram + +legend top left + |= Color |= Description | + | <#LightSalmon> | gPTP Engine core | + | <#Wheat> | Protocol processing | + | <#Lavender> | PHC adjustment | + | <#LightSkyBlue> | Platform abstraction | + | <#Beige> | Instrumentation | +endlegend + +package "score::ts::gptp" { + + class GptpEngine #LightSalmon { + - options_ : GptpEngineOptions + - rx_thread_ : std::thread + - pdelay_thread_ : std::thread + - socket_ : std::unique_ptr + - codec_ : FrameCodec + - parser_ : MessageParser + - sync_sm_ : SyncStateMachine + - pdelay_ : PeerDelayMeasurer + - phc_ : PhcAdjuster + - probe_mgr_ : ProbeManager + - recorder_ : Recorder + - snapshot_mutex_ : std::mutex + - latest_snapshot_ : PtpTimeInfo + + Initialize() : bool + + Deinitialize() : void + + ReadPTPSnapshot() : PtpTimeInfo + } + + interface IRawSocket #LightSkyBlue { + + Open(iface) : bool + + EnableHwTimestamping() : bool + + Recv(buf, timeout_ms) : RecvResult + + Send(buf, hw_ts) : bool + + GetFd() : int + + Close() : void + } + + class "RawSocket\n<>" as LinuxSocket #LightSkyBlue { + AF_PACKET + SO_TIMESTAMPING + } + + class "RawSocket\n<>" as QnxSocket #LightSkyBlue { + QNX raw-socket shim + } + + interface INetworkIdentity #LightSkyBlue { + + Resolve(iface) : bool + + GetClockIdentity() : ClockIdentity + } + + class NetworkIdentity #LightSkyBlue { + Derives EUI-64 from MAC\n(inserts 0xFF 0xFE) + } + + IRawSocket <|.. LinuxSocket + IRawSocket <|.. QnxSocket + INetworkIdentity <|.. NetworkIdentity + + GptpEngine *-- IRawSocket + GptpEngine *-- INetworkIdentity +} + +package "score::ts::gptp::details" { + class FrameCodec #Wheat + class MessageParser #Wheat + class SyncStateMachine #Wheat + class PeerDelayMeasurer #Wheat +} + +package "score::ts::gptp::phc" { + class PhcAdjuster #Lavender +} + +package "score::ts::gptp::instrument" { + class ProbeManager #Beige { + + {static} Instance() : ProbeManager& + + Record(point, data) : void + + SetRecorder(recorder) : void + } + + class Recorder #Beige { + - file_ : std::ofstream + - mutex_ : std::mutex + + Record(entry) : void + } + + ProbeManager --> Recorder +} + +GptpEngine *-- FrameCodec +GptpEngine *-- MessageParser +GptpEngine *-- SyncStateMachine +GptpEngine *-- PeerDelayMeasurer +GptpEngine *-- PhcAdjuster +GptpEngine *-- ProbeManager + +@enduml diff --git a/docs/TimeSlave/_assets/gptp_threading.puml b/docs/TimeSlave/_assets/gptp_threading.puml new file mode 100644 index 0000000..79ee3b2 --- /dev/null +++ b/docs/TimeSlave/_assets/gptp_threading.puml @@ -0,0 +1,51 @@ +@startuml gptp_threading_model + +title gPTP Engine Threading Model + +legend top left + |= Color |= Description | + | <#LightSalmon> | RxThread | + | <#LightSkyBlue> | PdelayThread | + | <#LightCyan> | Main Thread (TimeSlave) | +endlegend + +|#LightCyan| Main Thread +start +:Initialize GptpEngine; +:Start RxThread; +:Start PdelayThread; + +fork + |#LightSalmon| RxThread + repeat + :Wait for gPTP frame; + :Recv Sync frame; + :Parse + SyncStateMachine\nstore Sync timestamp; + :Recv FollowUp frame; + :Parse + SyncStateMachine\ncompute offset & rate ratio; + :Update latest_snapshot_\n(mutex protected); + repeat while (stop_token?) + stop + +fork again + |#LightSkyBlue| PdelayThread + repeat + :Sleep(pdelay_interval_ms); + :Send PDelayReq; + :Recv PDelayResp; + :Recv PDelayRespFollowUp\ncompute path delay; + :Update PDelayResult; + repeat while (stop_token?) + stop + +fork again + |#LightCyan| Main Thread + repeat + :ReadPTPSnapshot(); + :Publish PtpTimeInfo\nvia GptpIpcPublisher; + repeat while (stop_token?) + stop + +end fork + +@enduml diff --git a/docs/TimeSlave/_assets/ipc_channel.puml b/docs/TimeSlave/_assets/ipc_channel.puml new file mode 100644 index 0000000..bc7d9b4 --- /dev/null +++ b/docs/TimeSlave/_assets/ipc_channel.puml @@ -0,0 +1,52 @@ +@startuml +!theme plain + +title libTSClient Shared Memory IPC + +legend top left + |= Color |= Description | + | <#LightPink> | IPC components | + | <#LightCyan> | Shared memory region | +endlegend + +package "TimeSlave Process" { + class GptpIpcPublisher #LightPink { + - region_ : GptpIpcRegion* + - fd_ : int + + Init(name) : bool + + Publish(info) : void + + Destroy() : void + } +} + +package "Shared Memory" { + class GptpIpcRegion <> #LightCyan { + + magic : uint32_t = 0x47505440 + + seq : std::atomic + + data : PtpTimeInfo + -- + 64-byte aligned for\ncache line efficiency + } +} + +package "TimeDaemon Process" { + class GptpIpcReceiver #LightPink { + - region_ : const GptpIpcRegion* + - fd_ : int + + Init(name) : bool + + Receive() : std::optional + + Close() : void + } +} + +GptpIpcPublisher --> GptpIpcRegion : "shm_open(O_CREAT)\nmmap(PROT_WRITE)" +GptpIpcReceiver --> GptpIpcRegion : "shm_open(O_RDONLY)\nmmap(PROT_READ)" + +note right of GptpIpcRegion + **Seqlock Protocol:** + Writer: seq++ → memcpy → seq++ + Reader: read seq (even) → memcpy → check seq + Retry up to 20 times on torn read +end note + +@enduml diff --git a/docs/TimeSlave/_assets/ipc_sequence.puml b/docs/TimeSlave/_assets/ipc_sequence.puml new file mode 100644 index 0000000..7e7bda3 --- /dev/null +++ b/docs/TimeSlave/_assets/ipc_sequence.puml @@ -0,0 +1,46 @@ +@startuml +!theme plain + +title libTSClient Seqlock IPC Protocol + +participant "TimeSlave\n(GptpIpcPublisher)" as PUB #LightPink +participant "SharedMemory\n(GptpIpcRegion)" as SHM #LightCyan +participant "TimeDaemon\n(GptpIpcReceiver)" as RCV #LightPink + +== Initialization == + +PUB -> SHM : shm_open("/gptp_ptp_info", O_CREAT | O_RDWR) +PUB -> SHM : ftruncate(sizeof(GptpIpcRegion)) +PUB -> SHM : mmap(PROT_READ | PROT_WRITE) +PUB -> SHM : write magic = 0x47505440 + +... + +RCV -> SHM : shm_open("/gptp_ptp_info", O_RDONLY) +RCV -> SHM : mmap(PROT_READ) +RCV -> SHM : verify magic == 0x47505440 + +== Publish (Writer Side) == + +PUB -> SHM : seq.fetch_add(1, release) // seq becomes odd +PUB -> SHM : memcpy(data, &info, sizeof) +PUB -> SHM : seq.fetch_add(1, release) // seq becomes even + +== Receive (Reader Side) == + +loop up to 20 retries + RCV -> SHM : s1 = seq.load(acquire) + alt s1 is odd (write in progress) + RCV -> RCV : retry + else s1 is even + RCV -> SHM : memcpy(&local, data, sizeof) + RCV -> SHM : s2 = seq.load(acquire) + alt s1 == s2 + RCV --> RCV : return PtpTimeInfo + else s1 != s2 (torn read) + RCV -> RCV : retry + end + end +end + +@enduml diff --git a/docs/TimeSlave/_assets/timeslave_class.puml b/docs/TimeSlave/_assets/timeslave_class.puml new file mode 100644 index 0000000..e612d6f --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_class.puml @@ -0,0 +1,125 @@ +@startuml +!theme plain + +title TimeSlave Class Diagram + +legend top left + |= Color |= Description | + | <#LightCyan> | TimeSlave application | + | <#LightSalmon> | gPTP Engine core | + | <#Wheat> | Protocol processing | + | <#Lavender> | PHC adjustment | + | <#LightPink> | IPC components | + | <#Beige> | Data structures | +endlegend + +package "score::ts" { + + class TimeSlave #LightCyan { + - engine_ : GptpEngine + - publisher_ : GptpIpcPublisher + - clock_ : HighPrecisionLocalSteadyClock + + Initialize() : score::cpp::expected + + Run(stop_token) : score::cpp::expected + + Deinitialize() : score::cpp::expected + } + + class GptpEngine #LightSalmon { + - options_ : GptpEngineOptions + - rx_thread_ : std::thread + - pdelay_thread_ : std::thread + - sync_sm_ : SyncStateMachine + - pdelay_ : PeerDelayMeasurer + - socket_ : IRawSocket + - codec_ : FrameCodec + - parser_ : MessageParser + - phc_ : PhcAdjuster + - snapshot_mutex_ : std::mutex + - latest_snapshot_ : PtpTimeInfo + + Initialize() : bool + + Deinitialize() : void + + ReadPTPSnapshot() : PtpTimeInfo + - RxThreadFunc(stop_token) : void + - PdelayThreadFunc(stop_token) : void + } + + struct GptpEngineOptions #Beige { + + interface_name : std::string + + pdelay_interval_ms : uint32_t + + sync_timeout_ms : uint32_t + + time_jump_forward_ns : int64_t + + time_jump_backward_ns : int64_t + + phc_config : PhcConfig + } + + TimeSlave *-- GptpEngine + TimeSlave *-- "1" GptpIpcPublisher +} + +package "score::ts::gptp::details" { + class FrameCodec #Wheat { + + ParseEthernetHeader(buf) : EthernetHeader + + AddEthernetHeader(buf, dst_mac, src_mac) : void + } + + class MessageParser #Wheat { + + Parse(payload, hw_ts) : std::optional + } + + class SyncStateMachine #Wheat { + - last_sync_ : PTPMessage + - last_offset_ns_ : int64_t + - neighbor_rate_ratio_ : double + - timeout_ : std::atomic + + OnSync(msg) : void + + OnFollowUp(msg) : std::optional + + IsTimeout() : bool + + GetNeighborRateRatio() : double + } + + class PeerDelayMeasurer #Wheat { + - mutex_ : std::mutex + - result_ : PDelayResult + + SendRequest(socket) : void + + OnResponse(msg) : void + + OnResponseFollowUp(msg) : void + + GetResult() : PDelayResult + } + + struct SyncResult #Beige { + + master_ns : int64_t + + offset_ns : int64_t + + sync_fup_data : SyncFupData + + time_jump_forward : bool + + time_jump_backward : bool + } + + struct PDelayResult #Beige { + + path_delay_ns : int64_t + + valid : bool + } +} + +package "score::ts::gptp::phc" { + class PhcAdjuster #Lavender { + - config_ : PhcConfig + - fd_ : int + + IsEnabled() : bool + + AdjustOffset(offset_ns) : void + + AdjustFrequency(ppb) : void + } + + struct PhcConfig #Beige { + + enabled : bool + + device_path : std::string + + step_threshold_ns : int64_t + } +} + +GptpEngine *-- FrameCodec +GptpEngine *-- MessageParser +GptpEngine *-- SyncStateMachine +GptpEngine *-- PeerDelayMeasurer +GptpEngine *-- PhcAdjuster + +@enduml diff --git a/docs/TimeSlave/_assets/timeslave_data_flow.puml b/docs/TimeSlave/_assets/timeslave_data_flow.puml new file mode 100644 index 0000000..d7c4b15 --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_data_flow.puml @@ -0,0 +1,61 @@ +@startuml +!theme plain + +title TimeSlave Data Flow + +participant "Network\n(gPTP Master)" as NET #Beige +participant "RawSocket" as SOCK #LightSkyBlue +participant "FrameCodec" as FC #Wheat +participant "MessageParser" as MP #Wheat +participant "SyncStateMachine" as SSM #Wheat +participant "PeerDelayMeasurer" as PDM #Wheat +participant "PhcAdjuster" as PHC #Lavender +participant "GptpEngine" as GE #LightSalmon +participant "GptpIpcPublisher" as PUB #LightPink +participant "SharedMemory" as SHM #LightPink + +== RxThread — Sync/FollowUp Processing == + +NET -> SOCK : gPTP Sync frame\n(EtherType 0x88F7) +SOCK -> FC : raw buffer + HW timestamp +FC -> MP : Ethernet payload +MP -> SSM : OnSync(PTPMessage) +SSM -> SSM : store Sync timestamp + +NET -> SOCK : gPTP FollowUp frame +SOCK -> FC : raw buffer + HW timestamp +FC -> MP : Ethernet payload +MP -> SSM : OnFollowUp(PTPMessage) +SSM -> SSM : compute offset & neighborRateRatio +SSM --> MP : SyncResult{master_ns, offset_ns,\ntime_jump_flags} + +MP --> GE : update latest_snapshot_\n(mutex protected) + +== PdelayThread — Peer Delay Measurement == + +GE -> PDM : SendRequest() +PDM -> SOCK : PDelayReq frame +NET --> SOCK : PDelayResp frame +SOCK -> FC : raw buffer + HW timestamp +FC -> MP : Ethernet payload +MP -> PDM : OnResponse(msg) +NET --> SOCK : PDelayRespFollowUp frame +SOCK -> FC : raw buffer + HW timestamp +FC -> MP : Ethernet payload +MP -> PDM : OnResponseFollowUp(msg) +PDM -> PDM : path_delay = ((t2-t1)+(t4-t3c))/2 + +PDM --> GE : update PDelayResult + +== PHC Adjustment == + +GE -> PHC : AdjustOffset(offset_ns) +PHC -> PHC : step or frequency slew + +== Periodic Publish to Shared Memory == + +GE -> GE : ReadPTPSnapshot() +GE -> PUB : Publish(PtpTimeInfo) +PUB -> SHM : seqlock write\n(atomic seq++, memcpy, seq++) + +@enduml diff --git a/docs/TimeSlave/_assets/timeslave_deployment.puml b/docs/TimeSlave/_assets/timeslave_deployment.puml new file mode 100644 index 0000000..e70de49 --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_deployment.puml @@ -0,0 +1,59 @@ +@startuml +!theme plain + +title TimeSlave Deployment View + +node "ECU" { + package "TimeSlave Process" as TSP { + component [GptpEngine] as GE #LightSalmon + component [GptpIpcPublisher] as PUB #LightPink + + package "RxThread" as RXT { + component [FrameCodec] as FC #Wheat + component [MessageParser] as MP #Wheat + component [SyncStateMachine] as SSM #Wheat + } + + package "PdelayThread" as PDT { + component [PeerDelayMeasurer] as PDM #Wheat + } + + component [PhcAdjuster] as PHC #Lavender + component [ProbeManager] as PM #Beige + component [Recorder] as REC #Beige + } + + package "TimeDaemon Process" as TDP { + component [GptpIpcReceiver] as RCV #LightPink + } + + database "Shared Memory\n/gptp_ptp_info" as SHM + + interface "Raw Socket\n(AF_PACKET)" as SOCK + interface "PHC Device\n(/dev/ptpN)" as PHCDEV +} + +cloud "Network" as NET + +GE --> RXT +GE --> PDT +GE --> PHC +GE --> PUB + +FC --> MP +MP --> SSM +MP --> PDM + +PUB -[#green]-> SHM : seqlock write +RCV -[#green]-> SHM : seqlock read + +RXT -[#blue]-> SOCK : recv +PDT -[#blue]-> SOCK : send/recv + +PHC -[#orange]-> PHCDEV : clock_adjtime + +SOCK -[#blue]-> NET : gPTP frames\nEtherType 0x88F7 + +PM --> REC : probe events + +@enduml diff --git a/docs/TimeSlave/gptp_engine/_assets/gptp_engine_class.puml b/docs/TimeSlave/gptp_engine/_assets/gptp_engine_class.puml new file mode 100644 index 0000000..f4550ad --- /dev/null +++ b/docs/TimeSlave/gptp_engine/_assets/gptp_engine_class.puml @@ -0,0 +1,95 @@ +@startuml +!theme plain + +title gPTP Engine Internal Class Diagram + +package "score::ts::gptp" { + + class GptpEngine { + - options_ : GptpEngineOptions + - rx_thread_ : std::thread + - pdelay_thread_ : std::thread + - socket_ : std::unique_ptr + - codec_ : FrameCodec + - parser_ : MessageParser + - sync_sm_ : SyncStateMachine + - pdelay_ : PeerDelayMeasurer + - phc_ : PhcAdjuster + - probe_mgr_ : ProbeManager + - recorder_ : Recorder + - snapshot_mutex_ : std::mutex + - latest_snapshot_ : PtpTimeInfo + + Initialize() : bool + + Deinitialize() : void + + ReadPTPSnapshot() : PtpTimeInfo + } + + interface IRawSocket { + + Open(iface) : bool + + EnableHwTimestamping() : bool + + Recv(buf, timeout_ms) : RecvResult + + Send(buf, hw_ts) : bool + + GetFd() : int + + Close() : void + } + + class "RawSocket\n<>" as LinuxSocket { + AF_PACKET + SO_TIMESTAMPING + } + + class "RawSocket\n<>" as QnxSocket { + QNX raw-socket shim + } + + interface INetworkIdentity { + + Resolve(iface) : bool + + GetClockIdentity() : ClockIdentity + } + + class NetworkIdentity { + Derives EUI-64 from MAC\n(inserts 0xFF 0xFE) + } + + IRawSocket <|.. LinuxSocket + IRawSocket <|.. QnxSocket + INetworkIdentity <|.. NetworkIdentity + + GptpEngine *-- IRawSocket + GptpEngine *-- INetworkIdentity +} + +package "score::ts::gptp::details" { + class FrameCodec + class MessageParser + class SyncStateMachine + class PeerDelayMeasurer +} + +package "score::ts::gptp::phc" { + class PhcAdjuster +} + +package "score::ts::gptp::instrument" { + class ProbeManager { + + {static} Instance() : ProbeManager& + + Record(point, data) : void + + SetRecorder(recorder) : void + } + + class Recorder { + - file_ : std::ofstream + - mutex_ : std::mutex + + Record(entry) : void + } + + ProbeManager --> Recorder +} + +GptpEngine *-- FrameCodec +GptpEngine *-- MessageParser +GptpEngine *-- SyncStateMachine +GptpEngine *-- PeerDelayMeasurer +GptpEngine *-- PhcAdjuster +GptpEngine *-- ProbeManager + +@enduml diff --git a/docs/TimeSlave/gptp_engine/_assets/gptp_threading.puml b/docs/TimeSlave/gptp_engine/_assets/gptp_threading.puml new file mode 100644 index 0000000..93921e2 --- /dev/null +++ b/docs/TimeSlave/gptp_engine/_assets/gptp_threading.puml @@ -0,0 +1,53 @@ +@startuml +!theme plain + +title gPTP Engine Threading Model + +concise "RxThread" as RX +concise "PdelayThread" as PD +concise "Main Thread\n(TimeSlave)" as MAIN + +@0 +MAIN is "Initialize" + +@50 +RX is "Waiting" +PD is "Waiting" +MAIN is "Running" + +@100 +RX is "Recv Sync" +PD is "Sleep\n(interval)" + +@150 +RX is "Parse + SSM" + +@200 +RX is "Recv FollowUp" + +@250 +RX is "Parse + SSM\ncompute offset" + +@300 +RX is "Update snapshot" +PD is "SendRequest" + +@350 +RX is "Waiting" +PD is "Recv Resp" + +@400 +PD is "Recv RespFUp\ncompute delay" + +@450 +PD is "Update result" + +@500 +MAIN is "ReadPTPSnapshot\n→ Publish IPC" +RX is "Waiting" +PD is "Sleep\n(interval)" + +@550 +MAIN is "Running" + +@enduml diff --git a/docs/TimeSlave/gptp_engine/index.rst b/docs/TimeSlave/gptp_engine/index.rst new file mode 100644 index 0000000..e734c5e --- /dev/null +++ b/docs/TimeSlave/gptp_engine/index.rst @@ -0,0 +1,227 @@ +.. + # ******************************************************************************* + # Copyright (c) 2026 Contributors to the Eclipse Foundation + # + # See the NOTICE file(s) distributed with this work for additional + # information regarding copyright ownership. + # + # This program and the accompanying materials are made available under the + # terms of the Apache License Version 2.0 which is available at + # https://www.apache.org/licenses/LICENSE-2.0 + # + # SPDX-License-Identifier: Apache-2.0 + # ******************************************************************************* + +.. _gptp_engine_design: + +############################ +gPTP Engine Design +############################ + +.. contents:: Table of Contents + :depth: 3 + :local: + +Overview +======== + +The ``GptpEngine`` is the core protocol engine of TimeSlave. It implements the IEEE 802.1AS +gPTP slave clock functionality by managing two dedicated threads for network I/O and peer +delay measurement. + +Class view +========== + +.. raw:: html + +
+ +.. uml:: _assets/gptp_engine_class.puml + :alt: gPTP Engine Class Diagram + +.. raw:: html + +
+ +Threading model +=============== + +The GptpEngine operates with two background threads: + +.. raw:: html + +
+ +.. uml:: _assets/gptp_threading.puml + :alt: gPTP Engine Threading Model + +.. raw:: html + +
+ +RxThread +-------- + +The RxThread is the primary receive path. It runs a continuous loop: + +1. **Recv** — Blocks on ``IRawSocket::Recv()`` with a configurable timeout, receiving raw + Ethernet frames with hardware timestamps from the NIC. + +2. **Decode** — ``FrameCodec::ParseEthernetHeader()`` strips the Ethernet header (with VLAN + support) and validates the EtherType (``0x88F7``). + +3. **Parse** — ``MessageParser::Parse()`` decodes the PTP payload into a ``PTPMessage`` + structure, identifying the message type (Sync, FollowUp, PdelayResp, PdelayRespFollowUp). + +4. **Dispatch** — Based on message type: + + - **Sync** → ``SyncStateMachine::OnSync()`` stores the Sync timestamp + - **FollowUp** → ``SyncStateMachine::OnFollowUp()`` correlates with the preceding Sync, + computes ``offset_ns`` and ``neighborRateRatio``, and detects time jumps + - **PdelayResp** → ``PeerDelayMeasurer::OnResponse()`` + - **PdelayRespFollowUp** → ``PeerDelayMeasurer::OnResponseFollowUp()`` + +5. **Snapshot** — After processing, the latest ``PtpTimeInfo`` snapshot is updated under + mutex protection. + +PdelayThread +------------ + +The PdelayThread performs IEEE 802.1AS peer delay measurement on a periodic interval +(configurable via ``GptpEngineOptions::pdelay_interval_ms``): + +1. **Send** — ``PeerDelayMeasurer::SendRequest()`` transmits a ``PDelayReq`` frame via the + raw socket, capturing the hardware transmit timestamp (``t1``). + +2. **Receive** — The RxThread dispatches incoming ``PDelayResp`` (providing ``t2``) and + ``PDelayRespFollowUp`` (providing ``t3c``) to the ``PeerDelayMeasurer``. + +3. **Compute** — The peer delay is computed using the IEEE 802.1AS formula: + + .. code-block:: text + + path_delay = ((t2 - t1) + (t4 - t3c)) / 2 + + where ``t4`` is the local hardware receive timestamp of the PDelayResp frame. + +Thread safety is ensured via a mutex in ``PeerDelayMeasurer``, as ``SendRequest()`` runs on +the PdelayThread while ``OnResponse()``/``OnResponseFollowUp()`` are called from the +RxThread. + +Core components +=============== + +FrameCodec +---------- + +Handles raw Ethernet frame encoding and decoding: + +- ``ParseEthernetHeader()`` — Parses source/destination MAC, handles 802.1Q VLAN tags, + extracts EtherType and payload offset. +- ``AddEthernetHeader()`` — Constructs Ethernet headers for outgoing PDelayReq frames using + the standard PTP multicast destination MAC (``01:80:C2:00:00:0E``). + +MessageParser +------------- + +Parses the PTP wire format (IEEE 1588-v2) from raw payload bytes: + +- Validates the PTP header (version, domain, message length). +- Decodes message-type-specific bodies: ``SyncBody``, ``FollowUpBody``, ``PdelayReqBody``, + ``PdelayRespBody``, ``PdelayRespFollowUpBody``. +- All wire structures are packed (``__attribute__((packed))``) for direct memory mapping. + +SyncStateMachine +---------------- + +Implements the two-step Sync/FollowUp correlation logic: + +- **OnSync(msg)** — Stores the Sync message and its hardware receive timestamp. +- **OnFollowUp(msg)** — Matches with the preceding Sync by sequence ID, then computes: + + - ``offset_ns`` = master_time - slave_receive_time - path_delay + - ``neighborRateRatio`` from successive Sync intervals (master vs. slave clock progression) + - Time jump detection (forward/backward) against configurable thresholds + +- **Timeout detection** — Uses ``std::atomic`` for thread-safe timeout status, + set when no Sync is received within ``sync_timeout_ms``. + +PeerDelayMeasurer +----------------- + +Implements the IEEE 802.1AS two-step peer delay measurement protocol: + +- Manages the four timestamps (``t1``, ``t2``, ``t3c``, ``t4``) across two threads. +- ``SendRequest()`` — Builds and sends a PDelayReq frame, records ``t1`` from the + hardware transmit timestamp. +- ``OnResponse()`` / ``OnResponseFollowUp()`` — Records ``t2``, ``t3c``, ``t4`` and + computes the path delay when all timestamps are available. +- Returns ``PDelayResult`` with ``path_delay_ns`` and a ``valid`` flag. + +PhcAdjuster +----------- + +Synchronizes the PTP Hardware Clock (PHC) on the NIC: + +- **Step correction** — For large offsets exceeding ``step_threshold_ns``, applies an + immediate time step to the PHC. +- **Frequency slew** — For smaller offsets, adjusts the clock frequency (in ppb) for + smooth convergence. +- Platform-specific: Linux uses ``clock_adjtime()``, QNX uses EMAC PTP ioctls. +- Configured via ``PhcConfig`` (device path, step threshold, enable/disable flag). + +Instrumentation +=============== + +ProbeManager +------------ + +A singleton that records probe events at key processing points. Probe points include: + +- ``RxPacketReceived`` — Raw frame received from socket +- ``SyncFrameParsed`` — Sync message successfully parsed +- ``FollowUpProcessed`` — Offset computed from Sync/FollowUp pair +- ``OffsetComputed`` — Final offset value available +- ``PdelayReqSent`` — PDelayReq frame transmitted +- ``PdelayCompleted`` — Peer delay measurement completed +- ``PhcAdjusted`` — PHC adjustment applied + +The ``GPTP_PROBE()`` macro provides zero-overhead when probing is disabled. + +Recorder +-------- + +Thread-safe CSV file writer that persists probe events and other diagnostic data. Each +``RecordEntry`` contains a timestamp, event type, offset, peer delay, sequence ID, and +status flags. + +Configuration +============= + +The ``GptpEngineOptions`` struct provides all configurable parameters: + +.. list-table:: + :header-rows: 1 + :widths: 30 15 55 + + * - Parameter + - Type + - Description + * - ``interface_name`` + - string + - Network interface for gPTP frames (e.g., ``eth0``) + * - ``pdelay_interval_ms`` + - uint32_t + - Interval between PDelayReq transmissions + * - ``sync_timeout_ms`` + - uint32_t + - Timeout for Sync message reception before declaring timeout state + * - ``time_jump_forward_ns`` + - int64_t + - Threshold for forward time jump detection + * - ``time_jump_backward_ns`` + - int64_t + - Threshold for backward time jump detection + * - ``phc_config`` + - PhcConfig + - PHC device path, step threshold, and enable flag diff --git a/docs/TimeSlave/index.rst b/docs/TimeSlave/index.rst new file mode 100644 index 0000000..84d3843 --- /dev/null +++ b/docs/TimeSlave/index.rst @@ -0,0 +1,463 @@ +Concept for TimeSlave +====================== + +.. contents:: Table of Contents + :depth: 3 + :local: + +TimeSlave concept +------------------ + +Use Cases +~~~~~~~~~ + +TimeSlave is a standalone gPTP (IEEE 802.1AS) slave endpoint process that implements the low-level time synchronization protocol for the Eclipse SCORE time system. It is deployed as a separate process from the TimeDaemon to isolate real-time network I/O from the higher-level time validation and distribution logic. + +More precisely we can specify the following use cases for the TimeSlave: + +1. Receiving gPTP Sync/FollowUp messages from a Time Master on the Ethernet network +2. Measuring peer delay via the IEEE 802.1AS PDelayReq/PDelayResp exchange +3. Optionally adjusting the PTP Hardware Clock (PHC) on the NIC +4. Publishing the resulting ``PtpTimeInfo`` to shared memory for consumption by the TimeDaemon + +The raw architectural diagram is represented below. + +.. raw:: html + +
+ +.. uml:: _assets/timeslave_deployment.puml + :alt: Raw architectural diagram + +.. raw:: html + +
+ +Components decomposition +~~~~~~~~~~~~~~~~~~~~~~~~~ + +The design consists of several sw components: + +1. `TimeSlave Application <#timeslave-application-sw-component>`_ +2. `GptpEngine <#gptpengine-sw-component>`_ +3. `FrameCodec <#framecodec-sw-component>`_ +4. `MessageParser <#messageparser-sw-component>`_ +5. `SyncStateMachine <#syncstatemachine-sw-component>`_ +6. `PeerDelayMeasurer <#peerdelaymeasurer-sw-component>`_ +7. `PhcAdjuster <#phcadjuster-sw-component>`_ +8. `libTSClient <#libtsclient-sw-component>`_ + +Class view +~~~~~~~~~~ + +Main classes and components are presented on this diagram: + +.. raw:: html + +
+ +.. uml:: _assets/timeslave_class.puml + :alt: Class View + :width: 100% + :align: center + +.. raw:: html + +
+ +Data and control flow +~~~~~~~~~~~~~~~~~~~~~ + +The Data and Control flow are presented in the following diagram: + +.. raw:: html + +
+ +.. uml:: _assets/timeslave_data_flow.puml + :alt: Data and Control flow View + +.. raw:: html + +
+ +On this view you could see several "workers" scopes: + +1. RxThread scope +2. PdelayThread scope +3. Main thread (periodic publish) scope + +Each control flow is implemented with the dedicated thread and is independent from another ones. + +Control flows +^^^^^^^^^^^^^ + +RxThread scope +'''''''''''''' + +This control flow is responsible for the: + +1. receive raw gPTP Ethernet frames with hardware timestamps from the NIC via raw sockets +2. decode and parse the PTP messages (Sync, FollowUp, PdelayResp, PdelayRespFollowUp) +3. correlate Sync/FollowUp pairs and compute clock offset and neighborRateRatio +4. update the shared ``PtpTimeInfo`` snapshot under mutex protection + +PdelayThread scope +''''''''''''''''''' + +This control flow is responsible for the: + +1. periodically transmit PDelayReq frames and capture hardware transmit timestamps +2. coordinate with the RxThread to receive PDelayResp and PDelayRespFollowUp messages +3. compute the peer delay using the IEEE 802.1AS formula: ``path_delay = ((t2 - t1) + (t4 - t3c)) / 2`` + +Main thread (periodic publish) scope +'''''''''''''''''''''''''''''''''''''' + +This control flow is responsible for the: + +1. periodically call ``GptpEngine::ReadPTPSnapshot()`` to get the latest time measurement +2. enrich the snapshot with the local clock timestamp from ``HighPrecisionLocalSteadyClock`` +3. publish to shared memory via ``GptpIpcPublisher::Publish()`` + +Data types or events +^^^^^^^^^^^^^^^^^^^^ + +There are several data types, which components are communicating to each other: + +PTPMessage +'''''''''' + +``PTPMessage`` is a union-based container for decoded gPTP messages including the hardware receive timestamp. It is produced by ``MessageParser`` and consumed by ``SyncStateMachine`` and ``PeerDelayMeasurer``. + +SyncResult +'''''''''' + +``SyncResult`` is produced by ``SyncStateMachine::OnFollowUp()`` and contains the computed master timestamp, clock offset, Sync/FollowUp data, and time jump flags (forward/backward). + +PDelayResult +'''''''''''' + +``PDelayResult`` is produced by ``PeerDelayMeasurer`` and contains the computed path delay in nanoseconds and a validity flag. + +PtpTimeInfo +'''''''''''' + +``PtpTimeInfo`` is the aggregated snapshot that combines PTP status flags, Sync/FollowUp data, peer delay data, and a local clock reference. This is the data published to shared memory for the TimeDaemon. + +SW Components decomposition +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +TimeSlave Application SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``TimeSlave Application`` component is the main entry point for the TimeSlave process. It extends ``score::mw::lifecycle::Application`` and is responsible for orchestrating the overall lifecycle of the GptpEngine and the IPC publisher. + +Component requirements +'''''''''''''''''''''' + +The ``TimeSlave Application`` has the following requirements: + +- The ``TimeSlave Application`` shall implement the ``Initialize()`` method to create the ``GptpEngine`` with configured options, initialize the ``GptpIpcPublisher`` (creates the shared memory segment), and prepare the ``HighPrecisionLocalSteadyClock`` for local time reference +- The ``TimeSlave Application`` shall implement the ``Run()`` method to start the GptpEngine, enter a periodic publish loop, and monitor the ``stop_token`` for graceful shutdown +- The ``TimeSlave Application`` shall implement the ``Deinitialize()`` method to stop the GptpEngine threads and destroy the shared memory segment +- The ``TimeSlave Application`` shall periodically read the latest ``PtpTimeInfo`` snapshot, enrich it with the local clock timestamp, and publish it via ``GptpIpcPublisher`` + +GptpEngine SW component +^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``GptpEngine`` component is the core gPTP protocol engine. It manages two background threads (RxThread and PdelayThread) for network I/O and peer delay measurement, and exposes a thread-safe ``ReadPTPSnapshot()`` method for the main thread to read the latest time measurement. + +Component requirements +'''''''''''''''''''''' + +The ``GptpEngine`` has the following requirements: + +- The ``GptpEngine`` shall manage an RxThread for receiving and parsing gPTP frames from raw Ethernet sockets +- The ``GptpEngine`` shall manage a PdelayThread for periodic peer delay measurement +- The ``GptpEngine`` shall provide a thread-safe ``ReadPTPSnapshot()`` method that returns the latest ``PtpTimeInfo`` +- The ``GptpEngine`` shall support configurable parameters via ``GptpEngineOptions`` (interface name, PDelay interval, sync timeout, time jump thresholds, PHC configuration) +- The ``GptpEngine`` shall support exchangeability of the raw socket implementation for different platforms (Linux, QNX) + +Class view +'''''''''' + +The Class Diagram is presented below: + +.. raw:: html + +
+ +.. uml:: _assets/gptp_engine_class.puml + :alt: Class Diagram + +.. raw:: html + +
+ +Threading model +''''''''''''''' + +The GptpEngine operates with two background threads. The threading model is represented below: + +.. raw:: html + +
+ +.. uml:: _assets/gptp_threading.puml + :alt: Threading Model + +.. raw:: html + +
+ +Concurrency aspects +''''''''''''''''''' + +The ``GptpEngine`` uses the following synchronization mechanisms: + +- A ``std::mutex`` protects the ``latest_snapshot_`` field, shared between the RxThread (writer) and the main thread (reader via ``ReadPTPSnapshot()``) +- The ``PeerDelayMeasurer`` uses its own ``std::mutex`` to synchronize between the PdelayThread (``SendRequest()``) and the RxThread (``OnResponse()``, ``OnResponseFollowUp()``) +- The ``SyncStateMachine`` uses ``std::atomic`` for the timeout flag, which is read from the main thread and written from the RxThread + +FrameCodec SW component +^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``FrameCodec`` component handles raw Ethernet frame encoding and decoding for gPTP communication. + +Component requirements +'''''''''''''''''''''' + +The ``FrameCodec`` has the following requirements: + +- The ``FrameCodec`` shall parse incoming Ethernet frames, extracting source/destination MAC addresses, handling 802.1Q VLAN tags, and validating the EtherType (``0x88F7``) +- The ``FrameCodec`` shall construct outgoing Ethernet headers for PDelayReq frames using the standard PTP multicast destination MAC (``01:80:C2:00:00:0E``) + +MessageParser SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``MessageParser`` component parses the PTP wire format (IEEE 1588-v2) from raw payload bytes. + +Component requirements +'''''''''''''''''''''' + +The ``MessageParser`` has the following requirements: + +- The ``MessageParser`` shall validate the PTP header (version, domain, message length) +- The ``MessageParser`` shall decode all relevant message types: Sync, FollowUp, PdelayReq, PdelayResp, PdelayRespFollowUp +- The ``MessageParser`` shall use packed wire structures (``__attribute__((packed))``) for direct memory mapping of PTP messages + +SyncStateMachine SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``SyncStateMachine`` component implements the two-step Sync/FollowUp correlation logic. It correlates incoming Sync and FollowUp messages by sequence ID, computes the clock offset and neighbor rate ratio, and detects time jumps. + +Component requirements +'''''''''''''''''''''' + +The ``SyncStateMachine`` has the following requirements: + +- The ``SyncStateMachine`` shall store Sync messages and correlate them with subsequent FollowUp messages by sequence ID +- The ``SyncStateMachine`` shall compute the clock offset: ``offset_ns = master_time - slave_receive_time - path_delay`` +- The ``SyncStateMachine`` shall compute the ``neighborRateRatio`` from successive Sync intervals (master vs. slave clock progression) +- The ``SyncStateMachine`` shall detect forward and backward time jumps against configurable thresholds +- The ``SyncStateMachine`` shall provide thread-safe timeout detection via ``std::atomic``, set when no Sync is received within the configured timeout + +PeerDelayMeasurer SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``PeerDelayMeasurer`` component implements the IEEE 802.1AS two-step peer delay measurement protocol. It manages the four timestamps (``t1``, ``t2``, ``t3c``, ``t4``) across two threads. + +Component requirements +'''''''''''''''''''''' + +The ``PeerDelayMeasurer`` has the following requirements: + +- The ``PeerDelayMeasurer`` shall transmit PDelayReq frames and capture the hardware transmit timestamp (``t1``) +- The ``PeerDelayMeasurer`` shall receive PDelayResp (providing ``t2``, ``t4``) and PDelayRespFollowUp (providing ``t3c``) messages +- The ``PeerDelayMeasurer`` shall compute the peer delay using the IEEE 802.1AS formula: ``path_delay = ((t2 - t1) + (t4 - t3c)) / 2`` +- The ``PeerDelayMeasurer`` shall provide thread-safe access to the ``PDelayResult`` via a mutex, as ``SendRequest()`` runs on the PdelayThread while response handlers are called from the RxThread + +PhcAdjuster SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``PhcAdjuster`` component synchronizes the PTP Hardware Clock (PHC) on the NIC. It applies step corrections for large offsets and frequency slew for smooth convergence of small offsets. + +Component requirements +'''''''''''''''''''''' + +The ``PhcAdjuster`` has the following requirements: + +- The ``PhcAdjuster`` shall apply an immediate time step correction for offsets exceeding ``step_threshold_ns`` +- The ``PhcAdjuster`` shall apply frequency slew (in ppb) for offsets below the step threshold +- The ``PhcAdjuster`` shall support platform-specific implementations: ``clock_adjtime()`` on Linux, EMAC PTP ioctls on QNX +- The ``PhcAdjuster`` shall be configurable via ``PhcConfig`` (device path, step threshold, enable/disable flag) + +libTSClient SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``libTSClient`` component is the shared memory IPC library that connects the TimeSlave process to the TimeDaemon process. It provides a lock-free, single-writer/multi-reader communication channel using a seqlock protocol over POSIX shared memory. + +The component provides two sub components: publisher and receiver to be deployed on the TimeSlave and TimeDaemon sides accordingly. + +Component requirements +'''''''''''''''''''''' + +The ``libTSClient`` has the following requirements: + +- The ``libTSClient`` shall define a shared memory layout (``GptpIpcRegion``) with a magic number for validation, an atomic seqlock counter, and a ``PtpTimeInfo`` data payload +- The ``libTSClient`` shall align the shared memory region to 64 bytes (cache line size) to prevent false sharing +- The ``libTSClient`` shall provide a ``GptpIpcPublisher`` component that creates and manages the POSIX shared memory segment and writes ``PtpTimeInfo`` using the seqlock protocol +- The ``libTSClient`` shall provide a ``GptpIpcReceiver`` component that opens the shared memory segment read-only and reads ``PtpTimeInfo`` with up to 20 seqlock retries +- The ``libTSClient`` shall use the POSIX shared memory name ``/gptp_ptp_info`` by default + +Class view +'''''''''' + +The Class Diagram is presented below: + +.. raw:: html + +
+ +.. uml:: _assets/ipc_channel.puml + :alt: Class Diagram + +.. raw:: html + +
+ +Publish new data +'''''''''''''''' + +When ``TimeSlave Application`` has a new ``PtpTimeInfo`` snapshot, it publishes to the shared memory via the seqlock protocol: + +1. Increment ``seq`` (becomes odd — signals write in progress) +2. ``memcpy`` the data +3. Increment ``seq`` (becomes even — signals write complete) + +Receive data +'''''''''''' + +From TimeDaemon side, the receiver reads from the shared memory using the seqlock protocol with bounded retry: + +1. Read ``seq`` (must be even, otherwise retry) +2. ``memcpy`` the data +3. Read ``seq`` again (must match step 1, otherwise retry — torn read detected) +4. Return ``std::optional`` (empty if all 20 retries exhausted) + +The seqlock protocol workflow is presented in the following sequence diagram: + +.. raw:: html + +
+ +.. uml:: _assets/ipc_sequence.puml + :alt: Seqlock Protocol + +.. raw:: html + +
+ +Platform support +~~~~~~~~~~~~~~~~~ + +TimeSlave supports two target platforms with platform-specific implementations selected at compile time via Bazel ``select()``: + +.. list-table:: Platform Implementations + :header-rows: 1 + :widths: 25 35 40 + + * - Component + - Linux + - QNX + * - Raw Socket + - ``AF_PACKET`` with ``SO_TIMESTAMPING`` + - QNX raw-socket shim + * - Network Identity + - ``ioctl(SIOCGIFHWADDR)`` + - QNX-specific MAC resolution + * - PHC Adjuster + - ``clock_adjtime()`` + - EMAC PTP ioctls + * - HighPrecisionLocalSteadyClock + - ``std::chrono`` system clock + - QTIME clock API + +The ``IRawSocket`` and ``INetworkIdentity`` interfaces provide the abstraction boundary. Platform-specific source files are organized under ``score/TimeSlave/code/gptp/platform/linux/`` and ``score/TimeSlave/code/gptp/platform/qnx/``. + +Instrumentation +~~~~~~~~~~~~~~~~ + +ProbeManager +^^^^^^^^^^^^ + +The ``ProbeManager`` is a singleton that records probe events at key processing points in the gPTP engine. Probe points include: + +- ``RxPacketReceived`` — Raw frame received from socket +- ``SyncFrameParsed`` — Sync message successfully parsed +- ``FollowUpProcessed`` — Offset computed from Sync/FollowUp pair +- ``OffsetComputed`` — Final offset value available +- ``PdelayReqSent`` — PDelayReq frame transmitted +- ``PdelayCompleted`` — Peer delay measurement completed +- ``PhcAdjusted`` — PHC adjustment applied + +The ``GPTP_PROBE()`` macro provides zero-overhead when probing is disabled. + +Recorder +^^^^^^^^^ + +Thread-safe CSV file writer that persists probe events and other diagnostic data. Each ``RecordEntry`` contains a timestamp, event type, offset, peer delay, sequence ID, and status flags. + +Variability +~~~~~~~~~~~ + +Configuration +^^^^^^^^^^^^^ + +The ``GptpEngineOptions`` struct provides all configurable parameters for the gPTP engine: + +.. list-table:: GptpEngine Configuration + :header-rows: 1 + :widths: 30 15 55 + + * - Parameter + - Type + - Description + * - ``interface_name`` + - string + - Network interface for gPTP frames (e.g., ``eth0``) + * - ``pdelay_interval_ms`` + - uint32_t + - Interval between PDelayReq transmissions + * - ``sync_timeout_ms`` + - uint32_t + - Timeout for Sync message reception before declaring timeout state + * - ``time_jump_forward_ns`` + - int64_t + - Threshold for forward time jump detection + * - ``time_jump_backward_ns`` + - int64_t + - Threshold for backward time jump detection + * - ``phc_config`` + - PhcConfig + - PHC device path, step threshold, and enable flag + +The ``PhcConfig`` struct additionally contains: + +.. list-table:: PhcAdjuster Configuration + :header-rows: 1 + :widths: 30 15 55 + + * - Parameter + - Type + - Description + * - ``enabled`` + - bool + - Enable or disable PHC adjustment + * - ``device_path`` + - string + - Path to the PHC device (e.g., ``/dev/ptp0``) + * - ``step_threshold_ns`` + - int64_t + - Offset threshold above which a step correction is applied instead of frequency slew diff --git a/docs/TimeSlave/libTSClient/_assets/ipc_channel.puml b/docs/TimeSlave/libTSClient/_assets/ipc_channel.puml new file mode 100644 index 0000000..69e04b4 --- /dev/null +++ b/docs/TimeSlave/libTSClient/_assets/ipc_channel.puml @@ -0,0 +1,46 @@ +@startuml +!theme plain + +title libTSClient Shared Memory IPC + +package "TimeSlave Process" { + class GptpIpcPublisher { + - region_ : GptpIpcRegion* + - fd_ : int + + Init(name) : bool + + Publish(info) : void + + Destroy() : void + } +} + +package "Shared Memory" { + class GptpIpcRegion <> { + + magic : uint32_t = 0x47505440 + + seq : std::atomic + + data : PtpTimeInfo + -- + 64-byte aligned for\ncache line efficiency + } +} + +package "TimeDaemon Process" { + class GptpIpcReceiver { + - region_ : const GptpIpcRegion* + - fd_ : int + + Init(name) : bool + + Receive() : std::optional + + Close() : void + } +} + +GptpIpcPublisher --> GptpIpcRegion : "shm_open(O_CREAT)\nmmap(PROT_WRITE)" +GptpIpcReceiver --> GptpIpcRegion : "shm_open(O_RDONLY)\nmmap(PROT_READ)" + +note right of GptpIpcRegion + **Seqlock Protocol:** + Writer: seq++ → memcpy → seq++ + Reader: read seq (even) → memcpy → check seq + Retry up to 20 times on torn read +end note + +@enduml diff --git a/docs/TimeSlave/libTSClient/_assets/ipc_sequence.puml b/docs/TimeSlave/libTSClient/_assets/ipc_sequence.puml new file mode 100644 index 0000000..46fa582 --- /dev/null +++ b/docs/TimeSlave/libTSClient/_assets/ipc_sequence.puml @@ -0,0 +1,46 @@ +@startuml +!theme plain + +title libTSClient Seqlock IPC Protocol + +participant "TimeSlave\n(GptpIpcPublisher)" as PUB +participant "SharedMemory\n(GptpIpcRegion)" as SHM +participant "TimeDaemon\n(GptpIpcReceiver)" as RCV + +== Initialization == + +PUB -> SHM : shm_open("/gptp_ptp_info", O_CREAT | O_RDWR) +PUB -> SHM : ftruncate(sizeof(GptpIpcRegion)) +PUB -> SHM : mmap(PROT_READ | PROT_WRITE) +PUB -> SHM : write magic = 0x47505440 + +... + +RCV -> SHM : shm_open("/gptp_ptp_info", O_RDONLY) +RCV -> SHM : mmap(PROT_READ) +RCV -> SHM : verify magic == 0x47505440 + +== Publish (Writer Side) == + +PUB -> SHM : seq.fetch_add(1, release) // seq becomes odd +PUB -> SHM : memcpy(data, &info, sizeof) +PUB -> SHM : seq.fetch_add(1, release) // seq becomes even + +== Receive (Reader Side) == + +loop up to 20 retries + RCV -> SHM : s1 = seq.load(acquire) + alt s1 is odd (write in progress) + RCV -> RCV : retry + else s1 is even + RCV -> SHM : memcpy(&local, data, sizeof) + RCV -> SHM : s2 = seq.load(acquire) + alt s1 == s2 + RCV --> RCV : return PtpTimeInfo + else s1 != s2 (torn read) + RCV -> RCV : retry + end + end +end + +@enduml diff --git a/docs/TimeSlave/libTSClient/index.rst b/docs/TimeSlave/libTSClient/index.rst new file mode 100644 index 0000000..b843b5c --- /dev/null +++ b/docs/TimeSlave/libTSClient/index.rst @@ -0,0 +1,175 @@ +.. + # ******************************************************************************* + # Copyright (c) 2026 Contributors to the Eclipse Foundation + # + # See the NOTICE file(s) distributed with this work for additional + # information regarding copyright ownership. + # + # This program and the accompanying materials are made available under the + # terms of the Apache License Version 2.0 which is available at + # https://www.apache.org/licenses/LICENSE-2.0 + # + # SPDX-License-Identifier: Apache-2.0 + # ******************************************************************************* + +.. _libtsclient_design: + +############################ +libTSClient Design +############################ + +.. contents:: Table of Contents + :depth: 3 + :local: + +Overview +======== + +**libTSClient** is the shared memory IPC library that connects the TimeSlave process to the +TimeDaemon process. It provides a lock-free, single-writer/multi-reader communication +channel using a **seqlock protocol** over POSIX shared memory. + +The library is intentionally minimal — it consists of three headers and two source files — +to keep the IPC boundary simple, auditable, and suitable for safety-critical deployments. + +Architecture +============ + +.. raw:: html + +
+ +.. uml:: _assets/ipc_channel.puml + :alt: libTSClient IPC Architecture + +.. raw:: html + +
+ +Components +========== + +GptpIpcChannel +-------------- + +Defines the shared memory layout as the ``GptpIpcRegion`` structure: + +.. list-table:: + :header-rows: 1 + :widths: 20 20 60 + + * - Field + - Type + - Purpose + * - ``magic`` + - ``uint32_t`` + - Validation constant (``0x47505440``). Used by the receiver to confirm the shared + memory segment is valid and initialized. + * - ``seq`` + - ``std::atomic`` + - Seqlock counter. Odd values indicate a write in progress; even values indicate + a consistent state. + * - ``data`` + - ``PtpTimeInfo`` + - The actual time synchronization payload (PTP status, Sync/FollowUp data, + peer delay data, local clock reference). + +The structure is aligned to 64 bytes (cache line size) to prevent false sharing between +the writer and reader processes. + +The default POSIX shared memory name is ``/gptp_ptp_info`` (defined as ``kGptpIpcName``). + +GptpIpcPublisher +---------------- + +The **single-writer** component, used by TimeSlave: + +- ``Init(name)`` — Creates the POSIX shared memory segment via ``shm_open(O_CREAT | O_RDWR)``, + sizes it with ``ftruncate()``, maps it with ``mmap(PROT_READ | PROT_WRITE)``, and writes + the magic number. + +- ``Publish(info)`` — Writes a ``PtpTimeInfo`` using the seqlock protocol: + + 1. Increment ``seq`` (becomes odd — signals write in progress) + 2. ``memcpy`` the data + 3. Increment ``seq`` (becomes even — signals write complete) + +- ``Destroy()`` — Unmaps and unlinks the shared memory segment. + +GptpIpcReceiver +--------------- + +The **multi-reader** component, used by the TimeDaemon (via ``RealPTPEngine``): + +- ``Init(name)`` — Opens the existing shared memory segment via ``shm_open(O_RDONLY)`` and + maps it with ``mmap(PROT_READ)``. Validates the magic number. + +- ``Receive()`` — Reads ``PtpTimeInfo`` using the seqlock protocol with up to 20 retries: + + 1. Read ``seq`` (must be even, otherwise retry) + 2. ``memcpy`` the data + 3. Read ``seq`` again (must match step 1, otherwise retry — torn read detected) + 4. Return ``std::optional`` (empty if all retries exhausted) + +- ``Close()`` — Unmaps the shared memory. + +Seqlock protocol +================ + +.. raw:: html + +
+ +.. uml:: _assets/ipc_sequence.puml + :alt: Seqlock IPC Protocol Sequence + +.. raw:: html + +
+ +The seqlock provides the following properties: + +- **Lock-free for readers** — Readers never block the writer. A torn read is detected and + retried transparently. +- **Single writer** — Only one process (TimeSlave) writes to the shared memory. No + writer-writer contention. +- **Bounded retry** — The receiver retries up to 20 times. Under normal operation, + retries are rare because the write is a single ``memcpy`` of a small struct. +- **Cache-line alignment** — The 64-byte alignment of ``GptpIpcRegion`` prevents false + sharing, which is critical for cross-process shared memory performance. + +Data type +========= + +The ``PtpTimeInfo`` structure (defined in ``score/TimeDaemon/code/common/data_types/ptp_time_info.h``) +is the payload transferred through the IPC channel. It contains: + +.. list-table:: + :header-rows: 1 + :widths: 25 75 + + * - Field + - Content + * - ``PtpStatus`` + - Synchronization flags (synchronized, timeout, time leap indicators) + * - ``SyncFupData`` + - Sync and FollowUp message timestamps and correction fields + * - ``PDelayData`` + - Peer delay measurement results + * - Local clock value + - Reference timestamp from ``HighPrecisionLocalSteadyClock`` + +Build integration +================= + +The library is built as a Bazel ``cc_library`` target: + +.. code-block:: text + + //score/libTSClient:gptp_ipc + +It links against ``-lrt`` for POSIX shared memory (``shm_open``, ``shm_unlink``) and +depends on the ``PtpTimeInfo`` data type from the TimeDaemon common module. + +Both TimeSlave and TimeDaemon link against ``libTSClient`` — the publisher side in +TimeSlave, the receiver side in TimeDaemon. diff --git a/score/TimeDaemon/code/common/BUILD b/score/TimeDaemon/code/common/BUILD index ea4d4fd..fe5fa19 100644 --- a/score/TimeDaemon/code/common/BUILD +++ b/score/TimeDaemon/code/common/BUILD @@ -22,7 +22,7 @@ cc_library( ], features = COMPILER_WARNING_FEATURES, tags = ["QM"], - visibility = ["//score/TimeDaemon:__subpackages__"], + visibility = ["//score:__subpackages__"], deps = [], ) diff --git a/score/TimeDaemon/code/common/data_types/BUILD b/score/TimeDaemon/code/common/data_types/BUILD index e6b718d..d7a7468 100644 --- a/score/TimeDaemon/code/common/data_types/BUILD +++ b/score/TimeDaemon/code/common/data_types/BUILD @@ -22,7 +22,7 @@ cc_library( ], features = COMPILER_WARNING_FEATURES, tags = ["QM"], - visibility = ["//score/TimeDaemon:__subpackages__"], + visibility = ["//score:__subpackages__"], deps = ["//score/time/HighPrecisionLocalSteadyClock:interface"], ) diff --git a/score/TimeDaemon/code/ptp_machine/BUILD b/score/TimeDaemon/code/ptp_machine/BUILD index d596cfc..2e05898 100644 --- a/score/TimeDaemon/code/ptp_machine/BUILD +++ b/score/TimeDaemon/code/ptp_machine/BUILD @@ -23,6 +23,7 @@ cc_unit_test_suites_for_host_and_qnx( name = "unit_test_suite", test_suites_from_sub_packages = [ "//score/TimeDaemon/code/ptp_machine/core:unit_test_suite", + "//score/TimeDaemon/code/ptp_machine/real:unit_test_suite", "//score/TimeDaemon/code/ptp_machine/stub:unit_test_suite", ], visibility = ["//score/TimeDaemon:__subpackages__"], diff --git a/score/TimeDaemon/code/ptp_machine/real/BUILD b/score/TimeDaemon/code/ptp_machine/real/BUILD new file mode 100644 index 0000000..55ec5df --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/BUILD @@ -0,0 +1,56 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "gptp_real_machine", + srcs = [ + "factory.cpp", + ], + hdrs = [ + "factory.h", + "gptp_real_machine.h", + ], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score/TimeDaemon:__subpackages__"], + deps = [ + "//score/TimeDaemon/code/ptp_machine/core:ptp_machine", + "//score/TimeDaemon/code/ptp_machine/real/details:real_ptp_engine", + "//score/libTSClient:gptp_ipc", + ], +) + +cc_test( + name = "gptp_real_machine_test", + srcs = ["gptp_real_machine_test.cpp"], + tags = ["unit"], + deps = [ + ":gptp_real_machine", + "//score/libTSClient:gptp_ipc", + "@googletest//:gtest", + "@googletest//:gtest_main", + "@score_baselibs//score/mw/log:console_only_backend", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":gptp_real_machine_test"], + test_suites_from_sub_packages = [ + "//score/TimeDaemon/code/ptp_machine/real/details:unit_test_suite", + ], + visibility = ["//score/TimeDaemon:__subpackages__"], +) diff --git a/score/TimeDaemon/code/ptp_machine/real/details/BUILD b/score/TimeDaemon/code/ptp_machine/real/details/BUILD new file mode 100644 index 0000000..71588d2 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/details/BUILD @@ -0,0 +1,54 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "real_ptp_engine", + srcs = [ + "real_ptp_engine.cpp", + ], + hdrs = [ + "real_ptp_engine.h", + ], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score/TimeDaemon:__subpackages__"], + deps = [ + "//score/TimeDaemon/code/common:logging_contexts", + "//score/TimeDaemon/code/common/data_types:ptp_time_info", + "//score/libTSClient:gptp_ipc", + "@score_baselibs//score/mw/log:frontend", + ], +) + +cc_test( + name = "real_ptp_engine_test", + srcs = ["real_ptp_engine_test.cpp"], + tags = ["unit"], + deps = [ + ":real_ptp_engine", + "//score/libTSClient:gptp_ipc", + "@googletest//:gtest", + "@googletest//:gtest_main", + "@score_baselibs//score/mw/log:console_only_backend", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":real_ptp_engine_test"], + test_suites_from_sub_packages = [], + visibility = ["//score/TimeDaemon:__subpackages__"], +) diff --git a/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.cpp b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.cpp new file mode 100644 index 0000000..8258250 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.cpp @@ -0,0 +1,94 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h" + +#include "score/TimeDaemon/code/common/logging_contexts.h" +#include "score/mw/log/logging.h" + +namespace score +{ +namespace td +{ +namespace details +{ + +RealPTPEngine::RealPTPEngine(std::string ipc_name) noexcept : ipc_name_{std::move(ipc_name)} {} + +bool RealPTPEngine::Initialize() +{ + if (initialized_) + return true; + + initialized_ = receiver_.Init(ipc_name_); + if (initialized_) + { + score::mw::log::LogInfo(kGPtpMachineContext) << "RealPTPEngine: connected to IPC channel " << ipc_name_; + } + else + { + score::mw::log::LogError(kGPtpMachineContext) << "RealPTPEngine: failed to open IPC channel " << ipc_name_; + } + return initialized_; +} + +bool RealPTPEngine::Deinitialize() +{ + if (initialized_) + { + receiver_.Close(); + initialized_ = false; + } + return true; +} + +bool RealPTPEngine::ReadPTPSnapshot(PtpTimeInfo& info) +{ + if (!initialized_) + return false; + + auto result = receiver_.Receive(); + if (!result.has_value()) + return false; + + cached_ = result.value(); + + const bool time_ok = ReadTimeValueAndStatus(info); + const bool pdelay_ok = ReadPDelayMeasurementData(info); + const bool sync_ok = ReadSyncMeasurementData(info); + return time_ok && pdelay_ok && sync_ok; +} + +bool RealPTPEngine::ReadTimeValueAndStatus(PtpTimeInfo& info) noexcept +{ + info.local_time = cached_.local_time; + info.ptp_assumed_time = cached_.ptp_assumed_time; + info.rate_deviation = cached_.rate_deviation; + info.status = cached_.status; + return true; +} + +bool RealPTPEngine::ReadPDelayMeasurementData(PtpTimeInfo& info) const noexcept +{ + info.pdelay_data = cached_.pdelay_data; + return true; +} + +bool RealPTPEngine::ReadSyncMeasurementData(PtpTimeInfo& info) const noexcept +{ + info.sync_fup_data = cached_.sync_fup_data; + return true; +} + +} // namespace details +} // namespace td +} // namespace score diff --git a/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h new file mode 100644 index 0000000..992637c --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h @@ -0,0 +1,70 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_DETAILS_REAL_PTP_ENGINE_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_DETAILS_REAL_PTP_ENGINE_H + +#include "score/TimeDaemon/code/common/data_types/ptp_time_info.h" +#include "score/libTSClient/gptp_ipc_receiver.h" + +#include + +namespace score +{ +namespace td +{ +namespace details +{ + +/** + * @brief PTP engine that reads time data from the IPC channel written by TimeSlave. + */ +class RealPTPEngine final +{ + public: + explicit RealPTPEngine(std::string ipc_name = score::ts::details::kGptpIpcName) noexcept; + ~RealPTPEngine() noexcept = default; + + RealPTPEngine(const RealPTPEngine&) = delete; + RealPTPEngine& operator=(const RealPTPEngine&) = delete; + RealPTPEngine(RealPTPEngine&&) = delete; + RealPTPEngine& operator=(RealPTPEngine&&) = delete; + + /// Open and map the IPC channel. + /// @return true on success. + bool Initialize(); + + /// Unmap the IPC channel. + /// @return true (always succeeds). + bool Deinitialize(); + + /// Read a fresh snapshot from the IPC channel and populate @p info. + /// Delegates to ReadTimeValueAndStatus, ReadPDelayMeasurementData, + /// and ReadSyncMeasurementData. + bool ReadPTPSnapshot(PtpTimeInfo& info); + + bool ReadTimeValueAndStatus(PtpTimeInfo& info) noexcept; + bool ReadPDelayMeasurementData(PtpTimeInfo& info) const noexcept; + bool ReadSyncMeasurementData(PtpTimeInfo& info) const noexcept; + + private: + std::string ipc_name_; + score::ts::details::GptpIpcReceiver receiver_; + bool initialized_{false}; + PtpTimeInfo cached_{}; +}; + +} // namespace details +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_DETAILS_REAL_PTP_ENGINE_H diff --git a/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine_test.cpp b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine_test.cpp new file mode 100644 index 0000000..94b6254 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine_test.cpp @@ -0,0 +1,277 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h" + +#include "score/libTSClient/gptp_ipc_publisher.h" + +#include + +#include +#include + +namespace score +{ +namespace td +{ +namespace details +{ + +namespace +{ + +std::string UniqueShmName() +{ + static std::atomic counter{0}; + return "/gptp_rpe_ut_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1, std::memory_order_relaxed)); +} + +// Build a fully-populated PtpTimeInfo for roundtrip verification. +PtpTimeInfo MakeTestInfo() +{ + PtpTimeInfo info{}; + info.ptp_assumed_time = std::chrono::nanoseconds{9'876'543'210LL}; + info.rate_deviation = -0.25; + + info.status.is_synchronized = true; + info.status.is_correct = true; + info.status.is_timeout = false; + info.status.is_time_jump_future = false; + info.status.is_time_jump_past = false; + + info.sync_fup_data.precise_origin_timestamp = 100'000'000'000ULL; + info.sync_fup_data.reference_global_timestamp = 100'000'000'500ULL; + info.sync_fup_data.reference_local_timestamp = 100'000'001'000ULL; + info.sync_fup_data.sync_ingress_timestamp = 100'000'001'000ULL; + info.sync_fup_data.correction_field = 8U; + info.sync_fup_data.sequence_id = 55; + info.sync_fup_data.pdelay = 4'000U; + info.sync_fup_data.port_number = 1; + info.sync_fup_data.clock_identity = 0xCAFEBABEDEAD0001ULL; + + info.pdelay_data.request_origin_timestamp = 200'000'000'000ULL; + info.pdelay_data.request_receipt_timestamp = 200'000'001'000ULL; + info.pdelay_data.response_origin_timestamp = 200'000'001'000ULL; + info.pdelay_data.response_receipt_timestamp = 200'000'002'000ULL; + info.pdelay_data.pdelay = 1'000U; + info.pdelay_data.req_port_number = 2; + info.pdelay_data.resp_port_number = 3; + info.pdelay_data.req_clock_identity = 0x0102030405060708ULL; + info.pdelay_data.resp_clock_identity = 0x0807060504030201ULL; + return info; +} + +} // namespace + +class RealPTPEngineTest : public ::testing::Test +{ + protected: + void SetUp() override + { + name_ = UniqueShmName(); + engine_ = std::make_unique(name_); + } + + void TearDown() override + { + engine_->Deinitialize(); + pub_.Destroy(); + } + + std::string name_; + score::ts::details::GptpIpcPublisher pub_; + std::unique_ptr engine_; +}; + +// ── Lifecycle ──────────────────────────────────────────────────────────────── + +TEST_F(RealPTPEngineTest, Initialize_WhenShmNotExist_ReturnsFalse) +{ + // No publisher → shm doesn't exist. + EXPECT_FALSE(engine_->Initialize()); +} + +TEST_F(RealPTPEngineTest, Initialize_WhenShmExists_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + EXPECT_TRUE(engine_->Initialize()); +} + +TEST_F(RealPTPEngineTest, Initialize_CalledTwiceWhenInitialized_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Initialize()); // idempotent +} + +TEST_F(RealPTPEngineTest, Deinitialize_WhenNotInitialized_ReturnsTrue) +{ + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(RealPTPEngineTest, Deinitialize_AfterInitialize_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(RealPTPEngineTest, Deinitialize_CalledTwice_BothReturnTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Deinitialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(RealPTPEngineTest, ReInitialize_AfterDeinitialize_Succeeds) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + ASSERT_TRUE(engine_->Deinitialize()); + EXPECT_TRUE(engine_->Initialize()); +} + +// ── ReadPTPSnapshot ─────────────────────────────────────────────────────────── + +TEST_F(RealPTPEngineTest, ReadPTPSnapshot_WhenNotInitialized_ReturnsFalse) +{ + PtpTimeInfo info{}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(info)); +} + +TEST_F(RealPTPEngineTest, ReadPTPSnapshot_WithPublishedData_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakeTestInfo()); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadPTPSnapshot(result)); +} + +TEST_F(RealPTPEngineTest, ReadPTPSnapshot_CopiesTimeAndStatusCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const PtpTimeInfo expected = MakeTestInfo(); + pub_.Publish(expected); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.ptp_assumed_time, expected.ptp_assumed_time); + EXPECT_DOUBLE_EQ(result.rate_deviation, expected.rate_deviation); + EXPECT_EQ(result.status.is_synchronized, expected.status.is_synchronized); + EXPECT_EQ(result.status.is_correct, expected.status.is_correct); + EXPECT_EQ(result.status.is_timeout, expected.status.is_timeout); +} + +TEST_F(RealPTPEngineTest, ReadPTPSnapshot_CopiesSyncFupDataCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const PtpTimeInfo expected = MakeTestInfo(); + pub_.Publish(expected); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.sync_fup_data.precise_origin_timestamp, expected.sync_fup_data.precise_origin_timestamp); + EXPECT_EQ(result.sync_fup_data.reference_global_timestamp, expected.sync_fup_data.reference_global_timestamp); + EXPECT_EQ(result.sync_fup_data.sequence_id, expected.sync_fup_data.sequence_id); + EXPECT_EQ(result.sync_fup_data.pdelay, expected.sync_fup_data.pdelay); + EXPECT_EQ(result.sync_fup_data.clock_identity, expected.sync_fup_data.clock_identity); +} + +TEST_F(RealPTPEngineTest, ReadPTPSnapshot_CopiesPDelayDataCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const PtpTimeInfo expected = MakeTestInfo(); + pub_.Publish(expected); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.pdelay_data.pdelay, expected.pdelay_data.pdelay); + EXPECT_EQ(result.pdelay_data.req_port_number, expected.pdelay_data.req_port_number); + EXPECT_EQ(result.pdelay_data.resp_port_number, expected.pdelay_data.resp_port_number); + EXPECT_EQ(result.pdelay_data.req_clock_identity, expected.pdelay_data.req_clock_identity); + EXPECT_EQ(result.pdelay_data.resp_clock_identity, expected.pdelay_data.resp_clock_identity); +} + +// ── Individual sub-methods (called after ReadPTPSnapshot populates cache) ───── + +TEST_F(RealPTPEngineTest, ReadTimeValueAndStatus_FromCachedData_AlwaysReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakeTestInfo()); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo snap{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(snap)); + + // Call again on a fresh struct — should use the cached data. + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadTimeValueAndStatus(result)); + EXPECT_EQ(result.ptp_assumed_time, snap.ptp_assumed_time); + EXPECT_DOUBLE_EQ(result.rate_deviation, snap.rate_deviation); + EXPECT_EQ(result.status.is_synchronized, snap.status.is_synchronized); +} + +TEST_F(RealPTPEngineTest, ReadPDelayMeasurementData_FromCachedData_AlwaysReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakeTestInfo()); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo snap{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(snap)); + + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadPDelayMeasurementData(result)); + EXPECT_EQ(result.pdelay_data.pdelay, snap.pdelay_data.pdelay); +} + +TEST_F(RealPTPEngineTest, ReadSyncMeasurementData_FromCachedData_AlwaysReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakeTestInfo()); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo snap{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(snap)); + + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadSyncMeasurementData(result)); + EXPECT_EQ(result.sync_fup_data.sequence_id, snap.sync_fup_data.sequence_id); +} + +// Sub-methods on default-constructed cache (before any snapshot) return true +// with zeroed data. +TEST_F(RealPTPEngineTest, SubMethods_BeforeSnapshot_ReturnTrueWithZeroData) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadTimeValueAndStatus(result)); + EXPECT_TRUE(engine_->ReadPDelayMeasurementData(result)); + EXPECT_TRUE(engine_->ReadSyncMeasurementData(result)); + EXPECT_EQ(result.ptp_assumed_time, std::chrono::nanoseconds{0}); +} + +} // namespace details +} // namespace td +} // namespace score diff --git a/score/TimeDaemon/code/ptp_machine/real/factory.cpp b/score/TimeDaemon/code/ptp_machine/real/factory.cpp new file mode 100644 index 0000000..a9e9027 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/factory.cpp @@ -0,0 +1,27 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeDaemon/code/ptp_machine/real/factory.h" + +namespace score +{ +namespace td +{ + +std::shared_ptr CreateGPTPRealMachine(const std::string& name, const std::string& ipc_name) +{ + constexpr std::chrono::milliseconds updateInterval(50); + return std::make_shared(name, updateInterval, ipc_name); +} + +} // namespace td +} // namespace score diff --git a/score/TimeDaemon/code/ptp_machine/real/factory.h b/score/TimeDaemon/code/ptp_machine/real/factory.h new file mode 100644 index 0000000..d32bbaf --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/factory.h @@ -0,0 +1,44 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_FACTORY_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_FACTORY_H + +#include "score/TimeDaemon/code/ptp_machine/real/gptp_real_machine.h" +#include "score/libTSClient/gptp_ipc_channel.h" + +#include +#include + +namespace score +{ +namespace td +{ + +/** + * @brief Factory function to create a configured GPTPRealMachine. + * + * Creates a GPTPRealMachine backed by the real gPTP engine. + * The engine reads PtpTimeInfo snapshots published by TimeSlave via + * the IPC channel named @p ipc_name. + * + * @param name Logical name for the machine instance. + * @param ipc_name IPC channel name (default: kGptpIpcName). + * @return A fully configured GPTPRealMachine instance. + */ +std::shared_ptr CreateGPTPRealMachine(const std::string& name, + const std::string& ipc_name = score::ts::details::kGptpIpcName); + +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_FACTORY_H diff --git a/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine.h b/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine.h new file mode 100644 index 0000000..3860ba0 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine.h @@ -0,0 +1,38 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_GPTP_REAL_MACHINE_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_GPTP_REAL_MACHINE_H + +#include "score/TimeDaemon/code/ptp_machine/core/ptp_machine.h" +#include "score/TimeDaemon/code/ptp_machine/real/details/real_ptp_engine.h" + +namespace score +{ +namespace td +{ + +/// @brief PTPMachine instantiated with the real gPTP engine. +/// +/// Reads PtpTimeInfo snapshots written by TimeSlave via the IPC channel. +/// Construct via CreateGPTPRealMachine() (see factory.h) or directly: +/// +/// @code +/// auto machine = std::make_shared( +/// "real", std::chrono::milliseconds{50}, "/gptp_ptp_info"); +/// @endcode +using GPTPRealMachine = PTPMachine; + +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_REAL_GPTP_REAL_MACHINE_H diff --git a/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine_test.cpp b/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine_test.cpp new file mode 100644 index 0000000..705c40c --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/real/gptp_real_machine_test.cpp @@ -0,0 +1,127 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeDaemon/code/ptp_machine/real/gptp_real_machine.h" +#include "score/TimeDaemon/code/ptp_machine/real/factory.h" +#include "score/libTSClient/gptp_ipc_publisher.h" + +#include + +#include +#include +#include +#include + +namespace score +{ +namespace td +{ + +namespace +{ + +std::string UniqueShmName() +{ + static std::atomic counter{0}; + return "/gptp_rm_it_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1, std::memory_order_relaxed)); +} + +score::td::PtpTimeInfo MakePublishedInfo() +{ + score::td::PtpTimeInfo info{}; + info.ptp_assumed_time = std::chrono::nanoseconds{5'000'000'000LL}; + info.rate_deviation = 0.5; + info.status.is_synchronized = true; + info.status.is_correct = true; + info.sync_fup_data.sequence_id = 7U; + info.sync_fup_data.pdelay = 1'000U; + return info; +} + +} // namespace + +class GPTPRealMachineIntegrationTest : public ::testing::Test +{ + protected: + void SetUp() override + { + name_ = UniqueShmName(); + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakePublishedInfo()); + + machine_ = CreateGPTPRealMachine("RealPTPMachine", name_); + machine_->SetPublishCallback([this](const PtpTimeInfo& data) { + { + std::lock_guard lk(mu_); + published_ = data; + } + promise_.set_value(); + }); + } + + void TearDown() override + { + machine_->Stop(); + machine_.reset(); + pub_.Destroy(); + } + + std::string name_; + score::ts::details::GptpIpcPublisher pub_; + std::shared_ptr machine_; + std::promise promise_; + PtpTimeInfo published_{}; + std::mutex mu_; +}; + +TEST_F(GPTPRealMachineIntegrationTest, GetName_ReturnsConstructionName) +{ + EXPECT_EQ(machine_->GetName(), "RealPTPMachine"); +} + +TEST_F(GPTPRealMachineIntegrationTest, Init_WhenShmExists_ReturnsTrue) +{ + EXPECT_TRUE(machine_->Init()); +} + +TEST_F(GPTPRealMachineIntegrationTest, Init_WhenShmMissing_ReturnsFalse) +{ + auto m = CreateGPTPRealMachine("NoShm", "/gptp_nosuchshm_xyz"); + EXPECT_FALSE(m->Init()); +} + +TEST_F(GPTPRealMachineIntegrationTest, Start_DeliversPublishedData_ViaCallback) +{ + ASSERT_TRUE(machine_->Init()); + machine_->Start(); + + auto fut = promise_.get_future(); + ASSERT_EQ(fut.wait_for(std::chrono::milliseconds(500)), std::future_status::ready); + + std::lock_guard lk(mu_); + EXPECT_EQ(published_.ptp_assumed_time, std::chrono::nanoseconds{5'000'000'000LL}); + EXPECT_DOUBLE_EQ(published_.rate_deviation, 0.5); + EXPECT_TRUE(published_.status.is_synchronized); + EXPECT_TRUE(published_.status.is_correct); + EXPECT_EQ(published_.sync_fup_data.sequence_id, 7U); + EXPECT_EQ(published_.sync_fup_data.pdelay, 1'000U); +} + +TEST_F(GPTPRealMachineIntegrationTest, Init_CalledTwice_SecondCallReturnsSameResult) +{ + ASSERT_TRUE(machine_->Init()); + EXPECT_TRUE(machine_->Init()); +} + +} // namespace td +} // namespace score diff --git a/score/TimeSlave/BUILD b/score/TimeSlave/BUILD new file mode 100644 index 0000000..ca5de74 --- /dev/null +++ b/score/TimeSlave/BUILD @@ -0,0 +1,12 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* diff --git a/score/TimeSlave/code/BUILD b/score/TimeSlave/code/BUILD new file mode 100644 index 0000000..ca5de74 --- /dev/null +++ b/score/TimeSlave/code/BUILD @@ -0,0 +1,12 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* diff --git a/score/TimeSlave/code/application/BUILD b/score/TimeSlave/code/application/BUILD new file mode 100644 index 0000000..4f7eab4 --- /dev/null +++ b/score/TimeSlave/code/application/BUILD @@ -0,0 +1,33 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_binary( + name = "TimeSlave", + srcs = [ + "main.cpp", + "time_slave.cpp", + "time_slave.h", + ], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + deps = [ + "//score/TimeSlave/code/common:logging_contexts", + "//score/TimeSlave/code/gptp:gptp_engine", + "//score/libTSClient:gptp_ipc", + "//score/time/HighPrecisionLocalSteadyClock", + "@score_baselibs//score/mw/log:console_only_backend", + "@score_lifecycle_health//src/lifecycle_client_lib", + ], +) diff --git a/score/TimeSlave/code/application/main.cpp b/score/TimeSlave/code/application/main.cpp new file mode 100644 index 0000000..29f2478 --- /dev/null +++ b/score/TimeSlave/code/application/main.cpp @@ -0,0 +1,20 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/application/time_slave.h" + +#include "src/lifecycle_client_lib/include/runapplication.h" + +int main(int argc, const char* argv[]) +{ + return score::mw::lifecycle::run_application(argc, argv); +} diff --git a/score/TimeSlave/code/application/time_slave.cpp b/score/TimeSlave/code/application/time_slave.cpp new file mode 100644 index 0000000..c4e8d3e --- /dev/null +++ b/score/TimeSlave/code/application/time_slave.cpp @@ -0,0 +1,77 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/application/time_slave.h" + +#include "score/TimeSlave/code/common/logging_contexts.h" +#include "score/mw/log/logging.h" +#include "score/time/HighPrecisionLocalSteadyClock/details/factory_impl.h" + +#include + +namespace score +{ +namespace ts +{ + +TimeSlave::TimeSlave() = default; + +std::int32_t TimeSlave::Initialize(const score::mw::lifecycle::ApplicationContext& /*context*/) +{ + // Create the high-precision local clock for the gPTP engine + score::time::HighPrecisionLocalSteadyClock::FactoryImpl clock_factory{}; + auto clock = clock_factory.CreateHighPrecisionLocalSteadyClock(); + + engine_ = std::make_unique(opts_, std::move(clock)); + + if (!engine_->Initialize()) + { + score::mw::log::LogError(kGPtpMachineContext) << "TimeSlave: GptpEngine initialization failed"; + return -1; + } + + if (!publisher_.Init()) + { + score::mw::log::LogError(kGPtpMachineContext) << "TimeSlave: shared memory publisher initialization failed"; + return -1; + } + + score::mw::log::LogInfo(kGPtpMachineContext) << "TimeSlave initialized"; + return 0; +} + +std::int32_t TimeSlave::Run(const score::cpp::stop_token& token) +{ + constexpr auto kPublishInterval = std::chrono::milliseconds{50}; + + score::mw::log::LogInfo(kGPtpMachineContext) << "TimeSlave running"; + + while (!token.stop_requested()) + { + score::td::PtpTimeInfo info{}; + if (engine_->ReadPTPSnapshot(info)) + { + publisher_.Publish(info); + } + + std::this_thread::sleep_for(kPublishInterval); + } + + engine_->Deinitialize(); + publisher_.Destroy(); + + score::mw::log::LogInfo(kGPtpMachineContext) << "TimeSlave stopped"; + return 0; +} + +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/application/time_slave.h b/score/TimeSlave/code/application/time_slave.h new file mode 100644 index 0000000..5ae32a9 --- /dev/null +++ b/score/TimeSlave/code/application/time_slave.h @@ -0,0 +1,59 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_APPLICATION_TIME_SLAVE_H +#define SCORE_TIMESLAVE_CODE_APPLICATION_TIME_SLAVE_H + +#include "score/TimeSlave/code/gptp/gptp_engine.h" +#include "score/libTSClient/gptp_ipc_publisher.h" + +#include "src/lifecycle_client_lib/include/application.h" + +#include + +namespace score +{ +namespace ts +{ + +/** + * @brief Standalone TimeSlave process that runs the gPTP engine + * and publishes time data to shared memory. + * + * TimeSlave is the gPTP protocol endpoint. It runs GptpEngine internally + * (with RxThread + PdelayThread) and periodically writes PtpTimeInfo + * to shared memory for consumption by TimeDaemon via ShmPTPEngine. + */ +class TimeSlave final : public score::mw::lifecycle::Application +{ + public: + explicit TimeSlave(); + ~TimeSlave() noexcept override = default; + + TimeSlave(TimeSlave&&) noexcept = delete; + TimeSlave(const TimeSlave&) noexcept = delete; + TimeSlave& operator=(TimeSlave&&) & noexcept = delete; + TimeSlave& operator=(const TimeSlave&) & noexcept = delete; + + std::int32_t Initialize(const score::mw::lifecycle::ApplicationContext& context) override; + std::int32_t Run(const score::cpp::stop_token& token) override; + + private: + details::GptpEngineOptions opts_; + std::unique_ptr engine_; + details::GptpIpcPublisher publisher_; +}; + +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_APPLICATION_TIME_SLAVE_H diff --git a/score/TimeSlave/code/common/BUILD b/score/TimeSlave/code/common/BUILD new file mode 100644 index 0000000..45f383d --- /dev/null +++ b/score/TimeSlave/code/common/BUILD @@ -0,0 +1,18 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +cc_library( + name = "logging_contexts", + hdrs = ["logging_contexts.h"], + visibility = ["//score/TimeSlave:__subpackages__"], +) diff --git a/score/TimeSlave/code/common/logging_contexts.h b/score/TimeSlave/code/common/logging_contexts.h new file mode 100644 index 0000000..00ecc9e --- /dev/null +++ b/score/TimeSlave/code/common/logging_contexts.h @@ -0,0 +1,46 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +/* + * @Author: chenhao.gao chenhao.gao@ecarxgroup.com + * @Date: 2026-03-27 14:02:10 + * @LastEditors: chenhao.gao chenhao.gao@ecarxgroup.com + * @LastEditTime: 2026-03-27 14:03:37 + * @FilePath: /score_inc_time/score/TimeSlave/code/common/logging_contexts.h + * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE + */ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_COMMON_LOGGING_CONTEXTS_H +#define SCORE_TIMESLAVE_CODE_COMMON_LOGGING_CONTEXTS_H + +namespace score +{ +namespace ts +{ + +constexpr auto kGPtpMachineContext = "GPTP_SLAVE"; + +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_COMMON_LOGGING_CONTEXTS_H diff --git a/score/TimeSlave/code/gptp/BUILD b/score/TimeSlave/code/gptp/BUILD new file mode 100644 index 0000000..ca025a0 --- /dev/null +++ b/score/TimeSlave/code/gptp/BUILD @@ -0,0 +1,63 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "gptp_engine", + srcs = ["gptp_engine.cpp"], + hdrs = ["gptp_engine.h"], + features = COMPILER_WARNING_FEATURES, + linkopts = select({ + "@platforms//os:qnx": [ + "-lsocket", + "-lc", + ], + "//conditions:default": ["-lpthread"], + }), + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + "//score/TimeDaemon/code/common:logging_contexts", + "//score/TimeDaemon/code/common/data_types:ptp_time_info", + "//score/TimeSlave/code/gptp/details:gptp_details", + "//score/time/HighPrecisionLocalSteadyClock:interface", + "@score_baselibs//score/mw/log:frontend", + ], +) + +cc_test( + name = "gptp_engine_test", + srcs = ["gptp_engine_test.cpp"], + tags = ["unit"], + deps = [ + ":gptp_engine", + "//score/TimeSlave/code/gptp/details:i_network_identity", + "//score/TimeSlave/code/gptp/details:i_raw_socket", + "@googletest//:gtest", + "@googletest//:gtest_main", + "@score_baselibs//score/mw/log:console_only_backend", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":gptp_engine_test"], + test_suites_from_sub_packages = [ + "//score/TimeSlave/code/gptp/details:unit_test_suite", + "//score/TimeSlave/code/gptp/instrument:unit_test_suite", + "//score/TimeSlave/code/gptp/record:unit_test_suite", + ], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/details/BUILD b/score/TimeSlave/code/gptp/details/BUILD new file mode 100644 index 0000000..e1f1c20 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/BUILD @@ -0,0 +1,199 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "ptp_types", + hdrs = ["ptp_types.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], +) + +cc_library( + name = "i_raw_socket", + hdrs = ["i_raw_socket.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [], +) + +cc_library( + name = "i_network_identity", + hdrs = ["i_network_identity.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [":ptp_types"], +) + +cc_library( + name = "raw_socket", + srcs = select({ + "@platforms//os:qnx": ["//score/TimeSlave/code/gptp/platform/qnx:raw_socket_src"], + "//conditions:default": ["//score/TimeSlave/code/gptp/platform/linux:raw_socket_src"], + }), + hdrs = ["raw_socket.h"], + features = COMPILER_WARNING_FEATURES, + linkopts = select({ + "@platforms//os:qnx": ["-lsocket"], + "//conditions:default": [], + }), + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + ":i_raw_socket", + ":ptp_types", + ], +) + +cc_library( + name = "frame_codec", + srcs = ["frame_codec.cpp"], + hdrs = ["frame_codec.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [":ptp_types"], +) + +cc_library( + name = "message_parser", + srcs = ["message_parser.cpp"], + hdrs = ["message_parser.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [":ptp_types"], +) + +cc_library( + name = "sync_state_machine", + srcs = ["sync_state_machine.cpp"], + hdrs = ["sync_state_machine.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + ":ptp_types", + "//score/TimeDaemon/code/common/data_types:ptp_time_info", + ], +) + +cc_library( + name = "pdelay_measurer", + srcs = ["pdelay_measurer.cpp"], + hdrs = ["pdelay_measurer.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + ":frame_codec", + ":i_raw_socket", + ":ptp_types", + "//score/TimeDaemon/code/common/data_types:ptp_time_info", + ], +) + +cc_library( + name = "network_identity", + srcs = select({ + "@platforms//os:qnx": ["//score/TimeSlave/code/gptp/platform/qnx:network_identity_src"], + "//conditions:default": ["//score/TimeSlave/code/gptp/platform/linux:network_identity_src"], + }), + hdrs = ["network_identity.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + ":i_network_identity", + ":ptp_types", + ], +) + +cc_library( + name = "gptp_details", + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + ":frame_codec", + ":i_network_identity", + ":i_raw_socket", + ":message_parser", + ":network_identity", + ":pdelay_measurer", + ":ptp_types", + ":raw_socket", + ":sync_state_machine", + ], +) + +cc_test( + name = "pdelay_measurer_test", + srcs = ["pdelay_measurer_test.cpp"], + tags = ["unit"], + deps = [ + ":pdelay_measurer", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "frame_codec_test", + srcs = ["frame_codec_test.cpp"], + tags = ["unit"], + deps = [ + ":frame_codec", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "message_parser_test", + srcs = ["message_parser_test.cpp"], + tags = ["unit"], + deps = [ + ":message_parser", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "sync_state_machine_test", + srcs = ["sync_state_machine_test.cpp"], + tags = ["unit"], + deps = [ + ":sync_state_machine", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [ + ":frame_codec_test", + ":message_parser_test", + ":pdelay_measurer_test", + ":sync_state_machine_test", + ], + test_suites_from_sub_packages = [], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/details/frame_codec.cpp b/score/TimeSlave/code/gptp/details/frame_codec.cpp new file mode 100644 index 0000000..11491c7 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec.cpp @@ -0,0 +1,96 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/frame_codec.h" + +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +int Str2Mac(const char* s, unsigned char mac[kMacAddrLen]) noexcept +{ + unsigned int b[kMacAddrLen]{}; + if (std::sscanf(s, "%x:%x:%x:%x:%x:%x", &b[0], &b[1], &b[2], &b[3], &b[4], &b[5]) != kMacAddrLen) + { + return -1; + } + for (int i = 0; i < kMacAddrLen; ++i) + mac[i] = static_cast(b[i]); + return 0; +} + +} // namespace + +bool FrameCodec::ParseEthernetHeader(const std::uint8_t* frame, int frame_len, int& ptp_offset) const +{ + const int kEthHdrLen = static_cast(sizeof(ethhdr)); + if (frame_len < kEthHdrLen) + return false; + + ethhdr hdr{}; + std::memcpy(&hdr, frame, sizeof(hdr)); + + const auto etype = static_cast(ntohs(hdr.h_proto)); + + if (etype == static_cast(kEthP8021Q)) + { + // Skip 4-byte VLAN tag; re-read EtherType + if (frame_len < kEthHdrLen + kVlanTagLen + 2) + return false; + const uint16_t inner_etype_be = *reinterpret_cast(frame + kEthHdrLen + kVlanTagLen); + if (ntohs(inner_etype_be) != static_cast(kEthP1588)) + return false; + ptp_offset = kEthHdrLen + kVlanTagLen; + return true; + } + + if (etype != static_cast(kEthP1588)) + return false; + + ptp_offset = kEthHdrLen; + return true; +} + +bool FrameCodec::AddEthernetHeader(std::uint8_t* buf, unsigned int& buf_len) const +{ + constexpr unsigned int kMaxFrameSize = 2048U; + const unsigned int kHdrLen = static_cast(sizeof(ethhdr)); + + if (buf_len + kHdrLen > kMaxFrameSize) + return false; + + std::memmove(buf + kHdrLen, buf, buf_len); + + auto* hdr = reinterpret_cast(buf); + if (Str2Mac(kPtpSrcMac, hdr->h_source) != 0 || Str2Mac(kPtpDstMac, hdr->h_dest) != 0) + { + return false; + } + hdr->h_proto = htons(static_cast(kEthP1588)); + + buf_len += kHdrLen; + return true; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/frame_codec.h b/score/TimeSlave/code/gptp/details/frame_codec.h new file mode 100644 index 0000000..425105c --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec.h @@ -0,0 +1,66 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_FRAME_CODEC_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_FRAME_CODEC_H + +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief Ethernet frame encode/decode for PTP-over-L2. + * + * Uses the standard PTP multicast destination MAC 01:80:C2:00:00:0E and + * EtherType 0x88F7. VLAN-tagged frames are accepted on receive. + */ +class FrameCodec final +{ + public: + /** + * @brief Locate the PTP payload inside a raw Ethernet frame. + * + * Handles 802.1Q VLAN-tagged frames transparently. + * + * @param frame Raw frame bytes as received from the socket. + * @param frame_len Total length of @p frame in bytes. + * @param ptp_offset Output: byte offset where the PTP message starts. + * @return true if @p frame contains a PTP/1588 Ethertype, false otherwise. + */ + bool ParseEthernetHeader(const std::uint8_t* frame, int frame_len, int& ptp_offset) const; + + /** + * @brief Prepend an Ethernet header for PTP multicast transmission. + * + * Modifies @p buf in-place (shifts payload to make room) and increments + * @p buf_len by the size of the added header. + * + * @param buf Buffer large enough to hold existing payload plus header. + * @param buf_len In/out: payload length → frame length after prepend. + * @return true on success, false if the buffer would overflow. + */ + bool AddEthernetHeader(std::uint8_t* buf, unsigned int& buf_len) const; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_FRAME_CODEC_H diff --git a/score/TimeSlave/code/gptp/details/frame_codec_test.cpp b/score/TimeSlave/code/gptp/details/frame_codec_test.cpp new file mode 100644 index 0000000..ca5c486 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec_test.cpp @@ -0,0 +1,149 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/frame_codec.h" + +#include + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// Build a minimal raw Ethernet frame with the given EtherType in the ethhdr. +// The buffer is zero-initialized; callers fill in anything extra. +std::vector MakeEthFrame(std::uint16_t etype, int total_len) +{ + std::vector buf(static_cast(total_len), 0); + // h_proto at bytes 12-13 (big-endian) + const std::uint16_t etype_be = htons(etype); + std::memcpy(&buf[12], &etype_be, 2); + return buf; +} + +} // namespace + +class FrameCodecParseTest : public ::testing::Test +{ + protected: + FrameCodec codec_; +}; + +// ── ParseEthernetHeader ─────────────────────────────────────────────────────── + +TEST_F(FrameCodecParseTest, TooShort_ReturnsFalse) +{ + std::uint8_t tiny[10] = {}; + int offset = -1; + EXPECT_FALSE(codec_.ParseEthernetHeader(tiny, 10, offset)); +} + +TEST_F(FrameCodecParseTest, ExactlyEthHdrLength_NonPtp_ReturnsFalse) +{ + // 14 bytes, EtherType = 0x0800 (IPv4) — not PTP and not VLAN + auto buf = MakeEthFrame(0x0800, 14); + int offset = -1; + EXPECT_FALSE(codec_.ParseEthernetHeader(buf.data(), 14, offset)); +} + +TEST_F(FrameCodecParseTest, Eth1588_Valid_ReturnsTrueAndOffset14) +{ + // Plain PTP frame: ethhdr(14) + PTP payload + auto buf = MakeEthFrame(static_cast(kEthP1588), 80); + int offset = -1; + ASSERT_TRUE(codec_.ParseEthernetHeader(buf.data(), 80, offset)); + EXPECT_EQ(offset, 14); // PTP payload immediately after ethhdr +} + +TEST_F(FrameCodecParseTest, Vlan8021Q_ValidPtpInner_ReturnsTrueAndOffset18) +{ + // VLAN-tagged: ethhdr(14) + VLAN tag(4) + inner EtherType(2) + payload + // Minimum valid length = 20; inner EtherType is at bytes [18..19] + auto buf = MakeEthFrame(static_cast(kEthP8021Q), 60); + // Inner EtherType = kEthP1588 at offset 14 + kVlanTagLen = 18 + const std::uint16_t inner_be = htons(static_cast(kEthP1588)); + std::memcpy(&buf[14 + kVlanTagLen], &inner_be, 2); + int offset = -1; + ASSERT_TRUE(codec_.ParseEthernetHeader(buf.data(), 60, offset)); + EXPECT_EQ(offset, 14 + kVlanTagLen); +} + +TEST_F(FrameCodecParseTest, Vlan8021Q_TooShortForInnerType_ReturnsFalse) +{ + // kEthHdrLen(14) + kVlanTagLen(4) + 2 = 20; provide only 19 bytes + auto buf = MakeEthFrame(static_cast(kEthP8021Q), 19); + int offset = -1; + EXPECT_FALSE(codec_.ParseEthernetHeader(buf.data(), 19, offset)); +} + +TEST_F(FrameCodecParseTest, Vlan8021Q_NonPtpInnerType_ReturnsFalse) +{ + auto buf = MakeEthFrame(static_cast(kEthP8021Q), 30); + // Inner EtherType = IPv4 (non-PTP) + const std::uint16_t inner_be = htons(0x0800U); + std::memcpy(&buf[14 + kVlanTagLen], &inner_be, 2); + int offset = -1; + EXPECT_FALSE(codec_.ParseEthernetHeader(buf.data(), 30, offset)); +} + +TEST_F(FrameCodecParseTest, UnknownEtherType_ReturnsFalse) +{ + auto buf = MakeEthFrame(0xABCDU, 60); + int offset = -1; + EXPECT_FALSE(codec_.ParseEthernetHeader(buf.data(), 60, offset)); +} + +// ── AddEthernetHeader ───────────────────────────────────────────────────────── + +TEST_F(FrameCodecParseTest, AddEthernetHeader_NormalPayload_ReturnsTrueAndIncrementsLen) +{ + // Buffer large enough for payload + 14-byte header + constexpr unsigned int kPayloadLen = 44U; + std::uint8_t buf[256] = {}; + // Put a sentinel in the payload area so we can verify the shift + buf[0] = 0xDE; + buf[1] = 0xAD; + + unsigned int len = kPayloadLen; + ASSERT_TRUE(codec_.AddEthernetHeader(buf, len)); + EXPECT_EQ(len, kPayloadLen + 14U); + + // Payload was shifted right by 14 bytes + EXPECT_EQ(buf[14], 0xDE); + EXPECT_EQ(buf[15], 0xAD); + + // h_proto at bytes 12-13 should be kEthP1588 in network byte order + const std::uint16_t h_proto_be = htons(static_cast(kEthP1588)); + std::uint16_t actual{}; + std::memcpy(&actual, &buf[12], 2); + EXPECT_EQ(actual, h_proto_be); +} + +TEST_F(FrameCodecParseTest, AddEthernetHeader_PayloadTooLarge_ReturnsFalse) +{ + constexpr unsigned int kTooBig = 2048U; // buf_len + 14 > 2048 + std::uint8_t buf[4096] = {}; + unsigned int len = kTooBig; + EXPECT_FALSE(codec_.AddEthernetHeader(buf, len)); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/i_network_identity.h b/score/TimeSlave/code/gptp/details/i_network_identity.h new file mode 100644 index 0000000..92a4b1e --- /dev/null +++ b/score/TimeSlave/code/gptp/details/i_network_identity.h @@ -0,0 +1,44 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_NETWORK_IDENTITY_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_NETWORK_IDENTITY_H + +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Interface for resolving the IEEE 1588 ClockIdentity from a network interface. +class INetworkIdentity +{ + public: + virtual ~INetworkIdentity() noexcept = default; + + /// Resolve the ClockIdentity for @p iface_name. Returns true on success. + virtual bool Resolve(const std::string& iface_name) = 0; + + /// Return the resolved identity. Valid only after a successful Resolve(). + virtual ClockIdentity GetClockIdentity() const = 0; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_NETWORK_IDENTITY_H diff --git a/score/TimeSlave/code/gptp/details/i_raw_socket.h b/score/TimeSlave/code/gptp/details/i_raw_socket.h new file mode 100644 index 0000000..9858693 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/i_raw_socket.h @@ -0,0 +1,59 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_RAW_SOCKET_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_RAW_SOCKET_H + +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Interface for a platform raw socket used by GptpEngine and PeerDelayMeasurer. +class IRawSocket +{ + public: + virtual ~IRawSocket() noexcept = default; + + /// Open the socket bound to @p iface. Returns false on failure. + virtual bool Open(const std::string& iface) = 0; + + /// Configure hardware TX/RX timestamping. Returns false on failure. + virtual bool EnableHwTimestamping() = 0; + + /// Close the socket and release the file descriptor. + virtual void Close() = 0; + + /// Receive one frame. + /// @return Number of bytes received, 0 on timeout, -1 on error. + virtual int Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) = 0; + + /// Send one frame. + /// @return Number of bytes sent, or -1 on error. + virtual int Send(const void* buf, int len, ::timespec& hwts) = 0; + + /// Return the underlying file descriptor. + virtual int GetFd() const = 0; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_RAW_SOCKET_H diff --git a/score/TimeSlave/code/gptp/details/message_parser.cpp b/score/TimeSlave/code/gptp/details/message_parser.cpp new file mode 100644 index 0000000..fadc468 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/message_parser.cpp @@ -0,0 +1,115 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/message_parser.h" + +#include +#include + +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#define BSWAP64(x) __builtin_bswap64(x) +#else +#define BSWAP64(x) (x) +#endif + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +std::uint16_t LoadU16(const std::uint8_t* p) noexcept +{ + std::uint16_t v{}; + std::memcpy(&v, p, sizeof(v)); + return ntohs(v); +} + +std::uint32_t LoadU32(const std::uint8_t* p) noexcept +{ + std::uint32_t v{}; + std::memcpy(&v, p, sizeof(v)); + return ntohl(v); +} + +std::uint64_t LoadBe64(const std::uint8_t* p) noexcept +{ + std::uint64_t v{}; + std::memcpy(&v, p, sizeof(v)); + return BSWAP64(v); +} + +Timestamp LoadTimestamp(const std::uint8_t* p) noexcept +{ + Timestamp ts{}; + ts.seconds_msb = LoadU16(p); + ts.seconds_lsb = LoadU32(p + 2); + ts.nanoseconds = LoadU32(p + 6); + return ts; +} + +} // namespace + +bool GptpMessageParser::Parse(const std::uint8_t* payload, std::size_t payload_len, PTPMessage& msg) const +{ + if (payload == nullptr || payload_len < sizeof(PTPHeader)) + return false; + + msg.ptpHdr.tsmt = payload[0]; + msg.ptpHdr.version = payload[1]; + msg.ptpHdr.messageLength = LoadU16(payload + 2); + msg.ptpHdr.domainNumber = payload[4]; + msg.ptpHdr.reserved1 = payload[5]; + std::memcpy(msg.ptpHdr.flagField, payload + 6, 2); + msg.ptpHdr.correctionField = static_cast(LoadBe64(payload + 8)); + msg.ptpHdr.reserved2 = LoadU32(payload + 16); + std::memcpy(msg.ptpHdr.sourcePortIdentity.clockIdentity.id, payload + 20, 8); + msg.ptpHdr.sourcePortIdentity.portNumber = LoadU16(payload + 28); + msg.ptpHdr.sequenceId = LoadU16(payload + 30); + msg.ptpHdr.controlField = payload[32]; + msg.ptpHdr.logMessageInterval = static_cast(payload[33]); + + msg.msgtype = msg.ptpHdr.tsmt & 0x0FU; + + constexpr std::size_t kBodyOffset = 34U; + + switch (msg.msgtype) + { + case kPtpMsgtypeFollowUp: + if (payload_len >= kBodyOffset + sizeof(Timestamp)) + msg.follow_up.preciseOriginTimestamp = LoadTimestamp(payload + kBodyOffset); + break; + + case kPtpMsgtypePdelayResp: + if (payload_len >= kBodyOffset + sizeof(Timestamp)) + msg.pdelay_resp.responseOriginTimestamp = LoadTimestamp(payload + kBodyOffset); + break; + + case kPtpMsgtypePdelayRespFollowUp: + if (payload_len >= kBodyOffset + sizeof(Timestamp)) + msg.pdelay_resp_fup.responseOriginReceiptTimestamp = LoadTimestamp(payload + kBodyOffset); + break; + + default: + break; + } + + return true; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/message_parser.h b/score/TimeSlave/code/gptp/details/message_parser.h new file mode 100644 index 0000000..904b5c7 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/message_parser.h @@ -0,0 +1,55 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_MESSAGE_PARSER_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_MESSAGE_PARSER_H + +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief IEEE 802.1AS / 1588-v2 message parser. + * + * Decoupled from the socket layer: callers feed the PTP payload (post + * Ethernet-header stripping) as a byte buffer and receive a fully populated + * PTPMessage. + */ +class GptpMessageParser final +{ + public: + /** + * @brief Parse @p payload_len bytes at @p payload into @p msg. + * + * Populates the PTPHeader union fields and the message-type-specific body + * fields (Timestamps, PortIdentity, correctionField). Does NOT touch the + * hardware-timestamp fields (recvHardwareTS, sendHardwareTS) — those are + * filled by the caller after the socket recv. + * + * @return true if the payload contains a valid IEEE 1588 / 802.1AS header. + */ + bool Parse(const std::uint8_t* payload, std::size_t payload_len, PTPMessage& msg) const; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_MESSAGE_PARSER_H diff --git a/score/TimeSlave/code/gptp/details/message_parser_test.cpp b/score/TimeSlave/code/gptp/details/message_parser_test.cpp new file mode 100644 index 0000000..f42afdc --- /dev/null +++ b/score/TimeSlave/code/gptp/details/message_parser_test.cpp @@ -0,0 +1,210 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/message_parser.h" + +#include + +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// PTP header occupies exactly 34 bytes on the wire. +constexpr std::size_t kHdrSize = 34U; +// Timestamp body = 10 bytes (u16 + u32 + u32). +constexpr std::size_t kTsSize = 10U; + +// Store a 16-bit big-endian value at buf[off]. +void PutU16Be(std::uint8_t* buf, std::size_t off, std::uint16_t val) +{ + const std::uint16_t v = htons(val); + std::memcpy(buf + off, &v, 2); +} + +// Store a 32-bit big-endian value at buf[off]. +void PutU32Be(std::uint8_t* buf, std::size_t off, std::uint32_t val) +{ + const std::uint32_t v = htonl(val); + std::memcpy(buf + off, &v, 4); +} + +// Store a 64-bit big-endian value at buf[off]. +void PutU64Be(std::uint8_t* buf, std::size_t off, std::uint64_t val) +{ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + val = __builtin_bswap64(val); +#endif + std::memcpy(buf + off, &val, 8); +} + +// Build a minimal PTP payload of type `msgtype` with the given header fields. +// Optionally appends a 10-byte Timestamp body (seconds_lsb + nanoseconds). +std::vector BuildPayload(std::uint8_t msgtype, + std::uint16_t seqId, + std::int64_t correction = 0, + std::uint16_t port_number = 0, + std::uint64_t clock_id = 0, + std::uint32_t ts_sec_lsb = 0, + std::uint32_t ts_ns = 0) +{ + const std::size_t total = kHdrSize + kTsSize; + std::vector buf(total, 0); + + buf[0] = static_cast((kPtpTransportSpecific) | (msgtype & 0x0FU)); + buf[1] = kPtpVersion; + PutU16Be(buf.data(), 2, static_cast(total)); // messageLength + // domainNumber = 0 (default) + PutU64Be(buf.data(), 8, static_cast(correction)); // correctionField + // Clock identity is a raw byte array; store in native order so ClockIdentityToU64 roundtrips. + std::memcpy(buf.data() + 20, &clock_id, 8); + PutU16Be(buf.data(), 28, port_number); + PutU16Be(buf.data(), 30, seqId); + buf[32] = kCtlFollowUp; + + // Timestamp body at offset 34: seconds_msb(u16) + seconds_lsb(u32) + nanoseconds(u32) + PutU16Be(buf.data(), kHdrSize, 0U); // seconds_msb = 0 + PutU32Be(buf.data(), kHdrSize + 2, ts_sec_lsb); + PutU32Be(buf.data(), kHdrSize + 6, ts_ns); + + return buf; +} + +} // namespace + +class MessageParserTest : public ::testing::Test +{ + protected: + GptpMessageParser parser_; +}; + +// ── Rejection cases ─────────────────────────────────────────────────────────── + +TEST_F(MessageParserTest, NullPayload_ReturnsFalse) +{ + PTPMessage msg{}; + EXPECT_FALSE(parser_.Parse(nullptr, 64U, msg)); +} + +TEST_F(MessageParserTest, TooShortPayload_ReturnsFalse) +{ + std::uint8_t tiny[10] = {}; + PTPMessage msg{}; + EXPECT_FALSE(parser_.Parse(tiny, 10U, msg)); +} + +// ── Sync (no body decoded, only header) ─────────────────────────────────────── + +TEST_F(MessageParserTest, SyncMessage_ReturnsTrue_MsgtypeIsSync) +{ + auto buf = BuildPayload(kPtpMsgtypeSync, 7U); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.msgtype, kPtpMsgtypeSync); +} + +TEST_F(MessageParserTest, Header_SequenceId_DecodedCorrectly) +{ + const std::uint16_t kSeq = 0x1234U; + auto buf = BuildPayload(kPtpMsgtypeSync, kSeq); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.ptpHdr.sequenceId, kSeq); +} + +TEST_F(MessageParserTest, Header_CorrectionField_DecodedCorrectly) +{ + // correctionField = 65536 (0x10000) → CorrectionToTmv would give 1 ns + const std::int64_t kCorr = 65536LL; + auto buf = BuildPayload(kPtpMsgtypeSync, 1U, kCorr); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.ptpHdr.correctionField, kCorr); +} + +TEST_F(MessageParserTest, Header_SourcePortIdentity_DecodedCorrectly) +{ + const std::uint64_t kClockId = 0xCAFEBABEDEAD0001ULL; + const std::uint16_t kPort = 3U; + auto buf = BuildPayload(kPtpMsgtypeSync, 1U, 0, kPort, kClockId); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.ptpHdr.sourcePortIdentity.portNumber, kPort); + EXPECT_EQ(ClockIdentityToU64(msg.ptpHdr.sourcePortIdentity.clockIdentity), kClockId); +} + +// ── FollowUp body ───────────────────────────────────────────────────────────── + +TEST_F(MessageParserTest, FollowUp_Body_TimestampDecodedCorrectly) +{ + // precise_origin = 2 seconds + 500_000_000 ns + const std::uint32_t kSecLsb = 2U; + const std::uint32_t kNs = 500'000'000U; + auto buf = BuildPayload(kPtpMsgtypeFollowUp, 99U, 0, 0, 0, kSecLsb, kNs); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.msgtype, kPtpMsgtypeFollowUp); + EXPECT_EQ(msg.follow_up.preciseOriginTimestamp.seconds_lsb, kSecLsb); + EXPECT_EQ(msg.follow_up.preciseOriginTimestamp.nanoseconds, kNs); +} + +// ── PdelayResp body ─────────────────────────────────────────────────────────── + +TEST_F(MessageParserTest, PdelayResp_Body_TimestampDecodedCorrectly) +{ + const std::uint32_t kSecLsb = 3U; + const std::uint32_t kNs = 123'456'789U; + auto buf = BuildPayload(kPtpMsgtypePdelayResp, 5U, 0, 0, 0, kSecLsb, kNs); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.msgtype, kPtpMsgtypePdelayResp); + EXPECT_EQ(msg.pdelay_resp.responseOriginTimestamp.seconds_lsb, kSecLsb); + EXPECT_EQ(msg.pdelay_resp.responseOriginTimestamp.nanoseconds, kNs); +} + +// ── PdelayRespFollowUp body ─────────────────────────────────────────────────── + +TEST_F(MessageParserTest, PdelayRespFollowUp_Body_TimestampDecodedCorrectly) +{ + const std::uint32_t kSecLsb = 7U; + const std::uint32_t kNs = 999'000'000U; + auto buf = BuildPayload(kPtpMsgtypePdelayRespFollowUp, 11U, 0, 0, 0, kSecLsb, kNs); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.msgtype, kPtpMsgtypePdelayRespFollowUp); + EXPECT_EQ(msg.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_lsb, kSecLsb); + EXPECT_EQ(msg.pdelay_resp_fup.responseOriginReceiptTimestamp.nanoseconds, kNs); +} + +// ── Unknown type: header parsed, no body crash ──────────────────────────────── + +TEST_F(MessageParserTest, UnknownMsgtype_ReturnsTrue_HeaderParsed) +{ + // Use PdelayReq (type 0x2) which has no special body decoding branch. + auto buf = BuildPayload(kPtpMsgtypePdelayReq, 20U); + PTPMessage msg{}; + ASSERT_TRUE(parser_.Parse(buf.data(), buf.size(), msg)); + EXPECT_EQ(msg.msgtype, kPtpMsgtypePdelayReq); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/network_identity.h b/score/TimeSlave/code/gptp/details/network_identity.h new file mode 100644 index 0000000..3150bc0 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/network_identity.h @@ -0,0 +1,56 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_NETWORK_IDENTITY_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_NETWORK_IDENTITY_H + +#include "score/TimeSlave/code/gptp/details/i_network_identity.h" +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief Derive the IEEE 1588 ClockIdentity from a network interface. + * + * The identity is built from the interface's EUI-48 MAC address by inserting + * 0xFF 0xFE at positions 3–4 to form an EUI-64 (per IEEE 1588-2019 §7.5.2.2). + * Platform implementation: Linux + QNX via #ifdef. + */ +class NetworkIdentity : public INetworkIdentity +{ + public: + /// Resolve the ClockIdentity for @p iface_name. + /// @return true on success. + bool Resolve(const std::string& iface_name) override; + + /// Return the resolved identity. Valid only after a successful Resolve(). + ClockIdentity GetClockIdentity() const override + { + return identity_; + } + + private: + ClockIdentity identity_{}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_NETWORK_IDENTITY_H diff --git a/score/TimeSlave/code/gptp/details/pdelay_measurer.cpp b/score/TimeSlave/code/gptp/details/pdelay_measurer.cpp new file mode 100644 index 0000000..c13eae6 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/pdelay_measurer.cpp @@ -0,0 +1,137 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/pdelay_measurer.h" +#include "score/TimeSlave/code/gptp/details/frame_codec.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +PeerDelayMeasurer::PeerDelayMeasurer(const ClockIdentity& local_identity) noexcept : local_identity_{local_identity} {} + +int PeerDelayMeasurer::SendRequest(IRawSocket& socket) +{ + PTPMessage req{}; + req.ptpHdr.tsmt = kPtpMsgtypePdelayReq | kPtpTransportSpecific; + req.ptpHdr.version = kPtpVersion; + req.ptpHdr.domainNumber = 0; + req.ptpHdr.messageLength = htons(sizeof(PdelayReqBody)); + req.ptpHdr.flagField[0] = 0; + req.ptpHdr.flagField[1] = 0; + req.ptpHdr.correctionField = 0; + req.ptpHdr.reserved2 = 0; + req.ptpHdr.sourcePortIdentity.clockIdentity = local_identity_; + req.ptpHdr.sourcePortIdentity.portNumber = htons(0x0001U); + req.ptpHdr.sequenceId = htons(static_cast(seqnum_)); + req.ptpHdr.controlField = kCtlOther; + req.ptpHdr.logMessageInterval = 0x7F; + + // Save a copy with host-byte-order sequence ID for later matching + { + std::lock_guard lk(mutex_); + req_ = req; + req_.ptpHdr.sequenceId = static_cast(seqnum_); + } + ++seqnum_; + + auto buf = reinterpret_cast(&req); + unsigned int len = sizeof(PdelayReqBody); + FrameCodec codec; + if (!codec.AddEthernetHeader(buf, len)) + return -1; + + ::timespec hwts{}; + const int r = socket.Send(buf, static_cast(len), hwts); + if (r > 0) + { + std::lock_guard lk(mutex_); + req_.sendHardwareTS = TmvT{static_cast(hwts.tv_sec) * kNsPerSec + hwts.tv_nsec}; + } + return r; +} + +void PeerDelayMeasurer::OnResponse(const PTPMessage& msg) +{ + std::lock_guard lk(mutex_); + resp_ = msg; +} + +void PeerDelayMeasurer::OnResponseFollowUp(const PTPMessage& msg) +{ + { + std::lock_guard lk(mutex_); + resp_fup_ = msg; + } + ComputeAndStore(); +} + +void PeerDelayMeasurer::ComputeAndStore() noexcept +{ + std::lock_guard lk(mutex_); + + // All three messages must share the same sequence ID + if (req_.ptpHdr.sequenceId != resp_.ptpHdr.sequenceId) + return; + if (resp_.ptpHdr.sequenceId != resp_fup_.ptpHdr.sequenceId) + return; + + // t1 = HW send timestamp of our Pdelay_Req + const TmvT t1 = req_.sendHardwareTS; + // t2 = remote receipt time (from Pdelay_Resp body: requestReceiptTimestamp) + const TmvT t2 = resp_.parseMessageTs; + // t3 = remote send time (from Pdelay_Resp_FUP body) + corrections + const TmvT t3 = resp_fup_.parseMessageTs; + const TmvT c1 = CorrectionToTmv(resp_.ptpHdr.correctionField); + const TmvT c2 = CorrectionToTmv(resp_fup_.ptpHdr.correctionField); + const TmvT t3c = TmvT{t3.ns + c1.ns + c2.ns}; + // t4 = local HW receive timestamp of Pdelay_Resp + const TmvT t4 = resp_.recvHardwareTS; + + const std::int64_t delay = ((t2.ns - t1.ns) + (t4.ns - t3c.ns)) / 2LL; + + PDelayResult r{}; + r.path_delay_ns = delay; + r.valid = true; + + score::td::PDelayData& d = r.pdelay_data; + d.request_origin_timestamp = static_cast(t1.ns); + d.request_receipt_timestamp = static_cast(t2.ns); + d.response_origin_timestamp = static_cast(t3.ns); + d.response_receipt_timestamp = static_cast(t4.ns); + d.reference_global_timestamp = static_cast(t3c.ns); + d.reference_local_timestamp = static_cast(t4.ns); + d.sequence_id = resp_.ptpHdr.sequenceId; + d.pdelay = static_cast(delay); + d.req_port_number = req_.ptpHdr.sourcePortIdentity.portNumber; + d.req_clock_identity = ClockIdentityToU64(req_.ptpHdr.sourcePortIdentity.clockIdentity); + d.resp_port_number = resp_.ptpHdr.sourcePortIdentity.portNumber; + d.resp_clock_identity = ClockIdentityToU64(resp_.ptpHdr.sourcePortIdentity.clockIdentity); + + result_ = r; +} + +PDelayResult PeerDelayMeasurer::GetResult() const +{ + std::lock_guard lk(mutex_); + return result_; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/pdelay_measurer.h b/score/TimeSlave/code/gptp/details/pdelay_measurer.h new file mode 100644 index 0000000..981f3bb --- /dev/null +++ b/score/TimeSlave/code/gptp/details/pdelay_measurer.h @@ -0,0 +1,85 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PDELAY_MEASURER_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PDELAY_MEASURER_H + +#include "score/TimeDaemon/code/common/data_types/ptp_time_info.h" +#include "score/TimeSlave/code/gptp/details/i_raw_socket.h" +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Result produced by a completed Pdelay measurement cycle. +struct PDelayResult +{ + std::int64_t path_delay_ns{0}; + score::td::PDelayData pdelay_data{}; + bool valid{false}; +}; + +/** + * @brief Measures one-way peer delay using the IEEE 802.1AS Pdelay mechanism. + * + * Implements the IEEE 802.1AS two-step peer-delay measurement: + * path_delay = ((t2 − t1) + (t4 − t3c)) / 2 + * + * Thread-safety: @c SendRequest() is called from the PdelayThread. + * @c OnResponse() / @c OnResponseFollowUp() / @c GetResult() + * are called from the RxThread. An internal mutex makes the + * class safe for this two-thread usage pattern. + */ +class PeerDelayMeasurer final +{ + public: + explicit PeerDelayMeasurer(const ClockIdentity& local_identity) noexcept; + + /// Build and transmit a Pdelay_Req frame. @p socket must be open. + /// @return 0 on success, negative on error. + int SendRequest(IRawSocket& socket); + + /// Process an incoming Pdelay_Resp message. + void OnResponse(const PTPMessage& msg); + + /// Process an incoming Pdelay_Resp_Follow_Up message; triggers computation. + void OnResponseFollowUp(const PTPMessage& msg); + + /// Return the latest computed measurement (or invalid if none yet). + PDelayResult GetResult() const; + + private: + void ComputeAndStore() noexcept; + + ClockIdentity local_identity_{}; + + mutable std::mutex mutex_; + + int seqnum_{0}; + PTPMessage req_{}; + PTPMessage resp_{}; + PTPMessage resp_fup_{}; + PDelayResult result_{}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PDELAY_MEASURER_H diff --git a/score/TimeSlave/code/gptp/details/pdelay_measurer_test.cpp b/score/TimeSlave/code/gptp/details/pdelay_measurer_test.cpp new file mode 100644 index 0000000..f0362f3 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/pdelay_measurer_test.cpp @@ -0,0 +1,165 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/pdelay_measurer.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// Build a PTPMessage suitable for OnResponse / OnResponseFollowUp. +// seqId must be 0 to match the default-constructed req_ inside PeerDelayMeasurer +// (req_.ptpHdr.sequenceId == 0 before SendRequest is ever called). +PTPMessage MakeResp(std::uint16_t seqId, + std::int64_t parse_ts_ns, // t2 or t3 + std::int64_t recv_hw_ns = 0, // t4 (only used in Resp, not FUP) + std::int64_t corr_ns = 0) noexcept +{ + PTPMessage msg{}; + msg.ptpHdr.sequenceId = seqId; + msg.ptpHdr.correctionField = corr_ns << 16; // CorrectionToTmv does >> 16 + msg.parseMessageTs.ns = parse_ts_ns; + msg.recvHardwareTS.ns = recv_hw_ns; + return msg; +} + +} // namespace + +class PeerDelayMeasurerTest : public ::testing::Test +{ + protected: + // ClockIdentity is all-zeros; sufficient for the delay computation tests. + PeerDelayMeasurer measurer_{ClockIdentity{}}; +}; + +// ── Default state ───────────────────────────────────────────────────────────── + +TEST_F(PeerDelayMeasurerTest, GetResult_BeforeAnyMessage_IsInvalid) +{ + EXPECT_FALSE(measurer_.GetResult().valid); + EXPECT_EQ(measurer_.GetResult().path_delay_ns, 0LL); +} + +// ── Sequence-ID mismatch guards ─────────────────────────────────────────────── + +TEST_F(PeerDelayMeasurerTest, SeqIdMismatch_BetweenReqAndResp_NoResult) +{ + // Default req_.ptpHdr.sequenceId == 0; resp has seqId == 1 → mismatch. + measurer_.OnResponse(MakeResp(1U, 100LL, 180LL)); + measurer_.OnResponseFollowUp(MakeResp(1U, 80LL)); + EXPECT_FALSE(measurer_.GetResult().valid); +} + +TEST_F(PeerDelayMeasurerTest, SeqIdMismatch_BetweenRespAndFup_NoResult) +{ + // resp seqId == 0 (matches default req_), resp_fup seqId == 1 → mismatch. + measurer_.OnResponse(MakeResp(0U, 100LL, 180LL)); + measurer_.OnResponseFollowUp(MakeResp(1U, 80LL)); + EXPECT_FALSE(measurer_.GetResult().valid); +} + +// ── Delay computation (symmetric link) ─────────────────────────────────────── +// +// Default req_ gives: t1 = 0 ns (sendHardwareTS == 0) +// +// Chosen timestamps: +// t2 (resp.parseMessageTs) = 100 ns (remote receipt time) +// t3 (resp_fup.parseMessageTs) = 80 ns (remote send time) +// t4 (resp.recvHardwareTS) = 180 ns (local receive time) +// +// delay = ((t2 − t1) + (t4 − t3)) / 2 +// = ((100 − 0) + (180 − 80)) / 2 +// = (100 + 100) / 2 +// = 100 ns + +TEST_F(PeerDelayMeasurerTest, Computation_SymmetricLink_CorrectDelay) +{ + measurer_.OnResponse(MakeResp(0U, /*t2=*/100LL, /*t4=*/180LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, /*t3=*/80LL)); + + const PDelayResult r = measurer_.GetResult(); + ASSERT_TRUE(r.valid); + EXPECT_EQ(r.path_delay_ns, 100LL); +} + +TEST_F(PeerDelayMeasurerTest, Computation_AsymmetricLink_CorrectDelay) +{ + // t1=0, t2=200, t3=150, t4=400 → ((200-0) + (400-150)) / 2 = (200+250)/2 = 225 + measurer_.OnResponse(MakeResp(0U, 200LL, 400LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, 150LL)); + + const PDelayResult r = measurer_.GetResult(); + ASSERT_TRUE(r.valid); + EXPECT_EQ(r.path_delay_ns, 225LL); +} + +// ── Correction field applied to t3 ─────────────────────────────────────────── +// +// t1=0, t2=100, t4=180 +// t3=80 ns, correction_resp = 2 ns (stored as 2<<16), correction_fup = 0 +// t3c = t3 + c1 + c2 = 80 + 2 + 0 = 82 +// delay = ((100-0) + (180-82)) / 2 = (100+98) / 2 = 99 + +TEST_F(PeerDelayMeasurerTest, Computation_CorrectionField_AppliedToT3) +{ + measurer_.OnResponse(MakeResp(0U, 100LL, 180LL, /*corr_ns=*/2LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, 80LL)); + + const PDelayResult r = measurer_.GetResult(); + ASSERT_TRUE(r.valid); + EXPECT_EQ(r.path_delay_ns, 99LL); +} + +// ── PDelayData fields ───────────────────────────────────────────────────────── + +TEST_F(PeerDelayMeasurerTest, PDelayData_TimestampFields_PopulatedCorrectly) +{ + measurer_.OnResponse(MakeResp(0U, /*t2=*/100LL, /*t4=*/180LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, /*t3=*/80LL)); + + const score::td::PDelayData& d = measurer_.GetResult().pdelay_data; + EXPECT_EQ(d.request_origin_timestamp, 0ULL); // t1 + EXPECT_EQ(d.request_receipt_timestamp, 100ULL); // t2 + EXPECT_EQ(d.response_origin_timestamp, 80ULL); // t3 + EXPECT_EQ(d.response_receipt_timestamp, 180ULL); // t4 + EXPECT_EQ(d.pdelay, 100ULL); // computed delay +} + +// ── Multiple cycles: result updated on each valid completion ────────────────── + +TEST_F(PeerDelayMeasurerTest, SecondCycle_OverwritesPreviousResult) +{ + // First measurement: delay = 100 ns + measurer_.OnResponse(MakeResp(0U, 100LL, 180LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, 80LL)); + ASSERT_TRUE(measurer_.GetResult().valid); + + // Second measurement with same seqId (still 0): delay = 50 ns + // t1=0, t2=50, t3=25, t4=100 → ((50+75)/2=62 ... let me recalculate) + // t1=0, t2=50, t4=100, t3=50 → ((50-0)+(100-50))/2 = (50+50)/2 = 50 + measurer_.OnResponse(MakeResp(0U, 50LL, 100LL)); + measurer_.OnResponseFollowUp(MakeResp(0U, 50LL)); + + EXPECT_EQ(measurer_.GetResult().path_delay_ns, 50LL); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/ptp_types.h b/score/TimeSlave/code/gptp/details/ptp_types.h new file mode 100644 index 0000000..187bbec --- /dev/null +++ b/score/TimeSlave/code/gptp/details/ptp_types.h @@ -0,0 +1,222 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PTP_TYPES_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PTP_TYPES_H + +#include +#include +#include + +#ifndef _QNX_PLAT +#include +#else +// Minimal ethhdr definition for QNX +struct ethhdr +{ + unsigned char h_dest[6]; + unsigned char h_source[6]; + uint16_t h_proto; +}; +#endif + +#ifndef PACKED +#define PACKED __attribute__((packed)) +#endif + +namespace score +{ +namespace ts +{ +namespace details +{ + +// ─── EtherType constants ──────────────────────────────────────────────────── +constexpr int kEthP1588 = 0x88F7; +constexpr int kEthP8021Q = 0x8100; + +// ─── MAC / buffer sizes ───────────────────────────────────────────────────── +constexpr int kMacAddrLen = 6; +constexpr int kVlanTagLen = 4; + +// ─── PTP message-type codes ───────────────────────────────────────────────── +constexpr std::uint8_t kPtpMsgtypeSync = 0x0; +constexpr std::uint8_t kPtpMsgtypePdelayReq = 0x2; +constexpr std::uint8_t kPtpMsgtypePdelayResp = 0x3; +constexpr std::uint8_t kPtpMsgtypeFollowUp = 0x8; +constexpr std::uint8_t kPtpMsgtypePdelayRespFollowUp = 0xA; + +// ─── PTP header constants ──────────────────────────────────────────────────── +constexpr std::uint8_t kPtpTransportSpecific = (1U << 4U); +constexpr std::uint8_t kPtpVersion = 2U; + +constexpr std::int64_t kNsPerSec = 1'000'000'000LL; + +// ─── MAC addresses ─────────────────────────────────────────────────────────── +constexpr const char* kPtpSrcMac = "02:00:00:FF:00:11"; +constexpr const char* kPtpDstMac = "01:80:C2:00:00:0E"; + +// ─── Control field ─────────────────────────────────────────────────────────── +enum ControlField : std::uint8_t +{ + kCtlSync = 0, + kCtlDelayReq = 1, + kCtlFollowUp = 2, + kCtlDelayResp = 3, + kCtlManagement = 4, + kCtlOther = 5 +}; + +// ─── State machine states ──────────────────────────────────────────────────── +enum class SyncState : std::uint8_t +{ + kEmpty, + kHaveSync, + kHaveFup +}; + +// ─── Time value type ───────────────────────────────────────────────────────── +struct TmvT +{ + std::int64_t ns{0}; +}; + +// ─── PTP wire structures (all PACKED) ──────────────────────────────────────── +struct PACKED ClockIdentity +{ + std::uint8_t id[8]{}; +}; + +struct PACKED PortIdentity +{ + ClockIdentity clockIdentity; + std::uint16_t portNumber{0}; +}; + +struct PACKED Timestamp +{ + std::uint16_t seconds_msb{0}; + std::uint32_t seconds_lsb{0}; + std::uint32_t nanoseconds{0}; +}; + +struct PACKED PTPHeader +{ + std::uint8_t tsmt{0}; + std::uint8_t version{0}; + std::uint16_t messageLength{0}; + std::uint8_t domainNumber{0}; + std::uint8_t reserved1{0}; + std::uint8_t flagField[2]{}; + std::int64_t correctionField{0}; + std::uint32_t reserved2{0}; + PortIdentity sourcePortIdentity{}; + std::uint16_t sequenceId{0}; + std::uint8_t controlField{0}; + std::int8_t logMessageInterval{0}; +}; + +struct PACKED SyncBody +{ + PTPHeader ptpHdr{}; + Timestamp originTimestamp{}; +}; + +struct PACKED FollowUpBody +{ + PTPHeader ptpHdr{}; + Timestamp preciseOriginTimestamp{}; +}; + +struct PACKED PdelayReqBody +{ + PTPHeader ptpHdr{}; + Timestamp requestReceiptTimestamp{}; + PortIdentity reserved{}; +}; + +struct PACKED PdelayRespBody +{ + PTPHeader ptpHdr{}; + Timestamp responseOriginTimestamp{}; + PortIdentity requestingPortIdentity{}; +}; + +struct PACKED PdelayRespFollowUpBody +{ + PTPHeader ptpHdr{}; + Timestamp responseOriginReceiptTimestamp{}; + PortIdentity requestingPortIdentity{}; +}; + +struct PACKED RawMessageData +{ + std::uint8_t buffer[1500]{}; +}; + +struct PTPMessage +{ + union PACKED + { + PTPHeader ptpHdr; + SyncBody sync; + FollowUpBody follow_up; + PdelayReqBody pdelay_req; + PdelayRespBody pdelay_resp; + PdelayRespFollowUpBody pdelay_resp_fup; + RawMessageData data; + }; + + std::uint8_t msgtype{0}; + TmvT sendHardwareTS{}; + TmvT parseMessageTs{}; + TmvT recvHardwareTS{}; +}; + +static_assert(sizeof(PTPMessage) <= 1600, "PTPMessage too large"); + +// ─── Timestamp conversion helpers ──────────────────────────────────────────── +inline TmvT TimestampToTmv(const Timestamp& ts) noexcept +{ + const std::uint64_t sec = + (static_cast(ts.seconds_msb) << 32U) | static_cast(ts.seconds_lsb); + return TmvT{static_cast(sec * static_cast(kNsPerSec) + ts.nanoseconds)}; +} + +inline Timestamp TmvToTimestamp(const TmvT& x) noexcept +{ + Timestamp t{}; + const std::uint64_t sec = static_cast(x.ns) / 1'000'000'000ULL; + const std::uint64_t nsec = static_cast(x.ns) % 1'000'000'000ULL; + t.seconds_lsb = static_cast(sec & 0xFFFFFFFFULL); + t.seconds_msb = static_cast((sec >> 32U) & 0xFFFFULL); + t.nanoseconds = static_cast(nsec); + return t; +} + +inline TmvT CorrectionToTmv(std::int64_t corr) noexcept +{ + return TmvT{corr >> 16}; +} + +inline std::uint64_t ClockIdentityToU64(const ClockIdentity& ci) noexcept +{ + std::uint64_t v{0}; + std::memcpy(&v, ci.id, sizeof(v)); + return v; +} + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PTP_TYPES_H diff --git a/score/TimeSlave/code/gptp/details/raw_socket.h b/score/TimeSlave/code/gptp/details/raw_socket.h new file mode 100644 index 0000000..b0be138 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/raw_socket.h @@ -0,0 +1,90 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_RAW_SOCKET_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_RAW_SOCKET_H + +#include "score/TimeSlave/code/gptp/details/i_raw_socket.h" + +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief Platform raw socket for Ethernet I/O with hardware timestamping. + * + * On Linux uses AF_PACKET / SO_TIMESTAMPING. + * On QNX uses the QNX raw-socket shim. + */ +class RawSocket : public IRawSocket +{ + public: + RawSocket() noexcept = default; + ~RawSocket() override; + + RawSocket(const RawSocket&) = delete; + RawSocket& operator=(const RawSocket&) = delete; + RawSocket(RawSocket&&) = delete; + RawSocket& operator=(RawSocket&&) = delete; + + /// Open the socket bound to @p iface. Returns false on failure. + bool Open(const std::string& iface) override; + + /// Configure hardware TX/RX timestamping on the already-opened socket. + /// Returns false on failure. A no-op on platforms that don't support it. + bool EnableHwTimestamping() override; + + /// Close the socket and release the file descriptor. + void Close() override; + + /// Receive one frame. + /// + /// @param buf Output buffer. + /// @param buf_len Capacity of @p buf. + /// @param hwts Output: hardware receive timestamp (zeroed if unavailable). + /// @param timeout_ms <0 block indefinitely, 0 non-blocking, >0 timeout in ms. + /// @return Number of bytes received, 0 on timeout, -1 on error. + int Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) override; + + /// Send one frame. + /// + /// @param buf Frame data including Ethernet header. + /// @param len Number of bytes to send. + /// @param hwts Output: hardware transmit timestamp (zeroed if unavailable). + /// @return Number of bytes sent, or -1 on error. + int Send(const void* buf, int len, ::timespec& hwts) override; + + /// Return the underlying file descriptor (for advanced use / polling). + int GetFd() const override + { + return fd_; + } + + private: + int fd_{-1}; + std::string iface_{}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_RAW_SOCKET_H diff --git a/score/TimeSlave/code/gptp/details/sync_state_machine.cpp b/score/TimeSlave/code/gptp/details/sync_state_machine.cpp new file mode 100644 index 0000000..8c89af2 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/sync_state_machine.cpp @@ -0,0 +1,160 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/sync_state_machine.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +std::int64_t MonoNs() noexcept +{ + ::timespec ts{}; + ::clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * kNsPerSec + ts.tv_nsec; +} + +} // namespace + +SyncStateMachine::SyncStateMachine(std::int64_t jump_future_threshold_ns) noexcept + : jump_future_threshold_ns_{jump_future_threshold_ns} +{ +} + +void SyncStateMachine::OnSync(const PTPMessage& msg) +{ + switch (state_) + { + case SyncState::kEmpty: + last_sync_ = msg; + state_ = SyncState::kHaveSync; + break; + + case SyncState::kHaveSync: + // Newer Sync replaces the stale one (master sends faster than FUP arrives) + last_sync_ = msg; + break; + + case SyncState::kHaveFup: + // Buffered FUP is now stale; start fresh with the new Sync + last_sync_ = msg; + state_ = SyncState::kHaveSync; + break; + } +} + +std::optional SyncStateMachine::OnFollowUp(const PTPMessage& msg) +{ + switch (state_) + { + case SyncState::kEmpty: + // FUP arrived before its Sync — buffer it and wait + last_fup_ = msg; + state_ = SyncState::kHaveFup; + return std::nullopt; + + case SyncState::kHaveFup: + // Another FUP without a matching Sync — replace buffer + last_fup_ = msg; + return std::nullopt; + + case SyncState::kHaveSync: + if (last_sync_.ptpHdr.sequenceId != msg.ptpHdr.sequenceId) + { + // Sequence-ID mismatch: buffer the FUP and wait for matching Sync + last_fup_ = msg; + state_ = SyncState::kHaveFup; + return std::nullopt; + } + + { + SyncResult result = BuildResult(last_sync_, msg); + state_ = SyncState::kEmpty; + last_sync_mono_ns_.store(MonoNs(), std::memory_order_release); + return result; + } + } + return std::nullopt; +} + +bool SyncStateMachine::IsTimeout(std::int64_t mono_now_ns, std::int64_t timeout_ns) const +{ + if (timeout_ns <= 0) + return false; + const std::int64_t last = last_sync_mono_ns_.load(std::memory_order_acquire); + if (last == 0) + return false; // never synchronized yet — not a "timeout" + return (mono_now_ns - last) > timeout_ns; +} + +SyncResult SyncStateMachine::BuildResult(const PTPMessage& sync, const PTPMessage& fup) noexcept +{ + const TmvT sync_corr = CorrectionToTmv(sync.ptpHdr.correctionField); + const TmvT fup_corr = CorrectionToTmv(fup.ptpHdr.correctionField); + const TmvT fup_ts = TimestampToTmv(fup.follow_up.preciseOriginTimestamp); + + const std::int64_t master_ns = fup_ts.ns + sync_corr.ns + fup_corr.ns; + const std::int64_t offset_ns = sync.recvHardwareTS.ns - master_ns; + + SyncResult r{}; + r.master_ns = master_ns; + r.offset_ns = offset_ns; + + if (last_master_ns_ != 0) + { + const std::int64_t delta = master_ns - last_master_ns_; + if (delta < 0) + r.is_time_jump_past = true; + else if (jump_future_threshold_ns_ > 0 && delta > jump_future_threshold_ns_) + r.is_time_jump_future = true; + } + + score::td::SyncFupData& d = r.sync_fup_data; + d.precise_origin_timestamp = static_cast(fup_ts.ns); + d.reference_global_timestamp = static_cast(master_ns); + d.reference_local_timestamp = static_cast(sync.recvHardwareTS.ns); + d.sync_ingress_timestamp = static_cast(sync.recvHardwareTS.ns); + d.correction_field = static_cast(sync.ptpHdr.correctionField); + d.sequence_id = fup.ptpHdr.sequenceId; + d.pdelay = 0U; // filled by GptpEngine from IPeerDelayMeasurer + d.port_number = sync.ptpHdr.sourcePortIdentity.portNumber; + d.clock_identity = ClockIdentityToU64(sync.ptpHdr.sourcePortIdentity.clockIdentity); + + // IEEE 802.1AS Clause 11.4.1 + if (prev_slave_rx_ns_ != 0 && prev_master_origin_ns_ != 0) + { + const std::int64_t slave_interval = sync.recvHardwareTS.ns - prev_slave_rx_ns_; + const std::int64_t master_interval = master_ns - prev_master_origin_ns_; + if (master_interval > 0) + { + neighbor_rate_ratio_ = static_cast(slave_interval) / static_cast(master_interval); + } + } + prev_slave_rx_ns_ = sync.recvHardwareTS.ns; + prev_master_origin_ns_ = master_ns; + + last_master_ns_ = master_ns; + + return r; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/details/sync_state_machine.h b/score/TimeSlave/code/gptp/details/sync_state_machine.h new file mode 100644 index 0000000..abc657f --- /dev/null +++ b/score/TimeSlave/code/gptp/details/sync_state_machine.h @@ -0,0 +1,100 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_SYNC_STATE_MACHINE_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_SYNC_STATE_MACHINE_H + +#include "score/TimeDaemon/code/common/data_types/ptp_time_info.h" +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Output produced by a successful Sync+FollowUp pairing. +struct SyncResult +{ + std::int64_t master_ns{0}; ///< Grandmaster time (ns since epoch) + std::int64_t offset_ns{0}; ///< local hw_ts − master_ns + score::td::SyncFupData sync_fup_data{}; ///< Ready to copy into PtpTimeInfo (pdelay field filled by engine) + bool is_time_jump_future{false}; + bool is_time_jump_past{false}; +}; + +/** + * @brief Two-step Sync / Follow_Up correlation state machine + * (IEEE 802.1AS slave port). + * + * Detects forward time jumps (> @p jump_future_threshold_ns) and backward + * jumps. Computes neighborRateRatio from successive Sync intervals. + * Does NOT adjust any hardware clock; offset computation is purely + * informational for the upstream consumer. + * + * Thread-safety: NOT thread-safe. All calls must come from the same thread + * (the RxLoop thread in GptpEngine), except IsTimeout() which is atomic. + */ +class SyncStateMachine final +{ + public: + /// @param jump_future_threshold_ns Offset delta above which the state is + /// flagged as a future time jump. Set to 0 to disable detection. + explicit SyncStateMachine(std::int64_t jump_future_threshold_ns = 500'000'000LL) noexcept; + + /// Called when a Sync message is received (with its HW receive timestamp + /// already stored in @p msg.recvHardwareTS). + void OnSync(const PTPMessage& msg); + + /// Called when a FollowUp message is received. + /// @return A SyncResult on a successful Sync+FUP pairing, std::nullopt otherwise. + std::optional OnFollowUp(const PTPMessage& msg); + + /// @return true if no valid Sync+FUP has been received for longer than + /// @p timeout_ns nanoseconds (monotonic). + bool IsTimeout(std::int64_t mono_now_ns, std::int64_t timeout_ns) const; + + /// @return The latest computed neighborRateRatio (1.0 until first pair). + double GetNeighborRateRatio() const + { + return neighbor_rate_ratio_; + } + + private: + SyncResult BuildResult(const PTPMessage& sync, const PTPMessage& fup) noexcept; + + SyncState state_{SyncState::kEmpty}; + PTPMessage last_sync_{}; + PTPMessage last_fup_{}; + std::int64_t last_master_ns_{0}; + std::int64_t jump_future_threshold_ns_; + + // neighborRateRatio computation (IEEE 802.1AS Clause 11.4.1) + std::int64_t prev_slave_rx_ns_{0}; + std::int64_t prev_master_origin_ns_{0}; + double neighbor_rate_ratio_{1.0}; + + /// Monotonic timestamp of the last successful Sync+FUP pair (ns). + /// Atomic so that IsTimeout() can be called from a different thread. + std::atomic last_sync_mono_ns_{0}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_SYNC_STATE_MACHINE_H diff --git a/score/TimeSlave/code/gptp/details/sync_state_machine_test.cpp b/score/TimeSlave/code/gptp/details/sync_state_machine_test.cpp new file mode 100644 index 0000000..8b3b7bd --- /dev/null +++ b/score/TimeSlave/code/gptp/details/sync_state_machine_test.cpp @@ -0,0 +1,230 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/sync_state_machine.h" + +#include + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// Build a Sync PTPMessage with the given sequence ID and hardware RX timestamp. +// The correctionField encodes correction in sub-ns units (<<16 so >>16 == 0). +PTPMessage MakeSync(std::uint16_t seqId, std::int64_t recv_hw_ns, std::int64_t corr_ns = 0LL) noexcept +{ + PTPMessage msg{}; + msg.msgtype = kPtpMsgtypeSync; + msg.ptpHdr.sequenceId = seqId; + msg.ptpHdr.correctionField = corr_ns << 16; // CorrectionToTmv does >> 16 + msg.recvHardwareTS.ns = recv_hw_ns; + return msg; +} + +// Build a FollowUp PTPMessage with the given sequence ID and precise origin +// timestamp (in nanoseconds since epoch). +PTPMessage MakeFollowUp(std::uint16_t seqId, std::int64_t origin_ns, std::int64_t corr_ns = 0LL) noexcept +{ + PTPMessage msg{}; + msg.msgtype = kPtpMsgtypeFollowUp; + msg.ptpHdr.sequenceId = seqId; + msg.ptpHdr.correctionField = corr_ns << 16; + // Encode origin_ns into the preciseOriginTimestamp wire field. + msg.follow_up.preciseOriginTimestamp = TmvToTimestamp(TmvT{origin_ns}); + return msg; +} + +// Helper: deliver a matching Sync+FollowUp pair and return the SyncResult. +// Aborts the test if the pair does not produce a result. +SyncResult DeliverPair(SyncStateMachine& ssm, std::uint16_t seqId, std::int64_t recv_hw_ns, std::int64_t origin_ns) +{ + ssm.OnSync(MakeSync(seqId, recv_hw_ns)); + auto result = ssm.OnFollowUp(MakeFollowUp(seqId, origin_ns)); + if (!result.has_value()) + ADD_FAILURE() << "Expected SyncResult but got nullopt"; + return result.value_or(SyncResult{}); +} + +} // namespace + +class SyncStateMachineTest : public ::testing::Test +{ + protected: + // threshold = 500 ms + SyncStateMachine ssm_{500'000'000LL}; +}; + +// ── Basic pairing ───────────────────────────────────────────────────────────── + +TEST_F(SyncStateMachineTest, SyncThenFollowUp_MatchingSeq_ReturnsSyncResult) +{ + ssm_.OnSync(MakeSync(1U, 1'000'000'000LL)); + auto result = ssm_.OnFollowUp(MakeFollowUp(1U, 900'000'000LL)); + ASSERT_TRUE(result.has_value()); + // master_ns = origin_ns (no correction) + EXPECT_EQ(result->master_ns, 900'000'000LL); + // offset = recv_hw - master + EXPECT_EQ(result->offset_ns, 1'000'000'000LL - 900'000'000LL); +} + +TEST_F(SyncStateMachineTest, FollowUpBeforeSync_ReturnsNullopt) +{ + // kEmpty state: FUP arrives first → buffered, no result yet + auto result = ssm_.OnFollowUp(MakeFollowUp(1U, 0LL)); + EXPECT_FALSE(result.has_value()); +} + +TEST_F(SyncStateMachineTest, MultipleSyncs_ThenFollowUp_UsesLatestSync) +{ + // Two Syncs without a FUP between them — newer Sync should be used + ssm_.OnSync(MakeSync(1U, 1'000'000'000LL)); + ssm_.OnSync(MakeSync(2U, 2'000'000'000LL)); + // FUP with seqId == 2 (matches the newer Sync) + auto result = ssm_.OnFollowUp(MakeFollowUp(2U, 1'800'000'000LL)); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->master_ns, 1'800'000'000LL); +} + +TEST_F(SyncStateMachineTest, SeqIdMismatch_ReturnsNullopt_ThenMatchesOnNext) +{ + ssm_.OnSync(MakeSync(10U, 1'000'000'000LL)); + // FUP for a different seqId → no result; state becomes kHaveFup + auto r1 = ssm_.OnFollowUp(MakeFollowUp(99U, 0LL)); + EXPECT_FALSE(r1.has_value()); + + // Now deliver a Sync that matches the buffered FUP + ssm_.OnSync(MakeSync(99U, 2'000'000'000LL)); + auto r2 = ssm_.OnFollowUp(MakeFollowUp(99U, 1'900'000'000LL)); + ASSERT_TRUE(r2.has_value()); + EXPECT_EQ(r2->master_ns, 1'900'000'000LL); +} + +// ── SyncFupData fields ──────────────────────────────────────────────────────── + +TEST_F(SyncStateMachineTest, SyncFupData_SequenceId_SetFromFollowUp) +{ + const std::uint16_t kSeq = 42U; + ssm_.OnSync(MakeSync(kSeq, 1'000'000'000LL)); + auto result = ssm_.OnFollowUp(MakeFollowUp(kSeq, 900'000'000LL)); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->sync_fup_data.sequence_id, kSeq); +} + +TEST_F(SyncStateMachineTest, SyncFupData_PreciseOriginTimestamp_MatchesInput) +{ + const std::int64_t kOrigin = 5'000'000'000LL; // 5 s + ssm_.OnSync(MakeSync(1U, 6'000'000'000LL)); + auto result = ssm_.OnFollowUp(MakeFollowUp(1U, kOrigin)); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(static_cast(result->sync_fup_data.precise_origin_timestamp), kOrigin); +} + +// ── Jump detection ──────────────────────────────────────────────────────────── + +TEST_F(SyncStateMachineTest, JumpPast_Detected_OnSecondPair) +{ + // First pair establishes baseline master_ns = 2 s + DeliverPair(ssm_, 1U, 2'100'000'000LL, 2'000'000'000LL); + + // Second pair: master_ns goes backward → is_time_jump_past + auto result = ssm_.OnFollowUp(MakeFollowUp(2U, 1'000'000'000LL)); // no Sync preceding this on new seqId + + ssm_.OnSync(MakeSync(2U, 3'000'000'000LL)); + auto r2 = ssm_.OnFollowUp(MakeFollowUp(2U, 1'000'000'000LL)); + ASSERT_TRUE(r2.has_value()); + EXPECT_TRUE(r2->is_time_jump_past); + EXPECT_FALSE(r2->is_time_jump_future); +} + +TEST_F(SyncStateMachineTest, JumpFuture_Detected_WhenDeltaExceedsThreshold) +{ + // First pair: master_ns = 1 s + DeliverPair(ssm_, 1U, 1'100'000'000LL, 1'000'000'000LL); + + // Second pair: master_ns jumps by 2 s > threshold (500 ms) + ssm_.OnSync(MakeSync(2U, 3'100'000'000LL)); + auto r2 = ssm_.OnFollowUp(MakeFollowUp(2U, 3'000'000'000LL)); + ASSERT_TRUE(r2.has_value()); + EXPECT_TRUE(r2->is_time_jump_future); + EXPECT_FALSE(r2->is_time_jump_past); +} + +TEST_F(SyncStateMachineTest, NoJump_WhenFirstPair) +{ + // First pair — no previous baseline; no jump should be flagged + ssm_.OnSync(MakeSync(1U, 1'000'000'000LL)); + auto result = ssm_.OnFollowUp(MakeFollowUp(1U, 900'000'000LL)); + ASSERT_TRUE(result.has_value()); + EXPECT_FALSE(result->is_time_jump_past); + EXPECT_FALSE(result->is_time_jump_future); +} + +// ── neighborRateRatio ───────────────────────────────────────────────────────── + +TEST_F(SyncStateMachineTest, NeighborRateRatio_Default_IsOne) +{ + EXPECT_DOUBLE_EQ(ssm_.GetNeighborRateRatio(), 1.0); +} + +TEST_F(SyncStateMachineTest, NeighborRateRatio_AfterTwoPairs_Computed) +{ + // Pair 1: slave_rx = 1000 ms, master_origin = 1000 ms + DeliverPair(ssm_, 1U, 1'000'000'000LL, 1'000'000'000LL); + + // Pair 2: slave_rx = 2000 ms (+1000 ms), master_origin = 2010 ms (+1010 ms) + // ratio = 1000_000_000 / 1010_000_000 ≈ 0.99009... + DeliverPair(ssm_, 2U, 2'000'000'000LL, 2'010'000'000LL); + + const double expected = 1'000'000'000.0 / 1'010'000'000.0; + EXPECT_NEAR(ssm_.GetNeighborRateRatio(), expected, 1e-9); +} + +// ── IsTimeout ───────────────────────────────────────────────────────────────── + +TEST_F(SyncStateMachineTest, IsTimeout_BeforeFirstSync_ReturnsFalse) +{ + // last_sync_mono_ns_ == 0; should never be considered a timeout + EXPECT_FALSE(ssm_.IsTimeout(std::numeric_limits::max(), 1LL)); +} + +TEST_F(SyncStateMachineTest, IsTimeout_AfterSuccessfulPair_WithLargeNow_ReturnsTrue) +{ + DeliverPair(ssm_, 1U, 1'000'000'000LL, 900'000'000LL); + // Provide a mono_now far in the future; timeout = 1 s + EXPECT_TRUE(ssm_.IsTimeout(std::numeric_limits::max(), 1'000'000'000LL)); +} + +TEST_F(SyncStateMachineTest, IsTimeout_AfterSuccessfulPair_WithSmallDelta_ReturnsFalse) +{ + DeliverPair(ssm_, 1U, 1'000'000'000LL, 900'000'000LL); + // Provide mono_now = 0, which is before the recorded timestamp → not timed out + EXPECT_FALSE(ssm_.IsTimeout(0LL, 1'000'000'000LL)); +} + +TEST_F(SyncStateMachineTest, IsTimeout_ZeroTimeout_AlwaysReturnsFalse) +{ + DeliverPair(ssm_, 1U, 1'000'000'000LL, 900'000'000LL); + EXPECT_FALSE(ssm_.IsTimeout(std::numeric_limits::max(), 0LL)); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/gptp_engine.cpp b/score/TimeSlave/code/gptp/gptp_engine.cpp new file mode 100644 index 0000000..18fd0e7 --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine.cpp @@ -0,0 +1,316 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/gptp_engine.h" +#include "score/TimeSlave/code/gptp/details/network_identity.h" +#include "score/TimeSlave/code/gptp/details/raw_socket.h" + +#include "score/TimeDaemon/code/common/logging_contexts.h" +#include "score/mw/log/logging.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +constexpr int kRxTimeoutMs = 100; // poll timeout; keeps RxLoop responsive to shutdown +constexpr int kRxBufferSize = 2048; + +std::int64_t MonoNs() noexcept +{ + ::timespec ts{}; + ::clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000LL + ts.tv_nsec; +} + +} // namespace + +GptpEngine::GptpEngine(GptpEngineOptions opts, + std::unique_ptr local_clock) noexcept + : opts_{std::move(opts)}, + local_clock_{std::move(local_clock)}, + socket_{std::make_unique()}, + identity_{std::make_unique()}, + codec_{}, + parser_{}, + sync_sm_{opts_.jump_future_threshold_ns}, + pdelay_{nullptr} +{ +} + +GptpEngine::GptpEngine(GptpEngineOptions opts, + std::unique_ptr local_clock, + std::unique_ptr socket, + std::unique_ptr identity) noexcept + : opts_{std::move(opts)}, + local_clock_{std::move(local_clock)}, + socket_{std::move(socket)}, + identity_{std::move(identity)}, + codec_{}, + parser_{}, + sync_sm_{opts_.jump_future_threshold_ns}, + pdelay_{nullptr} +{ +} + +GptpEngine::~GptpEngine() noexcept +{ + (void)Deinitialize(); +} + +bool GptpEngine::Initialize() +{ + if (running_.load(std::memory_order_acquire)) + return true; + + if (!identity_->Resolve(opts_.iface_name)) + { + score::mw::log::LogError(score::td::kGPtpMachineContext) + << "GptpEngine: failed to resolve ClockIdentity for " << opts_.iface_name; + return false; + } + + pdelay_ = std::make_unique(identity_->GetClockIdentity()); + + if (!socket_->Open(opts_.iface_name)) + { + score::mw::log::LogError(score::td::kGPtpMachineContext) + << "GptpEngine: failed to open raw socket on " << opts_.iface_name; + return false; + } + + if (!socket_->EnableHwTimestamping()) + { + score::mw::log::LogWarn(score::td::kGPtpMachineContext) + << "GptpEngine: HW timestamping not available on " << opts_.iface_name << ", falling back to SW timestamps"; + } + + running_.store(true, std::memory_order_release); + + if (::pthread_create(&rx_thread_, nullptr, &RxThreadEntry, this) != 0) + { + score::mw::log::LogError(score::td::kGPtpMachineContext) << "GptpEngine: failed to create RxThread"; + running_.store(false, std::memory_order_release); + socket_->Close(); + return false; + } + rx_started_ = true; + + if (::pthread_create(&pdelay_thread_, nullptr, &PdelayThreadEntry, this) != 0) + { + score::mw::log::LogError(score::td::kGPtpMachineContext) << "GptpEngine: failed to create PdelayThread"; + (void)Deinitialize(); + return false; + } + pdelay_started_ = true; + + score::mw::log::LogInfo(score::td::kGPtpMachineContext) << "GptpEngine initialized on " << opts_.iface_name; + return true; +} + +bool GptpEngine::Deinitialize() +{ + running_.store(false, std::memory_order_release); + + // Close the socket first so that the RxThread's poll() unblocks + socket_->Close(); + + if (rx_started_) + { + ::pthread_join(rx_thread_, nullptr); + rx_started_ = false; + } + if (pdelay_started_) + { + ::pthread_join(pdelay_thread_, nullptr); + pdelay_started_ = false; + } + + score::mw::log::LogInfo(score::td::kGPtpMachineContext) << "GptpEngine deinitialized"; + return true; +} + +bool GptpEngine::ReadPTPSnapshot(score::td::PtpTimeInfo& info) +{ + if (!running_.load(std::memory_order_acquire)) + return false; + + const std::int64_t mono_now = MonoNs(); + const std::int64_t timeout_ns = static_cast(opts_.sync_timeout_ms) * 1'000'000LL; + + const bool timed_out = sync_sm_.IsTimeout(mono_now, timeout_ns); + + std::lock_guard lk(snapshot_mutex_); + snapshot_.local_time = local_clock_->Now(); + if (timed_out) + { + snapshot_.status.is_synchronized = false; + snapshot_.status.is_timeout = true; + snapshot_.status.is_correct = false; + } + info = snapshot_; + return true; +} + +void* GptpEngine::RxThreadEntry(void* arg) noexcept +{ + if (arg != nullptr) + static_cast(arg)->RxLoop(); + return nullptr; +} + +void* GptpEngine::PdelayThreadEntry(void* arg) noexcept +{ + if (arg != nullptr) + static_cast(arg)->PdelayLoop(); + return nullptr; +} + +void GptpEngine::RxLoop() noexcept +{ + std::uint8_t buf[kRxBufferSize]; + ::timespec hwts{}; + + while (running_.load(std::memory_order_acquire)) + { + std::memset(&hwts, 0, sizeof(hwts)); + const int n = socket_->Recv(buf, sizeof(buf), hwts, kRxTimeoutMs); + if (n <= 0) + continue; + HandlePacket(buf, n, hwts); + } +} + +void GptpEngine::PdelayLoop() noexcept +{ + ::timespec next{}; + ::clock_gettime(CLOCK_MONOTONIC, &next); + // Configurable warm-up before first Pdelay_Req (default 2 s) + const std::int64_t warmup_ns = static_cast(opts_.pdelay_warmup_ms) * 1'000'000LL; + const std::int64_t next_warmup_ns = + static_cast(next.tv_sec) * 1'000'000'000LL + next.tv_nsec + warmup_ns; + next.tv_sec = static_cast(next_warmup_ns / 1'000'000'000LL); + next.tv_nsec = static_cast(next_warmup_ns % 1'000'000'000LL); + + const std::int64_t interval_ns = + static_cast(opts_.pdelay_interval_ms > 0 ? opts_.pdelay_interval_ms : 1000) * 1'000'000LL; + + while (running_.load(std::memory_order_acquire)) + { + ::clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, nullptr); + if (!running_.load(std::memory_order_acquire)) + break; + + if (pdelay_) + { + (void)pdelay_->SendRequest(*socket_); + } + + const std::int64_t next_ns = + static_cast(next.tv_sec) * 1'000'000'000LL + next.tv_nsec + interval_ns; + next.tv_sec = static_cast(next_ns / 1'000'000'000LL); + next.tv_nsec = static_cast(next_ns % 1'000'000'000LL); + } +} + +void GptpEngine::HandlePacket(const std::uint8_t* frame, int len, const ::timespec& hwts) noexcept +{ + int ptp_offset = 0; + if (!codec_.ParseEthernetHeader(frame, len, ptp_offset)) + return; + + const auto* payload = frame + ptp_offset; + const std::size_t payload_len = static_cast(len - ptp_offset); + + PTPMessage msg{}; + if (!parser_.Parse(payload, payload_len, msg)) + return; + + const TmvT hw_ts{static_cast(hwts.tv_sec) * 1'000'000'000LL + hwts.tv_nsec}; + + switch (msg.msgtype) + { + case kPtpMsgtypeSync: + msg.recvHardwareTS = hw_ts; + sync_sm_.OnSync(msg); + break; + + case kPtpMsgtypeFollowUp: + msg.parseMessageTs = TimestampToTmv(msg.follow_up.preciseOriginTimestamp); + { + auto result = sync_sm_.OnFollowUp(msg); + if (result.has_value() && pdelay_) + { + const PDelayResult pdr = pdelay_->GetResult(); + // IEEE 802.1AS: subtract peer link delay from offset + if (pdr.valid) + { + result->offset_ns -= pdr.path_delay_ns; + result->sync_fup_data.pdelay = static_cast(pdr.path_delay_ns); + } + else + { + result->sync_fup_data.pdelay = 0U; + } + UpdateSnapshot(*result, pdr); + } + } + break; + + case kPtpMsgtypePdelayResp: + msg.recvHardwareTS = hw_ts; + msg.parseMessageTs = TimestampToTmv(msg.pdelay_resp.responseOriginTimestamp); + if (pdelay_) + pdelay_->OnResponse(msg); + break; + + case kPtpMsgtypePdelayRespFollowUp: + msg.parseMessageTs = TimestampToTmv(msg.pdelay_resp_fup.responseOriginReceiptTimestamp); + if (pdelay_) + pdelay_->OnResponseFollowUp(msg); + break; + + default: + break; + } +} + +void GptpEngine::UpdateSnapshot(const SyncResult& sync, const PDelayResult& pdelay) noexcept +{ + std::lock_guard lk(snapshot_mutex_); + + const std::int64_t local_rx_ns = static_cast(sync.sync_fup_data.reference_local_timestamp); + snapshot_.ptp_assumed_time = std::chrono::nanoseconds{local_rx_ns - sync.offset_ns}; + snapshot_.local_time = local_clock_->Now(); + snapshot_.rate_deviation = sync_sm_.GetNeighborRateRatio(); + + snapshot_.status.is_synchronized = true; + snapshot_.status.is_timeout = false; + snapshot_.status.is_time_jump_future = sync.is_time_jump_future; + snapshot_.status.is_time_jump_past = sync.is_time_jump_past; + snapshot_.status.is_correct = !sync.is_time_jump_future && !sync.is_time_jump_past; + + snapshot_.sync_fup_data = sync.sync_fup_data; + snapshot_.pdelay_data = pdelay.pdelay_data; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/gptp_engine.h b/score/TimeSlave/code/gptp/gptp_engine.h new file mode 100644 index 0000000..9170477 --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine.h @@ -0,0 +1,123 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_GPTP_ENGINE_H +#define SCORE_TIMESLAVE_CODE_GPTP_GPTP_ENGINE_H + +#include "score/TimeDaemon/code/common/data_types/ptp_time_info.h" +#include "score/TimeSlave/code/gptp/details/frame_codec.h" +#include "score/TimeSlave/code/gptp/details/i_network_identity.h" +#include "score/TimeSlave/code/gptp/details/i_raw_socket.h" +#include "score/TimeSlave/code/gptp/details/message_parser.h" +#include "score/TimeSlave/code/gptp/details/pdelay_measurer.h" +#include "score/TimeSlave/code/gptp/details/ptp_types.h" +#include "score/TimeSlave/code/gptp/details/sync_state_machine.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Configuration for GptpEngine. +struct GptpEngineOptions +{ + std::string iface_name = "eth0"; ///< Network interface for gPTP + int pdelay_interval_ms = 1000; ///< Period between Pdelay_Req transmissions (ms) + int pdelay_warmup_ms = 2000; ///< Delay before first Pdelay_Req (ms) + int sync_timeout_ms = 3300; ///< Declare timeout after this many ms without Sync + std::int64_t jump_future_threshold_ns = 500'000'000LL; ///< 500 ms +}; + +/** + * @brief gPTP engine for the TimeSlave process. + * + * Runs two POSIX threads: RxThread (receive/parse PTP frames) and + * PdelayThread (periodic Pdelay_Req transmission). + * + * ReadPTPSnapshot() is thread-safe once Initialize() returns true. + */ +class GptpEngine final +{ + public: + explicit GptpEngine(GptpEngineOptions opts, + std::unique_ptr local_clock) noexcept; + + /// Constructor for testing: inject fake socket and identity. + GptpEngine(GptpEngineOptions opts, + std::unique_ptr local_clock, + std::unique_ptr socket, + std::unique_ptr identity) noexcept; + + ~GptpEngine() noexcept; + + GptpEngine(const GptpEngine&) = delete; + GptpEngine& operator=(const GptpEngine&) = delete; + GptpEngine(GptpEngine&&) = delete; + GptpEngine& operator=(GptpEngine&&) = delete; + + /// Open the raw socket, enable HW timestamping, resolve the ClockIdentity, + /// and start the Rx and Pdelay background threads. + /// @return true on success. + bool Initialize(); + + /// Stop background threads and close the socket. + /// @return true (always succeeds). + bool Deinitialize(); + + /// Copy the latest measurement snapshot into @p info. + /// Non-blocking; returns false only if the engine is not initialized. + bool ReadPTPSnapshot(score::td::PtpTimeInfo& info); + + private: + static void* RxThreadEntry(void* arg) noexcept; + static void* PdelayThreadEntry(void* arg) noexcept; + void RxLoop() noexcept; + void PdelayLoop() noexcept; + + void HandlePacket(const std::uint8_t* frame, int len, const ::timespec& hwts) noexcept; + void UpdateSnapshot(const SyncResult& sync, const PDelayResult& pdelay) noexcept; + + GptpEngineOptions opts_; + + std::unique_ptr local_clock_; + std::unique_ptr socket_; + std::unique_ptr identity_; + FrameCodec codec_; + GptpMessageParser parser_; + SyncStateMachine sync_sm_; + std::unique_ptr pdelay_; + + mutable std::mutex snapshot_mutex_; + score::td::PtpTimeInfo snapshot_{}; + + std::atomic running_{false}; + pthread_t rx_thread_{}; + pthread_t pdelay_thread_{}; + bool rx_started_{false}; + bool pdelay_started_{false}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_GPTP_ENGINE_H diff --git a/score/TimeSlave/code/gptp/gptp_engine_test.cpp b/score/TimeSlave/code/gptp/gptp_engine_test.cpp new file mode 100644 index 0000000..76d6918 --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine_test.cpp @@ -0,0 +1,517 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/gptp_engine.h" +#include "score/TimeSlave/code/gptp/details/i_network_identity.h" +#include "score/TimeSlave/code/gptp/details/i_raw_socket.h" +#include "score/time/HighPrecisionLocalSteadyClock/high_precision_local_steady_clock.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// ── FakeClock ───────────────────────────────────────────────────────────────── + +class FakeClock final : public score::time::HighPrecisionLocalSteadyClock +{ + public: + score::time::HighPrecisionLocalSteadyClock::time_point Now() noexcept override + { + return score::time::HighPrecisionLocalSteadyClock::time_point{std::chrono::nanoseconds{42'000'000'000LL}}; + } +}; + +// ── FakeSocket ──────────────────────────────────────────────────────────────── + +class FakeSocket final : public IRawSocket +{ + public: + void Push(std::vector data, ::timespec hwts = {}) + { + { + std::lock_guard lk(mtx_); + frames_.push_back({std::move(data), hwts}); + } + cv_.notify_one(); + } + + bool Open(const std::string&) override + { + return true; + } + bool EnableHwTimestamping() override + { + return hw_ts_ok_; + } + + void Close() override + { + { + std::lock_guard lk(mtx_); + closed_ = true; + } + cv_.notify_all(); + } + + int Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) override + { + std::unique_lock lk(mtx_); + const auto timeout = std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 100); + cv_.wait_for(lk, timeout, [this] { + return closed_ || !frames_.empty(); + }); + if (closed_) + return -1; + if (frames_.empty()) + return 0; + auto& [data, ts] = frames_.front(); + const std::size_t n = std::min(data.size(), buf_len); + std::memcpy(buf, data.data(), n); + hwts = ts; + frames_.pop_front(); + return static_cast(n); + } + + int Send(const void*, int len, ::timespec&) override + { + return len; + } + int GetFd() const override + { + return -1; + } + + void SetHwTsOk(bool v) + { + hw_ts_ok_ = v; + } + + private: + std::deque, ::timespec>> frames_; + std::mutex mtx_; + std::condition_variable cv_; + bool closed_{false}; + bool hw_ts_ok_{true}; +}; + +// ── FakeIdentity ────────────────────────────────────────────────────────────── + +class FakeIdentity final : public INetworkIdentity +{ + public: + explicit FakeIdentity(bool resolve_ok = true) : resolve_ok_{resolve_ok} {} + + bool Resolve(const std::string&) override + { + return resolve_ok_; + } + + ClockIdentity GetClockIdentity() const override + { + ClockIdentity ci{}; + ci.id[0] = 0xAA; + ci.id[7] = 0xBB; + return ci; + } + + private: + bool resolve_ok_; +}; + +// ── Frame builders ──────────────────────────────────────────────────────────── + +// 14-byte Ethernet header with EtherType 0x88F7 (IEEE 1588) +void AppendEthHeader(std::vector& buf) +{ + // dst: 01:80:c2:00:00:0e + const std::uint8_t dst[6] = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x0E}; + // src: 02:00:00:ff:00:11 + const std::uint8_t src[6] = {0x02, 0x00, 0x00, 0xFF, 0x00, 0x11}; + buf.insert(buf.end(), dst, dst + 6); + buf.insert(buf.end(), src, src + 6); + buf.push_back(0x88); + buf.push_back(0xF7); +} + +// Build a 34-byte PTP header at the back of buf. +void AppendPtpHeader(std::vector& buf, + std::uint8_t msgtype, + std::uint16_t seqId, + std::uint8_t ctlField = 0) +{ + const std::size_t start = buf.size(); + buf.resize(start + 34, 0); + std::uint8_t* p = buf.data() + start; + p[0] = static_cast(0x10U | (msgtype & 0x0FU)); // tsmt + p[1] = 0x02; // version + const std::uint16_t len = htons(static_cast(buf.size() - 14)); + std::memcpy(p + 2, &len, 2); + const std::uint16_t seq = htons(seqId); + std::memcpy(p + 30, &seq, 2); + p[32] = ctlField; +} + +// Append a 10-byte Timestamp body (sec_msb=0, sec_lsb, ns). +void AppendTimestamp(std::vector& buf, std::uint32_t sec_lsb, std::uint32_t ns) +{ + const std::uint16_t msb = htons(0U); + const std::uint32_t sl = htonl(sec_lsb); + const std::uint32_t n = htonl(ns); + const std::uint8_t* p; + p = reinterpret_cast(&msb); + buf.insert(buf.end(), p, p + 2); + p = reinterpret_cast(&sl); + buf.insert(buf.end(), p, p + 4); + p = reinterpret_cast(&n); + buf.insert(buf.end(), p, p + 4); +} + +std::vector MakeSyncFrame(std::uint16_t seqId) +{ + std::vector f; + AppendEthHeader(f); + AppendPtpHeader(f, kPtpMsgtypeSync, seqId, /*ctl=*/0); + AppendTimestamp(f, 0, 0); // Sync body (origin timestamp, unused) + return f; +} + +std::vector MakeFollowUpFrame(std::uint16_t seqId, std::uint32_t sec_lsb, std::uint32_t ns) +{ + std::vector f; + AppendEthHeader(f); + AppendPtpHeader(f, kPtpMsgtypeFollowUp, seqId, /*ctl=*/2); + AppendTimestamp(f, sec_lsb, ns); + return f; +} + +std::vector MakePdelayRespFrame(std::uint16_t seqId) +{ + std::vector f; + AppendEthHeader(f); + AppendPtpHeader(f, kPtpMsgtypePdelayResp, seqId, /*ctl=*/5); + AppendTimestamp(f, 1, 0); // responseOriginTimestamp + // requesting port identity (10 bytes) + f.resize(f.size() + 10, 0); + return f; +} + +std::vector MakePdelayRespFupFrame(std::uint16_t seqId) +{ + std::vector f; + AppendEthHeader(f); + AppendPtpHeader(f, kPtpMsgtypePdelayRespFollowUp, seqId, /*ctl=*/5); + AppendTimestamp(f, 2, 0); // responseOriginReceiptTimestamp + f.resize(f.size() + 10, 0); // requesting port identity + return f; +} + +std::vector MakeUnknownFrame() +{ + std::vector f; + AppendEthHeader(f); + AppendPtpHeader(f, kPtpMsgtypePdelayReq, 0, /*ctl=*/5); + return f; +} + +// ── Test helpers ────────────────────────────────────────────────────────────── + +GptpEngineOptions FastOptions() +{ + GptpEngineOptions o; + o.iface_name = "lo"; + o.pdelay_warmup_ms = 0; // no warmup — first Pdelay_Req fires immediately + o.pdelay_interval_ms = 10; // 10 ms cycle + o.sync_timeout_ms = 3300; + o.jump_future_threshold_ns = 500'000'000LL; + return o; +} + +// Wait up to @p max_ms for snapshot.status.is_synchronized to become true. +bool WaitForSync(GptpEngine& eng, int max_ms = 500) +{ + for (int i = 0; i < max_ms / 10; ++i) + { + score::td::PtpTimeInfo info{}; + eng.ReadPTPSnapshot(info); + if (info.status.is_synchronized) + return true; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return false; +} + +// ── Fixtures ────────────────────────────────────────────────────────────────── + +// Fixture for tests that use real socket+identity paths (no injection). +class GptpEngineTest : public ::testing::Test +{ + protected: + void SetUp() override + { + engine_ = std::make_unique(FastOptions(), std::make_unique()); + } + + void TearDown() override + { + engine_->Deinitialize(); + } + + std::unique_ptr engine_; +}; + +// Fixture for tests that inject FakeSocket + FakeIdentity. +class GptpEngineFakeTest : public ::testing::Test +{ + protected: + void SetUp() override + { + auto sock = std::make_unique(); + auto identity = std::make_unique(); + socket_raw_ = sock.get(); + engine_ = std::make_unique( + FastOptions(), std::make_unique(), std::move(sock), std::move(identity)); + } + + void TearDown() override + { + engine_->Deinitialize(); + } + + FakeSocket* socket_raw_{nullptr}; + std::unique_ptr engine_; +}; + +} // namespace + +// ── GptpEngineTest — uninitialised paths ────────────────────────────────────── + +TEST_F(GptpEngineTest, Deinitialize_WhenNotInitialized_ReturnsTrue) +{ + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(GptpEngineTest, Deinitialize_CalledTwice_BothReturnTrue) +{ + EXPECT_TRUE(engine_->Deinitialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(GptpEngineTest, ReadPTPSnapshot_WhenNotInitialized_ReturnsFalse) +{ + score::td::PtpTimeInfo info{}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(info)); +} + +TEST_F(GptpEngineTest, ReadPTPSnapshot_InfoUnchanged_WhenNotInitialized) +{ + score::td::PtpTimeInfo info{}; + info.ptp_assumed_time = std::chrono::nanoseconds{999LL}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(info)); + EXPECT_EQ(info.ptp_assumed_time, std::chrono::nanoseconds{999LL}); +} + +// ── GptpEngineFakeTest — Initialize / Deinitialize ─────────────────────────── + +TEST_F(GptpEngineFakeTest, Initialize_WithFakeSocket_ReturnsTrue) +{ + EXPECT_TRUE(engine_->Initialize()); +} + +TEST_F(GptpEngineFakeTest, Initialize_CalledTwice_ReturnsTrueOnSecondCall) +{ + EXPECT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Initialize()); // already running → returns true +} + +TEST_F(GptpEngineFakeTest, Deinitialize_AfterInitialize_ReturnsTrue) +{ + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(GptpEngineFakeTest, ReadPTPSnapshot_AfterInitialize_ReturnsTrue) +{ + ASSERT_TRUE(engine_->Initialize()); + score::td::PtpTimeInfo info{}; + EXPECT_TRUE(engine_->ReadPTPSnapshot(info)); +} + +TEST_F(GptpEngineFakeTest, ReadPTPSnapshot_NotSynchronized_BeforeAnySync) +{ + ASSERT_TRUE(engine_->Initialize()); + score::td::PtpTimeInfo info{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(info)); + EXPECT_FALSE(info.status.is_synchronized); +} + +// ── GptpEngineFakeTest — identity failure ───────────────────────────────────── + +TEST(GptpEngineIdentityFailTest, Initialize_IdentityResolveFails_ReturnsFalse) +{ + auto sock = std::make_unique(); + auto identity = std::make_unique(/*resolve_ok=*/false); + GptpEngine eng{FastOptions(), std::make_unique(), std::move(sock), std::move(identity)}; + EXPECT_FALSE(eng.Initialize()); + EXPECT_TRUE(eng.Deinitialize()); +} + +// ── GptpEngineFakeTest — HW timestamp unavailable (warning path) ────────────── + +TEST_F(GptpEngineFakeTest, Initialize_HwTsUnavailable_StillReturnsTrue) +{ + socket_raw_->SetHwTsOk(false); + EXPECT_TRUE(engine_->Initialize()); +} + +// ── GptpEngineFakeTest — Sync + FollowUp → UpdateSnapshot ──────────────────── + +TEST_F(GptpEngineFakeTest, HandlePacket_SyncFollowUp_SnapshotBecomesSync) +{ + ASSERT_TRUE(engine_->Initialize()); + + // Send Sync then FollowUp with the same seqId. + ::timespec hwts{}; + hwts.tv_sec = 1; + hwts.tv_nsec = 500'000'000L; + socket_raw_->Push(MakeSyncFrame(1U), hwts); + socket_raw_->Push(MakeFollowUpFrame(1U, /*sec=*/2, /*ns=*/0)); + + EXPECT_TRUE(WaitForSync(*engine_)); + score::td::PtpTimeInfo info{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(info)); + EXPECT_TRUE(info.status.is_synchronized); + EXPECT_FALSE(info.status.is_timeout); +} + +TEST_F(GptpEngineFakeTest, HandlePacket_MultipleSyncFup_SnapshotUpdated) +{ + ASSERT_TRUE(engine_->Initialize()); + + for (std::uint16_t seq = 1U; seq <= 3U; ++seq) + { + socket_raw_->Push(MakeSyncFrame(seq)); + socket_raw_->Push(MakeFollowUpFrame(seq, seq, 0U)); + } + + EXPECT_TRUE(WaitForSync(*engine_)); +} + +// ── GptpEngineFakeTest — PdelayResp + PdelayRespFollowUp ───────────────────── + +TEST_F(GptpEngineFakeTest, HandlePacket_PdelayRespSequence_DoesNotCrash) +{ + ASSERT_TRUE(engine_->Initialize()); + + socket_raw_->Push(MakePdelayRespFrame(0U)); + socket_raw_->Push(MakePdelayRespFupFrame(0U)); + + // Just verify no crash; sleep briefly to let the RxThread process. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); +} + +// ── GptpEngineFakeTest — unknown msgtype (default branch) ──────────────────── + +TEST_F(GptpEngineFakeTest, HandlePacket_UnknownMsgtype_DefaultBranchNocrash) +{ + ASSERT_TRUE(engine_->Initialize()); + socket_raw_->Push(MakeUnknownFrame()); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); +} + +// ── GptpEngineFakeTest — bad Ethernet header ───────────────────────────────── + +TEST_F(GptpEngineFakeTest, HandlePacket_TooShortFrame_EarlyReturn) +{ + ASSERT_TRUE(engine_->Initialize()); + socket_raw_->Push({0x01, 0x02, 0x03}); // < 14 bytes, ParseEthernetHeader returns false + std::this_thread::sleep_for(std::chrono::milliseconds(30)); +} + +// ── GptpEngineFakeTest — Sync+FUP then timeout path ────────────────────────── + +TEST(GptpEngineTimeoutTest, ReadPTPSnapshot_TimeoutPath_IsTimeoutSet) +{ + // Use a very short timeout (50 ms) so we can trigger it quickly. + GptpEngineOptions opts = FastOptions(); + opts.sync_timeout_ms = 50; + + auto sock = std::make_unique(); + auto identity = std::make_unique(); + FakeSocket* raw_sock = sock.get(); + + GptpEngine eng{opts, std::make_unique(), std::move(sock), std::move(identity)}; + ASSERT_TRUE(eng.Initialize()); + + // First receive a Sync+FUP so the state machine records a timestamp. + ::timespec hwts{}; + hwts.tv_sec = 1; + raw_sock->Push(MakeSyncFrame(1U), hwts); + raw_sock->Push(MakeFollowUpFrame(1U, 2U, 0U)); + + // Wait for it to be processed and become synchronized. + bool got_sync = false; + for (int i = 0; i < 50; ++i) + { + score::td::PtpTimeInfo tmp{}; + eng.ReadPTPSnapshot(tmp); + if (tmp.status.is_synchronized) + { + got_sync = true; + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_TRUE(got_sync) << "engine never became synchronized"; + + // Now wait longer than sync_timeout_ms for the timeout to trigger. + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + score::td::PtpTimeInfo info{}; + ASSERT_TRUE(eng.ReadPTPSnapshot(info)); + EXPECT_TRUE(info.status.is_timeout); + EXPECT_FALSE(info.status.is_synchronized); + EXPECT_TRUE(eng.Deinitialize()); +} + +// ── Non-injectable path — nonexistent interface ─────────────────────────────── + +TEST(GptpEngineRealSocketTest, Initialize_NonExistentInterface_ReturnsFalse) +{ + GptpEngineOptions opts; + opts.iface_name = "nonexistent_iface_xyz"; + opts.pdelay_warmup_ms = 0; + GptpEngine eng{opts, std::make_unique()}; + EXPECT_FALSE(eng.Initialize()); + EXPECT_TRUE(eng.Deinitialize()); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/instrument/BUILD b/score/TimeSlave/code/gptp/instrument/BUILD new file mode 100644 index 0000000..48ca897 --- /dev/null +++ b/score/TimeSlave/code/gptp/instrument/BUILD @@ -0,0 +1,48 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "probe", + srcs = ["probe.cpp"], + hdrs = ["probe.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + "//score/TimeDaemon/code/common:logging_contexts", + "//score/TimeSlave/code/gptp/record:recorder", + "@score_baselibs//score/mw/log:frontend", + ], +) + +cc_test( + name = "probe_test", + srcs = ["probe_test.cpp"], + tags = ["unit"], + deps = [ + ":probe", + "@googletest//:gtest", + "@googletest//:gtest_main", + "@score_baselibs//score/mw/log:console_only_backend", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":probe_test"], + test_suites_from_sub_packages = [], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/instrument/probe.cpp b/score/TimeSlave/code/gptp/instrument/probe.cpp new file mode 100644 index 0000000..1312455 --- /dev/null +++ b/score/TimeSlave/code/gptp/instrument/probe.cpp @@ -0,0 +1,61 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/instrument/probe.h" + +#include "score/TimeDaemon/code/common/logging_contexts.h" +#include "score/mw/log/logging.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +ProbeManager& ProbeManager::Instance() +{ + static ProbeManager instance; + return instance; +} + +void ProbeManager::Trace(ProbePoint point, const ProbeData& data) +{ + score::mw::log::LogDebug(score::td::kGPtpMachineContext) + << "PROBE point=" << static_cast(point) << " ts=" << data.ts_mono_ns << " val=" << data.value_ns + << " seq=" << data.seq_id; + + if (recorder_ != nullptr && recorder_->IsEnabled()) + { + recorder_->Record(RecordEntry{ + data.ts_mono_ns, + RecordEvent::kProbe, + data.value_ns, + 0, + static_cast(data.seq_id), + static_cast(point), + }); + } +} + +std::int64_t ProbeMonoNs() noexcept +{ + ::timespec ts{}; + ::clock_gettime(CLOCK_MONOTONIC, &ts); + return ts.tv_sec * 1'000'000'000LL + ts.tv_nsec; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/instrument/probe.h b/score/TimeSlave/code/gptp/instrument/probe.h new file mode 100644 index 0000000..6b33bd2 --- /dev/null +++ b/score/TimeSlave/code/gptp/instrument/probe.h @@ -0,0 +1,101 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_INSTRUMENT_PROBE_H +#define SCORE_TIMESLAVE_CODE_GPTP_INSTRUMENT_PROBE_H + +#include "score/TimeSlave/code/gptp/record/recorder.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Measurement probe points within the gPTP pipeline. +enum class ProbePoint : std::uint8_t +{ + kRxPacketReceived = 0, + kSyncFrameParsed = 1, + kFollowUpProcessed = 2, + kOffsetComputed = 3, + kPdelayReqSent = 4, + kPdelayCompleted = 5, + kPhcAdjusted = 6, +}; + +/// Data payload for a single probe event. +struct ProbeData +{ + std::int64_t ts_mono_ns{0}; + std::int64_t value_ns{0}; + std::uint32_t seq_id{0}; +}; + +/** + * @brief Singleton manager for runtime measurement probes. + * + * When enabled, traces probe events to the logger and optionally to a Recorder. + * Controlled at runtime via SetEnabled(). + */ +class ProbeManager final +{ + public: + static ProbeManager& Instance(); + + void SetEnabled(bool enabled) + { + enabled_.store(enabled, std::memory_order_release); + } + bool IsEnabled() const + { + return enabled_.load(std::memory_order_acquire); + } + + /// Optional: link to a Recorder for persistent probe output. + void SetRecorder(Recorder* recorder) + { + recorder_ = recorder; + } + + /// Record a probe event. Thread-safe. + void Trace(ProbePoint point, const ProbeData& data); + + private: + ProbeManager() = default; + std::atomic enabled_{false}; + Recorder* recorder_{nullptr}; +}; + +/// Returns the current monotonic timestamp in nanoseconds. +std::int64_t ProbeMonoNs() noexcept; + +} // namespace details +} // namespace ts +} // namespace score + +// Convenience macro: zero overhead when probing is disabled. +// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) +#define GPTP_PROBE(point, ...) \ + do \ + { \ + if (::score::ts::details::ProbeManager::Instance().IsEnabled()) \ + { \ + ::score::ts::details::ProbeManager::Instance().Trace(point, {__VA_ARGS__}); \ + } \ + } while (0) + +#endif // SCORE_TIMESLAVE_CODE_GPTP_INSTRUMENT_PROBE_H diff --git a/score/TimeSlave/code/gptp/instrument/probe_test.cpp b/score/TimeSlave/code/gptp/instrument/probe_test.cpp new file mode 100644 index 0000000..e8f0bea --- /dev/null +++ b/score/TimeSlave/code/gptp/instrument/probe_test.cpp @@ -0,0 +1,168 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/instrument/probe.h" + +#include + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +// ProbeManager is a singleton; reset it between tests. +class ProbeManagerTest : public ::testing::Test +{ + protected: + void TearDown() override + { + ProbeManager::Instance().SetEnabled(false); + ProbeManager::Instance().SetRecorder(nullptr); + } +}; + +// ── Enable / disable ────────────────────────────────────────────────────────── + +TEST_F(ProbeManagerTest, DefaultState_IsDisabled) +{ + EXPECT_FALSE(ProbeManager::Instance().IsEnabled()); +} + +TEST_F(ProbeManagerTest, SetEnabled_True_IsEnabledReturnsTrue) +{ + ProbeManager::Instance().SetEnabled(true); + EXPECT_TRUE(ProbeManager::Instance().IsEnabled()); +} + +TEST_F(ProbeManagerTest, SetEnabled_FalseThenTrue_TogglesCorrectly) +{ + ProbeManager::Instance().SetEnabled(true); + ProbeManager::Instance().SetEnabled(false); + EXPECT_FALSE(ProbeManager::Instance().IsEnabled()); +} + +TEST_F(ProbeManagerTest, Instance_ReturnsSameSingleton) +{ + EXPECT_EQ(&ProbeManager::Instance(), &ProbeManager::Instance()); +} + +// ── Trace when disabled ─────────────────────────────────────────────────────── + +TEST_F(ProbeManagerTest, Trace_WhenDisabled_DoesNotCrash) +{ + ProbeData d{}; + d.ts_mono_ns = 1'000'000LL; + d.value_ns = 500LL; + d.seq_id = 1U; + EXPECT_NO_THROW(ProbeManager::Instance().Trace(ProbePoint::kSyncFrameParsed, d)); +} + +// ── Trace when enabled without recorder ─────────────────────────────────────── + +TEST_F(ProbeManagerTest, Trace_WhenEnabled_NoRecorder_DoesNotCrash) +{ + ProbeManager::Instance().SetEnabled(true); + ProbeData d{}; + d.ts_mono_ns = 2'000'000LL; + d.value_ns = -100LL; + d.seq_id = 2U; + EXPECT_NO_THROW(ProbeManager::Instance().Trace(ProbePoint::kFollowUpProcessed, d)); +} + +// ── Trace with recorder attached ───────────────────────────────────────────── + +class ProbeManagerWithRecorderTest : public ::testing::Test +{ + protected: + void SetUp() override + { + path_ = "/tmp/probe_test_" + std::to_string(::getpid()) + ".csv"; + Recorder::Config cfg; + cfg.enabled = true; + cfg.file_path = path_; + recorder_ = std::make_unique(cfg); + + ProbeManager::Instance().SetEnabled(true); + ProbeManager::Instance().SetRecorder(recorder_.get()); + } + + void TearDown() override + { + ProbeManager::Instance().SetEnabled(false); + ProbeManager::Instance().SetRecorder(nullptr); + std::remove(path_.c_str()); + } + + std::string path_; + std::unique_ptr recorder_; +}; + +TEST_F(ProbeManagerWithRecorderTest, Trace_WritesToRecorder) +{ + ProbeData d{}; + d.ts_mono_ns = 3'000'000LL; + d.value_ns = 42LL; + d.seq_id = 3U; + ProbeManager::Instance().Trace(ProbePoint::kPdelayCompleted, d); + + // Flush by replacing recorder (which closes file in destructor) + ProbeManager::Instance().SetRecorder(nullptr); + recorder_.reset(); + + // File should have header + 1 data line + std::ifstream f(path_); + int lines = 0; + std::string line; + while (std::getline(f, line)) + ++lines; + EXPECT_EQ(lines, 2); +} + +TEST_F(ProbeManagerWithRecorderTest, Trace_AllProbePoints_DoNotCrash) +{ + const ProbePoint points[] = { + ProbePoint::kRxPacketReceived, + ProbePoint::kSyncFrameParsed, + ProbePoint::kFollowUpProcessed, + ProbePoint::kOffsetComputed, + ProbePoint::kPdelayReqSent, + ProbePoint::kPdelayCompleted, + ProbePoint::kPhcAdjusted, + }; + for (auto p : points) + { + EXPECT_NO_THROW(ProbeManager::Instance().Trace(p, ProbeData{})); + } +} + +// ── ProbeMonoNs ─────────────────────────────────────────────────────────────── + +TEST(ProbeMonoNsTest, ReturnsPositiveValue) +{ + EXPECT_GT(ProbeMonoNs(), 0LL); +} + +TEST(ProbeMonoNsTest, MonotonicallyIncreasing) +{ + const std::int64_t t1 = ProbeMonoNs(); + const std::int64_t t2 = ProbeMonoNs(); + EXPECT_GE(t2, t1); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/phc/BUILD b/score/TimeSlave/code/gptp/phc/BUILD new file mode 100644 index 0000000..a0c2a53 --- /dev/null +++ b/score/TimeSlave/code/gptp/phc/BUILD @@ -0,0 +1,30 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "phc_adjuster", + srcs = select({ + "@platforms//os:qnx": ["//score/TimeSlave/code/gptp/platform/qnx:phc_adjuster_src"], + "//conditions:default": ["//score/TimeSlave/code/gptp/platform/linux:phc_adjuster_src"], + }), + hdrs = ["phc_adjuster.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = select({ + "@platforms//os:qnx": ["//score/TimeSlave/code/gptp/details:raw_socket"], + "//conditions:default": [], + }), +) diff --git a/score/TimeSlave/code/gptp/phc/phc_adjuster.h b/score/TimeSlave/code/gptp/phc/phc_adjuster.h new file mode 100644 index 0000000..a75fd25 --- /dev/null +++ b/score/TimeSlave/code/gptp/phc/phc_adjuster.h @@ -0,0 +1,75 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_PHC_PHC_ADJUSTER_H +#define SCORE_TIMESLAVE_CODE_GPTP_PHC_PHC_ADJUSTER_H + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Configuration for PHC hardware clock synchronization. +struct PhcConfig +{ + bool enabled = false; + std::string device = ""; ///< QNX: "emac0", Linux: "/dev/ptp0" + std::int64_t step_threshold_ns = 100'000'000LL; ///< >100ms = step, else slew +}; + +/** + * @brief Adjusts the PTP Hardware Clock (PHC) based on gPTP offset and rate. + * + * When enabled, applies step corrections for large offsets and frequency + * slew for continuous tracking. When disabled, all methods are no-ops. + * + * Platform-specific: Linux uses clock_adjtime(), QNX uses EMAC PTP ioctls. + */ +class PhcAdjuster final +{ + public: + explicit PhcAdjuster(PhcConfig cfg); + ~PhcAdjuster(); + + PhcAdjuster(const PhcAdjuster&) = delete; + PhcAdjuster& operator=(const PhcAdjuster&) = delete; + + /// @return true if hardware clock adjustment is enabled. + bool IsEnabled() const + { + return cfg_.enabled; + } + + /// Apply a time step or slew based on offset magnitude. + /// If |offset_ns| > step_threshold_ns, a step correction is applied; + /// otherwise the offset is ignored (frequency slew handles drift). + void AdjustOffset(std::int64_t offset_ns); + + /// Adjust the PHC frequency to track the master clock rate. + /// @param rate_ratio neighborRateRatio (1.0 = no drift). + void AdjustFrequency(double rate_ratio); + + private: + PhcConfig cfg_; + int phc_fd_{-1}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_PHC_PHC_ADJUSTER_H diff --git a/score/TimeSlave/code/gptp/platform/linux/BUILD b/score/TimeSlave/code/gptp/platform/linux/BUILD new file mode 100644 index 0000000..a29f395 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/BUILD @@ -0,0 +1,30 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +filegroup( + name = "raw_socket_src", + srcs = ["raw_socket.cpp"], + visibility = ["//score:__subpackages__"], +) + +filegroup( + name = "network_identity_src", + srcs = ["network_identity.cpp"], + visibility = ["//score:__subpackages__"], +) + +filegroup( + name = "phc_adjuster_src", + srcs = ["phc_adjuster.cpp"], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/platform/linux/network_identity.cpp b/score/TimeSlave/code/gptp/platform/linux/network_identity.cpp new file mode 100644 index 0000000..228ae50 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/network_identity.cpp @@ -0,0 +1,86 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/network_identity.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +/// Read the MAC address of @p iface_name into @p out_mac (6 bytes). +/// @return Number of MAC bytes written, or -1 on failure. +int ReadMac(const char* iface_name, unsigned char out_mac[8]) noexcept +{ + if (!iface_name || !out_mac) + return -1; + + ::ifreq ifr{}; + std::strncpy(ifr.ifr_name, iface_name, IFNAMSIZ - 1); + + const int fd = ::socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) + return -1; + + const int rc = ::ioctl(fd, SIOCGIFHWADDR, &ifr); + ::close(fd); + if (rc < 0) + return -1; + + std::memcpy(out_mac, ifr.ifr_hwaddr.sa_data, 6); + return 6; +} + +} // namespace + +bool NetworkIdentity::Resolve(const std::string& iface_name) +{ + unsigned char mac[8]{}; + const int len = ReadMac(iface_name.c_str(), mac); + + if (len == 6) + { + // EUI-48 → EUI-64: insert 0xFF 0xFE after the OUI (octets 0-2) + identity_.id[0] = mac[0]; + identity_.id[1] = mac[1]; + identity_.id[2] = mac[2]; + identity_.id[3] = 0xFFU; + identity_.id[4] = 0xFEU; + identity_.id[5] = mac[3]; + identity_.id[6] = mac[4]; + identity_.id[7] = mac[5]; + return true; + } + if (len == 8) + { + std::memcpy(identity_.id, mac, 8); + return true; + } + return false; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/platform/linux/phc_adjuster.cpp b/score/TimeSlave/code/gptp/platform/linux/phc_adjuster.cpp new file mode 100644 index 0000000..2f4d782 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/phc_adjuster.cpp @@ -0,0 +1,115 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/phc/phc_adjuster.h" + +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// clock_adjtime is not always exposed via glibc headers in cross-compilers. +// Use the syscall directly. +int phc_clock_adjtime(clockid_t clk_id, struct timex* tx) +{ + return static_cast(::syscall(SYS_clock_adjtime, clk_id, tx)); +} + +// Construct a clockid from a PHC file descriptor (kernel convention). +// See linux/include/uapi/linux/time.h +clockid_t phc_fd_to_clockid(int fd) +{ + // NOLINTNEXTLINE(hicpp-signed-bitwise) + return static_cast(~fd << 3 | 3); +} + +} // namespace + +PhcAdjuster::PhcAdjuster(PhcConfig cfg) : cfg_{std::move(cfg)} +{ + if (cfg_.enabled && !cfg_.device.empty()) + { + phc_fd_ = ::open(cfg_.device.c_str(), O_RDWR); + } +} + +PhcAdjuster::~PhcAdjuster() +{ + if (phc_fd_ >= 0) + { + ::close(phc_fd_); + phc_fd_ = -1; + } +} + +void PhcAdjuster::AdjustOffset(std::int64_t offset_ns) +{ + if (!cfg_.enabled || phc_fd_ < 0) + return; + + // Only step-correct large offsets; small ones are handled by frequency slew + if (std::abs(offset_ns) < cfg_.step_threshold_ns) + return; + + struct timex tx + { + }; + tx.modes = ADJ_SETOFFSET | ADJ_NANO; + tx.time.tv_sec = static_cast(offset_ns / 1'000'000'000LL); + tx.time.tv_usec = static_cast(offset_ns % 1'000'000'000LL); + + // Handle negative sub-second values + if (tx.time.tv_usec < 0) + { + tx.time.tv_sec -= 1; + tx.time.tv_usec += 1'000'000'000L; + } + + (void)phc_clock_adjtime(phc_fd_to_clockid(phc_fd_), &tx); +} + +void PhcAdjuster::AdjustFrequency(double rate_ratio) +{ + if (!cfg_.enabled || phc_fd_ < 0) + return; + + // Convert rate_ratio to ppb offset from 1.0, then to scaled ppm for kernel + // rate_ratio = slave_interval / master_interval + // ppb = (rate_ratio - 1.0) * 1e9 + // kernel expects freq in units of 2^-16 ppm = (ppb / 1000) * 65536 + const double ppb = (rate_ratio - 1.0) * 1e9; + const long scaled_ppm = static_cast(ppb / 1000.0 * 65536.0); + + struct timex tx + { + }; + tx.modes = ADJ_FREQUENCY; + tx.freq = scaled_ppm; + + (void)phc_clock_adjtime(phc_fd_to_clockid(phc_fd_), &tx); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/platform/linux/raw_socket.cpp b/score/TimeSlave/code/gptp/platform/linux/raw_socket.cpp new file mode 100644 index 0000000..90e03fc --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/raw_socket.cpp @@ -0,0 +1,200 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/raw_socket.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +void DrainErrQueue(int fd) noexcept +{ + char buf[2048]; + ::iovec iov{buf, sizeof(buf)}; + char ctrl[2048]; + ::msghdr msg{}; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = ctrl; + msg.msg_controllen = sizeof(ctrl); + + while (::recvmsg(fd, &msg, MSG_ERRQUEUE) > 0) + { + } +} + +} // namespace + +RawSocket::~RawSocket() +{ + Close(); +} + +bool RawSocket::Open(const std::string& iface) +{ + Close(); + + const int fd = ::socket(AF_PACKET, SOCK_RAW, htons(ETH_P_1588)); + if (fd < 0) + return false; + + ::ifreq ifr{}; + std::strncpy(ifr.ifr_name, iface.c_str(), IFNAMSIZ - 1); + if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) + { + ::close(fd); + return false; + } + + ::sockaddr_ll sa{}; + sa.sll_family = AF_PACKET; + sa.sll_protocol = htons(ETH_P_1588); + sa.sll_ifindex = ifr.ifr_ifindex; + if (::bind(fd, reinterpret_cast<::sockaddr*>(&sa), sizeof(sa)) < 0) + { + ::close(fd); + return false; + } + + // SO_BINDTODEVICE: best-effort, don't fail if it doesn't work + (void)::setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, iface.c_str(), static_cast(iface.size())); + + fd_ = fd; + iface_ = iface; + return true; +} + +bool RawSocket::EnableHwTimestamping() +{ + if (fd_ < 0) + return false; + + ::ifreq ifr{}; + ::hwtstamp_config cfg{}; + std::strncpy(ifr.ifr_name, iface_.c_str(), IFNAMSIZ - 1); + ifr.ifr_data = reinterpret_cast(&cfg); + + cfg.tx_type = HWTSTAMP_TX_ON; + cfg.rx_filter = HWTSTAMP_FILTER_ALL; + + if (::ioctl(fd_, SIOCSHWTSTAMP, &ifr) < 0) + { + // Fall back to PTP-only filter + cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; + (void)::ioctl(fd_, SIOCSHWTSTAMP, &ifr); + } + + const int ts_opts = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE; + if (::setsockopt(fd_, SOL_SOCKET, SO_TIMESTAMPING, &ts_opts, sizeof(ts_opts)) < 0) + { + return false; + } + return true; +} + +void RawSocket::Close() +{ + if (fd_ >= 0) + { + ::close(fd_); + fd_ = -1; + } + iface_.clear(); +} + +int RawSocket::Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) +{ + if (fd_ < 0 || buf == nullptr || buf_len == 0) + return -1; + + // Poll with caller-specified timeout + ::pollfd pfd{fd_, POLLIN, 0}; + const int pr = ::poll(&pfd, 1, timeout_ms); + if (pr == 0) + return 0; // timeout + if (pr < 0) + return -1; + + char ctrl[1024]; + ::iovec iov{buf, buf_len}; + ::msghdr msg{}; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = ctrl; + msg.msg_controllen = sizeof(ctrl); + + const int len = static_cast(::recvmsg(fd_, &msg, 0)); + if (len < 0) + return -1; + + std::memset(&hwts, 0, sizeof(hwts)); + for (::cmsghdr* cm = CMSG_FIRSTHDR(&msg); cm != nullptr; cm = CMSG_NXTHDR(&msg, cm)) + { + if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMPING) + { + const auto* ts = reinterpret_cast(CMSG_DATA(cm)); + if (ts[2].tv_sec != 0 || ts[2].tv_nsec != 0) + hwts = ts[2]; + } + } + return len; +} + +int RawSocket::Send(const void* buf, int len, ::timespec& hwts) +{ + if (fd_ < 0 || buf == nullptr || len <= 0) + return -1; + + DrainErrQueue(fd_); + + const int sent = static_cast(::send(fd_, buf, static_cast(len), 0)); + if (sent < 0) + return -1; + + // Retrieve TX hardware timestamp from error queue + ::pollfd pfd{fd_, POLLERR, 0}; + if (::poll(&pfd, 1, -1) > 0 && (pfd.revents & POLLERR) != 0) + { + std::uint8_t tmp[2048]; + ::timespec tx_hwts{}; + (void)Recv(tmp, sizeof(tmp), tx_hwts, 0); + hwts = tx_hwts; + } + else + { + std::memset(&hwts, 0, sizeof(hwts)); + } + return sent; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/platform/qnx/BUILD b/score/TimeSlave/code/gptp/platform/qnx/BUILD new file mode 100644 index 0000000..4bba537 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/BUILD @@ -0,0 +1,33 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +filegroup( + name = "raw_socket_src", + srcs = [ + "qnx_raw_shim.cpp", + "raw_socket.cpp", + ], + visibility = ["//score:__subpackages__"], +) + +filegroup( + name = "network_identity_src", + srcs = ["network_identity.cpp"], + visibility = ["//score:__subpackages__"], +) + +filegroup( + name = "phc_adjuster_src", + srcs = ["phc_adjuster.cpp"], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/platform/qnx/network_identity.cpp b/score/TimeSlave/code/gptp/platform/qnx/network_identity.cpp new file mode 100644 index 0000000..7172bec --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/network_identity.cpp @@ -0,0 +1,98 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/network_identity.h" + +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +/// Read the MAC address of @p iface_name into @p out_mac (6 or 8 bytes). +/// @return Number of MAC bytes written, or -1 on failure. +int ReadMac(const char* iface_name, unsigned char out_mac[8]) noexcept +{ + if (!iface_name || !out_mac) + return -1; + + ::ifaddrs* ifaddr = nullptr; + if (::getifaddrs(&ifaddr) != 0 || ifaddr == nullptr) + return -1; + + int result = -1; + for (::ifaddrs* ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) + { + if (!ifa->ifa_name || !ifa->ifa_addr) + continue; + if (std::strcmp(ifa->ifa_name, iface_name) != 0) + continue; + if (ifa->ifa_addr->sa_family != AF_LINK) + continue; + + const auto* sdl = reinterpret_cast(ifa->ifa_addr); + const auto* mac = reinterpret_cast(LLADDR(sdl)); + const int len = static_cast(sdl->sdl_alen); + if (len == 6 || len == 8) + { + std::memcpy(out_mac, mac, static_cast(len)); + result = len; + break; + } + } + + ::freeifaddrs(ifaddr); + return result; +} + +} // namespace + +bool NetworkIdentity::Resolve(const std::string& iface_name) +{ + unsigned char mac[8]{}; + const int len = ReadMac(iface_name.c_str(), mac); + + if (len == 6) + { + // EUI-48 → EUI-64: insert 0xFF 0xFE after the OUI (octets 0-2) + identity_.id[0] = mac[0]; + identity_.id[1] = mac[1]; + identity_.id[2] = mac[2]; + identity_.id[3] = 0xFFU; + identity_.id[4] = 0xFEU; + identity_.id[5] = mac[3]; + identity_.id[6] = mac[4]; + identity_.id[7] = mac[5]; + return true; + } + if (len == 8) + { + std::memcpy(identity_.id, mac, 8); + return true; + } + return false; +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/platform/qnx/phc_adjuster.cpp b/score/TimeSlave/code/gptp/platform/qnx/phc_adjuster.cpp new file mode 100644 index 0000000..dfc8aab --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/phc_adjuster.cpp @@ -0,0 +1,69 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/phc/phc_adjuster.h" + +#include + +// Extern C functions from qnx_raw_shim.cpp +extern "C" int qnx_phc_open(const char* phc_dev); +extern "C" int qnx_phc_adjtime_step(int phc_fd, long long offset_ns); +extern "C" int qnx_phc_adjfreq_ppb(int phc_fd, long long freq_ppb); + +namespace score +{ +namespace ts +{ +namespace details +{ + +PhcAdjuster::PhcAdjuster(PhcConfig cfg) : cfg_{std::move(cfg)} +{ + if (cfg_.enabled && !cfg_.device.empty()) + { + phc_fd_ = qnx_phc_open(cfg_.device.c_str()); + } +} + +PhcAdjuster::~PhcAdjuster() +{ + phc_fd_ = -1; +} + +void PhcAdjuster::AdjustOffset(std::int64_t offset_ns) +{ + if (!cfg_.enabled) + return; + + // Only step-correct large offsets; small ones are handled by frequency slew + if (std::abs(offset_ns) < cfg_.step_threshold_ns) + return; + + (void)qnx_phc_adjtime_step(phc_fd_, static_cast(offset_ns)); +} + +void PhcAdjuster::AdjustFrequency(double rate_ratio) +{ + if (!cfg_.enabled) + return; + + // Convert rate_ratio to ppb offset from 1.0 + // rate_ratio = slave_interval / master_interval + // ppb = (rate_ratio - 1.0) * 1e9 + const auto ppb = static_cast((rate_ratio - 1.0) * 1e9); + + (void)qnx_phc_adjfreq_ppb(phc_fd_, ppb); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/platform/qnx/qnx_raw_shim.cpp b/score/TimeSlave/code/gptp/platform/qnx/qnx_raw_shim.cpp new file mode 100644 index 0000000..44436fd --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/qnx_raw_shim.cpp @@ -0,0 +1,552 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ + +// QNX BPF-based raw socket shim for gPTP frame capture and transmission. +// Provides qnx_raw_open / qnx_raw_recv / qnx_raw_send / qnx_phc_* symbols +// declared in raw_socket.cpp (extern "C"). + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// QNX SDP 8.0: PTP API constants (from io-sock/ptp.h, inlined to avoid +// struct PortIdentity redefinition conflict with details/ptp_types.h). +#define PTP_GET_TIME 0x102 +#define PTP_SET_TIME 0x103 +struct ptp_time +{ + int64_t sec; + int32_t nsec; +}; + +// Inlined ptp_tstmp (from io-sock/ptp.h) — avoids PortIdentity name collision. +// A TX loopback frame contains an Ethernet header followed by this struct. +struct PtpTstmp +{ + uint32_t uid; + ptp_time time; +}; + +// ── EtherType constants ─────────────────────────────────────────────────────── +#ifndef ETH_P_8021Q +#define ETH_P_8021Q 0x8100U +#endif +#ifndef ETH_P_1588 +#define ETH_P_1588 0x88F7U +#endif + +// ── Self-contained ethernet header layout ──────────────────────────────────── +struct GptpEthHdr +{ + unsigned char h_dest[6]; + unsigned char h_source[6]; + uint16_t h_proto; +}; + +static constexpr int64_t kNsPerSec = 1'000'000'000LL; +static constexpr std::size_t kMaxBpfBufSz = 65536U; +static constexpr int kMaxTxScanTries = 8; + +// Caplen of a BPF TX loopback frame injected by the PTP driver: +// Ethernet header (14 B) + ptp_tstmp payload (4 + 12 = 16 B) = 30 B +static constexpr int kTxLoopbackCaplen = static_cast(sizeof(GptpEthHdr) + sizeof(PtpTstmp)); + +// ── BPF kernel filter: pass only IEEE 802.1AS (ETH_P_1588) frames ──────────── +// BPF_LD H ABS 12 — load EtherType (bytes 12-13) +// BPF_JEQ ETH_P_1588 — jump if match +// BPF_RET (u_int)-1 — keep entire packet +// BPF_RET 0 — drop +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +static struct bpf_insn kPtp1588FilterInsns[] = { + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETH_P_1588, 0, 1), + BPF_STMT(BPF_RET + BPF_K, static_cast(-1)), + BPF_STMT(BPF_RET + BPF_K, 0), +}; +static const u_int kPtp1588FilterLen = static_cast(sizeof(kPtp1588FilterInsns) / sizeof(kPtp1588FilterInsns[0])); + +// ── Per-thread BPF context ─────────────────────────────────────────────────── +struct QnxRawContext +{ + int bpf_fd = -1; + u_int bpf_buflen = 0; + char iface_name[IFNAMSIZ]{}; + unsigned char bpf_buf[kMaxBpfBufSz]{}; + ssize_t bpf_n = 0; + ssize_t bpf_off = 0; + bool initialized = false; + unsigned char tx_frame[ETHER_HDR_LEN + 1500]{}; + + // Secondary BPF fd with BIOCSSEESENT=1 for reading TX loopback timestamps. + // Lazily opened on first qnx_raw_send() call. + int tx_loopback_fd = -1; + u_int tx_loopback_buflen = 0; + unsigned char tx_loopback_buf[kMaxBpfBufSz]{}; + + ~QnxRawContext() + { + if (bpf_fd >= 0) + { + ::close(bpf_fd); + bpf_fd = -1; + } + if (tx_loopback_fd >= 0) + { + ::close(tx_loopback_fd); + tx_loopback_fd = -1; + } + } +}; + +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +thread_local QnxRawContext g_qnx_ctx; + +// ── Internal helpers ────────────────────────────────────────────────────────── + +// Convert a bpf_xhdr hardware timestamp to timespec. +// bpf_ts::bt_sec — seconds (int64_t) +// bpf_ts::bt_frac — binary fraction of a second (uint64_t, unit = 2^-64 s) +// This is equivalent to bintime2timespec() from . +static void bpf_ts_to_timespec(const bpf_xhdr* bh, struct timespec* ts) noexcept +{ + ts->tv_sec = static_cast(bh->bh_tstamp.bt_sec); + const uint64_t top32 = bh->bh_tstamp.bt_frac >> 32U; + ts->tv_nsec = static_cast((top32 * 1'000'000'000ULL) >> 32U); +} + +// Parse an Ethernet/VLAN frame; return byte offset of PTP payload or -1. +static int ptp_payload_offset(const unsigned char* frame, int caplen) +{ + if (caplen < static_cast(sizeof(GptpEthHdr))) + return -1; + + GptpEthHdr eth{}; + std::memcpy(ð, frame, sizeof(GptpEthHdr)); + uint16_t etype = ntohs(eth.h_proto); + int offset = static_cast(sizeof(GptpEthHdr)); + + if (etype == ETH_P_8021Q) + { + if (caplen < offset + 4) + return -1; + uint16_t inner{}; + std::memcpy(&inner, frame + offset + 2, sizeof(uint16_t)); + etype = ntohs(inner); + offset += 4; + } + + return (etype == ETH_P_1588) ? offset : -1; +} + +// Open a secondary BPF fd on the same interface as main_fd, with +// BIOCSSEESENT=1 so our own TX frames appear as loopback records. +// Stores the resulting buffer length in g_qnx_ctx.tx_loopback_buflen. +// Returns the new fd or -1. +static int open_tx_loopback_fd(int main_fd) noexcept +{ + // Retrieve interface name from the already-bound main fd. + ::ifreq ifr{}; + if (::ioctl(main_fd, BIOCGETIF, &ifr) < 0) + return -1; + + char devpath[256]{}; + const char* sock_env = std::getenv("SOCK"); + if (sock_env != nullptr && sock_env[0] != '\0') + std::snprintf(devpath, sizeof(devpath), "%s/dev/bpf0", sock_env); + else + std::snprintf(devpath, sizeof(devpath), "/dev/bpf"); + + int lfd = ::open(devpath, O_RDWR); + if (lfd < 0) + return -1; + + if (::ioctl(lfd, BIOCSETIF, &ifr) < 0) + { + ::close(lfd); + return -1; + } + + // Enable loopback so our sent frames are visible on this fd. + u_int one = 1U; + (void)::ioctl(lfd, BIOCSSEESENT, &one); + (void)::ioctl(lfd, BIOCIMMEDIATE, &one); + + // Request PTP hardware timestamps in bpf_xhdr format. + u_int bpf_ts = BPF_T_PTP | BPF_T_BINTIME; + (void)::ioctl(lfd, BIOCSTSTAMP, &bpf_ts); + + // Apply the same ETH_P_1588 kernel filter. + struct bpf_program prog + { + kPtp1588FilterLen, kPtp1588FilterInsns + }; + (void)::ioctl(lfd, BIOCSETF, &prog); + + u_int buflen = 0U; + if (::ioctl(lfd, BIOCGBLEN, &buflen) < 0 || buflen == 0U || buflen > kMaxBpfBufSz) + { + ::close(lfd); + return -1; + } + g_qnx_ctx.tx_loopback_buflen = buflen; + return lfd; +} + +// ── Public C interface ──────────────────────────────────────────────────────── + +extern "C" int qnx_raw_open(const char* ifname) +{ + if (ifname == nullptr) + { + errno = EINVAL; + return -1; + } + + std::strlcpy(g_qnx_ctx.iface_name, ifname, sizeof(g_qnx_ctx.iface_name)); + + char devpath[256]{}; + const char* sock_env = std::getenv("SOCK"); + if (sock_env != nullptr && sock_env[0] != '\0') + std::snprintf(devpath, sizeof(devpath), "%s/dev/bpf0", sock_env); + else + std::snprintf(devpath, sizeof(devpath), "/dev/bpf"); + + int fd = ::open(devpath, O_RDWR); + if (fd < 0) + return -1; + + ::ifreq ifr{}; + std::strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + if (::ioctl(fd, BIOCSETIF, &ifr) < 0) + { + ::close(fd); + return -1; + } + + // Do NOT see our own TX frames on the main fd; use tx_loopback_fd instead. + int zero = 0; + (void)::ioctl(fd, BIOCSSEESENT, &zero); + + u_int yes = 1U; + (void)::ioctl(fd, BIOCIMMEDIATE, &yes); + (void)::ioctl(fd, BIOCPROMISC, &yes); + + // Request PTP hardware timestamps in bpf_xhdr format (IEEE 1588 clock). + // Falls back gracefully: if unsupported, timestamps will be zero and + // qnx_raw_recv() will fall back to CLOCK_REALTIME. + u_int bpf_ts = BPF_T_PTP | BPF_T_BINTIME; + (void)::ioctl(fd, BIOCSTSTAMP, &bpf_ts); + + // Install kernel BPF filter: discard all non-ETH_P_1588 frames early. + struct bpf_program prog + { + kPtp1588FilterLen, kPtp1588FilterInsns + }; + (void)::ioctl(fd, BIOCSETF, &prog); // best-effort; userspace filter still runs + + if (::ioctl(fd, BIOCGBLEN, &g_qnx_ctx.bpf_buflen) < 0) + { + ::close(fd); + return -1; + } + if (g_qnx_ctx.bpf_buflen > kMaxBpfBufSz) + { + ::close(fd); + errno = ENOMEM; + return -1; + } + + g_qnx_ctx.bpf_fd = fd; + g_qnx_ctx.initialized = true; + return fd; +} + +extern "C" int qnx_raw_recv(int fd, void* buf, int buf_len, timespec* hwts, int nonblock) +{ + if (fd < 0 || buf == nullptr || buf_len <= 0 || hwts == nullptr) + { + errno = EINVAL; + return -1; + } + if (!g_qnx_ctx.initialized || g_qnx_ctx.bpf_buflen == 0) + { + errno = EINVAL; + return -1; + } + + if (nonblock != 0) + { + int flags = ::fcntl(fd, F_GETFL, 0); + if (flags >= 0) + (void)::fcntl(fd, F_SETFL, flags | O_NONBLOCK); + } + + for (;;) + { + // Refill BPF read buffer when exhausted. + if (g_qnx_ctx.bpf_off >= g_qnx_ctx.bpf_n) + { + ssize_t n = ::read(fd, g_qnx_ctx.bpf_buf, g_qnx_ctx.bpf_buflen); + if (n < 0) + return -1; + if (n == 0) + { + if (nonblock != 0) + { + errno = EAGAIN; + return -1; + } + continue; + } + g_qnx_ctx.bpf_n = n; + g_qnx_ctx.bpf_off = 0; + } + + // Need at least sizeof(bpf_xhdr) bytes for the header. + if (g_qnx_ctx.bpf_off + static_cast(sizeof(bpf_xhdr)) > g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + // Verify 8-byte alignment required by bpf_xhdr. + const auto ptr_val = reinterpret_cast(g_qnx_ctx.bpf_buf + g_qnx_ctx.bpf_off); + if (ptr_val % alignof(bpf_xhdr) != 0U) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + const auto* bh = reinterpret_cast(g_qnx_ctx.bpf_buf + g_qnx_ctx.bpf_off); + + // Bounds checks. + if (bh->bh_hdrlen < static_cast(sizeof(bpf_xhdr)) || + bh->bh_caplen > static_cast(g_qnx_ctx.bpf_n) || + g_qnx_ctx.bpf_off + static_cast(bh->bh_hdrlen) + static_cast(bh->bh_caplen) > + g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + const unsigned char* pkt = reinterpret_cast(bh) + bh->bh_hdrlen; + const int caplen = static_cast(bh->bh_caplen); + const ssize_t next_off = g_qnx_ctx.bpf_off + static_cast(BPF_WORDALIGN(bh->bh_hdrlen + bh->bh_caplen)); + + // Skip TX loopback frames (BIOCSSEESENT=0 should prevent them on the + // main fd, but guard defensively: a loopback frame has a fixed small + // caplen equal to ETH header + ptp_tstmp, not a valid PTP message). + if (caplen == kTxLoopbackCaplen) + { + g_qnx_ctx.bpf_off = next_off; + continue; + } + + const int ptp_off = ptp_payload_offset(pkt, caplen); + if (ptp_off >= 0) + { + // Use PTP hardware RX timestamp from bpf_xhdr. + // bt_sec==0 && bt_frac==0 means the driver did not provide a HW + // timestamp; fall back to CLOCK_REALTIME in that case. + if (bh->bh_tstamp.bt_sec != 0 || bh->bh_tstamp.bt_frac != 0) + { + bpf_ts_to_timespec(bh, hwts); + } + else + { + (void)::clock_gettime(CLOCK_REALTIME, hwts); + } + + const int frame_len = std::min(caplen, buf_len); + std::memcpy(buf, pkt, static_cast(frame_len)); + g_qnx_ctx.bpf_off = next_off; + return frame_len; + } + + g_qnx_ctx.bpf_off = next_off; + } +} + +extern "C" int qnx_raw_send(int fd, const void* buf, int len, timespec* hwts) +{ + if (fd < 0 || buf == nullptr || len <= 0 || hwts == nullptr) + { + errno = EINVAL; + return -1; + } + if (static_cast(len) > 1500U) + { + errno = EMSGSIZE; + return -1; + } + + std::memcpy(g_qnx_ctx.tx_frame, buf, static_cast(len)); + ssize_t n = ::write(fd, g_qnx_ctx.tx_frame, static_cast(len)); + if (n < 0) + return -1; + + // Attempt to obtain a hardware TX timestamp via the BPF loopback mechanism: + // 1. BIOCGTSTAMPID returns the UID assigned to the just-sent frame. + // 2. The driver inserts a loopback record on fds with BIOCSSEESENT=1; + // its payload is a ptp_tstmp struct carrying the actual HW timestamp. + // 3. We scan the secondary loopback fd for a record whose uid matches. + // If any step fails, we fall back to a CLOCK_REALTIME software timestamp. + uint32_t tx_uid = 0U; + if (::ioctl(fd, BIOCGTSTAMPID, &tx_uid) == 0) + { + // Lazy-open the secondary fd (needs BIOCGETIF to recover iface name). + if (g_qnx_ctx.tx_loopback_fd < 0) + g_qnx_ctx.tx_loopback_fd = open_tx_loopback_fd(fd); + + if (g_qnx_ctx.tx_loopback_fd >= 0 && g_qnx_ctx.tx_loopback_buflen > 0) + { + const int lfd = g_qnx_ctx.tx_loopback_fd; + + // Non-blocking scan: the loopback frame typically arrives within + // a few microseconds; we try kMaxTxScanTries reads. + int flags = ::fcntl(lfd, F_GETFL, 0); + (void)::fcntl(lfd, F_SETFL, (flags >= 0 ? flags : 0) | O_NONBLOCK); + + for (int tries = 0; tries < kMaxTxScanTries; ++tries) + { + ssize_t nr = ::read(lfd, g_qnx_ctx.tx_loopback_buf, g_qnx_ctx.tx_loopback_buflen); + if (nr <= 0) + break; + + ssize_t off = 0; + while (off + static_cast(sizeof(bpf_xhdr)) <= nr) + { + const auto pv = reinterpret_cast(g_qnx_ctx.tx_loopback_buf + off); + if (pv % alignof(bpf_xhdr) != 0U) + break; + + const auto* bh = reinterpret_cast(g_qnx_ctx.tx_loopback_buf + off); + + if (bh->bh_hdrlen < static_cast(sizeof(bpf_xhdr)) || + off + static_cast(bh->bh_hdrlen) + static_cast(bh->bh_caplen) > nr) + break; + + const unsigned char* pkt = reinterpret_cast(bh) + bh->bh_hdrlen; + const int caplen = static_cast(bh->bh_caplen); + const ssize_t next = off + static_cast(BPF_WORDALIGN(bh->bh_hdrlen + bh->bh_caplen)); + + // A TX loopback record has a fixed caplen and contains a + // ptp_tstmp payload right after the Ethernet header. + if (caplen == kTxLoopbackCaplen) + { + const auto* tstmp = reinterpret_cast(pkt + sizeof(GptpEthHdr)); + if (tstmp->uid == tx_uid) + { + hwts->tv_sec = static_cast(tstmp->time.sec); + hwts->tv_nsec = static_cast(tstmp->time.nsec); + return static_cast(len); + } + } + off = next; + } + } + } + } + + // Fallback: software TX timestamp. + (void)::clock_gettime(CLOCK_REALTIME, hwts); + return static_cast(len); +} + +// ── PHC clock adjustment (QNX SDP 8.0 io-sock/ptp.h ioctl path) ────────────── + +extern "C" int qnx_phc_open(const char* phc_dev) +{ + if (phc_dev != nullptr && phc_dev[0] != '\0' && phc_dev[0] != '/') + std::strlcpy(g_qnx_ctx.iface_name, phc_dev, sizeof(g_qnx_ctx.iface_name)); + return 0; +} + +extern "C" int qnx_phc_adjtime_step(int /*phc_fd*/, long long offset_ns) +{ + if (offset_ns == 0) + return 0; + + const int s = ::socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) + return -1; + + struct + { + struct ifdrv ifd; + struct ptp_time tm; + } cmd{}; + + std::strncpy(cmd.ifd.ifd_name, g_qnx_ctx.iface_name, sizeof(cmd.ifd.ifd_name) - 1U); + cmd.ifd.ifd_len = sizeof(cmd.tm); + cmd.ifd.ifd_data = &cmd.tm; + cmd.ifd.ifd_cmd = PTP_GET_TIME; + + if (::ioctl(s, SIOCGDRVSPEC, &cmd) == -1) + { + ::close(s); + return -1; + } + + const int64_t cur_ns = cmd.tm.sec * kNsPerSec + static_cast(cmd.tm.nsec); + const int64_t new_ns = cur_ns + static_cast(offset_ns); + + cmd.tm.sec = new_ns / kNsPerSec; + cmd.tm.nsec = static_cast(new_ns % kNsPerSec); + if (cmd.tm.nsec < 0) + { + cmd.tm.nsec += static_cast(kNsPerSec); + cmd.tm.sec -= 1; + } + + cmd.ifd.ifd_cmd = PTP_SET_TIME; + const int r = ::ioctl(s, SIOCSDRVSPEC, &cmd); + ::close(s); + return r; +} + +extern "C" int qnx_phc_adjfreq_ppb(int /*phc_fd*/, long long freq_ppb) +{ + if (freq_ppb == 0) + return 0; + + const int s = ::socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) + return -1; + + // Convert ppb to ppm (EMAC_PTP_ADJ_FREQ_PPM expects ppm) + int ppm = static_cast(freq_ppb / 1000LL); + + struct + { + struct ifdrv ifd; + int adj_ppm; + } cmd{}; + + std::strncpy(cmd.ifd.ifd_name, g_qnx_ctx.iface_name, sizeof(cmd.ifd.ifd_name) - 1U); + cmd.ifd.ifd_len = sizeof(cmd.adj_ppm); + cmd.ifd.ifd_data = &cmd.adj_ppm; + cmd.ifd.ifd_cmd = 0x200; // EMAC_PTP_ADJ_FREQ_PPM + cmd.adj_ppm = ppm; + + const int r = ::ioctl(s, SIOCGDRVSPEC, &cmd); + ::close(s); + return r; +} diff --git a/score/TimeSlave/code/gptp/platform/qnx/raw_socket.cpp b/score/TimeSlave/code/gptp/platform/qnx/raw_socket.cpp new file mode 100644 index 0000000..a970708 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/raw_socket.cpp @@ -0,0 +1,89 @@ +/******************************************************************************** + * Copyright (c) 2025 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/details/raw_socket.h" + +#include +#include +#include +#include +#include +#include +#include + +// QNX raw shim C linkage (provided by existing qnx_raw_shim target) +extern "C" { +int qnx_raw_open(const char* ifname); +int qnx_raw_recv(int fd, void* buf, int len, ::timespec* hwts, int nonblock); +int qnx_raw_send(int fd, void* buf, int len, ::timespec* hwts); +} // extern "C" + +namespace score +{ +namespace ts +{ +namespace details +{ + +RawSocket::~RawSocket() +{ + Close(); +} + +bool RawSocket::Open(const std::string& iface) +{ + Close(); + fd_ = qnx_raw_open(iface.c_str()); + if (fd_ < 0) + return false; + iface_ = iface; + return true; +} + +bool RawSocket::EnableHwTimestamping() +{ + // HW timestamping configured inside qnx_raw_open; nothing more needed. + return true; +} + +void RawSocket::Close() +{ + if (fd_ >= 0) + { + ::close(fd_); + fd_ = -1; + } + iface_.clear(); +} + +int RawSocket::Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) +{ + if (fd_ < 0 || buf == nullptr || buf_len == 0) + return -1; + + const int nonblock = (timeout_ms == 0) ? 1 : 0; + // QNX shim: nonblock==0 means blocking; only full non-blocking is supported. + // For timeout > 0 we fall back to a blocking call (best effort). + (void)timeout_ms; + return qnx_raw_recv(fd_, buf, static_cast(buf_len), &hwts, nonblock); +} + +int RawSocket::Send(const void* buf, int len, ::timespec& hwts) +{ + if (fd_ < 0 || buf == nullptr || len <= 0) + return -1; + return qnx_raw_send(fd_, const_cast(buf), len, &hwts); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/record/BUILD b/score/TimeSlave/code/gptp/record/BUILD new file mode 100644 index 0000000..3dd006a --- /dev/null +++ b/score/TimeSlave/code/gptp/record/BUILD @@ -0,0 +1,42 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "recorder", + srcs = ["recorder.cpp"], + hdrs = ["recorder.h"], + features = COMPILER_WARNING_FEATURES, + tags = ["QM"], + visibility = ["//score:__subpackages__"], +) + +cc_test( + name = "recorder_test", + srcs = ["recorder_test.cpp"], + tags = ["unit"], + deps = [ + ":recorder", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":recorder_test"], + test_suites_from_sub_packages = [], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/record/recorder.cpp b/score/TimeSlave/code/gptp/record/recorder.cpp new file mode 100644 index 0000000..f56ee55 --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder.cpp @@ -0,0 +1,52 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/record/recorder.h" + +namespace score +{ +namespace ts +{ +namespace details +{ + +Recorder::Recorder(Config cfg) : cfg_{std::move(cfg)} +{ + if (cfg_.enabled) + { + file_.open(cfg_.file_path, std::ios::out | std::ios::app); + if (file_.is_open()) + { + // Write CSV header if the file is empty + file_.seekp(0, std::ios::end); + if (file_.tellp() == 0) + { + file_ << "mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags\n"; + } + } + } +} + +void Recorder::Record(const RecordEntry& entry) +{ + if (!cfg_.enabled || !file_.is_open()) + return; + + std::lock_guard lk(mutex_); + file_ << entry.mono_ns << ',' << static_cast(entry.event) << ',' << entry.offset_ns << ',' << entry.pdelay_ns + << ',' << entry.seq_id << ',' << static_cast(entry.status_flags) << '\n'; + file_.flush(); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/TimeSlave/code/gptp/record/recorder.h b/score/TimeSlave/code/gptp/record/recorder.h new file mode 100644 index 0000000..839bf16 --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder.h @@ -0,0 +1,89 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_RECORD_RECORDER_H +#define SCORE_TIMESLAVE_CODE_GPTP_RECORD_RECORDER_H + +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Event types that can be recorded. +enum class RecordEvent : std::uint8_t +{ + kSyncReceived = 0, + kPdelayCompleted = 1, + kClockJump = 2, + kOffsetThreshold = 3, + kProbe = 4, +}; + +/// A single record entry written to the log file. +struct RecordEntry +{ + std::int64_t mono_ns{0}; + RecordEvent event{RecordEvent::kSyncReceived}; + std::int64_t offset_ns{0}; + std::int64_t pdelay_ns{0}; + std::uint16_t seq_id{0}; + std::uint8_t status_flags{0}; +}; + +/** + * @brief Thread-safe CSV file recorder for gPTP events. + * + * When enabled, appends CSV lines to the configured file path. + * Format: mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags + */ +class Recorder final +{ + public: + struct Config + { + bool enabled = false; + std::string file_path = "/var/log/gptp_record.csv"; + std::int64_t offset_threshold_ns = 1'000'000LL; ///< 1 ms + }; + + explicit Recorder(Config cfg); + ~Recorder() = default; + + Recorder(const Recorder&) = delete; + Recorder& operator=(const Recorder&) = delete; + + bool IsEnabled() const + { + return cfg_.enabled && file_.is_open(); + } + + /// Record an entry. Thread-safe. + void Record(const RecordEntry& entry); + + private: + Config cfg_; + std::mutex mutex_; + std::ofstream file_; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_RECORD_RECORDER_H diff --git a/score/TimeSlave/code/gptp/record/recorder_test.cpp b/score/TimeSlave/code/gptp/record/recorder_test.cpp new file mode 100644 index 0000000..35736dd --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder_test.cpp @@ -0,0 +1,184 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/record/recorder.h" + +#include + +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +std::string TempPath() +{ + return "/tmp/recorder_test_" + std::to_string(::getpid()) + ".csv"; +} + +} // namespace + +// ── Disabled recorder ──────────────────────────────────────────────────────── + +TEST(RecorderTest, Disabled_IsEnabledReturnsFalse) +{ + Recorder::Config cfg; + cfg.enabled = false; + Recorder r{cfg}; + EXPECT_FALSE(r.IsEnabled()); +} + +TEST(RecorderTest, Disabled_RecordDoesNotCrash) +{ + Recorder::Config cfg; + cfg.enabled = false; + Recorder r{cfg}; + EXPECT_NO_THROW(r.Record(RecordEntry{})); +} + +// ── Enabled with bad path ───────────────────────────────────────────────────── + +TEST(RecorderTest, Enabled_BadPath_IsEnabledReturnsFalse) +{ + Recorder::Config cfg; + cfg.enabled = true; + cfg.file_path = "/no/such/dir/recorder_test.csv"; + Recorder r{cfg}; + EXPECT_FALSE(r.IsEnabled()); +} + +TEST(RecorderTest, Enabled_BadPath_RecordDoesNotCrash) +{ + Recorder::Config cfg; + cfg.enabled = true; + cfg.file_path = "/no/such/dir/recorder_test.csv"; + Recorder r{cfg}; + EXPECT_NO_THROW(r.Record(RecordEntry{})); +} + +// ── Enabled with valid path ─────────────────────────────────────────────────── + +class RecorderFileTest : public ::testing::Test +{ + protected: + void SetUp() override + { + path_ = TempPath(); + } + void TearDown() override + { + std::remove(path_.c_str()); + } + + Recorder MakeRecorder() + { + Recorder::Config cfg; + cfg.enabled = true; + cfg.file_path = path_; + return Recorder{cfg}; + } + + std::string path_; +}; + +TEST_F(RecorderFileTest, IsEnabled_ReturnsTrue) +{ + auto r = MakeRecorder(); + EXPECT_TRUE(r.IsEnabled()); +} + +TEST_F(RecorderFileTest, NewFile_ContainsCsvHeader) +{ + { + auto r = MakeRecorder(); + } // destructor closes file + + std::ifstream f(path_); + std::string line; + ASSERT_TRUE(std::getline(f, line)); + EXPECT_EQ(line, "mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags"); +} + +TEST_F(RecorderFileTest, Record_WritesOneDataLine) +{ + auto r = MakeRecorder(); + + RecordEntry e{}; + e.mono_ns = 123456789LL; + e.event = RecordEvent::kSyncReceived; + e.offset_ns = -500LL; + e.pdelay_ns = 1000LL; + e.seq_id = 42U; + e.status_flags = 0x03U; + r.Record(e); + + // Flush by destroying the recorder before reading back + r.Record(RecordEntry{}); // second line +} + +TEST_F(RecorderFileTest, Record_MultipleEntries_AllFlushedToFile) +{ + { + auto r = MakeRecorder(); + for (int i = 0; i < 5; ++i) + { + RecordEntry e{}; + e.mono_ns = static_cast(i) * 1'000'000LL; + e.event = RecordEvent::kPdelayCompleted; + e.seq_id = static_cast(i); + r.Record(e); + } + } + + // Count lines: header + 5 data lines = 6 + std::ifstream f(path_); + int lines = 0; + std::string line; + while (std::getline(f, line)) + ++lines; + EXPECT_EQ(lines, 6); +} + +TEST_F(RecorderFileTest, Record_FieldsWrittenCorrectly) +{ + { + auto r = MakeRecorder(); + RecordEntry e{}; + e.mono_ns = 9'000'000'000LL; + e.event = RecordEvent::kClockJump; + e.offset_ns = 12345LL; + e.pdelay_ns = 999LL; + e.seq_id = 7U; + e.status_flags = 0x01U; + r.Record(e); + } + + std::ifstream f(path_); + std::string header, data; + ASSERT_TRUE(std::getline(f, header)); + ASSERT_TRUE(std::getline(f, data)); + + // Format: mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags + EXPECT_EQ(data, "9000000000,2,12345,999,7,1"); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/BUILD b/score/libTSClient/BUILD new file mode 100644 index 0000000..1807bcc --- /dev/null +++ b/score/libTSClient/BUILD @@ -0,0 +1,54 @@ +# ******************************************************************************* +# Copyright (c) 2026 Contributors to the Eclipse Foundation +# +# See the NOTICE file(s) distributed with this work for additional +# information regarding copyright ownership. +# +# This program and the accompanying materials are made available under the +# terms of the Apache License Version 2.0 which is available at +# https://www.apache.org/licenses/LICENSE-2.0 +# +# SPDX-License-Identifier: Apache-2.0 +# ******************************************************************************* + +load("@score_baselibs//:bazel/unit_tests.bzl", "cc_unit_test_suites_for_host_and_qnx") +load("@score_baselibs//score/language/safecpp:toolchain_features.bzl", "COMPILER_WARNING_FEATURES") + +cc_library( + name = "gptp_ipc", + srcs = [ + "gptp_ipc_publisher.cpp", + "gptp_ipc_receiver.cpp", + ], + hdrs = [ + "gptp_ipc.h", + "gptp_ipc_channel.h", + "gptp_ipc_publisher.h", + "gptp_ipc_receiver.h", + ], + features = COMPILER_WARNING_FEATURES, + linkopts = ["-lrt"], + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [ + "//score/TimeDaemon/code/common/data_types:ptp_time_info", + ], +) + +cc_test( + name = "gptp_ipc_test", + srcs = ["gptp_ipc_test.cpp"], + tags = ["unit"], + deps = [ + ":gptp_ipc", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [":gptp_ipc_test"], + test_suites_from_sub_packages = [], + visibility = ["//score:__subpackages__"], +) diff --git a/score/libTSClient/gptp_ipc.h b/score/libTSClient/gptp_ipc.h new file mode 100644 index 0000000..73ebf44 --- /dev/null +++ b/score/libTSClient/gptp_ipc.h @@ -0,0 +1,20 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_LIBTSCLIENT_GPTP_IPC_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_H + +#include "score/libTSClient/gptp_ipc_channel.h" +#include "score/libTSClient/gptp_ipc_publisher.h" +#include "score/libTSClient/gptp_ipc_receiver.h" + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_H diff --git a/score/libTSClient/gptp_ipc_channel.h b/score/libTSClient/gptp_ipc_channel.h new file mode 100644 index 0000000..f7cc936 --- /dev/null +++ b/score/libTSClient/gptp_ipc_channel.h @@ -0,0 +1,56 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_LIBTSCLIENT_GPTP_IPC_CHANNEL_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_CHANNEL_H + +#include "score/TimeDaemon/code/common/data_types/ptp_time_info.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Default POSIX shared memory name for the gPTP IPC channel. +static constexpr char kGptpIpcName[] = "/gptp_ptp_info"; + +/// Magic number to validate the shared memory region ('GPTP'). +static constexpr std::uint32_t kGptpIpcMagic = 0x47505450U; + +/** + * @brief Shared memory layout for gPTP IPC (seqlock protocol). + * + * Single-writer (TimeSlave), multi-reader (TimeDaemon via RealPTPEngine). + * Aligned to 64 bytes (cache line) to avoid false sharing. + * + * Seqlock protocol: + * - Writer: seq++ (odd = writing), write data, seq_confirm = seq (even = readable) + * - Reader: read seq, read data, read seq_confirm; retry if seq != seq_confirm or odd + */ +struct alignas(64) GptpIpcRegion +{ + std::uint32_t magic{kGptpIpcMagic}; + std::atomic seq{0}; + score::td::PtpTimeInfo data{}; + std::atomic seq_confirm{0}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_CHANNEL_H diff --git a/score/libTSClient/gptp_ipc_publisher.cpp b/score/libTSClient/gptp_ipc_publisher.cpp new file mode 100644 index 0000000..2c4cbc2 --- /dev/null +++ b/score/libTSClient/gptp_ipc_publisher.cpp @@ -0,0 +1,96 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/libTSClient/gptp_ipc_publisher.h" + +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +GptpIpcPublisher::~GptpIpcPublisher() +{ + Destroy(); +} + +bool GptpIpcPublisher::Init(const std::string& ipc_name) +{ + ipc_name_ = ipc_name; + + shm_fd_ = ::shm_open(ipc_name_.c_str(), O_CREAT | O_RDWR, 0666); + if (shm_fd_ < 0) + return false; + + if (::ftruncate(shm_fd_, static_cast(sizeof(GptpIpcRegion))) != 0) + { + ::close(shm_fd_); // LCOV_EXCL_LINE + shm_fd_ = -1; // LCOV_EXCL_LINE + return false; // LCOV_EXCL_LINE + } + + void* ptr = ::mmap(nullptr, sizeof(GptpIpcRegion), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd_, 0); + if (ptr == MAP_FAILED) + { + ::close(shm_fd_); // LCOV_EXCL_LINE + shm_fd_ = -1; // LCOV_EXCL_LINE + return false; // LCOV_EXCL_LINE + } + + region_ = new (ptr) GptpIpcRegion{}; + return true; +} + +void GptpIpcPublisher::Publish(const score::td::PtpTimeInfo& info) +{ + if (region_ == nullptr) + return; + + const std::uint32_t next = region_->seq.load(std::memory_order_relaxed) + 1U; + region_->seq.store(next, std::memory_order_release); + + std::atomic_thread_fence(std::memory_order_release); + std::memcpy(®ion_->data, &info, sizeof(score::td::PtpTimeInfo)); + std::atomic_thread_fence(std::memory_order_release); + + region_->seq_confirm.store(next + 1U, std::memory_order_release); + region_->seq.store(next + 1U, std::memory_order_release); +} + +void GptpIpcPublisher::Destroy() +{ + if (region_ != nullptr) + { + ::munmap(region_, sizeof(GptpIpcRegion)); + region_ = nullptr; + } + if (shm_fd_ >= 0) + { + ::close(shm_fd_); + shm_fd_ = -1; + } + if (!ipc_name_.empty()) + { + ::shm_unlink(ipc_name_.c_str()); + ipc_name_.clear(); + } +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/gptp_ipc_publisher.h b/score/libTSClient/gptp_ipc_publisher.h new file mode 100644 index 0000000..b0f4509 --- /dev/null +++ b/score/libTSClient/gptp_ipc_publisher.h @@ -0,0 +1,62 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_LIBTSCLIENT_GPTP_IPC_PUBLISHER_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_PUBLISHER_H + +#include "score/libTSClient/gptp_ipc_channel.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief Single-writer publisher for the gPTP IPC channel. + * + * Creates the POSIX shared memory segment and writes PtpTimeInfo using + * the seqlock protocol. Used by TimeSlave. + */ +class GptpIpcPublisher final +{ + public: + GptpIpcPublisher() = default; + ~GptpIpcPublisher(); + + GptpIpcPublisher(const GptpIpcPublisher&) = delete; + GptpIpcPublisher& operator=(const GptpIpcPublisher&) = delete; + + /// Create and map the shared memory segment. + /// @return true on success. + bool Init(const std::string& ipc_name = kGptpIpcName); + + /// Publish a PtpTimeInfo snapshot using seqlock. + void Publish(const score::td::PtpTimeInfo& info); + + /// Unmap and unlink the shared memory segment. + void Destroy(); + + private: + GptpIpcRegion* region_{nullptr}; + int shm_fd_{-1}; + std::string ipc_name_; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_PUBLISHER_H diff --git a/score/libTSClient/gptp_ipc_receiver.cpp b/score/libTSClient/gptp_ipc_receiver.cpp new file mode 100644 index 0000000..fbc6422 --- /dev/null +++ b/score/libTSClient/gptp_ipc_receiver.cpp @@ -0,0 +1,101 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/libTSClient/gptp_ipc_receiver.h" + +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +static constexpr int kMaxRetries = 20; + +GptpIpcReceiver::~GptpIpcReceiver() +{ + Close(); +} + +bool GptpIpcReceiver::Init(const std::string& ipc_name) +{ + shm_fd_ = ::shm_open(ipc_name.c_str(), O_RDONLY, 0); + if (shm_fd_ < 0) + return false; + + void* ptr = ::mmap(nullptr, sizeof(GptpIpcRegion), PROT_READ, MAP_SHARED, shm_fd_, 0); + if (ptr == MAP_FAILED) + { + ::close(shm_fd_); + shm_fd_ = -1; + return false; + } + + region_ = static_cast(ptr); + + if (region_->magic != kGptpIpcMagic) + { + Close(); + return false; + } + + return true; +} + +std::optional GptpIpcReceiver::Receive() +{ + if (region_ == nullptr) + return std::nullopt; + + for (int attempt = 0; attempt < kMaxRetries; ++attempt) + { + const std::uint32_t seq1 = region_->seq.load(std::memory_order_acquire); + + if ((seq1 & 1U) != 0U) + continue; + + std::atomic_thread_fence(std::memory_order_acquire); + score::td::PtpTimeInfo data{}; + std::memcpy(&data, ®ion_->data, sizeof(score::td::PtpTimeInfo)); + std::atomic_thread_fence(std::memory_order_acquire); + + const std::uint32_t seq2 = region_->seq_confirm.load(std::memory_order_acquire); + + if (seq1 == seq2) + return data; + } + + return std::nullopt; +} + +void GptpIpcReceiver::Close() +{ + if (region_ != nullptr) + { + ::munmap(const_cast(region_), sizeof(GptpIpcRegion)); + region_ = nullptr; + } + if (shm_fd_ >= 0) + { + ::close(shm_fd_); + shm_fd_ = -1; + } +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/gptp_ipc_receiver.h b/score/libTSClient/gptp_ipc_receiver.h new file mode 100644 index 0000000..4d4dc49 --- /dev/null +++ b/score/libTSClient/gptp_ipc_receiver.h @@ -0,0 +1,63 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#ifndef SCORE_LIBTSCLIENT_GPTP_IPC_RECEIVER_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_RECEIVER_H + +#include "score/libTSClient/gptp_ipc_channel.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/** + * @brief Multi-reader receiver for the gPTP IPC channel. + * + * Opens an existing POSIX shared memory segment (read-only) and reads + * PtpTimeInfo using the seqlock protocol. Used by RealPTPEngine. + */ +class GptpIpcReceiver final +{ + public: + GptpIpcReceiver() = default; + ~GptpIpcReceiver(); + + GptpIpcReceiver(const GptpIpcReceiver&) = delete; + GptpIpcReceiver& operator=(const GptpIpcReceiver&) = delete; + + /// Open and map the shared memory segment (read-only). + /// @return true on success. + bool Init(const std::string& ipc_name = kGptpIpcName); + + /// Read a PtpTimeInfo snapshot using seqlock (up to 20 retries). + /// @return The data if consistent, or std::nullopt on contention failure. + std::optional Receive(); + + /// Unmap the shared memory segment. + void Close(); + + private: + const GptpIpcRegion* region_{nullptr}; + int shm_fd_{-1}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_RECEIVER_H diff --git a/score/libTSClient/gptp_ipc_test.cpp b/score/libTSClient/gptp_ipc_test.cpp new file mode 100644 index 0000000..fbaa0f4 --- /dev/null +++ b/score/libTSClient/gptp_ipc_test.cpp @@ -0,0 +1,352 @@ +/******************************************************************************** + * Copyright (c) 2026 Contributors to the Eclipse Foundation + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Apache License Version 2.0 which is available at + * https://www.apache.org/licenses/LICENSE-2.0 + * + * SPDX-License-Identifier: Apache-2.0 + ********************************************************************************/ +#include "score/libTSClient/gptp_ipc_channel.h" +#include "score/libTSClient/gptp_ipc_publisher.h" +#include "score/libTSClient/gptp_ipc_receiver.h" + +#include + +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// Generate a unique POSIX shm name per invocation (avoids cross-test pollution). +std::string UniqueShmName() +{ + static std::atomic counter{0}; + return "/gptp_ipc_ut_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1, std::memory_order_relaxed)); +} + +// RAII helper: creates shm manually (without GptpIpcPublisher) for edge-case +// testing; cleans up in destructor. +struct ManualShm +{ + std::string name; + void* ptr = MAP_FAILED; + std::size_t size = sizeof(GptpIpcRegion); + + explicit ManualShm(const std::string& n) : name{n} + { + const int fd = ::shm_open(name.c_str(), O_CREAT | O_RDWR, 0666); + if (fd < 0) + return; + if (::ftruncate(fd, static_cast(size)) != 0) + { + ::close(fd); + return; + } + ptr = ::mmap(nullptr, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + ::close(fd); + } + + ~ManualShm() + { + if (ptr != MAP_FAILED) + ::munmap(ptr, size); + ::shm_unlink(name.c_str()); + } + + bool Valid() const + { + return ptr != MAP_FAILED; + } + GptpIpcRegion* Region() + { + return static_cast(ptr); + } +}; + +} // namespace + +// ── GptpIpcPublisher ────────────────────────────────────────────────────────── + +class GptpIpcPublisherTest : public ::testing::Test +{ + protected: + void TearDown() override + { + pub_.Destroy(); + } + + GptpIpcPublisher pub_; +}; + +TEST_F(GptpIpcPublisherTest, Init_ValidName_ReturnsTrue) +{ + EXPECT_TRUE(pub_.Init(UniqueShmName())); +} + +TEST_F(GptpIpcPublisherTest, Publish_WithoutInit_DoesNotCrash) +{ + // region_ is nullptr; Publish() must return silently. + score::td::PtpTimeInfo info{}; + EXPECT_NO_THROW(pub_.Publish(info)); +} + +TEST_F(GptpIpcPublisherTest, Destroy_CalledTwice_DoesNotCrash) +{ + ASSERT_TRUE(pub_.Init(UniqueShmName())); + pub_.Destroy(); + EXPECT_NO_THROW(pub_.Destroy()); +} + +TEST_F(GptpIpcPublisherTest, Destroy_WithoutInit_DoesNotCrash) +{ + EXPECT_NO_THROW(pub_.Destroy()); +} + +// ── GptpIpcReceiver ─────────────────────────────────────────────────────────── + +class GptpIpcReceiverTest : public ::testing::Test +{ + protected: + void TearDown() override + { + rx_.Close(); + } + + GptpIpcReceiver rx_; +}; + +TEST_F(GptpIpcReceiverTest, Init_ShmNotExist_ReturnsFalse) +{ + EXPECT_FALSE(rx_.Init("/gptp_nonexistent_" + std::to_string(::getpid()))); +} + +TEST_F(GptpIpcReceiverTest, Close_WithoutInit_DoesNotCrash) +{ + EXPECT_NO_THROW(rx_.Close()); +} + +TEST_F(GptpIpcReceiverTest, Close_CalledTwice_DoesNotCrash) +{ + EXPECT_NO_THROW(rx_.Close()); + EXPECT_NO_THROW(rx_.Close()); +} + +TEST_F(GptpIpcReceiverTest, Receive_WithoutInit_ReturnsNullopt) +{ + EXPECT_FALSE(rx_.Receive().has_value()); +} + +// ── Publisher + Receiver roundtrip ──────────────────────────────────────────── + +class GptpIpcRoundtripTest : public ::testing::Test +{ + protected: + void SetUp() override + { + name_ = UniqueShmName(); + } + void TearDown() override + { + rx_.Close(); + pub_.Destroy(); + } + + std::string name_; + GptpIpcPublisher pub_; + GptpIpcReceiver rx_; +}; + +TEST_F(GptpIpcRoundtripTest, ReceiverInit_AfterPublisherInit_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + EXPECT_TRUE(rx_.Init(name_)); +} + +TEST_F(GptpIpcRoundtripTest, ReceiverReceive_BeforeAnyPublish_ReturnsDefaultData) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + // seq == seq_confirm == 0: both even and equal → seqlock considers readable. + auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->ptp_assumed_time, std::chrono::nanoseconds{0}); +} + +TEST_F(GptpIpcRoundtripTest, PublishReceive_BasicFields_RoundtripCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + score::td::PtpTimeInfo info{}; + info.ptp_assumed_time = std::chrono::nanoseconds{1'234'567'890LL}; + info.rate_deviation = 0.75; + info.status.is_synchronized = true; + info.status.is_correct = true; + + pub_.Publish(info); + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->ptp_assumed_time, info.ptp_assumed_time); + EXPECT_DOUBLE_EQ(result->rate_deviation, info.rate_deviation); + EXPECT_TRUE(result->status.is_synchronized); + EXPECT_TRUE(result->status.is_correct); + EXPECT_FALSE(result->status.is_timeout); + EXPECT_FALSE(result->status.is_time_jump_future); + EXPECT_FALSE(result->status.is_time_jump_past); +} + +TEST_F(GptpIpcRoundtripTest, PublishReceive_StatusFlags_RoundtripCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + score::td::PtpTimeInfo info{}; + info.status.is_timeout = true; + info.status.is_time_jump_future = true; + info.status.is_time_jump_past = false; + info.status.is_synchronized = false; + info.status.is_correct = false; + + pub_.Publish(info); + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_TRUE(result->status.is_timeout); + EXPECT_TRUE(result->status.is_time_jump_future); + EXPECT_FALSE(result->status.is_time_jump_past); + EXPECT_FALSE(result->status.is_synchronized); +} + +TEST_F(GptpIpcRoundtripTest, PublishReceive_SyncFupData_RoundtripCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + score::td::PtpTimeInfo info{}; + info.sync_fup_data.precise_origin_timestamp = 100'000'000'000ULL; + info.sync_fup_data.reference_global_timestamp = 100'000'001'000ULL; + info.sync_fup_data.reference_local_timestamp = 100'000'001'500ULL; + info.sync_fup_data.sync_ingress_timestamp = 100'000'001'500ULL; + info.sync_fup_data.correction_field = 42U; + info.sync_fup_data.sequence_id = 77; + info.sync_fup_data.pdelay = 3'000U; + info.sync_fup_data.port_number = 1; + info.sync_fup_data.clock_identity = 0xAABBCCDDEEFF0011ULL; + + pub_.Publish(info); + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->sync_fup_data.precise_origin_timestamp, 100'000'000'000ULL); + EXPECT_EQ(result->sync_fup_data.reference_global_timestamp, 100'000'001'000ULL); + EXPECT_EQ(result->sync_fup_data.sequence_id, 77); + EXPECT_EQ(result->sync_fup_data.pdelay, 3'000U); + EXPECT_EQ(result->sync_fup_data.clock_identity, 0xAABBCCDDEEFF0011ULL); +} + +TEST_F(GptpIpcRoundtripTest, PublishReceive_PDelayData_RoundtripCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + score::td::PtpTimeInfo info{}; + info.pdelay_data.request_origin_timestamp = 1'000'000'000ULL; + info.pdelay_data.request_receipt_timestamp = 1'000'001'000ULL; + info.pdelay_data.response_origin_timestamp = 1'000'001'000ULL; + info.pdelay_data.response_receipt_timestamp = 1'000'002'000ULL; + info.pdelay_data.pdelay = 1'000U; + info.pdelay_data.req_port_number = 1; + info.pdelay_data.resp_port_number = 2; + info.pdelay_data.req_clock_identity = 0x1122334455667788ULL; + + pub_.Publish(info); + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->pdelay_data.request_origin_timestamp, 1'000'000'000ULL); + EXPECT_EQ(result->pdelay_data.pdelay, 1'000U); + EXPECT_EQ(result->pdelay_data.req_port_number, 1); + EXPECT_EQ(result->pdelay_data.resp_port_number, 2); + EXPECT_EQ(result->pdelay_data.req_clock_identity, 0x1122334455667788ULL); +} + +TEST_F(GptpIpcRoundtripTest, MultiplePublish_LastValueIsVisible) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + for (int i = 1; i <= 5; ++i) + { + score::td::PtpTimeInfo info{}; + info.ptp_assumed_time = std::chrono::nanoseconds{static_cast(i) * 1'000'000'000LL}; + pub_.Publish(info); + } + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->ptp_assumed_time, std::chrono::nanoseconds{5'000'000'000LL}); +} + +// ── Edge cases via ManualShm ────────────────────────────────────────────────── + +TEST_F(GptpIpcRoundtripTest, ReceiverInit_WrongMagic_ReturnsFalse) +{ + ManualShm shm{name_}; + ASSERT_TRUE(shm.Valid()); + + // Placement-new initializes magic = kGptpIpcMagic; overwrite with bad value. + new (shm.Region()) GptpIpcRegion{}; + const std::uint32_t bad = 0xDEADBEEFU; + std::memcpy(shm.ptr, &bad, sizeof(bad)); + + EXPECT_FALSE(rx_.Init(name_)); +} + +TEST_F(GptpIpcRoundtripTest, Receive_PersistentOddSeq_ExhaustsRetriesAndReturnsNullopt) +{ + ManualShm shm{name_}; + ASSERT_TRUE(shm.Valid()); + + // seq=1 (odd = writer active), seq_confirm=0; seqlock never resolves. + auto* region = new (shm.Region()) GptpIpcRegion{}; + region->seq.store(1U, std::memory_order_relaxed); + region->seq_confirm.store(0U, std::memory_order_relaxed); + + ASSERT_TRUE(rx_.Init(name_)); + EXPECT_FALSE(rx_.Receive().has_value()); +} + +TEST_F(GptpIpcRoundtripTest, Receive_SeqConfirmMismatch_ExhaustsRetriesAndReturnsNullopt) +{ + ManualShm shm{name_}; + ASSERT_TRUE(shm.Valid()); + + // seq=4 (even, not writing) but seq_confirm=2 → mismatch: write still pending. + auto* region = new (shm.Region()) GptpIpcRegion{}; + region->seq.store(4U, std::memory_order_relaxed); + region->seq_confirm.store(2U, std::memory_order_relaxed); + + ASSERT_TRUE(rx_.Init(name_)); + EXPECT_FALSE(rx_.Receive().has_value()); +} + +} // namespace details +} // namespace ts +} // namespace score