diff --git a/docs/TimeSlave/_assets/gptp_engine/gptp_engine_class.puml b/docs/TimeSlave/_assets/gptp_engine/gptp_engine_class.puml new file mode 100644 index 0000000..f079879 --- /dev/null +++ b/docs/TimeSlave/_assets/gptp_engine/gptp_engine_class.puml @@ -0,0 +1,136 @@ +@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::details" { + + class GptpEngine #LightSalmon { + - opts_ : GptpEngineOptions + - rx_thread_ : std::thread + - pdelay_thread_ : std::thread + - socket_ : unique_ptr + - identity_ : unique_ptr + - codec_ : FrameCodec + - parser_ : GptpMessageParser + - sync_sm_ : SyncStateMachine + - pdelay_ : PeerDelayMeasurer + - phc_adjuster_ : PhcAdjuster + - snapshot_mutex_ : std::mutex + - pending_snapshot_ : GptpIpcData + - current_snapshot_ : GptpIpcData + + Initialize() : bool + + Deinitialize() : bool + + FinalizeSnapshot() : void + + ReadPTPSnapshot(data : GptpIpcData&) : bool + } + + interface IRawSocket #LightSkyBlue { + + Open(iface) : bool + + EnableHwTimestamping() : bool + + Recv(buf, timeout_ms) : RecvResult + + Send(buf, len) : 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 + + GetMac() : MacAddress + } + + class NetworkIdentity #LightSkyBlue { + Derives EUI-64 from MAC\n(inserts 0xFF 0xFE) + } + + class FrameCodec #Wheat { + + ParseEthernetHeader(buf) : EthernetHeader + + AddEthernetHeader(buf, dst_mac, src_mac) : void + } + + class GptpMessageParser #Wheat { + + Parse(payload, len, msg_out) : bool + } + + class SyncStateMachine #Wheat { + - timeout_ : atomic + + OnSync(msg) : void + + OnFollowUp(msg) : optional + + IsTimeout() : bool + + GetNeighborRateRatio() : double + } + + class PeerDelayMeasurer #Wheat { + - mutex_ : std::mutex + - result_ : PDelayResult + + SendRequest(socket) : void + + OnResponse(msg) : void + + OnResponseFollowUp(msg) : void + + GetResult() : PDelayResult + } + + class PhcAdjuster #Lavender { + - cfg_ : PhcConfig + - phc_fd_ : int + + IsEnabled() : bool + + AdjustOffset(offset_ns) : void + + AdjustFrequency(rate_ratio) : void + } + + struct PhcConfig #Lavender { + + enabled : bool = false + + device : string + + step_threshold_ns : int64_t = 100000000 + } + + IRawSocket <|.. LinuxSocket + IRawSocket <|.. QnxSocket + INetworkIdentity <|.. NetworkIdentity + PhcAdjuster *-- PhcConfig + + GptpEngine *-- IRawSocket + GptpEngine *-- INetworkIdentity + GptpEngine *-- FrameCodec + GptpEngine *-- GptpMessageParser + GptpEngine *-- SyncStateMachine + GptpEngine *-- PeerDelayMeasurer + GptpEngine *-- PhcAdjuster +} + +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 *-- ProbeManager + +@enduml diff --git a/docs/TimeSlave/_assets/gptp_engine/gptp_threading.puml b/docs/TimeSlave/_assets/gptp_engine/gptp_threading.puml new file mode 100644 index 0000000..79ee3b2 --- /dev/null +++ b/docs/TimeSlave/_assets/gptp_engine/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/libtsclient/ipc_channel.puml b/docs/TimeSlave/_assets/libtsclient/ipc_channel.puml new file mode 100644 index 0000000..ead16c4 --- /dev/null +++ b/docs/TimeSlave/_assets/libtsclient/ipc_channel.puml @@ -0,0 +1,70 @@ +@startuml +!theme plain + +title libTSClient Shared Memory IPC + +legend top left + |= Color |= Description | + | <#LightPink> | IPC components | + | <#LightCyan> | Shared memory region | + | <#LightSalmon> | TimeDaemon adapter | +endlegend + +package "TimeSlave Process" { + class GptpIpcPublisher #LightPink { + - region_ : GptpIpcRegion* + - shm_fd_ : int + + Init(name) : bool + + Publish(data : GptpIpcData) : void + + Destroy() : void + } +} + +package "Shared Memory" { + class GptpIpcRegion <> #LightCyan { + + magic : atomic = 0x47505450 + + seq : atomic + + data : GptpIpcData + + seq_confirm : atomic + -- + 64-byte aligned for\ncache line efficiency + } +} + +package "TimeDaemon Process" { + class GptpIpcReceiver #LightPink { + - region_ : const GptpIpcRegion* + - shm_fd_ : int + + Init(name) : bool + + Receive() : std::optional + + Close() : void + } + + class ShmPTPEngine #LightSalmon { + - receiver_ : GptpIpcReceiver + - ipc_name_ : string + + Initialize() : bool + + Deinitialize() : bool + + ReadPTPSnapshot(info : PtpTimeInfo&) : bool + } +} + +GptpIpcPublisher --> GptpIpcRegion : "shm_open(O_CREAT)\nmmap(PROT_WRITE)" +GptpIpcReceiver --> GptpIpcRegion : "shm_open(O_RDONLY)\nmmap(PROT_READ)" +ShmPTPEngine *-- GptpIpcReceiver +ShmPTPEngine ..> "PtpTimeInfo" : converts to + +note right of GptpIpcRegion + **Seqlock Protocol:** + Writer: seq++ (odd) → fence → memcpy → seq_confirm++, seq++ (even) + Reader: read seq1 (even) → memcpy → fence → read seq2, seq3 + retry if seq1 != seq2 or seq1 != seq3 + Retry up to 20 times on torn read +end note + +note bottom of ShmPTPEngine + Maps GptpIpcData fields to PtpTimeInfo. + Instantiated as GPTPShmMachine via CreateGPTPShmMachine(). +end note + +@enduml diff --git a/docs/TimeSlave/_assets/libtsclient/ipc_sequence.puml b/docs/TimeSlave/_assets/libtsclient/ipc_sequence.puml new file mode 100644 index 0000000..4139f95 --- /dev/null +++ b/docs/TimeSlave/_assets/libtsclient/ipc_sequence.puml @@ -0,0 +1,52 @@ +@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 = 0x47505450 ('GPTP') + +... + +RCV -> SHM : shm_open("/gptp_ptp_info", O_RDONLY) +RCV -> SHM : mmap(PROT_READ) +RCV -> SHM : verify magic == 0x47505450 + +== Publish (Writer Side) == + +PUB -> SHM : seq.fetch_add(1, relaxed) // seq becomes odd (write in progress) +PUB -> SHM : atomic_thread_fence(release) +PUB -> SHM : memcpy(&data, &src, sizeof(GptpIpcData)) +PUB -> SHM : seq_confirm.store(seq+1, release) // seq_confirm becomes even +PUB -> SHM : seq.store(seq+1, release) // seq becomes even (write done) + +== Receive (Reader Side) == + +loop up to 20 retries + RCV -> SHM : seq1 = seq.load(acquire) + alt seq1 is odd (write in progress) + RCV -> RCV : retry + else seq1 is even + RCV -> SHM : memcpy(&local, &data, sizeof(GptpIpcData)) + RCV -> SHM : atomic_thread_fence(acq_rel) + RCV -> SHM : seq2 = seq_confirm.load(acquire) + RCV -> SHM : seq3 = seq.load(acquire) + alt seq1 == seq2 && seq1 == seq3 + RCV --> RCV : return GptpIpcData (consistent) + else torn read (new write started) + RCV -> RCV : retry + end + end +end + +RCV --> RCV : return std::nullopt (exhausted retries) + +@enduml diff --git a/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_class.puml b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_class.puml new file mode 100644 index 0000000..1b5486c --- /dev/null +++ b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_class.puml @@ -0,0 +1,105 @@ +@startuml shm_ptp_engine_class_diagram +!theme plain + +title ShmPTPEngine: Class Diagram + +legend top left + |= Color |= Description | + | <#Beige> | Base classes / data types | + | <#Wheat> | PTPMachine / ShmPTPEngine | + | <#LightPink> | libTSClient IPC | +endlegend + +package "score::td" { + class "GPTPShmMachine" as real_machine #Wheat { + type alias for PTPMachine + -- + Constructed via CreateGPTPShmMachine() + } +} + +package "score::td (base classes)" { + abstract class "BaseMachine" as base_machine #Beige { + + GetName() : string + + Init() : bool + } + + abstract class "ProactiveMachine" as proactive_machine #Beige { + + Start() : void + + Stop() : void + } + + abstract class "PeriodicMachine" as periodic_machine #Beige { + # PeriodicTask() : void + } + + abstract class "Producer" as producer #Beige { + + SetPublishCallback(cb) : void + # Publish(data : T) : void + } + + class "PTPMachine" as ptp_machine #Wheat { + - engine_ : PTPEngine + + Init() : bool + + SetPublishCallback(cb) : void + # PeriodicTask() : void + } + + base_machine <|-- proactive_machine + proactive_machine <|-- periodic_machine + periodic_machine <|-- ptp_machine + producer <|.. ptp_machine +} + +package "score::td::details" { + class ShmPTPEngine #Wheat { + - ipc_name_ : string + - receiver_ : GptpIpcReceiver + - initialized_ : bool + + ShmPTPEngine(ipc_name : string) + + Initialize() : bool + + Deinitialize() : bool + + ReadPTPSnapshot(info : PtpTimeInfo&) : bool + } +} + +package "score::ts::details" { + class GptpIpcReceiver #LightPink { + + Init(name : string) : bool + + Receive() : optional + + Close() : void + } +} + +package "Data Types" { + class GptpIpcData #Beige { + + ptp_assumed_time : chrono::nanoseconds + + local_time : chrono::nanoseconds + + rate_deviation : double + + status : GptpIpcStatus + + sync_fup_data : GptpIpcSyncFupData + + pdelay_data : GptpIpcPDelayData + } + + class PtpTimeInfo #Beige { + + ptp_assumed_time + + local_time + + rate_deviation + + status + + sync_fup_data + + pdelay_data + } +} + +ptp_machine *-- ShmPTPEngine : PTPEngine = ShmPTPEngine +ShmPTPEngine *-- GptpIpcReceiver +ShmPTPEngine ..> GptpIpcData : reads +ShmPTPEngine ..> PtpTimeInfo : produces +real_machine --|> ptp_machine : alias + +note right of ShmPTPEngine + Maps GptpIpcData → PtpTimeInfo + on every ReadPTPSnapshot() call. +end note + +@enduml diff --git a/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_init_seq.puml b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_init_seq.puml new file mode 100644 index 0000000..1a3d2f6 --- /dev/null +++ b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_init_seq.puml @@ -0,0 +1,82 @@ +@startuml shm_ptp_engine_init_seq +!theme plain + +title ShmPTPEngine: Initialization Sequence + +hide footbox +autonumber "[00]" + +legend top left + |= Color |= Description | + | <#LightCyan> | TimeDaemon | + | <#Wheat> | GPTPShmMachine | + | <#LightPink> | libTSClient IPC | +endlegend + +participant "TimeBaseHandler" as tb #LightCyan +participant "GPTPShmMachine\n(PTPMachine)" as machine #Wheat +participant "ShmPTPEngine" as engine #Wheat +participant "GptpIpcReceiver" as receiver #LightPink +participant "MessageBroker" as broker #LightCyan + +== Construction == + +tb -> machine ** : CreateGPTPShmMachine("shm", "/gptp_ptp_info") +activate tb +activate machine +machine -> engine ** : ShmPTPEngine("/gptp_ptp_info") +engine -> receiver ** : GptpIpcReceiver() +machine --> tb +deactivate machine +deactivate tb + +== Initialization == + +tb -> machine : Init() +activate tb +activate machine +machine -> engine : Initialize() +activate engine +engine -> receiver : Init("/gptp_ptp_info") +activate receiver +note right of receiver + shm_open(O_RDONLY) + mmap(PROT_READ) + verify magic == 0x47505450 +end note +receiver --> engine : true / false +deactivate receiver +engine --> machine +deactivate engine +machine --> tb +deactivate machine +deactivate tb + +== Setup Producer == + +tb -> broker : subscribe machine to "raw_ptp_data" topic +activate tb +activate broker +broker -> machine : SetPublishCallback(broker::OnNewData) +activate machine +machine --> broker +deactivate machine +broker --> tb +deactivate broker +deactivate tb + +== Start Periodic Operation == + +tb -> machine : Start() +activate tb +activate machine +machine -> machine : start periodic thread +note right + Begin periodic IPC reads + from shared memory +end note +machine --> tb +deactivate machine +deactivate tb + +@enduml diff --git a/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_read_seq.puml b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_read_seq.puml new file mode 100644 index 0000000..0a6a914 --- /dev/null +++ b/docs/TimeSlave/_assets/shm_ptp_engine/shm_ptp_engine_read_seq.puml @@ -0,0 +1,73 @@ +@startuml shm_ptp_engine_read_seq +!theme plain + +title ShmPTPEngine: Periodic Read and Publish Workflow + +hide footbox +autonumber "[00]" + +legend top left + |= Color |= Description | + | <#Wheat> | GPTPShmMachine | + | <#LightPink> | libTSClient IPC | + | <#LightCyan> | Shared Memory | + | <#PaleTurquoise> | MessageBroker | + | <#LightBlue> | ControlFlowDivider | +endlegend + +participant "PTPMachine\n(PeriodicTask)" as machine #Wheat +participant "ShmPTPEngine" as engine #Wheat +participant "GptpIpcReceiver" as receiver #LightPink +participant "SharedMemory\n(GptpIpcRegion)" as shm #LightCyan +participant "MessageBroker" as broker #PaleTurquoise +participant "ControlFlowDivider" as cfd #LightBlue + +loop periodic (e.g., every 50 ms) + activate machine + machine -> machine : PeriodicTask() + + machine -> engine : ReadPTPSnapshot(info) + activate engine + engine -> receiver : Receive() + activate receiver + receiver -> shm : seqlock read (up to 20 retries) + activate shm + note right of shm + 1. read seq1 (acquire, must be even) + 2. memcpy GptpIpcData + 3. fence, read seq_confirm + seq + 4. verify seq1 == seq2 == seq3 + end note + shm --> receiver : GptpIpcData or contention + deactivate shm + receiver --> engine : optional + deactivate receiver + + alt data available + engine -> engine : map GptpIpcData → PtpTimeInfo + note right + status, ptp_assumed_time, + local_time, rate_deviation, + sync_fup_data, pdelay_data + end note + engine --> machine : true (PtpTimeInfo filled) + else no data (nullopt) + engine --> machine : false + end + deactivate engine + + alt ReadPTPSnapshot returned true + machine -> machine : Publish(PtpTimeInfo) + machine -> broker : publish_callback_(PtpTimeInfo) + activate broker + broker -> cfd : subscription.callback_(PtpTimeInfo) + activate cfd + cfd --> broker + deactivate cfd + broker --> machine + deactivate broker + end + deactivate machine +end + +@enduml diff --git a/docs/TimeSlave/_assets/timeslave_class.puml b/docs/TimeSlave/_assets/timeslave_class.puml new file mode 100644 index 0000000..588766b --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_class.puml @@ -0,0 +1,128 @@ +@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 { + - opts_ : GptpEngineOptions + - engine_ : unique_ptr + - publisher_ : GptpIpcPublisher + + Initialize(ctx) : int32_t + + Run(stop_token) : int32_t + } + + class GptpEngine #LightSalmon { + - opts_ : GptpEngineOptions + - rx_thread_ : std::thread + - pdelay_thread_ : std::thread + - sync_sm_ : SyncStateMachine + - pdelay_ : PeerDelayMeasurer + - phc_adjuster_ : PhcAdjuster + - socket_ : unique_ptr + - identity_ : unique_ptr + - codec_ : FrameCodec + - parser_ : GptpMessageParser + - snapshot_mutex_ : std::mutex + - pending_snapshot_ : GptpIpcData + - current_snapshot_ : GptpIpcData + + Initialize() : bool + + Deinitialize() : bool + + FinalizeSnapshot() : void + + ReadPTPSnapshot(data : GptpIpcData&) : bool + } + + struct GptpEngineOptions #Beige { + + iface_name : string = "eth0" + + pdelay_interval_ms : int = 1000 + + pdelay_warmup_ms : int = 2000 + + sync_timeout_ms : int = 3300 + + jump_future_threshold_ns : int64_t = 500000000 + } + + TimeSlave *-- GptpEngine +} + +package "score::ts::details" { + class FrameCodec #Wheat { + + ParseEthernetHeader(frame, frame_len, ptp_offset) : bool + + AddEthernetHeader(buf, buf_len, src_mac, buf_capacity) : bool + } + + class GptpMessageParser #Wheat { + + Parse(payload, len, msg_out) : bool + } + + class SyncStateMachine #Wheat { + - timeout_ : atomic + + OnSync(msg) : void + + OnFollowUp(msg) : 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 : GptpIpcSyncFupData + + time_jump_forward : bool + + time_jump_backward : bool + } + + struct PDelayResult #Beige { + + pdelay_data : GptpIpcPDelayData + + valid : bool + } + + class GptpIpcPublisher #LightPink { + - region_ : GptpIpcRegion* + - shm_fd_ : int + + Init(name) : bool + + Publish(data : GptpIpcData) : void + + Destroy() : void + } + + class PhcAdjuster #Lavender { + - cfg_ : PhcConfig + - phc_fd_ : int + + IsEnabled() : bool + + AdjustOffset(offset_ns) : void + + AdjustFrequency(rate_ratio) : void + } + + struct PhcConfig #Beige { + + enabled : bool = false + + device : string + + step_threshold_ns : int64_t = 100000000 + } +} + +GptpEngine *-- FrameCodec +GptpEngine *-- GptpMessageParser +GptpEngine *-- SyncStateMachine +GptpEngine *-- PeerDelayMeasurer +GptpEngine *-- PhcAdjuster +PhcAdjuster *-- PhcConfig +TimeSlave *-- "1" GptpIpcPublisher + +@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..3981db4 --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_data_flow.puml @@ -0,0 +1,64 @@ +@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 +participant "TimeSlave\n(main thread)" as TS #LightCyan + +== 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 --> GE : SyncResult{master_ns, offset_ns,\ntime_jump_flags} +GE -> GE : update pending_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 == + +TS -> GE : FinalizeSnapshot() +GE -> GE : check timeout, commit\npending_snapshot_ → current_snapshot_ +TS -> GE : ReadPTPSnapshot(data) +GE --> TS : GptpIpcData +TS -> PUB : Publish(GptpIpcData) +PUB -> SHM : seqlock write\n(seq odd → fence → memcpy → seq_confirm, seq even) + +@enduml diff --git a/docs/TimeSlave/_assets/timeslave_deployment.puml b/docs/TimeSlave/_assets/timeslave_deployment.puml new file mode 100644 index 0000000..2e922ca --- /dev/null +++ b/docs/TimeSlave/_assets/timeslave_deployment.puml @@ -0,0 +1,60 @@ +@startuml +!theme plain +skinparam nodesep 40 +skinparam ranksep 50 +skinparam componentStyle rectangle + +title TimeSlave Deployment View + +node "Time Master" as TM { + component [PTP Grandmaster\nClock] as GMC +} + +node "ECU" as ECU { + + package "TimeSlave Process" as TSP { + component [GptpEngine\n«RxThread + PdelayThread»] as GE #LightSalmon + component [PhcAdjuster] as PHC #Lavender + component [GptpIpcPublisher] as PUB #LightPink + component [ProbeManager + Recorder] as INST #Beige + } + + component [«hw clock»\nPHC Device] as PHCDEV + + database "Shared Memory\n/gptp_ptp_info" as SHM #LightCyan + + package "TimeDaemon Process" as TDP { + component [ShmPTPEngine] as SHME #LightSalmon + component [GptpIpcReceiver] as RCV #LightPink + } +} + +cloud "Network\n(Ethernet)" as NET + +' Top-level flow +GMC -down[#purple]-> GE : PTP sync messages\n(EtherType 0x88F7) +GE -down[#blue]-> NET : PDelayReq / PDelayResp +NET -up[#blue]-> GE + +' Hardware clock adjustment +GE -down-> PHC : offset / rate ratio +PHC -down[#orange]-> PHCDEV : clock_adjtime /\nEMAC ioctl + +' Shared memory IPC +GE -down-> PUB +PUB -down[#green]-> SHM : seqlock write\n(GptpIpcData) +RCV -up[#green]-> SHM : seqlock read\n(GptpIpcData) + +' TimeDaemon internal +SHME -down-* RCV + +' Instrumentation +GE .down.> INST : probe events + +note right of TDP + ShmPTPEngine wraps GptpIpcReceiver. + Converts GptpIpcData → PtpTimeInfo. + Instantiated as GPTPShmMachine. +end note + +@enduml diff --git a/docs/TimeSlave/index.rst b/docs/TimeSlave/index.rst new file mode 100644 index 0000000..29c56a4 --- /dev/null +++ b/docs/TimeSlave/index.rst @@ -0,0 +1,828 @@ +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 ``GptpIpcData`` 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>`_ +9. `ShmPTPEngine <#shmptpengine-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::FinalizeSnapshot()`` to check timeout and commit the pending snapshot +2. call ``GptpEngine::ReadPTPSnapshot(data)`` to copy the latest ``GptpIpcData`` into a local variable +3. publish to shared memory via ``GptpIpcPublisher::Publish(data)`` + +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 TimeDaemon-internal aggregated snapshot. It is **not** the shared memory type; it is produced by ``ShmPTPEngine::ReadPTPSnapshot()`` by field-mapping from ``GptpIpcData`` into the format expected by the TimeDaemon pipeline. + +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 create the ``HighPrecisionLocalSteadyClock`` for the engine +- The ``TimeSlave Application`` shall implement the ``Run()`` method to enter a periodic publish loop (50 ms interval) and monitor the ``stop_token`` for graceful shutdown +- On each loop iteration, ``TimeSlave Application`` shall call ``GptpEngine::FinalizeSnapshot()``, then ``GptpEngine::ReadPTPSnapshot(data)``, and publish the resulting ``GptpIpcData`` via ``GptpIpcPublisher::Publish(data)`` +- The ``TimeSlave Application`` shall call ``GptpEngine::Deinitialize()`` and ``GptpIpcPublisher::Destroy()`` after the ``stop_token`` is set + +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 ``FinalizeSnapshot()`` method that checks for sync timeout, applies status flags, and commits the pending snapshot to the current snapshot; this must be called before ``ReadPTPSnapshot()`` +- The ``GptpEngine`` shall provide a ``ReadPTPSnapshot(GptpIpcData&)`` method that copies the latest committed snapshot into the caller's buffer and returns false only if the engine is not initialized +- The ``GptpEngine`` shall support configurable parameters via ``GptpEngineOptions`` (interface name, PDelay interval, PDelay warmup, sync timeout, time-jump threshold, 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/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_engine/gptp_threading.puml + :alt: Threading Model + +.. raw:: html + +
+ +Concurrency aspects +''''''''''''''''''' + +The ``GptpEngine`` uses the following synchronization mechanisms: + +- A ``std::mutex`` protects the ``pending_snapshot_`` and ``current_snapshot_`` fields (both ``GptpIpcData``): the RxThread writes ``pending_snapshot_``; the main thread calls ``FinalizeSnapshot()`` (commits pending to current) and ``ReadPTPSnapshot()`` (reads current) +- 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 + +Hardware timestamping fallback +''''''''''''''''''''''''''''''' + +During ``Initialize()``, ``GptpEngine`` calls ``IRawSocket::EnableHwTimestamping()`` to request NIC-level receive timestamps (``SO_TIMESTAMPING`` on Linux). If the NIC does not support hardware timestamping, the call returns ``false`` and a warning is logged: + +.. code-block:: none + + GptpEngine: HW timestamping not available on , falling back to SW timestamps + +The engine continues to run normally. The difference between the two modes: + +.. list-table:: + :header-rows: 1 + :widths: 30 35 35 + + * - Field + - HW timestamping available + - SW timestamping fallback + * - ``recvHardwareTS`` (Sync receive time) + - NIC hardware timestamp (nanosecond precision, captured at wire level) + - Software timestamp (captured at socket receive, higher jitter) + * - ``sync_fup_data.reference_local_timestamp`` + - Derived from NIC hardware timestamp + - Derived from software timestamp + * - ``GptpIpcData.local_time`` + - Always ``CLOCK_MONOTONIC`` (unaffected) + - Always ``CLOCK_MONOTONIC`` (unaffected) + * - Clock offset accuracy + - High (sub-microsecond typical) + - Reduced (jitter depends on OS scheduling latency) + +The fallback does not affect protocol correctness — Sync/FollowUp correlation and peer delay measurement continue to work — but the computed clock offset will be less accurate due to higher receive timestamp jitter. + +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. + +Timestamp definitions +''''''''''''''''''''' + +.. list-table:: Peer Delay Timestamps (IEEE 802.1AS) + :header-rows: 1 + :widths: 10 20 30 40 + + * - Symbol + - Message + - Captured by + - Meaning + * - ``t1`` + - PDelayReq (TX) + - Slave (PdelayThread) + - HW transmit timestamp of the PDelayReq frame leaving the slave NIC + * - ``t2`` + - PDelayResp (RX) + - Master → carried in PDelayResp body + - HW receive timestamp of the PDelayReq frame arriving at the master NIC + * - ``t3c`` + - PDelayRespFollowUp + - Master → carried in PDelayRespFollowUp body + - HW transmit timestamp of the PDelayResp frame leaving the master NIC ("corrected" because it includes the master's turnaround correction) + * - ``t4`` + - PDelayResp (RX) + - Slave (RxThread) + - HW receive timestamp of the PDelayResp frame arriving at the slave NIC + +The peer delay formula is: ``path_delay = ((t2 - t1) + (t4 - t3c)) / 2`` + +- ``(t2 - t1)`` = propagation time from slave → master +- ``(t4 - t3c)`` = propagation time from master → slave +- The average of the two gives the one-way link delay + +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 discard PDelayResp and PDelayRespFollowUp messages whose sequence ID does not match the most recently transmitted PDelayReq +- The ``PeerDelayMeasurer`` shall suppress the path-delay result when more than one PDelayResp is received for a single PDelayReq (detection of non-time-aware bridges per IEEE 802.1AS) +- 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) + +Fallback behavior when PHC is unavailable +'''''''''''''''''''''''''''''''''''''''''' + +The ``PhcAdjuster`` degrades gracefully in two scenarios: + +1. **PHC disabled** (``PhcConfig.enabled = false``, the default): ``AdjustOffset()`` and ``AdjustFrequency()`` are no-ops. The gPTP protocol pipeline (Sync/FollowUp reception, peer-delay measurement, ``GptpIpcData`` publishing) is completely unaffected. The hardware clock is not touched. + +2. **PHC enabled but device inaccessible** (e.g., ``/dev/ptp0`` does not exist on Linux, or the EMAC interface name is wrong on QNX): + + - **Linux**: the constructor calls ``open(device, O_RDWR)``; on failure ``phc_fd_`` stays at ``-1``. Both ``AdjustOffset()`` and ``AdjustFrequency()`` guard against ``phc_fd_ < 0`` and return immediately — a true silent skip with no system call. + + - **QNX**: ``qnx_phc_open()`` always returns ``0`` and never fails — it only stores the device name in a thread-local context. There is no ``phc_fd_ < 0`` guard. The adjustment methods always call ``qnx_phc_adjtime_step()`` / ``qnx_phc_adjfreq_ppb()``, which internally create a UDP socket and issue ``SIOCGDRVSPEC`` / ``SIOCSDRVSPEC`` ioctls. If the socket or ioctl fails (e.g., wrong interface name, unsupported hardware), the function returns ``-1``, but the caller discards it with a ``(void)`` cast. There is no explicit skip — the call is always attempted and errors are silently absorbed. + +In both scenarios TimeSlave continues to track the master clock and publish accurate ``GptpIpcData`` snapshots (including offset and status flags) to shared memory. The downstream TimeDaemon and any applications consuming time are unaffected — only the NIC hardware clock itself will drift relative to PTP time. + +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 (``0x47505450`` = 'GPTP') for validation, an atomic seqlock counter (``seq``), a confirmation counter (``seq_confirm``), and a ``GptpIpcData`` 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 (in ``score::ts::details``) that creates and manages the POSIX shared memory segment and writes ``GptpIpcData`` using the seqlock protocol +- The ``libTSClient`` shall provide a ``GptpIpcReceiver`` component (in ``score::ts::details``) that opens the shared memory segment read-only and reads ``GptpIpcData`` 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/libtsclient/ipc_channel.puml + :alt: Class Diagram + +.. raw:: html + +
+ +Publish new data +'''''''''''''''' + +When ``TimeSlave Application`` has a new ``GptpIpcData`` snapshot, it publishes to the shared memory via the seqlock protocol: + +1. Increment ``seq`` (becomes odd — signals write in progress); a release fence is applied +2. ``memcpy`` the ``GptpIpcData`` +3. Store ``seq_confirm = seq + 1`` and increment ``seq`` (both become 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 ``seq1`` with acquire ordering (must be even, otherwise retry — write in progress) +2. ``memcpy`` the ``GptpIpcData`` +3. Apply an acquire-release fence; read ``seq_confirm`` as ``seq2`` and re-read ``seq`` as ``seq3`` +4. If ``seq1 == seq2 == seq3``, the read is consistent; otherwise retry — torn read detected +5. Return ``std::optional`` (empty if all 20 retries exhausted) + +The seqlock protocol workflow is presented in the following sequence diagram: + +.. raw:: html + +
+ +.. uml:: _assets/libtsclient/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 37 38 + + * - Component + - Linux + - QNX + * - Raw Socket + - ``AF_PACKET`` + ``SO_TIMESTAMPING``; HW RX timestamp via ``recvmsg`` ``SCM_TIMESTAMPING`` + - BPF (``/dev/bpf``); HW RX timestamp via ``bpf_xhdr.bh_tstamp`` (``BIOCSTSTAMP BPF_T_BINTIME|BPF_T_PTP``); TX PHC timestamp via dedicated TX loopback fd (``BIOCSSEESENT``), filtered to Pdelay_Req frames only (BPF message-type 0x02); single static context (not thread-local) + * - Network Identity + - ``ioctl(SIOCGIFHWADDR)`` → EUI-48 → EUI-64 + - ``getifaddrs()`` + ``AF_LINK`` / ``sockaddr_dl`` (``LLADDR``) → EUI-48/64 + * - PHC Adjuster + - ``clock_adjtime()`` (``SYS_clock_adjtime`` syscall); step via ``ADJ_SETOFFSET|ADJ_NANO``; slew via ``ADJ_FREQUENCY`` (scaled-ppm) + - ``SIOCGDRVSPEC`` / ``SIOCSDRVSPEC`` on UDP socket; step via ``PTP_GET_TIME`` (0x102) + ``PTP_SET_TIME`` (0x103); slew via ``EMAC_PTP_ADJ_FREQ_PPM`` (0x200) in ppm + * - HighPrecisionLocalSteadyClock + - ``CLOCK_MONOTONIC`` via ``clock_gettime()`` + - QNX ``ClockCycles()`` CPU instruction (reads hardware performance counter directly, equivalent to ``RDTSC`` on x86 / ``CNTVCT`` on ARM64), converted to nanoseconds via cycles-per-second calibration. Used instead of ``clock_gettime()`` because QNX ``CLOCK_MONOTONIC`` resolution is limited to microsecond level, whereas ``ClockCycles()`` provides nanosecond-level precision with no syscall overhead. + +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 traces probe events at key processing points in the gPTP engine. It emits a ``LogDebug`` entry on every ``Trace()`` call and forwards the event to a linked ``Recorder`` (if set and enabled). Probing is controlled at runtime via ``SetEnabled()``; the ``GPTP_PROBE()`` macro provides zero overhead when disabled. + +Supported probe points (``ProbePoint`` enum): + +.. list-table:: ProbePoint Events + :header-rows: 1 + :widths: 10 30 60 + + * - Value + - Enumerator + - Trigger + * - 0 + - ``kRxPacketReceived`` + - Raw Ethernet frame received from socket (RxThread) + * - 1 + - ``kSyncFrameParsed`` + - Sync message successfully decoded by ``GptpMessageParser`` + * - 2 + - ``kFollowUpProcessed`` + - FollowUp received; ``SyncStateMachine::OnFollowUp()`` returned a ``SyncResult`` + * - 3 + - ``kOffsetComputed`` + - Final clock offset value available after Sync/FollowUp correlation + * - 4 + - ``kPdelayReqSent`` + - PDelayReq frame transmitted by ``PeerDelayMeasurer`` + * - 5 + - ``kPdelayCompleted`` + - Peer delay computation finished (all four timestamps collected) + * - 6 + - ``kPhcAdjusted`` + - ``PhcAdjuster`` applied a step or frequency correction + +When a probe event is forwarded to the ``Recorder``, it is written with ``RecordEvent::kProbe`` and the ``ProbePoint`` value stored in the ``status_flags`` field of the CSV row. + +Recorder +^^^^^^^^^ + +Thread-safe CSV file writer. When enabled, appends one row per event to the configured file. The file is opened in append mode (``ios::app``); a CSV header is written only if the file is newly created (size == 0). + +**Status model:** the ``Recorder`` starts in the state determined by ``Config.enabled``. If a write error occurs (``file_.good()`` fails after a flush), ``enabled_`` is atomically set to ``false`` and all subsequent ``Record()`` calls become no-ops. The file is never re-opened after an error. + +Configuration (``Recorder::Config``): + +.. list-table:: Recorder Configuration + :header-rows: 1 + :widths: 30 15 55 + + * - Parameter + - Type + - Description + * - ``enabled`` + - bool + - Enable or disable recording; default: ``false`` + * - ``file_path`` + - string + - Output CSV file path; default: ``/var/log/gptp_record.csv`` + * - ``offset_threshold_ns`` + - int64_t + - Reserved for ``kOffsetThreshold`` events (threshold above which offsets are logged); default: ``1 000 000`` (1 ms) + * - ``flush_interval`` + - uint32_t + - Number of rows between explicit ``file_.flush()`` calls; default: ``8`` + +CSV output format:: + + mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags + +Supported ``RecordEvent`` values written to the ``event`` column: + +.. list-table:: RecordEvent Values + :header-rows: 1 + :widths: 10 30 60 + + * - Value + - Enumerator + - Description + * - 0 + - ``kSyncReceived`` + - A Sync message was received and processed + * - 1 + - ``kPdelayCompleted`` + - A full peer delay measurement cycle completed + * - 2 + - ``kClockJump`` + - A forward or backward time jump was detected + * - 3 + - ``kOffsetThreshold`` + - Clock offset exceeded ``offset_threshold_ns`` + * - 4 + - ``kProbe`` + - Forwarded from ``ProbeManager::Trace()``; ``status_flags`` column carries the ``ProbePoint`` value + +Logging configuration +~~~~~~~~~~~~~~~~~~~~~ + +The TimeSlave and its TimeDaemon-side adapter use the following logging contexts: + +.. list-table:: Logging Contexts + :header-rows: 1 + :widths: 35 20 45 + + * - Component + - Context ID + - Comments + * - TimeSlave Application + - TSAP + - **T**\ ime\ **S**\ lave **App**\ lication lifecycle (Initialize / Run) + * - gPTP Engine (RxThread / PdelayThread) + - GTPS + - **GPTP** **SLAVE** engine — low-level protocol processing + * - ShmPTPEngine (TimeDaemon side) + - GPTP + - TimeDaemon **GPTP** machine adapter (Initialize / ReadPTPSnapshot) + +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 + * - ``iface_name`` + - string + - Network interface for gPTP frames (e.g., ``emac0``); default: ``"emac0"`` + * - ``pdelay_interval_ms`` + - int + - Interval between PDelayReq transmissions (ms); default: ``1000`` + * - ``pdelay_warmup_ms`` + - int + - Delay before the first PDelayReq is sent (ms); default: ``2000`` + * - ``sync_timeout_ms`` + - int + - Timeout for Sync message reception before declaring timeout state (ms); default: ``3300`` + * - ``jump_future_threshold_ns`` + - int64_t + - Threshold above which a positive clock offset is flagged as a forward time jump (ns); default: ``500 000 000`` + * - ``phc_config`` + - PhcConfig + - PHC hardware clock adjustment settings (see ``PhcConfig`` table below); disabled by default + +The ``PhcConfig`` struct (embedded in ``GptpEngineOptions``) contains: + +.. list-table:: PhcAdjuster Configuration + :header-rows: 1 + :widths: 30 15 55 + + * - Parameter + - Type + - Description + * - ``enabled`` + - bool + - Enable or disable PHC adjustment; default: ``false`` + * - ``device`` + - string + - PHC device identifier: ``/dev/ptp0`` on Linux, ``emac0`` on QNX + * - ``step_threshold_ns`` + - int64_t + - Offset threshold above which a step correction is applied instead of frequency slew (ns); default: ``100 000 000`` + +Scalability +^^^^^^^^^^^ + +The TimeSlave architecture supports the following extensibility points: + +Platform extensibility +'''''''''''''''''''''' + +1. New target platforms can be supported by implementing the ``IRawSocket`` and ``INetworkIdentity`` interfaces under a new ``platform//`` directory and selecting the implementation via ``Bazel select()`` +2. The ``PhcAdjuster`` platform implementations (``clock_adjtime`` on Linux, EMAC ioctls on QNX) can be extended for additional hardware without changing protocol logic + +Protocol extensibility +'''''''''''''''''''''' + +1. The ``GptpEngine`` accepts injected ``IRawSocket`` and ``INetworkIdentity`` dependencies, making it straightforward to test or replace individual platform abstractions +2. The shared memory IPC channel name is configurable (``GptpIpcPublisher::Init(name)`` / ``GptpIpcReceiver::Init(name)``), allowing multiple gPTP instances per ECU if needed + +TimeDaemon integration extensibility +'''''''''''''''''''''''''''''''''''''' + +1. The ``ShmPTPEngine`` implements the same ``PTPEngine`` concept as other ``PTPMachine`` backends, making it transparently exchangeable with any other engine implementation +2. Alternative IPC mechanisms (e.g., socket-based) can be introduced by implementing a new engine class without modifying the ``PTPMachine`` template or downstream components + +ShmPTPEngine SW component +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The ``ShmPTPEngine`` component (in ``score::td::details``) is the TimeDaemon-side adapter that reads ``GptpIpcData`` from the shared memory channel written by TimeSlave and converts it into the ``PtpTimeInfo`` structure expected by the TimeDaemon pipeline. + +It is instantiated as ``GPTPShmMachine`` — a type alias for ``PTPMachine`` — which connects ``ShmPTPEngine`` to the TimeDaemon's internal ``MessageBroker``. + +Component requirements +'''''''''''''''''''''' + +The ``ShmPTPEngine`` has the following requirements: + +- The ``ShmPTPEngine`` shall call ``GptpIpcReceiver::Init(ipc_name)`` during ``Initialize()`` to open the shared memory channel +- The ``ShmPTPEngine`` shall call ``GptpIpcReceiver::Receive()`` in ``ReadPTPSnapshot()`` to fetch the latest ``GptpIpcData`` +- The ``ShmPTPEngine`` shall map all fields of ``GptpIpcData`` to the corresponding fields of ``PtpTimeInfo`` (status flags, Sync/FollowUp data, peer-delay data, time references) +- The ``ShmPTPEngine`` shall call ``GptpIpcReceiver::Close()`` during ``Deinitialize()`` +- The ``ShmPTPEngine`` shall be instantiatable with a configurable IPC channel name (default: ``/gptp_ptp_info``) + +Class view +'''''''''' + +The Class Diagram is presented below: + +.. raw:: html + +
+ +.. uml:: _assets/shm_ptp_engine/shm_ptp_engine_class.puml + :alt: Class Diagram + +.. raw:: html + +
+ +Component initialization +'''''''''''''''''''''''' + +During initialization the ``ShmPTPEngine`` shall open the shared memory channel to be able to read from it. + +The initialization workflow is represented in the following sequence diagram: + +.. raw:: html + +
+ +.. uml:: _assets/shm_ptp_engine/shm_ptp_engine_init_seq.puml + :alt: Initialization workflow + +.. raw:: html + +
+ +Read PTP snapshot +''''''''''''''''' + +After ``ShmPTPEngine`` reads the latest ``GptpIpcData`` from shared memory, it maps it to ``PtpTimeInfo`` and publishes via the ``MessageBroker``. + +The periodic read and publish workflow is described below: + +.. raw:: html + +
+ +.. uml:: _assets/shm_ptp_engine/shm_ptp_engine_read_seq.puml + :alt: Periodic read and publish workflow + +.. raw:: html + +
+ +Data mapping +'''''''''''' + +``ShmPTPEngine::ReadPTPSnapshot()`` performs a field-by-field mapping from ``GptpIpcData`` to ``PtpTimeInfo``: + +.. list-table:: GptpIpcData → PtpTimeInfo Mapping + :header-rows: 1 + :widths: 50 50 + + * - ``GptpIpcData`` field + - ``PtpTimeInfo`` field + * - ``ptp_assumed_time`` + - ``ptp_assumed_time`` + * - ``local_time`` + - ``local_time`` (wrapped in ``ReferenceClock::time_point``) + * - ``rate_deviation`` + - ``rate_deviation`` + * - ``status.is_synchronized`` + - ``status.is_synchronized`` + * - ``status.is_timeout`` + - ``status.is_timeout`` + * - ``status.is_time_jump_future`` + - ``status.is_time_jump_future`` + * - ``status.is_time_jump_past`` + - ``status.is_time_jump_past`` + * - ``status.is_correct`` + - ``status.is_correct`` + * - ``sync_fup_data.*`` (9 fields) + - ``sync_fup_data.*`` (direct copy) + * - ``pdelay_data.*`` (12 fields) + - ``pdelay_data.*`` (direct copy) + +Factory +''''''' + +``CreateGPTPShmMachine(name, ipc_name)`` is a convenience factory function in ``score::td`` that creates a configured ``GPTPShmMachine`` (``shared_ptr``) backed by ``ShmPTPEngine``: + +.. code-block:: cpp + + auto machine = CreateGPTPShmMachine("shm", "/gptp_ptp_info"); + +Using in test environment +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Using in ITF +^^^^^^^^^^^^ + +Normal behavior is expected. TimeSlave runs as a standalone process, communicates over real Ethernet, and writes to ``/gptp_ptp_info`` shared memory as in production. + +Using in Component Tests on the host +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Overview +'''''''' + +The ``TimeSlave`` and its constituent components can be tested on an x86 Linux host without PTP hardware or a real network. The key platform-dependent abstractions all have test-injectable counterparts: + +.. list-table:: Testable Abstractions + :header-rows: 1 + :widths: 30 35 35 + + * - Abstraction + - Production implementation + - Test replacement + * - ``IRawSocket`` + - ``RawSocket`` (AF_PACKET) + - ``FakeSocket`` (push-based frame queue) + * - ``INetworkIdentity`` + - ``NetworkIdentity`` (ioctl) + - ``FakeIdentity`` (fixed clock identity) + * - ``HighPrecisionLocalSteadyClock`` + - Platform clock (Linux / QNX) + - ``FakeClock`` (returns fixed timestamp) + +The ``GptpEngine`` provides a dedicated test constructor that accepts injected implementations: + +.. code-block:: cpp + + GptpEngine engine(opts, + std::make_unique(), + std::make_unique(), + std::make_unique()); + +This allows complete white-box testing of the Sync/FollowUp correlation, peer-delay measurement, timeout detection, and time-jump flagging logic by pushing crafted PTP frames directly into the ``FakeSocket`` queue. + +The ``GptpIpcPublisher`` and ``GptpIpcReceiver`` rely on POSIX shared memory (``shm_open``), which works on any Linux host, so ``ShmPTPEngine`` component tests can run end-to-end using real IPC without modification. diff --git a/docs/index.rst b/docs/index.rst index 14bf088..03e6a29 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -43,6 +43,7 @@ For a detailed concept and architectural design, please refer to the :doc:`TimeD :caption: Contents: time/index + TimeSlave/index Project Layout -------------- diff --git a/score/TimeDaemon/code/ptp_machine/BUILD b/score/TimeDaemon/code/ptp_machine/BUILD index d596cfc..6111572 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/shm:unit_test_suite", "//score/TimeDaemon/code/ptp_machine/stub:unit_test_suite", ], visibility = ["//score/TimeDaemon:__subpackages__"], diff --git a/score/TimeDaemon/code/ptp_machine/shm/BUILD b/score/TimeDaemon/code/ptp_machine/shm/BUILD new file mode 100644 index 0000000..96fbc14 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/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_shm_machine", + srcs = [ + "factory.cpp", + ], + hdrs = [ + "factory.h", + "gptp_shm_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/shm/details:shm_ptp_engine", + "//score/libTSClient:gptp_ipc", + ], +) + +cc_test( + name = "gptp_shm_machine_test", + srcs = ["gptp_shm_machine_test.cpp"], + tags = ["unit"], + deps = [ + ":gptp_shm_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_shm_machine_test"], + test_suites_from_sub_packages = [ + "//score/TimeDaemon/code/ptp_machine/shm/details:unit_test_suite", + ], + visibility = ["//score/TimeDaemon:__subpackages__"], +) diff --git a/score/TimeDaemon/code/ptp_machine/shm/details/BUILD b/score/TimeDaemon/code/ptp_machine/shm/details/BUILD new file mode 100644 index 0000000..86750eb --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/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 = "shm_ptp_engine", + srcs = [ + "shm_ptp_engine.cpp", + ], + hdrs = [ + "shm_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 = "shm_ptp_engine_test", + srcs = ["shm_ptp_engine_test.cpp"], + tags = ["unit"], + deps = [ + ":shm_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 = [":shm_ptp_engine_test"], + test_suites_from_sub_packages = [], + visibility = ["//score/TimeDaemon:__subpackages__"], +) diff --git a/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.cpp b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.cpp new file mode 100644 index 0000000..e202c15 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.cpp @@ -0,0 +1,99 @@ +/******************************************************************************** + * 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/shm/details/shm_ptp_engine.h" + +#include "score/TimeDaemon/code/common/logging_contexts.h" +#include "score/libTSClient/gptp_ipc_data.h" +#include "score/mw/log/logging.h" + +namespace score +{ +namespace td +{ +namespace details +{ + +ShmPTPEngine::ShmPTPEngine(std::string ipc_name) noexcept : ipc_name_{std::move(ipc_name)} {} + +bool ShmPTPEngine::Initialize() +{ + if (initialized_) + return true; + + initialized_ = receiver_.Init(ipc_name_); + if (initialized_) + { + score::mw::log::LogInfo(kGPtpMachineContext) << "ShmPTPEngine: connected to IPC channel " << ipc_name_; + } + else + { + score::mw::log::LogError(kGPtpMachineContext) << "ShmPTPEngine: failed to open IPC channel " << ipc_name_; + } + return initialized_; +} + +bool ShmPTPEngine::Deinitialize() +{ + if (initialized_) + { + receiver_.Close(); + initialized_ = false; + } + return true; +} + +bool ShmPTPEngine::ReadPTPSnapshot(PtpTimeInfo& info) +{ + if (!initialized_) + return false; + + auto result = receiver_.Receive(); + if (!result.has_value()) + return false; + + const score::ts::GptpIpcData& d = result.value(); + info.ptp_assumed_time = d.ptp_assumed_time; + info.local_time = PtpTimeInfo::ReferenceClock::time_point{d.local_time}; + info.rate_deviation = d.rate_deviation; + info.status.is_synchronized = d.status.is_synchronized; + info.status.is_timeout = d.status.is_timeout; + info.status.is_time_jump_future = d.status.is_time_jump_future; + info.status.is_time_jump_past = d.status.is_time_jump_past; + info.status.is_correct = d.status.is_correct; + info.sync_fup_data.precise_origin_timestamp = d.sync_fup_data.precise_origin_timestamp; + info.sync_fup_data.reference_global_timestamp = d.sync_fup_data.reference_global_timestamp; + info.sync_fup_data.reference_local_timestamp = d.sync_fup_data.reference_local_timestamp; + info.sync_fup_data.sync_ingress_timestamp = d.sync_fup_data.sync_ingress_timestamp; + info.sync_fup_data.correction_field = d.sync_fup_data.correction_field; + info.sync_fup_data.sequence_id = d.sync_fup_data.sequence_id; + info.sync_fup_data.pdelay = d.sync_fup_data.pdelay; + info.sync_fup_data.port_number = d.sync_fup_data.port_number; + info.sync_fup_data.clock_identity = d.sync_fup_data.clock_identity; + info.pdelay_data.request_origin_timestamp = d.pdelay_data.request_origin_timestamp; + info.pdelay_data.request_receipt_timestamp = d.pdelay_data.request_receipt_timestamp; + info.pdelay_data.response_origin_timestamp = d.pdelay_data.response_origin_timestamp; + info.pdelay_data.response_receipt_timestamp = d.pdelay_data.response_receipt_timestamp; + info.pdelay_data.reference_global_timestamp = d.pdelay_data.reference_global_timestamp; + info.pdelay_data.reference_local_timestamp = d.pdelay_data.reference_local_timestamp; + info.pdelay_data.sequence_id = d.pdelay_data.sequence_id; + info.pdelay_data.pdelay = d.pdelay_data.pdelay; + info.pdelay_data.req_port_number = d.pdelay_data.req_port_number; + info.pdelay_data.req_clock_identity = d.pdelay_data.req_clock_identity; + info.pdelay_data.resp_port_number = d.pdelay_data.resp_port_number; + info.pdelay_data.resp_clock_identity = d.pdelay_data.resp_clock_identity; + return true; +} + +} // namespace details +} // namespace td +} // namespace score diff --git a/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.h b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.h new file mode 100644 index 0000000..9a9055c --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.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_TIMEDAEMON_CODE_PTP_MACHINE_SHM_DETAILS_SHM_PTP_ENGINE_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_DETAILS_SHM_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 shared-memory IPC channel + * written by TimeSlave via GptpIpcPublisher. + * + * Converts the libTSClient-internal GptpIpcData to the TimeDaemon PtpTimeInfo + * data model. + */ +class ShmPTPEngine final +{ + public: + explicit ShmPTPEngine(std::string ipc_name = score::ts::details::kGptpIpcName) noexcept; + ~ShmPTPEngine() noexcept = default; + + ShmPTPEngine(const ShmPTPEngine&) = delete; + ShmPTPEngine& operator=(const ShmPTPEngine&) = delete; + ShmPTPEngine(ShmPTPEngine&&) = delete; + ShmPTPEngine& operator=(ShmPTPEngine&&) = delete; + + bool Initialize(); + + bool Deinitialize(); + + bool ReadPTPSnapshot(PtpTimeInfo& info); + + private: + std::string ipc_name_; + score::ts::details::GptpIpcReceiver receiver_; + bool initialized_{false}; +}; + +} // namespace details +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_DETAILS_SHM_PTP_ENGINE_H diff --git a/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine_test.cpp b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine_test.cpp new file mode 100644 index 0000000..e67324b --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine_test.cpp @@ -0,0 +1,216 @@ +/******************************************************************************** + * 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/shm/details/shm_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_shm_ut_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1, std::memory_order_relaxed)); +} + +/// Build a fully-populated GptpIpcData for roundtrip verification. +score::ts::GptpIpcData MakeTestIpcData() +{ + score::ts::GptpIpcData d{}; + d.ptp_assumed_time = std::chrono::nanoseconds{9'876'543'210LL}; + d.local_time = std::chrono::nanoseconds{42'000'000'000LL}; + d.rate_deviation = -0.25; + + d.status.is_synchronized = true; + d.status.is_correct = true; + d.status.is_timeout = false; + d.status.is_time_jump_future = false; + d.status.is_time_jump_past = false; + + d.sync_fup_data.precise_origin_timestamp = 100'000'000'000ULL; + d.sync_fup_data.reference_global_timestamp = 100'000'000'500ULL; + d.sync_fup_data.reference_local_timestamp = 100'000'001'000ULL; + d.sync_fup_data.sync_ingress_timestamp = 100'000'001'000ULL; + d.sync_fup_data.correction_field = 8U; + d.sync_fup_data.sequence_id = 55; + d.sync_fup_data.pdelay = 4'000U; + d.sync_fup_data.port_number = 1; + d.sync_fup_data.clock_identity = 0xCAFEBABEDEAD0001ULL; + + d.pdelay_data.request_origin_timestamp = 200'000'000'000ULL; + d.pdelay_data.request_receipt_timestamp = 200'000'001'000ULL; + d.pdelay_data.response_origin_timestamp = 200'000'001'000ULL; + d.pdelay_data.response_receipt_timestamp = 200'000'002'000ULL; + d.pdelay_data.pdelay = 1'000U; + d.pdelay_data.req_port_number = 2; + d.pdelay_data.resp_port_number = 3; + d.pdelay_data.req_clock_identity = 0x0102030405060708ULL; + d.pdelay_data.resp_clock_identity = 0x0807060504030201ULL; + return d; +} + +} // namespace + +class ShmPTPEngineTest : 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(ShmPTPEngineTest, Initialize_WhenShmNotExist_ReturnsFalse) +{ + EXPECT_FALSE(engine_->Initialize()); +} + +TEST_F(ShmPTPEngineTest, Initialize_WhenShmExists_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + EXPECT_TRUE(engine_->Initialize()); +} + +TEST_F(ShmPTPEngineTest, Initialize_CalledTwiceWhenInitialized_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Initialize()); // idempotent +} + +TEST_F(ShmPTPEngineTest, Deinitialize_WhenNotInitialized_ReturnsTrue) +{ + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(ShmPTPEngineTest, Deinitialize_AfterInitialize_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(ShmPTPEngineTest, Deinitialize_CalledTwice_BothReturnTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + EXPECT_TRUE(engine_->Deinitialize()); + EXPECT_TRUE(engine_->Deinitialize()); +} + +TEST_F(ShmPTPEngineTest, ReInitialize_AfterDeinitialize_Succeeds) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(engine_->Initialize()); + ASSERT_TRUE(engine_->Deinitialize()); + EXPECT_TRUE(engine_->Initialize()); +} + +// ── ReadPTPSnapshot ─────────────────────────────────────────────────────────── + +TEST_F(ShmPTPEngineTest, ReadPTPSnapshot_WhenNotInitialized_ReturnsFalse) +{ + PtpTimeInfo info{}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(info)); +} + +TEST_F(ShmPTPEngineTest, ReadPTPSnapshot_WithPublishedData_ReturnsTrue) +{ + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakeTestIpcData()); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + EXPECT_TRUE(engine_->ReadPTPSnapshot(result)); +} + +TEST_F(ShmPTPEngineTest, ReadPTPSnapshot_CopiesTimeAndStatusCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const score::ts::GptpIpcData src = MakeTestIpcData(); + pub_.Publish(src); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.ptp_assumed_time, src.ptp_assumed_time); + EXPECT_DOUBLE_EQ(result.rate_deviation, src.rate_deviation); + EXPECT_EQ(result.status.is_synchronized, src.status.is_synchronized); + EXPECT_EQ(result.status.is_correct, src.status.is_correct); + EXPECT_EQ(result.status.is_timeout, src.status.is_timeout); +} + +TEST_F(ShmPTPEngineTest, ReadPTPSnapshot_CopiesSyncFupDataCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const score::ts::GptpIpcData src = MakeTestIpcData(); + pub_.Publish(src); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.sync_fup_data.precise_origin_timestamp, src.sync_fup_data.precise_origin_timestamp); + EXPECT_EQ(result.sync_fup_data.reference_global_timestamp, src.sync_fup_data.reference_global_timestamp); + EXPECT_EQ(result.sync_fup_data.sequence_id, src.sync_fup_data.sequence_id); + EXPECT_EQ(result.sync_fup_data.pdelay, src.sync_fup_data.pdelay); + EXPECT_EQ(result.sync_fup_data.clock_identity, src.sync_fup_data.clock_identity); +} + +TEST_F(ShmPTPEngineTest, ReadPTPSnapshot_CopiesPDelayDataCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + const score::ts::GptpIpcData src = MakeTestIpcData(); + pub_.Publish(src); + ASSERT_TRUE(engine_->Initialize()); + + PtpTimeInfo result{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(result)); + + EXPECT_EQ(result.pdelay_data.pdelay, src.pdelay_data.pdelay); + EXPECT_EQ(result.pdelay_data.req_port_number, src.pdelay_data.req_port_number); + EXPECT_EQ(result.pdelay_data.resp_port_number, src.pdelay_data.resp_port_number); + EXPECT_EQ(result.pdelay_data.req_clock_identity, src.pdelay_data.req_clock_identity); + EXPECT_EQ(result.pdelay_data.resp_clock_identity, src.pdelay_data.resp_clock_identity); +} + +} // namespace details +} // namespace td +} // namespace score diff --git a/score/TimeDaemon/code/ptp_machine/shm/factory.cpp b/score/TimeDaemon/code/ptp_machine/shm/factory.cpp new file mode 100644 index 0000000..d863159 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/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/shm/factory.h" + +namespace score +{ +namespace td +{ + +std::shared_ptr CreateGPTPShmMachine(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/shm/factory.h b/score/TimeDaemon/code/ptp_machine/shm/factory.h new file mode 100644 index 0000000..65a5f4c --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/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_SHM_FACTORY_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_FACTORY_H + +#include "score/TimeDaemon/code/ptp_machine/shm/gptp_shm_machine.h" +#include "score/libTSClient/gptp_ipc_channel.h" + +#include +#include + +namespace score +{ +namespace td +{ + +/** + * @brief Factory function to create a configured GPTPShmMachine. + * + * Creates a GPTPShmMachine backed by the shared-memory 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 GPTPShmMachine instance. + */ +std::shared_ptr CreateGPTPShmMachine(const std::string& name, + const std::string& ipc_name = score::ts::details::kGptpIpcName); + +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_FACTORY_H diff --git a/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_machine.h b/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_machine.h new file mode 100644 index 0000000..0da9f41 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_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_SHM_GPTP_SHM_MACHINE_H +#define SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_GPTP_SHM_MACHINE_H + +#include "score/TimeDaemon/code/ptp_machine/core/ptp_machine.h" +#include "score/TimeDaemon/code/ptp_machine/shm/details/shm_ptp_engine.h" + +namespace score +{ +namespace td +{ + +/// @brief PTPMachine instantiated with the shared-memory gPTP engine. +/// +/// Reads PtpTimeInfo snapshots written by TimeSlave via the IPC channel. +/// Construct via CreateGPTPShmMachine() (see factory.h) or directly: +/// +/// @code +/// auto machine = std::make_shared( +/// "shm", std::chrono::milliseconds{50}, "/gptp_ptp_info"); +/// @endcode +using GPTPShmMachine = PTPMachine; + +} // namespace td +} // namespace score + +#endif // SCORE_TIMEDAEMON_CODE_PTP_MACHINE_SHM_GPTP_SHM_MACHINE_H diff --git a/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_machine_test.cpp b/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_machine_test.cpp new file mode 100644 index 0000000..0d96d07 --- /dev/null +++ b/score/TimeDaemon/code/ptp_machine/shm/gptp_shm_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/shm/gptp_shm_machine.h" +#include "score/TimeDaemon/code/ptp_machine/shm/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::ts::GptpIpcData MakePublishedInfo() +{ + score::ts::GptpIpcData 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 GPTPShmMachineIntegrationTest : public ::testing::Test +{ + protected: + void SetUp() override + { + name_ = UniqueShmName(); + ASSERT_TRUE(pub_.Init(name_)); + pub_.Publish(MakePublishedInfo()); + + machine_ = CreateGPTPShmMachine("ShmPTPMachine", 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(GPTPShmMachineIntegrationTest, GetName_ReturnsConstructionName) +{ + EXPECT_EQ(machine_->GetName(), "ShmPTPMachine"); +} + +TEST_F(GPTPShmMachineIntegrationTest, Init_WhenShmExists_ReturnsTrue) +{ + EXPECT_TRUE(machine_->Init()); +} + +TEST_F(GPTPShmMachineIntegrationTest, Init_WhenShmMissing_ReturnsFalse) +{ + auto m = CreateGPTPShmMachine("NoShm", "/gptp_nosuchshm_xyz"); + EXPECT_FALSE(m->Init()); +} + +TEST_F(GPTPShmMachineIntegrationTest, 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(GPTPShmMachineIntegrationTest, 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..9075524 --- /dev/null +++ b/score/TimeSlave/BUILD @@ -0,0 +1,23 @@ +# ******************************************************************************* +# 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") + +cc_unit_test_suites_for_host_and_qnx( + name = "unit_test_suite", + cc_unit_tests = [], + test_suites_from_sub_packages = [ + "//score/TimeSlave/code/gptp:unit_test_suite", + ], + visibility = ["//score:__subpackages__"], +) 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..1361a31 --- /dev/null +++ b/score/TimeSlave/code/application/BUILD @@ -0,0 +1,32 @@ +# ******************************************************************************* +# 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_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..a4345de --- /dev/null +++ b/score/TimeSlave/code/application/time_slave.cpp @@ -0,0 +1,81 @@ +/******************************************************************************** + * 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 + +namespace score +{ +namespace ts +{ + +namespace +{ + +constexpr std::int32_t kInitSuccess = 0; +constexpr std::int32_t kInitFailure = -1; + +} // namespace + +TimeSlave::TimeSlave() = default; + +std::int32_t TimeSlave::Initialize(const score::mw::lifecycle::ApplicationContext& /*context*/) +{ + engine_ = std::make_unique(opts_); + + if (!engine_->Initialize()) + { + score::mw::log::LogError(kTimeSlaveAppContext) << "TimeSlave: GptpEngine initialization failed"; + return kInitFailure; + } + + if (!publisher_.Init()) + { + score::mw::log::LogError(kTimeSlaveAppContext) << "TimeSlave: shared memory publisher initialization failed"; + return kInitFailure; + } + + score::mw::log::LogInfo(kTimeSlaveAppContext) << "TimeSlave initialized"; + return kInitSuccess; +} + +std::int32_t TimeSlave::Run(const score::cpp::stop_token& token) +{ + constexpr auto kPublishInterval = std::chrono::milliseconds{50}; + + score::mw::log::LogInfo(kTimeSlaveAppContext) << "TimeSlave running"; + + while (!token.stop_requested()) + { + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + if (engine_->ReadPTPSnapshot(data)) + { + publisher_.Publish(data); + } + + std::this_thread::sleep_for(kPublishInterval); + } + + engine_->Deinitialize(); + publisher_.Destroy(); + + score::mw::log::LogInfo(kTimeSlaveAppContext) << "TimeSlave stopped"; + return kInitSuccess; +} + +} // 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..80c7182 --- /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: + TimeSlave(); + ~TimeSlave() noexcept override = default; + + TimeSlave(TimeSlave&&) = delete; + TimeSlave(const TimeSlave&) = delete; + TimeSlave& operator=(TimeSlave&&) & = delete; + TimeSlave& operator=(const TimeSlave&) & = 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..e432223 --- /dev/null +++ b/score/TimeSlave/code/common/logging_contexts.h @@ -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 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_COMMON_LOGGING_CONTEXTS_H +#define SCORE_TIMESLAVE_CODE_COMMON_LOGGING_CONTEXTS_H + +namespace score +{ +namespace ts +{ + +/// Logging context for the gPTP protocol engine (RxThread / PdelayThread). +constexpr auto kGPtpMachineContext = "GTPS"; + +/// Logging context for the TimeSlave application lifecycle (Initialize / Run). +constexpr auto kTimeSlaveAppContext = "TSAP"; + +} // 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..9a19ed8 --- /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/TimeSlave/code/common:logging_contexts", + "//score/TimeSlave/code/gptp/details:gptp_details", + "//score/TimeSlave/code/gptp/phc:phc_adjuster", + "//score/libTSClient:gptp_ipc", + "@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..5c1f331 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/BUILD @@ -0,0 +1,233 @@ +# ******************************************************************************* +# 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_os_syscalls", + hdrs = ["i_os_syscalls.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_os_syscalls", + ":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 = "clock_util", + hdrs = ["clock_util.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 = [ + ":clock_util", + ":ptp_types", + "//score/libTSClient:gptp_ipc", + ], +) + +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/libTSClient:gptp_ipc", + ], +) + +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 = "raw_socket_test", + srcs = ["raw_socket_test.cpp"], + tags = ["unit"], + target_compatible_with = ["@platforms//os:linux"], + deps = [ + ":network_identity", + ":raw_socket", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +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", + ":raw_socket_test", + ":sync_state_machine_test", + ], + test_suites_from_sub_packages = [], + visibility = ["//score:__subpackages__"], +) diff --git a/score/TimeSlave/code/gptp/details/clock_util.h b/score/TimeSlave/code/gptp/details/clock_util.h new file mode 100644 index 0000000..bfa3dde --- /dev/null +++ b/score/TimeSlave/code/gptp/details/clock_util.h @@ -0,0 +1,40 @@ +/******************************************************************************** + * 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_CLOCK_UTIL_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_CLOCK_UTIL_H + +#include "score/TimeSlave/code/gptp/details/ptp_types.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +inline std::int64_t MonoNs() noexcept +{ + ::timespec ts{}; + if (::clock_gettime(CLOCK_MONOTONIC, &ts) != 0) + return 0; + return static_cast(ts.tv_sec) * kNsPerSec + ts.tv_nsec; +} + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_CLOCK_UTIL_H 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..a195b0d --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec.cpp @@ -0,0 +1,99 @@ +/******************************************************************************** + * 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 +{ + +constexpr std::array kPtpDstMacBytes = { + 0x01U, 0x80U, 0xC2U, 0x00U, 0x00U, 0x0EU}; + +constexpr std::size_t kVlanTciLen = 2U; + +} // namespace + +bool FrameCodec::ParseEthernetHeader(const std::uint8_t* frame, int frame_len, int& ptp_offset) const +{ + // Convert to size_t once (after the negative guard) to avoid signed/unsigned + // comparisons when mixing frame_len (int) with size constants (std::size_t). + if (frame_len <= 0) + return false; + const std::size_t len = static_cast(frame_len); + + constexpr std::size_t kEthHdrLen = sizeof(ethhdr); + if (len < kEthHdrLen) + return false; + + ethhdr hdr{}; + std::memcpy(&hdr, frame, sizeof(hdr)); + + const auto etype = static_cast(ntohs(hdr.h_proto)); + + if (etype == kEthP8021Q) + { + // After the 14-byte ethhdr, the 802.1Q VLAN overhead is: + // offset 14–15: TCI (2 bytes) + // offset 16–17: inner EtherType (2 bytes) ← read from here + // offset 18+ : PTP payload ← ptp_offset + if (len < kEthHdrLen + kVlanTagLen + 2U) + return false; + std::uint16_t inner_etype_be{}; + std::memcpy(&inner_etype_be, frame + kEthHdrLen + kVlanTciLen, sizeof(inner_etype_be)); + if (static_cast(ntohs(inner_etype_be)) != kEthP1588) + return false; + ptp_offset = static_cast(kEthHdrLen + kVlanTagLen); + return true; + } + + if (etype != kEthP1588) + return false; + + ptp_offset = static_cast(kEthHdrLen); + return true; +} + +bool FrameCodec::AddEthernetHeader(std::uint8_t* buf, + unsigned int& buf_len, + const std::array& src_mac, + std::size_t buf_capacity) const +{ + const unsigned int kHdrLen = static_cast(sizeof(ethhdr)); + + if (buf_capacity < kHdrLen || buf_len > static_cast(buf_capacity) - kHdrLen) + return false; + + std::memmove(buf + kHdrLen, buf, buf_len); + + auto* hdr = reinterpret_cast(buf); + std::memcpy(hdr->h_dest, kPtpDstMacBytes.data(), kMacAddrLen); + std::memcpy(hdr->h_source, src_mac.data(), kMacAddrLen); + 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..edcef5c --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec.h @@ -0,0 +1,72 @@ +/******************************************************************************** + * 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 +#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. + * @param src_mac Source MAC address (should be the port's own MAC). + * @param buf_capacity Total allocated size of @p buf in bytes; used to detect overflow. + * @return true on success, false if the buffer would overflow. + */ + bool AddEthernetHeader(std::uint8_t* buf, + unsigned int& buf_len, + const std::array& src_mac, + std::size_t buf_capacity) 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..1912db4 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/frame_codec_test.cpp @@ -0,0 +1,153 @@ +/******************************************************************************** + * 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 +#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) +{ + // IEEE 802.1Q layout: ethhdr(14) | TCI(2) | inner EtherType(2) | payload + // offset 14-15: TCI + // offset 16-17: inner EtherType ← written here + // offset 18+ : PTP payload ← ptp_offset == 18 == 14 + kVlanTagLen + auto buf = MakeEthFrame(static_cast(kEthP8021Q), 60); + const std::uint16_t inner_be = htons(static_cast(kEthP1588)); + std::memcpy(&buf[14 + 2], &inner_be, 2); // inner EtherType at offset 16 + int offset = -1; + ASSERT_TRUE(codec_.ParseEthernetHeader(buf.data(), 60, offset)); + EXPECT_EQ(offset, 14 + kVlanTagLen); // PTP payload at offset 18 +} + +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; + const std::array src_mac = {0x02U, 0x00U, 0x00U, 0xFFU, 0x00U, 0x11U}; + ASSERT_TRUE(codec_.AddEthernetHeader(buf, len, src_mac, sizeof(buf))); + 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 > capacity + std::uint8_t buf[4096] = {}; + unsigned int len = kTooBig; + const std::array src_mac = {0x02U, 0x00U, 0x00U, 0xFFU, 0x00U, 0x11U}; + EXPECT_FALSE(codec_.AddEthernetHeader(buf, len, src_mac, kTooBig)); +} + +} // 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_os_syscalls.h b/score/TimeSlave/code/gptp/details/i_os_syscalls.h new file mode 100644 index 0000000..fd4e8f9 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/i_os_syscalls.h @@ -0,0 +1,102 @@ +/******************************************************************************** + * 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_OS_SYSCALLS_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_OS_SYSCALLS_H + +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Thin abstraction over the POSIX system calls used by RawSocket. +/// The default production path calls the real syscalls; a fake can be injected +/// in unit tests to exercise every branch without requiring CAP_NET_RAW. +class IOsSyscalls +{ + public: + virtual ~IOsSyscalls() = default; + + virtual int socket_call(int domain, int type, int protocol) noexcept = 0; + virtual int ioctl_call(int fd, unsigned long req, void* arg) noexcept = 0; + virtual int bind_call(int fd, const ::sockaddr* addr, ::socklen_t addrlen) noexcept = 0; + virtual int setsockopt_call(int fd, + int level, + int optname, + const void* optval, + ::socklen_t optlen) noexcept = 0; + virtual int close_call(int fd) noexcept = 0; + virtual int poll_call(::pollfd* fds, ::nfds_t nfds, int timeout) noexcept = 0; + virtual ::ssize_t recvmsg_call(int fd, ::msghdr* msg, int flags) noexcept = 0; + virtual ::ssize_t send_call(int fd, const void* buf, ::size_t len, int flags) noexcept = 0; +}; + +/// Real production implementation — delegates directly to the OS. +class RealOsSyscalls final : public IOsSyscalls +{ + public: + static RealOsSyscalls& Instance() noexcept + { + static RealOsSyscalls s; + return s; + } + + int socket_call(int d, int t, int p) noexcept override + { + return ::socket(d, t, p); + } + int ioctl_call(int fd, unsigned long req, void* arg) noexcept override + { + return ::ioctl(fd, req, arg); + } + int bind_call(int fd, const ::sockaddr* a, ::socklen_t l) noexcept override + { + return ::bind(fd, a, l); + } + int setsockopt_call(int fd, int lv, int opt, const void* v, ::socklen_t l) noexcept override + { + return ::setsockopt(fd, lv, opt, v, l); + } + int close_call(int fd) noexcept override + { + return ::close(fd); + } + int poll_call(::pollfd* fds, ::nfds_t n, int t) noexcept override + { + return ::poll(fds, n, t); + } + ::ssize_t recvmsg_call(int fd, ::msghdr* m, int f) noexcept override + { + return ::recvmsg(fd, m, f); + } + ::ssize_t send_call(int fd, const void* b, ::size_t l, int f) noexcept override + { + return ::send(fd, b, l, f); + } + + private: + RealOsSyscalls() = default; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_TIMESLAVE_CODE_GPTP_DETAILS_I_OS_SYSCALLS_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..687db78 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/message_parser.cpp @@ -0,0 +1,155 @@ +/******************************************************************************** + * 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 + +namespace +{ + +inline std::uint64_t ByteSwap64(std::uint64_t v) noexcept +{ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + return __builtin_bswap64(v); +#else + return v; +#endif +} + +} // namespace + +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 ByteSwap64(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; + + const std::uint16_t declared_len = + static_cast((static_cast(payload[2]) << 8U) | payload[3]); + if (declared_len < sizeof(PTPHeader) || static_cast(declared_len) > payload_len) + return false; + + // Validate transportSpecific nibble (must be 0x1 for 802.1AS) and PTP version. + if ((payload[0] & 0xF0U) != kPtpTransportSpecific) + return false; + if ((payload[1] & 0x0FU) != kPtpVersion) + 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.requestReceiptTimestamp = LoadTimestamp(payload + kBodyOffset); + // Also parse requestingPortIdentity (8-byte ClockIdentity + 2-byte portNumber) + // so that ComputeAndStoreUnlocked() can verify the response is addressed to us. + constexpr std::size_t kPidOffset = kBodyOffset + sizeof(Timestamp); // = 44 + if (payload_len >= kPidOffset + 10U) + { + std::memcpy(msg.pdelay_resp.requestingPortIdentity.clockIdentity.id, + payload + kPidOffset, 8); + msg.pdelay_resp.requestingPortIdentity.portNumber = LoadU16(payload + kPidOffset + 8U); + } + } + break; + + case kPtpMsgtypePdelayRespFollowUp: + if (payload_len >= kBodyOffset + sizeof(Timestamp)) + { + msg.pdelay_resp_fup.responseOriginReceiptTimestamp = LoadTimestamp(payload + kBodyOffset); + constexpr std::size_t kPidOffset = kBodyOffset + sizeof(Timestamp); // = 44 + if (payload_len >= kPidOffset + 10U) + { + std::memcpy(msg.pdelay_resp_fup.requestingPortIdentity.clockIdentity.id, + payload + kPidOffset, 8); + msg.pdelay_resp_fup.requestingPortIdentity.portNumber = + LoadU16(payload + kPidOffset + 8U); + } + } + 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..94980da --- /dev/null +++ b/score/TimeSlave/code/gptp/details/message_parser_test.cpp @@ -0,0 +1,243 @@ +/******************************************************************************** + * 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] = static_cast(ControlField::kFollowUp); + + // 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.requestReceiptTimestamp.seconds_lsb, kSecLsb); + EXPECT_EQ(msg.pdelay_resp.requestReceiptTimestamp.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); +} + +// ── TimestampToTmv / TmvToTimestamp overflow guards ─────────────────────────── + +TEST_F(MessageParserTest, TimestampToTmv_SecExceedsMax_ReturnsZero) +{ + // seconds_msb=3 → sec = 3 * 2^32 = 12,884,901,888 > kMaxSec (9,223,372,036) + Timestamp ts{}; + ts.seconds_msb = 3U; + ts.seconds_lsb = 0U; + ts.nanoseconds = 0U; + const TmvT result = TimestampToTmv(ts); + EXPECT_EQ(result.ns, 0LL); +} + +TEST_F(MessageParserTest, TimestampToTmv_TotalNsExceedsMax_ReturnsZero) +{ + // sec = kMaxSec = 9,223,372,036 (seconds_msb=2, seconds_lsb=633,437,444) + // total_ns = kMaxSec * 1e9 + 854,775,808 > INT64_MAX + Timestamp ts{}; + ts.seconds_msb = 2U; + ts.seconds_lsb = 633'437'444U; + ts.nanoseconds = 854'775'808U; + const TmvT result = TimestampToTmv(ts); + EXPECT_EQ(result.ns, 0LL); +} + +TEST_F(MessageParserTest, TmvToTimestamp_NegativeNs_ReturnsZeroTimestamp) +{ + const Timestamp ts = TmvToTimestamp(TmvT{-1LL}); + EXPECT_EQ(ts.seconds_msb, 0U); + EXPECT_EQ(ts.seconds_lsb, 0U); + EXPECT_EQ(ts.nanoseconds, 0U); +} + +} // 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..20de210 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/pdelay_measurer.cpp @@ -0,0 +1,180 @@ +/******************************************************************************** + * 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 +#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(seqnum_); + req.ptpHdr.controlField = static_cast(ControlField::kOther); + req.ptpHdr.logMessageInterval = 0x7F; + + // Save a copy with host-byte-order fields for later matching. + // portNumber and sequenceId are stored in host byte order so that + // memcmp/equality checks in ComputeAndStoreUnlocked() agree with the + // host-order values produced by GgtpMessageParser (LoadU16/ntohs). + { + std::lock_guard lk(mutex_); + req_ = req; + req_.ptpHdr.sequenceId = seqnum_; + req_.ptpHdr.sourcePortIdentity.portNumber = 0x0001U; // host byte order + req_.sendHardwareTS = TmvT{-1}; // sentinel: TX timestamp pending + resp_count_ = 0U; + ++seqnum_; // uint16_t: wraps naturally at 0xFFFF + } + + // Derive the source MAC from the EUI-64 ClockIdentity (reverse EUI-48→EUI-64 + // expansion: OUI = id[0..2], vendor = id[5..7]). + const std::array src_mac = {local_identity_.id[0], + local_identity_.id[1], + local_identity_.id[2], + local_identity_.id[5], + local_identity_.id[6], + local_identity_.id[7]}; + + // Use a separate stack buffer — never alias the PTPMessage object itself as a + // raw frame buffer; AddEthernetHeader() shifts the payload in-place and would + // write beyond sizeof(PTPMessage). + std::uint8_t buf[2048]{}; + unsigned int len = sizeof(PdelayReqBody); + std::memcpy(buf, &req, len); + + FrameCodec codec; + if (!codec.AddEthernetHeader(buf, len, src_mac, sizeof(buf))) + 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_); + if (msg.ptpHdr.sequenceId != req_.ptpHdr.sequenceId) + return; + ++resp_count_; + resp_ = msg; +} + +void PeerDelayMeasurer::OnResponseFollowUp(const PTPMessage& msg) +{ + std::lock_guard lk(mutex_); + if (msg.ptpHdr.sequenceId != req_.ptpHdr.sequenceId) + return; + resp_fup_ = msg; + ComputeAndStoreUnlocked(); +} + +void PeerDelayMeasurer::ComputeAndStoreUnlocked() noexcept +{ + if (resp_count_ > 1U) // multiple responses → non-time-aware bridge detected + return; + if (req_.ptpHdr.sequenceId != resp_.ptpHdr.sequenceId) + return; + if (resp_.ptpHdr.sequenceId != resp_fup_.ptpHdr.sequenceId) + return; + + // Reject if t1 has not been recorded yet (TX timestamp still pending after Send()). + // Without this guard, a response arriving in the race window between the first + // lock release and the sendHardwareTS assignment would produce a garbage delay. + // Sentinel value -1 means "TX timestamp pending"; 0 is a valid timestamp (t=0). + if (req_.sendHardwareTS.ns < 0) + return; + + if (std::memcmp(&resp_.pdelay_resp.requestingPortIdentity, + &req_.ptpHdr.sourcePortIdentity, + sizeof(PortIdentity)) != 0) + return; + if (std::memcmp(&resp_fup_.pdelay_resp_fup.requestingPortIdentity, + &req_.ptpHdr.sourcePortIdentity, + sizeof(PortIdentity)) != 0) + return; + + // t1 = BPF_T_BINTIME (PHC) send timestamp of our Pdelay_Req (TX loopback fd) + 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 = BPF_T_BINTIME (PHC) receive timestamp of Pdelay_Resp (main BPF fd) + const TmvT t4 = resp_.recvHardwareTS; + + const std::int64_t delay = ((t2.ns - t1.ns) + (t4.ns - t3c.ns)) / 2LL; + + if (delay < 0) + return; + + PDelayResult r{}; + r.path_delay_ns = delay; + r.valid = true; + + score::ts::GptpIpcPDelayData& 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; + resp_count_ = 0U; +} + +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..c9d3152 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/pdelay_measurer.h @@ -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 + ********************************************************************************/ +#ifndef SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PDELAY_MEASURER_H +#define SCORE_TIMESLAVE_CODE_GPTP_DETAILS_PDELAY_MEASURER_H + +#include "score/libTSClient/gptp_ipc_data.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::ts::GptpIpcPDelayData 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 ComputeAndStoreUnlocked() noexcept; + + ClockIdentity local_identity_{}; + + mutable std::mutex mutex_; + + std::uint16_t seqnum_{0U}; + std::uint16_t resp_count_{0U}; // Pdelay_Resp messages received for the current request + 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..362c289 --- /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::ts::GptpIpcPDelayData& 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..bbdbcb8 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/ptp_types.h @@ -0,0 +1,228 @@ +/******************************************************************************** + * 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 +#include + +#ifndef __QNXNTO__ +#include +#else +// Minimal ethhdr definition for QNX +struct ethhdr +{ + unsigned char h_dest[6]; + unsigned char h_source[6]; + uint16_t h_proto; +}; +#endif + +#define SCORE_TS_PACKED __attribute__((packed)) + +namespace score +{ +namespace ts +{ +namespace details +{ + +// ─── EtherType constants ──────────────────────────────────────────────────── +constexpr std::uint16_t kEthP1588 = 0x88F7U; +constexpr std::uint16_t kEthP8021Q = 0x8100U; + +// ─── MAC / buffer sizes ───────────────────────────────────────────────────── +constexpr std::size_t kMacAddrLen = 6U; +constexpr std::size_t kVlanTagLen = 4U; + +// ─── 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; + +// ─── Control field ─────────────────────────────────────────────────────────── +enum class ControlField : std::uint8_t +{ + kSync = 0, + kDelayReq = 1, + kFollowUp = 2, + kDelayResp = 3, + kManagement = 4, + kOther = 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 SCORE_TS_PACKED) ─────────────────────────────── +struct SCORE_TS_PACKED ClockIdentity +{ + std::uint8_t id[8]{}; +}; + +struct SCORE_TS_PACKED PortIdentity +{ + ClockIdentity clockIdentity; + std::uint16_t portNumber{0}; +}; + +struct SCORE_TS_PACKED Timestamp +{ + std::uint16_t seconds_msb{0}; + std::uint32_t seconds_lsb{0}; + std::uint32_t nanoseconds{0}; +}; + +struct SCORE_TS_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 SCORE_TS_PACKED SyncBody +{ + PTPHeader ptpHdr{}; + Timestamp originTimestamp{}; +}; + +struct SCORE_TS_PACKED FollowUpBody +{ + PTPHeader ptpHdr{}; + Timestamp preciseOriginTimestamp{}; +}; + +struct SCORE_TS_PACKED PdelayReqBody +{ + PTPHeader ptpHdr{}; + Timestamp requestReceiptTimestamp{}; + PortIdentity reserved{}; +}; + +struct SCORE_TS_PACKED PdelayRespBody +{ + PTPHeader ptpHdr{}; + Timestamp requestReceiptTimestamp{}; ///< IEEE 802.1AS: t₂ — time the remote peer received our PdelayReq + PortIdentity requestingPortIdentity{}; +}; + +struct SCORE_TS_PACKED PdelayRespFollowUpBody +{ + PTPHeader ptpHdr{}; + Timestamp responseOriginReceiptTimestamp{}; + PortIdentity requestingPortIdentity{}; +}; + +struct SCORE_TS_PACKED RawMessageData +{ + std::uint8_t buffer[1500]{}; +}; + +struct PTPMessage +{ + union SCORE_TS_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{}; + std::int64_t recvMonoNs{0}; // CLOCK_MONOTONIC at packet reception; set for Sync only +}; + +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); + constexpr std::uint64_t kMaxNs = + static_cast(std::numeric_limits::max()); + constexpr std::uint64_t kMaxSec = kMaxNs / static_cast(kNsPerSec); + if (sec > kMaxSec) + return TmvT{}; + const std::uint64_t total_ns = sec * static_cast(kNsPerSec) + ts.nanoseconds; + if (total_ns > kMaxNs) + return TmvT{}; + return TmvT{static_cast(total_ns)}; +} + +inline Timestamp TmvToTimestamp(const TmvT& x) noexcept +{ + if (x.ns < 0) + return Timestamp{}; // negative timestamps are invalid on the wire + 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 / 65536LL}; +} + +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..9f148cf --- /dev/null +++ b/score/TimeSlave/code/gptp/details/raw_socket.h @@ -0,0 +1,94 @@ +/******************************************************************************** + * 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_os_syscalls.h" +#include "score/TimeSlave/code/gptp/details/i_raw_socket.h" + +#include +#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: + /// @param sys Optional syscall shim for unit testing. nullptr → real OS calls. + explicit RawSocket(IOsSyscalls* sys = nullptr) noexcept; + ~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_.load(std::memory_order_relaxed); + } + + private: + IOsSyscalls* sys_{nullptr}; + std::atomic 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/raw_socket_test.cpp b/score/TimeSlave/code/gptp/details/raw_socket_test.cpp new file mode 100644 index 0000000..4e9f786 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/raw_socket_test.cpp @@ -0,0 +1,614 @@ +/******************************************************************************** + * 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/network_identity.h" +#include "score/TimeSlave/code/gptp/details/raw_socket.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// ── FakeOsSyscalls ───────────────────────────────────────────────────────────── +// +// A controllable IOsSyscalls implementation that never touches the real OS. +// Each method has configurable return values; side-effects (like filling cmsg +// data) are controlled by boolean flags. + +class FakeOsSyscalls final : public IOsSyscalls +{ + public: + // ── socket ──────────────────────────────────────────────────────────────── + int socket_fd{42}; // fd returned on success + bool socket_fail{false}; // true → return -1 + + int socket_call(int /*domain*/, int /*type*/, int /*protocol*/) noexcept override + { + if (socket_fail) + { + errno = EPERM; + return -1; + } + return socket_fd; + } + + // ── ioctl ───────────────────────────────────────────────────────────────── + bool ioctl_siocgifindex_fail{false}; // SIOCGIFINDEX failure + bool ioctl_siocshwtstamp_fail{false}; // first SIOCSHWTSTAMP failure (fallback exercised) + + int ioctl_call(int /*fd*/, unsigned long req, void* /*arg*/) noexcept override + { + if (req == SIOCGIFINDEX) + return ioctl_siocgifindex_fail ? -1 : 0; + if (req == SIOCSHWTSTAMP) + { + if (ioctl_siocshwtstamp_fail) + { + ioctl_siocshwtstamp_fail = false; // first call fails; second succeeds + return -1; + } + return 0; + } + return 0; + } + + // ── bind ───────────────────────────────────────────────────────────────── + bool bind_fail{false}; + + int bind_call(int /*fd*/, const ::sockaddr* /*addr*/, ::socklen_t /*addrlen*/) noexcept override + { + return bind_fail ? -1 : 0; + } + + // ── setsockopt ──────────────────────────────────────────────────────────── + bool setsockopt_fail{false}; + + int setsockopt_call(int /*fd*/, + int /*level*/, + int /*optname*/, + const void* /*optval*/, + ::socklen_t /*optlen*/) noexcept override + { + return setsockopt_fail ? -1 : 0; + } + + // ── close ──────────────────────────────────────────────────────────────── + int close_count{0}; + int last_closed_fd{-1}; + + int close_call(int fd) noexcept override + { + ++close_count; + last_closed_fd = fd; + return 0; + } + + // ── poll ───────────────────────────────────────────────────────────────── + int poll_result{0}; // 0=timeout, -1=error, >0=ready + int poll_revents{POLLIN}; + + int poll_call(::pollfd* fds, ::nfds_t /*nfds*/, int /*timeout*/) noexcept override + { + if (fds != nullptr) + fds[0].revents = (poll_result > 0) ? static_cast(poll_revents) : 0; + return poll_result; + } + + // ── recvmsg ─────────────────────────────────────────────────────────────── + // Regular (non-errqueue) recvmsg — used by Recv(). + ::ssize_t recvmsg_result{-1}; // bytes returned; -1 = error + bool recvmsg_fill_hwts{false}; // fill SO_TIMESTAMPING cmsg with ts[2]={1,500000000} + + // MSG_ERRQUEUE recvmsg — used by DrainErrQueue (before send) and TX timestamp (after send). + // Sequence per Send() call: + // calls 0 .. errqueue_drain_count-1 → return 1 (drain loop body runs) + // call errqueue_drain_count → return -1 (terminates DrainErrQueue loop) + // call errqueue_drain_count+1 → TX timestamp: fill hwts if tx_fill_hwts, return tx_result + int errqueue_drain_count{0}; // how many drain entries to simulate + bool tx_fill_hwts{false}; // fill SO_TIMESTAMPING cmsg in TX-timestamp recvmsg + ::ssize_t tx_result{14}; // TX-timestamp recvmsg return value + + int recvmsg_call_count{0}; + + ::ssize_t recvmsg_call(int /*fd*/, ::msghdr* msg, int flags) noexcept override + { + ++recvmsg_call_count; + + if ((flags & MSG_ERRQUEUE) != 0) + { + const int idx = errqueue_call_count_++; + if (idx < errqueue_drain_count) + return 1; // drain entry present — loop body exercises + if (idx == errqueue_drain_count) + return -1; // end of drain queue → loop exits + // idx > errqueue_drain_count: this is the TX-timestamp recvmsg + if (tx_fill_hwts && msg != nullptr && msg->msg_control != nullptr && + msg->msg_controllen >= CMSG_SPACE(3 * sizeof(::timespec))) + { + auto* cm = reinterpret_cast<::cmsghdr*>(msg->msg_control); + cm->cmsg_level = SOL_SOCKET; + cm->cmsg_type = SO_TIMESTAMPING; + cm->cmsg_len = CMSG_LEN(3 * sizeof(::timespec)); + auto* ts = reinterpret_cast<::timespec*>(CMSG_DATA(cm)); + ts[0] = {0, 0}; + ts[1] = {0, 0}; + ts[2] = {1, 500'000'000L}; + msg->msg_controllen = CMSG_SPACE(3 * sizeof(::timespec)); + } + return tx_result; + } + + // Regular recvmsg (from Recv()). + if (recvmsg_result < 0) + return -1; + + // Optionally inject SO_TIMESTAMPING cmsg for Recv() hwts extraction. + if (recvmsg_fill_hwts && msg != nullptr && msg->msg_control != nullptr && + msg->msg_controllen >= CMSG_SPACE(3 * sizeof(::timespec))) + { + auto* cm = reinterpret_cast<::cmsghdr*>(msg->msg_control); + cm->cmsg_level = SOL_SOCKET; + cm->cmsg_type = SO_TIMESTAMPING; + cm->cmsg_len = CMSG_LEN(3 * sizeof(::timespec)); + auto* ts = reinterpret_cast<::timespec*>(CMSG_DATA(cm)); + ts[0] = {0, 0}; + ts[1] = {0, 0}; + ts[2] = {1, 500'000'000L}; + msg->msg_controllen = CMSG_SPACE(3 * sizeof(::timespec)); + } + + if (msg != nullptr && msg->msg_iov != nullptr && recvmsg_result > 0) + { + const std::size_t n = + std::min(static_cast(recvmsg_result), msg->msg_iov[0].iov_len); + std::memset(msg->msg_iov[0].iov_base, 0, n); + } + return recvmsg_result; + } + + private: + int errqueue_call_count_{0}; // counts MSG_ERRQUEUE recvmsg calls (resets per test instance) + + public: + // ── send ────────────────────────────────────────────────────────────────── + ::ssize_t send_result{14}; // bytes "sent"; -1 = error + + ::ssize_t send_call(int /*fd*/, + const void* /*buf*/, + ::size_t len, + int /*flags*/) noexcept override + { + if (send_result < 0) + return -1; + return static_cast<::ssize_t>(len); + } +}; + +// Helper: open the socket successfully using the given fake syscalls. +void OpenSocket(RawSocket& sock, FakeOsSyscalls& /*fake*/) +{ + (void)sock.Open("eth0"); +} + +} // namespace + +// ── RawSocket — closed-state guard paths ────────────────────────────────────── + +TEST(RawSocketTest, DefaultConstruct_GetFd_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + fake.socket_fail = true; // never actually open + RawSocket sock{&fake}; + EXPECT_EQ(sock.GetFd(), -1); +} + +TEST(RawSocketTest, Close_WhenNotOpen_IsNoOp) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + EXPECT_NO_THROW(sock.Close()); + EXPECT_EQ(sock.GetFd(), -1); + EXPECT_EQ(fake.close_count, 0); // no real fd was open +} + +TEST(RawSocketTest, EnableHwTimestamping_WhenNotOpen_ReturnsFalse) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + EXPECT_FALSE(sock.EnableHwTimestamping()); +} + +TEST(RawSocketTest, Recv_WhenNotOpen_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + std::uint8_t buf[64] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 0), -1); +} + +TEST(RawSocketTest, Recv_NullBuf_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(nullptr, 64U, hwts, 0), -1); +} + +TEST(RawSocketTest, Recv_ZeroBufLen_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[1] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, 0U, hwts, 0), -1); +} + +TEST(RawSocketTest, Send_WhenNotOpen_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + const std::uint8_t data[14] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 14, hwts), -1); +} + +TEST(RawSocketTest, Send_NullBuf_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + ::timespec hwts{}; + EXPECT_EQ(sock.Send(nullptr, 14, hwts), -1); +} + +TEST(RawSocketTest, Send_ZeroLen_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[1] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 0, hwts), -1); +} + +TEST(RawSocketTest, Send_NegativeLen_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[1] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, -1, hwts), -1); +} + +// ── RawSocket — Open() failure paths ───────────────────────────────────────── + +TEST(RawSocketTest, Open_SocketCallFails_ReturnsFalse) +{ + FakeOsSyscalls fake; + fake.socket_fail = true; + RawSocket sock{&fake}; + EXPECT_FALSE(sock.Open("eth0")); + EXPECT_EQ(sock.GetFd(), -1); +} + +TEST(RawSocketTest, Open_IoctlSiocgifindexFails_ReturnsFalse) +{ + FakeOsSyscalls fake; + fake.ioctl_siocgifindex_fail = true; + RawSocket sock{&fake}; + EXPECT_FALSE(sock.Open("eth0")); + EXPECT_EQ(sock.GetFd(), -1); + // The fake fd must have been closed on failure + EXPECT_EQ(fake.close_count, 1); + EXPECT_EQ(fake.last_closed_fd, fake.socket_fd); +} + +TEST(RawSocketTest, Open_BindFails_ReturnsFalse) +{ + FakeOsSyscalls fake; + fake.bind_fail = true; + RawSocket sock{&fake}; + EXPECT_FALSE(sock.Open("eth0")); + EXPECT_EQ(sock.GetFd(), -1); + EXPECT_EQ(fake.close_count, 1); +} + +TEST(RawSocketTest, Open_Success_ReturnsTrueAndStoresFd) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + EXPECT_TRUE(sock.Open("eth0")); + EXPECT_EQ(sock.GetFd(), fake.socket_fd); +} + +TEST(RawSocketTest, Open_NonExistentInterface_ReturnsFalse) +{ + // Uses RealOsSyscalls; ioctl(SIOCGIFINDEX) will fail for unknown iface. + RawSocket sock; + EXPECT_FALSE(sock.Open("nonexistent_eth_zzz")); +} + +TEST(RawSocketTest, Open_NonExistentInterface_GetFdRemainsNegativeOne) +{ + RawSocket sock; + (void)sock.Open("nonexistent_eth_zzz"); + EXPECT_EQ(sock.GetFd(), -1); +} + +// ── RawSocket — Close() ─────────────────────────────────────────────────────── + +TEST(RawSocketTest, Close_AfterOpen_CallsCloseOnFd) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + EXPECT_EQ(sock.GetFd(), fake.socket_fd); + sock.Close(); + EXPECT_EQ(sock.GetFd(), -1); + EXPECT_EQ(fake.close_count, 1); + EXPECT_EQ(fake.last_closed_fd, fake.socket_fd); +} + +TEST(RawSocketTest, Close_CalledTwiceAfterOpen_IsIdempotent) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + sock.Close(); + EXPECT_NO_THROW(sock.Close()); + EXPECT_EQ(sock.GetFd(), -1); + EXPECT_EQ(fake.close_count, 1); // second Close() is a no-op +} + +TEST(RawSocketTest, Destructor_AfterOpen_ClosesSocket) +{ + FakeOsSyscalls fake; + { + RawSocket sock{&fake}; + OpenSocket(sock, fake); + EXPECT_EQ(sock.GetFd(), fake.socket_fd); + } // destructor calls Close() + EXPECT_EQ(fake.close_count, 1); +} + +// ── RawSocket — EnableHwTimestamping() ─────────────────────────────────────── + +TEST(RawSocketTest, EnableHwTimestamping_Success_ReturnsTrue) +{ + FakeOsSyscalls fake; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + EXPECT_TRUE(sock.EnableHwTimestamping()); +} + +TEST(RawSocketTest, EnableHwTimestamping_SiocshwtstampFallback_StillReturnsTrue) +{ + // First SIOCSHWTSTAMP ioctl fails → fallback (second call) is attempted. + FakeOsSyscalls fake; + fake.ioctl_siocshwtstamp_fail = true; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + EXPECT_TRUE(sock.EnableHwTimestamping()); +} + +TEST(RawSocketTest, EnableHwTimestamping_SetsockoptFails_ReturnsFalse) +{ + FakeOsSyscalls fake; + fake.setsockopt_fail = true; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + EXPECT_FALSE(sock.EnableHwTimestamping()); +} + +// ── RawSocket — Recv() ──────────────────────────────────────────────────────── + +TEST(RawSocketTest, Recv_PollTimeout_ReturnsZero) +{ + FakeOsSyscalls fake; + fake.poll_result = 0; // timeout + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[64] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 10), 0); +} + +TEST(RawSocketTest, Recv_PollError_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + fake.poll_result = -1; // poll error + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[64] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 10), -1); +} + +TEST(RawSocketTest, Recv_RecvmsgFails_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + fake.poll_result = 1; + fake.recvmsg_result = -1; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[64] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 10), -1); +} + +TEST(RawSocketTest, Recv_Success_NoTimestamp_ReturnsLen) +{ + FakeOsSyscalls fake; + fake.poll_result = 1; + fake.recvmsg_result = 14; // 14 bytes received + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[256] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 10), 14); + EXPECT_EQ(hwts.tv_sec, 0); + EXPECT_EQ(hwts.tv_nsec, 0); +} + +TEST(RawSocketTest, Recv_WithSoTimestampingCmsg_ExtractsHwts) +{ + FakeOsSyscalls fake; + fake.poll_result = 1; + fake.recvmsg_result = 14; + fake.recvmsg_fill_hwts = true; // inject ts[2]={1, 500_000_000} + RawSocket sock{&fake}; + OpenSocket(sock, fake); + std::uint8_t buf[256] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Recv(buf, sizeof(buf), hwts, 10), 14); + EXPECT_EQ(hwts.tv_sec, 1); + EXPECT_EQ(hwts.tv_nsec, 500'000'000L); +} + +// ── RawSocket — Send() ──────────────────────────────────────────────────────── + +TEST(RawSocketTest, Send_SendFails_ReturnsNegativeOne) +{ + FakeOsSyscalls fake; + fake.send_result = -1; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[14] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 14, hwts), -1); +} + +TEST(RawSocketTest, Send_Success_PollNoTxTs_ReturnsSentBytes) +{ + FakeOsSyscalls fake; + fake.send_result = 14; + fake.poll_result = 0; // no TX-timestamp event + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[14] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 14, hwts), 14); + EXPECT_EQ(hwts.tv_sec, 0); + EXPECT_EQ(hwts.tv_nsec, 0); +} + +TEST(RawSocketTest, Send_Success_TxTimestampCmsg_ExtractsHwts) +{ + // poll returns POLLERR → MSG_ERRQUEUE recvmsg fills SO_TIMESTAMPING cmsg. + FakeOsSyscalls fake; + fake.send_result = 14; + fake.poll_result = 1; + fake.poll_revents = POLLERR; + fake.tx_result = 14; // TX recvmsg succeeds + fake.tx_fill_hwts = true; // inject ts[2]={1,500_000_000} + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[14] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 14, hwts), 14); + EXPECT_EQ(hwts.tv_sec, 1); + EXPECT_EQ(hwts.tv_nsec, 500'000'000L); +} + +TEST(RawSocketTest, Send_DrainErrQueue_WhileBodyExecuted) +{ + // recvmsg_drain_limit=1 → first MSG_ERRQUEUE call returns 1 (while body runs), + // second returns -1 → loop exits. Covers DrainErrQueue's while body. + FakeOsSyscalls fake; + fake.send_result = 14; + fake.poll_result = 0; + fake.errqueue_drain_count = 1; + RawSocket sock{&fake}; + OpenSocket(sock, fake); + const std::uint8_t data[14] = {}; + ::timespec hwts{}; + EXPECT_EQ(sock.Send(data, 14, hwts), 14); + // recvmsg was called: 1 DrainErrQueue call (returns 1) + 1 call (returns -1) + EXPECT_GE(fake.recvmsg_call_count, 2); +} + +// ── NetworkIdentity ─────────────────────────────────────────────────────────── + +TEST(NetworkIdentityTest, GetClockIdentity_BeforeResolve_ReturnsZeroIdentity) +{ + NetworkIdentity ni; + const ClockIdentity id = ni.GetClockIdentity(); + for (const std::uint8_t b : id.id) + { + EXPECT_EQ(b, 0U); + } +} + +TEST(NetworkIdentityTest, Resolve_NonExistentInterface_ReturnsFalse) +{ + NetworkIdentity ni; + EXPECT_FALSE(ni.Resolve("nonexistent_eth_zzz")); +} + +TEST(NetworkIdentityTest, Resolve_NonExistentInterface_GetClockIdentityRemainsZero) +{ + NetworkIdentity ni; + (void)ni.Resolve("nonexistent_eth_zzz"); + const ClockIdentity id = ni.GetClockIdentity(); + for (const std::uint8_t b : id.id) + { + EXPECT_EQ(b, 0U); + } +} + +TEST(NetworkIdentityTest, Resolve_LoInterface_ReturnsTrue) +{ + // lo has MAC 00:00:00:00:00:00; the EUI-48→EUI-64 conversion inserts + // 0xFF 0xFE at positions 3–4 regardless of the MAC value. + NetworkIdentity ni; + EXPECT_TRUE(ni.Resolve("lo")); +} + +TEST(NetworkIdentityTest, GetClockIdentity_AfterResolveOnLo_HasFfFeBytes) +{ + NetworkIdentity ni; + ASSERT_TRUE(ni.Resolve("lo")); + const ClockIdentity id = ni.GetClockIdentity(); + // EUI-48 → EUI-64: bytes 3 and 4 must be 0xFF and 0xFE + EXPECT_EQ(id.id[3], 0xFFU); + EXPECT_EQ(id.id[4], 0xFEU); +} + +TEST(NetworkIdentityTest, Resolve_CalledTwice_SecondCallSucceeds) +{ + NetworkIdentity ni; + ASSERT_TRUE(ni.Resolve("lo")); + EXPECT_TRUE(ni.Resolve("lo")); +} + +} // namespace details +} // namespace ts +} // namespace score 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..e3cd0dc --- /dev/null +++ b/score/TimeSlave/code/gptp/details/sync_state_machine.cpp @@ -0,0 +1,161 @@ +/******************************************************************************** + * 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 "score/TimeSlave/code/gptp/details/clock_util.h" + +namespace score +{ +namespace ts +{ +namespace details +{ + + +SyncStateMachine::SyncStateMachine(std::int64_t jump_future_threshold_ns) noexcept + : jump_future_threshold_ns_{jump_future_threshold_ns}, + created_mono_ns_{MonoNs()} +{ +} + +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) + { + const std::int64_t start = created_mono_ns_.load(std::memory_order_relaxed); + return (mono_now_ns - start) > timeout_ns; + } + 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; + r.sync_mono_ns = sync.recvMonoNs; + + if (has_previous_master_) + { + 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; + } + + const auto to_u64 = [](std::int64_t v) noexcept -> std::uint64_t { + return v >= 0 ? static_cast(v) : 0U; + }; + + score::ts::GptpIpcSyncFupData& d = r.sync_fup_data; + d.precise_origin_timestamp = to_u64(fup_ts.ns); + d.reference_global_timestamp = to_u64(master_ns); + d.reference_local_timestamp = to_u64(sync.recvHardwareTS.ns); + d.sync_ingress_timestamp = to_u64(sync.recvHardwareTS.ns); + d.correction_field = to_u64(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_; + // Both intervals must be strictly positive: a non-positive slave_interval + // indicates a HW timestamp rollback or clock step, which would produce a + // nonsensical (negative or zero) rate ratio published to PtpTimeInfo. + if (master_interval > 0 && slave_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; + has_previous_master_ = true; + + 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..4a0d479 --- /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/libTSClient/gptp_ipc_data.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 + std::int64_t sync_mono_ns{0}; ///< CLOCK_MONOTONIC when the Sync frame was received + score::ts::GptpIpcSyncFupData sync_fup_data{}; ///< Ready to copy into GptpIpcData (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}; + bool has_previous_master_{false}; + std::int64_t jump_future_threshold_ns_; + + std::int64_t prev_slave_rx_ns_{0}; + std::int64_t prev_master_origin_ns_{0}; + double neighbor_rate_ratio_{1.0}; + + std::atomic last_sync_mono_ns_{0}; + std::atomic created_mono_ns_; +}; + +} // 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..157e3d0 --- /dev/null +++ b/score/TimeSlave/code/gptp/details/sync_state_machine_test.cpp @@ -0,0 +1,231 @@ +/******************************************************************************** + * 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 + std::ignore = 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) +{ + // Before first sync, IsTimeout uses the object creation time as baseline. + // Passing now=0 gives (0 - created_mono_ns_) which is negative → not > threshold. + EXPECT_FALSE(ssm_.IsTimeout(0LL, 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..782725c --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine.cpp @@ -0,0 +1,347 @@ +/******************************************************************************** + * 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/clock_util.h" +#include "score/TimeSlave/code/gptp/details/network_identity.h" +#include "score/TimeSlave/code/gptp/details/raw_socket.h" + +#include "score/TimeSlave/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; + +} // namespace + +GptpEngine::GptpEngine(GptpEngineOptions opts) noexcept + : opts_{std::move(opts)}, + socket_{std::make_unique()}, + identity_{std::make_unique()}, + codec_{}, + parser_{}, + sync_sm_{opts_.jump_future_threshold_ns}, + pdelay_{nullptr}, + phc_{opts_.phc_config} +{ +} + +GptpEngine::GptpEngine(GptpEngineOptions opts, + std::unique_ptr socket, + std::unique_ptr identity) noexcept + : opts_{std::move(opts)}, + socket_{std::move(socket)}, + identity_{std::move(identity)}, + codec_{}, + parser_{}, + sync_sm_{opts_.jump_future_threshold_ns}, + pdelay_{nullptr}, + phc_{opts_.phc_config} +{ +} + +GptpEngine::~GptpEngine() noexcept +{ + Deinitialize(); +} + +bool GptpEngine::Initialize() +{ + if (running_.load(std::memory_order_acquire)) + return true; + + if (!identity_->Resolve(opts_.iface_name)) + { + score::mw::log::LogError(kTimeSlaveAppContext) + << "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(kTimeSlaveAppContext) + << "GptpEngine: failed to open raw socket on " << opts_.iface_name; + return false; + } + + if (!socket_->EnableHwTimestamping()) + { + score::mw::log::LogWarn(kTimeSlaveAppContext) + << "GptpEngine: HW timestamping not available on " << opts_.iface_name << ", falling back to SW timestamps"; + } + + running_.store(true, std::memory_order_release); + + try + { + // std::thread constructor throws std::system_error if the OS cannot + // create a new thread (e.g. EAGAIN — thread limit reached). + rx_thread_ = std::thread([this]() noexcept { RxLoop(); }); + } + catch (const std::system_error& e) + { + score::mw::log::LogError(kTimeSlaveAppContext) << "GptpEngine: failed to create RxThread: " << std::string_view{e.what()}; + running_.store(false, std::memory_order_release); + socket_->Close(); + return false; + } + + try + { + pdelay_thread_ = std::thread([this]() noexcept { PdelayLoop(); }); + } + catch (const std::system_error& e) + { + score::mw::log::LogError(kTimeSlaveAppContext) << "GptpEngine: failed to create PdelayThread: " << std::string_view{e.what()}; + Deinitialize(); + return false; + } + + score::mw::log::LogInfo(kTimeSlaveAppContext) << "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_thread_.joinable()) + rx_thread_.join(); + if (pdelay_thread_.joinable()) + pdelay_thread_.join(); + + score::mw::log::LogInfo(kTimeSlaveAppContext) << "GptpEngine deinitialized"; + return true; +} + +void GptpEngine::FinalizeSnapshot() noexcept +{ + if (!running_.load(std::memory_order_acquire)) + return; + + const std::int64_t mono_now = MonoNs(); + const std::int64_t timeout_ns = static_cast(opts_.sync_timeout_ms) * 1'000'000LL; + + std::lock_guard lk(snapshot_mutex_); + const bool timed_out = sync_sm_.IsTimeout(mono_now, timeout_ns); + current_snapshot_ = pending_snapshot_; + if (timed_out) + { + current_snapshot_.status.is_synchronized = false; + current_snapshot_.status.is_timeout = true; + current_snapshot_.status.is_correct = false; + } +} + +bool GptpEngine::ReadPTPSnapshot(score::ts::GptpIpcData& data) const noexcept +{ + if (!running_.load(std::memory_order_acquire)) + return false; + + std::lock_guard lk(snapshot_mutex_); + data = current_snapshot_; + return true; +} + +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{}; + if (::clock_gettime(CLOCK_MONOTONIC, &next) != 0) + { + score::mw::log::LogError(kGPtpMachineContext) + << "GptpEngine: clock_gettime failed in PdelayLoop, thread exiting"; + return; + } + // 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)) + { + const std::int64_t target_ns = + static_cast(next.tv_sec) * 1'000'000'000LL + next.tv_nsec; + + while (running_.load(std::memory_order_acquire)) + { + const std::int64_t remaining = target_ns - MonoNs(); + if (remaining <= 0) + break; + constexpr std::int64_t kSliceNs = 50'000'000LL; + const std::int64_t sleep_ns = remaining < kSliceNs ? remaining : kSliceNs; + const ::timespec slice{0, static_cast(sleep_ns)}; + ::clock_nanosleep(CLOCK_MONOTONIC, 0, &slice, nullptr); + } + + if (!running_.load(std::memory_order_acquire)) + break; + + if (pdelay_) + { + (void)pdelay_->SendRequest(*socket_); + } + + const std::int64_t next_ns = target_ns + 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; + msg.recvMonoNs = MonoNs(); + 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.requestReceiptTimestamp); + 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 +{ + const double rate_ratio = sync_sm_.GetNeighborRateRatio(); + + { + std::lock_guard lk(snapshot_mutex_); + + const std::int64_t local_rx_ns = static_cast(sync.sync_fup_data.reference_local_timestamp); + pending_snapshot_.ptp_assumed_time = std::chrono::nanoseconds{local_rx_ns - sync.offset_ns}; + pending_snapshot_.local_time = std::chrono::nanoseconds{sync.sync_mono_ns}; + pending_snapshot_.rate_deviation = rate_ratio; + + pending_snapshot_.status.is_synchronized = true; + pending_snapshot_.status.is_timeout = false; + pending_snapshot_.status.is_time_jump_future = sync.is_time_jump_future; + pending_snapshot_.status.is_time_jump_past = sync.is_time_jump_past; + pending_snapshot_.status.is_correct = !sync.is_time_jump_future && !sync.is_time_jump_past; + + pending_snapshot_.sync_fup_data = sync.sync_fup_data; + pending_snapshot_.pdelay_data = pdelay.pdelay_data; + } + + if (phc_.IsEnabled()) + { + const bool is_step = + (sync.offset_ns >= opts_.phc_config.step_threshold_ns) || + (sync.offset_ns <= -opts_.phc_config.step_threshold_ns); + + phc_.AdjustOffset(sync.offset_ns); + phc_.AdjustFrequency(rate_ratio); + + if (is_step) + { + score::mw::log::LogInfo(kGPtpMachineContext) + << "PHC step applied: offset=" << sync.offset_ns << " ns"; + } + else + { + score::mw::log::LogInfo(kGPtpMachineContext) + << "PHC slew: offset=" << sync.offset_ns << " ns" + << " rate_ratio=" << rate_ratio; + } + } +} + +} // 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..c30e2f2 --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine.h @@ -0,0 +1,130 @@ +/******************************************************************************** + * 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/libTSClient/gptp_ipc_data.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 "score/TimeSlave/code/gptp/phc/phc_adjuster.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Configuration for GptpEngine. +struct GptpEngineOptions +{ + std::string iface_name = "emac0"; ///< 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 + PhcConfig phc_config{}; ///< PHC hardware clock adjustment (disabled by default) +}; + +/** + * @brief gPTP engine for the TimeSlave process. + * + * Runs two POSIX threads: RxThread (receive/parse PTP frames) and + * PdelayThread (periodic Pdelay_Req transmission). + * + * Dual-snapshot design: + * - pending_snapshot_: filled by the RxThread on every Sync+FollowUp + * - current_snapshot_: a committed, fully-flagged snapshot + * + * Callers should: + * 1. Call FinalizeSnapshot() to check timeout and commit pending to current. + * 2. Call ReadPTPSnapshot() (const) to retrieve the current snapshot. + */ +class GptpEngine final +{ + public: + explicit GptpEngine(GptpEngineOptions opts) noexcept; + + /// Constructor for testing: inject fake socket and identity. + GptpEngine(GptpEngineOptions opts, + 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(); + + /// Check for sync timeout, apply status flags, and commit pending_snapshot_ + /// to current_snapshot_. Must be called periodically before ReadPTPSnapshot(). + void FinalizeSnapshot() noexcept; + + /// Copy the latest committed snapshot into @p data. + /// Non-blocking; returns false only if the engine is not initialized. + bool ReadPTPSnapshot(score::ts::GptpIpcData& data) const noexcept; + + private: + 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 socket_; + std::unique_ptr identity_; + FrameCodec codec_; + GptpMessageParser parser_; + SyncStateMachine sync_sm_; + std::unique_ptr pdelay_; + PhcAdjuster phc_; + + mutable std::mutex snapshot_mutex_; + score::ts::GptpIpcData pending_snapshot_{}; ///< Filled by RxThread on Sync+FollowUp + score::ts::GptpIpcData current_snapshot_{}; ///< Committed by FinalizeSnapshot() + + std::atomic running_{false}; + std::thread rx_thread_; + std::thread pdelay_thread_; +}; + +} // 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..6579d5c --- /dev/null +++ b/score/TimeSlave/code/gptp/gptp_engine_test.cpp @@ -0,0 +1,600 @@ +/******************************************************************************** + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +namespace +{ + +// ── 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(); + } + + void SetOpenOk(bool v) + { + open_ok_ = v; + } + + bool Open(const std::string&) override + { + return open_ok_; + } + 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}; + bool open_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::ts::GptpIpcData data{}; + eng.FinalizeSnapshot(); + eng.ReadPTPSnapshot(data); + if (data.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()); + } + + 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::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::ts::GptpIpcData data{}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(data)); +} + +TEST_F(GptpEngineTest, ReadPTPSnapshot_InfoUnchanged_WhenNotInitialized) +{ + score::ts::GptpIpcData data{}; + data.ptp_assumed_time = std::chrono::nanoseconds{999LL}; + EXPECT_FALSE(engine_->ReadPTPSnapshot(data)); + EXPECT_EQ(data.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()); + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + EXPECT_TRUE(engine_->ReadPTPSnapshot(data)); +} + +TEST_F(GptpEngineFakeTest, ReadPTPSnapshot_NotSynchronized_BeforeAnySync) +{ + ASSERT_TRUE(engine_->Initialize()); + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(data)); + EXPECT_FALSE(data.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::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_)); + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(data)); + EXPECT_TRUE(data.status.is_synchronized); + EXPECT_FALSE(data.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::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::ts::GptpIpcData tmp{}; + eng.FinalizeSnapshot(); + 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::ts::GptpIpcData info{}; + eng.FinalizeSnapshot(); + 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}; + EXPECT_FALSE(eng.Initialize()); + EXPECT_TRUE(eng.Deinitialize()); +} + +// ── Socket-open-fail path (lines 87–90 of gptp_engine.cpp) ─────────────────── + +TEST(GptpEngineSocketFailTest, Initialize_SocketOpenFails_ReturnsFalse) +{ + auto sock = std::make_unique(); + auto identity = std::make_unique(); + sock->SetOpenOk(false); + GptpEngine eng{FastOptions(), std::move(sock), std::move(identity)}; + EXPECT_FALSE(eng.Initialize()); + EXPECT_TRUE(eng.Deinitialize()); +} + +// ── Time-jump detection (UpdateSnapshot lines 301–303) ─────────────────────── + +namespace +{ + +bool WaitForFlag(GptpEngine& eng, bool (*pred)(const score::ts::GptpIpcData&), int max_ms = 1000) +{ + for (int i = 0; i < max_ms / 10; ++i) + { + score::ts::GptpIpcData data{}; + eng.FinalizeSnapshot(); + eng.ReadPTPSnapshot(data); + if (pred(data)) + return true; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return false; +} + +} // namespace + +TEST_F(GptpEngineFakeTest, HandlePacket_TwoSyncFup_TimeJumpFuture_Detected) +{ + ASSERT_TRUE(engine_->Initialize()); + + // Pair 1: master_ns ≈ 2 s + ::timespec hwts1{1, 0}; + socket_raw_->Push(MakeSyncFrame(1U), hwts1); + socket_raw_->Push(MakeFollowUpFrame(1U, /*sec=*/2U, /*ns=*/0U)); + + // Pair 2: master_ns ≈ 3 s (delta = 1 s > 500 ms threshold → is_time_jump_future) + ::timespec hwts2{2, 0}; + socket_raw_->Push(MakeSyncFrame(2U), hwts2); + socket_raw_->Push(MakeFollowUpFrame(2U, /*sec=*/3U, /*ns=*/0U)); + + const bool got = + WaitForFlag(*engine_, [](const score::ts::GptpIpcData& d) { return d.status.is_time_jump_future; }); + EXPECT_TRUE(got); + + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(data)); + EXPECT_TRUE(data.status.is_time_jump_future); + EXPECT_FALSE(data.status.is_correct); +} + +TEST_F(GptpEngineFakeTest, HandlePacket_TwoSyncFup_TimeJumpPast_Detected) +{ + ASSERT_TRUE(engine_->Initialize()); + + // Pair 1: master_ns ≈ 3 s + ::timespec hwts1{1, 0}; + socket_raw_->Push(MakeSyncFrame(1U), hwts1); + socket_raw_->Push(MakeFollowUpFrame(1U, /*sec=*/3U, /*ns=*/0U)); + + // Pair 2: master_ns ≈ 2 s (delta < 0 → is_time_jump_past) + ::timespec hwts2{2, 0}; + socket_raw_->Push(MakeSyncFrame(2U), hwts2); + socket_raw_->Push(MakeFollowUpFrame(2U, /*sec=*/2U, /*ns=*/0U)); + + const bool got = + WaitForFlag(*engine_, [](const score::ts::GptpIpcData& d) { return d.status.is_time_jump_past; }); + EXPECT_TRUE(got); + + engine_->FinalizeSnapshot(); + score::ts::GptpIpcData data{}; + ASSERT_TRUE(engine_->ReadPTPSnapshot(data)); + EXPECT_TRUE(data.status.is_time_jump_past); + EXPECT_FALSE(data.status.is_correct); +} + +} // 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..0c63e06 --- /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/TimeSlave/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..70bb781 --- /dev/null +++ b/score/TimeSlave/code/gptp/instrument/probe.cpp @@ -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 + ********************************************************************************/ +#include "score/TimeSlave/code/gptp/instrument/probe.h" + +#include "score/TimeSlave/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::ts::kGPtpMachineContext) + << "PROBE point=" << static_cast(point) << " ts=" << data.ts_mono_ns << " val=" << data.value_ns + << " seq=" << data.seq_id; + + Recorder* const rec = recorder_.load(std::memory_order_acquire); + if (rec != nullptr && rec->IsEnabled()) + { + rec->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{}; + if (::clock_gettime(CLOCK_MONOTONIC, &ts) != 0) + return 0; + return static_cast(ts.tv_sec) * 1'000'000'000LL + static_cast(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..8e03863 --- /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_.store(recorder, std::memory_order_release); + } + + /// Record a probe event. Thread-safe. + void Trace(ProbePoint point, const ProbeData& data); + + private: + ProbeManager() = default; + std::atomic enabled_{false}; + std::atomic 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..ca3a074 --- /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() noexcept; + + 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..733c6c7 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/phc_adjuster.cpp @@ -0,0 +1,116 @@ +/******************************************************************************** + * 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 +#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). +clockid_t phc_fd_to_clockid(int fd) +{ + return static_cast((~static_cast(fd) << 3U) | 3U); +} + +} // 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() noexcept +{ + 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; + + if (!std::isfinite(rate_ratio) || rate_ratio < 0.5 || rate_ratio > 2.0) + return; + + const double ppb = (rate_ratio - 1.0) * 1e9; + const double raw_scaled = ppb / 1000.0 * 65536.0; + constexpr double kMaxScaled = 33'554'432.0; + const double clamped = raw_scaled < -kMaxScaled ? -kMaxScaled : (raw_scaled > kMaxScaled ? kMaxScaled : raw_scaled); + const long scaled_ppm = static_cast(clamped); + + 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..5a71e73 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/linux/raw_socket.cpp @@ -0,0 +1,230 @@ +/******************************************************************************** + * 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, IOsSyscalls& sys) 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 (sys.recvmsg_call(fd, &msg, MSG_ERRQUEUE) > 0) + { + } +} + +} // namespace + +RawSocket::RawSocket(IOsSyscalls* sys) noexcept + : sys_{sys != nullptr ? sys : &RealOsSyscalls::Instance()} +{ +} + +RawSocket::~RawSocket() +{ + Close(); +} + +bool RawSocket::Open(const std::string& iface) +{ + Close(); + + const int fd = sys_->socket_call(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); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (sys_->ioctl_call(fd, SIOCGIFINDEX, &ifr) < 0) + { + sys_->close_call(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 (sys_->bind_call(fd, reinterpret_cast<::sockaddr*>(&sa), sizeof(sa)) < 0) + { + sys_->close_call(fd); + return false; + } + + // SO_BINDTODEVICE: best-effort, don't fail if it doesn't work + (void)sys_->setsockopt_call( + fd, SOL_SOCKET, SO_BINDTODEVICE, iface.c_str(), static_cast(iface.size())); + + fd_.store(fd, std::memory_order_release); + iface_ = iface; + return true; +} + +bool RawSocket::EnableHwTimestamping() +{ + const int fd = fd_.load(std::memory_order_relaxed); + if (fd < 0) + return false; + + ::ifreq ifr{}; + ::hwtstamp_config cfg{}; + std::strncpy(ifr.ifr_name, iface_.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_data = reinterpret_cast(&cfg); + + cfg.tx_type = HWTSTAMP_TX_ON; + cfg.rx_filter = HWTSTAMP_FILTER_ALL; + + if (sys_->ioctl_call(fd, SIOCSHWTSTAMP, &ifr) < 0) + { + // Fall back to PTP-only filter + cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; + (void)sys_->ioctl_call(fd, SIOCSHWTSTAMP, &ifr); + } + + const int ts_opts = + SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE; + if (sys_->setsockopt_call(fd, SOL_SOCKET, SO_TIMESTAMPING, &ts_opts, sizeof(ts_opts)) < 0) + { + return false; + } + return true; +} + +void RawSocket::Close() +{ + const int fd = fd_.exchange(-1, std::memory_order_acq_rel); + if (fd >= 0) + sys_->close_call(fd); + iface_.clear(); +} + +int RawSocket::Recv(std::uint8_t* buf, std::size_t buf_len, ::timespec& hwts, int timeout_ms) +{ + const int fd = fd_.load(std::memory_order_acquire); + if (fd < 0 || buf == nullptr || buf_len == 0) + return -1; + + // Poll with caller-specified timeout + ::pollfd pfd{fd, POLLIN, 0}; + const int pr = sys_->poll_call(&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(sys_->recvmsg_call(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) + { + if (cm->cmsg_len < CMSG_LEN(3 * sizeof(::timespec))) + continue; + 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) +{ + const int fd = fd_.load(std::memory_order_acquire); + if (fd < 0 || buf == nullptr || len <= 0) + return -1; + + DrainErrQueue(fd, *sys_); + + const int sent = static_cast(sys_->send_call(fd, buf, static_cast(len), 0)); + if (sent < 0) + return -1; + + constexpr int kTxTsTimeoutMs = 50; + ::pollfd pfd{fd, POLLERR, 0}; + std::memset(&hwts, 0, sizeof(hwts)); + if (sys_->poll_call(&pfd, 1, kTxTsTimeoutMs) > 0 && (pfd.revents & POLLERR) != 0) + { + std::uint8_t tmp[2048]; + ::iovec iov{tmp, sizeof(tmp)}; + char ctrl[512]; + ::msghdr msg{}; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = ctrl; + msg.msg_controllen = sizeof(ctrl); + + if (sys_->recvmsg_call(fd, &msg, MSG_ERRQUEUE) >= 0) + { + for (::cmsghdr* cm = CMSG_FIRSTHDR(&msg); cm != nullptr; cm = CMSG_NXTHDR(&msg, cm)) + { + if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMPING) + { + // SO_TIMESTAMPING delivers three timespec values: [0]=SW, [1]=HW-transformed, + // [2]=HW-raw. Verify the cmsg payload is large enough before indexing ts[2]. + if (cm->cmsg_len < CMSG_LEN(3 * sizeof(::timespec))) + continue; + const auto* ts = reinterpret_cast(CMSG_DATA(cm)); + if (ts[2].tv_sec != 0 || ts[2].tv_nsec != 0) + hwts = ts[2]; + } + } + } + } + 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..6110cb3 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/qnx_raw_shim.cpp @@ -0,0 +1,614 @@ +/******************************************************************************** + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PTP_GET_TIME 0x102 +#define PTP_SET_TIME 0x103 +// EMAC_PTP_ADJ_FREQ_PPM: Qualcomm BSP (hw/iosock/emac_ioctl.h), ptp_ppm_t = int +// Positive ppm = speed up PHC, negative = slow down. +static constexpr unsigned long kEmacPtpAdjFreqPpm = 52UL; +struct ptp_time +{ + int64_t sec; + int32_t nsec; +}; + +struct ptp_tstmp +{ + struct + { + std::int64_t sec; // EMAC PHC TX hardware timestamp seconds, offset 0 + std::int32_t nsec; // EMAC PHC TX hardware timestamp nanoseconds, offset 8 + // implicit 4-byte trailing pad: sizeof(this struct) = 16 + } time; + std::uint32_t uid; // per-TX frame matching uid (BIOCGTSTAMPID), offset 16 + // implicit 4-byte trailing pad: sizeof(ptp_tstmp) = 24 +}; + + +#ifndef ETH_P_8021Q +#define ETH_P_8021Q 0x8100U +#endif +#ifndef ETH_P_1588 +#define ETH_P_1588 0x88F7U +#endif + +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; + +// PHC frequency adjustment state (PI controller). +// g_skip_freq_after_step: skip N cycles after a step correction so the +// clock can stabilise before re-applying rate-ratio based slewing. +// g_smoothed_comp_ppb: P term — EMA of raw_ppb (α=0.2), fast convergence. +// g_integral_ppb: I term — slow integrator of P, eliminates the E/2 +// steady-state error that a pure P/EMA controller leaves behind. +static int g_skip_freq_after_step = 0; +static double g_smoothed_comp_ppb = 0.0; // P term: EMA of raw_ppb (ppb) +static double g_integral_ppb = 0.0; // I term: integrator (ppb) + +static_assert(sizeof(ptp_tstmp) == 24U, "ptp_tstmp: time{sec:8+nsec:4+pad:4}=16 + uid:4 + pad:4 = 24"); +static constexpr int kTxLoopbackCaplen = static_cast(sizeof(GptpEthHdr) + sizeof(ptp_tstmp)); + +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])); + +static struct bpf_insn kPdelayReqFilterInsns[] = { + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12), // load EtherType + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETH_P_1588, 0, 4), // != 0x88F7 → FAIL + BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 14), // load PTP tsmt byte + BPF_STMT(BPF_ALU + BPF_AND + BPF_K, 0x0FU), // mask message type + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x02U, 0, 1), // != Pdelay_Req → FAIL + BPF_STMT(BPF_RET + BPF_K, static_cast(-1)), // PASS + BPF_STMT(BPF_RET + BPF_K, 0U), // FAIL +}; +static const u_int kPdelayReqFilterLen = + static_cast(sizeof(kPdelayReqFilterInsns) / sizeof(kPdelayReqFilterInsns[0])); + +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]{}; + + int promisc_sock = -1; + + int tx_lb_fd = -1; + u_int tx_lb_buflen = 0; + unsigned char tx_lb_buf[kMaxBpfBufSz]{}; + + std::atomic inject_t1_ns{-1LL}; + + ~QnxRawContext() + { + if (bpf_fd >= 0) { ::close(bpf_fd); bpf_fd = -1; } + if (tx_lb_fd >= 0) { ::close(tx_lb_fd); tx_lb_fd = -1; } + if (promisc_sock >= 0) { ::close(promisc_sock); promisc_sock = -1; } + } +}; + +static QnxRawContext g_qnx_ctx; + +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); +} + +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; +} + +static void join_eth_multicast(const char* ifname, const unsigned char mac[6]) noexcept +{ + int s = ::socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) return; + ::ifreq ifr{}; + ::strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + ifr.ifr_addr.sa_len = static_cast(1U + 1U + ETHER_ADDR_LEN); + ifr.ifr_addr.sa_family = AF_UNSPEC; + std::memcpy(ifr.ifr_addr.sa_data, mac, 6); + (void)::ioctl(s, SIOCADDMULTI, &ifr); + ::close(s); +} + +static int set_iface_promisc(const char* ifname) noexcept +{ + int s = ::socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) return -1; + ::ifreq ifr{}; + ::strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + if (::ioctl(s, SIOCGIFFLAGS, &ifr) == 0) + { + ifr.ifr_flags |= IFF_PROMISC | IFF_ALLMULTI; + (void)::ioctl(s, SIOCSIFFLAGS, &ifr); + } + return s; // keep open — closed in ~QnxRawContext() +} + +static int open_tx_loopback_fd(const char* ifname) noexcept +{ + 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"); + + const int fd = ::open(devpath, O_RDWR); + if (fd < 0) return -1; + + ::ifreq ifr{}; + ::strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + if (::ioctl(fd, BIOCSETIF, &ifr) < 0) { ::close(fd); return -1; } + + u_int one = 1U; + (void)::ioctl(fd, BIOCSSEESENT, &one); // capture TX frames + (void)::ioctl(fd, BIOCIMMEDIATE, &one); // no batching delay + + u_int bpf_ts = BPF_T_BINTIME | BPF_T_PTP; + (void)::ioctl(fd, BIOCSTSTAMP, &bpf_ts); + + struct bpf_program prog{kPdelayReqFilterLen, kPdelayReqFilterInsns}; + if (::ioctl(fd, BIOCSETF, &prog) < 0) { ::close(fd); return -1; } + + u_int buflen = 0U; + if (::ioctl(fd, BIOCGBLEN, &buflen) < 0 || buflen > kMaxBpfBufSz) + { + ::close(fd); + return -1; + } + g_qnx_ctx.tx_lb_buflen = buflen; + return fd; +} + +extern "C" int qnx_raw_open(const char* ifname) +{ + if (ifname == nullptr) { errno = EINVAL; return -1; } + + ::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{}; + ::strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + if (::ioctl(fd, BIOCSETIF, &ifr) < 0) { ::close(fd); return -1; } + + u_int seesent = 0U; + (void)::ioctl(fd, BIOCSSEESENT, &seesent); + + u_int yes = 1U; + (void)::ioctl(fd, BIOCIMMEDIATE, &yes); + + g_qnx_ctx.promisc_sock = set_iface_promisc(ifname); + (void)::ioctl(fd, BIOCPROMISC, &yes); + + u_int bpf_ts = BPF_T_BINTIME | BPF_T_PTP; + (void)::ioctl(fd, BIOCSTSTAMP, &bpf_ts); + + struct bpf_program prog{kPtp1588FilterLen, kPtp1588FilterInsns}; + if (::ioctl(fd, BIOCSETF, &prog) < 0) { ::close(fd); return -1; } + + 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; + + g_qnx_ctx.tx_lb_fd = open_tx_loopback_fd(ifname); + + static const unsigned char kPtpP2PMac[6] = {0x01U, 0x80U, 0xC2U, 0x00U, 0x00U, 0x0EU}; + static const unsigned char kPtp1588Mac[6] = {0x01U, 0x1BU, 0x19U, 0x00U, 0x00U, 0x00U}; + join_eth_multicast(ifname, kPtpP2PMac); + join_eth_multicast(ifname, kPtp1588Mac); + + 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 (;;) + { + if (g_qnx_ctx.bpf_off >= g_qnx_ctx.bpf_n) + { + if (nonblock == 0) + { + struct pollfd pfd{fd, POLLIN, 0}; + const int pr = ::poll(&pfd, 1, 100); + if (pr < 0) return -1; + if (pr == 0) { errno = ETIMEDOUT; return -1; } + if ((pfd.revents & (POLLERR | POLLHUP | POLLNVAL)) != 0) return -1; + } + + const 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; + } + + static constexpr ssize_t kBhHdrMinBytes = + static_cast(offsetof(bpf_xhdr, bh_hdrlen)) + + static_cast(sizeof(u_short)); // = 26 + + if (g_qnx_ctx.bpf_off + kBhHdrMinBytes > g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + const unsigned char* bh_raw = g_qnx_ctx.bpf_buf + g_qnx_ctx.bpf_off; + bpf_u_int32 bh_caplen = 0; + u_short bh_hdrlen = 0; + std::memcpy(&bh_caplen, bh_raw + offsetof(bpf_xhdr, bh_caplen), sizeof(bpf_u_int32)); + std::memcpy(&bh_hdrlen, bh_raw + offsetof(bpf_xhdr, bh_hdrlen), sizeof(u_short)); + + if (bh_hdrlen < static_cast(kBhHdrMinBytes) || + bh_caplen > static_cast(g_qnx_ctx.bpf_n) || + g_qnx_ctx.bpf_off + static_cast(bh_hdrlen) + + static_cast(bh_caplen) > g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + const unsigned char* pkt = bh_raw + bh_hdrlen; + const int caplen = static_cast(bh_caplen); + const ssize_t next_off = + g_qnx_ctx.bpf_off + static_cast(BPF_WORDALIGN(bh_hdrlen + bh_caplen)); + + if (caplen == kTxLoopbackCaplen) + { + ptp_tstmp tstmp{}; + std::memcpy(&tstmp, pkt + sizeof(GptpEthHdr), sizeof(ptp_tstmp)); + const std::int64_t t1_ns = + tstmp.time.sec * kNsPerSec + static_cast(tstmp.time.nsec); + if (t1_ns > 0) + { + g_qnx_ctx.inject_t1_ns.store(t1_ns, std::memory_order_release); + std::fprintf(stderr, "[t1-inject] uid=%u ts=%lld.%09d\n", + tstmp.uid, + static_cast(tstmp.time.sec), + tstmp.time.nsec); + } + g_qnx_ctx.bpf_off = next_off; + continue; + } + + const int ptp_off = ptp_payload_offset(pkt, caplen); + if (ptp_off < 0) + { + g_qnx_ctx.bpf_off = next_off; + continue; + } + + const uint8_t msgtype = static_cast(pkt[ptp_off]) & 0x0Fu; + const auto* bh = reinterpret_cast(bh_raw); + bool t4_set = false; + if (bh->bh_tstamp.bt_sec != 0 || bh->bh_tstamp.bt_frac != 0) + { + bpf_ts_to_timespec(bh, hwts); + t4_set = true; + if (msgtype == 0x03u) + { + std::fprintf(stderr, "[t4] bpf_phc ts=%lld.%09ld\n", + static_cast(hwts->tv_sec), + hwts->tv_nsec); + } + } + // PTP_GET_TIME fallback: use PHC hardware time when no BPF timestamp + // is available, so rate_ratio reflects ΔPHC / ΔCLOCK_REALTIME. + if (!t4_set && g_qnx_ctx.promisc_sock >= 0) + { + 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(g_qnx_ctx.promisc_sock, SIOCGDRVSPEC, &cmd) == 0) + { + hwts->tv_sec = static_cast(cmd.tm.sec); + hwts->tv_nsec = static_cast(cmd.tm.nsec); + t4_set = true; + if (msgtype == 0x03u) + { + std::fprintf(stderr, "[t4] PTP_GET_TIME ts=%lld.%09ld\n", + static_cast(cmd.tm.sec), + static_cast(cmd.tm.nsec)); + } + } + } + if (!t4_set) + { + (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; + } +} + +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)); + + g_qnx_ctx.inject_t1_ns.store(-1LL, std::memory_order_relaxed); + + if (::write(fd, g_qnx_ctx.tx_frame, static_cast(len)) < 0) + return -1; + + for (int i = 0; i < 100; ++i) + { + const std::int64_t t1 = g_qnx_ctx.inject_t1_ns.load(std::memory_order_acquire); + if (t1 > 0) + { + hwts->tv_sec = static_cast(t1 / kNsPerSec); + hwts->tv_nsec = static_cast(t1 % kNsPerSec); + std::fprintf(stderr, "[t1] inject ts=%lld.%09ld\n", + static_cast(hwts->tv_sec), + hwts->tv_nsec); + return len; + } + ::usleep(100U); // 100 µs + } + + if (g_qnx_ctx.promisc_sock >= 0) + { + 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(g_qnx_ctx.promisc_sock, SIOCGDRVSPEC, &cmd) == 0) + { + hwts->tv_sec = static_cast(cmd.tm.sec); + hwts->tv_nsec = static_cast(cmd.tm.nsec); + std::fprintf(stderr, "[t1] PTP_GET ts=%lld.%09ld (inject timeout)\n", + static_cast(hwts->tv_sec), + hwts->tv_nsec); + return len; + } + } + + (void)::clock_gettime(CLOCK_REALTIME, hwts); + std::fprintf(stderr, "[t1] CLOCK_RT ts=%lld.%09ld (fallback)\n", + static_cast(hwts->tv_sec), + static_cast(hwts->tv_nsec)); + return len; +} + +extern "C" int qnx_phc_open(const char* phc_dev) +{ + if (phc_dev != nullptr && phc_dev[0] != '\0' && phc_dev[0] != '/') + ::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, SIOCGDRVSPEC, &cmd); + if (r == 0) + { + std::fprintf(stderr, "[phc-step] offset=%lld ns new=%lld.%09d\n", + static_cast(offset_ns), + static_cast(cmd.tm.sec), + cmd.tm.nsec); + // After a hard step, skip 3 frequency-adjustment cycles and reset + // the smoothed estimate so stale rate data doesn't corrupt slewing. + g_skip_freq_after_step = 3; + g_smoothed_comp_ppb = 0.0; + g_integral_ppb = 0.0; + } + else + { + std::fprintf(stderr, "[phc-step] PTP_SET_TIME failed errno=%d\n", errno); + } + ::close(s); + return r; +} + +extern "C" int qnx_phc_adjfreq_ppb(int /*phc_fd*/, long long freq_ppb) +{ + // Skip a few cycles immediately after a step correction. + if (g_skip_freq_after_step > 0) + { + --g_skip_freq_after_step; + return 0; + } + + constexpr double kAlpha = 0.2; + constexpr double kKi = 0.002; + constexpr double kICap = 300'000.0; // I term anti-windup: ±300 ppm + constexpr double kTotCap = 400'000.0; // combined output cap: ±400 ppm + + // --- P term: EMA of raw_ppb --- + g_smoothed_comp_ppb = kAlpha * static_cast(freq_ppb) + + (1.0 - kAlpha) * g_smoothed_comp_ppb; + + // --- I term: slow integrator of P; clamp to prevent wind-up --- + g_integral_ppb += kKi * g_smoothed_comp_ppb; + if (g_integral_ppb > kICap) g_integral_ppb = kICap; + if (g_integral_ppb < -kICap) g_integral_ppb = -kICap; + + // --- Combined PI output --- + double combined = g_smoothed_comp_ppb + g_integral_ppb; + if (combined > kTotCap) combined = kTotCap; + if (combined < -kTotCap) combined = -kTotCap; + + // ppb → ppm with sign flip: + // positive error = slave running fast → apply negative adj_ppm to slow PHC down + const int adj_ppm = -static_cast(combined / 1000.0); + if (adj_ppm == 0) return 0; // below 1 ppm resolution, skip ioctl + + const int s = ::socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) return -1; + + struct + { + struct ifdrv ifd; + int ppm; + } cmd{}; + std::strncpy(cmd.ifd.ifd_name, g_qnx_ctx.iface_name, sizeof(cmd.ifd.ifd_name) - 1U); + cmd.ifd.ifd_cmd = kEmacPtpAdjFreqPpm; + cmd.ifd.ifd_len = sizeof(int); + cmd.ifd.ifd_data = &cmd.ppm; + cmd.ppm = adj_ppm; + + const int r = ::ioctl(s, SIOCGDRVSPEC, &cmd); + std::fprintf(stderr, "[phc-freq] raw_ppb=%lld P=%.0f I=%.0f adj_ppm=%d r=%d%s\n", + static_cast(freq_ppb), + g_smoothed_comp_ppb, + g_integral_ppb, + adj_ppm, r, + r != 0 ? " (FAILED)" : ""); + ::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..5a9aea1 --- /dev/null +++ b/score/TimeSlave/code/gptp/platform/qnx/raw_socket.cpp @@ -0,0 +1,91 @@ +/******************************************************************************** + * 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(IOsSyscalls* /*sys*/) noexcept {} + +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..006385f --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder.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/record/recorder.h" + +namespace score +{ +namespace ts +{ +namespace details +{ + +Recorder::Recorder(Config cfg) : cfg_{std::move(cfg)}, enabled_{cfg_.enabled} +{ + if (cfg_.enabled) + { + file_.open(cfg_.file_path, std::ios::out | std::ios::app); + if (file_.is_open()) + { + file_.seekp(0, std::ios::end); + if (file_.good() && file_.tellp() == std::streampos{0}) + { + file_ << "mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags\n"; + } + } + } +} + +void Recorder::Record(const RecordEntry& entry) +{ + if (!enabled_.load(std::memory_order_relaxed) || !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'; + + ++flush_counter_; + if (flush_counter_ >= cfg_.flush_interval) + { + file_.flush(); + flush_counter_ = 0U; + if (!file_.good()) + { + enabled_.store(false, std::memory_order_relaxed); + } + } +} + +} // 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..2fc4d95 --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder.h @@ -0,0 +1,93 @@ +/******************************************************************************** + * 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 +#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 + std::uint32_t flush_interval = 8U; + }; + + explicit Recorder(Config cfg); + ~Recorder() = default; + + Recorder(const Recorder&) = delete; + Recorder& operator=(const Recorder&) = delete; + + bool IsEnabled() const + { + return enabled_.load(std::memory_order_relaxed) && file_.is_open(); + } + + /// Record an entry. Thread-safe. + void Record(const RecordEntry& entry); + + private: + Config cfg_; + std::atomic enabled_{false}; + std::mutex mutex_; + std::ofstream file_; + std::uint32_t flush_counter_{0U}; +}; + +} // 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..21ecce2 --- /dev/null +++ b/score/TimeSlave/code/gptp/record/recorder_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/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"); +} + +TEST_F(RecorderFileTest, ExistingFile_HeaderNotWrittenAgain) +{ + // Pre-populate the file so tellp() != 0 when the Recorder opens it in + // append mode — the header-write branch must be skipped. + { + std::ofstream f(path_); + f << "existing_line\n"; + } + + { + auto r = MakeRecorder(); + } + + std::ifstream f(path_); + std::string line1, line2; + ASSERT_TRUE(std::getline(f, line1)); + EXPECT_EQ(line1, "existing_line"); + if (std::getline(f, line2)) + { + EXPECT_NE(line2, "mono_ns,event,offset_ns,pdelay_ns,seq_id,status_flags"); + } +} + +TEST_F(RecorderFileTest, Record_FlushIntervalOne_TriggersFlushAfterEachRecord) +{ + // flush_interval=1: after the first Record() flush_counter_ reaches the + // threshold, exercising the flush branch and the counter reset path. + Recorder::Config cfg; + cfg.enabled = true; + cfg.file_path = path_; + cfg.flush_interval = 1U; + Recorder r{cfg}; + + RecordEntry e{}; + e.mono_ns = 42LL; + e.event = RecordEvent::kSyncReceived; + r.Record(e); + + std::ifstream f(path_); + int lines = 0; + std::string line; + while (std::getline(f, line)) + ++lines; + EXPECT_GE(lines, 2); // header + at least 1 data line +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/BUILD b/score/libTSClient/BUILD new file mode 100644 index 0000000..445363d --- /dev/null +++ b/score/libTSClient/BUILD @@ -0,0 +1,91 @@ +# ******************************************************************************* +# 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_data.h", + "gptp_ipc_publisher.h", + "gptp_ipc_receiver.h", + ], + features = COMPILER_WARNING_FEATURES, + linkopts = select({ + "@platforms//os:qnx": [], + "//conditions:default": ["-lrt"], + }), + tags = ["QM"], + visibility = ["//score:__subpackages__"], + deps = [], +) + +cc_test( + name = "gptp_ipc_publisher_test", + srcs = [ + "gptp_ipc_publisher_test.cpp", + "gptp_ipc_test_utils.h", + ], + tags = ["unit"], + deps = [ + ":gptp_ipc", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "gptp_ipc_receiver_test", + srcs = [ + "gptp_ipc_receiver_test.cpp", + "gptp_ipc_test_utils.h", + ], + tags = ["unit"], + deps = [ + ":gptp_ipc", + "@googletest//:gtest", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "gptp_ipc_roundtrip_test", + srcs = [ + "gptp_ipc_roundtrip_test.cpp", + "gptp_ipc_test_utils.h", + ], + 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_publisher_test", + ":gptp_ipc_receiver_test", + ":gptp_ipc_roundtrip_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..1a95a04 --- /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/libTSClient/gptp_ipc_data.h" + +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Default POSIX shared memory name for the gPTP IPC channel. +constexpr char kGptpIpcName[] = "/gptp_ptp_info"; + +/// Magic number to validate the shared memory region ('GPTP'). +inline constexpr std::uint32_t kGptpIpcMagic = 0x47505450U; + +/** + * @brief Shared memory layout for gPTP IPC (seqlock protocol). + * + * Single-writer (TimeSlave), multi-reader (TimeDaemon via ShmPTPEngine). + * 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::atomic magic{kGptpIpcMagic}; + std::atomic seq{0}; + score::ts::GptpIpcData data{}; + std::atomic seq_confirm{1}; +}; + +} // namespace details +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_CHANNEL_H diff --git a/score/libTSClient/gptp_ipc_data.h b/score/libTSClient/gptp_ipc_data.h new file mode 100644 index 0000000..3774193 --- /dev/null +++ b/score/libTSClient/gptp_ipc_data.h @@ -0,0 +1,90 @@ +/******************************************************************************** + * 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_DATA_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_DATA_H + +#include +#include + +namespace score +{ +namespace ts +{ + +/** + * @brief IPC-layer status flags transmitted from TimeSlave to TimeDaemon. + */ +struct GptpIpcStatus +{ + bool is_synchronized; + bool is_timeout; + bool is_time_jump_future; + bool is_time_jump_past; + bool is_correct; +}; + +/** + * @brief IPC-layer Sync+FollowUp measurement data. + */ +struct GptpIpcSyncFupData +{ + std::uint64_t precise_origin_timestamp; + std::uint64_t reference_global_timestamp; + std::uint64_t reference_local_timestamp; + std::uint64_t sync_ingress_timestamp; + std::uint64_t correction_field; + std::uint16_t sequence_id; + std::uint64_t pdelay; + std::uint32_t port_number; + std::uint64_t clock_identity; +}; + +/** + * @brief IPC-layer peer-delay measurement data. + */ +struct GptpIpcPDelayData +{ + std::uint64_t request_origin_timestamp; + std::uint64_t request_receipt_timestamp; + std::uint64_t response_origin_timestamp; + std::uint64_t response_receipt_timestamp; + std::uint64_t reference_global_timestamp; + std::uint64_t reference_local_timestamp; + std::uint16_t sequence_id; + std::uint64_t pdelay; + std::uint32_t req_port_number; + std::uint64_t req_clock_identity; + std::uint32_t resp_port_number; + std::uint64_t resp_clock_identity; +}; + +/** + * @brief IPC data snapshot written by TimeSlave and read by TimeDaemon. + * + * This type is internal to libTSClient and intentionally decoupled from + * score::td::PtpTimeInfo. Callers are responsible for mapping between the two. + */ +struct GptpIpcData +{ + std::chrono::nanoseconds ptp_assumed_time; + std::chrono::nanoseconds local_time; ///< Local monotonic time of the last Sync frame + double rate_deviation; + GptpIpcStatus status; + GptpIpcSyncFupData sync_fup_data; + GptpIpcPDelayData pdelay_data; +}; + +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_DATA_H diff --git a/score/libTSClient/gptp_ipc_publisher.cpp b/score/libTSClient/gptp_ipc_publisher.cpp new file mode 100644 index 0000000..3d153ce --- /dev/null +++ b/score/libTSClient/gptp_ipc_publisher.cpp @@ -0,0 +1,108 @@ +/******************************************************************************** + * 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 +#include + +static_assert(std::is_trivially_copyable::value, + "GptpIpcData must be trivially copyable for seqlock memcpy to be valid"); + +namespace score +{ +namespace ts +{ +namespace details +{ + +GptpIpcPublisher::~GptpIpcPublisher() +{ + Destroy(); +} + +bool GptpIpcPublisher::Init(const std::string& ipc_name) +{ + if (region_ != nullptr) + return true; + + ipc_name_ = ipc_name; + + (void)::shm_unlink(ipc_name_.c_str()); + + shm_fd_ = ::shm_open(ipc_name_.c_str(), O_CREAT | O_RDWR, 0600); + 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::ts::GptpIpcData& data) +{ + if (region_ == nullptr) + return; + + const std::uint32_t next = region_->seq.load(std::memory_order_relaxed) + 1U; + region_->seq.store(next, std::memory_order_relaxed); + // Release fence: prevents the data writes below from being reordered before + // the seq=odd store above on weakly-ordered CPUs (ARM64/QNX). The acquire + // half of acq_rel is unnecessary for a seqlock writer; release suffices here. + std::atomic_thread_fence(std::memory_order_release); + + std::memcpy(®ion_->data, &data, sizeof(score::ts::GptpIpcData)); + + 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) + { + region_->~GptpIpcRegion(); + ::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..3e20711 --- /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 GptpIpcData snapshot using seqlock. + void Publish(const score::ts::GptpIpcData& data); + + /// 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_publisher_test.cpp b/score/libTSClient/gptp_ipc_publisher_test.cpp new file mode 100644 index 0000000..43a106a --- /dev/null +++ b/score/libTSClient/gptp_ipc_publisher_test.cpp @@ -0,0 +1,68 @@ +/******************************************************************************** + * 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 "score/libTSClient/gptp_ipc_test_utils.h" + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +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) +{ + score::ts::GptpIpcData data{}; + EXPECT_NO_THROW(pub_.Publish(data)); +} + +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()); +} + +TEST_F(GptpIpcPublisherTest, Init_CalledTwice_ReturnsTrueOnSecondCall) +{ + // region_ != nullptr after first Init → second call returns true immediately. + ASSERT_TRUE(pub_.Init(UniqueShmName())); + EXPECT_TRUE(pub_.Init(UniqueShmName())); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/gptp_ipc_receiver.cpp b/score/libTSClient/gptp_ipc_receiver.cpp new file mode 100644 index 0000000..710e054 --- /dev/null +++ b/score/libTSClient/gptp_ipc_receiver.cpp @@ -0,0 +1,123 @@ +/******************************************************************************** + * 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 +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +static constexpr int kMaxRetries = 20; + +GptpIpcReceiver::~GptpIpcReceiver() +{ + Close(); +} + +bool GptpIpcReceiver::Init(const std::string& ipc_name) +{ + if (region_ != nullptr) + return true; + + shm_fd_ = ::shm_open(ipc_name.c_str(), O_RDONLY, 0); + if (shm_fd_ < 0) + return false; + + { + struct ::stat st{}; + if (::fstat(shm_fd_, &st) != 0 || static_cast(st.st_size) < sizeof(GptpIpcRegion)) + { + ::close(shm_fd_); + shm_fd_ = -1; + 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.load(std::memory_order_acquire) != 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; // write in progress, retry + + score::ts::GptpIpcData data{}; + std::memcpy(&data, ®ion_->data, sizeof(score::ts::GptpIpcData)); + + // acq_rel fence: prevents data reads from floating past the consistency checks below + // (release half prevents memcpy reordering after the fence on ARM64), and prevents + // the seq/seq_confirm loads below from floating before the data reads (acquire half). + std::atomic_thread_fence(std::memory_order_acq_rel); + + const std::uint32_t seq2 = region_->seq_confirm.load(std::memory_order_acquire); + // Re-read seq to detect a write that started AFTER our initial seq1 snapshot. + // Without this, a writer that sets seq=odd after seq1 was loaded would go undetected: + // seq_confirm would still hold the old even value, causing the reader to return + // partially-written data (seqlock race on all multi-core platforms). + const std::uint32_t seq3 = region_->seq.load(std::memory_order_acquire); + + if (seq1 == seq2 && seq1 == seq3) + 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..dd3d440 --- /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 ShmPTPEngine. + */ +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 GptpIpcData 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_receiver_test.cpp b/score/libTSClient/gptp_ipc_receiver_test.cpp new file mode 100644 index 0000000..694d378 --- /dev/null +++ b/score/libTSClient/gptp_ipc_receiver_test.cpp @@ -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 + ********************************************************************************/ +#include "score/libTSClient/gptp_ipc_publisher.h" +#include "score/libTSClient/gptp_ipc_receiver.h" +#include "score/libTSClient/gptp_ipc_test_utils.h" + +#include + +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +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()); +} + +TEST_F(GptpIpcReceiverTest, Init_CalledTwice_ReturnsTrueOnSecondCall) +{ + // region_ != nullptr after first Init → second call returns true immediately. + GptpIpcPublisher pub; + const std::string name = UniqueShmName(); + ASSERT_TRUE(pub.Init(name)); + ASSERT_TRUE(rx_.Init(name)); + EXPECT_TRUE(rx_.Init(name)); + pub.Destroy(); +} + +TEST_F(GptpIpcReceiverTest, Init_TooSmallShm_ReturnsFalse) +{ + // Create a shm segment smaller than GptpIpcRegion so the fstat size check fails. + const std::string name = UniqueShmName(); + const int fd = ::shm_open(name.c_str(), O_CREAT | O_RDWR, 0600); + ASSERT_GE(fd, 0); + ASSERT_EQ(::ftruncate(fd, 1), 0); + ::close(fd); + + EXPECT_FALSE(rx_.Init(name)); + + ::shm_unlink(name.c_str()); +} + +} // namespace details +} // namespace ts +} // namespace score diff --git a/score/libTSClient/gptp_ipc_roundtrip_test.cpp b/score/libTSClient/gptp_ipc_roundtrip_test.cpp new file mode 100644 index 0000000..37508eb --- /dev/null +++ b/score/libTSClient/gptp_ipc_roundtrip_test.cpp @@ -0,0 +1,218 @@ +/******************************************************************************** + * 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 "score/libTSClient/gptp_ipc_receiver.h" +#include "score/libTSClient/gptp_ipc_test_utils.h" + +#include + +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +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_ReturnsNullopt) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + // seq_confirm is initialised to 1 (≠ seq=0) by GptpIpcRegion's constructor, + // so the seqlock always mismatches before the first Publish() call. + EXPECT_FALSE(rx_.Receive().has_value()); +} + +TEST_F(GptpIpcRoundtripTest, PublishReceive_BasicFields_RoundtripCorrectly) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + + score::ts::GptpIpcData data{}; + data.ptp_assumed_time = std::chrono::nanoseconds{1'234'567'890LL}; + data.rate_deviation = 0.75; + data.status.is_synchronized = true; + data.status.is_correct = true; + + pub_.Publish(data); + + const auto result = rx_.Receive(); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->ptp_assumed_time, data.ptp_assumed_time); + EXPECT_DOUBLE_EQ(result->rate_deviation, data.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::ts::GptpIpcData data{}; + data.status.is_timeout = true; + data.status.is_time_jump_future = true; + data.status.is_synchronized = false; + data.status.is_correct = false; + + pub_.Publish(data); + + 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::ts::GptpIpcData data{}; + data.sync_fup_data.precise_origin_timestamp = 100'000'000'000ULL; + data.sync_fup_data.reference_global_timestamp = 100'000'001'000ULL; + data.sync_fup_data.reference_local_timestamp = 100'000'001'500ULL; + data.sync_fup_data.sync_ingress_timestamp = 100'000'001'500ULL; + data.sync_fup_data.correction_field = 42U; + data.sync_fup_data.sequence_id = 77; + data.sync_fup_data.pdelay = 3'000U; + data.sync_fup_data.port_number = 1; + data.sync_fup_data.clock_identity = 0xAABBCCDDEEFF0011ULL; + + pub_.Publish(data); + + 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::ts::GptpIpcData data{}; + data.pdelay_data.request_origin_timestamp = 1'000'000'000ULL; + data.pdelay_data.request_receipt_timestamp = 1'000'001'000ULL; + data.pdelay_data.response_origin_timestamp = 1'000'001'000ULL; + data.pdelay_data.response_receipt_timestamp = 1'000'002'000ULL; + data.pdelay_data.pdelay = 1'000U; + data.pdelay_data.req_port_number = 1; + data.pdelay_data.resp_port_number = 2; + data.pdelay_data.req_clock_identity = 0x1122334455667788ULL; + + pub_.Publish(data); + + 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::ts::GptpIpcData data{}; + data.ptp_assumed_time = std::chrono::nanoseconds{static_cast(i) * 1'000'000'000LL}; + pub_.Publish(data); + } + + 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()); + + 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()); + + 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()); + + 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 diff --git a/score/libTSClient/gptp_ipc_test.cpp b/score/libTSClient/gptp_ipc_test.cpp new file mode 100644 index 0000000..a73094c --- /dev/null +++ b/score/libTSClient/gptp_ipc_test.cpp @@ -0,0 +1,384 @@ +/******************************************************************************** + * 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()); +} + +TEST_F(GptpIpcPublisherTest, Init_CalledTwice_ReturnsTrueOnSecondCall) +{ + // region_ != nullptr after first Init → second call returns true immediately. + ASSERT_TRUE(pub_.Init(UniqueShmName())); + EXPECT_TRUE(pub_.Init(UniqueShmName())); +} + +// ── 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()); +} + +TEST_F(GptpIpcReceiverTest, Init_CalledTwice_ReturnsTrueOnSecondCall) +{ + // region_ != nullptr after first Init → second call returns true immediately. + GptpIpcPublisher pub; + const std::string name = UniqueShmName(); + ASSERT_TRUE(pub.Init(name)); + ASSERT_TRUE(rx_.Init(name)); + EXPECT_TRUE(rx_.Init(name)); + pub.Destroy(); +} + +TEST_F(GptpIpcReceiverTest, Init_TooSmallShm_ReturnsFalse) +{ + // Create a shm segment smaller than GptpIpcRegion so the fstat size check fails. + const std::string name = UniqueShmName(); + const int fd = ::shm_open(name.c_str(), O_CREAT | O_RDWR, 0600); + ASSERT_GE(fd, 0); + ASSERT_EQ(::ftruncate(fd, 1), 0); + ::close(fd); + + EXPECT_FALSE(rx_.Init(name)); + + ::shm_unlink(name.c_str()); +} + +// ── 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_ReturnsNullopt) +{ + ASSERT_TRUE(pub_.Init(name_)); + ASSERT_TRUE(rx_.Init(name_)); + // seq_confirm is initialised to 1 (≠ seq=0) by GptpIpcRegion's constructor, + // so the seqlock always mismatches before the first Publish() call. + // Receive() must exhaust its retries and return std::nullopt. + EXPECT_FALSE(rx_.Receive().has_value()); +} + +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 diff --git a/score/libTSClient/gptp_ipc_test_utils.h b/score/libTSClient/gptp_ipc_test_utils.h new file mode 100644 index 0000000..d838dee --- /dev/null +++ b/score/libTSClient/gptp_ipc_test_utils.h @@ -0,0 +1,82 @@ +/******************************************************************************** + * 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_TEST_UTILS_H +#define SCORE_LIBTSCLIENT_GPTP_IPC_TEST_UTILS_H + +#include "score/libTSClient/gptp_ipc_channel.h" + +#include +#include +#include +#include +#include + +namespace score +{ +namespace ts +{ +namespace details +{ + +/// Generate a unique POSIX shm name per invocation (avoids cross-test pollution). +inline 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 details +} // namespace ts +} // namespace score + +#endif // SCORE_LIBTSCLIENT_GPTP_IPC_TEST_UTILS_H