diff --git a/.bazelrc b/.bazelrc index 51cac5a..9275aa7 100644 --- a/.bazelrc +++ b/.bazelrc @@ -7,3 +7,11 @@ test --test_output=errors common --registry=https://raw.githubusercontent.com/eclipse-score/bazel_registry/main/ common --registry=https://bcr.bazel.build + +build:linux_aarch64 --platforms=//platforms:linux_aarch64 +build:qnx_aarch64 --platforms=//platforms:qnx_aarch64 + +build:qnx_aarch64 --action_env=QNX_HOST=/work/toolchains/qnx710/qnx710/host/linux/x86_64 +build:qnx_aarch64 --action_env=QNX_TARGET=/work/toolchains/qnx710/qnx710/target/qnx7 +build:qnx_aarch64 --action_env=PATH=/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin:/usr/bin:/bin +build:qnx_aarch64 --action_env=COMPILER_PATH=/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0/8.3.0:/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin:/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/lib/gcc/aarch64-unknown-nto-qnx7.1.0/8.3.0:/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/lib/gcc/aarch64-unknown-nto-qnx7.1.0:/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/lib/gcc/aarch64-unknown-nto-qnx7.1.0/8.3.0/../../../../aarch64-unknown-nto-qnx7.1.0/bin/aarch64-unknown-nto-qnx7.1.0/8.3.0:/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/lib/gcc/aarch64-unknown-nto-qnx7.1.0/8.3.0/../../../../aarch64-unknown-nto-qnx7.1.0/bin diff --git a/BUILD b/BUILD index 473b5d5..436fda8 100644 --- a/BUILD +++ b/BUILD @@ -33,16 +33,24 @@ copyright_checker( visibility = ["//visibility:public"], ) -dash_license_checker( - src = "//examples:cargo_lock", - file_type = "", # let it auto-detect based on project_config - project_config = PROJECT_CONFIG, - visibility = ["//visibility:public"], -) +# dash_license_checker only supports rust and python +# dash_license_checker( +# src = "//examples:cargo_lock", +# file_type = "", # let it auto-detect based on project_config +# project_config = PROJECT_CONFIG, +# visibility = ["//visibility:public"], +# ) # Add target for formatting checks use_format_targets() +# Documentation generation tool +alias( + name = "doc-gen", + actual = "//tools:doc_gen", + visibility = ["//visibility:public"], +) + docs( source_dir = "docs", ) diff --git a/MODULE.bazel b/MODULE.bazel index 1ec606d..fc13f90 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -37,6 +37,7 @@ bazel_dep(name = "rules_cc", version = "0.2.1") # LLVM Toolchains Rules - host configuration bazel_dep(name = "toolchains_llvm", version = "1.4.0") +bazel_dep(name = "platforms", version = "0.0.11") llvm = use_extension("@toolchains_llvm//toolchain/extensions:llvm.bzl", "llvm") llvm.toolchain( @@ -48,6 +49,11 @@ use_repo(llvm, "llvm_toolchain_llvm") register_toolchains("@llvm_toolchain//:all") +register_toolchains( + "//toolchains:linux_aarch64_toolchain", + "//toolchains:qnx_aarch64_toolchain", +) + # tooling bazel_dep(name = "score_tooling", version = "1.0.1") bazel_dep(name = "aspect_rules_lint", version = "1.5.3") diff --git a/docs/README_DOC_GENERATION.md b/docs/README_DOC_GENERATION.md new file mode 100644 index 0000000..0ebcdd0 --- /dev/null +++ b/docs/README_DOC_GENERATION.md @@ -0,0 +1,230 @@ +# Documentation Generation for SCORE Time Synchronization + +This guide explains how to automatically generate documentation (.rst and .puml files) from C++ source code. + +## Quick Start + +### Generate All Documentation + +```bash +# Using bazel (recommended) +bazel run //:doc-gen -- --all + +# Or directly with Python +python3 tools/generate_docs.py --all +``` + +### Build HTML Documentation + +```bash +bazel run //:docs +``` + +### View Documentation + +Open `bazel-bin/docs/docs/html/index.html` in your browser. + +## Available Options + +The documentation generator supports several options: + +```bash +# Generate only API documentation +bazel run //:doc-gen -- --api + +# Generate only architecture diagrams +bazel run //:doc-gen -- --arch + +# Generate only sequence diagrams +bazel run //:doc-gen -- --seq + +# Generate all and update index.rst +bazel run //:doc-gen -- --all --update-index +``` + +## Generated Files + +After running `--all`, you will have: + +``` +docs/ +├── api/ +│ ├── index.rst # API overview +│ ├── core_types.rst # SharedState, SyncLogEntry, enums +│ ├── ipc.rst # ShmRegion, seqlock functions +│ └── utilities.rst # ClockNs, ParseInteger, PthreadLockGuard +├── diagrams/ +│ ├── index.rst # Diagrams overview +│ ├── architecture.puml # High-level system architecture +│ ├── class_relationships.puml # Core class diagram +│ └── sequence_gptp_sync.puml # gPTP synchronization workflow +└── index.rst (updated) # Main documentation index +``` + +## Using with Claude Code + +You can use the `/doc-gen` skill with Claude Code: + +``` +User: Generate documentation for score_time +Assistant: [Automatically runs documentation generator] +``` + +The skill is defined in `.claude/skills/doc_generator_skill.md`. + +## Customization + +### Adding New Diagrams + +To add a new PlantUML diagram: + +1. Create the `.puml` file in `docs/diagrams/` +2. Reference it in `docs/diagrams/index.rst`: + +```rst +.. uml:: diagrams/my_new_diagram.puml + :caption: My New Diagram Description +``` + +### Extending API Documentation + +The generator automatically extracts classes from: +- `src/common/include/score_time/` (public API) +- `src/tsyncd/engine/` (daemon implementation) + +To improve extraction: +1. Add Doxygen-style comments to your C++ code: + +```cpp +/** + * @brief Brief description of the class + * + * Detailed description here. + */ +class MyClass { + /** + * @brief Brief description of method + * @param arg1 Description of arg1 + * @return Description of return value + */ + int MyMethod(int arg1); +}; +``` + +2. Re-run the generator: + +```bash +bazel run //:doc-gen -- --api +``` + +### Manual Tweaks + +After auto-generation, you can manually edit: +- `docs/api/*.rst` files to add examples +- `docs/diagrams/*.puml` files to refine visual layout +- `docs/index.rst` to reorganize structure + +**Important:** Remember that re-running the generator will overwrite your changes. Consider: +1. Committing generated files to git +2. Using separate `*_custom.rst` files for manual content +3. Editing `tools/generate_docs.py` to customize templates + +## Integration with CI/CD + +To ensure documentation stays up-to-date: + +### GitHub Actions Example + +```yaml +name: Documentation + +on: + push: + branches: [main] + pull_request: + +jobs: + build-docs: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + + - name: Generate Documentation + run: bazel run //:doc-gen -- --all + + - name: Build HTML Documentation + run: bazel run //:docs + + - name: Upload Documentation + uses: actions/upload-artifact@v3 + with: + name: documentation + path: bazel-bin/docs/docs/html/ +``` + +### Pre-commit Hook + +Add to `.git/hooks/pre-commit`: + +```bash +#!/bin/bash +# Regenerate docs before commit +bazel run //:doc-gen -- --all +git add docs/ +``` + +## Troubleshooting + +### "Module 'sphinx_needs' not found" + +Ensure you have the required Sphinx extensions: + +```bash +pip install sphinx sphinx-design sphinx-needs sphinxcontrib-plantuml +``` + +### "PlantUML diagrams not rendering" + +Install PlantUML: + +```bash +# Ubuntu/Debian +sudo apt-get install plantuml + +# macOS +brew install plantuml +``` + +Configure path in `docs/conf.py`: + +```python +plantuml = 'java -jar /path/to/plantuml.jar' +``` + +### "Cannot find header file" + +Check that the paths in `tools/generate_docs.py` match your project structure: + +```python +include_dirs = [ + project_root / "src" / "common" / "include" / "score_time", + project_root / "src" / "tsyncd" / "engine", +] +``` + +## Contributing + +When adding new C++ components: + +1. Write Doxygen comments for public APIs +2. Run `/doc-gen --all` to update documentation +3. Review generated `.rst` and `.puml` files +4. Add custom examples if needed +5. Commit both code and documentation + +## References + +- [Sphinx Documentation](https://www.sphinx-doc.org/) +- [PlantUML Guide](https://plantuml.com/) +- [ReStructuredText Primer](https://www.sphinx-doc.org/en/master/usage/restructuredtext/basics.html) +- [Sphinx C++ Domain](https://www.sphinx-doc.org/en/master/usage/restructuredtext/domains.html#the-c-domain) diff --git a/docs/api/core_types.rst b/docs/api/core_types.rst new file mode 100644 index 0000000..47f7a25 --- /dev/null +++ b/docs/api/core_types.rst @@ -0,0 +1,60 @@ +.. ****************************************************************************** + 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 + ****************************************************************************** + +Core Types API Reference +======================== + +.. contents:: + :local: + :depth: 2 + +Overview +-------- + +This module provides core types for the SCORE Time Synchronization system. + + +Namespace: ``score_time::ipc`` +------------------------------ + + +SyncLogEntry +~~~~~~~~~~~~ + +.. cpp:struct:: score_time::ipc::SyncLogEntry + + Single entry in the synchronization event log Uses per-entry seqlock protocol for lock-free concurrent access between writer (tsyncd daemon) and readers (client applications). **Seqlock Protocol:** - seq is even (0, 2, 4, ...) when entry is readable - seq is odd (1, 3, 5, ...) when writer is updating the entry - Readers retry if seq is odd or changes during read All fields are atomic to prevent torn reads on architectures where 64-bit loads are not naturally atomic. + + **Defined in:** :code:`shared_state.hpp` + + +SharedState +~~~~~~~~~~~ + +.. cpp:struct:: score_time::ipc::SharedState + + Main shared memory structure for IPC between tsyncd daemon and clients This structure is mapped into shared memory (typically /dev/shm/score_time) and provides lock-free access to synchronized time information using seqlock protocol. **Memory Layout:** - Header fields (magic, version, size) for validation - Vehicle time data (gPTP/IEEE 802.1AS synchronized time) - Absolute time data (UTC/wall-clock time from GNSS/NTP) - Circular event logs for diagnostics **Concurrency:** - Single writer (tsyncd daemon) - Multiple readers (client applications) - Uses double-buffering seqlock for vehicle/absolute time - Uses per-entry seqlock for log entries **Version History:** - v1-v3: Original implementations - v4: Added per-entry seqlock to prevent torn reads in logs + + **Defined in:** :code:`shared_state.hpp` + + **Usage:** Shared memory structure for lock-free IPC. + + +Examples +-------- + +See the test files for usage examples: + +- Unit tests: ``tests/cpp/`` +- Integration tests: ``tests/integration/`` + diff --git a/docs/api/index.rst b/docs/api/index.rst new file mode 100644 index 0000000..03298cf --- /dev/null +++ b/docs/api/index.rst @@ -0,0 +1,9 @@ +API Reference +============= + +.. toctree:: + :maxdepth: 2 + + core_types + ipc + utilities diff --git a/docs/api/ipc.rst b/docs/api/ipc.rst new file mode 100644 index 0000000..5005785 --- /dev/null +++ b/docs/api/ipc.rst @@ -0,0 +1,48 @@ +.. ****************************************************************************** + 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 + ****************************************************************************** + +IPC API Reference +================= + +.. contents:: + :local: + :depth: 2 + +Overview +-------- + +This module provides ipc for the SCORE Time Synchronization system. + + +Namespace: ``score_time::ipc`` +------------------------------ + + +ShmRegion +~~~~~~~~~ + +.. cpp:class:: score_time::ipc::ShmRegion + + RAII wrapper for POSIX shared memory regions Manages lifecycle of shared memory objects (/dev/shm on Linux, shared memory objects on QNX) with automatic cleanup. Uses shm_open + mmap for portable POSIX shared memory access. **Features:** - RAII: Automatically unmaps and closes on destruction - Move-only: Prevents accidental copies of memory mappings - Create-or-open semantics for both producer and consumer - Size validation for safety **Typical Usage:** - Producer (tsyncd): Open with create_or_open=true, writes SharedState - Consumer (clients): Open with create_or_open=false, reads SharedState + + **Defined in:** :code:`shm_region.hpp` + + +Examples +-------- + +See the test files for usage examples: + +- Unit tests: ``tests/cpp/`` +- Integration tests: ``tests/integration/`` + diff --git a/docs/api/utilities.rst b/docs/api/utilities.rst new file mode 100644 index 0000000..d3e2e58 --- /dev/null +++ b/docs/api/utilities.rst @@ -0,0 +1,50 @@ +.. ****************************************************************************** + 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 + ****************************************************************************** + +Utilities API Reference +======================= + +.. contents:: + :local: + :depth: 2 + +Overview +-------- + +This module provides utilities for the SCORE Time Synchronization system. + + +Namespace: ``score_time::utils`` +-------------------------------- + + +PthreadLockGuard +~~~~~~~~~~~~~~~~ + +.. cpp:class:: score_time::utils::PthreadLockGuard + + RAII wrapper for pthread_mutex_t Automatically locks the mutex on construction and unlocks on destruction. This prevents deadlocks from forgotten unlocks or early returns. + + **Defined in:** :code:`pthread_lock_guard.hpp` + + **Usage:** RAII wrapper for automatic resource management. + + +Examples +-------- + +See the test files for usage examples: + +- Unit tests: ``tests/cpp/`` +- Integration tests: ``tests/integration/`` + diff --git a/docs/diagrams/architecture.puml b/docs/diagrams/architecture.puml new file mode 100644 index 0000000..220534a --- /dev/null +++ b/docs/diagrams/architecture.puml @@ -0,0 +1,41 @@ +@startuml +!theme plain +skinparam componentStyle rectangle + +package "SCORE Time Synchronization" { + component [tsyncd Daemon] as daemon + component [libscore_time] as lib + database "Shared Memory" as shm + + package "Common Utilities" { + component [ClockNs] as clock + component [PthreadLockGuard] as lock + component [ParseInteger/Double] as parser + } +} + +cloud "Network" { + component [gPTP Master] as gptp + component [NTP Server] as ntp +} + +actor "Application" as app + +daemon --> gptp : "Sync via\nEthernet/PTP" +daemon --> ntp : "Query via\nUDP/123" +daemon --> shm : "Writes time\n(seqlock)" +lib --> shm : "Reads time\n(seqlock)" +app --> lib : "GetVehicleTime()\nGetAbsoluteTime()" +daemon ..> clock : uses +daemon ..> lock : uses +daemon ..> parser : uses +lib ..> clock : uses + +note right of shm + Lock-free IPC + Seqlock pattern + Atomic operations + Version: 4 +end note + +@enduml diff --git a/docs/diagrams/class_relationships.puml b/docs/diagrams/class_relationships.puml new file mode 100644 index 0000000..1b51e1b --- /dev/null +++ b/docs/diagrams/class_relationships.puml @@ -0,0 +1,99 @@ +@startuml +!theme plain + +package "score_time::ipc" { + class SharedState { + +kMagic: uint32_t {static} + +kVersion: uint16_t {static} + -- + +magic: atomic + +version: atomic + +vehicle_seq: atomic + +vehicle_base_ns: atomic + +vehicle_base_mono_ns: atomic + +vehicle_acc: atomic + +vehicle_tpq: atomic + -- + +vehicle_log[256]: SyncLogEntry + +abs_log[256]: SyncLogEntry + } + + class SyncLogEntry { + +seq: atomic + +monotonic_ns: atomic + +type: atomic + +flags: atomic + +v1: atomic + +v2: atomic + } + + class ShmRegion { + -fd_: int + -addr_: void* + -size_: size_t + -- + +Open(name): bool + +Close(): void + +Addr(): void* + +Size(): size_t + } + + SharedState *-- "512" SyncLogEntry : contains + ShmRegion ..> SharedState : "maps to" +} + +package "score_time::utils" { + class PthreadLockGuard { + -mtx_: pthread_mutex_t* + -- + +PthreadLockGuard(mtx) + +~PthreadLockGuard() + -- + {method} Deleted: copy, move + } + + class "ParseInteger" as ParseInt { + +ParseInteger(str, out): bool {static} + } + + class ParseDouble { + +ParseDouble(str, out): bool {static} + } + + class ClockNs { + +ClockNsSafe(clk): optional {static} + +ClockNs(clk): int64_t {static} + } +} + +package "tsyncd" { + class TimeSyncEngine { + -ctx_: Context + -shared_: SharedState* + -shm_: ShmRegion + -- + +Start(): bool + +Stop(): void + -RxLoop(): void + -PdelayLoop(): void + -AbsLoop(): void + } +} + +TimeSyncEngine --> SharedState : writes to +TimeSyncEngine --> ShmRegion : owns +TimeSyncEngine ..> PthreadLockGuard : uses +TimeSyncEngine ..> ClockNs : uses + +note right of SharedState + Seqlock protocol: + seq odd = writing + seq even = readable +end note + +note right of PthreadLockGuard + RAII lock wrapper + Aborts on error +end note + +@enduml diff --git a/docs/diagrams/index.rst b/docs/diagrams/index.rst new file mode 100644 index 0000000..eb84a38 --- /dev/null +++ b/docs/diagrams/index.rst @@ -0,0 +1,23 @@ +Architecture Diagrams +===================== + +System Architecture +------------------- + +.. uml:: architecture.puml + :caption: High-level system architecture + +Class Relationships +------------------- + +.. uml:: class_relationships.puml + :caption: Core class relationships + +Sequence Diagrams +----------------- + +gPTP Synchronization +~~~~~~~~~~~~~~~~~~~~ + +.. uml:: sequence_gptp_sync.puml + :caption: gPTP synchronization workflow diff --git a/docs/diagrams/sequence_gptp_sync.puml b/docs/diagrams/sequence_gptp_sync.puml new file mode 100644 index 0000000..7fb119b --- /dev/null +++ b/docs/diagrams/sequence_gptp_sync.puml @@ -0,0 +1,48 @@ +@startuml +!theme plain + +participant "gPTP Master" as Master +participant "tsyncd::RxLoop" as RxLoop +participant "SharedState" as SHM +participant "Client App" as Client + +== gPTP Synchronization == + +Master -> RxLoop: Sync message\n(HW timestamp t1) +activate RxLoop +RxLoop -> RxLoop: Extract timestamp +RxLoop -> Master: Delay_Req\n(HW timestamp t3) +deactivate RxLoop + +Master -> RxLoop: Delay_Resp\n(t2, t4 timestamps) +activate RxLoop + +RxLoop -> RxLoop: Calculate offset\noffset = ((t2-t1) + (t3-t4)) / 2 + +RxLoop -> SHM: WriteVehicle(base_ns, base_mono_ns, ...) +note right + Seqlock write protocol: + 1. seq.fetch_add(1, release) // odd = writing + 2. Write all fields with release stores + 3. seq.fetch_add(1, release) // even = complete +end note +deactivate RxLoop + +...Time passes... + +Client -> SHM: ReadVehicle(out_base_ns, out_base_mono_ns, ...) +activate Client +note right + Seqlock read protocol (retry up to 1000x): + 1. seq_a = seq.load(acquire) + 2. if seq_a is odd: retry + 3. Read all fields with acquire loads + 4. seq_b = seq.load(acquire) + 5. if seq_a != seq_b: retry + 6. Success! +end note +SHM --> Client: vehicle_base_ns, accuracy +Client -> Client: current = base_ns + (now_mono - base_mono) +deactivate Client + +@enduml diff --git a/docs/index.rst b/docs/index.rst index f8e53da..66ef93a 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -12,10 +12,10 @@ # SPDX-License-Identifier: Apache-2.0 # ******************************************************************************* -Module Template Documentation -============================= +SCORE Time Synchronization Documentation +========================================== -This documentation describes the structure, usage and configuration of the Bazel-based C++/Rust module template. +This documentation describes the SCORE (Scalable Core for Automotive Real-time Environments) Time Synchronization system, which provides high-precision time synchronization for automotive applications using gPTP (IEEE 802.1AS) and NTP. .. contents:: Table of Contents :depth: 2 @@ -24,59 +24,135 @@ This documentation describes the structure, usage and configuration of the Bazel Overview -------- -This repository provides a standardized setup for projects using **C++** or **Rust** and **Bazel** as a build system. -It integrates best practices for build, test, CI/CD and documentation. +SCORE Time Synchronization is a safety-critical time synchronization daemon (**tsyncd**) and client library (**libscore_time**) designed for automotive systems. It provides: -Requirements +- **Vehicle Time**: High-precision synchronized time using gPTP (IEEE 802.1AS) over Ethernet +- **Absolute Time**: UTC/wall-clock time from GNSS, NTP, or kernel RTC +- **Lock-free IPC**: Shared memory communication using seqlock protocol for zero-copy reads +- **AUTOSAR Compliance**: Designed for ASIL QM level with safety qualifiers +- **Multi-platform**: Supports Linux and QNX operating systems + +Key Features ------------ -.. stkh_req:: Example Functional Requirement - :id: stkh_req__docgen_enabled__example - :status: valid - :safety: QM - :security: YES - :reqtype: Functional - :rationale: Ensure documentation builds are possible for all modules +**Time Synchronization** + +- gPTP (IEEE 802.1AS) slave implementation for vehicle time +- NTP client for absolute time synchronization +- Support for external time sources (GNSS) +- Automatic fallback and error recovery + +**Performance & Safety** + +- Lock-free shared memory IPC using seqlock protocol +- Zero-copy time reads for clients +- ASIL QM compliance with safety qualifiers +- Sub-microsecond precision for vehicle time +**Platform Support** + +- Linux with PTP Hardware Clock (PHC) support +- QNX with BPF packet capture +- Bazel build system for reproducible builds Project Layout -------------- -The module template includes the following top-level structure: +The SCORE Time Synchronization project includes the following structure: -- `src/`: Main C++/Rust sources +- `src/common/`: Shared IPC and utility code + - `include/score_time/ipc/`: Shared memory structures (SharedState, ShmRegion) + - `include/score_time/utils/`: Utilities (PthreadLockGuard, time functions, parsers) +- `src/tsyncd/`: Time synchronization daemon implementation + - `engine/`: Core synchronization engine and state machine + - `platform/`: Platform-specific code (Linux, QNX) + - `protocol/`: gPTP and NTP protocol implementations +- `src/libscore_time/`: Client library for reading synchronized time - `tests/`: Unit and integration tests -- `examples/`: Usage examples -- `docs/`: Documentation using `docs-as-code` -- `.github/workflows/`: CI/CD pipelines +- `docs/`: API documentation and architecture diagrams Quick Start ----------- -To build the module: +**Building the Project** + +To build all components: .. code-block:: bash + # Build all components bazel build //src/... -To run tests: + # Build tsyncd daemon only + bazel build //src/tsyncd:tsyncd -.. code-block:: bash + # Build client library + bazel build //src/libscore_time:score_time + # Run all tests bazel test //tests/... +**Running tsyncd Daemon** + +.. code-block:: bash + + # Run with default configuration + sudo bazel run //src/tsyncd:tsyncd -- --iface eth0 + + # Run with custom configuration file + sudo bazel run //src/tsyncd:tsyncd -- --config /path/to/config.ini + +**Using Client Library** + +.. code-block:: cpp + + #include "score_time/ipc/shared_state.hpp" + #include "score_time/ipc/shm_region.hpp" + + // Open shared memory + score_time::ipc::ShmRegion shm; + shm.Open("/score_time_vehicle_time", sizeof(score_time::ipc::SharedState), false); + + auto* state = static_cast(shm.Addr()); + + // Read vehicle time + std::int64_t base_ns, mono_ns; + score_time::AccuracyQualifier acc; + score_time::TimePointQualifier tpq; + + if (score_time::ipc::ReadVehicle(*state, base_ns, mono_ns, acc, tpq)) { + // Use synchronized time + } + Configuration ------------- -The `project_config.bzl` file defines metadata used by Bazel macros. +The daemon can be configured via command-line arguments or a configuration file (`config.ini`). -Example: +**Key Configuration Parameters:** -.. code-block:: python +- `iface_name`: Network interface for gPTP (e.g., "eth0") +- `phc_device`: PTP Hardware Clock device (e.g., "/dev/ptp0") +- `shm_name`: Shared memory object name (default: "/score_time_vehicle_time") +- `ntp_servers`: List of NTP servers for absolute time +- `abs_mode`: Publish-only or discipline system clock - PROJECT_CONFIG = { - "asil_level": "QM", - "source_code": ["cpp", "rust"] - } +See `docs/configuration.md` for full configuration options. + + +API Reference +------------- + +.. toctree:: + :maxdepth: 2 + + api/index + +Architecture Diagrams +--------------------- + +.. toctree:: + :maxdepth: 1 + + diagrams/index -This enables conditional behavior (e.g., choosing `clang-tidy` for C++ or `clippy` for Rust). diff --git a/docs/time.drawio.svg b/docs/time.drawio.svg new file mode 100644 index 0000000..a34f3b1 --- /dev/null +++ b/docs/time.drawio.svg @@ -0,0 +1,568 @@ + + + + + + + + + + + +
+
+
+ External clock source +
+
+
+ + + External clock source + + + + + + + + + + + + +
+
+
+ Local ECU +
+
+
+ + + Local ECU + + + + + + + + + + + +
+
+
+ «process» +
+ + tsyncd + +
+
+
+ + + «process»... + + + + + + + + + + + + + + + +
+
+
+ shared memory +
+
+
+ + + shared memory + + + + + + + + + + + + + + + + + +
+
+
+ + gptp protocol + +
+
+
+ + + gptp protocol + + + + + + + + + + + + + + + + +
+
+
+ + ntp protocol + +
+
+
+ + + ntp protocol + + + + + + + + + + + + + + + + +
+
+
+ + vehicle time sync + +
+
+
+ + + vehicle time sync + + + + + + + + + + + + + + + + +
+
+
+ + absolute time sync + +
+
+
+ + + absolute time sync + + + + + + + + + + + + +
+
+
+ write +
+
+
+ + + write + + + + + + + + + + + + +
+
+
+ shared memory manager +
+
+
+ + + shared memory mana... + + + + + + + + + + + +
+
+
+ + «process» +
+ + Application1 + + +
+
+
+ + + «process»... + + + + + + + + + + + + + + + + +
+
+
+ mmap +
+
+
+ + + mmap + + + + + + + + + + + + +
+
+
+ + libscore_time + +
+
+
+ + + libscore_time + + + + + + + + + + + +
+
+
+ «process» +
+ + Application2 + +
+
+
+ + + «process»... + + + + + + + + + + + + + + + + +
+
+
+ mmap +
+
+
+ + + mmap + + + + + + + + + + + + +
+
+
+ + libscore_time + +
+
+
+ + + libscore_time + + + + + + + + + + + +
+
+
+ «process» +
+ + gptp time master + +
+
+
+ + + «process»... + + + + + + + + + + + + + + + +
+
+
+ «process» +
+ + ntp time master + +
+
+
+ + + «process»... + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ data +
+
+
+ + + data + + + + + + + + + + + +
+
+
+ meta data +
+
+
+ + + meta data + + + + + + + + + + + +
+
+
+ vehicle struct +
+
+
+ + + vehicle struct + + + + + + + + + + + +
+
+
+ absolute struct +
+
+
+ + + absolute stru... + + + + + + + + + + Text is not SVG - cannot display + + + + diff --git a/platforms/BUILD b/platforms/BUILD new file mode 100644 index 0000000..2559cc2 --- /dev/null +++ b/platforms/BUILD @@ -0,0 +1,28 @@ +package( + default_visibility = ["//visibility:public"], +) + +constraint_setting( + name = "os", +) + +constraint_value( + name = "qnx", + constraint_setting = ":os", +) + +platform( + name = "linux_aarch64", + constraint_values = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], +) + +platform( + name = "qnx_aarch64", + constraint_values = [ + "@platforms//cpu:aarch64", + ":qnx", + ], +) diff --git a/project_config.bzl b/project_config.bzl index f764a1d..8f203ed 100644 --- a/project_config.bzl +++ b/project_config.bzl @@ -1,5 +1,5 @@ # project_config.bzl PROJECT_CONFIG = { "asil_level": "QM", - "source_code": ["rust"], + "source_code": ["cpp"], } diff --git a/src/BUILD b/src/BUILD index e69de29..b4a031b 100644 --- a/src/BUILD +++ b/src/BUILD @@ -0,0 +1,15 @@ +# ******************************************************************************* +# 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 +# ******************************************************************************* +package( + default_visibility = ["//visibility:public"], +) diff --git a/src/common/BUILD b/src/common/BUILD new file mode 100644 index 0000000..ecad83d --- /dev/null +++ b/src/common/BUILD @@ -0,0 +1,24 @@ +# ******************************************************************************* +# 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 +# ******************************************************************************* +package( + default_visibility = ["//visibility:public"], +) + +cc_library( + name = "common_ipc", + srcs = [ + "shared_memory/shm_region.cpp", + ], + hdrs = glob(["include/**/*.hpp"]), + strip_include_prefix = "include", +) diff --git a/src/common/include/score_time/ipc/shared_state.hpp b/src/common/include/score_time/ipc/shared_state.hpp new file mode 100644 index 0000000..3ad6556 --- /dev/null +++ b/src/common/include/score_time/ipc/shared_state.hpp @@ -0,0 +1,421 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include +#include +#include + +namespace score_time +{ + + /** + * @brief Vehicle time accuracy qualifier + * + * Indicates the synchronization status and reliability of vehicle time + * obtained via gPTP (IEEE 802.1AS). Used by clients to determine if + * the time is suitable for safety-critical operations. + */ + enum class AccuracyQualifier : std::uint8_t + { + kNoTime = 0, ///< No time available yet + kNotSynchronized, ///< Time available but not synchronized to grandmaster + kSynchronized, ///< Successfully synchronized to gPTP grandmaster + kUnstable, ///< Synchronization unstable or degraded + kTimeJumpDetected, ///< Discontinuous time change detected + }; + + /** + * @brief ASIL (Automotive Safety Integrity Level) qualifier for time point + * + * Indicates the safety integrity level of the time synchronization. + * Complies with ISO 26262 functional safety standard. + */ + enum class TimePointQualifier : std::uint8_t + { + kQM = 0, ///< Quality Management (non-safety-critical) + kASIL_B, ///< ASIL-B safety integrity level + }; + + /** + * @brief Absolute (UTC/wall-clock) time accuracy qualifier + * + * Indicates the estimated inaccuracy range of absolute time + * obtained from external sources (GNSS, NTP, kernel RTC). + * Corresponds to AUTOSAR Absolute Time Accuracy levels. + */ + enum class AbsoluteAccuracyQualifier : std::uint8_t + { + kInaccGreaterThan24h = 0, ///< Inaccuracy > 24 hours + kInaccLessThan24h, ///< Inaccuracy < 24 hours + kInaccLessThan1h, ///< Inaccuracy < 1 hour + kInaccLessThan15min, ///< Inaccuracy < 15 minutes + kInaccLessThan60s, ///< Inaccuracy < 60 seconds + kInaccLessThan10s, ///< Inaccuracy < 10 seconds + kInaccLessThan1s, ///< Inaccuracy < 1 second + kInaccLessThan500ms, ///< Inaccuracy < 500 milliseconds + kInaccLessThan100ms, ///< Inaccuracy < 100 milliseconds + kInaccLessThan50ms, ///< Inaccuracy < 50 milliseconds + kInaccLessThan10ms, ///< Inaccuracy < 10 milliseconds + kInaccuracyNotAvailable, ///< Inaccuracy information not available + }; + + /** + * @brief Absolute time security/trustworthiness qualifier + * + * Indicates the trustworthiness of the absolute time source. + * Higher values indicate more secure/authenticated time sources + * (e.g., cryptographically signed GNSS vs. unauthenticated NTP). + */ + enum class AbsoluteSecurityQualifier : std::uint8_t + { + kNoTimeAvailable = 0, ///< No absolute time available + kNotTrustworthy, ///< Source is not trustworthy (unauthenticated) + kTrustworthy, ///< Source is trustworthy (basic validation) + kVeryTrustworthy, ///< Source is highly trustworthy (authenticated/signed) + }; + + /** + * @brief Synchronization log event types + * + * Defines event types stored in the circular log buffers (vehicle_log and abs_log). + * Each event has associated data in v1 and v2 fields of SyncLogEntry. + */ + enum class SyncLogEvent : std::uint16_t + { + kVehicleState = 1, ///< Vehicle time state change (v1=acc enum, v2=tpq enum) + kVehicleOffset = 2, ///< Vehicle time offset update (v1=offset_ns, v2=path_delay_ns) + kVehiclePeerDelay = 3, ///< gPTP peer delay measurement (v1=path_delay_ns) + kAbsState = 100, ///< Absolute time state change (v1=acc enum, v2=sec enum) + kAbsUpdate = 101, ///< Absolute time update (v1=inaccuracy_ns, v2=source: 0=kernel, 1=external, 2=NTP) + kAbsOffset = 102, ///< Absolute time offset estimate (v1=offset_ns_est, v2=inaccuracy_ns) + }; + + namespace ipc + { + + static constexpr std::size_t kVehicleLogCapacity = 256; + static constexpr std::size_t kAbsLogCapacity = 256; + + /** + * @brief Single entry in the synchronization event log + * + * Uses per-entry seqlock protocol for lock-free concurrent access between + * writer (tsyncd daemon) and readers (client applications). + * + * **Seqlock Protocol:** + * - seq is even (0, 2, 4, ...) when entry is readable + * - seq is odd (1, 3, 5, ...) when writer is updating the entry + * - Readers retry if seq is odd or changes during read + * + * All fields are atomic to prevent torn reads on architectures where + * 64-bit loads are not naturally atomic. + */ + struct SyncLogEntry final + { + std::atomic seq{0}; ///< Seqlock: even=readable, odd=writing + std::atomic monotonic_ns{0}; ///< Monotonic timestamp of event (CLOCK_MONOTONIC) + std::atomic type{0}; ///< Event type (SyncLogEvent) + std::atomic flags{0}; ///< Reserved for future use + std::atomic v1{0}; ///< Event-specific data field 1 (see SyncLogEvent) + std::atomic v2{0}; ///< Event-specific data field 2 (see SyncLogEvent) + }; + + /** + * @brief Main shared memory structure for IPC between tsyncd daemon and clients + * + * This structure is mapped into shared memory (typically /dev/shm/score_time) and + * provides lock-free access to synchronized time information using seqlock protocol. + * + * **Memory Layout:** + * - Header fields (magic, version, size) for validation + * - Vehicle time data (gPTP/IEEE 802.1AS synchronized time) + * - Absolute time data (UTC/wall-clock time from GNSS/NTP) + * - Circular event logs for diagnostics + * + * **Concurrency:** + * - Single writer (tsyncd daemon) + * - Multiple readers (client applications) + * - Uses double-buffering seqlock for vehicle/absolute time + * - Uses per-entry seqlock for log entries + * + * **Version History:** + * - v1-v3: Original implementations + * - v4: Added per-entry seqlock to prevent torn reads in logs + * + * @note All atomic fields use appropriate memory_order for lock-free correctness + * @warning Clients must check magic and version before accessing other fields + */ + struct SharedState final + { + static constexpr std::uint32_t kMagic = 0x53434F52; ///< Magic number 'SCOR' for validation + static constexpr std::uint16_t kVersion = 4; ///< Structure version (incremented on ABI break) + + std::atomic magic{kMagic}; + std::atomic version{kVersion}; + std::atomic reserved0{0}; + std::atomic struct_size{static_cast(sizeof(SharedState))}; + + + std::atomic vehicle_seq{0}; + std::atomic vehicle_base_ns{0}; + std::atomic vehicle_base_mono_ns{0}; + + std::atomic vehicle_acc{AccuracyQualifier::kNoTime}; + std::atomic vehicle_tpq{TimePointQualifier::kQM}; + + std::atomic vehicle_last_offset_ns{0}; + std::atomic vehicle_path_delay_ns{0}; + + std::atomic vehicle_log_head{0}; + std::atomic vehicle_log_dropped{0}; + SyncLogEntry vehicle_log[kVehicleLogCapacity]{}; + + std::atomic abs_seq{0}; + std::atomic abs_base_utc_ns{0}; + std::atomic abs_base_mono_ns{0}; + + std::atomic abs_acc{AbsoluteAccuracyQualifier::kInaccuracyNotAvailable}; + std::atomic abs_sec{AbsoluteSecurityQualifier::kNoTimeAvailable}; + std::atomic abs_inaccuracy_ns{0}; + std::atomic abs_offset_ns_est{0}; + std::atomic abs_jitter_ns_est{0}; + std::atomic abs_last_update_mono_ns{0}; + std::atomic abs_source{0}; + std::atomic reserved1{0}; + std::atomic reserved2{0}; + + std::atomic abs_log_head{0}; + std::atomic abs_log_dropped{0}; + SyncLogEntry abs_log[kAbsLogCapacity]{}; + }; + + /** + * @brief Write vehicle time to shared memory using seqlock protocol + * + * Called by tsyncd daemon to update vehicle time after gPTP synchronization. + * Uses seqlock protocol to allow lock-free reads by multiple clients. + * + * **Seqlock Protocol:** + * 1. Increment vehicle_seq (odd = writing in progress) + * 2. Write all vehicle time fields + * 3. Increment vehicle_seq again (even = read complete) + * + * @param s Reference to SharedState in shared memory + * @param base_vehicle_ns Vehicle time base in nanoseconds (gPTP synchronized time) + * @param base_mono_ns Monotonic time base in nanoseconds (CLOCK_MONOTONIC) + * @param acc Accuracy qualifier indicating synchronization status + * @param tpq Time point qualifier indicating ASIL level + * @param last_offset_ns Last measured offset from grandmaster in nanoseconds + * @param path_delay_ns Peer-to-peer delay in nanoseconds + * + * @note This function is noexcept and cannot fail + * @note Only tsyncd daemon should call this function (single writer assumption) + */ + inline void WriteVehicle(SharedState &s, + std::int64_t base_vehicle_ns, + std::int64_t base_mono_ns, + AccuracyQualifier acc, + TimePointQualifier tpq, + std::int64_t last_offset_ns, + std::int64_t path_delay_ns) noexcept + { + s.vehicle_seq.fetch_add(1, std::memory_order_release); + s.vehicle_base_ns.store(base_vehicle_ns, std::memory_order_release); + s.vehicle_base_mono_ns.store(base_mono_ns, std::memory_order_release); + s.vehicle_acc.store(acc, std::memory_order_release); + s.vehicle_tpq.store(tpq, std::memory_order_release); + s.vehicle_last_offset_ns.store(last_offset_ns, std::memory_order_release); + s.vehicle_path_delay_ns.store(path_delay_ns, std::memory_order_release); + s.vehicle_seq.fetch_add(1, std::memory_order_release); // Fixed: was acq_rel + } + + /** + * @brief Read vehicle time from shared memory using seqlock protocol + * + * Called by client applications to read vehicle time in a lock-free manner. + * Uses seqlock protocol with retry to handle concurrent writes. + * + * **Seqlock Read Protocol:** + * 1. Read vehicle_seq (must be even) + * 2. Read all vehicle time fields + * 3. Re-read vehicle_seq + * 4. If seq changed or was odd, retry (up to 1000 attempts) + * + * @param s Const reference to SharedState in shared memory + * @param[out] base_vehicle_ns Vehicle time base in nanoseconds + * @param[out] base_mono_ns Monotonic time base in nanoseconds + * @param[out] acc Accuracy qualifier + * @param[out] tpq Time point qualifier + * @return true if read successful, false if exceeded retry limit + * + * @note Returns false only under extreme contention (unlikely in practice) + * @note Uses 1000 retry limit for safety-critical system resilience + */ + inline bool ReadVehicle(const SharedState &s, + std::int64_t &base_vehicle_ns, + std::int64_t &base_mono_ns, + AccuracyQualifier &acc, + TimePointQualifier &tpq) + { + // Higher retry limit for safety-critical systems - 1000 attempts + // provides resilience under high contention + for (int i = 0; i < 1000; ++i) + { + const auto a = s.vehicle_seq.load(std::memory_order_acquire); + if (a & 1U) + continue; + + base_vehicle_ns = s.vehicle_base_ns.load(std::memory_order_acquire); + base_mono_ns = s.vehicle_base_mono_ns.load(std::memory_order_acquire); + acc = s.vehicle_acc.load(std::memory_order_acquire); + tpq = s.vehicle_tpq.load(std::memory_order_acquire); + + const auto b = s.vehicle_seq.load(std::memory_order_acquire); + if (a == b) + return true; + } + return false; + } + + /** + * @brief Write absolute (UTC) time to shared memory using seqlock protocol + * + * Called by tsyncd daemon to update absolute time from external sources + * (GNSS, NTP, kernel RTC). Uses same seqlock protocol as WriteVehicle. + * + * @param s Reference to SharedState in shared memory + * @param base_utc_ns UTC time base in nanoseconds since Unix epoch + * @param base_mono_ns Monotonic time base in nanoseconds (CLOCK_MONOTONIC) + * @param acc Absolute accuracy qualifier + * @param sec Security/trustworthiness qualifier + * @param inaccuracy_ns Estimated inaccuracy in nanoseconds + * @param offset_ns_est Estimated offset from true UTC in nanoseconds + * @param jitter_ns_est Estimated jitter in nanoseconds + * @param last_update_mono_ns Monotonic timestamp of last update + * @param source Time source: 0=kernel, 1=external/GNSS, 2=NTP + * + * @note This function is noexcept and cannot fail + * @note Only tsyncd daemon should call this function (single writer assumption) + */ + inline void WriteAbsolute(SharedState &s, + std::int64_t base_utc_ns, + std::int64_t base_mono_ns, + AbsoluteAccuracyQualifier acc, + AbsoluteSecurityQualifier sec, + std::int64_t inaccuracy_ns, + std::int64_t offset_ns_est, + std::int64_t jitter_ns_est, + std::int64_t last_update_mono_ns, + std::uint8_t source) noexcept + { + s.abs_seq.fetch_add(1, std::memory_order_release); + s.abs_base_utc_ns.store(base_utc_ns, std::memory_order_release); + s.abs_base_mono_ns.store(base_mono_ns, std::memory_order_release); + s.abs_acc.store(acc, std::memory_order_release); + s.abs_sec.store(sec, std::memory_order_release); + s.abs_inaccuracy_ns.store(inaccuracy_ns, std::memory_order_release); + s.abs_offset_ns_est.store(offset_ns_est, std::memory_order_release); + s.abs_jitter_ns_est.store(jitter_ns_est, std::memory_order_release); + s.abs_last_update_mono_ns.store(last_update_mono_ns, std::memory_order_release); + s.abs_source.store(source, std::memory_order_release); + s.abs_seq.fetch_add(1, std::memory_order_release); // Fixed: was acq_rel + } + + /** + * @brief Read absolute (UTC) time from shared memory using seqlock protocol + * + * Called by client applications to read absolute time in a lock-free manner. + * Uses same seqlock read protocol as ReadVehicle with 1000 retry limit. + * + * @param s Const reference to SharedState in shared memory + * @param[out] base_utc_ns UTC time base in nanoseconds since Unix epoch + * @param[out] base_mono_ns Monotonic time base in nanoseconds + * @param[out] acc Absolute accuracy qualifier + * @param[out] sec Security/trustworthiness qualifier + * @param[out] inaccuracy_ns Estimated inaccuracy in nanoseconds + * @param[out] source Time source (0=kernel, 1=external, 2=NTP) + * @return true if read successful, false if exceeded retry limit + * + * @note Returns false only under extreme contention (unlikely in practice) + */ + inline bool ReadAbsolute(const SharedState &s, + std::int64_t &base_utc_ns, + std::int64_t &base_mono_ns, + AbsoluteAccuracyQualifier &acc, + AbsoluteSecurityQualifier &sec, + std::int64_t &inaccuracy_ns, + std::uint8_t &source) + { + // Higher retry limit for safety-critical systems - 1000 attempts + // provides resilience under high contention + for (int i = 0; i < 1000; ++i) + { + const auto a = s.abs_seq.load(std::memory_order_acquire); + if (a & 1U) + continue; + + base_utc_ns = s.abs_base_utc_ns.load(std::memory_order_acquire); + base_mono_ns = s.abs_base_mono_ns.load(std::memory_order_acquire); + acc = s.abs_acc.load(std::memory_order_acquire); + sec = s.abs_sec.load(std::memory_order_acquire); + inaccuracy_ns = s.abs_inaccuracy_ns.load(std::memory_order_acquire); + source = s.abs_source.load(std::memory_order_acquire); + + const auto b = s.abs_seq.load(std::memory_order_acquire); + if (a == b) + return true; + } + return false; + } + + inline void LogVehicle(SharedState &s, + std::int64_t mono_ns, + SyncLogEvent type, + std::int64_t v1, + std::int64_t v2) + { + const auto head = s.vehicle_log_head.fetch_add(1, std::memory_order_acq_rel); + const std::size_t idx = static_cast(head % kVehicleLogCapacity); + + auto &entry = s.vehicle_log[idx]; + entry.seq.fetch_add(1, std::memory_order_release); // Odd = writing (发布"正在写入"标志) + entry.monotonic_ns.store(mono_ns, std::memory_order_relaxed); + entry.type.store(static_cast(type), std::memory_order_relaxed); + entry.flags.store(0u, std::memory_order_relaxed); + entry.v1.store(v1, std::memory_order_relaxed); + entry.v2.store(v2, std::memory_order_relaxed); + entry.seq.fetch_add(1, std::memory_order_release); // Even = complete (发布写入完成) + } + + inline void LogAbsolute(SharedState &s, + std::int64_t mono_ns, + SyncLogEvent type, + std::int64_t v1, + std::int64_t v2) + { + const auto head = s.abs_log_head.fetch_add(1, std::memory_order_acq_rel); + const std::size_t idx = static_cast(head % kAbsLogCapacity); + + auto &entry = s.abs_log[idx]; + entry.seq.fetch_add(1, std::memory_order_release); // Odd = writing (发布"正在写入"标志) + entry.monotonic_ns.store(mono_ns, std::memory_order_relaxed); + entry.type.store(static_cast(type), std::memory_order_relaxed); + entry.flags.store(0u, std::memory_order_relaxed); + entry.v1.store(v1, std::memory_order_relaxed); + entry.v2.store(v2, std::memory_order_relaxed); + entry.seq.fetch_add(1, std::memory_order_release); // Even = complete (发布写入完成) + } + + } +} diff --git a/src/common/include/score_time/ipc/shm_region.hpp b/src/common/include/score_time/ipc/shm_region.hpp new file mode 100644 index 0000000..680f00b --- /dev/null +++ b/src/common/include/score_time/ipc/shm_region.hpp @@ -0,0 +1,84 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include +#include + +namespace score_time::ipc +{ + + /** + * @brief RAII wrapper for POSIX shared memory regions + * + * Manages lifecycle of shared memory objects (/dev/shm on Linux, shared memory + * objects on QNX) with automatic cleanup. Uses shm_open + mmap for portable + * POSIX shared memory access. + * + * **Features:** + * - RAII: Automatically unmaps and closes on destruction + * - Move-only: Prevents accidental copies of memory mappings + * - Create-or-open semantics for both producer and consumer + * - Size validation for safety + * + * **Typical Usage:** + * - Producer (tsyncd): Open with create_or_open=true, writes SharedState + * - Consumer (clients): Open with create_or_open=false, reads SharedState + * + * @note Not thread-safe: Each thread should have its own ShmRegion instance + * @note Shared memory persists until explicitly unlinked or system reboot + */ + class ShmRegion final + { + public: + ShmRegion() = default; + ~ShmRegion(); + + ShmRegion(const ShmRegion &) = delete; + ShmRegion &operator=(const ShmRegion &) = delete; + + ShmRegion(ShmRegion &&other) noexcept; + ShmRegion &operator=(ShmRegion &&other) noexcept; + + /** + * @brief Open or create a shared memory region + * + * @param name Shared memory name (e.g., "/score_time") + * @param size Size of region in bytes (must match SharedState size) + * @param create_or_open If true, create if missing; if false, open existing only + * @return true if successful, false on error + * + * @note If create_or_open=true and region exists, it will be resized to match size parameter + * @note If create_or_open=false and region doesn't exist, returns false + */ + bool Open(const std::string &name, std::size_t size, bool create_or_open); + + /** + * @brief Close and unmap the shared memory region + * + * Called automatically by destructor. Safe to call multiple times. + */ + void Close(); + + void *Addr() const { return addr_; } ///< Get mapped memory address + std::size_t Size() const { return size_; } ///< Get region size in bytes + int Fd() const { return fd_; } ///< Get file descriptor (for debugging) + bool Valid() const { return addr_ != nullptr; } ///< Check if region is open and mapped + + private: + std::string name_; + int fd_ = -1; + void *addr_ = nullptr; + std::size_t size_ = 0; + }; + +} diff --git a/src/common/include/score_time/utils/pthread_lock_guard.hpp b/src/common/include/score_time/utils/pthread_lock_guard.hpp new file mode 100644 index 0000000..9d3143b --- /dev/null +++ b/src/common/include/score_time/utils/pthread_lock_guard.hpp @@ -0,0 +1,88 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include +#include +#include + +namespace score_time::utils +{ + + /** + * @brief RAII wrapper for pthread_mutex_t + * + * Automatically locks the mutex on construction and unlocks on destruction. + * This prevents deadlocks from forgotten unlocks or early returns. + * + * @note This class is not copyable or movable + * @warning Does not check pthread_mutex_lock/unlock return values. + * Assumes the mutex is properly initialized and not in an error state. + * For safety-critical systems requiring full AUTOSAR compliance, + * consider adding explicit error checks if required by your project rules. + * + * Example usage: + * @code + * pthread_mutex_t mtx; + * pthread_mutex_init(&mtx, nullptr); + * { + * PthreadLockGuard lock(&mtx); + * // Critical section - mutex is locked + * doSomething(); + * } // Mutex automatically unlocked here + * pthread_mutex_destroy(&mtx); + * @endcode + */ + class PthreadLockGuard final + { + public: + /** + * @brief Construct and lock the mutex + * @param mtx Pointer to pthread_mutex_t (must not be null) + */ + explicit PthreadLockGuard(pthread_mutex_t *mtx) noexcept : mtx_(mtx) + { + assert(mtx_ && "PthreadLockGuard: mutex pointer must not be null"); + const int ret = ::pthread_mutex_lock(mtx_); + // In safety-critical systems, lock failure is unrecoverable + if (ret != 0) + { + std::abort(); // Terminate immediately - cannot proceed without lock + } + } + + /** + * @brief Destruct and unlock the mutex + */ + ~PthreadLockGuard() noexcept + { + assert(mtx_ && "PthreadLockGuard: mutex pointer must not be null"); + const int ret = ::pthread_mutex_unlock(mtx_); + // In safety-critical systems, unlock failure is unrecoverable + if (ret != 0) + { + std::abort(); // Terminate immediately - mutex state corrupted + } + } + + // Delete copy and move operations + PthreadLockGuard(const PthreadLockGuard &) = delete; + PthreadLockGuard &operator=(const PthreadLockGuard &) = delete; + PthreadLockGuard(PthreadLockGuard &&) = delete; + PthreadLockGuard &operator=(PthreadLockGuard &&) = delete; + + private: + pthread_mutex_t *mtx_; + }; + +} // namespace score_time::utils diff --git a/src/common/include/score_time/utils/string_parser.hpp b/src/common/include/score_time/utils/string_parser.hpp new file mode 100644 index 0000000..d4168bb --- /dev/null +++ b/src/common/include/score_time/utils/string_parser.hpp @@ -0,0 +1,111 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace score_time::utils +{ + + /** + * @brief Parse integer from string without exceptions (C++17 std::from_chars) + * + * @tparam T Integer type (int, long, int64_t, uint32_t, etc.) + * @param str Input string to parse + * @param out Output parameter to store parsed value + * @return true if parsing succeeded, false otherwise + * + * @note This function never throws exceptions + */ + template + [[nodiscard]] bool ParseInteger(std::string_view str, T &out) noexcept + { + if (str.empty()) + { + return false; + } + + auto [ptr, ec] = std::from_chars(str.data(), str.data() + str.size(), out); + return ec == std::errc{} && ptr == str.data() + str.size(); + } + + /** + * @brief Parse double from string without exceptions + * + * @param str Input string to parse + * @param out Output parameter to store parsed value + * @return true if parsing succeeded, false otherwise + * + * @note Uses strtod with careful error handling since std::from_chars + * for floating point may not be available in all C++17 implementations + * @note Uses fixed-size buffer to avoid dynamic allocation and exceptions. + * Maximum input length is 127 characters. + */ + [[nodiscard]] inline bool ParseDouble(std::string_view str, double &out) noexcept + { + if (str.empty()) + { + return false; + } + + // Use fixed-size buffer to avoid std::string allocation (no exceptions) + constexpr std::size_t kMaxDoubleStrLen = 127; + if (str.size() > kMaxDoubleStrLen) + { + return false; // String too long + } + + // Copy to null-terminated buffer for strtod + char buffer[kMaxDoubleStrLen + 1]; + std::memcpy(buffer, str.data(), str.size()); + buffer[str.size()] = '\0'; + + char *end = nullptr; + errno = 0; + const double val = std::strtod(buffer, &end); + + // Check for errors: + // - errno == ERANGE: overflow or underflow + // - end != expected: not all characters consumed + if (errno == ERANGE || end != buffer + str.size()) + { + return false; + } + + out = val; + return true; + } + + /** + * @brief Parse integer from std::string (convenience overload) + */ + template + [[nodiscard]] bool ParseInteger(const std::string &str, T &out) noexcept + { + return ParseInteger(std::string_view(str), out); + } + + /** + * @brief Parse double from std::string (convenience overload) + */ + [[nodiscard]] inline bool ParseDouble(const std::string &str, double &out) + { + return ParseDouble(std::string_view(str), out); + } + +} // namespace score_time::utils diff --git a/src/common/include/score_time/utils/time_utils.hpp b/src/common/include/score_time/utils/time_utils.hpp new file mode 100644 index 0000000..f01b584 --- /dev/null +++ b/src/common/include/score_time/utils/time_utils.hpp @@ -0,0 +1,57 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include +#include +#include + +namespace score_time::utils +{ + + /** + * @brief Safely read clock time with error checking + * @param clk Clock ID (CLOCK_MONOTONIC, CLOCK_REALTIME, etc.) + * @return Nanoseconds since epoch, or nullopt on error + */ + [[nodiscard]] inline std::optional ClockNsSafe(clockid_t clk) noexcept + { + ::timespec ts{}; + if (::clock_gettime(clk, &ts) != 0) + { + return std::nullopt; + } + + // Check for overflow: tv_sec * 1e9 must fit in int64_t + // Maximum safe value: INT64_MAX / 1e9 = ~9223372036 seconds (~292 years) + constexpr std::int64_t kMaxSafeSeconds = 9223372036LL; + if (ts.tv_sec > kMaxSafeSeconds || ts.tv_sec < -kMaxSafeSeconds) + { + return std::nullopt; + } + + return static_cast(ts.tv_sec) * 1'000'000'000LL + + static_cast(ts.tv_nsec); + } + + /** + * @brief Read clock time, returning 0 on error (backward-compatible API) + * @param clk Clock ID + * @return Nanoseconds since epoch, or 0 on error + */ + inline std::int64_t ClockNs(clockid_t clk) noexcept + { + return ClockNsSafe(clk).value_or(0); + } + +} // namespace score_time::utils diff --git a/src/common/shared_memory/shm_region.cpp b/src/common/shared_memory/shm_region.cpp new file mode 100644 index 0000000..75c3c74 --- /dev/null +++ b/src/common/shared_memory/shm_region.cpp @@ -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 +********************************************************************************/ +#include "score_time/ipc/shm_region.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace score_time::ipc { + +ShmRegion::~ShmRegion() { Close(); } + +ShmRegion::ShmRegion(ShmRegion&& other) noexcept { + *this = std::move(other); +} + +ShmRegion& ShmRegion::operator=(ShmRegion&& other) noexcept { + if (this == &other) return *this; + Close(); + name_ = std::move(other.name_); + fd_ = other.fd_; + addr_ = other.addr_; + size_ = other.size_; + other.fd_ = -1; + other.addr_ = nullptr; + other.size_ = 0; + return *this; +} + +bool ShmRegion::Open(const std::string& name, std::size_t size, bool create_or_open) { + Close(); + name_ = name; + size_ = size; + + int oflag = O_RDWR; + if (create_or_open) oflag |= O_CREAT; + + fd_ = ::shm_open(name.c_str(), oflag, 0660); + if (fd_ < 0) { + return false; + } + + if (create_or_open) { + if (::ftruncate(fd_, static_cast(size_)) != 0) { + Close(); + return false; + } + } else { + // If open only, we can still accept a larger actual size. Read current size. + struct stat st{}; + if (::fstat(fd_, &st) == 0) { + constexpr std::size_t MAX_SHM_SIZE = 1024 * 1024 * 1024; // 1GB + if (st.st_size < 0 || static_cast(st.st_size) > MAX_SHM_SIZE) { + std::fprintf(stderr, "shm_region: invalid size %ld\n", static_cast(st.st_size)); + Close(); + return false; + } + if (st.st_size > 0) { + size_ = static_cast(st.st_size); + } + } + } + + addr_ = ::mmap(nullptr, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0); + if (addr_ == MAP_FAILED) { + addr_ = nullptr; + Close(); + return false; + } + return true; +} + +void ShmRegion::Close() { + if (addr_) { + ::munmap(addr_, size_); + addr_ = nullptr; + } + if (fd_ >= 0) { + ::close(fd_); + fd_ = -1; + } + size_ = 0; + name_.clear(); +} + +} // namespace score_time::ipc diff --git a/src/libscore_time/BUILD b/src/libscore_time/BUILD new file mode 100644 index 0000000..f9e4056 --- /dev/null +++ b/src/libscore_time/BUILD @@ -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 +# ******************************************************************************* +package( + default_visibility = ["//visibility:public"], +) + +config_setting( + name = "is_qnx", + constraint_values = ["//platforms:qnx"], +) + +cc_library( + name = "score_time", + srcs = [ + "time_client/score_time.cpp", + ], + hdrs = glob(["include/**/*.hpp"]), + copts = + ["-std=c++17"] + + select({ + ":is_qnx": [ + "-D_QNX710_", + "-D_QNX_SOURCE", + "-D__EXTENSIONS__", + ], + "//conditions:default": [], + }), + include_prefix = "score_time", + linkopts = select({ + ":is_qnx": [ + "-lsocket", + "-lc", + "-lc++", + ], + "//conditions:default": [ + "-lrt", + "-lpthread", + "-lstdc++", + "-lsupc++", + ], + }), + strip_include_prefix = "include", + deps = [ + "//src/common:common_ipc", + ], +) diff --git a/src/libscore_time/include/mocks.hpp b/src/libscore_time/include/mocks.hpp new file mode 100644 index 0000000..bd292db --- /dev/null +++ b/src/libscore_time/include/mocks.hpp @@ -0,0 +1,88 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include "score_time.hpp" + +#include + +namespace score_time +{ + class ManualMonotonicClock : public IMonotonicClock + { + public: + ManualMonotonicClock() = default; + explicit ManualMonotonicClock(std::int64_t start_ns) : now_ns_(start_ns) {} + + TimePoint Now() const override { return TimePoint(now_ns_.load(std::memory_order_acquire)); } + std::int64_t NowNs() const { return now_ns_.load(std::memory_order_acquire); } + void SetNowNs(std::int64_t v) { now_ns_.store(v, std::memory_order_release); } + void AdvanceNs(std::int64_t delta) { now_ns_.fetch_add(delta, std::memory_order_acq_rel); } + + private: + std::atomic now_ns_{0}; + }; + + class ManualHighPrecisionClock : public IHighPrecisionClock + { + public: + ManualHighPrecisionClock() = default; + explicit ManualHighPrecisionClock(std::int64_t start_ns) : now_ns_(start_ns) {} + + TimePoint Now() const override { return TimePoint(now_ns_.load(std::memory_order_acquire)); } + std::int64_t NowNs() const { return now_ns_.load(std::memory_order_acquire); } + void SetNowNs(std::int64_t v) { now_ns_.store(v, std::memory_order_release); } + void AdvanceNs(std::int64_t delta) { now_ns_.fetch_add(delta, std::memory_order_acq_rel); } + + private: + std::atomic now_ns_{0}; + }; + + class ManualVehicleTime : public IVehicleTime + { + public: + TimePoint Now() const override { return TimePoint(now_ns_.load(std::memory_order_acquire)); } + AccuracyQualifier GetAccuracyQualifier() const override { return acc_.load(std::memory_order_acquire); } + TimePointQualifier GetTimePointQualifier() const override { return tpq_.load(std::memory_order_acquire); } + + void SetNowNs(std::int64_t v) { now_ns_.store(v, std::memory_order_release); } + void SetAccuracy(AccuracyQualifier v) { acc_.store(v, std::memory_order_release); } + void SetTimePointQualifier(TimePointQualifier v) { tpq_.store(v, std::memory_order_release); } + + private: + std::atomic now_ns_{0}; + std::atomic acc_{AccuracyQualifier::kNoTime}; + std::atomic tpq_{TimePointQualifier::kQM}; + }; + + class ManualAbsoluteTime : public IAbsoluteTime + { + public: + TimePoint Now() const override { return TimePoint(now_utc_ns_.load(std::memory_order_acquire)); } + AbsoluteAccuracyQualifier GetAccuracyQualifier() const override { return acc_.load(std::memory_order_acquire); } + AbsoluteSecurityQualifier GetSecurityQualifier() const override { return sec_.load(std::memory_order_acquire); } + std::int64_t GetEstimatedInaccuracyNs() const override { return inacc_ns_.load(std::memory_order_acquire); } + + void SetNowUtcNs(std::int64_t v) { now_utc_ns_.store(v, std::memory_order_release); } + void SetAccuracy(AbsoluteAccuracyQualifier v) { acc_.store(v, std::memory_order_release); } + void SetSecurity(AbsoluteSecurityQualifier v) { sec_.store(v, std::memory_order_release); } + void SetEstimatedInaccuracyNs(std::int64_t v) { inacc_ns_.store(v, std::memory_order_release); } + + private: + std::atomic now_utc_ns_{0}; + std::atomic acc_{AbsoluteAccuracyQualifier::kInaccuracyNotAvailable}; + std::atomic sec_{AbsoluteSecurityQualifier::kNoTimeAvailable}; + std::atomic inacc_ns_{-1}; + }; + +} diff --git a/src/libscore_time/include/score_time.hpp b/src/libscore_time/include/score_time.hpp new file mode 100644 index 0000000..bb104be --- /dev/null +++ b/src/libscore_time/include/score_time.hpp @@ -0,0 +1,123 @@ +/******************************************************************************** +* Copyright (c) 2025 Contributors to the Eclipse Foundation +* +* See the NOTICE file(s) distributed with this work for additional +* information regarding copyright ownership. +* +* This program and the accompanying materials are made available under the +* terms of the Apache License Version 2.0 which is available at +* https://www.apache.org/licenses/LICENSE-2.0 +* +* SPDX-License-Identifier: Apache-2.0 +********************************************************************************/ +#pragma once +#include +#include +#include + +#include "score_time/ipc/shared_state.hpp" + +namespace score_time +{ + struct VehicleClockTag + { + }; + struct AbsoluteClockTag + { + }; + struct HighPrecisionClockTag + { + }; + struct MonotonicClockTag + { + }; + + template + class TimePoint final + { + public: + constexpr TimePoint() : ns_(0) {} + explicit constexpr TimePoint(std::int64_t ns_since_epoch) : ns_(ns_since_epoch) {} + constexpr std::int64_t ns_since_epoch() const { return ns_; } + + private: + std::int64_t ns_; + }; + + class IVehicleTime + { + public: + virtual ~IVehicleTime() = default; + virtual TimePoint Now() const = 0; + virtual AccuracyQualifier GetAccuracyQualifier() const = 0; + virtual TimePointQualifier GetTimePointQualifier() const = 0; + }; + + class IAbsoluteTime + { + public: + virtual ~IAbsoluteTime() = default; + virtual TimePoint Now() const = 0; + virtual AbsoluteAccuracyQualifier GetAccuracyQualifier() const = 0; + virtual AbsoluteSecurityQualifier GetSecurityQualifier() const = 0; + // Best-effort inaccuracy estimate (ns). 0 means unknown. + virtual std::int64_t GetEstimatedInaccuracyNs() const = 0; + }; + + class IHighPrecisionClock + { + public: + virtual ~IHighPrecisionClock() = default; + virtual TimePoint Now() const = 0; + }; + + class IMonotonicClock + { + public: + virtual ~IMonotonicClock() = default; + virtual TimePoint Now() const = 0; + }; + + struct Options + { + // IPC shared memory name written by tsyncd + std::string shm_name = "/score_time_vehicle_time"; + std::size_t shm_size = 16384; + + // Map high precision clock to CLOCK_REALTIME or CLOCK_TAI (if available) + enum class HighPrecisionMapping : std::uint8_t + { + kRealtime = 0, + kTAI = 1 + }; + HighPrecisionMapping high_precision_mapping = HighPrecisionMapping::kRealtime; + }; + + struct SyncLogEntry + { + std::int64_t monotonic_ns{0}; + SyncLogEvent type{SyncLogEvent::kVehicleState}; + std::int64_t v1{0}; + std::int64_t v2{0}; + }; + + class ScoreTime + { + public: + static std::unique_ptr Create(const Options &opt); + + virtual ~ScoreTime() = default; + + virtual const IVehicleTime &VehicleTime() const = 0; + virtual const IAbsoluteTime &AbsoluteTime() const = 0; + virtual const IHighPrecisionClock &HighPrecisionClock() const = 0; + virtual const IMonotonicClock &MonotonicClock() const = 0; + + virtual std::int64_t LastOffsetNs() const = 0; + virtual std::int64_t PathDelayNs() const = 0; + + virtual std::size_t ReadVehicleSyncLog(SyncLogEntry *out, std::size_t capacity) const = 0; + virtual std::size_t ReadAbsoluteSyncLog(SyncLogEntry *out, std::size_t capacity) const = 0; + }; + +} // namespace score_time diff --git a/src/libscore_time/time_client/score_time.cpp b/src/libscore_time/time_client/score_time.cpp new file mode 100644 index 0000000..ee8f684 --- /dev/null +++ b/src/libscore_time/time_client/score_time.cpp @@ -0,0 +1,269 @@ +/******************************************************************************** +* 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_time/score_time.hpp" + +#include "score_time/ipc/shm_region.hpp" +#include "score_time/utils/time_utils.hpp" + +#include +#include +#include +#include + +namespace score_time +{ + + class MonotonicClockImpl final : public IMonotonicClock + { + public: + TimePoint Now() const override + { + return TimePoint(utils::ClockNs(CLOCK_MONOTONIC)); + } + }; + + class HighPrecisionClockImpl final : public IHighPrecisionClock + { + public: + explicit HighPrecisionClockImpl(Options::HighPrecisionMapping m) : mapping_(m) {} + TimePoint Now() const override + { +#ifdef CLOCK_TAI + const clockid_t clk = (mapping_ == Options::HighPrecisionMapping::kTAI) ? CLOCK_TAI : CLOCK_REALTIME; +#else + const clockid_t clk = CLOCK_REALTIME; +#endif + return TimePoint(utils::ClockNs(clk)); + } + + private: + Options::HighPrecisionMapping mapping_; + }; + + class VehicleTimeImpl final : public IVehicleTime + { + public: + explicit VehicleTimeImpl(const ipc::SharedState *s) : s_(s) {} + + TimePoint Now() const override + { + if (!s_) + return TimePoint(0); + + std::int64_t base_vehicle{}, base_mono{}; + AccuracyQualifier acc{}; + TimePointQualifier tpq{}; + if (!ipc::ReadVehicle(*s_, base_vehicle, base_mono, acc, tpq)) + { + return TimePoint(0); + } + (void)tpq; + if (acc == AccuracyQualifier::kNoTime || acc == AccuracyQualifier::kNotSynchronized) + { + return TimePoint(0); + } + const auto mono_now = utils::ClockNs(CLOCK_MONOTONIC); + return TimePoint(base_vehicle + (mono_now - base_mono)); + } + + AccuracyQualifier GetAccuracyQualifier() const override + { + return s_ ? s_->vehicle_acc.load(std::memory_order_acquire) : AccuracyQualifier::kNoTime; + } + + TimePointQualifier GetTimePointQualifier() const override + { + return s_ ? s_->vehicle_tpq.load(std::memory_order_acquire) : TimePointQualifier::kQM; + } + + private: + const ipc::SharedState *s_; + }; + + class AbsoluteTimeImpl final : public IAbsoluteTime + { + public: + explicit AbsoluteTimeImpl(const ipc::SharedState *s) : s_(s) {} + + TimePoint Now() const override + { + if (!s_) + return TimePoint(0); + std::int64_t base_utc{}, base_mono{}, inacc{}; + AbsoluteAccuracyQualifier acc{}; + AbsoluteSecurityQualifier sec{}; + std::uint8_t source{}; + if (!ipc::ReadAbsolute(*s_, base_utc, base_mono, acc, sec, inacc, source)) + { + return TimePoint(0); + } + (void)acc; + (void)source; + if (sec == AbsoluteSecurityQualifier::kNoTimeAvailable) + { + return TimePoint(0); + } + const auto mono_now = utils::ClockNs(CLOCK_MONOTONIC); + return TimePoint(base_utc + (mono_now - base_mono)); + } + + AbsoluteAccuracyQualifier GetAccuracyQualifier() const override + { + return s_ ? s_->abs_acc.load(std::memory_order_acquire) + : AbsoluteAccuracyQualifier::kInaccuracyNotAvailable; + } + + AbsoluteSecurityQualifier GetSecurityQualifier() const override + { + return s_ ? s_->abs_sec.load(std::memory_order_acquire) + : AbsoluteSecurityQualifier::kNoTimeAvailable; + } + + std::int64_t GetEstimatedInaccuracyNs() const override + { + return s_ ? s_->abs_inaccuracy_ns.load(std::memory_order_acquire) : 0; + } + + private: + const ipc::SharedState *s_; + }; + + static std::size_t ReadLogRing(const ipc::SyncLogEntry *ring, + std::size_t capacity, + std::uint32_t head, + SyncLogEntry *out, + std::size_t out_cap) + { + if (!out || out_cap == 0) + return 0; + + const std::size_t available = (head < capacity) ? head : capacity; + const std::size_t n = (available < out_cap) ? available : out_cap; + // Newest -> oldest + for (std::size_t i = 0; i < n; ++i) + { + const std::uint32_t idx = head - 1u - static_cast(i); + const std::size_t r = static_cast(idx % capacity); + const auto &e = ring[r]; + + // Seqlock read with retry - increased to 1000 for consistency with WriteVehicle/WriteAbsolute + for (int retry = 0; retry < 1000; ++retry) + { + const auto seq_before = e.seq.load(std::memory_order_acquire); + if (seq_before & 1U) + continue; // Writer in progress + + const std::int64_t mono = e.monotonic_ns.load(std::memory_order_relaxed); + const std::uint16_t type = e.type.load(std::memory_order_relaxed); + const std::int64_t v1 = e.v1.load(std::memory_order_relaxed); + const std::int64_t v2 = e.v2.load(std::memory_order_relaxed); + + const auto seq_after = e.seq.load(std::memory_order_acquire); + if (seq_before == seq_after) + { + out[i] = SyncLogEntry{mono, static_cast(type), v1, v2}; + break; + } + } + } + return n; + } + + class ScoreTimeImpl final : public ScoreTime + { + public: + explicit ScoreTimeImpl(const Options &opt) + : opt_(opt), + high_prec_(opt.high_precision_mapping) {} + + bool Init() + { + if (!shm_.Open(opt_.shm_name, opt_.shm_size, /*create_or_open=*/false)) + { + state_ = nullptr; + vehicle_ = VehicleTimeImpl(nullptr); + absolute_ = AbsoluteTimeImpl(nullptr); + return true; + } + state_ = reinterpret_cast(shm_.Addr()); + + // Basic validation + const auto magic = state_->magic.load(std::memory_order_acquire); + const auto ver = state_->version.load(std::memory_order_acquire); + if (magic != ipc::SharedState::kMagic) + { + state_ = nullptr; + } + else if (ver != ipc::SharedState::kVersion && ver != 1) + { + // Allow reading legacy v1 (vehicle only) + state_ = nullptr; + } + + vehicle_ = VehicleTimeImpl(state_); + absolute_ = AbsoluteTimeImpl((ver == ipc::SharedState::kVersion) ? state_ : nullptr); + return true; + } + + const IVehicleTime &VehicleTime() const override { return vehicle_; } + const IAbsoluteTime &AbsoluteTime() const override { return absolute_; } + const IHighPrecisionClock &HighPrecisionClock() const override { return high_prec_; } + const IMonotonicClock &MonotonicClock() const override { return mono_; } + + std::int64_t LastOffsetNs() const override + { + return state_ ? state_->vehicle_last_offset_ns.load(std::memory_order_acquire) : 0; + } + std::int64_t PathDelayNs() const override + { + return state_ ? state_->vehicle_path_delay_ns.load(std::memory_order_acquire) : 0; + } + + std::size_t ReadVehicleSyncLog(SyncLogEntry *out, std::size_t capacity) const override + { + if (!state_) + return 0; + const auto head = state_->vehicle_log_head.load(std::memory_order_acquire); + return ReadLogRing(state_->vehicle_log, ipc::kVehicleLogCapacity, head, out, capacity); + } + std::size_t ReadAbsoluteSyncLog(SyncLogEntry *out, std::size_t capacity) const override + { + if (!state_) + return 0; + const auto ver = state_->version.load(std::memory_order_acquire); + if (ver != ipc::SharedState::kVersion) + return 0; + const auto head = state_->abs_log_head.load(std::memory_order_acquire); + return ReadLogRing(state_->abs_log, ipc::kAbsLogCapacity, head, out, capacity); + } + + private: + Options opt_; + mutable ipc::ShmRegion shm_; + ipc::SharedState *state_ = nullptr; + + MonotonicClockImpl mono_{}; + HighPrecisionClockImpl high_prec_; + VehicleTimeImpl vehicle_{nullptr}; + AbsoluteTimeImpl absolute_{nullptr}; + }; + + std::unique_ptr ScoreTime::Create(const Options &opt) + { + auto impl = std::make_unique(opt); + if (!impl->Init()) + return nullptr; + return impl; + } + +} diff --git a/src/tsyncd/BUILD b/src/tsyncd/BUILD new file mode 100644 index 0000000..5819ce2 --- /dev/null +++ b/src/tsyncd/BUILD @@ -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 +# ******************************************************************************* +package( + default_visibility = ["//visibility:public"], +) + +config_setting( + name = "is_qnx", + constraint_values = ["//platforms:qnx"], +) + +config_setting( + name = "is_linux", + constraint_values = ["@platforms//os:linux"], +) + +cc_binary( + name = "tsyncd", + srcs = + [ + "config/config_loader.cpp", + "daemon/main.cpp", + "engine/time_sync_engine.cpp", + "protocol/eth_protocol.cpp", + "protocol/gptp_protocol.cpp", + "protocol/net_identity.cpp", + "protocol/ntp_client.cpp", + "protocol/raw_socket.cpp", + ] + + # QNX-only + select({ + ":is_qnx": ["platform/qnx/qnx_raw_shim.cpp"], + "//conditions:default": [], + }) + + select({ + ":is_linux": ["platform/linux/linux_raw_shim.cpp"], + "//conditions:default": [], + }) + glob([ + "daemon/*.hpp", + "engine/*.hpp", + "protocol/*.hpp", + "config/*.hpp", + "platform/qnx/*.hpp", + "platform/linux/*.hpp", + ]), + copts = + ["-std=c++17"] + + select({ + ":is_qnx": [ + "-D_QNX710_", + "-D_QNX_SOURCE", + "-D__EXTENSIONS__", + ], + "//conditions:default": [], + }), + data = [ + "config/tsyncd.conf", + ], + includes = [ + "config", + "daemon", + "engine", + "platform/linux", + "platform/qnx", + "protocol", + ], + linkopts = select({ + ":is_qnx": [ + "-lsocket", + "-lc", + "-lc++", + ], + "//conditions:default": [ + "-lrt", + "-lpthread", + "-lstdc++", + "-lsupc++", + ], + }), + deps = [ + "//src/common:common_ipc", + "//src/libscore_time:score_time", + ], +) diff --git a/src/tsyncd/config/config_loader.cpp b/src/tsyncd/config/config_loader.cpp new file mode 100644 index 0000000..7edf94b --- /dev/null +++ b/src/tsyncd/config/config_loader.cpp @@ -0,0 +1,415 @@ +/******************************************************************************** +* 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 "config_loader.hpp" +#include "score_time/utils/string_parser.hpp" + +#include +#include +#include +#include +#include + +namespace tsyncd +{ + namespace + { + static inline void ltrim(std::string &s) + { + std::size_t i = 0; + while (i < s.size() && std::isspace(static_cast(s[i]))) + ++i; + s.erase(0, i); + } + + static inline void rtrim(std::string &s) + { + if (s.empty()) + return; + std::size_t i = s.size(); + while (i > 0 && std::isspace(static_cast(s[i - 1]))) + --i; + s.erase(i); + } + + static inline void trim(std::string &s) + { + ltrim(s); + rtrim(s); + } + + static bool iequals(const std::string &a, const std::string &b) + { + if (a.size() != b.size()) + return false; + for (std::size_t i = 0; i < a.size(); ++i) + { + if (std::tolower(static_cast(a[i])) != + std::tolower(static_cast(b[i]))) + return false; + } + return true; + } + + static bool parse_bool(const std::string &val, bool &out) + { + if (val.empty()) + return false; + + if (val == "0") + { + out = false; + return true; + } + if (val == "1") + { + out = true; + return true; + } + + if (iequals(val, "true") || iequals(val, "yes") || iequals(val, "on")) + { + out = true; + return true; + } + if (iequals(val, "false") || iequals(val, "no") || iequals(val, "off")) + { + out = false; + return true; + } + + return false; + } + + static void split_by_comma(const std::string &val, std::vector &out) + { + out.clear(); + std::size_t start = 0; + while (start < val.size()) + { + std::size_t pos = val.find(',', start); + std::string token; + if (pos == std::string::npos) + { + token = val.substr(start); + start = val.size(); + } + else + { + token = val.substr(start, pos - start); + start = pos + 1; + } + trim(token); + if (!token.empty()) + out.push_back(token); + } + } + + static bool parse_abs_mode(const std::string &val, EngineOptions::AbsMode &out) + { + if (val.empty()) + return false; + + bool is_num = true; + for (char c : val) + { + if (!std::isdigit(static_cast(c)) && c != '+' && c != '-') + { + is_num = false; + break; + } + } + + if (is_num) + { + int v = 0; + if (!score_time::utils::ParseInteger(val, v)) + { + return false; + } + if (v == 0) + { + out = EngineOptions::AbsMode::kPublishOnly; + return true; + } + if (v == 1) + { + out = EngineOptions::AbsMode::kDisciplineSystemClock; + return true; + } + return false; + } + + if (iequals(val, "publish_only") || iequals(val, "publish-only")) + { + out = EngineOptions::AbsMode::kPublishOnly; + return true; + } + if (iequals(val, "discipline") || + iequals(val, "discipline_system_clock") || + iequals(val, "discipline-system-clock")) + { + out = EngineOptions::AbsMode::kDisciplineSystemClock; + return true; + } + + return false; + } + + static void apply_kv(const std::string &key, const std::string &val, EngineOptions &opt) + { + if (key == "iface_name") + { + opt.iface_name = val; + } + else if (key == "phc_device") + { + opt.phc_device = val; + } + else if (key == "shm_name") + { + opt.shm_name = val; + } + else if (key == "shm_size") + { + std::uint64_t size_val; + if (!score_time::utils::ParseInteger(val, size_val)) + { + std::cerr << "[WARN] invalid shm_size: " << val << std::endl; + return; + } + opt.shm_size = static_cast(size_val); + } + else if (key == "abs_mode") + { + EngineOptions::AbsMode m; + if (parse_abs_mode(val, m)) + opt.abs_mode = m; + else + std::cerr << "[WARN] invalid abs_mode: " << val << std::endl; + } + else if (key == "ntp_servers") + { + std::vector v; + split_by_comma(val, v); + if (!v.empty()) + opt.ntp_servers = v; + } + else if (key == "ntp_port") + { + int port_val; + if (!score_time::utils::ParseInteger(val, port_val) || port_val < 0 || port_val > 65535) + { + std::cerr << "[WARN] invalid ntp_port: " << val << std::endl; + return; + } + opt.ntp_port = port_val; + } + else if (key == "ntp_query_interval_ms") + { + int interval_val; + if (!score_time::utils::ParseInteger(val, interval_val) || interval_val < 0) + { + std::cerr << "[WARN] invalid ntp_query_interval_ms: " << val << std::endl; + return; + } + opt.ntp_query_interval_ms = interval_val; + } + else if (key == "ntp_request_timeout_ms" || key == "ntp_timeout_ms") + { + int timeout_val; + if (!score_time::utils::ParseInteger(val, timeout_val) || timeout_val < 0) + { + std::cerr << "[WARN] invalid ntp_request_timeout_ms: " << val << std::endl; + return; + } + opt.ntp_request_timeout_ms = timeout_val; + } + else if (key == "ntp_samples_to_lock") + { + int samples_val; + if (!score_time::utils::ParseInteger(val, samples_val) || samples_val < 0) + { + std::cerr << "[WARN] invalid ntp_samples_to_lock: " << val << std::endl; + return; + } + opt.ntp_samples_to_lock = samples_val; + } + else if (key == "ntp_offset_ewma_alpha") + { + double alpha_val; + if (!score_time::utils::ParseDouble(val, alpha_val) || alpha_val < 0.0 || alpha_val > 1.0) + { + std::cerr << "[WARN] invalid ntp_offset_ewma_alpha: " << val << std::endl; + return; + } + opt.ntp_offset_ewma_alpha = alpha_val; + } + else if (key == "ntp_jitter_ewma_alpha") + { + double alpha_val; + if (!score_time::utils::ParseDouble(val, alpha_val) || alpha_val < 0.0 || alpha_val > 1.0) + { + std::cerr << "[WARN] invalid ntp_jitter_ewma_alpha: " << val << std::endl; + return; + } + opt.ntp_jitter_ewma_alpha = alpha_val; + } + else if (key == "abs_publish_interval_ms") + { + int interval_val; + if (!score_time::utils::ParseInteger(val, interval_val) || interval_val < 0) + { + std::cerr << "[WARN] invalid abs_publish_interval_ms: " << val << std::endl; + return; + } + opt.abs_publish_interval_ms = interval_val; + } + else if (key == "abs_external_enable") + { + bool b{}; + if (parse_bool(val, b)) + opt.abs_external_enable = b; + else + std::cerr << "[WARN] invalid abs_external_enable: " << val << std::endl; + } + else if (key == "abs_source_socket") + { + opt.abs_source_socket = val; + } + else if (key == "abs_source_timeout_ms" || key == "abs_timeout_ms") + { + int timeout_val; + if (!score_time::utils::ParseInteger(val, timeout_val) || timeout_val < 0) + { + std::cerr << "[WARN] invalid abs_source_timeout_ms: " << val << std::endl; + return; + } + opt.abs_source_timeout_ms = timeout_val; + } + else if (key == "pdelay_req_interval_ms" || key == "pdelay_cycle") + { + int interval_val; + if (!score_time::utils::ParseInteger(val, interval_val) || interval_val < 0) + { + std::cerr << "[WARN] invalid pdelay_req_interval_ms: " << val << std::endl; + return; + } + opt.pdelay_req_interval_ms = interval_val; + } + else if (key == "pdelay_timeout_ms") + { + int timeout_val; + if (!score_time::utils::ParseInteger(val, timeout_val) || timeout_val < 0) + { + std::cerr << "[WARN] invalid pdelay_timeout_ms: " << val << std::endl; + return; + } + opt.pdelay_timeout_ms = timeout_val; + } + else if (key == "sync_timeout_ms") + { + int timeout_val; + if (!score_time::utils::ParseInteger(val, timeout_val) || timeout_val < 0) + { + std::cerr << "[WARN] invalid sync_timeout_ms: " << val << std::endl; + return; + } + opt.sync_timeout_ms = timeout_val; + } + else if (key == "unstable_offset_threshold_ns") + { + std::int64_t threshold_val; + if (!score_time::utils::ParseInteger(val, threshold_val)) + { + std::cerr << "[WARN] invalid unstable_offset_threshold_ns: " << val << std::endl; + return; + } + opt.unstable_offset_threshold_ns = threshold_val; + } + else if (key == "jump_future_threshold_ns") + { + std::int64_t threshold_val; + if (!score_time::utils::ParseInteger(val, threshold_val)) + { + std::cerr << "[WARN] invalid jump_future_threshold_ns: " << val << std::endl; + return; + } + opt.jump_future_threshold_ns = threshold_val; + } + else + { + std::cerr << "[INFO] ignore unknown config key: " << key << std::endl; + } + } + + } // namespace + + bool LoadEngineOptionsFromFile(const std::string &path, EngineOptions &opt) + { + std::ifstream in(path); + if (!in.is_open()) + { + std::cerr << "[WARN] cannot open config file: " << path << std::endl; + return false; + } + + std::string line; + std::string section; + int line_no = 0; + + while (std::getline(in, line)) + { + ++line_no; + if (!line.empty() && line.back() == '\r') + line.pop_back(); + + std::string raw = line; + trim(line); + + if (line.empty()) + continue; + + if (line[0] == '#' || line[0] == ';') + continue; + + if (line.front() == '[' && line.back() == ']') + { + section = line.substr(1, line.size() - 2); + trim(section); + continue; + } + + auto pos = line.find('='); + if (pos == std::string::npos) + { + std::cerr << "[WARN] invalid line (no '=') at " << line_no << ": " << raw << std::endl; + continue; + } + + std::string key = line.substr(0, pos); + std::string val = line.substr(pos + 1); + trim(key); + trim(val); + if (key.empty()) + { + std::cerr << "[WARN] empty key at line " << line_no << std::endl; + continue; + } + + apply_kv(key, val, opt); + } + + return true; + } + +} // namespace tsyncd diff --git a/src/tsyncd/config/config_loader.hpp b/src/tsyncd/config/config_loader.hpp new file mode 100644 index 0000000..2d8ff9d --- /dev/null +++ b/src/tsyncd/config/config_loader.hpp @@ -0,0 +1,21 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include "time_sync_engine.hpp" +#include + +namespace tsyncd +{ + bool LoadEngineOptionsFromFile(const std::string &path, EngineOptions &opt); +} // namespace tsyncd diff --git a/src/tsyncd/config/tsyncd.conf b/src/tsyncd/config/tsyncd.conf new file mode 100644 index 0000000..c8644c4 --- /dev/null +++ b/src/tsyncd/config/tsyncd.conf @@ -0,0 +1,52 @@ +[general] +iface_name = dwc00 + +phc_device = /dev/ptp0 + +shm_name = /score_time_vehicle_time +shm_size = 4096 + +# 0 = kPublishOnly +# 1 = kDisciplineSystemClock +abs_mode = 0 + + +[ntp] +ntp_servers = pool.ntp.org + +ntp_port = 123 + +ntp_query_interval_ms = 1000 + +ntp_request_timeout_ms = 250 + +ntp_samples_to_lock = 3 + +ntp_offset_ewma_alpha = 0.2 +ntp_jitter_ewma_alpha = 0.2 + + +[abs] +abs_publish_interval_ms = 200 + +abs_external_enable = 0 + +abs_source_socket = /run/score_time/abs_time_source.sock + +abs_source_timeout_ms = 5000 + + +[pdelay] +pdelay_req_interval_ms = 1000 + +pdelay_timeout_ms = 1000 + + +[sync] +sync_timeout_ms = 1000 + + +[threshold] +unstable_offset_threshold_ns = 10000 + +jump_future_threshold_ns = 600000000 diff --git a/src/tsyncd/daemon/main.cpp b/src/tsyncd/daemon/main.cpp new file mode 100644 index 0000000..b496740 --- /dev/null +++ b/src/tsyncd/daemon/main.cpp @@ -0,0 +1,69 @@ +/******************************************************************************** +* 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 "time_sync_engine.hpp" +#include "config_loader.hpp" + +static std::atomic g_exit{false}; + +static void signal_handler(int signum) +{ + if (signum == SIGTERM || signum == SIGINT) + g_exit.store(true, std::memory_order_release); +} + +static void init_signals() +{ + std::signal(SIGTERM, signal_handler); + std::signal(SIGINT, signal_handler); +} + +int main(int argc, char **argv) +{ + init_signals(); + + tsyncd::EngineOptions opt; + + std::string config_path = "./tsyncd.conf"; + for (int i = 1; i < argc; ++i) + { + if (std::strcmp(argv[i], "--config") == 0 && i + 1 < argc) + { + config_path = argv[++i]; + } + } + + tsyncd::LoadEngineOptionsFromFile(config_path, opt); + + tsyncd::TimeSyncEngine engine(opt); + if (!engine.Start()) + { + std::fprintf(stderr, "tsyncd: failed to start\n"); + return 1; + } + + while (!g_exit.load(std::memory_order_acquire)) + { + ::sleep(1); + } + + engine.Stop(); + std::fprintf(stderr, "tsyncd: exit\n"); + return 0; +} diff --git a/src/tsyncd/daemon/tsync_types.hpp b/src/tsyncd/daemon/tsync_types.hpp new file mode 100644 index 0000000..9b9ba75 --- /dev/null +++ b/src/tsyncd/daemon/tsync_types.hpp @@ -0,0 +1,209 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include +#include +#include +#include +#include +#include + +#ifndef _QNX710_ +#include +#include +#else +constexpr int ETH_P_1588 = 0x88F7; +constexpr int ETH_P_8021Q = 0x8100; +#endif + +#ifndef PACKED +#define PACKED __attribute__((packed)) +#endif + +constexpr const char *kDefaultPhcDevice = "/dev/ptp0"; +constexpr const char *kDefaultIfaceName = "eth0"; + +constexpr const char *PTP_SRC_MAC = "02:00:00:FF:00:11"; +constexpr const char *PTP_DST_MAC = "01:80:C2:00:00:0E"; + +constexpr int MAC_ADDR_LEN = 6; +constexpr int VLAN_TAG_LEN = 4; +constexpr int GUID_OFFSET = 36; +constexpr int GUID_LEN = 8; +constexpr int MAX_MSG_QUEUE_SIZE = 5; +constexpr std::int64_t NS_PER_SEC = 1'000'000'000LL; + +constexpr int NO_ERROR = 0; + +constexpr std::uint8_t PTP_TRANSPORT_SPECIFIC = (1 << 4); +constexpr std::uint8_t PTP_VERSION = 2; + +constexpr std::uint8_t PTP_MSGTYPE_SYNC = 0x0; +constexpr std::uint8_t PTP_MSGTYPE_DELAY_REQ = 0x1; +constexpr std::uint8_t PTP_MSGTYPE_PDELAY_REQ = 0x2; +constexpr std::uint8_t PTP_MSGTYPE_PDELAY_RESP = 0x3; +constexpr std::uint8_t PTP_MSGTYPE_FOLLOW_UP = 0x8; +constexpr std::uint8_t PTP_MSGTYPE_DELAY_RESP = 0x9; +constexpr std::uint8_t PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP = 0xA; + +enum ControlField : std::uint8_t +{ + CTL_SYNC, + CTL_DELAY_REQ, + CTL_FOLLOW_UP, + CTL_DELAY_RESP, + CTL_MANAGEMENT, + CTL_OTHER +}; + +enum class TsyncState : std::uint8_t +{ + kEmpty, + kHaveSync, + kHaveFup +}; + +enum class TsyncEvent : std::uint8_t +{ + kOther, + kRecvSync, + kRecvFup +}; + +struct ClockIdentity +{ + std::uint8_t id[8]; +}; + +struct tmv_t +{ + std::int64_t ns; +}; + +struct PACKED PortIdentity +{ + ClockIdentity clockIdentity; + std::uint16_t portNumber; +}; + +struct PACKED Timestamp +{ + std::uint16_t seconds_msb; + std::uint32_t seconds_lsb; + std::uint32_t nanoseconds; +}; + +struct PACKED PTPHeader +{ + std::uint8_t tsmt; + std::uint8_t version; + std::uint16_t messageLength; + std::uint8_t domainNumber; + std::uint8_t reserved1; + std::uint8_t flagField[2]; + std::int64_t correctionField; + std::uint32_t reserved2; + PortIdentity sourcePortIdentity; + std::uint16_t sequenceId; + std::uint8_t controlField; + std::int8_t logMessageInterval; +}; + +struct PACKED SyncBody +{ + PTPHeader ptpHdr; + Timestamp originTimestamp; +}; +struct PACKED FollowUpBody +{ + PTPHeader ptpHdr; + Timestamp preciseOriginTimestamp; + std::uint8_t suffix[0]; +}; +struct PACKED PdelayReqBody +{ + PTPHeader ptpHdr; + Timestamp requestReceiptTimestamp; + PortIdentity reserved; +}; +struct PACKED PdelayRespBody +{ + PTPHeader ptpHdr; + Timestamp responseOriginTimestamp; + PortIdentity requestingPortIdentity; +}; +struct PACKED PdelayRespFollowUpBody +{ + PTPHeader ptpHdr; + Timestamp responseOriginReceiptTimestamp; + PortIdentity requestingPortIdentity; +}; + +struct PACKED MessageData +{ + std::uint8_t buffer[1500]; +}; + +struct PTPMessage +{ + union + { + PTPHeader ptpHdr; + SyncBody sync; + FollowUpBody follow_up; + PdelayReqBody pdelay_req; + PdelayRespBody pdelay_resp; + PdelayRespFollowUpBody pdelay_resp_fup; + MessageData data; + } PACKED; + + std::uint8_t msgtype = 0; + tmv_t sendHardwareTS{0}; + tmv_t parseMessageTs{0}; + tmv_t recvHardwareTS{0}; +}; + +// Ensure PTPMessage fits within Ethernet frame size constraint +static_assert(sizeof(PTPMessage) <= 1600, "PTPMessage exceeds maximum Ethernet frame size"); + +struct PTPMessageQueue +{ + PTPMessage save_message[MAX_MSG_QUEUE_SIZE]; + int save_index = 0; +}; + +struct Context +{ + int raw_fd = -1; + int phc_fd = -1; + clockid_t clk_id{}; + int pdelay_seqnum = 0; + std::int64_t path_delay = 0; + std::int64_t last_master_ts = 0; + + ClockIdentity clockIdentity{}; + + PTPMessageQueue syncMsgQueue{}; + PTPMessageQueue fupMsgQueue{}; + + // Simplified state machine cache + PTPMessage last_sync{}; + PTPMessage last_fup{}; + + PTPMessage peer_delay_req{}; + PTPMessage peer_delay_resp{}; + PTPMessage peer_delay_fup{}; + + pthread_mutex_t pdelay_lock{}; + TsyncState state = TsyncState::kEmpty; +}; diff --git a/src/tsyncd/engine/time_sync_engine.cpp b/src/tsyncd/engine/time_sync_engine.cpp new file mode 100644 index 0000000..86c3127 --- /dev/null +++ b/src/tsyncd/engine/time_sync_engine.cpp @@ -0,0 +1,1027 @@ +/******************************************************************************** +* 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 "time_sync_engine.hpp" + +#include "eth_protocol.hpp" +#include "gptp_protocol.hpp" +#include "raw_socket.hpp" +#include "net_identity.hpp" +#include "score_time/utils/pthread_lock_guard.hpp" +#include "score_time/utils/time_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef _QNX710_ +#include +#include +#include +#include +#else +#include "qnx_raw_shim.hpp" +#endif + +namespace tsyncd +{ + + namespace + { + + static void *rx_entry(void *arg) + { + if (!arg) + { + return nullptr; // Defensive: should never happen in correct usage + } + reinterpret_cast(arg)->RxLoop(); + return nullptr; + } + static void *pdelay_entry(void *arg) + { + if (!arg) + { + return nullptr; // Defensive: should never happen in correct usage + } + reinterpret_cast(arg)->PdelayLoop(); + return nullptr; + } + static void *abs_entry(void *arg) + { + if (!arg) + { + return nullptr; // Defensive: should never happen in correct usage + } + reinterpret_cast(arg)->AbsLoop(); + return nullptr; + } + + struct __attribute__((packed)) ExternalAbsMsg + { + std::uint32_t magic; + std::uint16_t version; + std::uint16_t reserved; + std::int64_t utc_ns; + std::int64_t inaccuracy_ns; + std::uint8_t sec_qual; + std::uint8_t reserved2[7]; + }; + + constexpr std::uint32_t kAbsMsgMagic = 0x54494D45U; + constexpr std::uint16_t kAbsMsgVer = 1; + + } // namespace + + TimeSyncEngine::TimeSyncEngine(const EngineOptions &opt) : opt_(opt), ctx_{} + { + ctx_.raw_fd = -1; + ctx_.phc_fd = -1; + ctx_.state = TsyncState::kEmpty; + + // Initialize mutex with error checking + const int mutex_ret = pthread_mutex_init(&ctx_.pdelay_lock, nullptr); + if (mutex_ret != 0) + { + std::fprintf(stderr, "tsyncd: pthread_mutex_init failed: %s (errno=%d)\n", + std::strerror(mutex_ret), mutex_ret); + std::abort(); // Critical failure in safety-critical system + } + + ntp::Client::Options copt; + copt.servers = opt_.ntp_servers; + copt.port = opt_.ntp_port; + copt.timeout_ms = opt_.ntp_request_timeout_ms; + ntp_client_ = ntp::Client(copt); + + ntp::Estimator::Options eopt; + eopt.samples_to_lock = opt_.ntp_samples_to_lock; + eopt.offset_ewma_alpha = opt_.ntp_offset_ewma_alpha; + eopt.jitter_ewma_alpha = opt_.ntp_jitter_ewma_alpha; + ntp_estimator_ = ntp::Estimator(eopt); + } + + TimeSyncEngine::~TimeSyncEngine() + { + Stop(); + pthread_mutex_destroy(&ctx_.pdelay_lock); + } + + bool TimeSyncEngine::Start() + { + std::cout << "[DEBUG] Start() called" << std::endl; + + // Initialize stop flag FIRST, before any initialization that might check it + stop_.store(false, std::memory_order_seq_cst); + + if (!InitShm()) + { + std::cout << "[DEBUG] InitShm() FAILED" << std::endl; + return false; + } + std::cout << "[DEBUG] InitShm() OK" << std::endl; + + if (!InitPhc()) + { + std::cout << "[DEBUG] InitPhc() FAILED" << std::endl; + return false; + } + std::cout << "[DEBUG] InitPhc() OK" << std::endl; + + if (!InitRawSocket()) + { + std::cout << "[DEBUG] InitRawSocket() FAILED" << std::endl; + return false; + } + std::cout << "[DEBUG] InitRawSocket() OK" << std::endl; + + if (!InitHwTimestamping()) + { + std::cout << "[DEBUG] InitHwTimestamping() FAILED" << std::endl; + return false; + } + std::cout << "[DEBUG] InitHwTimestamping() OK" << std::endl; + + (void)InitAbsSourceSocket(); + std::cout << "[DEBUG] InitAbsSourceSocket() OK (optional)" << std::endl; + + if (pthread_create(&rx_th_, nullptr, &rx_entry, this) != 0) + { + std::cout << "[DEBUG] RX thread creation FAILED" << std::endl; + return false; + } + rx_started_ = true; + std::cout << "[DEBUG] RX thread created OK" << std::endl; + + if (pthread_create(&pdelay_th_, nullptr, &pdelay_entry, this) != 0) + { + std::cout << "[DEBUG] PDelay thread creation FAILED" << std::endl; + Stop(); + return false; + } + pdelay_started_ = true; + std::cout << "[DEBUG] PDelay thread created OK" << std::endl; + + if (pthread_create(&abs_th_, nullptr, &abs_entry, this) != 0) + { + std::cout << "[DEBUG] Abs thread creation FAILED" << std::endl; + Stop(); + return false; + } + abs_started_ = true; + std::cout << "[DEBUG] Abs thread created OK" << std::endl; + + std::cout << "[DEBUG] Start() completed successfully" << std::endl; + return true; + } + + void TimeSyncEngine::Stop() + { + stop_.store(true, std::memory_order_seq_cst); + + if (rx_started_) + { + pthread_join(rx_th_, nullptr); + rx_started_ = false; + } + if (pdelay_started_) + { + pthread_join(pdelay_th_, nullptr); + pdelay_started_ = false; + } + if (abs_started_) + { + pthread_join(abs_th_, nullptr); + abs_started_ = false; + } + + if (abs_sock_fd_ >= 0) + { + ::close(abs_sock_fd_); + abs_sock_fd_ = -1; + } + if (ctx_.raw_fd >= 0) + { + ::close(ctx_.raw_fd); + ctx_.raw_fd = -1; + } + if (ctx_.phc_fd >= 0) + { + ::close(ctx_.phc_fd); + ctx_.phc_fd = -1; + } + shared_ = nullptr; + shm_.Close(); + } + + bool TimeSyncEngine::InitShm() + { + const std::size_t need = sizeof(score_time::ipc::SharedState); + const std::size_t size = (opt_.shm_size < need) ? need : opt_.shm_size; + if (!shm_.Open(opt_.shm_name, size, /*create_or_open=*/true)) + { + std::fprintf(stderr, "tsyncd: shm open failed (%s)\n", opt_.shm_name.c_str()); + return false; + } + + void *raw_addr = shm_.Addr(); + const std::size_t shm_size = shm_.Size(); + + // Validate alignment + if (reinterpret_cast(raw_addr) % alignof(score_time::ipc::SharedState) != 0) + { + std::fprintf(stderr, "tsyncd: shared memory not aligned\n"); + return false; + } + + // Validate size + if (shm_size < sizeof(score_time::ipc::SharedState)) + { + std::fprintf(stderr, "tsyncd: shared memory too small: %zu < %zu\n", + shm_size, sizeof(score_time::ipc::SharedState)); + return false; + } + + shared_ = reinterpret_cast(raw_addr); + + const auto magic = shared_->magic.load(std::memory_order_acquire); + const auto ver = shared_->version.load(std::memory_order_acquire); + const auto struct_size = shared_->struct_size.load(std::memory_order_acquire); + if (magic != score_time::ipc::SharedState::kMagic || + ver != score_time::ipc::SharedState::kVersion || + struct_size != sizeof(score_time::ipc::SharedState)) + { + std::memset(shared_, 0, sizeof(*shared_)); + shared_->magic.store(score_time::ipc::SharedState::kMagic, std::memory_order_release); + shared_->version.store(score_time::ipc::SharedState::kVersion, std::memory_order_release); + shared_->struct_size.store(static_cast(sizeof(score_time::ipc::SharedState)), std::memory_order_release); + shared_->vehicle_acc.store(score_time::AccuracyQualifier::kNoTime, std::memory_order_release); + shared_->vehicle_tpq.store(score_time::TimePointQualifier::kQM, std::memory_order_release); + shared_->abs_acc.store(score_time::AbsoluteAccuracyQualifier::kInaccuracyNotAvailable, std::memory_order_release); + shared_->abs_sec.store(score_time::AbsoluteSecurityQualifier::kNoTimeAvailable, std::memory_order_release); + } + return true; + } + + bool TimeSyncEngine::InitAbsSourceSocket() + { + if (!opt_.abs_external_enable) + return false; + if (opt_.abs_source_socket.empty()) + return false; + + const int fd = ::socket(AF_UNIX, SOCK_DGRAM, 0); + if (fd < 0) + return false; + + ::sockaddr_un addr{}; + addr.sun_family = AF_UNIX; + std::snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", opt_.abs_source_socket.c_str()); + + ::unlink(addr.sun_path); + if (::bind(fd, reinterpret_cast(&addr), sizeof(addr)) < 0) + { + ::close(fd); + return false; + } + + const int flags = ::fcntl(fd, F_GETFL, 0); + ::fcntl(fd, F_SETFL, flags | O_NONBLOCK); + + abs_sock_fd_ = fd; + return true; + } + + bool TimeSyncEngine::InitPhc() + { +#ifdef _QNX710_ + ctx_.phc_fd = qnx_phc_open(opt_.phc_device.c_str()); + ctx_.clk_id = CLOCK_MONOTONIC; +#else + ctx_.phc_fd = ::open(opt_.phc_device.c_str(), O_RDWR); + while (ctx_.phc_fd < 0 && !stop_.load(std::memory_order_seq_cst)) + { + std::fprintf(stderr, "tsyncd: failed to open PHC %s (%d), retry...\n", opt_.phc_device.c_str(), errno); + ::sleep(1); + ctx_.phc_fd = ::open(opt_.phc_device.c_str(), O_RDWR); + } + if (ctx_.phc_fd < 0) + return false; + + ctx_.clk_id = (~(clockid_t)(ctx_.phc_fd) << 3) | 3; +#endif + return InitClockIdentity(); + } + + bool TimeSyncEngine::InitRawSocket() + { + return setup_raw_socket(ctx_.raw_fd, opt_.iface_name.c_str()) == 0; + } + + bool TimeSyncEngine::InitHwTimestamping() + { +#ifdef _QNX710_ + return true; +#else + struct ifreq ifr{}; + struct hwtstamp_config cfg{}; + std::snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", opt_.iface_name.c_str()); + ifr.ifr_data = reinterpret_cast(&cfg); + + cfg.tx_type = HWTSTAMP_TX_ON; + cfg.rx_filter = HWTSTAMP_FILTER_ALL; + + if (::ioctl(ctx_.raw_fd, SIOCSHWTSTAMP, &ifr) < 0) + { + cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; + (void)::ioctl(ctx_.raw_fd, SIOCSHWTSTAMP, &ifr); + } + + int ts_opts = SOF_TIMESTAMPING_TX_HARDWARE | + SOF_TIMESTAMPING_RX_HARDWARE | + SOF_TIMESTAMPING_RAW_HARDWARE; + if (::setsockopt(ctx_.raw_fd, SOL_SOCKET, SO_TIMESTAMPING, &ts_opts, sizeof(ts_opts)) < 0) + { + std::fprintf(stderr, "tsyncd: setsockopt(SO_TIMESTAMPING) failed (%d)\n", errno); + return false; + } + return true; +#endif + } + + bool TimeSyncEngine::InitClockIdentity() + { + return generate_clock_identity(ctx_.clockIdentity, opt_.iface_name.c_str()) == 0; + } + + void TimeSyncEngine::RxLoop() + { + ::timespec hwts{}; + + while (!stop_.load(std::memory_order_seq_cst)) + { + const int n = raw_recvMsg(ctx_.raw_fd, rx_buffer_.data(), &hwts, /*flag=*/0); + if (n <= 0) + continue; + HandlePacket(rx_buffer_.data(), n, hwts); + } + } + + void TimeSyncEngine::PdelayLoop() + { + ::timespec next{}; + ::clock_gettime(CLOCK_MONOTONIC, &next); + + next.tv_sec += 2; + + const std::int64_t interval_ns = + (opt_.pdelay_req_interval_ms > 0 + ? static_cast(opt_.pdelay_req_interval_ms) * 1'000'000LL + : 1'000'000'000LL); + + while (!stop_.load(std::memory_order_seq_cst)) + { + ::clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, nullptr); + + if (stop_.load(std::memory_order_seq_cst)) + break; + + { + score_time::utils::PthreadLockGuard lock(&ctx_.pdelay_lock); + (void)SendPdelayRequest(); + } + + // Calculate next wake time + const std::int64_t next_ns = timespec_to_tmv(next).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 TimeSyncEngine::AbsLoop() + { + std::int64_t last_state_hash = 0; + + ::timespec next{}; + ::clock_gettime(CLOCK_MONOTONIC, &next); + const std::int64_t interval_ns = static_cast(opt_.abs_publish_interval_ms) * 1'000'000LL; + + while (!stop_.load(std::memory_order_seq_cst)) + { + const std::int64_t mono_now = score_time::utils::ClockNs(CLOCK_MONOTONIC); + if (opt_.sync_timeout_ms > 0) + { + const std::int64_t last_sync = last_sync_event_mono_ns_.load(std::memory_order_acquire); + if (last_sync > 0) + { + const std::int64_t delta = mono_now - last_sync; + const std::int64_t timeout_ns = static_cast(opt_.sync_timeout_ms) * 1'000'000LL; + if (delta > timeout_ns) + { + if (!sync_timeout_logged_.load(std::memory_order_acquire)) + { + std::cout << "[WARN] Sync timeout: last Sync/Fup older than " + << (delta / 1'000'000LL) << " ms" << std::endl; + sync_timeout_logged_.store(true, std::memory_order_release); + } + } + else + { + sync_timeout_logged_.store(false, std::memory_order_release); + } + } + } + + if (opt_.pdelay_timeout_ms > 0) + { + const bool waiting = pdelay_waiting_resp_.load(std::memory_order_acquire); + const std::int64_t req_ts = last_pdelay_req_mono_ns_.load(std::memory_order_acquire); + + if (waiting && req_ts > 0) + { + const std::int64_t delta = mono_now - req_ts; + const std::int64_t timeout_ns = static_cast(opt_.pdelay_timeout_ms) * 1'000'000LL; + + if (delta > timeout_ns) + { + if (!pdelay_timeout_logged_.load(std::memory_order_acquire)) + { + const auto cnt = pdelay_consecutive_loss_count_.load(std::memory_order_acquire) + 1; + pdelay_consecutive_loss_count_.store(cnt, std::memory_order_release); + std::cout << "[WARN] Pdelay response timeout, consecutive lost count=" + << cnt << std::endl; + pdelay_timeout_logged_.store(true, std::memory_order_release); + } + pdelay_waiting_resp_.store(false, std::memory_order_release); + } + else + { + pdelay_timeout_logged_.store(false, std::memory_order_release); + } + } + else + { + pdelay_timeout_logged_.store(false, std::memory_order_release); + } + } + + if (abs_sock_fd_ >= 0) + { + for (;;) + { + ExternalAbsMsg msg{}; + const ssize_t r = ::recv(abs_sock_fd_, &msg, sizeof(msg), 0); + if (r < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + break; + break; + } + if (static_cast(r) < sizeof(ExternalAbsMsg)) + continue; + if (msg.magic != kAbsMsgMagic || msg.version != kAbsMsgVer) + continue; + + abs_ext_.utc_ns = msg.utc_ns; + abs_ext_.inaccuracy_ns = msg.inaccuracy_ns; + abs_ext_.sec = static_cast(msg.sec_qual); + abs_ext_.mono_rx_ns = score_time::utils::ClockNs(CLOCK_MONOTONIC); + abs_ext_.valid = true; + } + } + + if (!opt_.ntp_servers.empty() && + (next_ntp_query_mono_ns_ == 0 || mono_now >= next_ntp_query_mono_ns_)) + { + auto s = ntp_client_.QueryOnce(); + if (s) + { + ntp_estimator_.Update(*s); + if (shared_) + { + const auto est = ntp_estimator_.Snapshot(); + score_time::ipc::LogAbsolute(*shared_, mono_now, score_time::SyncLogEvent::kAbsUpdate, + est.inaccuracy_ns, /*v2=*/2); + } + } + else + { + ntp_estimator_.MarkTimeout(mono_now); + } + next_ntp_query_mono_ns_ = mono_now + static_cast(opt_.ntp_query_interval_ms) * 1'000'000LL; + } + + const bool ext_ok = + (abs_sock_fd_ >= 0) && abs_ext_.valid && + (mono_now - abs_ext_.mono_rx_ns) <= (static_cast(opt_.abs_source_timeout_ms) * 1'000'000LL); + + if (ext_ok) + { + PublishAbsoluteFromExternal(); + } + else + { + PublishAbsoluteFromNtp(); + } + + if (shared_) + { + const auto acc = shared_->abs_acc.load(std::memory_order_acquire); + const auto sec = shared_->abs_sec.load(std::memory_order_acquire); + const auto src = shared_->abs_source.load(std::memory_order_acquire); + const std::int64_t h = (static_cast(acc) << 32) | + (static_cast(sec) << 16) | + static_cast(src); + if (h != last_state_hash) + { + score_time::ipc::LogAbsolute(*shared_, mono_now, score_time::SyncLogEvent::kAbsState, + static_cast(acc), static_cast(sec)); + last_state_hash = h; + } + } + + // Calculate next wake time + const std::int64_t next_ns = timespec_to_tmv(next).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); + + ::clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, nullptr); + } + } + + score_time::AbsoluteAccuracyQualifier TimeSyncEngine::MapInaccuracyToQual(std::int64_t inacc_ns) const + { + if (inacc_ns <= 0) + return score_time::AbsoluteAccuracyQualifier::kInaccuracyNotAvailable; + + const std::int64_t s = 1'000'000'000LL; + const std::int64_t min = 60LL * s; + const std::int64_t h = 60LL * min; + const std::int64_t d = 24LL * h; + + if (inacc_ns > d) + return score_time::AbsoluteAccuracyQualifier::kInaccGreaterThan24h; + if (inacc_ns > h) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan24h; + if (inacc_ns > 15LL * min) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan1h; + if (inacc_ns > 60LL * s) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan15min; + if (inacc_ns > 10LL * s) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan60s; + if (inacc_ns > 1LL * s) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan10s; + if (inacc_ns > 500'000'000LL) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan1s; + if (inacc_ns > 100'000'000LL) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan500ms; + if (inacc_ns > 50'000'000LL) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan100ms; + if (inacc_ns > 10'000'000LL) + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan50ms; + return score_time::AbsoluteAccuracyQualifier::kInaccLessThan10ms; + } + + void TimeSyncEngine::PublishAbsoluteFromExternal() + { + if (!shared_) + return; + const std::int64_t mono_now = score_time::utils::ClockNs(CLOCK_MONOTONIC); + const std::int64_t utc = abs_ext_.utc_ns; + const std::int64_t inacc = abs_ext_.inaccuracy_ns; + const auto acc = MapInaccuracyToQual(inacc); + const auto sec = abs_ext_.sec; + score_time::ipc::WriteAbsolute(*shared_, utc, mono_now, acc, sec, inacc, + /*offset_ns_est=*/0, + /*jitter_ns_est=*/0, + /*last_update_mono_ns=*/abs_ext_.mono_rx_ns, + /*source=*/1); + score_time::ipc::LogAbsolute(*shared_, mono_now, score_time::SyncLogEvent::kAbsUpdate, inacc, 1); + } + + void TimeSyncEngine::PublishAbsoluteFromNtp() + { + if (!shared_) + return; + + const std::int64_t mono_now = score_time::utils::ClockNs(CLOCK_MONOTONIC); + const auto est = ntp_estimator_.Snapshot(); + + if (!est.locked) + { + score_time::ipc::WriteAbsolute(*shared_, 0, 0, + score_time::AbsoluteAccuracyQualifier::kInaccuracyNotAvailable, + score_time::AbsoluteSecurityQualifier::kNoTimeAvailable, + 0, 0, 0, est.last_update_mono_ns, + /*source=*/2); + return; + } + + const std::int64_t rt_now = score_time::utils::ClockNs(CLOCK_REALTIME); + const std::int64_t utc_est = rt_now + est.offset_ns; + + const auto acc = MapInaccuracyToQual(est.inaccuracy_ns); + const auto sec = score_time::AbsoluteSecurityQualifier::kNotTrustworthy; + + score_time::ipc::WriteAbsolute(*shared_, utc_est, mono_now, acc, sec, + est.inaccuracy_ns, est.offset_ns, est.jitter_ns, est.last_update_mono_ns, + /*source=*/2); + + score_time::ipc::LogAbsolute(*shared_, mono_now, score_time::SyncLogEvent::kAbsOffset, + est.offset_ns, est.inaccuracy_ns); + } + + void TimeSyncEngine::HandlePacket(const unsigned char *frame, int frame_len, const ::timespec &hwts) + { + int eth_offset = 0; + PTPMessage msg{}; + ::ethhdr eth{}; + parse_ethernet_header(frame, eth, eth_offset); + if (frame_len <= eth_offset) + return; + + if (!parse_gptp_message(frame + eth_offset, static_cast(frame_len - eth_offset), msg)) + return; + + { + const std::uint8_t tsmt = msg.ptpHdr.tsmt; + const std::uint8_t transportSpecific = static_cast(tsmt & 0xF0U); + if (transportSpecific != PTP_TRANSPORT_SPECIFIC) + { + std::cout << "[WARN] Invalid transportSpecific field: " + << static_cast(transportSpecific) + << " (expected " << static_cast(PTP_TRANSPORT_SPECIFIC) << ")" + << std::endl; + } + } + + switch (msg.msgtype) + { + case PTP_MSGTYPE_SYNC: + msg.recvHardwareTS = timespec_to_tmv(hwts); + SyncFupStateMachine(TsyncEvent::kRecvSync, msg); + break; + case PTP_MSGTYPE_FOLLOW_UP: + msg.parseMessageTs = Timestamp_to_tmv(msg.follow_up.preciseOriginTimestamp); + SyncFupStateMachine(TsyncEvent::kRecvFup, msg); + break; + case PTP_MSGTYPE_PDELAY_REQ: + { + score_time::utils::PthreadLockGuard lock(&ctx_.pdelay_lock); + (void)SendPdelayRespAndFup(msg); + } + break; + case PTP_MSGTYPE_PDELAY_RESP: + msg.recvHardwareTS = timespec_to_tmv(hwts); + msg.parseMessageTs = Timestamp_to_tmv(msg.pdelay_resp.responseOriginTimestamp); + ctx_.peer_delay_resp = msg; + break; + case PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP: + msg.parseMessageTs = Timestamp_to_tmv(msg.pdelay_resp_fup.responseOriginReceiptTimestamp); + ctx_.peer_delay_fup = msg; + { + score_time::utils::PthreadLockGuard lock(&ctx_.pdelay_lock); + ComputePeerDelay(); + } + break; + default: + break; + } + } + + void TimeSyncEngine::SyncFupStateMachine(TsyncEvent ev, const PTPMessage &msg) + { + switch (ctx_.state) + { + case TsyncState::kEmpty: + if (ev == TsyncEvent::kRecvSync) + { + ctx_.last_sync = msg; + ctx_.state = TsyncState::kHaveSync; + } + else if (ev == TsyncEvent::kRecvFup) + { + ctx_.last_fup = msg; + ctx_.state = TsyncState::kHaveFup; + } + break; + case TsyncState::kHaveSync: + if (ev == TsyncEvent::kRecvSync) + { + ctx_.last_sync = msg; + } + else if (ev == TsyncEvent::kRecvFup) + { + const auto &sync = ctx_.last_sync; + if (sync.ptpHdr.sequenceId == msg.ptpHdr.sequenceId) + { + PortSynchronize(sync.recvHardwareTS, + msg.parseMessageTs, + correction_to_tmv(sync.ptpHdr.correctionField), + correction_to_tmv(msg.ptpHdr.correctionField), + msg.ptpHdr.sequenceId); + ctx_.state = TsyncState::kEmpty; + } + else + { + ctx_.last_fup = msg; + ctx_.state = TsyncState::kHaveFup; + } + } + break; + case TsyncState::kHaveFup: + if (ev == TsyncEvent::kRecvFup) + { + ctx_.last_fup = msg; + } + else if (ev == TsyncEvent::kRecvSync) + { + ctx_.last_sync = msg; + ctx_.state = TsyncState::kHaveSync; + } + break; + } + } + + int TimeSyncEngine::SendPdelayRequest() + { + PTPMessage req{}; + ::timespec hwts{}; + + req.ptpHdr.tsmt = PTP_MSGTYPE_PDELAY_REQ | PTP_TRANSPORT_SPECIFIC; + req.ptpHdr.version = PTP_VERSION; + req.ptpHdr.domainNumber = 0; + req.ptpHdr.messageLength = htons(sizeof(PdelayReqBody)); + req.ptpHdr.reserved1 = 0; + req.ptpHdr.flagField[0] = 0; + req.ptpHdr.flagField[1] = 0; + req.ptpHdr.correctionField = htobe64(0); + req.ptpHdr.reserved2 = 0; + req.ptpHdr.sourcePortIdentity.clockIdentity = ctx_.clockIdentity; + req.ptpHdr.sourcePortIdentity.portNumber = htons(0x01); + req.ptpHdr.sequenceId = htons(static_cast(ctx_.pdelay_seqnum++)); + req.ptpHdr.controlField = CTL_OTHER; + req.ptpHdr.logMessageInterval = 0x7F; + + ctx_.peer_delay_req = req; + ctx_.peer_delay_req.ptpHdr.sequenceId = ntohs(ctx_.peer_delay_req.ptpHdr.sequenceId); + + unsigned int len = sizeof(PdelayReqBody); + add_ethernet_header(reinterpret_cast(&req), len); + const int r = raw_sendMsg(ctx_.raw_fd, &req, static_cast(len), &hwts); + ctx_.peer_delay_req.sendHardwareTS = timespec_to_tmv(hwts); + + const std::int64_t mono_now = score_time::utils::ClockNs(CLOCK_MONOTONIC); + last_pdelay_req_mono_ns_.store(mono_now, std::memory_order_release); + pdelay_waiting_resp_.store(true, std::memory_order_release); + + (void)r; + return r; + } + + int TimeSyncEngine::SendPdelayRespAndFup(const PTPMessage &req_in) + { + PTPMessage rsp{}; + PTPMessage fup{}; + ::timespec hwts{}; + + rsp.ptpHdr.tsmt = PTP_MSGTYPE_PDELAY_RESP | PTP_TRANSPORT_SPECIFIC; + rsp.ptpHdr.version = PTP_VERSION; + rsp.ptpHdr.domainNumber = req_in.ptpHdr.domainNumber; + rsp.ptpHdr.messageLength = htons(sizeof(PdelayRespBody)); + rsp.ptpHdr.reserved1 = 0; + rsp.ptpHdr.flagField[0] = 0x02; + rsp.ptpHdr.flagField[1] = 0x00; + rsp.ptpHdr.correctionField = req_in.ptpHdr.correctionField; + rsp.ptpHdr.reserved2 = 0; + rsp.ptpHdr.sourcePortIdentity.clockIdentity = ctx_.clockIdentity; + rsp.ptpHdr.sourcePortIdentity.portNumber = htons(0x01); + rsp.ptpHdr.sequenceId = htons(req_in.ptpHdr.sequenceId); + rsp.ptpHdr.controlField = CTL_OTHER; + rsp.ptpHdr.logMessageInterval = 0x7F; + + rsp.pdelay_resp.responseOriginTimestamp = tmv_to_Timestamp(req_in.recvHardwareTS); + rsp.pdelay_resp.requestingPortIdentity.clockIdentity = req_in.ptpHdr.sourcePortIdentity.clockIdentity; + rsp.pdelay_resp.requestingPortIdentity.portNumber = htons(req_in.ptpHdr.sourcePortIdentity.portNumber); + + rsp.pdelay_resp.responseOriginTimestamp.seconds_lsb = htonl(rsp.pdelay_resp.responseOriginTimestamp.seconds_lsb); + rsp.pdelay_resp.responseOriginTimestamp.seconds_msb = htons(rsp.pdelay_resp.responseOriginTimestamp.seconds_msb); + rsp.pdelay_resp.responseOriginTimestamp.nanoseconds = htonl(rsp.pdelay_resp.responseOriginTimestamp.nanoseconds); + + unsigned int len = sizeof(PdelayRespBody); + add_ethernet_header(reinterpret_cast(&rsp), len); + const int r1 = raw_sendMsg(ctx_.raw_fd, &rsp, static_cast(len), &hwts); + const tmv_t rsp_ts = timespec_to_tmv(hwts); + + fup.ptpHdr.tsmt = PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP | PTP_TRANSPORT_SPECIFIC; + fup.ptpHdr.version = PTP_VERSION; + fup.ptpHdr.domainNumber = req_in.ptpHdr.domainNumber; + fup.ptpHdr.messageLength = htons(sizeof(PdelayRespFollowUpBody)); + fup.ptpHdr.reserved1 = 0; + fup.ptpHdr.flagField[0] = 0; + fup.ptpHdr.flagField[1] = 0; + fup.ptpHdr.correctionField = req_in.ptpHdr.correctionField; + fup.ptpHdr.reserved2 = 0; + fup.ptpHdr.sourcePortIdentity.clockIdentity = ctx_.clockIdentity; + fup.ptpHdr.sourcePortIdentity.portNumber = htons(0x01); + fup.ptpHdr.sequenceId = htons(req_in.ptpHdr.sequenceId); + fup.ptpHdr.controlField = CTL_OTHER; + fup.ptpHdr.logMessageInterval = 0x7F; + + fup.pdelay_resp_fup.responseOriginReceiptTimestamp = tmv_to_Timestamp(rsp_ts); + fup.pdelay_resp_fup.requestingPortIdentity.clockIdentity = req_in.ptpHdr.sourcePortIdentity.clockIdentity; + fup.pdelay_resp_fup.requestingPortIdentity.portNumber = htons(req_in.ptpHdr.sourcePortIdentity.portNumber); + + fup.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_lsb = htonl(fup.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_lsb); + fup.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_msb = htons(fup.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_msb); + fup.pdelay_resp_fup.responseOriginReceiptTimestamp.nanoseconds = htonl(fup.pdelay_resp_fup.responseOriginReceiptTimestamp.nanoseconds); + + len = sizeof(PdelayRespFollowUpBody); + add_ethernet_header(reinterpret_cast(&fup), len); + const int r2 = raw_sendMsg(ctx_.raw_fd, &fup, static_cast(len), &hwts); + (void)r1; + return r2; + } + + void TimeSyncEngine::ComputePeerDelay() + { + auto &req = ctx_.peer_delay_req; + auto &rsp = ctx_.peer_delay_resp; + auto &fup = ctx_.peer_delay_fup; + + if (req.ptpHdr.sequenceId != rsp.ptpHdr.sequenceId) + return; + if (rsp.ptpHdr.sequenceId != fup.ptpHdr.sequenceId) + return; + + const tmv_t t1 = req.sendHardwareTS; + const tmv_t t2 = rsp.parseMessageTs; + const tmv_t t3 = fup.parseMessageTs; + const tmv_t t4 = rsp.recvHardwareTS; + + const tmv_t c1 = correction_to_tmv(rsp.ptpHdr.correctionField); + const tmv_t c2 = correction_to_tmv(fup.ptpHdr.correctionField); + const tmv_t t3c{t3.ns + c1.ns + c2.ns}; + + ctx_.path_delay = ((t2.ns - t1.ns) + (t4.ns - t3c.ns)) / 2; + std::cout << "[DEBUG] ComputePeerDelay: t1=" << t1.ns + << " t2=" << t2.ns + << " t3=" << t3.ns + << " t4=" << t4.ns + << " path_delay=" << ctx_.path_delay << std::endl; + + const std::int64_t mono_now = score_time::utils::ClockNs(CLOCK_MONOTONIC); + last_pdelay_event_mono_ns_.store(mono_now, std::memory_order_release); + pdelay_waiting_resp_.store(false, std::memory_order_release); + pdelay_consecutive_loss_count_.store(0, std::memory_order_release); + + if (shared_) + { + shared_->vehicle_path_delay_ns.store(ctx_.path_delay, std::memory_order_release); + score_time::ipc::LogVehicle(*shared_, mono_now, score_time::SyncLogEvent::kVehiclePeerDelay, + ctx_.path_delay, 0); + } + } + + void TimeSyncEngine::PortSynchronize(const tmv_t &sync_hw_ts, + const tmv_t &fup_msg_ts, + const tmv_t &sync_corr, + const tmv_t &fup_corr, + std::uint16_t seq_id) + { + const std::int64_t master_ns = fup_msg_ts.ns + sync_corr.ns + fup_corr.ns; + if (master_ns <= 0) + return; + + score_time::AccuracyQualifier acc = score_time::AccuracyQualifier::kSynchronized; + + const std::int64_t delta = master_ns - ctx_.last_master_ts; + if (ctx_.last_master_ts != 0 && delta <= 0) + { + acc = score_time::AccuracyQualifier::kTimeJumpDetected; + } + else if (ctx_.last_master_ts != 0 && delta > opt_.jump_future_threshold_ns) + { + acc = score_time::AccuracyQualifier::kUnstable; + } + + const std::int64_t offset = sync_hw_ts.ns - master_ns; + + std::cout << "[DEBUG] PortSynchronize: sync_hw_ts=" << sync_hw_ts.ns + << " master_ns=" << master_ns + << " offset=" << offset << std::endl; + + if (std::llabs(offset) > opt_.unstable_offset_threshold_ns && + acc == score_time::AccuracyQualifier::kSynchronized) + { + acc = score_time::AccuracyQualifier::kUnstable; + } + +#ifndef _QNX710_ + if (ctx_.phc_fd >= 0) + { + ::timespec ts{}; + if (::clock_gettime(ctx_.clk_id, &ts) == 0) + { + ts.tv_nsec -= offset; + normalize_timespec(ts); + if (::clock_settime(ctx_.clk_id, &ts) != 0) + { + std::fprintf(stderr, "tsyncd: clock_settime failed: %s\n", std::strerror(errno)); + acc = score_time::AccuracyQualifier::kNotSynchronized; + } + } + } +#else + (void)qnx_phc_adjtime_step(ctx_.phc_fd, -offset); +#endif + + ctx_.last_master_ts = master_ns; + + const score_time::TimePointQualifier tpq = + (acc == score_time::AccuracyQualifier::kSynchronized) + ? score_time::TimePointQualifier::kASIL_B + : score_time::TimePointQualifier::kQM; + + const std::int64_t mono_ns = score_time::utils::ClockNs(CLOCK_MONOTONIC); + + last_sync_event_mono_ns_.store(mono_ns, std::memory_order_release); + + if (shared_) + { + const auto prev_base = shared_->vehicle_base_ns.load(std::memory_order_acquire); + score_time::AccuracyQualifier acc2 = acc; + if (prev_base != 0 && master_ns < prev_base) + { + acc2 = score_time::AccuracyQualifier::kTimeJumpDetected; + } + + score_time::ipc::WriteVehicle(*shared_, master_ns, mono_ns, acc2, tpq, offset, ctx_.path_delay); + score_time::ipc::LogVehicle(*shared_, mono_ns, score_time::SyncLogEvent::kVehicleOffset, offset, ctx_.path_delay); + score_time::ipc::LogVehicle(*shared_, mono_ns, score_time::SyncLogEvent::kVehicleState, + static_cast(acc2), static_cast(tpq)); + } + + (void)seq_id; + } + + tmv_t TimeSyncEngine::timespec_to_tmv(const ::timespec &ts) + { + return tmv_t{static_cast(ts.tv_sec) * NS_PER_SEC + ts.tv_nsec}; + } + + tmv_t TimeSyncEngine::correction_to_tmv(std::int64_t corr) + { + return tmv_t{corr >> 16}; + } + + tmv_t TimeSyncEngine::Timestamp_to_tmv(const Timestamp &ts) + { + const std::uint64_t sec = (static_cast(ts.seconds_msb) << 32) | + static_cast(ts.seconds_lsb); + return tmv_t{static_cast(sec * static_cast(NS_PER_SEC) + ts.nanoseconds)}; + } + + Timestamp TimeSyncEngine::tmv_to_Timestamp(const tmv_t &x) + { + 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 >> 32) & 0xFFFFULL); + t.nanoseconds = static_cast(nsec); + return t; + } + + void TimeSyncEngine::normalize_timespec(::timespec &ts) + { + while (ts.tv_nsec >= 1'000'000'000L) + { + ts.tv_nsec -= 1'000'000'000L; + ts.tv_sec += 1; + } + while (ts.tv_nsec < 0) + { + ts.tv_nsec += 1'000'000'000L; + ts.tv_sec -= 1; + } + } + +} diff --git a/src/tsyncd/engine/time_sync_engine.hpp b/src/tsyncd/engine/time_sync_engine.hpp new file mode 100644 index 0000000..963806e --- /dev/null +++ b/src/tsyncd/engine/time_sync_engine.hpp @@ -0,0 +1,210 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include "tsync_types.hpp" +#include "ntp_client.hpp" +#include "score_time/ipc/shared_state.hpp" +#include "score_time/ipc/shm_region.hpp" +#include +#include +#include +#include + +namespace tsyncd +{ + + /** + * @brief Configuration options for TimeSyncEngine + * + * Contains all configurable parameters for the tsyncd daemon including: + * - gPTP network interface and PHC device + * - Shared memory configuration + * - NTP client settings for absolute time + * - External time source configuration + * - Timeout and threshold values + */ + struct EngineOptions + { + std::string iface_name = kDefaultIfaceName; ///< Network interface for gPTP (e.g., "eth0") + std::string phc_device = kDefaultPhcDevice; ///< PTP Hardware Clock device (e.g., "/dev/ptp0") + std::string shm_name = "/score_time_vehicle_time"; ///< Shared memory object name + std::size_t shm_size = 4096; ///< Shared memory size in bytes + + /** + * @brief Absolute time operation mode + */ + enum class AbsMode : std::uint8_t + { + kPublishOnly = 0, ///< Only publish to shared memory, don't adjust system clock + kDisciplineSystemClock = 1 ///< Discipline system clock (requires CAP_SYS_TIME) + }; + AbsMode abs_mode = AbsMode::kPublishOnly; + + // NTP client configuration + std::vector ntp_servers = {"pool.ntp.org"}; ///< NTP server hostnames/IPs + int ntp_port = 123; ///< NTP server port (default 123) + int ntp_query_interval_ms = 1000; ///< NTP query interval in milliseconds + int ntp_request_timeout_ms = 250; ///< NTP request timeout in milliseconds + int ntp_samples_to_lock = 3; ///< Samples needed before considering NTP locked + double ntp_offset_ewma_alpha = 0.2; ///< EWMA smoothing factor for offset (0-1) + double ntp_jitter_ewma_alpha = 0.2; ///< EWMA smoothing factor for jitter (0-1) + + int abs_publish_interval_ms = 200; ///< Absolute time publish interval to shared memory + + // External absolute time source configuration + bool abs_external_enable = false; ///< Enable external time source (GNSS, etc.) + std::string abs_source_socket = "/run/score_time/abs_time_source.sock"; ///< Unix socket path + int abs_source_timeout_ms = 5000; ///< External source timeout in milliseconds + + int pdelay_req_interval_ms = 1000; ///< gPTP peer delay request interval + + // Timeout thresholds (0 = disabled) + int sync_timeout_ms = 0; ///< Sync message timeout (detect master loss) + int pdelay_timeout_ms = 0; ///< Peer delay timeout (detect link down) + + // Time quality thresholds + std::int64_t unstable_offset_threshold_ns = 10'000; ///< Offset threshold for unstable state + std::int64_t jump_future_threshold_ns = 600'000'000; ///< Jump detection threshold (600ms) + }; + + /** + * @brief Main time synchronization engine for tsyncd daemon + * + * Implements gPTP (IEEE 802.1AS) slave functionality and publishes synchronized + * time to shared memory for IPC with client applications. + * + * **Architecture:** + * - RxLoop: Receives and processes gPTP messages (Sync, Follow_Up, Pdelay_Resp, etc.) + * - PdelayLoop: Periodically sends peer delay requests to measure link delay + * - AbsLoop: Queries NTP/external sources and publishes absolute time + * + * **Thread Safety:** + * - Three worker threads managed by Start()/Stop() + * - Uses mutexes for internal state synchronization + * - Publishes to shared memory using lock-free seqlock protocol + * + * **Lifecycle:** + * 1. Construct with EngineOptions + * 2. Call Start() to initialize resources and launch threads + * 3. Threads run until Stop() is called + * 4. Destructor ensures cleanup + * + * @note This class is move-only (implicitly via deleted copy constructors) + * @warning Only one instance should run per network interface + */ + class TimeSyncEngine final + { + public: + explicit TimeSyncEngine(const EngineOptions &opt); + ~TimeSyncEngine(); + + /** + * @brief Start the synchronization engine + * + * Initializes PHC, raw socket, shared memory, and launches worker threads. + * + * @return true if all initialization succeeded and threads started + * @return false if any initialization failed (check logs for details) + * + * @note Acquires resources (file descriptors, shared memory, threads) + * @note If Start() fails, Stop() is called automatically to clean up + */ + bool Start(); + + /** + * @brief Stop the synchronization engine + * + * Signals worker threads to stop, waits for them to exit, and releases resources. + * Safe to call multiple times or even if Start() failed. + * + * @note Blocks until all threads have exited (may take up to ~1 second) + */ + void Stop(); + + void RxLoop(); ///< Worker thread: receive and process gPTP messages + void PdelayLoop(); ///< Worker thread: send periodic peer delay requests + void AbsLoop(); ///< Worker thread: query and publish absolute time + + private: + bool InitPhc(); + bool InitRawSocket(); + bool InitHwTimestamping(); + bool InitClockIdentity(); + bool InitShm(); + bool InitAbsSourceSocket(); + + void PublishAbsoluteFromNtp(); + void PublishAbsoluteFromExternal(); + score_time::AbsoluteAccuracyQualifier MapInaccuracyToQual(std::int64_t inacc_ns) const; + + void HandlePacket(const unsigned char *frame, int frame_len, const ::timespec &hwts); + void SyncFupStateMachine(TsyncEvent ev, const PTPMessage &msg); + int SendPdelayRequest(); + int SendPdelayRespAndFup(const PTPMessage &req); + + void ComputePeerDelay(); + + void PortSynchronize(const tmv_t &sync_hw_ts, + const tmv_t &fup_msg_ts, + const tmv_t &sync_corr, + const tmv_t &fup_corr, + std::uint16_t seq_id); + + static tmv_t timespec_to_tmv(const ::timespec &ts); + static tmv_t correction_to_tmv(std::int64_t corr); + static tmv_t Timestamp_to_tmv(const Timestamp &ts); + static Timestamp tmv_to_Timestamp(const tmv_t &x); + static void normalize_timespec(::timespec &ts); + + private: + EngineOptions opt_; + std::atomic stop_{false}; + + Context ctx_{}; + + score_time::ipc::ShmRegion shm_; + score_time::ipc::SharedState *shared_ = nullptr; + + ntp::Client ntp_client_{ntp::Client::Options{}}; + ntp::Estimator ntp_estimator_{ntp::Estimator::Options{}}; + std::int64_t next_ntp_query_mono_ns_ = 0; + + std::atomic last_sync_event_mono_ns_{0}; + std::atomic last_pdelay_event_mono_ns_{0}; + std::atomic last_pdelay_req_mono_ns_{0}; + std::atomic pdelay_waiting_resp_{false}; + std::atomic pdelay_consecutive_loss_count_{0}; + std::atomic sync_timeout_logged_{false}; + std::atomic pdelay_timeout_logged_{false}; + + int abs_sock_fd_ = -1; + struct ExternalAbsSample + { + std::int64_t utc_ns{0}; + std::int64_t inaccuracy_ns{0}; + score_time::AbsoluteSecurityQualifier sec{score_time::AbsoluteSecurityQualifier::kNoTimeAvailable}; + std::int64_t mono_rx_ns{0}; + bool valid{false}; + } abs_ext_; + + pthread_t rx_th_{}; + pthread_t pdelay_th_{}; + pthread_t abs_th_{}; + bool rx_started_ = false; + bool pdelay_started_ = false; + bool abs_started_ = false; + + std::array rx_buffer_{}; + }; + +} diff --git a/src/tsyncd/platform/linux/linux_raw_shim.cpp b/src/tsyncd/platform/linux/linux_raw_shim.cpp new file mode 100644 index 0000000..05d7d5c --- /dev/null +++ b/src/tsyncd/platform/linux/linux_raw_shim.cpp @@ -0,0 +1,171 @@ +/******************************************************************************** +* 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 "linux_raw_shim.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +namespace { + +static void clear_msg_errqueue(int sockfd) +{ + char buf[2048]; + struct iovec iov{}; + struct msghdr msg{}; + char control[2048]; + + iov.iov_base = buf; + iov.iov_len = sizeof(buf); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + while (true) + { + const ssize_t len = ::recvmsg(sockfd, &msg, MSG_ERRQUEUE); + if (len < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + break; + break; + } + } +} + +} // namespace + +extern "C" { + +int linux_raw_open(const char *ifname) +{ + int fd = ::socket(AF_PACKET, SOCK_RAW, htons(ETH_P_1588)); + if (fd < 0) + { + perror("socket(AF_PACKET) failed"); + return -1; + } + + struct ifreq ifr{}; + std::strncpy(ifr.ifr_name, ifname, IFNAMSIZ - 1); + if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) + { + perror("ioctl(SIOCGIFINDEX) failed"); + ::close(fd); + return -1; + } + + struct sockaddr_ll sock_address{}; + sock_address.sll_family = AF_PACKET; + sock_address.sll_protocol = htons(ETH_P_1588); + sock_address.sll_ifindex = ifr.ifr_ifindex; + if (::bind(fd, reinterpret_cast(&sock_address), sizeof(sock_address)) < 0) + { + perror("bind(raw) failed"); + ::close(fd); + return -1; + } + + if (::setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, ifname, std::strlen(ifname)) != 0) + { + perror("setsockopt(SO_BINDTODEVICE) failed"); + // keep running; bind already done + } + + return fd; +} + +int linux_raw_recv(int fd, void *buf, int buf_len, struct timespec *hwts, int flag) +{ + if (fd < 0 || !buf || buf_len <= 0 || !hwts) + return -1; + + int recvFlag = 0; + if (flag == 1) + recvFlag = MSG_ERRQUEUE; + + char control[1024]; + struct iovec iov{}; + struct msghdr recvMsg{}; + std::memset(control, 0, sizeof(control)); + + struct pollfd fds[1]{}; + fds[0].fd = fd; + int poll_res = 1; + if (flag == 0) + { + fds[0].events = POLLIN; + poll_res = ::poll(fds, 1, -1); + if (poll_res <= 0) + return -1; + } + + iov.iov_base = buf; + iov.iov_len = static_cast(buf_len); + recvMsg.msg_iov = &iov; + recvMsg.msg_iovlen = 1; + recvMsg.msg_control = control; + recvMsg.msg_controllen = sizeof(control); + + const int len = static_cast(::recvmsg(fd, &recvMsg, recvFlag)); + if (len < 0) + return -1; + + for (struct cmsghdr *cm = CMSG_FIRSTHDR(&recvMsg); cm != nullptr; cm = CMSG_NXTHDR(&recvMsg, cm)) + { + if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMPING) + { + auto *ts = reinterpret_cast(CMSG_DATA(cm)); + if (ts[2].tv_sec != 0 || ts[2].tv_nsec != 0) + *hwts = ts[2]; + } + } + + return len; +} + +int linux_raw_send(int fd, void *buf, int len, struct timespec *hwts) +{ + if (fd < 0 || !buf || len <= 0 || !hwts) + return -1; + + clear_msg_errqueue(fd); + + const int sent = static_cast(::send(fd, buf, len, 0)); + if (sent < 0) + return -1; + + struct pollfd fds[1]{}; + fds[0].fd = fd; + fds[0].events = POLLERR; + const int poll_res = ::poll(fds, 1, -1); + if (poll_res < 0) + return -1; + + if (fds[0].revents & POLLERR) + { + return linux_raw_recv(fd, buf, len, hwts, 1); + } + return sent; +} + +} // extern "C" diff --git a/src/tsyncd/platform/linux/linux_raw_shim.hpp b/src/tsyncd/platform/linux/linux_raw_shim.hpp new file mode 100644 index 0000000..de7a64d --- /dev/null +++ b/src/tsyncd/platform/linux/linux_raw_shim.hpp @@ -0,0 +1,26 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include + +#ifdef __cplusplus +extern "C" { +#endif + +int linux_raw_open(const char *ifname); +int linux_raw_recv(int fd, void *buf, int buf_len, struct timespec *hwts, int flag); +int linux_raw_send(int fd, void *buf, int len, struct timespec *hwts); + +#ifdef __cplusplus +} +#endif diff --git a/src/tsyncd/platform/qnx/qnx_raw_shim.cpp b/src/tsyncd/platform/qnx/qnx_raw_shim.cpp new file mode 100644 index 0000000..f0fd74a --- /dev/null +++ b/src/tsyncd/platform/qnx/qnx_raw_shim.cpp @@ -0,0 +1,473 @@ +/******************************************************************************** +* 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 "qnx_raw_shim.hpp" +#include "tsync_types.hpp" +#include "eth_protocol.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +// BPF buffer size: use static buffer to avoid malloc/free in thread_local +// Typical BPF buffer size is 32KB-64KB; we use 64KB as safe maximum +static constexpr std::size_t kMaxBpfBufSize = 65536; + +struct QnxRawContext +{ + int bpf_fd = -1; + u_int bpf_buflen = 0; + char iface_name[IFNAMSIZ] = ""; + unsigned char bpf_buf[kMaxBpfBufSize]; // Static buffer, no malloc/free needed + ssize_t bpf_n = 0; + ssize_t bpf_off = 0; + bool bpf_buf_initialized = false; + unsigned char tx_frame[ETHER_HDR_LEN + 1500]{}; // TX frame buffer to avoid stack allocation + + // Destructor ensures proper cleanup of file descriptor when thread exits + // Critical for thread_local usage - prevents fd leaks when threads terminate + ~QnxRawContext() + { + if (bpf_fd >= 0) + { + ::close(bpf_fd); + bpf_fd = -1; + } + } +}; + +thread_local QnxRawContext g_qnx_ctx; + +static int get_hwts_tx_rx(const char *ifname, int dir, const PTPHeader *ptp_hdr, timespec *ts) +{ + if (!ifname || !ptp_hdr || !ts) + { + errno = EINVAL; + return -1; + } + + int s = socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) + { + return -1; + } + + struct + { + struct ifdrv ifd; + ptp_extts_t extts; + } cmd_time_stamp; + + std::memset(&cmd_time_stamp, 0, sizeof(cmd_time_stamp)); + + std::strncpy(cmd_time_stamp.ifd.ifd_name, ifname, sizeof(cmd_time_stamp.ifd.ifd_name) - 1); + cmd_time_stamp.ifd.ifd_name[sizeof(cmd_time_stamp.ifd.ifd_name) - 1] = '\0'; + + cmd_time_stamp.ifd.ifd_cmd = (dir ? PTP_GET_RX_TIMESTAMP : PTP_GET_TX_TIMESTAMP); + cmd_time_stamp.ifd.ifd_len = sizeof(ptp_extts_t); + cmd_time_stamp.ifd.ifd_data = &cmd_time_stamp.extts; + + cmd_time_stamp.extts.msg_type = ptp_hdr->tsmt & 0x0f; + cmd_time_stamp.extts.sport_id = ntohs(ptp_hdr->sourcePortIdentity.portNumber); + cmd_time_stamp.extts.sequence_id = ntohs(ptp_hdr->sequenceId); + + std::memcpy(cmd_time_stamp.extts.clock_identity, + ptp_hdr->sourcePortIdentity.clockIdentity.id, + sizeof(cmd_time_stamp.extts.clock_identity)); + + cmd_time_stamp.extts.ts.sec = 0; + cmd_time_stamp.extts.ts.nsec = 0; + + if (devctl(s, SIOCGDRVSPEC, &cmd_time_stamp, sizeof(cmd_time_stamp), nullptr) == -1) + { + close(s); + return -1; + } + + close(s); + + if (cmd_time_stamp.extts.ts.sec == 0 && cmd_time_stamp.extts.ts.nsec == 0) + { + errno = EAGAIN; + return -1; + } + + ts->tv_sec = static_cast(cmd_time_stamp.extts.ts.sec); + ts->tv_nsec = static_cast(cmd_time_stamp.extts.ts.nsec); + return 0; +} + +extern "C" int qnx_raw_open(const char *ifname) +{ + std::cout << "[DEBUG] qnx_raw_open: ifname=" << (ifname ? ifname : "NULL") << std::endl; + + if (!ifname) + { + errno = EINVAL; + return -1; + } + + strlcpy(g_qnx_ctx.iface_name, ifname, sizeof(g_qnx_ctx.iface_name)); + + char devpath[256] = {0}; + const char *sock = std::getenv("SOCK"); + + if (sock && sock[0]) + { + std::snprintf(devpath, sizeof(devpath), "%s/dev/bpf0", sock); + } + else + { + std::snprintf(devpath, sizeof(devpath), "/dev/bpf"); + } + + int fd = open(devpath, O_RDWR); + if (fd < 0) + { + std::cout << "[DEBUG] qnx_raw_open: open " << devpath << " FAILED: " + << std::strerror(errno) << std::endl; + return -1; + } + std::cout << "[DEBUG] qnx_raw_open: open " << devpath << " OK, fd=" << fd << std::endl; + + ifreq ifr; + std::memset(&ifr, 0, sizeof(ifr)); + strlcpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name)); + + if (ioctl(fd, BIOCSETIF, &ifr) < 0) + { + std::cout << "[DEBUG] qnx_raw_open: BIOCSETIF(" << ifr.ifr_name << ") FAILED: " + << std::strerror(errno) << std::endl; + close(fd); + return -1; + } + std::cout << "[DEBUG] qnx_raw_open: BIOCSETIF(" << ifr.ifr_name << ") OK" << std::endl; + + int zero = 0; + if (ioctl(fd, BIOCSSEESENT, &zero, sizeof(zero)) < 0) + { + std::cout << "[DEBUG] qnx_raw_open: BIOCSSEESENT FAILED (ignored): " + << std::strerror(errno) << std::endl; + } + + u_int yes = 1; + if (ioctl(fd, BIOCIMMEDIATE, &yes) < 0) + { + std::cout << "[DEBUG] qnx_raw_open: BIOCIMMEDIATE FAILED (ignored): " + << std::strerror(errno) << std::endl; + } + + if (ioctl(fd, BIOCPROMISC, &yes) < 0) + { + std::cout << "[DEBUG] qnx_raw_open: BIOCPROMISC FAILED (ignored): " + << std::strerror(errno) << std::endl; + } + + if (ioctl(fd, BIOCGBLEN, &g_qnx_ctx.bpf_buflen) < 0) + { + std::cout << "[DEBUG] qnx_raw_open: BIOCGBLEN FAILED: " + << std::strerror(errno) << std::endl; + close(fd); + return -1; + } + + // Validate buffer size doesn't exceed our static buffer + if (g_qnx_ctx.bpf_buflen > kMaxBpfBufSize) + { + std::cout << "[DEBUG] qnx_raw_open: BPF buffer size " << g_qnx_ctx.bpf_buflen + << " exceeds maximum " << kMaxBpfBufSize << std::endl; + close(fd); + errno = ENOMEM; + return -1; + } + + g_qnx_ctx.bpf_buf_initialized = true; + g_qnx_ctx.bpf_fd = fd; + + std::cout << "[DEBUG] qnx_raw_open: SUCCESS, fd=" << fd << ", ifname=" << g_qnx_ctx.iface_name << std::endl; + return fd; +} + +extern "C" int qnx_raw_recv(int fd, void *buf, int buf_len, timespec *hwts, int nonblock) +{ + if (fd < 0 || !buf || buf_len <= 0 || !hwts) + { + errno = EINVAL; + return -1; + } + if (g_qnx_ctx.bpf_buflen == 0 || !g_qnx_ctx.bpf_buf_initialized) + { + errno = EINVAL; + return -1; + } + + if (nonblock) + { + int flags = fcntl(fd, F_GETFL, 0); + if (flags >= 0) + { + fcntl(fd, F_SETFL, flags | O_NONBLOCK); + } + } + + // bpf_buf is now a static array, no malloc needed + + for (;;) + { + if (g_qnx_ctx.bpf_off >= g_qnx_ctx.bpf_n) + { + ssize_t n = read(fd, g_qnx_ctx.bpf_buf, g_qnx_ctx.bpf_buflen); + if (n < 0) + { + return -1; + } + if (n == 0) + { + if (nonblock) + { + errno = EAGAIN; + return -1; + } + continue; + } + g_qnx_ctx.bpf_n = n; + g_qnx_ctx.bpf_off = 0; + } + + if (g_qnx_ctx.bpf_off + static_cast(sizeof(bpf_hdr)) > g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + // Validate alignment before reinterpret_cast (safety-critical requirement) + const auto ptr_value = reinterpret_cast(g_qnx_ctx.bpf_buf + g_qnx_ctx.bpf_off); + if (ptr_value % alignof(bpf_hdr) != 0) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; // Skip to end + continue; + } + + auto *bh = reinterpret_cast(g_qnx_ctx.bpf_buf + g_qnx_ctx.bpf_off); + + // Check for integer overflow and bounds + if (bh->bh_hdrlen > static_cast(g_qnx_ctx.bpf_n) || + bh->bh_caplen > static_cast(g_qnx_ctx.bpf_n) || + bh->bh_hdrlen + bh->bh_caplen < bh->bh_hdrlen || // Overflow check + g_qnx_ctx.bpf_off + static_cast(bh->bh_hdrlen) + static_cast(bh->bh_caplen) > g_qnx_ctx.bpf_n) + { + g_qnx_ctx.bpf_off = g_qnx_ctx.bpf_n; + continue; + } + + unsigned char *pkt = reinterpret_cast(bh) + bh->bh_hdrlen; + int caplen = static_cast(bh->bh_caplen); + + ssize_t next_off = g_qnx_ctx.bpf_off + BPF_WORDALIGN(bh->bh_hdrlen + bh->bh_caplen); + + if (caplen >= static_cast(sizeof(::ethhdr))) + { + + ::ethhdr eth{}; + int ptp_offset = 0; + tsyncd::parse_ethernet_header(pkt, eth, ptp_offset); + + uint16_t ethertype = ntohs(eth.h_proto); + + if (ethertype == ETH_P_8021Q) + { + if (caplen < ptp_offset) + { + g_qnx_ctx.bpf_off = next_off; + continue; + } + // Use memcpy to avoid unaligned access + uint16_t ethertype_vlan; + std::memcpy(ðertype_vlan, pkt + static_cast(sizeof(::ethhdr)) + 2, sizeof(uint16_t)); + ethertype = ntohs(ethertype_vlan); + } + + if (ethertype == ETH_P_1588 && + caplen >= ptp_offset + static_cast(sizeof(PTPHeader))) + { + + int frame_len = caplen; + if (frame_len > buf_len) + { + frame_len = buf_len; + } + std::memcpy(buf, pkt, static_cast(frame_len)); + + const auto *ph = reinterpret_cast(pkt + ptp_offset); + + timespec ts{}; + if (get_hwts_tx_rx(g_qnx_ctx.iface_name, 1, ph, &ts) < 0) + { + clock_gettime(CLOCK_REALTIME, &ts); + } + *hwts = ts; + + g_qnx_ctx.bpf_off = next_off; + return frame_len; + } + } + + g_qnx_ctx.bpf_off = next_off; + } +} + +extern "C" int qnx_raw_send(int fd, const void *buf, int len, timespec *hwts) +{ + if (fd < 0 || !buf || len <= 0 || !hwts) + { + errno = EINVAL; + return -1; + } + + unsigned int frame_len = static_cast(len); + + if (frame_len > 1500) + { + errno = EMSGSIZE; + return -1; + } + + std::memcpy(g_qnx_ctx.tx_frame, buf, frame_len); + + ssize_t n = write(fd, g_qnx_ctx.tx_frame, frame_len); + if (n < 0) + { + return -1; + } + + const auto *ph = reinterpret_cast(g_qnx_ctx.tx_frame + ETHER_HDR_LEN); + timespec ts{}; + if (get_hwts_tx_rx(g_qnx_ctx.iface_name, 0, ph, &ts) < 0) + { + clock_gettime(CLOCK_REALTIME, &ts); + } + *hwts = ts; + + return len; +} + +extern "C" int qnx_phc_open(const char *phc_dev) +{ + if (phc_dev && 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) +{ + (void)phc_fd; + + if (offset_ns == 0) + { + std::cout << "[DEBUG] qnx_phc_adjtime_step: offset_ns == 0, no-op" << std::endl; + return 0; + } + + int s = socket(AF_INET, SOCK_DGRAM, 0); + if (s < 0) + { + std::cout << "[DEBUG] qnx_phc_adjtime_step: socket() FAILED: " + << std::strerror(errno) << std::endl; + return -1; + } + + struct + { + ifdrv ifd; + ptp_time_t tm; + } cmd; + + std::memset(&cmd, 0, sizeof(cmd)); + + std::strncpy(cmd.ifd.ifd_name, g_qnx_ctx.iface_name, sizeof(cmd.ifd.ifd_name) - 1); + cmd.ifd.ifd_name[sizeof(cmd.ifd.ifd_name) - 1] = '\0'; + cmd.ifd.ifd_len = sizeof(cmd.tm); + cmd.ifd.ifd_data = &cmd.tm; + + cmd.ifd.ifd_cmd = PTP_GET_TIME; + if (devctl(s, SIOCGDRVSPEC, &cmd, sizeof(cmd), nullptr) == -1) + { + std::cout << "[DEBUG] qnx_phc_adjtime_step: GET_TIME FAILED on " << cmd.ifd.ifd_name + << ": " << std::strerror(errno) << std::endl; + close(s); + return -1; + } + + int64_t cur_ns = static_cast(cmd.tm.sec) * NS_PER_SEC + + static_cast(cmd.tm.nsec); + + int64_t new_ns = cur_ns + offset_ns; + + if (new_ns < static_cast(INT32_MIN) * NS_PER_SEC) + { + new_ns = static_cast(INT32_MIN) * NS_PER_SEC; + } + if (new_ns > static_cast(INT32_MAX) * NS_PER_SEC) + { + new_ns = static_cast(INT32_MAX) * NS_PER_SEC; + } + + int32_t new_sec = static_cast(new_ns / NS_PER_SEC); + int32_t new_nsec = static_cast(new_ns % NS_PER_SEC); + if (new_nsec < 0) + { + new_nsec += NS_PER_SEC; + new_sec -= 1; + } + + cmd.tm.sec = new_sec; + cmd.tm.nsec = new_nsec; + + cmd.ifd.ifd_cmd = PTP_SET_TIME; + if (devctl(s, SIOCGDRVSPEC, &cmd, sizeof(cmd), nullptr) == -1) + { + std::cout << "[DEBUG] qnx_phc_adjtime_step: SET_TIME FAILED on " << cmd.ifd.ifd_name + << ": " << std::strerror(errno) << std::endl; + close(s); + return -1; + } + + std::cout << "[DEBUG] qnx_phc_adjtime_step: SUCCESS, sec=" << new_sec + << ", nsec=" << new_nsec << std::endl; + + close(s); + return 0; +} + +extern "C" int qnx_phc_adjfreq_ppb(int phc_fd, long long freq_ppb) +{ + (void)phc_fd; + (void)freq_ppb; + return 0; +} diff --git a/src/tsyncd/platform/qnx/qnx_raw_shim.hpp b/src/tsyncd/platform/qnx/qnx_raw_shim.hpp new file mode 100644 index 0000000..b235d69 --- /dev/null +++ b/src/tsyncd/platform/qnx/qnx_raw_shim.hpp @@ -0,0 +1,30 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include + +#ifdef __cplusplus +extern "C" { +#endif + +int qnx_raw_open(const char *ifname); +int qnx_raw_recv(int fd, void *buf, int buf_len, struct timespec *hwts, int nonblock); +int qnx_raw_send(int fd, const void *buf, int len, struct timespec *hwts); + +int qnx_phc_open(const char *phc_dev); +int qnx_phc_adjtime_step(int phc_fd, long long offset_ns); +int qnx_phc_adjfreq_ppb(int phc_fd, long long freq_ppb); + +#ifdef __cplusplus +} +#endif diff --git a/src/tsyncd/protocol/eth_protocol.cpp b/src/tsyncd/protocol/eth_protocol.cpp new file mode 100644 index 0000000..75ef206 --- /dev/null +++ b/src/tsyncd/protocol/eth_protocol.cpp @@ -0,0 +1,65 @@ +/******************************************************************************** +* 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 "eth_protocol.hpp" +#include "tsync_types.hpp" + +#include +#include +#include + +namespace tsyncd +{ + namespace + { + static int str2mac(const char *s, unsigned char mac[MAC_ADDR_LEN]) + { + unsigned int b[MAC_ADDR_LEN]{}; + const int c = std::sscanf(s, "%x:%x:%x:%x:%x:%x", &b[0], &b[1], &b[2], &b[3], &b[4], &b[5]); + if (c != MAC_ADDR_LEN) + return -1; + for (int i = 0; i < MAC_ADDR_LEN; i++) + mac[i] = static_cast(b[i]); + return 0; + } + } + + int add_ethernet_header(unsigned char *buffer, unsigned int &buffer_len) + { + // buffer is assumed large enough (caller uses PTPMessage buffer size). Keep simple. + unsigned char tmp[2048]; + if (buffer_len + sizeof(struct ethhdr) > sizeof(tmp)) + return -1; + + auto *hdr = reinterpret_cast(tmp); + if (str2mac(PTP_SRC_MAC, hdr->h_source) != 0 || str2mac(PTP_DST_MAC, hdr->h_dest) != 0) + { + return -1; + } + hdr->h_proto = htons(ETH_P_1588); + + std::memcpy(tmp + sizeof(struct ethhdr), buffer, buffer_len); + buffer_len += sizeof(struct ethhdr); + std::memcpy(buffer, tmp, buffer_len); + return 0; + } + + void parse_ethernet_header(const unsigned char *buffer, ethhdr ð_header, int &offset) + { + std::memcpy(ð_header, buffer, sizeof(ethhdr)); + const unsigned short ether_type = ntohs(eth_header.h_proto); + if (ether_type == ETH_P_8021Q) + offset = static_cast(sizeof(struct ethhdr)) + VLAN_TAG_LEN; + else + offset = static_cast(sizeof(struct ethhdr)); + } +} diff --git a/src/tsyncd/protocol/eth_protocol.hpp b/src/tsyncd/protocol/eth_protocol.hpp new file mode 100644 index 0000000..bc1c40d --- /dev/null +++ b/src/tsyncd/protocol/eth_protocol.hpp @@ -0,0 +1,32 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include +#include +#include "tsync_types.hpp" + +#ifdef _QNX710_ +struct ethhdr { + unsigned char h_dest[MAC_ADDR_LEN]; + unsigned char h_source[MAC_ADDR_LEN]; + uint16_t h_proto; +}; +#else +#include +#endif + +namespace tsyncd +{ + int add_ethernet_header(unsigned char *buffer, unsigned int &buffer_len); + void parse_ethernet_header(const unsigned char *buffer, ethhdr ð_header, int &offset); +} diff --git a/src/tsyncd/protocol/gptp_protocol.cpp b/src/tsyncd/protocol/gptp_protocol.cpp new file mode 100644 index 0000000..58e320f --- /dev/null +++ b/src/tsyncd/protocol/gptp_protocol.cpp @@ -0,0 +1,93 @@ +/******************************************************************************** +* 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 "gptp_protocol.hpp" +#include +#include + +#ifndef _QNX710_ +#include +#endif + +namespace tsyncd +{ + namespace + { + static inline std::uint16_t load_u16(const unsigned char *p) + { + std::uint16_t v; + std::memcpy(&v, p, sizeof(v)); + return ntohs(v); + } + static inline std::uint32_t load_u32(const unsigned char *p) + { + std::uint32_t v; + std::memcpy(&v, p, sizeof(v)); + return ntohl(v); + } + static inline std::uint64_t load_be64(const unsigned char *p) + { + std::uint64_t v; + std::memcpy(&v, p, sizeof(v)); + #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + v = __builtin_bswap64(v); + #endif + return v; + } + } + + bool parse_gptp_message(const unsigned char *buffer, std::size_t buffer_len, PTPMessage &ptp_message) + { + if (buffer_len < sizeof(PTPHeader)) + return false; + + ptp_message.ptpHdr.tsmt = buffer[0]; + ptp_message.ptpHdr.version = buffer[1]; + ptp_message.ptpHdr.messageLength = load_u16(buffer + 2); + ptp_message.ptpHdr.domainNumber = buffer[4]; + ptp_message.ptpHdr.reserved1 = buffer[5]; + std::memcpy(ptp_message.ptpHdr.flagField, buffer + 6, 2); + ptp_message.ptpHdr.correctionField = static_cast(load_be64(buffer + 8)); + ptp_message.ptpHdr.reserved2 = load_u32(buffer + 16); + + std::memcpy(ptp_message.ptpHdr.sourcePortIdentity.clockIdentity.id, buffer + 20, 8); + ptp_message.ptpHdr.sourcePortIdentity.portNumber = load_u16(buffer + 28); + + ptp_message.ptpHdr.sequenceId = load_u16(buffer + 30); + ptp_message.ptpHdr.controlField = buffer[32]; + ptp_message.ptpHdr.logMessageInterval = static_cast(buffer[33]); + + ptp_message.msgtype = ptp_message.ptpHdr.tsmt & 0x0F; + + if (ptp_message.msgtype == PTP_MSGTYPE_FOLLOW_UP) + { + ptp_message.follow_up.preciseOriginTimestamp.seconds_msb = load_u16(buffer + 34); + ptp_message.follow_up.preciseOriginTimestamp.seconds_lsb = load_u32(buffer + 36); + ptp_message.follow_up.preciseOriginTimestamp.nanoseconds = load_u32(buffer + 40); + } + else if (ptp_message.msgtype == PTP_MSGTYPE_PDELAY_RESP) + { + ptp_message.pdelay_resp.responseOriginTimestamp.seconds_msb = load_u16(buffer + 34); + ptp_message.pdelay_resp.responseOriginTimestamp.seconds_lsb = load_u32(buffer + 36); + ptp_message.pdelay_resp.responseOriginTimestamp.nanoseconds = load_u32(buffer + 40); + } + else if (ptp_message.msgtype == PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP) + { + ptp_message.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_msb = load_u16(buffer + 34); + ptp_message.pdelay_resp_fup.responseOriginReceiptTimestamp.seconds_lsb = load_u32(buffer + 36); + ptp_message.pdelay_resp_fup.responseOriginReceiptTimestamp.nanoseconds = load_u32(buffer + 40); + } + + return true; + } + +} diff --git a/src/tsyncd/protocol/gptp_protocol.hpp b/src/tsyncd/protocol/gptp_protocol.hpp new file mode 100644 index 0000000..2de2af9 --- /dev/null +++ b/src/tsyncd/protocol/gptp_protocol.hpp @@ -0,0 +1,21 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include "tsync_types.hpp" +#include +#include + +namespace tsyncd +{ + bool parse_gptp_message(const unsigned char *buffer, std::size_t buffer_len, PTPMessage &msg); +} diff --git a/src/tsyncd/protocol/net_identity.cpp b/src/tsyncd/protocol/net_identity.cpp new file mode 100644 index 0000000..ea27783 --- /dev/null +++ b/src/tsyncd/protocol/net_identity.cpp @@ -0,0 +1,123 @@ +/******************************************************************************** +* Copyright (c) 2025 Contributors to the Eclipse Foundation +* +* See the NOTICE file(s) distributed with this work for additional +* information regarding copyright ownership. +* +* This program and the accompanying materials are made available under the +* terms of the Apache License Version 2.0 which is available at +* https://www.apache.org/licenses/LICENSE-2.0 +* +* SPDX-License-Identifier: Apache-2.0 +********************************************************************************/ +#include "net_identity.hpp" + +#include +#include + +#include +#include +#include +#include + +#ifdef _QNX710_ +#include +#include +#include +#include +#else +#include +#include +#endif + +namespace tsyncd +{ + namespace + { + #ifdef _QNX710_ + int iface_mac(const char *name, unsigned char *out_mac, int &out_len) + { + if (!name || !out_mac) + return -1; + + ::ifaddrs *ifaddr = nullptr; + if (::getifaddrs(&ifaddr) != 0 || !ifaddr) + return -1; + + int result = -1; + for (::ifaddrs *ifa = ifaddr; ifa; ifa = ifa->ifa_next) + { + if (!ifa->ifa_name || !ifa->ifa_addr) + continue; + if (std::strcmp(ifa->ifa_name, name) != 0) + continue; + if (ifa->ifa_addr->sa_family != AF_LINK) + continue; + + auto *sdl = reinterpret_cast(ifa->ifa_addr); + const unsigned char *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)); + out_len = len; + result = 0; + break; + } + } + + ::freeifaddrs(ifaddr); + return result; + } + #else + int iface_mac(const char *name, unsigned char *out_mac, int &out_len) + { + if (!name || !out_mac) + return -1; + + ::ifreq ifr{}; + std::snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", name); + + 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); + out_len = 6; + return 0; + } + #endif + } + + int generate_clock_identity(ClockIdentity &ci, const char *iface_name) + { + unsigned char mac[8]{}; + int len = 0; + if (iface_mac(iface_name, mac, len) != 0) + return -1; + + if (len == 6) + { + ci.id[0] = mac[0]; + ci.id[1] = mac[1]; + ci.id[2] = mac[2]; + ci.id[3] = 0xFF; + ci.id[4] = 0xFE; + ci.id[5] = mac[3]; + ci.id[6] = mac[4]; + ci.id[7] = mac[5]; + return 0; + } + if (len == 8) + { + std::memcpy(ci.id, mac, 8); + return 0; + } + return -1; + } +} diff --git a/src/tsyncd/protocol/net_identity.hpp b/src/tsyncd/protocol/net_identity.hpp new file mode 100644 index 0000000..53cbb74 --- /dev/null +++ b/src/tsyncd/protocol/net_identity.hpp @@ -0,0 +1,21 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once + +#include "tsync_types.hpp" + +namespace tsyncd +{ + // Build IEEE 1588 ClockIdentity from interface address. + int generate_clock_identity(ClockIdentity &ci, const char *iface_name); +} diff --git a/src/tsyncd/protocol/ntp_client.cpp b/src/tsyncd/protocol/ntp_client.cpp new file mode 100644 index 0000000..0f6de67 --- /dev/null +++ b/src/tsyncd/protocol/ntp_client.cpp @@ -0,0 +1,219 @@ +/******************************************************************************** +* 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 "ntp_client.hpp" +#include "score_time/utils/time_utils.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace tsyncd::ntp +{ + namespace + { + static constexpr std::uint64_t kUnixToNtpSeconds = 2208988800ULL; // 1970->1900 + +#pragma pack(push, 1) + struct NtpPacket + { + std::uint8_t li_vn_mode; + std::uint8_t stratum; + std::uint8_t poll; + std::int8_t precision; + std::uint32_t root_delay; + std::uint32_t root_dispersion; + std::uint32_t ref_id; + std::uint64_t ref_ts; + std::uint64_t orig_ts; + std::uint64_t recv_ts; + std::uint64_t tx_ts; + }; +#pragma pack(pop) + + inline std::uint64_t HostToNtp(std::int64_t unix_ns) + { + const std::int64_t unix_s = unix_ns / 1'000'000'000LL; + const std::int64_t unix_rem_ns = unix_ns - unix_s * 1'000'000'000LL; + const std::uint64_t ntp_s = static_cast(unix_s) + kUnixToNtpSeconds; + const std::uint64_t frac = (static_cast(unix_rem_ns) << 32) / 1'000'000'000ULL; + return (ntp_s << 32) | (frac & 0xFFFFFFFFULL); + } + + inline std::int64_t NtpToUnixNs(std::uint64_t ntp_ts_be) + { + const std::uint64_t ntp_ts = be64toh(ntp_ts_be); + const std::uint64_t ntp_s = ntp_ts >> 32; + const std::uint64_t frac = ntp_ts & 0xFFFFFFFFULL; + if (ntp_s < kUnixToNtpSeconds) + return 0; + const std::uint64_t unix_s = ntp_s - kUnixToNtpSeconds; + const std::uint64_t unix_ns = unix_s * 1'000'000'000ULL + (frac * 1'000'000'000ULL >> 32); + return static_cast(unix_ns); + } + + inline bool Resolve(const std::string &host, int port, ::sockaddr_storage &out, socklen_t &outlen) + { + ::addrinfo hints{}; + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_protocol = IPPROTO_UDP; + + ::addrinfo *res = nullptr; + const std::string sport = std::to_string(port); + if (::getaddrinfo(host.c_str(), sport.c_str(), &hints, &res) != 0 || !res) + return false; + + for (::addrinfo *p = res; p; p = p->ai_next) + { + if (!p->ai_addr || p->ai_addrlen <= 0) + continue; + if (static_cast(p->ai_addrlen) > sizeof(out)) + continue; + std::memcpy(&out, p->ai_addr, static_cast(p->ai_addrlen)); + outlen = static_cast(p->ai_addrlen); + ::freeaddrinfo(res); + return true; + } + ::freeaddrinfo(res); + return false; + } + + } // namespace + + Client::Client(Options opt) : opt_(std::move(opt)) {} + + std::optional Client::QueryOnce() const + { + for (const auto &host : opt_.servers) + { + ::sockaddr_storage addr{}; + socklen_t addrlen = 0; + if (!Resolve(host, opt_.port, addr, addrlen)) + continue; + + const int fd = ::socket(addr.ss_family, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) + continue; + + ::timeval tv{}; + tv.tv_sec = opt_.timeout_ms / 1000; + tv.tv_usec = (opt_.timeout_ms % 1000) * 1000; + (void)::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + + NtpPacket req{}; + req.li_vn_mode = static_cast((0 << 6) | (4 << 3) | 3); // LI=0, VN=4, MODE=3(client) + + const std::int64_t t1 = score_time::utils::ClockNs(CLOCK_REALTIME); + req.tx_ts = htobe64(HostToNtp(t1)); + + const ssize_t sent = ::sendto(fd, &req, sizeof(req), 0, reinterpret_cast(&addr), addrlen); + if (sent != static_cast(sizeof(req))) + { + ::close(fd); + continue; + } + + NtpPacket resp{}; + ::sockaddr_storage peer{}; + socklen_t peerlen = sizeof(peer); + const ssize_t recvd = ::recvfrom(fd, &resp, sizeof(resp), 0, reinterpret_cast<::sockaddr *>(&peer), &peerlen); + const std::int64_t t4 = score_time::utils::ClockNs(CLOCK_REALTIME); + const std::int64_t mono_rx = score_time::utils::ClockNs(CLOCK_MONOTONIC); + ::close(fd); + if (recvd < static_cast(sizeof(resp))) + continue; + + const std::int64_t t2 = NtpToUnixNs(resp.recv_ts); + const std::int64_t t3 = NtpToUnixNs(resp.tx_ts); + if (t2 <= 0 || t3 <= 0) + continue; + + // NTP formulas. Offset is server - local. + const std::int64_t offset = ((t2 - t1) + (t3 - t4)) / 2; + const std::int64_t delay = (t4 - t1) - (t3 - t2); + if (delay < 0) + continue; + + Sample s{}; + s.offset_ns = offset; + s.delay_ns = delay; + s.mono_rx_ns = mono_rx; + s.local_tx_ns = t1; + s.local_rx_ns = t4; + s.server_rx_ns = t2; + s.server_tx_ns = t3; + return s; + } + return std::nullopt; + } + + Estimator::Estimator(Options opt) : opt_(std::move(opt)) {} + + void Estimator::Update(const Sample &s) + { + if (s.delay_ns <= 0 || s.delay_ns > opt_.max_reasonable_delay_ns) + { + est_.bad_samples++; + return; + } + + // Jitter proxy: half of delay + absolute change in offset + const std::int64_t new_jitter = (s.delay_ns / 2) + std::llabs(s.offset_ns - est_.offset_ns); + + if (est_.good_samples == 0) + { + est_.offset_ns = s.offset_ns; + est_.jitter_ns = new_jitter; + } + else + { + est_.offset_ns = static_cast( + (1.0 - opt_.offset_ewma_alpha) * static_cast(est_.offset_ns) + + opt_.offset_ewma_alpha * static_cast(s.offset_ns)); + est_.jitter_ns = static_cast( + (1.0 - opt_.jitter_ewma_alpha) * static_cast(est_.jitter_ns) + + opt_.jitter_ewma_alpha * static_cast(new_jitter)); + } + + // Inaccuracy: at least half of delay, plus some jitter margin. + est_.inaccuracy_ns = std::max(s.delay_ns / 2, est_.jitter_ns); + + est_.good_samples++; + est_.last_update_mono_ns = s.mono_rx_ns; + if (est_.good_samples >= static_cast(opt_.samples_to_lock)) + { + est_.locked = true; + } + } + + void Estimator::MarkTimeout(std::int64_t mono_now_ns) + { + est_.bad_samples++; + // If too long without updates, drop lock. + if (est_.last_update_mono_ns != 0 && (mono_now_ns - est_.last_update_mono_ns) > 5'000'000'000LL) + { // 5s + est_.locked = false; + est_.good_samples = 0; + } + } + + Estimate Estimator::Snapshot() const { return est_; } + +} // namespace tsyncd::ntp diff --git a/src/tsyncd/protocol/ntp_client.hpp b/src/tsyncd/protocol/ntp_client.hpp new file mode 100644 index 0000000..0b647ae --- /dev/null +++ b/src/tsyncd/protocol/ntp_client.hpp @@ -0,0 +1,84 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include +#include +#include +#include + +namespace tsyncd::ntp +{ + + struct Sample + { + std::int64_t offset_ns{0}; // server_time - local_time + std::int64_t delay_ns{0}; // round-trip delay estimate + std::int64_t mono_rx_ns{0}; // CLOCK_MONOTONIC at receive + std::int64_t local_tx_ns{0}; // CLOCK_REALTIME at send + std::int64_t local_rx_ns{0}; // CLOCK_REALTIME at receive + std::int64_t server_rx_ns{0}; // server receive time (UTC) + std::int64_t server_tx_ns{0}; // server transmit time (UTC) + }; + + class Client + { + public: + struct Options + { + std::vector servers = {"pool.ntp.org"}; + int port = 123; + int timeout_ms = 250; + }; + + explicit Client(Options opt); + + std::optional QueryOnce() const; + + private: + Options opt_; + }; + + struct Estimate + { + bool locked{false}; + std::int64_t offset_ns{0}; // filtered offset + std::int64_t inaccuracy_ns{0}; // best-effort bound + std::int64_t jitter_ns{0}; // filtered jitter proxy + std::int64_t last_update_mono_ns{0}; + std::uint32_t good_samples{0}; + std::uint32_t bad_samples{0}; + }; + + class Estimator + { + public: + struct Options + { + int samples_to_lock = 3; + double offset_ewma_alpha = 0.2; + double jitter_ewma_alpha = 0.2; + std::int64_t max_reasonable_delay_ns = 500'000'000; // 500ms + }; + + explicit Estimator(Options opt); + + void Update(const Sample &s); + void MarkTimeout(std::int64_t mono_now_ns); + Estimate Snapshot() const; + + private: + Options opt_; + Estimate est_; + }; + +} // namespace tsyncd::ntp diff --git a/src/tsyncd/protocol/raw_socket.cpp b/src/tsyncd/protocol/raw_socket.cpp new file mode 100644 index 0000000..0328927 --- /dev/null +++ b/src/tsyncd/protocol/raw_socket.cpp @@ -0,0 +1,92 @@ +/******************************************************************************** +* 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 "raw_socket.hpp" +#include "tsync_types.hpp" + +#include +#include + +#ifdef _QNX710_ +#include "qnx_raw_shim.hpp" +#else +#include "linux_raw_shim.hpp" +#endif + +namespace tsyncd +{ + +#ifdef _QNX710_ + +int setup_raw_socket(int &sockFd, const char *interface_name) +{ + int fd = qnx_raw_open(interface_name); + if (fd < 0) { + sockFd = -1; + return -1; + } + sockFd = fd; + return 0; +} + +int raw_recvMsg(int sockFd, void *recv_buffer, ::timespec *hwts, int flag) +{ + if (sockFd < 0 || !recv_buffer || !hwts) { + return -1; + } + const int nonblock = (flag != 0) ? 1 : 0; + const int max_len = 1500; + return qnx_raw_recv(sockFd, recv_buffer, max_len, hwts, nonblock); +} + +int raw_sendMsg(int sockFd, void *buffer, int bufLen, ::timespec *hwts) +{ + if (sockFd < 0 || !buffer || bufLen <= 0 || !hwts) { + return -1; + } + return qnx_raw_send(sockFd, buffer, bufLen, hwts); +} + +#else + +int setup_raw_socket(int &sockFd, const char *interface_name) +{ + int fd = linux_raw_open(interface_name); + if (fd < 0) + return -1; + + sockFd = fd; + return 0; +} + +int raw_recvMsg(int sockFd, void *recv_buffer, ::timespec *hwts, int flag) +{ + if (sockFd < 0 || !recv_buffer || !hwts) + return -1; + + char buf[2048]; + const int len = linux_raw_recv(sockFd, buf, static_cast(sizeof(buf)), hwts, flag); + if (len < 0) + return -1; + + std::memcpy(recv_buffer, buf, static_cast(len)); + return len; +} + +int raw_sendMsg(int sockFd, void *buffer, int bufLen, ::timespec *hwts) +{ + return linux_raw_send(sockFd, buffer, bufLen, hwts); +} + +#endif + +} // namespace tsyncd diff --git a/src/tsyncd/protocol/raw_socket.hpp b/src/tsyncd/protocol/raw_socket.hpp new file mode 100644 index 0000000..936e8c6 --- /dev/null +++ b/src/tsyncd/protocol/raw_socket.hpp @@ -0,0 +1,21 @@ +/******************************************************************************** +* 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 +********************************************************************************/ +#pragma once +#include + +namespace tsyncd +{ + int setup_raw_socket(int &sockFd, const char *interface_name); + int raw_sendMsg(int sockFd, void *send_buffer, int bufLen, ::timespec *hwts); + int raw_recvMsg(int sockFd, void *recv_buffer, ::timespec *hwts, int flag); // flag:0 blocking,1 errqueue +} diff --git a/toolchains/BUILD b/toolchains/BUILD new file mode 100644 index 0000000..5d3f458 --- /dev/null +++ b/toolchains/BUILD @@ -0,0 +1,140 @@ +load(":cc_toolchain_config.bzl", "aarch64_toolchain_config") + +package( + default_visibility = ["//visibility:public"], +) + +# ========== Linux aarch64 ========== + +filegroup( + name = "linux_aarch64_files", + srcs = [], +) + +aarch64_toolchain_config( + name = "linux_aarch64_toolchain_config", + builtin_sysroot = "/opt/bstos/linux-23/sysroots/aarch64-bst-linux", + compiler = "gcc", + cxx_builtin_include_directories = [ + "/opt/bstos/linux-23/sysroots/aarch64-bst-linux/usr/include", + "", + "/work/toolchains/a1000_linux/esr/fc_linux_pack/usr/include", + "/work/toolchains/a1000_linux/bstos_2.3.5.4/sysroots/x86_64-bstsdk-linux/usr/lib/aarch64-bst-linux/gcc/aarch64-bst-linux/8.3.0/include", + "/work/toolchains/a1000_linux/bstos_2.3.5.4/sysroots/x86_64-bstsdk-linux/usr/lib/aarch64-bst-linux/gcc/aarch64-bst-linux/8.3.0/include-fixed", + "/work/toolchains/a1000_linux/bstos_2.3.5.4/sysroots/x86_64-bstsdk-linux/usr/lib/gcc/aarch64-bst-linux/8.3.0/include", + ], + target_libc = "glibc", + target_system_name = "aarch64-linux-gnu", + tool_path_names = [ + "ar", + "cpp", + "gcc", + "g++", + "ld", + "nm", + "objcopy", + "objdump", + "strip", + ], + tool_path_paths = [ + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-ar", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-cpp", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-gcc", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-g++", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-ld", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-nm", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-objcopy", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-objdump", + "/opt/bstos/linux-23/sysroots/x86_64-bstsdk-linux/usr/bin/aarch64-bst-linux/aarch64-bst-linux-strip", + ], + toolchain_identifier = "linux_aarch64_gnu", +) + +cc_toolchain( + name = "linux_aarch64_cc_toolchain", + all_files = ":linux_aarch64_files", + ar_files = ":linux_aarch64_files", + compiler_files = ":linux_aarch64_files", + dwp_files = ":linux_aarch64_files", + linker_files = ":linux_aarch64_files", + objcopy_files = ":linux_aarch64_files", + strip_files = ":linux_aarch64_files", + toolchain_config = ":linux_aarch64_toolchain_config", + toolchain_identifier = "linux_aarch64_gnu", +) + +toolchain( + name = "linux_aarch64_toolchain", + target_compatible_with = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], + toolchain = ":linux_aarch64_cc_toolchain", + toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", +) + +# ========== QNX aarch64 ========== + +filegroup( + name = "qnx_aarch64_files", + srcs = [], +) + +aarch64_toolchain_config( + name = "qnx_aarch64_toolchain_config", + builtin_sysroot = "/work/toolchains/qnx710/qnx710/target/qnx7", + compiler = "gcc", + cxx_builtin_include_directories = [ + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/lib/gcc/aarch64-unknown-nto-qnx7.1.0/8.3.0/include", + "/work/toolchains/qnx710/qnx710/target/qnx7/usr/include/c++/v1", + "/work/toolchains/qnx710/qnx710/target/qnx7/usr/include", + ], + target_libc = "qnx", + target_system_name = "aarch64-unknown-nto-qnx", + tool_path_names = [ + "ar", + "cpp", + "gcc", + "g++", + "ld", + "nm", + "objcopy", + "objdump", + "strip", + ], + tool_path_paths = [ + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-ar", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-cpp", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-gcc", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-g++", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-ld", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-nm", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-objcopy", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-objdump", + "/work/toolchains/qnx710/qnx710/host/linux/x86_64/usr/bin/aarch64-unknown-nto-qnx7.1.0-strip", + ], + toolchain_identifier = "qnx_aarch64_gnu", +) + +cc_toolchain( + name = "qnx_aarch64_cc_toolchain", + all_files = ":qnx_aarch64_files", + ar_files = ":qnx_aarch64_files", + compiler_files = ":qnx_aarch64_files", + dwp_files = ":qnx_aarch64_files", + linker_files = ":qnx_aarch64_files", + objcopy_files = ":qnx_aarch64_files", + strip_files = ":qnx_aarch64_files", + toolchain_config = ":qnx_aarch64_toolchain_config", + toolchain_identifier = "qnx_aarch64_gnu", +) + +toolchain( + name = "qnx_aarch64_toolchain", + target_compatible_with = [ + "@platforms//cpu:aarch64", + "//platforms:qnx", + ], + toolchain = ":qnx_aarch64_cc_toolchain", + toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", +) diff --git a/toolchains/cc_toolchain_config.bzl b/toolchains/cc_toolchain_config.bzl new file mode 100644 index 0000000..87fe998 --- /dev/null +++ b/toolchains/cc_toolchain_config.bzl @@ -0,0 +1,45 @@ +load("@rules_cc//cc:cc_toolchain_config_lib.bzl", "tool_path") + +def _aarch64_toolchain_config_impl(ctx): + names = ctx.attr.tool_path_names + paths = ctx.attr.tool_path_paths + if len(names) != len(paths): + fail("tool_path_names and tool_path_paths length mismatch: %d vs %d" % (len(names), len(paths))) + + tool_paths = [] + for i in range(len(names)): + tool_paths.append(tool_path(name = names[i], path = paths[i])) + + return [cc_common.create_cc_toolchain_config_info( + ctx = ctx, + toolchain_identifier = ctx.attr.toolchain_identifier, + host_system_name = "local", + target_system_name = ctx.attr.target_system_name, + target_cpu = "aarch64", + target_libc = ctx.attr.target_libc, + compiler = ctx.attr.compiler, + abi_version = ctx.attr.abi_version, + abi_libc_version = ctx.attr.abi_libc_version, + tool_paths = tool_paths, + cxx_builtin_include_directories = ctx.attr.cxx_builtin_include_directories, + builtin_sysroot = ctx.attr.builtin_sysroot, + features = [], + action_configs = [], + )] + +aarch64_toolchain_config = rule( + implementation = _aarch64_toolchain_config_impl, + attrs = { + "toolchain_identifier": attr.string(mandatory = True), + "target_system_name": attr.string(mandatory = True), + "compiler": attr.string(mandatory = True), + "tool_path_names": attr.string_list(mandatory = True), + "tool_path_paths": attr.string_list(mandatory = True), + "cxx_builtin_include_directories": attr.string_list(mandatory = True), + "builtin_sysroot": attr.string(default = ""), + "target_libc": attr.string(default = "unknown"), + "abi_version": attr.string(default = "unknown"), + "abi_libc_version": attr.string(default = "unknown"), + }, + provides = [CcToolchainConfigInfo], +) diff --git a/tools/BUILD b/tools/BUILD new file mode 100644 index 0000000..617d7a9 --- /dev/null +++ b/tools/BUILD @@ -0,0 +1,25 @@ +# ******************************************************************************* +# 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 +# ******************************************************************************* + +py_binary( + name = "doc_generator", + srcs = ["generate_docs.py"], + visibility = ["//visibility:public"], +) + +sh_binary( + name = "doc_gen", + srcs = ["doc_gen.sh"], + data = [":doc_generator"], + visibility = ["//visibility:public"], +) diff --git a/tools/doc_gen.sh b/tools/doc_gen.sh new file mode 100644 index 0000000..4f6b34a --- /dev/null +++ b/tools/doc_gen.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# ******************************************************************************* +# 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 +# ******************************************************************************* + +set -e + +# Find the doc_generator binary +GENERATOR=$(dirname "$0")/doc_generator + +# Run documentation generator with all arguments passed through +exec python3 "$GENERATOR" "$@" diff --git a/tools/generate_docs.py b/tools/generate_docs.py new file mode 100644 index 0000000..e143c66 --- /dev/null +++ b/tools/generate_docs.py @@ -0,0 +1,747 @@ +#!/usr/bin/env python3 +# ******************************************************************************* +# 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 +# ******************************************************************************* + +""" +Auto-generate documentation (.rst and .puml files) from C++ source code. + +This script scans the SCORE Time Synchronization codebase and generates: +1. API documentation in ReStructuredText (.rst) format +2. Architecture diagrams in PlantUML (.puml) format +3. Sequence diagrams for key workflows +""" + +import argparse +import re +import sys +from pathlib import Path +from typing import List, Dict, Optional, Tuple +from dataclasses import dataclass + + +@dataclass +class CppClass: + """Represents a C++ class extracted from source.""" + name: str + namespace: str + brief: str + file_path: str + is_struct: bool = False + members: List[str] = None + methods: List[str] = None + + def __post_init__(self): + if self.members is None: + self.members = [] + if self.methods is None: + self.methods = [] + + +@dataclass +class CppFunction: + """Represents a C++ function.""" + name: str + signature: str + brief: str + returns: str + file_path: str + + +def extract_doxygen_comment(lines: List[str], start_idx: int) -> Tuple[Optional[str], int]: + """ + Extract Doxygen-style comment block starting at start_idx. + Returns (comment_text, next_line_index) + """ + if start_idx >= len(lines): + return None, start_idx + + line = lines[start_idx].strip() + + # Check for /** ... */ style + if line.startswith('/**'): + comment_lines = [] + i = start_idx + in_comment = True + + while i < len(lines) and in_comment: + line = lines[i].strip() + if line.startswith('/**'): + line = line[3:].strip() + if line.endswith('*/'): + line = line[:-2].strip() + in_comment = False + elif line.startswith('*'): + line = line[1:].strip() + + if line: + comment_lines.append(line) + i += 1 + + return '\n'.join(comment_lines), i + + # Check for /// style + if line.startswith('///'): + comment_lines = [] + i = start_idx + while i < len(lines) and lines[i].strip().startswith('///'): + line = lines[i].strip()[3:].strip() + if line: + comment_lines.append(line) + i += 1 + return '\n'.join(comment_lines), i + + return None, start_idx + + +def extract_classes_from_file(file_path: Path) -> List[CppClass]: + """Extract C++ classes from header file.""" + classes = [] + + try: + with open(file_path, 'r', encoding='utf-8') as f: + lines = f.readlines() + except Exception as e: + print(f"Warning: Could not read {file_path}: {e}", file=sys.stderr) + return classes + + i = 0 + while i < len(lines): + line = lines[i].strip() + + # Look for class/struct definition + class_match = re.match(r'(class|struct)\s+(\w+)', line) + if class_match: + is_struct = class_match.group(1) == 'struct' + class_name = class_match.group(2) + + # Extract preceding Doxygen comment by looking backwards + comment = extract_comment_before_line(lines, i) + brief = extract_brief_from_comment(comment) if comment else f"{class_name} class" + + # Extract namespace + namespace = extract_namespace(lines, i) + + cpp_class = CppClass( + name=class_name, + namespace=namespace, + brief=brief, + file_path=str(file_path), + is_struct=is_struct + ) + classes.append(cpp_class) + + i += 1 + + return classes + + +def extract_comment_before_line(lines: List[str], target_line: int) -> Optional[str]: + """ + Extract Doxygen comment block immediately before target line. + Skips empty lines and looks for /** ... */ or /// blocks. + """ + # Look backwards from target line, skip empty lines + i = target_line - 1 + while i >= 0 and not lines[i].strip(): + i -= 1 + + if i < 0: + return None + + # Check if we're at the end of a comment block + line = lines[i].strip() + + # Case 1: Multi-line /** ... */ comment + if line.endswith('*/'): + comment_lines = [] + in_comment = True + + while i >= 0 and in_comment: + line = lines[i].strip() + + if line.startswith('/**'): + # Remove /** prefix + line = line[3:].strip() + if line.endswith('*/'): + line = line[:-2].strip() + if line: + comment_lines.insert(0, line) + in_comment = False + elif line.endswith('*/'): + # Remove */ suffix + line = line[:-2].strip() + if line.startswith('*'): + line = line[1:].strip() + if line: + comment_lines.insert(0, line) + elif line.startswith('*'): + # Remove leading * + line = line[1:].strip() + if line: + comment_lines.insert(0, line) + else: + in_comment = False + + i -= 1 + + return '\n'.join(comment_lines) if comment_lines else None + + # Case 2: /// style comments + if line.startswith('///'): + comment_lines = [] + while i >= 0 and lines[i].strip().startswith('///'): + line = lines[i].strip()[3:].strip() + if line: + comment_lines.insert(0, line) + i -= 1 + return '\n'.join(comment_lines) if comment_lines else None + + return None + + +def extract_namespace(lines: List[str], class_line_idx: int) -> str: + """Extract namespace for a class definition.""" + namespace_parts = [] + for i in range(class_line_idx - 1, -1, -1): + line = lines[i].strip() + ns_match = re.match(r'namespace\s+([\w:]+)', line) + if ns_match: + namespace_parts.insert(0, ns_match.group(1)) + return '::'.join(namespace_parts) if namespace_parts else "" + + +def extract_brief_from_comment(comment: str) -> str: + """Extract @brief description from Doxygen comment.""" + if not comment: + return "" + + # Try to extract @brief tag + brief_match = re.search(r'@brief\s+(.+?)(?:\n\n|\n@|$)', comment, re.DOTALL) + if brief_match: + return brief_match.group(1).strip().replace('\n', ' ') + + # If no @brief tag, use first paragraph + lines = comment.split('\n') + brief_lines = [] + for line in lines: + line = line.strip() + if not line: # Empty line ends the brief + break + if line.startswith('@'): # Tag starts, end brief + break + brief_lines.append(line) + + return ' '.join(brief_lines) if brief_lines else "" + + +def extract_full_description(comment: str) -> str: + """Extract full description from Doxygen comment, including examples.""" + if not comment: + return "" + + # Remove @brief section + desc = re.sub(r'@brief\s+.+?(?:\n\n|\n@)', '', comment, flags=re.DOTALL) + + # Clean up remaining text + lines = [] + for line in desc.split('\n'): + line = line.strip() + if line: + lines.append(line) + + return '\n\n '.join(lines) if lines else "" + + +def generate_api_rst(component_name: str, classes: List[CppClass], output_file: Path): + """Generate RST file for component API documentation.""" + content = f""".. ****************************************************************************** + 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 + ****************************************************************************** + +{component_name} API Reference +{'=' * (len(component_name) + 14)} + +.. contents:: + :local: + :depth: 2 + +Overview +-------- + +This module provides {component_name.lower()} for the SCORE Time Synchronization system. + +""" + + # Group classes by namespace + by_namespace = {} + for cls in classes: + ns = cls.namespace or "global" + if ns not in by_namespace: + by_namespace[ns] = [] + by_namespace[ns].append(cls) + + # Generate documentation for each namespace + for namespace, ns_classes in sorted(by_namespace.items()): + if namespace != "global": + title = f"Namespace: ``{namespace}``" + content += f"\n{title}\n" + content += "-" * len(title) + "\n\n" + + for cls in ns_classes: + full_name = f"{namespace}::{cls.name}" if namespace != "global" else cls.name + + content += f"\n{cls.name}\n" + content += "~" * len(cls.name) + "\n\n" + + cls_type = "struct" if cls.is_struct else "class" + content += f".. cpp:{cls_type}:: {full_name}\n\n" + + if cls.brief: + # Wrap brief description with proper indentation + brief_lines = cls.brief.split('\n') + for line in brief_lines: + if line.strip(): + content += f" {line}\n" + content += "\n" + + content += f" **Defined in:** :code:`{Path(cls.file_path).name}`\n\n" + + # Add usage note for key classes + if "Lock" in cls.name: + content += " **Usage:** RAII wrapper for automatic resource management.\n\n" + elif "Shared" in cls.name: + content += " **Usage:** Shared memory structure for lock-free IPC.\n\n" + elif "Parse" in cls.name: + content += " **Usage:** Exception-free string parsing for configuration.\n\n" + + # Add examples section + content += "\nExamples\n" + content += "--------\n\n" + content += "See the test files for usage examples:\n\n" + content += "- Unit tests: ``tests/cpp/``\n" + content += "- Integration tests: ``tests/integration/``\n\n" + + # Write to file + output_file.parent.mkdir(parents=True, exist_ok=True) + with open(output_file, 'w', encoding='utf-8') as f: + f.write(content) + + print(f"✅ Generated {output_file}") + + +def generate_architecture_puml(output_file: Path): + """Generate high-level architecture diagram.""" + content = """@startuml +!theme plain +skinparam componentStyle rectangle + +package "SCORE Time Synchronization" { + component [tsyncd Daemon] as daemon + component [libscore_time] as lib + database "Shared Memory" as shm + + package "Common Utilities" { + component [ClockNs] as clock + component [PthreadLockGuard] as lock + component [ParseInteger/Double] as parser + } +} + +cloud "Network" { + component [gPTP Master] as gptp + component [NTP Server] as ntp +} + +actor "Application" as app + +daemon --> gptp : "Sync via\\nEthernet/PTP" +daemon --> ntp : "Query via\\nUDP/123" +daemon --> shm : "Writes time\\n(seqlock)" +lib --> shm : "Reads time\\n(seqlock)" +app --> lib : "GetVehicleTime()\\nGetAbsoluteTime()" +daemon ..> clock : uses +daemon ..> lock : uses +daemon ..> parser : uses +lib ..> clock : uses + +note right of shm + Lock-free IPC + Seqlock pattern + Atomic operations + Version: 4 +end note + +@enduml +""" + + output_file.parent.mkdir(parents=True, exist_ok=True) + with open(output_file, 'w', encoding='utf-8') as f: + f.write(content) + + print(f"✅ Generated {output_file}") + + +def generate_class_diagram_puml(output_file: Path): + """Generate core class relationships diagram.""" + content = """@startuml +!theme plain + +package "score_time::ipc" { + class SharedState { + +kMagic: uint32_t {static} + +kVersion: uint16_t {static} + -- + +magic: atomic + +version: atomic + +vehicle_seq: atomic + +vehicle_base_ns: atomic + +vehicle_base_mono_ns: atomic + +vehicle_acc: atomic + +vehicle_tpq: atomic + -- + +vehicle_log[256]: SyncLogEntry + +abs_log[256]: SyncLogEntry + } + + class SyncLogEntry { + +seq: atomic + +monotonic_ns: atomic + +type: atomic + +flags: atomic + +v1: atomic + +v2: atomic + } + + class ShmRegion { + -fd_: int + -addr_: void* + -size_: size_t + -- + +Open(name): bool + +Close(): void + +Addr(): void* + +Size(): size_t + } + + SharedState *-- "512" SyncLogEntry : contains + ShmRegion ..> SharedState : "maps to" +} + +package "score_time::utils" { + class PthreadLockGuard { + -mtx_: pthread_mutex_t* + -- + +PthreadLockGuard(mtx) + +~PthreadLockGuard() + -- + {method} Deleted: copy, move + } + + class "ParseInteger" as ParseInt { + +ParseInteger(str, out): bool {static} + } + + class ParseDouble { + +ParseDouble(str, out): bool {static} + } + + class ClockNs { + +ClockNsSafe(clk): optional {static} + +ClockNs(clk): int64_t {static} + } +} + +package "tsyncd" { + class TimeSyncEngine { + -ctx_: Context + -shared_: SharedState* + -shm_: ShmRegion + -- + +Start(): bool + +Stop(): void + -RxLoop(): void + -PdelayLoop(): void + -AbsLoop(): void + } +} + +TimeSyncEngine --> SharedState : writes to +TimeSyncEngine --> ShmRegion : owns +TimeSyncEngine ..> PthreadLockGuard : uses +TimeSyncEngine ..> ClockNs : uses + +note right of SharedState + Seqlock protocol: + seq odd = writing + seq even = readable +end note + +note right of PthreadLockGuard + RAII lock wrapper + Aborts on error +end note + +@enduml +""" + + output_file.parent.mkdir(parents=True, exist_ok=True) + with open(output_file, 'w', encoding='utf-8') as f: + f.write(content) + + print(f"✅ Generated {output_file}") + + +def generate_sequence_gptp_puml(output_file: Path): + """Generate gPTP synchronization sequence diagram.""" + content = """@startuml +!theme plain + +participant "gPTP Master" as Master +participant "tsyncd::RxLoop" as RxLoop +participant "SharedState" as SHM +participant "Client App" as Client + +== gPTP Synchronization == + +Master -> RxLoop: Sync message\\n(HW timestamp t1) +activate RxLoop +RxLoop -> RxLoop: Extract timestamp +RxLoop -> Master: Delay_Req\\n(HW timestamp t3) +deactivate RxLoop + +Master -> RxLoop: Delay_Resp\\n(t2, t4 timestamps) +activate RxLoop + +RxLoop -> RxLoop: Calculate offset\\noffset = ((t2-t1) + (t3-t4)) / 2 + +RxLoop -> SHM: WriteVehicle(base_ns, base_mono_ns, ...) +note right + Seqlock write protocol: + 1. seq.fetch_add(1, release) // odd = writing + 2. Write all fields with release stores + 3. seq.fetch_add(1, release) // even = complete +end note +deactivate RxLoop + +...Time passes... + +Client -> SHM: ReadVehicle(out_base_ns, out_base_mono_ns, ...) +activate Client +note right + Seqlock read protocol (retry up to 1000x): + 1. seq_a = seq.load(acquire) + 2. if seq_a is odd: retry + 3. Read all fields with acquire loads + 4. seq_b = seq.load(acquire) + 5. if seq_a != seq_b: retry + 6. Success! +end note +SHM --> Client: vehicle_base_ns, accuracy +Client -> Client: current = base_ns + (now_mono - base_mono) +deactivate Client + +@enduml +""" + + output_file.parent.mkdir(parents=True, exist_ok=True) + with open(output_file, 'w', encoding='utf-8') as f: + f.write(content) + + print(f"✅ Generated {output_file}") + + +def update_index_rst(docs_dir: Path, has_api: bool, has_diagrams: bool): + """Update docs/index.rst to include generated documentation.""" + index_file = docs_dir / "index.rst" + + # Read existing content + if index_file.exists(): + with open(index_file, 'r', encoding='utf-8') as f: + content = f.read() + else: + content = "" + + # Check if toctree exists + if ".. toctree::" not in content: + # Append new toctree + additions = "\n\n" + if has_api: + additions += "API Reference\n" + additions += "-------------\n\n" + additions += ".. toctree::\n" + additions += " :maxdepth: 2\n\n" + additions += " api/index\n\n" + + if has_diagrams: + additions += "Architecture Diagrams\n" + additions += "--------------------\n\n" + additions += ".. toctree::\n" + additions += " :maxdepth: 1\n\n" + additions += " diagrams/index\n\n" + + with open(index_file, 'a', encoding='utf-8') as f: + f.write(additions) + + print(f"✅ Updated {index_file}") + + +def main(): + parser = argparse.ArgumentParser(description="Generate SCORE Time documentation") + parser.add_argument('--api', action='store_true', help="Generate API documentation") + parser.add_argument('--arch', action='store_true', help="Generate architecture diagrams") + parser.add_argument('--seq', action='store_true', help="Generate sequence diagrams") + parser.add_argument('--all', action='store_true', help="Generate all documentation") + parser.add_argument('--update-index', action='store_true', help="Update docs/index.rst") + + args = parser.parse_args() + + # Default to --all if no options specified + if not (args.api or args.arch or args.seq or args.all): + args.all = True + + if args.all: + args.api = args.arch = args.seq = args.update_index = True + + # Determine project root + script_path = Path(__file__).resolve() + project_root = script_path.parent.parent + docs_dir = project_root / "docs" + + print(f"Generating documentation for SCORE Time Synchronization...") + print(f"Project root: {project_root}") + + # Generate API documentation + if args.api: + print("\n📚 Generating API documentation...") + + # Scan header files + include_dirs = [ + project_root / "src" / "common" / "include" / "score_time", + project_root / "src" / "tsyncd" / "engine", + ] + + components = { + "Core Types": [], + "IPC": [], + "Utilities": [], + } + + for include_dir in include_dirs: + if not include_dir.exists(): + continue + + for header_file in include_dir.rglob("*.hpp"): + classes = extract_classes_from_file(header_file) + + # Categorize classes + file_str = str(header_file) + if "shared_state" in file_str or "types" in file_str: + components["Core Types"].extend(classes) + elif "ipc" in file_str or "shm" in file_str: + components["IPC"].extend(classes) + elif "utils" in file_str: + components["Utilities"].extend(classes) + + # Generate RST files + api_dir = docs_dir / "api" + for component_name, classes in components.items(): + if classes: + output_file = api_dir / f"{component_name.lower().replace(' ', '_')}.rst" + generate_api_rst(component_name, classes, output_file) + + # Generate API index + api_index = api_dir / "index.rst" + api_index_content = """API Reference +============= + +.. toctree:: + :maxdepth: 2 + +""" + for component_name in components.keys(): + filename = component_name.lower().replace(' ', '_') + api_index_content += f" {filename}\n" + + api_dir.mkdir(parents=True, exist_ok=True) + with open(api_index, 'w', encoding='utf-8') as f: + f.write(api_index_content) + print(f"✅ Generated {api_index}") + + # Generate architecture diagrams + if args.arch: + print("\n🏗️ Generating architecture diagrams...") + diagrams_dir = docs_dir / "diagrams" + + generate_architecture_puml(diagrams_dir / "architecture.puml") + generate_class_diagram_puml(diagrams_dir / "class_relationships.puml") + + # Generate sequence diagrams + if args.seq: + print("\n📊 Generating sequence diagrams...") + diagrams_dir = docs_dir / "diagrams" + + generate_sequence_gptp_puml(diagrams_dir / "sequence_gptp_sync.puml") + + # Generate diagrams index + diagrams_index = diagrams_dir / "index.rst" + diagrams_index_content = """Architecture Diagrams +===================== + +System Architecture +------------------- + +.. uml:: architecture.puml + :caption: High-level system architecture + +Class Relationships +------------------- + +.. uml:: class_relationships.puml + :caption: Core class relationships + +Sequence Diagrams +----------------- + +gPTP Synchronization +~~~~~~~~~~~~~~~~~~~~ + +.. uml:: sequence_gptp_sync.puml + :caption: gPTP synchronization workflow +""" + + with open(diagrams_index, 'w', encoding='utf-8') as f: + f.write(diagrams_index_content) + print(f"✅ Generated {diagrams_index}") + + # Update index + if args.update_index: + update_index_rst(docs_dir, args.api, args.arch or args.seq) + + print("\n✅ Documentation generation complete!") + print(f"\nNext steps:") + print(f" 1. Review generated files in {docs_dir}") + print(f" 2. Build documentation: bazel run //:docs") + print(f" 3. View HTML: open bazel-bin/docs/docs/html/index.html") + + +if __name__ == "__main__": + main()