diff --git a/CMakeLists.txt b/CMakeLists.txt index 521ec03c30..4f6aea64a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,8 @@ option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_PLUGIN_WUJI_GLOVE "Build Wuji glove plugin (requires the wuji_sdk C SDK)" OFF) option(BUILD_PLUGIN_SHARPA_AVATAR "Build Sharpa AvatarExo Glove plugin (Linux; skips if the Avatar SDK is missing)" OFF) +option(BUILD_PLUGIN_SENSING "Build SENSING GMSL camera plugin (Jetson only; requires SIPL, NVENC and the LiveKit SDK)" OFF) +option(BUILD_VIDEO_STAGE "Build the PC GPU video stage (needs a CUDA toolkit; tests need a CUDA GPU)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 ROS 2 reference integration (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index cdef8fe380..5b9b792ceb 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -25,6 +25,25 @@ if(BUILD_VIZ) add_subdirectory(viz) endif() +# The LiveKit SDK and publisher library: the PC video stage publishes to LiveKit. The camera producer on the Orin does +# not use LiveKit; it sends over its direct links. +if(BUILD_VIDEO_STAGE) + add_subdirectory(livekit_publisher) +endif() + +if(BUILD_VIDEO_STAGE) + add_subdirectory(video_stage) +elseif(BUILD_PLUGIN_SENSING) + # The SENSING plugin sends AVS1 records to the video stage and needs only its record codec and sender. + add_subdirectory(video_stage/wire) + add_subdirectory(video_stage/sender) +endif() + +# The camera producer's management channel, also used by the mock producer. +if(BUILD_PLUGIN_SENSING OR BUILD_VIDEO_STAGE) + add_subdirectory(sensing_control) +endif() + if(BUILD_PLUGINS) add_subdirectory(plugins) endif() diff --git a/src/livekit_publisher/CMakeLists.txt b/src/livekit_publisher/CMakeLists.txt new file mode 100644 index 0000000000..daa5a1de99 --- /dev/null +++ b/src/livekit_publisher/CMakeLists.txt @@ -0,0 +1,95 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# LiveKit +# ============================================================================== +# Each selected sensor is published as a LiveKit camera track. The SDK is a prebuilt package (headers and shared libraries), pinned to a +# release and checked against its hash; point LIVEKIT_SDK_DIR at an unpacked copy to use another. +set(_livekit_local ON) +if(NOT LIVEKIT_SDK_DIR) + set(_livekit_local OFF) + if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") + set(_livekit_arch "arm64") + set(_livekit_hash "e3a97f71943c0bf9fa4ebd70927ca2906b31dcd6c14b42312b1225b4b868e4ad") + elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|AMD64") + set(_livekit_arch "x64") + set(_livekit_hash "5e0feb2e519c6ff131dd2d1c45149115057ae4e21a324fdb249e1789d5614e7b") + else() + message(FATAL_ERROR "No prebuilt LiveKit SDK for ${CMAKE_SYSTEM_PROCESSOR}; set LIVEKIT_SDK_DIR.") + endif() + include(FetchContent) + FetchContent_Declare( + livekit_sdk + URL https://github.com/livekit/client-sdk-cpp/releases/download/v1.12.4/livekit-sdk-ubuntu-24.04-${_livekit_arch}-1.12.4.tar.gz + URL_HASH SHA256=${_livekit_hash} + ) + FetchContent_MakeAvailable(livekit_sdk) + set(LIVEKIT_SDK_DIR "${livekit_sdk_SOURCE_DIR}") +endif() +# The package ships no licence files. The pinned download supplies the SDK's own; a local SDK uses +# LICENSE/NOTICE from its directory, or LIVEKIT_SDK_LICENSE_FILE / LIVEKIT_SDK_NOTICE_FILE, and needs no network. +foreach(_notice LICENSE NOTICE) + set(_override "${LIVEKIT_SDK_${_notice}_FILE}") + if(_override) + set(_found "${_override}") + elseif(_livekit_local) + set(_found "") + foreach(_candidate "${LIVEKIT_SDK_DIR}/${_notice}" "${LIVEKIT_SDK_DIR}/../${_notice}") + if(NOT _found AND EXISTS "${_candidate}") + set(_found "${_candidate}") + endif() + endforeach() + if(NOT _found) + message(FATAL_ERROR + "LIVEKIT_SDK_DIR is set but ${_notice} was not found in it. Put the SDK's ${_notice} file there " + "or pass -DLIVEKIT_SDK_${_notice}_FILE=/path/to/${_notice}.") + endif() + else() + if(_notice STREQUAL "LICENSE") + set(_notice_hash "09e8a9bcec8067104652c168685ab0931e7868f9c8284b66f5ae6edae5f1130b") + else() + set(_notice_hash "6d0f7c5e0deb284362296c5da110dd353831da2e67ca77cb5abc051b951b0fac") + endif() + FetchContent_Declare( + livekit_sdk_${_notice} + URL https://raw.githubusercontent.com/livekit/client-sdk-cpp/v1.12.4/${_notice} + URL_HASH SHA256=${_notice_hash} + DOWNLOAD_NO_EXTRACT TRUE + ) + FetchContent_MakeAvailable(livekit_sdk_${_notice}) + # FetchContent publishes _SOURCE_DIR. + string(TOLOWER "${_notice}" _notice_lower) + set(_found "${livekit_sdk_${_notice_lower}_SOURCE_DIR}/${_notice}") + endif() + set(SENSING_LIVEKIT_${_notice}_FILE "${_found}" CACHE INTERNAL "") +endforeach() +find_package(LiveKit CONFIG REQUIRED PATHS "${LIVEKIT_SDK_DIR}" NO_DEFAULT_PATH) +message(STATUS "LiveKit SDK=${LIVEKIT_SDK_DIR}") +# Cache, so tests/ (a sibling directory) can find the same SDK. +set(SENSING_LIVEKIT_SDK_DIR "${LIVEKIT_SDK_DIR}" CACHE INTERNAL "") + + + +# The LiveKit SDK behind a small interface: join a room, publish a video track, submit H.264 access units. +# No LiveKit type appears in an installed header, and nothing here touches CUDA. +add_library(sensing_livekit STATIC + livekit_session.cpp + inc/sensing_livekit/livekit_session.hpp + inc/sensing_livekit/video_track_sink.hpp +) + +target_include_directories(sensing_livekit + INTERFACE + $ +) + +target_link_libraries(sensing_livekit + PUBLIC + log_bridge::log_bridge_core + video_stage::wire + PRIVATE + LiveKit::livekit +) + +add_library(sensing::livekit ALIAS sensing_livekit) diff --git a/src/livekit_publisher/inc/sensing_livekit/livekit_session.hpp b/src/livekit_publisher/inc/sensing_livekit/livekit_session.hpp new file mode 100644 index 0000000000..7cf33bcfea --- /dev/null +++ b/src/livekit_publisher/inc/sensing_livekit/livekit_session.hpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "video_track_sink.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct LiveKitSessionImpl; + +/** + * @brief Keeps the LiveKit SDK initialised while it is held. + * + * The SDK is process-wide and must be shut down only when nothing in the process uses it, so every + * user holds one of these and the last release shuts it down. + */ +class LiveKitSdkReference +{ +public: + LiveKitSdkReference(); + ~LiveKitSdkReference(); + LiveKitSdkReference(const LiveKitSdkReference&) = delete; + LiveKitSdkReference& operator=(const LiveKitSdkReference&) = delete; +}; + +struct PublishSpec +{ + /// Name subscribers see for the track. + std::string name; + uint32_t width = 0; + uint32_t height = 0; + /// Upper bound the upstream encoder may use; WebRTC adapts below it. + uint64_t max_bitrate_bps = 0; + double max_fps = 30.0; +}; + +/** + * @brief One participant in one LiveKit room. + * + * The room and the participant's identity come from the join token. A terminal loss of the room + * (the server went away, the participant was removed) sets failed(); the SDK's own bounded + * reconnection runs first and is not wrapped in another supervisor. + */ +class LiveKitSession +{ +public: + /// @throws std::runtime_error if the room cannot be joined. The token is never logged. + static std::unique_ptr connect(const std::string& url, const std::string& token); + + /// Unpublish every track (destroy them first), then leave the room. + ~LiveKitSession(); + + LiveKitSession(const LiveKitSession&) = delete; + LiveKitSession& operator=(const LiveKitSession&) = delete; + + /// Publishes an H.264 camera track that carries pre-encoded access units; the SDK does not encode. + /// @throws std::runtime_error if publication fails. + std::unique_ptr publish_video(const PublishSpec& spec); + + /// True once the room has been lost for good. + bool failed() const; + std::string failure() const; + +private: + explicit LiveKitSession(std::shared_ptr impl); + std::shared_ptr m_impl; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/livekit_publisher/inc/sensing_livekit/video_track_sink.hpp b/src/livekit_publisher/inc/sensing_livekit/video_track_sink.hpp new file mode 100644 index 0000000000..2eb813e640 --- /dev/null +++ b/src/livekit_publisher/inc/sensing_livekit/video_track_sink.hpp @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +using video_stage::EncodedVideoFrame; + +/// What the receiving side asked of the encoder since the last call. +struct EncoderFeedback +{ + /// The next frame must be an IDR with its parameter sets. + bool keyframe_requested = false; + /// Latest bitrate target from congestion control, if one was requested. + std::optional target_bitrate_bps; +}; + +/** + * @brief One published video track that carries frames already encoded by hardware. + * + * The SDK forwards each access unit without encoding it again. A single thread must drive one sink. + */ +class VideoTrackSink +{ +public: + virtual ~VideoTrackSink() = default; + + /// Size of the encoded pictures this track was published for. + virtual uint32_t width() const = 0; + virtual uint32_t height() const = 0; + + /// Publishes one access unit; the bytes are copied before this returns. + /// @return false when the SDK did not accept the frame, which can break the reference chain: + /// the caller must follow with a keyframe. + /// @throws std::runtime_error when the frame cannot be published at all. + virtual bool submit(const EncodedVideoFrame& frame) = 0; + + /// Keyframe and rate requests accumulated since the previous call. + virtual EncoderFeedback take_feedback() = 0; + + /// One line naming the transport's view of the stream, or empty if unavailable. + virtual std::string describe() = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/livekit_publisher/livekit_session.cpp b/src/livekit_publisher/livekit_session.cpp new file mode 100644 index 0000000000..69c8c8847a --- /dev/null +++ b/src/livekit_publisher/livekit_session.cpp @@ -0,0 +1,392 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_livekit/livekit_session.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +std::mutex g_sdk_mutex; +int g_sdk_users = 0; + +const char* reason_name(livekit::DisconnectReason reason) +{ + using R = livekit::DisconnectReason; + switch (reason) + { + case R::ClientInitiated: + return "client initiated"; + case R::DuplicateIdentity: + return "another participant joined with the same identity"; + case R::ServerShutdown: + return "the server is shutting down"; + case R::ParticipantRemoved: + return "the participant was removed from the room"; + case R::RoomDeleted: + return "the room was deleted"; + default: + return "connection lost"; + } +} + +} // namespace + +LiveKitSdkReference::LiveKitSdkReference() +{ + std::lock_guard lock(g_sdk_mutex); + if (g_sdk_users == 0) + livekit::initialize(livekit::LogLevel::Warn); + ++g_sdk_users; +} + +LiveKitSdkReference::~LiveKitSdkReference() +{ + std::lock_guard lock(g_sdk_mutex); + if (--g_sdk_users == 0) + livekit::shutdown(); +} + +struct LiveKitSessionImpl : livekit::RoomDelegate +{ + ~LiveKitSessionImpl() override + { + stop_clock_sync(); + // The members below are destroyed before the room: stop callbacks reaching them, then leave the + // room while the SDK is still up (its reference is released after the room). + room.setDelegate(nullptr); + room.disconnect(); + } + + // A clock-sync request from the PC or the web page. t2 is read first, before anything else runs; the reply is + // sent from a small thread so this SDK callback never waits on a send, and t3 is written just before it. + void onUserPacketReceived(livekit::Room&, const livekit::UserDataPacketEvent& event) override + { + const std::int64_t t2 = monotonic_ns(); + if (event.topic != kClockSyncTopic || event.participant == nullptr) + return; + const auto request = parse_clock_sync_request(event.data); + if (!request) + return; + { + std::lock_guard lock(sync_mutex); + // Bounded: a flood drops the oldest request, whose requester would discard a late reply anyway. + if (sync_queue.size() >= kMaxPendingSync) + sync_queue.pop_front(); + sync_queue.push_back({ event.participant->identity(), *request, t2 }); + } + sync_wake.notify_one(); + } + + void start_clock_sync() + { + sync_thread = std::thread([this] { run_clock_sync(); }); + } + + void stop_clock_sync() + { + { + std::lock_guard lock(sync_mutex); + sync_stopping = true; + } + sync_wake.notify_all(); + if (sync_thread.joinable()) + sync_thread.join(); + } + + void run_clock_sync() + { + for (;;) + { + PendingSync pending; + { + std::unique_lock lock(sync_mutex); + sync_wake.wait(lock, [this] { return sync_stopping || !sync_queue.empty(); }); + if (sync_stopping) + return; + pending = std::move(sync_queue.front()); + sync_queue.pop_front(); + } + auto reply = build_clock_sync_reply(pending.request, pending.t2, 0); + try + { + const auto local = room.localParticipant().lock(); + if (!local) + continue; + // t3 as late as possible: nothing but the call follows it. + set_clock_sync_reply_t3(reply, monotonic_ns()); + local->publishData(reply, false, { pending.identity }, kClockSyncTopic); + } + catch (const std::exception& e) + { + logger->warn("clock-sync reply failed: {}", e.what()); + } + } + } + + void onDisconnected(livekit::Room&, const livekit::DisconnectedEvent& event) override + { + if (event.reason == livekit::DisconnectReason::ClientInitiated) + return; + fail(std::string("LiveKit room lost: ") + reason_name(event.reason)); + } + + void onReconnecting(livekit::Room&, const livekit::ReconnectingEvent&) override + { + logger->warn("LiveKit connection interrupted; the SDK is reconnecting"); + } + + void onReconnected(livekit::Room&, const livekit::ReconnectedEvent&) override + { + logger->info("LiveKit connection restored"); + } + + void fail(const std::string& message) + { + std::lock_guard lock(mutex); + if (!failed.load()) + { + failure = message; + failed.store(true); + logger->error("{}", message); + } + } + + struct PendingSync + { + std::string identity; + ClockSyncRequest request; + std::int64_t t2 = 0; + }; + static constexpr std::size_t kMaxPendingSync = 16; + std::mutex sync_mutex; + std::condition_variable sync_wake; + std::deque sync_queue; + bool sync_stopping = false; + std::thread sync_thread; + + // Declared before the room, so destroyed after it. + LiveKitSdkReference sdk; + livekit::Room room; + std::atomic failed{ false }; + mutable std::mutex mutex; + std::string failure; + std::shared_ptr logger = isaaccapture::Logger::get("isaaccapture.plugins.sensing.LiveKitSession"); +}; + +namespace +{ + +/// A track fed with H.264 access units that were encoded upstream. +class LiveKitVideoTrack : public VideoTrackSink +{ +public: + LiveKitVideoTrack(std::shared_ptr session, const PublishSpec& spec) + : m_session(std::move(session)), m_width(spec.width), m_height(spec.height) + { + m_source = std::make_shared( + livekit::VideoCodec::H264, static_cast(spec.width), static_cast(spec.height)); + m_track = livekit::LocalVideoTrack::createLocalVideoTrack(spec.name, m_source); + + livekit::TrackPublishOptions options; + options.source = livekit::TrackSource::SOURCE_CAMERA; + options.video_codec = livekit::VideoCodec::H264; + // Pre-encoded access units go through as they are: no software or SDK-selected encoder. + options.video_encoder = livekit::VideoEncoderBackend::PreEncoded; + options.simulcast = false; + options.video_encoding = livekit::VideoEncodingOptions{ spec.max_bitrate_bps, spec.max_fps }; + options.frame_metadata_features = livekit::FrameMetadataFeatures{ true, true, true }; + + auto participant = m_session->room.localParticipant().lock(); + if (!participant) + throw std::runtime_error("LiveKit: no local participant to publish '" + spec.name + "'"); + try + { + participant->publishTrack(m_track, options); + } + catch (const std::exception& e) + { + throw std::runtime_error("LiveKit: cannot publish track '" + spec.name + "': " + e.what()); + } + m_participant = participant; + } + + ~LiveKitVideoTrack() override + { + try + { + if (auto participant = m_participant.lock()) + participant->unpublishTrack(m_track->sid()); + } + catch (const std::exception& e) + { + m_session->logger->warn("LiveKit: unpublish failed: {}", e.what()); + } + } + + uint32_t width() const override + { + return m_width; + } + uint32_t height() const override + { + return m_height; + } + + bool submit(const EncodedVideoFrame& frame) override + { + if (m_session->failed.load()) + throw std::runtime_error(m_session->failure); + + livekit::EncodedVideoSource::Frame out; + out.is_keyframe = frame.keyframe; + out.width = m_width; + out.height = m_height; + out.timestamp_us = static_cast(frame.timestamp_us); + out.data.assign(frame.data, frame.data + frame.size); + livekit::VideoFrameMetadata metadata; + // Zero means the publisher cannot vouch for a capture time: the frame is sent without one, and a consumer + // that needs a timestamp (a recording) says so instead of using a guessed one. + if (frame.user_timestamp_us != 0) + metadata.user_timestamp_us = frame.user_timestamp_us; + metadata.frame_id = frame.frame_id; + std::vector tsc(8); + for (int i = 0; i < 8; ++i) + tsc[i] = static_cast((frame.capture_tsc >> (8 * i)) & 0xFF); + metadata.user_data = std::move(tsc); + out.metadata = std::move(metadata); + + try + { + return m_source->captureFrame(out); + } + catch (const std::exception& e) + { + throw std::runtime_error(std::string("LiveKit: cannot submit a frame: ") + e.what()); + } + } + + EncoderFeedback take_feedback() override + { + EncoderFeedback feedback; + try + { + const auto pending = m_source->takeFeedback(); + feedback.keyframe_requested = pending.keyframe_requested; + if (pending.rate_control && pending.rate_control->target_bitrate_bps > 0) + feedback.target_bitrate_bps = static_cast(pending.rate_control->target_bitrate_bps); + } + catch (const std::exception& e) + { + throw std::runtime_error(std::string("LiveKit: cannot read encoder feedback: ") + e.what()); + } + return feedback; + } + + // Called from the polling thread, which must not wait on the network: this returns the last answer + // and starts, or collects, a stats request that was asked for earlier. + std::string describe() override + { + try + { + if (m_stats.valid() && m_stats.wait_for(std::chrono::seconds(0)) == std::future_status::ready) + { + for (const auto& entry : m_stats.get()) + { + const auto* out = std::get_if(&entry.stats); + if (!out || out->outbound.frame_width == 0) + continue; + const auto& o = out->outbound; + m_description = "encoder " + o.encoder_implementation + ", " + std::to_string(o.frame_width) + "x" + + std::to_string(o.frame_height) + " @ " + + std::to_string(static_cast(o.frames_per_second)) + " fps, " + + std::to_string(static_cast(o.target_bitrate / 1000)) + " kbps target"; + break; + } + } + if (!m_stats.valid()) + m_stats = m_track->getStats(); + } + catch (const std::exception&) + { + } + return m_description; + } + +private: + std::shared_ptr m_session; + uint32_t m_width; + uint32_t m_height; + std::shared_ptr m_source; + std::shared_ptr m_track; + std::weak_ptr m_participant; + decltype(std::declval().getStats()) m_stats; + std::string m_description; +}; + +} // namespace + +LiveKitSession::LiveKitSession(std::shared_ptr impl) : m_impl(std::move(impl)) +{ +} + +LiveKitSession::~LiveKitSession() = default; + +std::unique_ptr LiveKitSession::connect(const std::string& url, const std::string& token) +{ + auto impl = std::make_shared(); + impl->room.setDelegate(impl.get()); + + livekit::RoomOptions options; + options.auto_subscribe = false; // this process only publishes + options.dynacast = false; + if (!impl->room.connect(url, token, options)) + { + throw std::runtime_error("LiveKit: cannot join the room at " + url + + " (check the URL, that the server is reachable and that the token is valid)"); + } + impl->logger->info("Joined LiveKit room at {}", url); + impl->start_clock_sync(); + return std::unique_ptr(new LiveKitSession(std::move(impl))); +} + +std::unique_ptr LiveKitSession::publish_video(const PublishSpec& spec) +{ + return std::make_unique(m_impl, spec); +} + +bool LiveKitSession::failed() const +{ + return m_impl->failed.load(); +} + +std::string LiveKitSession::failure() const +{ + std::lock_guard lock(m_impl->mutex); + return m_impl->failure; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index c26d9384c2..7a3a400b15 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -44,3 +44,7 @@ endif() if(BUILD_PLUGIN_SHARPA_AVATAR) add_subdirectory(sharpa_avatar) endif() +# Gated: Jetson only -- SIPL and the Tegra NVENC entry point are L4T-only. +if(BUILD_PLUGIN_SENSING) + add_subdirectory(sensing) +endif() diff --git a/src/plugins/sensing/AGENTS.md b/src/plugins/sensing/AGENTS.md index a8abfd9bc2..5e5277c0d2 100644 --- a/src/plugins/sensing/AGENTS.md +++ b/src/plugins/sensing/AGENTS.md @@ -21,3 +21,12 @@ Container preview must carry the desktop display and authorization into the imag Support SSH launch with a local DISPLAY: when XAUTHORITY is unset, check the current user's GDM authority file as well as ~/.Xauthority. + +A frame lease keeps its capture backend alive, so a held frame is an ownership +cycle. Every cleanup path, including a failed start, +must join the worker threads and drop the held frames, not only normal stop. +A camera session that will not stop ends the process; do not retry in-process. +Only a camera's owner thread calls its buffer stream; a frame lease posts its +buffer to that owner's mailbox and makes no SDK call. +Check an SDK call's return type before testing it for success: Argus +stopRepeat returns a capture-id range, not a Status. diff --git a/src/plugins/sensing/CAMERA_SETUP.md b/src/plugins/sensing/CAMERA_SETUP.md new file mode 100644 index 0000000000..0707af7125 --- /dev/null +++ b/src/plugins/sensing/CAMERA_SETUP.md @@ -0,0 +1,251 @@ + + +# SENSING camera setup guide + +This guide takes a Jetson AGX Orin and SENSING GMSL cameras from a pile of parts to cameras that stream. It covers the +physical setup, where most problems come from (which cable goes into which connector, and how the carrier board is +powered), and the operating system setup the cameras need. It stops once the cameras are confirmed streaming. + +For the plugin's source layout see [`README.md`](README.md). For the published reference documentation see the +[SENSING user guide](https://nvidia.github.io/IsaacCapture/main/device/camera/sensing.html). + +## Two kinds of camera + +Jetson reads cameras through one of two software stacks, and which one you need depends on how the camera is wired to +the board, not on its brand. + +- **SIPL cameras (GMSL)** connect through a deserializer on a carrier board, such as the SENSING SG8A, over a coax + cable. NVIDIA's Sensor Input Processing Library (SIPL) configures the deserializer and cameras from a platform file. + The SENSING SHW5G on the SG8A is this kind. **This guide covers SIPL cameras.** +- **Argus cameras (CSI)** are exposed by the Jetson camera driver as devices, typically MIPI CSI sensors described in + the device tree, and are reached through `libargus` and `nvargus-daemon`. You pick one by index. Nothing in this guide + applies to them. + +| | SIPL | Argus | +| --- | --- | --- | +| Typical cameras | GMSL cameras on a carrier such as the SG8A | CSI cameras exposed as V4L2 or Argus devices | +| Configuration | Platform JSON, a named config, per-link masks | Device tree and sensor drivers | +| Camera selection | Sensor number, which follows the link index | Camera index | +| Rule of thumb | A deserializer and coax cable sit between camera and board | The camera appears as `/dev/video*` | + +The same camera model can be reached either way, depending on the board. On the SG8A rig Argus reports no camera +devices (no `/dev/video*`, and "virtual enumerator not found in proc device-tree"), so SIPL is the only option there. +Do not run both stacks against the same cameras. + +## Step 1: Gather the parts + +| Part | What it is | +| --- | --- | +| Jetson AGX Orin Developer Kit | Runs L4T R39.2.1 (JetPack 7.2.1) and has its own power supply, which this guide does not cover. | +| SENSING SG8A board | The GMSL carrier. On this rig it is installed inside the devkit's enclosure at the front edge. Its two quad receptacles and a white 2-pin power connector face out, beside the devkit's three front-panel buttons and LED. | +| Carrier power supply | A 12 V adapter, plus a black cable with a female DC barrel jack on one end and a white 2-pin connector (red and black wires) on the other. | +| Cameras | SHW5G modules. Each has one coax jack on the rear (the vendor lists an Amphenol FAKRA Z connector). | +| Camera cables | Two 2 m black coax cables (labelled "2米", Chinese for 2 m), one per camera, each with a teal FAKRA connector and grey strain relief at the camera end. | +| Breakout harness | One teal quad plug (four coax sockets in a 2x2 block) with two coax cables leading to two single teal FAKRA jacks. | + +This guide assumes the SG8A board is already installed in the devkit. Installing or removing it is covered by +SENSING's hardware manual, not here. + +### What the parts look like + +![The parts laid out: two cameras on a bracket with their labelled 2 m cables, loose teal connectors, jumper wires with a barrel plug, and the devkit at the lower right](images/setup-parts-overview.jpg) + +*The parts laid out. The two cameras sit on a bracket, each with a labelled 2 m cable ending in a teal connector. Loose +teal connectors and a cable with a barrel plug lie nearby, and the devkit is at the lower right.* + +![The breakout harness: two single teal FAKRA jacks and one teal quad plug with four coax sockets](images/setup-harness-connectors.jpg) + +*The breakout harness. The two single teal FAKRA jacks each take one camera cable. The teal quad plug, with its four +coax sockets in a 2x2 block, goes into the carrier.* + +![An SHW5G camera in a mounting bracket, seen from the lens end](images/setup-camera-front.jpg) + +*An SHW5G camera in a mounting bracket, seen from the lens end.* + +![The rear of a camera: one round coax jack recessed in the bracket](images/setup-camera-rear.jpg) + +*The rear of the camera: one round coax jack, recessed in the bracket on this rig. The camera cable's teal connector +mates here.* + +### The connectors, end to end + +Every connector in the camera chain is teal, the FAKRA code for Z. Each connection mates one specific pair: + +| Connection | One side | Other side | +| --- | --- | --- | +| Camera to camera cable | The camera's rear coax jack | The camera cable's teal FAKRA connector with grey strain relief | +| Camera cable to harness | The camera cable's other end | One of the harness's two single teal FAKRA jacks (each a square teal housing with a centre contact; one has a purple tab) | +| Harness to carrier | The harness's teal quad plug (2x2 sockets) | A black latched quad receptacle on the SG8A (four round coax pins in a 2x2 block) | +| Carrier power | The carrier's white 2-pin connector | The black power cable, whose barrel jack takes the 12 V adapter's plug | + +The two receptacles are **CN1** (left, carrying COAX0 to COAX3) and **CN2** (right, carrying COAX4 to COAX7), as +labelled in the vendor documentation. The camera chain uses CN1 only. Leave CN2 empty. + +Notes on the parts: + +- **SHW5G and SHF5G are two models of one camera family.** The vendor lists them together as its "5MP Global" + monocular GMSL2 cameras. Both have the same 2560x1984 sensor, frame rate (up to 60 fps in global-shutter mode, 50 fps + in rolling-shutter mode), GMSL2 interface, FAKRA Z connector and power requirements. They differ in lens: the SHW5G + is the wide-angle model (130° horizontal field of view) and the SHF5G is ultra-wide (200°). This guide and its + configuration use the SHW5G. +- **The carrier needs its own supply.** Power it from a 12 V adapter, and check the current rating with SENSING before + choosing one. +- **The cameras need no separate supply.** The vendor wiki lists them as single-coax cameras powered over the same + cable at 9 to 16 V, drawing less than 200 mA at 12 V each. The carrier provides that power. +- **Nothing on the Orin plugs directly into a camera's round coax jack.** The only connection to the carrier is the + quad plug on the harness. Spare FAKRA cables lying around the rig are not part of this setup. +- **Leave the jumper wires as they are.** Coloured jumper wires may be plugged into the devkit's 40-pin header. Do not + add or remove any without checking SENSING's manual. + +## Step 2: Connect the cameras and power + +**Power everything down first.** The cameras are powered through the coax, so connecting them to a live carrier can +damage the cameras or the carrier's serializers. Shut the Orin down and unplug both the devkit's supply and the +carrier's 12 V supply. + +The finished chain looks like this: + +```text +12 V adapter ── DC barrel jack ── black cable ── white 2-pin connector ── SG8A board (front edge of the devkit) + +SG8A CN1 receptacle (left) ◄── teal quad plug ── harness ─┬─ single teal jack ── 2 m coax cable ── camera + └─ single teal jack ── 2 m coax cable ── camera +``` + +1. **Camera cable to camera.** Push each 2 m cable's teal FAKRA connector onto the rear jack of its camera until it + clicks. Check that it is fully seated. +2. **Camera cable to harness.** Connect the other end of each 2 m cable to one of the harness's two single teal + jacks, until it clicks. For a single camera, connect one cable and leave the other jack open. +3. **Harness to carrier.** Push the harness's teal quad plug into the **left** receptacle (CN1) on the front edge of the + devkit. The plug is latched and keyed, so it fits only one way and clicks when seated. If it will not go in, stop; + do not force it. +4. **Carrier power.** Connect the white 2-pin connector to the carrier, and the black cable's barrel jack to the 12 V + adapter's plug. +5. **Power on.** Plug in the carrier's 12 V supply first, then power on the devkit. This is the order used on this + rig. Green LEDs light on the board when it is powered. + +![The front edge of the devkit: three buttons and an LED, and the SG8A board's two empty quad receptacles, with the white 2-pin power connector at the left edge](images/setup-carrier-front-panel.jpg) + +*The carrier end before connecting. The devkit's front edge has three buttons and an LED. The SG8A board's two quad +receptacles sit beside them, each a latched black housing with four coax pins in a 2x2 block. CN1 is on the left and +CN2 on the right. The white 2-pin power connector, with red and black wires, is at the left edge.* + +![The harness's teal quad plug seated in the left receptacle, with two coax cables leaving it; the right receptacle is empty](images/setup-harness-in-cn1.jpg) + +*The connected harness. The teal quad plug is seated in the left receptacle (CN1), with two coax cables leaving it. The +right receptacle (CN2) stays empty.* + +![A side view of the unit showing the SG8A board with its two quad receptacles, its white 2-pin power connector, and jumper wires on a 40-pin header](images/setup-carrier-inside-devkit.jpg) + +*A side view of the unit with its side open. The SG8A board sits at the front, with its quad receptacles at the bottom +and the white 2-pin power connector beside them. The coloured wires on the right are plugged into a 40-pin header.* + +With this harness in CN1 the two cameras come up as links 2 and 3 (COAX2 and COAX3, which the vendor documents as +CAM2 and CAM3). The cameras need to be on those contacts; the harness is what puts them there. + +| Cameras connected | Link masks | Sensors | +| --- | --- | --- | +| Two | `0x0000 0x1100` | `sensor=0` and `sensor=1` | +| One, on COAX3 | `0x0000 0x1000` | `sensor=1` | + +Sensor numbers follow the link index, not a count from zero. To find out which of the harness's two jacks is sensor 0 +and which is sensor 1, connect one camera at a time and run the Step 4 check; the sensor that streams belongs to that +jack. Either order works, and swapping the cameras only swaps the sensor numbers. + +## Step 3: Set up the operating system + +The Orin must be on L4T R39.2.1 (JetPack 7.2.1); check `/etc/nv_tegra_release`. + +### Flash JetPack (new or reflashed Orin) + +Skip this if the check above already shows R39.2.1. The AGX Orin is flashed over USB-C from an x86_64 Ubuntu host with +[NVIDIA SDK Manager](https://developer.nvidia.com/sdk-manager). It does not boot a generic installer image from a USB +drive, and an amd64 Ubuntu ISO is the wrong architecture. + +1. **Recovery mode.** With the Orin's power adapter connected and the unit off, connect the host to the USB-C port next + to the 40-pin header. Hold the middle (Force Recovery) button, press and release Power, then release Force Recovery + after about 2 seconds. `lsusb` on the host should list `NVIDIA Corp. APX`. +2. **Target.** In SDK Manager choose **Jetson AGX Orin** (the 64 GB developer kit), JetPack **7.2.1** and **Direct + Flash**. Leave DeepStream and Holoscan unticked. SDK Manager opens on Jetson AGX Thor by default, so check the target + after every launch. If it asks for the exact model, choose the developer kit version that matches the module's memory. +3. **Flash.** In the flash dialog choose **Pre-Config**, set a username and password, pick eMMC or NVMe (if an SSD is + fitted) and click **Flash**. Keep the host awake and the cable connected until the Orin reboots. An interrupted + flash leaves a partial image; put the Orin back in recovery mode and run it again. +4. **SDK components (optional).** SDK Manager then offers to install CUDA and the other components over USB + (`192.168.55.1`) or Ethernet. Skip it, or install them later with `sudo apt install nvidia-jetpack`. + +If the host driver step stalls on a "Pending kernel upgrade" dialog, press Enter in SDK Manager's terminal tab. If that +tab ignores input, `sudo pkill -f 'needrestart -m u'` from another terminal releases it. + +Once the Orin boots, log in as the user you set (`ssh @192.168.55.1` over the USB-C cable), then run the host +setup below. + +### Packages to install + +On the freshly flashed Orin, install these before the host setup: + +```bash +sudo apt update +sudo apt install -y git nvidia-jetpack +``` + +| Package | Why | +| --- | --- | +| `git` | `setup.sh` fetches the vendor driver package from its public repository with a sparse clone. | +| `nvidia-jetpack` | The CUDA, TensorRT and other JetPack components that SDK Manager skips when you decline its post-flash install. | + +`sudo` is also required: `setup.sh` installs files under `/usr` and `/boot` and adds your user to the `i2c` and `gpio` +groups. Everything else the cameras need (the SIPL libraries, camera HAL, ISP tuning files, the device-tree overlay and +`nvsipl_camera`) comes from the vendor package that `setup.sh` installs, not from apt. + +For the container route, the host also needs Docker with the NVIDIA container runtime (`docker.io` and +`nvidia-container-toolkit`, normally already present with `nvidia-jetpack`). The container image installs its own +packages (`libegl1 libgles2 libglib2.0-0 libxext6 libxcb-glx0`), so install nothing extra for it on the host. + +### Host setup + +Run the host setup once, choose the Jetson-IO overlay **Jetson Sensing SG8A-AGON-G2Y-A1 SIPL GMSL2x8** when it asks, and reboot: + +```bash +src/plugins/sensing/scripts/setup.sh +``` + +The script installs the vendor drivers, camera tuning files and device-tree overlay for the carrier. Run it with +`--help` to see its options. After the reboot the carrier is enabled and the cameras can be opened. + +## Step 4: Check that the cameras stream + +Use NVIDIA's own tool, which needs nothing beyond the OS setup and no `sudo`: + +```bash +nvsipl_camera -t src/plugins/sensing/configs/shw5g.json -c SHW5G_2 -m "0x0000 0x1100" \ + --enable-camera-hal -s -Z -r 5 +``` + +Use your own link masks from Step 2. A working camera reports about 60 fps and `Frame drops: 0`. Both cameras of a +two-camera rig stream together at that rate. + +If initialization fails with `opcode = 0x9 / Parse failed after 5 successful operations / status: 10`, an enabled link +has no camera behind it. Power down, reseat the quad plug and the cable ends, and try again. On this rig the fix was a +restart, reseating the board and trying a different socket. If you are unsure which link a camera is on, run the +command once per mask and keep the one that reports frame rates. + +## Troubleshooting + +| Symptom | What to check | +| --- | --- | +| `opcode = 0x9 / ... status: 10` at startup | An enabled link has no camera. Power down, reseat the quad plug and cable ends, try again (Step 4). | +| No camera on the expected sensor | The quad plug is in CN2 instead of CN1, or a cable end is not fully seated. | +| A connector will not seat | Connectors are keyed. Do not force it; check the orientation and that the connector type matches. | +| Carrier appears dead | Check the 12 V supply, the barrel jack and the white 2-pin connector. No green LEDs means no power. | +| Sensor numbers do not match what you expect | Sensor numbers follow the link index, not a count from zero. | +| Cameras are not found after a hardware change | Power down fully, unplug both supplies, reseat, and power up again. | +| `/dev/video*` is empty | Expected on this carrier. The cameras are reached through SIPL, not Argus. | + +## References + +- SHW5G / SHF5G monocular camera, vendor wiki: + diff --git a/src/plugins/sensing/CMakeLists.txt b/src/plugins/sensing/CMakeLists.txt new file mode 100644 index 0000000000..c4621ce56c --- /dev/null +++ b/src/plugins/sensing/CMakeLists.txt @@ -0,0 +1,204 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# SENSING Camera Plugin: Jetson SIPL capture, hardware H.264 encode, direct streaming to the PC video stage +# ============================================================================== +# Two leaf targets, one per directory (cmake/cmake-structure.md rule 1): +# core/ sensing_core -- SIPL capture, encoder, conversion, track output +# app/ camera_plugin_sensing +# +# Prerequisites: the Jetson Multimedia API and SIPL header trees (see the dependency checks below). +# +# Configure with: +# cmake -B build -DBUILD_PLUGIN_SENSING=ON +# ============================================================================== + +enable_language(CUDA) + +if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + # Orin is 87, Thor is 101; the rest cover desktop dev boxes. + set(CMAKE_CUDA_ARCHITECTURES "80;86;87;89;90") +endif() + +find_package(CUDAToolkit REQUIRED) + +set(_tegra_lib_dirs "/usr/lib/aarch64-linux-gnu/nvidia" "/usr/lib/aarch64-linux-gnu/tegra") + +# ============================================================================== +# SIPL +# ============================================================================== +# Off for an Argus-only rig, which needs neither the SIPL headers nor its libraries. +option(SENSING_WITH_SIPL "Build the SIPL capture backend" ON) +if(SENSING_WITH_SIPL) + # Headers ship in Jetson_SIPL_API_R_aarch64.tbz2, published alongside the + # L4T release. Not in any apt pool, so setup_container.sh fetches it by URL. + if(NOT SIPL_API_DIR) + find_path(SIPL_API_DIR sipl/include/NvSIPLCamera.hpp + PATHS "/usr/src/jetson_sipl_api" + "/opt/nvidia/jetson_sipl_api" + "$ENV{SIPL_API_DIR}" + ) + endif() + + if(NOT SIPL_API_DIR) + message(FATAL_ERROR + "================================================================================\n" + "SENSING camera plugin requires the Jetson SIPL API headers.\n" + "\n" + "Run the container setup, which fetches them:\n" + " src/plugins/sensing/scripts/setup.sh\n" + "\n" + "Or point CMake at an existing copy:\n" + " cmake -B build -DSIPL_API_DIR=/path/to/jetson_sipl_api\n" + "\n" + "Or disable this plugin:\n" + " cmake -B build -DBUILD_PLUGIN_SENSING=OFF\n" + "================================================================================\n" + ) + endif() + + set(SIPL_INCLUDE_DIRS + "${SIPL_API_DIR}/sipl/include" + "${SIPL_API_DIR}/sipl/include/query/include" + "${SIPL_API_DIR}/sipl/include/nvsci" + ) + message(STATUS "SENSING plugin: SIPL=${SIPL_API_DIR}") + + # L4T ships no .so dev symlinks for these, hence the explicit sonames. + foreach(_lib nvsipl nvsipl_query nvscibuf nvscisync nvbufsurface_nvsci) + string(TOUPPER "${_lib}" _upper) + find_library(${_upper}_LIBRARY + NAMES ${_lib} lib${_lib}.so.1 lib${_lib}.so.1.0.0 + PATHS ${_tegra_lib_dirs} + ) + if(NOT ${_upper}_LIBRARY) + message(FATAL_ERROR + "lib${_lib} not found. It is supplied by the L4T BSP and, in a container, by the " + "NVIDIA container runtime. Run src/plugins/sensing/verify.sh to see what is missing.") + endif() + list(APPEND SIPL_LIBRARIES ${${_upper}_LIBRARY}) + endforeach() +endif() + +# EGL stays on the capture path: NvBufSurfaceMapEglImage is how a SIPL +# NvSciBufObj becomes a CUeglFrame. Interop path B in the design notes would +# remove this; path A does not. +find_path(EGL_INCLUDE_DIRS EGL/egl.h) +find_library(EGL_LIBRARIES NAMES EGL) +if(NOT EGL_INCLUDE_DIRS OR NOT EGL_LIBRARIES) + message(FATAL_ERROR "SENSING camera plugin requires EGL. Install libegl1-mesa-dev.") +endif() + +# ============================================================================== +# Jetson Multimedia API tree +# ============================================================================== +# Found before the Argus block below, whose header search is relative to it. +if(NOT JETSON_MMAPI_DIR) + find_path(JETSON_MMAPI_DIR include/NvVideoEncoder.h + PATHS "/usr/src/jetson_multimedia_api" + ) +endif() +if(NOT JETSON_MMAPI_DIR) + message(FATAL_ERROR + "================================================================================\n" + "SENSING camera plugin requires the Jetson Multimedia API encoder sources.\n" + "\n" + "setup_container.sh fetches the version matching /etc/nv_tegra_release:\n" + " src/plugins/sensing/scripts/setup.sh\n" + "\n" + "Or point CMake at an existing tree:\n" + " cmake -B build -DJETSON_MMAPI_DIR=/path/to/jetson_multimedia_api\n" + "================================================================================\n" + ) +endif() +message(STATUS "SENSING plugin: Jetson Multimedia API=${JETSON_MMAPI_DIR}") + +# ============================================================================== +# Argus (optional) +# ============================================================================== +# The Argus backend captures NV12 into DMA-BUFs the hardware encoder reads. It needs the Argus headers from the +# Multimedia API and libnvargus; none of this is built or run by default. +option(SENSING_WITH_ARGUS "Build the Argus capture backend (not built or run by the author)" OFF) +# Argus normally runs through nvargus-daemon, and its client library is libnvargus_socketclient. The single-process +# libnvargus does not connect to the daemon, so it is an explicit choice. +option(SENSING_ARGUS_SINGLE_PROCESS "Link single-process libnvargus instead of the nvargus-daemon client" OFF) +if(SENSING_WITH_ARGUS) + if(SENSING_ARGUS_SINGLE_PROCESS) + set(_argus_lib_name nvargus) + else() + set(_argus_lib_name nvargus_socketclient) + endif() + find_path(ARGUS_INCLUDE_DIR Argus/Argus.h HINTS "${JETSON_MMAPI_DIR}/argus/include") + find_library(ARGUS_LIBRARY NAMES ${_argus_lib_name} PATHS ${_tegra_lib_dirs}) + if(NOT ARGUS_INCLUDE_DIR OR NOT ARGUS_LIBRARY) + message(FATAL_ERROR + "SENSING_WITH_ARGUS needs the Argus headers (/argus/include) and lib${_argus_lib_name}.") + endif() + message(STATUS "SENSING plugin: Argus=${ARGUS_INCLUDE_DIR} (${ARGUS_LIBRARY})") +endif() + +# ============================================================================== +# Jetson V4L2 encoder +# ============================================================================== +# Jetson has no libnvidia-encode (the Video Codec SDK is dGPU-only), so H.264 +# comes from the V4L2 M2M engine via NvVideoEncoder in the Multimedia API. +set(_mmapi_classes "${JETSON_MMAPI_DIR}/samples/common/classes") +set(JETSON_MMAPI_SOURCES + "${_mmapi_classes}/NvVideoEncoder.cpp" + "${_mmapi_classes}/NvV4l2Element.cpp" + "${_mmapi_classes}/NvV4l2ElementPlane.cpp" + "${_mmapi_classes}/NvElement.cpp" + "${_mmapi_classes}/NvElementProfiler.cpp" + "${_mmapi_classes}/NvBuffer.cpp" + "${_mmapi_classes}/NvBufSurface.cpp" + "${_mmapi_classes}/NvLogging.cpp" +) + +find_library(NVBUFSURFACE_LIBRARY NAMES nvbufsurface PATHS ${_tegra_lib_dirs}) +if(NOT NVBUFSURFACE_LIBRARY) + message(FATAL_ERROR "libnvbufsurface not found. Install nvidia-l4t-multimedia.") +endif() +# The NvBufSurf allocation wrapper in samples/common/classes/NvBufSurface.cpp also calls the transform API. +find_library(NVBUFSURFTRANSFORM_LIBRARY NAMES nvbufsurftransform PATHS ${_tegra_lib_dirs}) +if(NOT NVBUFSURFTRANSFORM_LIBRARY) + message(FATAL_ERROR "libnvbufsurftransform not found. Install nvidia-l4t-multimedia.") +endif() + +# NVIDIA's patched libv4l2 carries the Tegra M2M encoder ioctls, so search the +# tegra directories before the generic one. L4T ships no .so dev symlink, hence +# the explicit soname in NAMES. +find_library(V4L2_LIBRARY + NAMES v4l2 libv4l2.so.0 + PATHS ${_tegra_lib_dirs} + NO_DEFAULT_PATH +) +if(NOT V4L2_LIBRARY) + find_library(V4L2_LIBRARY NAMES v4l2 libv4l2.so.0) +endif() +if(NOT V4L2_LIBRARY) + message(FATAL_ERROR "libv4l2 not found. Install nvidia-l4t-multimedia (or libv4l-dev).") +endif() +message(STATUS "SENSING plugin: V4L2=${V4L2_LIBRARY}") + +# No libnvjpeg here on purpose: on R39 libnvbufsurface has no libjpeg +# dependency at all (readelf -d lists only EGL, cuda, dl, pthread, rt, c), and +# the tegra lib dir ships no libnvjpeg to link against. + +# ============================================================================== +# Targets +# ============================================================================== +message(STATUS "Building SENSING camera plugin") + +add_subdirectory(core) +add_subdirectory(app) + +if(BUILD_TESTING) + add_subdirectory(tests) +endif() + +install(FILES + "${CMAKE_CURRENT_SOURCE_DIR}/plugin.yaml" + "${CMAKE_CURRENT_SOURCE_DIR}/README.md" + DESTINATION plugins/sensing_camera +) diff --git a/src/plugins/sensing/README.md b/src/plugins/sensing/README.md index b4d0952c1b..cca8d41cf9 100644 --- a/src/plugins/sensing/README.md +++ b/src/plugins/sensing/README.md @@ -38,3 +38,88 @@ Keep the file byte-identical to the vendor copy. Compare new vendor drops before replacing it; do not hand-edit the board wiring. Its CSI ports, I2C buses, deserializer addresses, GPIOs, and link modes must match the installed SIPL drivers. + +## Orin: build and run the camera producer + +The application is **`camera_plugin_sensing`**. On the Orin it does one job: capture the GMSL cameras with SIPL, +encode H.264 on the hardware encoder, and send each camera's video over TCP to the PC video stage +(`src/video_stage`), which publishes it to LiveKit. The producer has no LiveKit client of its own. Everything else +(CaptureServer, the egocap backend, recording, the browser) runs on the PC. + +Prerequisites: an AGX Orin on JetPack 7.2.1 / L4T R39.2.1 with the SENSING drivers installed and the cameras +working, CUDA, CMake 3.24 or newer (JetPack ships 3.22; `pip install --user cmake` is enough), `uv` and `patchelf` +(the repository's top-level build needs both even with the Python bindings off), and a network path to the PC running +`video_stage_worker`. An Argus-only build (`-DSENSING_WITH_SIPL=OFF -DSENSING_WITH_ARGUS=ON`) also works on JetPack 6.2 / +L4T R36.4.3. + +```bash +# 1. Once: vendor drivers (see scripts/setup.sh --help). The Multimedia API and SIPL header trees are +# separate prerequisites that setup.sh does not fetch; CMake looks for them as described below. +src/plugins/sensing/scripts/setup.sh +# 2. Build. The viewer, examples, tests and Python bindings are not needed (the viewer needs libxkbcommon-dev). +# SIPL_API_DIR and JETSON_MMAPI_DIR point at the unpacked SIPL and Multimedia API trees. +cmake -B build -DBUILD_PLUGIN_SENSING=ON -DBUILD_VIZ=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF \ + -DBUILD_PYTHON_BINDINGS=OFF -DSIPL_API_DIR=... -DJETSON_MMAPI_DIR=... +cmake --build build --target camera_plugin_sensing -j +# 3. Check the cameras are visible. +build/src/plugins/sensing/app/camera_plugin_sensing --list-sensors +# 4. Stream. Each direct= is the port the PC worker listens on for that camera (video_stage_worker --stream=PORT:TRACK). +build/src/plugins/sensing/app/camera_plugin_sensing \ + --add-stream=sensor=0,name=sensing-left,direct=PC_HOST:5600 \ + --add-stream=sensor=1,name=sensing-right,direct=PC_HOST:5601 +``` + +The stream `name` is what the PC and the management channel call a camera; the PC worker's `--stream=PORT:TRACK` +decides the LiveKit track name. Stop with Ctrl-C. `--help` lists every option (`--fps`, `--bitrate`, `--scale-div`, +`--platform-config`, `--link-masks`). +`--camera-backend=auto|sipl|argus` selects the capture backend once at startup: an explicit choice must match or fail +with its own reason, and `auto` picks the only matching backend (with several it names the override instead of +guessing). SIPL is built by default (`-DSENSING_WITH_SIPL=OFF` for an Argus-only rig). The Argus backend +(`-DSENSING_WITH_ARGUS=ON`, Argus device index with `--add-stream=sensor=N,...`, `--argus-mode=K`) captures NV12 into +DMA-BUFs that the hardware encoder reads without a copy. **The Argus code has never been built or run: there is no +Argus SDK or Argus camera available to the author.** Argus DMA-BUF input cannot be combined with +`--scale-div` above 1. + +## Management channel and camera faults + +A camera that cannot start (no power cable, no GMSL link) does not end the producer. It reports the problem in plain +words, retries every few seconds (`--retry-seconds`), and starts streaming once the cable is fixed. The same process +can offer a WebSocket management channel, off unless `--control-token-file=PATH` names a file holding a shared secret: + +```bash +camera_plugin_sensing ... --control-token-file=/etc/isaaccapture/control.token # listens on 0.0.0.0:5610 +``` + +Clients send one JSON object per message and get one reply; the producer also pushes `state_changed` and `error` +events. `get_status` reports the state (`starting`, `streaming`, `camera_fault`, `error`, `stopping`), the fault with +a code and what to check, each stream's frame counts, and where its direct link goes. `set_direct_target` sends a +stream's encoded video to a new PC address and port from now on (an empty host turns the direct link off); the choice +is remembered across camera restarts but not across a producer restart, where the command line applies. Every bad +request gets an error reply with a code and a sentence. The token goes in `Authorization: Bearer ...` (or `?token=`). + +```bash +src/sensing_control/tools/orin_control.py ORIN_HOST --token-file control.token status +src/sensing_control/tools/orin_control.py ORIN_HOST --token-file control.token set-target sensing-left 192.168.1.205 5600 +``` + +`sensing_mock_producer` runs the same channel, retry loop and direct sender over looped H.264 files, with +`--simulate-fault-seconds=N` to show the unplugged-camera path, so the PC side can be tried without an Orin. + +`SENSING_TSC_PROBE=1` logs the SIPL capture counter beside the host clocks twice a second, as separate uncalibrated +samples. The counter's units are unconfirmed, so do not subtract or fit them. + +Status: built and run on an AGX Orin by the author (both sensors, 2560x1984 at 30 fps, H.264; not reproduced in CI). +A failed build or run reports the file or dependency check that failed. + +### Header trees the build needs + +CMake searches for them when the `-D` variables are not given: + +- `JETSON_MMAPI_DIR`: the Jetson Multimedia API tree, containing `include/NvVideoEncoder.h` and + `samples/common/classes/`. Searched in `/usr/src/jetson_multimedia_api`. +- `SIPL_API_DIR`: the Jetson SIPL API tree, containing `sipl/include/NvSIPLCamera.hpp` (the + `Jetson_SIPL_API_R_aarch64.tbz2` package published with the L4T release). Searched in + `/usr/src/jetson_sipl_api`, `/opt/nvidia/jetson_sipl_api` and `$SIPL_API_DIR`. + +The steps to download and unpack both trees are not documented in this repository yet; the user guide is the +place for them. diff --git a/src/plugins/sensing/app/CMakeLists.txt b/src/plugins/sensing/app/CMakeLists.txt new file mode 100644 index 0000000000..c9b91decf4 --- /dev/null +++ b/src/plugins/sensing/app/CMakeLists.txt @@ -0,0 +1,29 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(camera_plugin_sensing main.cpp) + +set_target_properties(camera_plugin_sensing PROPERTIES INSTALL_RPATH "$ORIGIN") + +target_link_libraries(camera_plugin_sensing PRIVATE sensing::core sensing::control) + +# The plugin resolves configs/shw5g.json relative to its own executable, so the +# config has to sit beside the binary in the build tree as well as the install +# tree -- otherwise it only works after `cmake --install`. +add_custom_command(TARGET camera_plugin_sensing POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_directory + "${CMAKE_CURRENT_SOURCE_DIR}/../configs" + "$/configs" + COMMENT "Staging SIPL platform configs beside camera_plugin_sensing" +) + +# Everything below is the `producer` install component: `cmake --install --component producer` stages +# exactly what scripts/deploy_orin.py packages for an Orin. +install(TARGETS camera_plugin_sensing + RUNTIME DESTINATION plugins/sensing_camera + COMPONENT producer +) +install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/../configs/" + DESTINATION plugins/sensing_camera/configs + COMPONENT producer +) diff --git a/src/plugins/sensing/app/main.cpp b/src/plugins/sensing/app/main.cpp new file mode 100644 index 0000000000..77599facc6 --- /dev/null +++ b/src/plugins/sensing/app/main.cpp @@ -0,0 +1,682 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "core/camera_backend.hpp" +#include "core/capture_factory.hpp" +#include "core/sensing_camera.hpp" + +#include +#include +#include +#include +#include + +#ifdef SENSING_HAS_SIPL +# include "core/sipl_camera.hpp" +#endif +#ifdef SENSING_HAS_ARGUS +# include "core/argus_capture.hpp" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::sensing; + +// ============================================================================= +// Signal handling +// ============================================================================= + +static std::atomic g_stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + g_stop_requested.store(true, std::memory_order_relaxed); + } +} + +// ============================================================================= +// Argument parsers +// ============================================================================= + +/// "sensor=N,name=NAME[,direct=HOST:PORT]" -> one sensor and the stream that carries it. +static StreamConfig parse_stream_arg(const std::string& arg) +{ + StreamConfig cfg{}; + bool has_sensor = false; + + std::istringstream ss(arg); + std::string token; + while (std::getline(ss, token, ',')) + { + auto eq = token.find('='); + if (eq == std::string::npos) + throw std::runtime_error("Invalid key=value in --add-stream: '" + token + "'"); + + auto key = token.substr(0, eq); + auto val = token.substr(eq + 1); + + if (key == "sensor") + { + cfg.sensor_id = static_cast(std::stoul(val)); + has_sensor = true; + } + else if (key == "name") + { + // What the PC, the management channel and the logs call this stream. + cfg.name = val; + } + else if (key == "direct") + { + // HOST:PORT of this stream's PC video-stage port; the producer connects and reconnects. + const auto colon = val.rfind(':'); + if (colon == std::string::npos || colon == 0 || colon + 1 == val.size()) + throw std::runtime_error("--add-stream direct= must be HOST:PORT"); + cfg.direct_host = val.substr(0, colon); + cfg.direct_port = static_cast(std::stoul(val.substr(colon + 1))); + } + else + { + throw std::runtime_error("Unknown key in --add-stream: '" + key + "'"); + } + } + + if (!has_sensor || cfg.name.empty()) + throw std::runtime_error("--add-stream requires sensor=,name="); + + return cfg; +} + +/// "0x0000 0x1100" -> {0x0000, 0x1100}. Same spelling as nvsipl_camera -m, so a +/// working vendor command line can be copied across verbatim. +static std::vector parse_link_masks(const std::string& arg) +{ + std::vector masks; + std::istringstream ss(arg); + std::string token; + while (ss >> token) + { + masks.push_back(static_cast(std::stoul(token, nullptr, 0))); + } + if (masks.empty()) + throw std::runtime_error("--link-masks needs at least one mask, e.g. --link-masks='0x0000 0x1100'"); + return masks; +} + +/// The vendored platform config, resolved next to the executable so it works +/// from the build tree and the install tree alike, whatever the cwd. Empty when +/// it is not there, which leaves --platform-config required. +static std::string default_platform_config() +{ + char buf[PATH_MAX]; + const ssize_t n = ::readlink("/proc/self/exe", buf, sizeof(buf) - 1); + if (n <= 0) + return {}; + buf[n] = '\0'; + const std::string exe(buf); + const auto slash = exe.rfind('/'); + if (slash == std::string::npos) + return {}; + const std::string candidate = exe.substr(0, slash) + "/configs/shw5g.json"; + return ::access(candidate.c_str(), R_OK) == 0 ? candidate : std::string{}; +} + +// ============================================================================= +// Usage +// ============================================================================= + +static void print_usage(const char* program_name) +{ + std::cout + << "Usage: " << program_name << " --add-stream=... [options]\n" + << "\n" + << "Captures each SENSING sensor (SIPL or Argus), encodes it as H.264 on the hardware encoder and sends\n" + << "it over TCP to the PC video stage, which publishes it onwards.\n" + << "\n" + << "Streams (repeat for each sensor):\n" + << " --add-stream=sensor=N,name=NAME[,direct=HOST:PORT]\n" + << " camera N (SIPL pipeline or Argus device index), called NAME;\n" + << " direct= is where the PC video stage listens for it (it can also be\n" + << " set later over the management channel)\n" + << " --list-sensors print the cameras, then exit\n" + << " --camera-backend=auto|sipl|argus auto picks the one backend that matches (default);\n" + << " (a backend must also be built in)\n" + << " --argus-mode=N Argus sensor mode index (default: 0)\n" + << "\n" + << "Platform:\n" + << " --platform-config=PATH platform JSON (default: configs/shw5g.json beside this\n" + << " binary)\n" + << " --config-name=NAME named config inside it (default: SHW5G_2)\n" + << " --link-masks='M M' one mask per deserializer (default: '0x0000 0x1100')\n" + << " --nito-dir=DIR ISP tuning directory (default: /var/nvidia/nvcam/settings/sipl)\n" + << "\n" + << "Encoding:\n" + << " --fps=N frame rate cap of each stream (default: 30)\n" + << " --scale-div=N reduce every stream by this integer divisor (default: 1)\n" + << " --timestamp-basis=B what each frame's timestamp is: monotonic_us (default) is this host's\n" + << " monotonic clock when the frame was ready to encode; capture_monotonic_us\n" + << " (UNVERIFIED on hardware) is the sensor's capture event on that clock and fails\n" + << " the stream if it cannot be placed. The direct link carries this clock only.\n" + << " The streams file on the PC must declare the same basis.\n" + << " --frame-correlation=C how an encoded unit is matched to its capture: stamp (default) by the stamp the\n" + << " encoder returns, sending a unit that matches none without a capture time; fifo\n" + << " assumes one unit per input in order (approximate; not for recording)\n" + << " --bitrate=N target bitrate of each stream's encoder in bps (default: 8000000)\n" + << " --gop=N IDR period in frames (default: fps*5)\n" + << "\n" + << "Management channel (a WebSocket server; off unless a token file is given):\n" + << " --control-token-file=PATH file holding the shared secret clients must present\n" + << " --control-port=N port to listen on (default: 5610)\n" + << " --control-bind=ADDR IPv4 address to listen on (default: 0.0.0.0)\n" + << " --retry-seconds=N wait between attempts to start the cameras (default: 5)\n" + << "\n" + << "Other:\n" + << " --gpu-id=N CUDA device index (default: 0)\n" + << " --isp0-buffers=N ISP0 buffers per sensor (default: 6)\n" + << " --full-range full-range YUV\n" + << " --swap-uv swap the chroma planes\n" + << "\n" + << "Example:\n" + << " " << program_name << " --add-stream=sensor=0,name=sensing-left,direct=192.168.1.205:5600 \\\n" + << " --add-stream=sensor=1,name=sensing-right,direct=192.168.1.205:5601\n"; +} + +// ============================================================================= +// Producer session and release +// ============================================================================= + +/// The release this binary was installed from (".../releases//..."), or a placeholder when run by hand. +static std::string release_id() +{ + char buffer[PATH_MAX]; + const ssize_t length = ::readlink("/proc/self/exe", buffer, sizeof(buffer) - 1); + if (length <= 0) + return "unknown"; + const std::string path(buffer, static_cast(length)); + const std::string marker = "/releases/"; + const auto at = path.find(marker); + if (at == std::string::npos) + return "unmanaged"; + const auto start = at + marker.size(); + return path.substr(start, path.find('/', start) - start); +} + +/// One capture-and-publish pipeline as the supervisor sees it. +class ProducerSession : public CameraSession +{ +public: + ProducerSession(const SensingConfig& config, + std::unique_ptr capture, + const std::vector& streams) + : m_camera(config, std::move(capture), streams), m_last_stats(std::chrono::steady_clock::now()) + { + std::cout << "------------------------------------------------------------" << std::endl; + std::cout << "Capturing." << std::endl; + } + + void update() override + { + m_camera.update(); + const auto now = std::chrono::steady_clock::now(); + if (now - m_last_stats >= std::chrono::seconds(5)) + { + m_camera.print_stats(); + m_last_stats = now; + } + } + + void flush() override + { + m_camera.flush(); + m_camera.print_stats(); + } + + std::vector streams() const override + { + std::vector out; + for (const auto& figures : m_camera.figures()) + { + StreamStatus entry; + entry.track = figures.track; + entry.sensor_id = figures.sensor_id; + entry.frames = figures.frames; + entry.published = figures.published; + entry.direct_host = figures.direct.host; + entry.direct_port = figures.direct.port; + entry.direct_connected = figures.direct.connected; + entry.direct_error = figures.direct.error; + out.push_back(std::move(entry)); + } + return out; + } + + void set_direct_target(const std::string& track, const std::string& host, uint16_t port) override + { + m_camera.set_direct_target(track, host, port); + } + +private: + SensingCamera m_camera; + std::chrono::steady_clock::time_point m_last_stats; +}; + +// ============================================================================= +// Main +// ============================================================================= + +int main(int argc, char** argv) +try +{ + SensingConfig camera_config; + std::map stream_map; + ControlServerConfig control_config; + std::string control_token_file; + uint32_t retry_seconds = 5; + bool list_sensors = false; + CameraBackend requested_backend = CameraBackend::Auto; + + for (int i = 1; i < argc; ++i) + { + std::string arg = argv[i]; + + if (arg == "--help" || arg == "-h") + { + print_usage(argv[0]); + return 0; + } + else if (arg.find("--add-stream=") == 0) + { + auto cfg = parse_stream_arg(arg.substr(13)); + if (!stream_map.emplace(cfg.sensor_id, cfg).second) + throw std::runtime_error("sensor " + std::to_string(cfg.sensor_id) + " given twice"); + } + else if (arg.find("--platform-config=") == 0) + { + camera_config.platform_config_json = arg.substr(18); + } + else if (arg.find("--config-name=") == 0) + { + camera_config.platform_config_name = arg.substr(14); + } + else if (arg.find("--link-masks=") == 0) + { + camera_config.link_masks = parse_link_masks(arg.substr(13)); + } + else if (arg.find("--nito-dir=") == 0) + { + camera_config.nito_dir = arg.substr(11); + } + else if (arg == "--list-sensors") + { + list_sensors = true; + } + else if (arg.find("--argus-mode=") == 0) + { + camera_config.argus_sensor_mode = static_cast(std::stoul(arg.substr(13))); + } + else if (arg.find("--camera-backend=") == 0) + { + const auto parsed = parse_camera_backend(arg.substr(17)); + if (!parsed) + { + std::cerr << "Error: --camera-backend must be auto, argus or sipl." << std::endl; + return 1; + } + requested_backend = *parsed; + } + else if (arg.find("--control-token-file=") == 0) + { + control_token_file = arg.substr(21); + } + else if (arg.find("--control-port=") == 0) + { + const unsigned long port = std::stoul(arg.substr(15)); + if (port < 1 || port > 65535) + throw std::runtime_error("--control-port must be from 1 to 65535"); + control_config.port = static_cast(port); + } + else if (arg.find("--control-bind=") == 0) + { + control_config.bind_address = arg.substr(15); + } + else if (arg.find("--retry-seconds=") == 0) + { + retry_seconds = static_cast(std::stoul(arg.substr(16))); + } + else if (arg.find("--fps=") == 0) + { + camera_config.encode.fps = static_cast(std::stoul(arg.substr(6))); + } + else if (arg.find("--timestamp-basis=") == 0) + { + const std::string basis = arg.substr(18); + if (basis == "monotonic_us") + camera_config.encode.timestamp_basis = TimestampBasis::FrameReadyMonotonic; + else if (basis == "capture_monotonic_us") + camera_config.encode.timestamp_basis = TimestampBasis::SensorCaptureMonotonic; + else + { + std::cerr + << "Error: --timestamp-basis must be monotonic_us or capture_monotonic_us: the direct link carries the sender's monotonic clock only." + << std::endl; + return 1; + } + } + else if (arg.find("--frame-correlation=") == 0) + { + const std::string how = arg.substr(20); + if (how == "stamp") + camera_config.encode.correlation = FrameCorrelation::Stamp; + else if (how == "fifo") + camera_config.encode.correlation = FrameCorrelation::Fifo; + else + { + std::cerr << "Error: --frame-correlation must be stamp or fifo." << std::endl; + return 1; + } + } + else if (arg.find("--bitrate=") == 0) + { + camera_config.encode.bitrate_bps = static_cast(std::stoul(arg.substr(10))); + } + else if (arg.find("--scale-div=") == 0) + { + camera_config.encode.scale_div = static_cast(std::stoul(arg.substr(12))); + } + else if (arg.find("--gop=") == 0) + { + camera_config.gop = static_cast(std::stoul(arg.substr(6))); + } + else if (arg.find("--gpu-id=") == 0) + { + camera_config.gpu_id = std::stoi(arg.substr(9)); + } + else if (arg.find("--isp0-buffers=") == 0) + { + camera_config.isp0_buffers = static_cast(std::stoul(arg.substr(15))); + } + else if (arg == "--full-range") + { + camera_config.full_range = true; + } + else if (arg == "--swap-uv") + { + camera_config.swap_uv = true; + } + else if (arg.find("--plugin-root-id=") == 0) + { + // plugin-root-id is a default argument, so we don't need to store it + } + else + { + std::cerr << "Unknown option: " << arg << std::endl; + print_usage(argv[0]); + return 1; + } + } + + // This process captures and never renders, but EGL is on its critical path: + // NvBufSurfaceMapEglImage() is how a SIPL NvSciBufObj reaches CUDA, and + // libnvbufsurface resolves its own display via + // eglGetDisplay(EGL_DEFAULT_DISPLAY). GLVND hands out Mesa's EGL rather than + // Tegra's whenever DISPLAY names an X server Tegra EGL cannot drive (Xvfb, + // X11 forwarding), and that choice cannot be made per-call -- DISPLAY has to + // be gone from the environment before the first EGL call, including the probes below. + ::unsetenv("DISPLAY"); + + // Each backend reports whether it can open the configured rig; probing parses configuration or enumerates + // devices and opens no camera, so --list-sensors works on a rig whose cameras are unplugged. + std::vector camera_ids; + for (const auto& entry : stream_map) + camera_ids.push_back(entry.first); + + std::vector sipl_cameras; + std::vector argus_cameras; + BackendProbe sipl_probe; + sipl_probe.backend = CameraBackend::Sipl; + BackendProbe argus_probe; + argus_probe.backend = CameraBackend::Argus; + +#ifdef SENSING_HAS_SIPL + sipl_probe.available = true; + // An explicit --camera-backend names the one backend to use: the other SDK is not initialised just to be probed. + if (requested_backend != CameraBackend::Argus) + { + if (camera_config.platform_config_json.empty()) + camera_config.platform_config_json = default_platform_config(); + if (camera_config.platform_config_json.empty()) + { + sipl_probe.detail = + "no platform config: configs/shw5g.json was not found beside this binary; pass " + "--platform-config=PATH"; + } + else + { + // The query parses the configuration; it does not prove that the cameras are present. + try + { + for (const auto& s : SiplCamera::query(camera_config.platform_config_json, + camera_config.platform_config_name, camera_config.link_masks)) + sipl_cameras.push_back({ s.id, s.name, s.width, s.height, s.fps }); + sipl_probe.matches = !sipl_cameras.empty(); + if (!sipl_probe.matches) + sipl_probe.detail = "the platform configuration names no sensors"; + } + catch (const std::exception& e) + { + sipl_probe.detail = e.what(); + } + } + } +#else + sipl_probe.detail = "this build has no SIPL support"; +#endif + +#ifdef SENSING_HAS_ARGUS + argus_probe.available = true; + if (requested_backend != CameraBackend::Sipl) + { + try + { + argus_cameras = ArgusCapture::query(camera_config.argus_sensor_mode); + // Every requested camera must exist; with none requested (--list-sensors) any camera counts. + argus_probe.matches = !argus_cameras.empty(); + for (uint32_t id : camera_ids) + { + const bool found = std::any_of( + argus_cameras.begin(), argus_cameras.end(), [id](const CameraInfo& c) { return c.id == id; }); + if (!found) + { + argus_probe.matches = false; + argus_probe.detail = "no Argus camera device " + std::to_string(id); + } + } + if (argus_cameras.empty()) + argus_probe.detail = "Argus reports no camera devices"; + } + catch (const std::exception& e) + { + argus_probe.detail = e.what(); + } + } +#else + argus_probe.detail = "this build has no Argus support (configure with -DSENSING_WITH_ARGUS=ON)"; +#endif + + const CameraBackend backend = select_camera_backend(requested_backend, { sipl_probe, argus_probe }); + const std::vector& cameras = backend == CameraBackend::Argus ? argus_cameras : sipl_cameras; + std::cout << "Camera backend: " << camera_backend_name(backend) << std::endl; + + if (list_sensors) + { + std::cout << cameras.size() << " camera(s)\n"; + for (const auto& c : cameras) + { + std::cout << " sensor=" << c.id << " " << c.name << " " << c.width << "x" << c.height << " @ " << c.fps + << " fps\n"; + } + return 0; + } + + if (stream_map.empty()) + { + std::cerr << "Error: at least one --add-stream is required." << std::endl; + print_usage(argv[0]); + return 1; + } + std::vector stream_configs; + stream_configs.reserve(stream_map.size()); + for (auto& [_, cfg] : stream_map) + { + stream_configs.push_back(std::move(cfg)); + } + + // Check the settings that need no hardware or network before anything is opened. + if (camera_config.encode.fps == 0) + throw std::runtime_error("--fps must be at least 1"); + if (camera_config.encode.bitrate_bps < 100'000) + throw std::runtime_error("--bitrate must be at least 100000"); + { + const bool any_direct = std::any_of( + stream_configs.begin(), stream_configs.end(), [](const StreamConfig& s) { return !s.direct_host.empty(); }); + if (!any_direct && control_token_file.empty()) + std::cerr << "Warning: no stream has a direct=HOST:PORT destination and there is no management channel: " + "nothing will receive the video. Give --add-stream a direct=HOST:PORT or set " + "--control-token-file." + << std::endl; + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + std::cout << "============================================================" << std::endl; + std::cout << "SENSING Camera Plugin Starting (" << camera_backend_name(backend) << ")" << std::endl; + std::cout << "============================================================" << std::endl; + + // A stream naming a sensor that does not exist is a configuration error, not a fault to retry. + for (const auto& stream : stream_configs) + { + const bool known = + std::any_of(cameras.begin(), cameras.end(), [&](const CameraInfo& c) { return c.id == stream.sensor_id; }); + if (!known) + throw std::runtime_error("sensor=" + std::to_string(stream.sensor_id) + " is not a camera of '" + + camera_config.platform_config_name + "'; run --list-sensors"); + } + + // acquire_latest() is non-blocking, so poll at roughly twice the fastest sensor's frame rate rather than + // spinning a core flat out. SIPL captures on its own threads and is unaffected; with Argus the polling loop also + // returns buffers to the camera, so a stalled loop stalls capture. + double fastest_fps = 30.0; + for (const auto& c : cameras) + fastest_fps = std::max(fastest_fps, c.fps); + + ProducerStatus status(release_id()); + std::vector configured; + for (const auto& stream : stream_configs) + { + StreamStatus entry; + entry.track = stream.name; + entry.sensor_id = stream.sensor_id; + entry.direct_host = stream.direct_host; + entry.direct_port = stream.direct_port; + configured.push_back(entry); + } + + // The cameras and the room are opened by the factory, again after any failure, so an unplugged camera is a state + // to report and retry, not the end of the process. + const SessionFactory factory = [&](const DirectTargets& targets) -> std::unique_ptr + { + std::vector streams = stream_configs; + for (auto& stream : streams) + { + const auto found = targets.find(stream.name); + if (found != targets.end()) + { + stream.direct_host = found->second.host; + stream.direct_port = found->second.port; + } + } + // The camera driver explains a failure only in its own log lines, so they are collected while the cameras + // start and attached to the failure; the fault table reads them (an open circuit, a lock timeout, an I2C NACK). + DriverLogCapture driver_log; + try + { + auto capture = make_capture(backend, camera_config, camera_ids); + auto session = std::make_unique(camera_config, std::move(capture), streams); + driver_log.finish(); + return session; + } + catch (const std::exception& e) + { + const std::string lines = driver_log.finish(); + throw std::runtime_error(lines.empty() ? std::string(e.what()) : + std::string(e.what()) + " | driver log: " + lines); + } + }; + + // Destruction runs in reverse declaration order, so the server (declared last) is stopped and joined while the + // handler, supervisor and status its thread calls into still exist, on return and on exception alike. + // `channel` is what the supervisor publishes through; it is set once the server exists. + ControlServer* channel = nullptr; + const auto publish = [&channel](const std::string& text) + { + if (channel != nullptr) + channel->broadcast(text); + }; + SupervisorConfig supervisor_config; + supervisor_config.retry_interval = std::chrono::seconds(std::max(retry_seconds, 1)); + supervisor_config.poll_interval = std::chrono::milliseconds(static_cast(500.0 / fastest_fps)); + ProducerSupervisor supervisor(status, configured, factory, publish, supervisor_config); + ControlHandler handler(status, supervisor); + std::unique_ptr server; + + if (control_token_file.empty()) + { + std::cout << "Management channel: off (give --control-token-file to turn it on)" << std::endl; + } + else + { + control_config.token = read_secret_file(control_token_file); + server = std::make_unique( + control_config, [&handler](const std::string& text) { return handler.handle(text); }); + channel = server.get(); + std::cout << "Management channel: listening on " << control_config.bind_address << ":" << server->port() + << std::endl; + } + + std::cout << "------------------------------------------------------------" << std::endl; + std::cout << "Running. Press Ctrl+C to stop." << std::endl; + + supervisor.run([] { return g_stop_requested.load(std::memory_order_relaxed); }); + channel = nullptr; // nothing publishes once the run has ended + + std::cout << "------------------------------------------------------------" << std::endl; + std::cout << "Plugin stopped" << std::endl; + std::cout << "============================================================" << std::endl; + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error occurred" << std::endl; + return 1; +} diff --git a/src/plugins/sensing/core/CMakeLists.txt b/src/plugins/sensing/core/CMakeLists.txt new file mode 100644 index 0000000000..7b6d103884 --- /dev/null +++ b/src/plugins/sensing/core/CMakeLists.txt @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# SIPL capture, colour conversion, the hardware encoder and the output that sends encoded video to the PC. +add_library(sensing_core STATIC + sensing_camera.cpp + camera_backend.cpp + sensor_clock.cpp + capture_factory.cpp + jetson_encoder.cpp + video_output.cpp + rgba_to_nv12.cu + rgba_box_downscale.cu + ${JETSON_MMAPI_SOURCES} +) +add_library(sensing::core ALIAS sensing_core) + +if(SENSING_WITH_SIPL) + target_sources(sensing_core PRIVATE sipl_camera.cpp sipl_capture.cpp yuv_to_rgba.cu) + target_compile_definitions(sensing_core PUBLIC SENSING_HAS_SIPL) +endif() + +# The Argus backend is optional: it needs the Argus headers and libnvargus, which a SIPL-only rig does not have. +if(SENSING_WITH_ARGUS) + target_sources(sensing_core PRIVATE argus_capture.cpp) + target_compile_definitions(sensing_core PUBLIC SENSING_HAS_ARGUS) + target_include_directories(sensing_core PRIVATE ${ARGUS_INCLUDE_DIR}) + target_link_libraries(sensing_core PRIVATE ${ARGUS_LIBRARY}) +endif() + +# RESOLVE_DEVICE_SYMBOLS as well as SEPARABLE_COMPILATION: without it the device-link step only happens +# for consumers in a directory where CUDA is an enabled language. Resolving here makes the archive +# self-contained. +set_target_properties(sensing_core PROPERTIES + CUDA_SEPARABLE_COMPILATION ON + CUDA_RESOLVE_DEVICE_SYMBOLS ON +) + +target_include_directories(sensing_core + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/.. + PRIVATE + # sipl_camera.hpp pimpls the SIPL and NvSci types away, so these stay private: they drag in + # NvMedia and X11-style macros. + ${SIPL_INCLUDE_DIRS} + ${EGL_INCLUDE_DIRS} + ${JETSON_MMAPI_DIR}/include +) + +# The Multimedia API reference sources carry legacy warnings. +target_compile_options(sensing_core PRIVATE + $<$:-Wno-reorder -Wno-uninitialized -Wno-unused-variable + -Wno-unused-parameter -Wno-missing-field-initializers -Wno-sign-compare> +) + +target_link_libraries(sensing_core + PUBLIC + video_stage::sender + PRIVATE + CUDA::cudart + CUDA::cuda_driver + ${SIPL_LIBRARIES} + ${EGL_LIBRARIES} + ${NVBUFSURFACE_LIBRARY} + ${NVBUFSURFTRANSFORM_LIBRARY} + ${V4L2_LIBRARY} +) diff --git a/src/plugins/sensing/core/argus_capture.cpp b/src/plugins/sensing/core/argus_capture.cpp new file mode 100644 index 0000000000..96f93d70b6 --- /dev/null +++ b/src/plugins/sensing/core/argus_capture.cpp @@ -0,0 +1,628 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Argus buffer-stream capture into NvBufSurface DMA-BUFs. The Argus calls follow the Jetson Multimedia API's +// client-owned-buffer pattern. Built and run on an AGX Orin by a reviewer before each camera's buffer stream was given +// a single owner thread (buffer_owner.hpp); that structure and the bounded teardown have not been built or run. + +#include "argus_capture.hpp" + +#include "buffer_owner.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +void check(Argus::Status status, const std::string& what) +{ + if (status != Argus::STATUS_OK) + throw std::runtime_error("Argus: " + what + " failed (status " + std::to_string(static_cast(status)) + ")"); +} + +// A blocking acquire returns as soon as a frame arrives; the wait only bounds how long a camera with no frames +// (an empty port) takes to notice a stop. The value needs validating on the Orin. +constexpr uint64_t kAcquireTimeoutNs = 100'000'000; +// Per camera, how long a stopped session may take to go idle before it is treated as hung. +constexpr uint64_t kIdleTimeoutNs = 3'000'000'000; +// Whole-teardown budget. An SDK call that never returns cannot be cancelled, so past this the process ends: no +// cleanup may run on state the camera hardware could still be writing to. +constexpr std::chrono::seconds kStopDeadline{ 20 }; +// How long the owner may take to stop new captures (a stopRepeat) before that is treated as hung. +constexpr std::chrono::seconds kStopAckTimeout{ 5 }; + +std::shared_ptr logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.sensing.ArgusCapture"); +} + +// The SDK call teardown is in, recorded before the call is made so a call that never returns is still named. +struct ActiveCall +{ + std::atomic what{ nullptr }; + std::atomic camera{ -1 }; +}; +ActiveCall g_active_call; + +[[noreturn]] void fatal(const char* what, int64_t camera_id) +{ + std::fprintf(stderr, "Argus: %s (camera %lld); ending the process\n", what, static_cast(camera_id)); + std::_Exit(EXIT_FAILURE); +} + +// Ends the process if teardown does not finish in time. Scope it around the blocking SDK calls. +class StopWatchdog +{ +public: + explicit StopWatchdog(std::chrono::seconds limit) + : m_thread( + [this, limit] + { + std::unique_lock lock(m_mutex); + if (!m_cv.wait_for(lock, limit, [this] { return m_done; })) + { + const char* what = g_active_call.what.load(); + fatal((std::string("teardown did not finish in ") + (what ? what : "an unknown step")).c_str(), + g_active_call.camera.load()); + } + }) + { + } + StopWatchdog(const StopWatchdog&) = delete; + StopWatchdog& operator=(const StopWatchdog&) = delete; + ~StopWatchdog() + { + { + std::lock_guard lock(m_mutex); + m_done = true; + } + m_cv.notify_all(); + m_thread.join(); + } + +private: + std::mutex m_mutex; + std::condition_variable m_cv; + bool m_done = false; + std::thread m_thread; // last: it starts using the members above +}; + +// Runs one blocking SDK call and names it in the log when it is slow, so a hang points at the exact call. +template +auto timed(const char* what, int64_t camera_id, F&& call) +{ + g_active_call.camera.store(camera_id); + g_active_call.what.store(what); + const auto begin = std::chrono::steady_clock::now(); + struct Guard + { + const char* what; + int64_t camera_id; + std::chrono::steady_clock::time_point begin; + ~Guard() + { + g_active_call.what.store(nullptr); + const auto ms = + std::chrono::duration_cast(std::chrono::steady_clock::now() - begin); + if (ms.count() >= 1000) + logger()->warn("camera {}: {} took {} ms", camera_id, what, ms.count()); + } + } guard{ what, camera_id, begin }; + return call(); +} + +uint64_t clock_ns(clockid_t clock) +{ + timespec ts{}; + clock_gettime(clock, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000ull + static_cast(ts.tv_nsec); +} + +// One NV12 DMA-BUF the camera writes into. Block-linear: only the hardware encoder reads it. +struct CaptureBuffer +{ + uint32_t index = 0; ///< position in the camera's pool; names the buffer in the return mailbox + int fd = -1; + NvBufSurface* surface = nullptr; + bool egl_mapped = false; + Argus::UniqueObj argus; + + ~CaptureBuffer() + { + argus.reset(); // the Argus buffer refers to the EGL image, so it goes first + if (egl_mapped) + NvBufSurfaceUnMapEglImage(surface, 0); + if (surface) + NvBufSurfaceDestroy(surface); + } +}; + +} // namespace + +struct ArgusCapture::Impl : std::enable_shared_from_this +{ + struct Camera + { + CameraInfo info; + Argus::CameraDevice* device = nullptr; + Argus::SensorMode* mode = nullptr; + Argus::UniqueObj session; + Argus::UniqueObj stream; + Argus::IBufferOutputStream* buffers = nullptr; + Argus::UniqueObj request; + std::vector> pool; + /// Numbers frames whose capture has no metadata, so the consumer never mistakes them for one re-read frame. + uint32_t fallback_sequence = 0; + + /// The camera's buffer-stream calls, and the one thread that makes them (see BufferOwner). The owner is + /// declared last so it, and its thread, go before the port and the objects above. + std::unique_ptr port; + std::unique_ptr owner; + }; + + // An initialised EGL display is released by this member whether construction completes or throws: a destructor + // that never runs for a failed constructor cannot be the only place that calls eglTerminate. + struct EglDisplay + { + EGLDisplay handle = EGL_NO_DISPLAY; + EglDisplay() = default; + EglDisplay(const EglDisplay&) = delete; + EglDisplay& operator=(const EglDisplay&) = delete; + ~EglDisplay() + { + terminate(); + } + void terminate() + { + if (handle != EGL_NO_DISPLAY) + eglTerminate(std::exchange(handle, EGL_NO_DISPLAY)); + } + }; + + Argus::UniqueObj provider; + EglDisplay egl; + std::vector> cameras; + std::vector infos; + std::map by_id; + bool started = false; + bool stopped = false; + + Impl(const ArgusConfig& config) + { + // Peak demand per camera: the encoder's pinned buffers, the output's pending frame, its worker's frame and + // one fresh acquire. + if (config.buffer_count < kEncoderInputBuffers + 3) + throw std::runtime_error("Argus: buffer_count must be at least " + std::to_string(kEncoderInputBuffers + 3) + + " so the camera never waits for a buffer the encoder holds"); + provider.reset(Argus::CameraProvider::create()); + auto* provider_iface = Argus::interface_cast(provider); + if (!provider_iface) + throw std::runtime_error("Argus: no camera provider (is nvargus-daemon running?)"); + + egl.handle = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (egl.handle == EGL_NO_DISPLAY || !eglInitialize(egl.handle, nullptr, nullptr)) + throw std::runtime_error("Argus: EGL display initialisation failed"); + + std::vector devices; + check(provider_iface->getCameraDevices(&devices), "getCameraDevices"); + for (uint32_t index : config.devices) + { + if (index >= devices.size()) + throw std::runtime_error("Argus: no camera device " + std::to_string(index) + " (found " + + std::to_string(devices.size()) + ")"); + auto camera = std::make_unique(); + camera->device = devices[index]; + camera->mode = sensor_mode(camera->device, config.sensor_mode); + auto* mode_iface = Argus::interface_cast(camera->mode); + if (!mode_iface) + throw std::runtime_error("Argus: sensor mode has no mode interface"); + const auto size = mode_iface->getResolution(); + const uint64_t period_ns = mode_iface->getFrameDurationRange().min(); + if (period_ns == 0) + throw std::runtime_error("Argus: sensor mode has a zero frame duration"); + camera->info = { index, "argus" + std::to_string(index), size.width(), + size.height(), 1e9 / static_cast(period_ns), PixelKind::DmaBufNv12 }; + open(*camera, provider_iface, config.buffer_count); + camera->port = std::make_unique(*this, *camera); + camera->owner = std::make_unique( + *camera->port, config.buffer_count, std::chrono::nanoseconds(kAcquireTimeoutNs)); + infos.push_back(camera->info); + by_id[index] = camera.get(); + cameras.push_back(std::move(camera)); + } + } + + ~Impl() + { + stop(); + // Closing the owners, destroying the Argus objects and the EGL display is SDK work too and can hang: one + // deadline covers all of it. + const StopWatchdog watchdog(kStopDeadline); + g_active_call.camera.store(-1); + g_active_call.what.store("returning the last buffers"); + // Every lease has been dropped by now (each keeps this object alive), so each owner returns its last buffers + // while the streams still exist, then ends. + for (auto& camera : cameras) + { + camera->owner->close(); + // The owner waited for the session to go idle once every buffer was back. The hardware may still own the + // session's buffers if it did not, and a retry in this process would pile new sessions on top of it. This + // executable is dedicated to the cameras, so that ends the process; starting it again is the supervisor's + // job (the deployed unit restarts it on failure). + if (!camera->owner->closed_cleanly()) + fatal("a session did not go idle after stopRepeat", camera->info.id); + } + g_active_call.what.store("destroying the camera objects"); + for (auto& camera : cameras) + { + camera->request.reset(); + camera->pool.clear(); // Argus buffers, then the DMA-BUFs behind them + camera->stream.reset(); + camera->session.reset(); + } + cameras.clear(); + provider.reset(); + g_active_call.what.store("terminating the EGL display"); + egl.terminate(); // after the Argus objects above; the member's destructor then has nothing left to do + } + + static Argus::SensorMode* sensor_mode(Argus::CameraDevice* device, uint32_t index) + { + auto* properties = Argus::interface_cast(device); + std::vector modes; + if (!properties || properties->getAllSensorModes(&modes) != Argus::STATUS_OK || index >= modes.size()) + throw std::runtime_error("Argus: sensor mode " + std::to_string(index) + " is not available"); + return modes[index]; + } + + // Session, buffer stream, DMA-BUF pool and repeating request for one camera. + void open(Camera& camera, Argus::ICameraProvider* provider_iface, uint32_t buffer_count) + { + Argus::Status status = Argus::STATUS_OK; + camera.session.reset(provider_iface->createCaptureSession(camera.device, &status)); + auto* session = Argus::interface_cast(camera.session); + if (!session) + throw std::runtime_error("Argus: createCaptureSession failed (status " + + std::to_string(static_cast(status)) + ")"); + + Argus::UniqueObj settings( + session->createOutputStreamSettings(Argus::STREAM_TYPE_BUFFER)); + auto* settings_iface = Argus::interface_cast(settings); + auto* buffer_settings_iface = Argus::interface_cast(settings); + if (!settings_iface || !buffer_settings_iface) + throw std::runtime_error("Argus: createOutputStreamSettings failed"); + settings_iface->setCameraDevice(camera.device); + check(buffer_settings_iface->setBufferType(Argus::BUFFER_TYPE_EGL_IMAGE), "setBufferType"); + buffer_settings_iface->setMetadataEnable(true); + + camera.stream.reset(session->createOutputStream(settings.get())); + camera.buffers = Argus::interface_cast(camera.stream); + if (!camera.buffers) + throw std::runtime_error("Argus: createOutputStream failed"); + + Argus::UniqueObj egl_settings(camera.buffers->createBufferSettings()); + auto* egl_iface = Argus::interface_cast(egl_settings); + if (!egl_iface) + throw std::runtime_error("Argus: createBufferSettings failed"); + check(egl_iface->setEGLDisplay(egl.handle), "setEGLDisplay"); + + for (uint32_t i = 0; i < buffer_count; ++i) + camera.pool.push_back(make_buffer(camera, egl_settings.get(), egl_iface)); + + camera.request.reset(session->createRequest(Argus::CAPTURE_INTENT_PREVIEW)); + auto* request = Argus::interface_cast(camera.request); + if (!request) + throw std::runtime_error("Argus: createRequest failed"); + check(request->enableOutputStream(camera.stream.get()), "enableOutputStream"); + auto* source = Argus::interface_cast(request->getSourceSettings()); + if (!source) + throw std::runtime_error("Argus: no source settings"); + check(source->setSensorMode(camera.mode), "setSensorMode"); + // Hold the sensor mode's own frame rate: auto-exposure may otherwise stretch the frame period in dim light. + const uint64_t period_ns = Argus::interface_cast(camera.mode)->getFrameDurationRange().min(); + check(source->setFrameDurationRange(Argus::Range(period_ns, period_ns)), "setFrameDurationRange"); + } + + // One DMA-BUF handed to Argus as a capture buffer; it starts out released, ready to be written. + std::unique_ptr make_buffer(Camera& camera, + Argus::BufferSettings* egl_settings, + Argus::IEGLImageBufferSettings* egl_iface) + { + auto buffer = std::make_unique(); + buffer->index = static_cast(camera.pool.size()); + NvBufSurf::NvCommonAllocateParams params; + std::memset(¶ms, 0, sizeof(params)); + params.memtag = NvBufSurfaceTag_CAMERA; + params.width = camera.info.width; + params.height = camera.info.height; + params.colorFormat = NVBUF_COLOR_FORMAT_NV12; + params.layout = NVBUF_LAYOUT_BLOCK_LINEAR; + params.memType = NVBUF_MEM_SURFACE_ARRAY; + if (NvBufSurf::NvAllocate(¶ms, 1, &buffer->fd) != 0 || + NvBufSurfaceFromFd(buffer->fd, reinterpret_cast(&buffer->surface)) != 0 || !buffer->surface) + throw std::runtime_error("Argus: allocating a capture DMA-BUF failed"); + if (NvBufSurfaceMapEglImage(buffer->surface, 0) != 0) + throw std::runtime_error("Argus: mapping a capture DMA-BUF to an EGL image failed"); + buffer->egl_mapped = true; + check(egl_iface->setEGLImage(buffer->surface->surfaceList[0].mappedAddr.eglImage), "setEGLImage"); + + buffer->argus.reset(camera.buffers->createBuffer(egl_settings)); + auto* iface = Argus::interface_cast(buffer->argus); + if (!iface) + throw std::runtime_error("Argus: createBuffer failed"); + iface->setClientData(buffer.get()); + check(camera.buffers->releaseBuffer(buffer->argus.get()), "releaseBuffer (initial)"); + return buffer; + } + + // Tied to the lifetime the issued frame leases keep alive; called once the shared_ptr exists. + void attach() + { + for (auto& camera : cameras) + camera->owner->set_keepalive(weak_from_this()); + } + + void start() + { + if (started) + throw std::logic_error("ArgusCapture: capture cannot be restarted; create a new instance"); + started = true; + std::vector repeating; + try + { + for (auto& camera : cameras) + { + auto* session = Argus::interface_cast(camera->session); + check(session->repeat(camera->request.get()), "repeat"); + repeating.push_back(camera.get()); + } + // Runtime ownership of each stream passes to its owner thread here; nothing above runs after this. + for (auto& camera : cameras) + camera->owner->start(); + } + catch (...) + { + undo_start(repeating); + throw; + } + } + + // A failed start: end whichever owner threads began, then stop the sessions that were repeating. With the + // owners gone nothing else touches the streams, so the calls are made here. + void undo_start(const std::vector& repeating) + { + stopped = true; + const StopWatchdog watchdog(kStopDeadline); + for (auto& camera : cameras) + timed("ending the owner thread", camera->info.id, + [&] + { + camera->owner->close(); + return 0; + }); + for (Camera* camera : repeating) + if (!stop_session(*camera)) + fatal("a session did not go idle after stopRepeat", camera->info.id); + } + + // Stops new captures on every camera's owner thread: all are asked before any is waited for, so one camera's + // slow stop does not delay the others'. Frames the consumers still hold stay valid and their buffers keep coming + // back; the wait for the sessions to go idle is deferred to the end (see ~Impl), when no consumer holds one. + void stop() + { + if (!started || stopped) + return; + stopped = true; + const StopWatchdog watchdog(kStopDeadline); + for (auto& camera : cameras) + camera->owner->request_stop(); + for (auto& camera : cameras) + { + const bool ok = timed("waiting for the owner to stop capture", camera->info.id, + [&] { return camera->owner->wait_stopped(kStopAckTimeout); }); + if (!ok) + fatal("a session did not stop", camera->info.id); + } + } + + bool stop_session(Camera& camera) + { + auto* session = Argus::interface_cast(camera.session); + timed("stopRepeat", camera.info.id, [&] { return session->stopRepeat(); }); + return timed("waitForIdle", camera.info.id, [&] { return session->waitForIdle(kIdleTimeoutNs); }) == + Argus::STATUS_OK; + } + + // The SDK calls of one camera's buffer stream. BufferOwner makes them all, on its one thread. + class Port : public BufferPort + { + public: + Port(Impl& impl, Camera& camera) : m_impl(impl), m_camera(camera) + { + } + + Acquired acquire(uint64_t timeout_ns) override + { + Acquired out; + Argus::Status status = Argus::STATUS_OK; + Argus::Buffer* buffer = m_camera.buffers->acquireBuffer(timeout_ns, &status); + if (!buffer) + { + if (status == Argus::STATUS_TIMEOUT) + return out; + out.result = Result::Failed; + out.error = "Argus: acquireBuffer failed (status " + std::to_string(static_cast(status)) + ")"; + return out; + } + auto* iface = Argus::interface_cast(buffer); + const auto* data = iface ? static_cast(iface->getClientData()) : nullptr; + if (!data) + { + // Its slot is unknown, so it cannot go through the mailbox: give it straight back. + m_camera.buffers->releaseBuffer(buffer); + out.result = Result::Failed; + out.error = "Argus: a captured buffer carries no client data"; + return out; + } + out.holds_buffer = true; + out.slot = data->index; + try + { + out.frame = m_impl.make_frame(m_camera, buffer, *data); + out.result = Result::Frame; + } + catch (const std::exception& e) + { + out.result = Result::Failed; + out.error = e.what(); + } + return out; + } + + bool release(uint32_t slot) override + { + if (slot >= m_camera.pool.size()) + return false; + return m_camera.buffers->releaseBuffer(m_camera.pool[slot]->argus.get()) == Argus::STATUS_OK; + } + + bool stop_capture() override + { + auto* session = Argus::interface_cast(m_camera.session); + // stopRepeat returns the range of capture ids it queued, not a Status: a nonempty range is not a failure. + timed("stopRepeat", m_camera.info.id, [&] { return session->stopRepeat(); }); + return true; + } + + bool wait_idle() override + { + auto* session = Argus::interface_cast(m_camera.session); + return timed("waitForIdle", m_camera.info.id, [&] { return session->waitForIdle(kIdleTimeoutNs); }) == + Argus::STATUS_OK; + } + + private: + Impl& m_impl; + Camera& m_camera; + }; + + // The frame for a buffer that has just arrived; its stamps are the arrival time. The owner adds the lease. + CaptureFrame make_frame(Camera& camera, Argus::Buffer* buffer, const CaptureBuffer& data) + { + CaptureFrame frame; + frame.camera_id = camera.info.id; + frame.width = camera.info.width; + frame.height = camera.info.height; + frame.monotonic_ns = clock_ns(CLOCK_MONOTONIC); + frame.unix_ns = clock_ns(CLOCK_REALTIME); + + auto* iface = Argus::interface_cast(buffer); + // The sensor's own timestamp, unscaled and in Argus's clock domain; the capture id numbers the frames. + const auto* metadata = Argus::interface_cast(iface->getMetadata()); + if (metadata) + { + frame.sensor_time = metadata->getSensorTimestamp(); + frame.sensor_domain = SensorTimeDomain::MonotonicRawNs; + frame.sequence = metadata->getCaptureId(); + } + else + { + frame.sequence = ++camera.fallback_sequence; + } + frame.pixels = DmaBufNv12Pixels{ data.fd }; + return frame; + } +}; + +ArgusCapture::ArgusCapture(const ArgusConfig& config) : m_impl(std::make_shared(config)) +{ + m_impl->attach(); +} + +ArgusCapture::~ArgusCapture() +{ + // Outstanding leases keep the Impl, and with it the Argus objects, alive until they are released. + m_impl->stop(); +} + +std::vector ArgusCapture::query(uint32_t sensor_mode) +{ + Argus::UniqueObj provider(Argus::CameraProvider::create()); + auto* provider_iface = Argus::interface_cast(provider); + if (!provider_iface) + throw std::runtime_error("Argus: no camera provider (is nvargus-daemon running?)"); + std::vector devices; + check(provider_iface->getCameraDevices(&devices), "getCameraDevices"); + + std::vector infos; + for (uint32_t index = 0; index < devices.size(); ++index) + { + // A device without this mode cannot be opened with it, but must not hide the other cameras. + auto* properties = Argus::interface_cast(devices[index]); + std::vector modes; + if (!properties || properties->getAllSensorModes(&modes) != Argus::STATUS_OK || sensor_mode >= modes.size()) + continue; + const auto* mode = Argus::interface_cast(modes[sensor_mode]); + if (!mode) + continue; + const auto size = mode->getResolution(); + const uint64_t period_ns = mode->getFrameDurationRange().min(); + infos.push_back({ index, "argus" + std::to_string(index), size.width(), size.height(), + period_ns ? 1e9 / static_cast(period_ns) : 0.0, PixelKind::DmaBufNv12 }); + } + return infos; +} + +const std::vector& ArgusCapture::cameras() const +{ + return m_impl->infos; +} + +void ArgusCapture::start() +{ + m_impl->start(); +} + +void ArgusCapture::stop() +{ + m_impl->stop(); +} + +std::optional ArgusCapture::acquire_latest(uint32_t camera_id) +{ + const auto found = m_impl->by_id.find(camera_id); + if (found == m_impl->by_id.end()) + throw std::runtime_error("Argus: no camera " + std::to_string(camera_id)); + // The camera's owner thread keeps the newest frame; take it, if there is one. + return found->second->owner->take_latest(); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/argus_capture.hpp b/src/plugins/sensing/core/argus_capture.hpp new file mode 100644 index 0000000000..fceb722311 --- /dev/null +++ b/src/plugins/sensing/core/argus_capture.hpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// The Argus backend behind the sender's capture interface: each camera streams NV12 into a pool of NvBufSurface +// DMA-BUFs that the Jetson hardware encoder reads directly. Built only with SENSING_WITH_ARGUS, which needs the +// Argus headers and libnvargus; the Argus types stay out of this header. + +#pragma once + +#include "camera_capture.hpp" +#include "jetson_encoder.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct ArgusConfig +{ + /// Argus camera device indices to capture; CameraInfo::id is the device index. + std::vector devices; + /// Index into each device's sensor modes (as listed by query()). + uint32_t sensor_mode = 0; + /// Capture buffers per camera. The encoder pins kEncoderInputBuffers, the output's pending mailbox and its + /// worker's frame hold one each, and a fresh acquire needs one more; fewer and the camera stalls. + uint32_t buffer_count = kEncoderInputBuffers + 4; +}; + +class ArgusCapture : public CameraCapture +{ +public: + /// @throws std::runtime_error naming the failing Argus or EGL step. + explicit ArgusCapture(const ArgusConfig& config); + ~ArgusCapture() override; + + /// Every Argus device with the given sensor mode's size and rate. Opens no camera. + static std::vector query(uint32_t sensor_mode); + + const std::vector& cameras() const override; + void start() override; + void stop() override; + std::optional acquire_latest(uint32_t camera_id) override; + +private: + // Shared with the frame leases it issues, so the Argus objects outlive every outstanding frame. + struct Impl; + std::shared_ptr m_impl; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/buffer_owner.hpp b/src/plugins/sensing/core/buffer_owner.hpp new file mode 100644 index 0000000000..df95d93378 --- /dev/null +++ b/src/plugins/sensing/core/buffer_owner.hpp @@ -0,0 +1,422 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// One thread owns every runtime call on a camera's buffer stream. Frame leases are released from any thread, so they +// only post the buffer's slot to a mailbox; the owner makes the actual release. No vendor SDK type appears here, so +// the ownership rules are testable with a fake port. + +#pragma once + +#include "camera_capture.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// Buffers waiting to go back to the stream. post() is for any thread and makes no outside call; the owner drains. +/// Capacity is the pool size, fixed up front: a slot is posted at most once per lease, so it never reallocates. +class BufferReturnMailbox +{ +public: + explicit BufferReturnMailbox(size_t capacity) : m_capacity(capacity) + { + m_pending.reserve(capacity); + } + + /// False, and counted, if the mailbox is full: that means a slot was returned twice, which is a bug. + bool post(uint32_t slot) + { + { + std::lock_guard lock(m_mutex); + if (m_pending.size() >= m_capacity) + { + ++m_rejected; + return false; + } + m_pending.push_back(slot); + } + m_cv.notify_one(); + return true; + } + + /// Wakes the owner's wait without posting anything. + void wake() + { + { + std::lock_guard lock(m_mutex); + m_woken = true; + } + m_cv.notify_one(); + } + + /// Owner: moves every pending slot into `out`. + void take(std::vector& out) + { + std::lock_guard lock(m_mutex); + out.insert(out.end(), m_pending.begin(), m_pending.end()); + m_pending.clear(); + } + + /// Owner: blocks until a slot is posted, wake() is called, or the time is up. + void wait(std::chrono::nanoseconds timeout) + { + std::unique_lock lock(m_mutex); + m_cv.wait_for(lock, timeout, [this] { return !m_pending.empty() || m_woken; }); + m_woken = false; + } + + uint64_t rejected() const + { + std::lock_guard lock(m_mutex); + return m_rejected; + } + +private: + const size_t m_capacity; + mutable std::mutex m_mutex; + std::condition_variable m_cv; + std::vector m_pending; + bool m_woken = false; + uint64_t m_rejected = 0; +}; + +/// The thin SDK calls the owner makes. All of them run on the owner's thread, never two at once. +class BufferPort +{ +public: + enum class Result + { + Frame, + Timeout, + Failed, + }; + + struct Acquired + { + Result result = Result::Timeout; + /// True when the stream handed over a buffer, whether or not the frame could be built from it: that buffer + /// is then the owner's to return. The frame's lease is left empty; the owner sets it. + bool holds_buffer = false; + uint32_t slot = 0; + CaptureFrame frame; + std::string error; + }; + + virtual ~BufferPort() = default; + /// Waits up to `timeout_ns` for the next frame. + virtual Acquired acquire(uint64_t timeout_ns) = 0; + /// Gives the buffer in `slot` back to the stream. False: the stream refused it. + virtual bool release(uint32_t slot) = 0; + /// Stops new captures from starting (no waiting). False: it could not. + virtual bool stop_capture() = 0; + /// Waits for capture that was stopped to go idle. The owner calls it once every buffer is back, so the wait + /// never depends on a buffer a consumer still holds. False: it did not go idle. + virtual bool wait_idle() = 0; +}; + +/** + * @brief Runs one camera's buffer stream on its own thread and keeps only the newest frame. + * + * The thread blocks on the port's finite acquire, stamps nothing itself, and replaces the single latest frame; a + * replaced frame goes back through the same mailbox as any other. When every buffer is out it waits on the mailbox + * instead of in an acquire that needs a returned buffer. Stopping has two parts: request_stop() stops new captures and + * clears the latest frame but keeps returning buffers; close() ends the thread after draining the mailbox, and only + * then waits for the stream to go idle. Frames handed + * out stay valid across both; the owner must outlive every lease it issued, which `keepalive` can tie to a larger + * object. + */ +class BufferOwner +{ +public: + BufferOwner(BufferPort& port, size_t pool_size, std::chrono::nanoseconds wait = std::chrono::milliseconds(100)) + : m_port(port), m_pool_size(pool_size), m_wait(wait), m_mailbox(pool_size) + { + } + BufferOwner(const BufferOwner&) = delete; + BufferOwner& operator=(const BufferOwner&) = delete; + ~BufferOwner() + { + close(); + } + + /// Objects the issued leases keep alive until they are released; weak, so the owner never owns its owner. + void set_keepalive(std::weak_ptr keepalive) + { + m_keepalive = std::move(keepalive); + } + + /// @throws std::system_error if the thread cannot be created. + void start() + { + if (m_started) + throw std::logic_error("BufferOwner: already started"); + m_thread = std::thread([this] { run(); }); + m_started = true; + } + + /// Asks the owner thread to stop capture; returns at once. + void request_stop() + { + if (!m_started || !m_thread.joinable()) + { + finish_stop(true); // no thread is running, so there is nothing to stop through it + return; + } + Command expected = Command::Run; + m_command.compare_exchange_strong(expected, Command::Stop); + m_mailbox.wake(); + } + + /// True once capture has stopped and gone idle; false if that took longer than `timeout` or the port said no. + bool wait_stopped(std::chrono::nanoseconds timeout) + { + std::unique_lock lock(m_state_mutex); + return m_state_cv.wait_for(lock, timeout, [this] { return m_stop_done; }) && m_stop_ok; + } + + /// Ends the thread after returning every buffer posted so far; it does not stop capture (see request_stop). + /// Idempotent. Not for the owner thread itself. + void close() + { + m_command.store(Command::Exit); + m_mailbox.wake(); + if (m_thread.joinable()) + m_thread.join(); + } + + /// The newest frame not yet taken. @throws std::runtime_error with the reason if the owner failed. + std::optional take_latest() + { + std::lock_guard lock(m_state_mutex); + if (!m_error.empty()) + throw std::runtime_error(m_error); + std::optional frame = std::move(m_latest); + m_latest.reset(); + return frame; + } + + /// After close(): every buffer was returned and, if capture was stopped, the stream went idle with nothing out. + bool closed_cleanly() const + { + std::lock_guard lock(m_state_mutex); + return m_closed_clean; + } + + uint64_t rejected_returns() const + { + return m_mailbox.rejected(); + } + +private: + enum class Command + { + Run, + Stop, + Exit, + }; + + void run() + { + std::vector returned; + returned.reserve(m_pool_size); + bool stopped = false; + for (;;) + { + drain(returned); + const Command command = m_command.load(); + if (command == Command::Exit) + break; + if (command == Command::Stop && !stopped) + { + // Return the held frame's buffer before the blocking idle wait: the owner cannot return anything while + // it is inside that wait. + clear_latest(); + drain(returned); + bool ok = false; + try + { + ok = m_port.stop_capture(); + } + catch (const std::exception& e) + { + fail(e.what()); + } + finish_stop(ok); + stopped = true; + continue; + } + // Nothing to acquire into, or nothing more to acquire: wait for returns or a command instead. + if (stopped || !m_acquiring || m_outstanding >= m_pool_size) + { + m_mailbox.wait(m_wait); + continue; + } + BufferPort::Acquired got; + try + { + got = m_port.acquire(static_cast(m_wait.count())); + } + catch (const std::exception& e) + { + // A port reports failure through its result; a throw is a bug, and it took no buffer we know of. + fail(e.what()); + m_acquiring = false; + continue; + } + if (got.holds_buffer) + ++m_outstanding; + if (got.result == BufferPort::Result::Timeout) + continue; + if (got.result == BufferPort::Result::Failed) + { + if (got.holds_buffer) + give_back(got.slot); + fail(got.error.empty() ? "the buffer stream failed" : got.error); + m_acquiring = false; + continue; + } + got.frame.lease = make_lease(got.slot); + std::optional replaced; + { + std::lock_guard lock(m_state_mutex); + replaced = std::move(m_latest); + m_latest = std::move(got.frame); + } + // `replaced` goes out of scope here, outside the lock: its lease posts the buffer back. + } + // The thread is ending: every posted buffer goes back, then (if capture was stopped) the stream must go idle + // with nothing out. A buffer still out, or a stream that will not go idle, is reported to close()'s caller. + drain(returned); + clear_latest(); + drain(returned); + bool clean = m_outstanding == 0; + if (stopped) + { + bool idle = false; + try + { + idle = m_port.wait_idle(); + } + catch (const std::exception& e) + { + fail(e.what()); + } + if (!idle) + fail("the buffer stream did not go idle"); + clean = clean && idle; + drain(returned); + clean = clean && m_outstanding == 0; + } + std::lock_guard lock(m_state_mutex); + m_closed_clean = clean; + } + + // Returns every posted buffer to the stream. A buffer the stream refuses stays counted as out, and acquiring + // stops: the stream is failing and the pool no longer adds up. + void drain(std::vector& returned) + { + returned.clear(); + m_mailbox.take(returned); + for (uint32_t slot : returned) + if (!give_back(slot)) + m_acquiring = false; + } + + bool give_back(uint32_t slot) + { + bool ok = false; + try + { + ok = m_port.release(slot); + } + catch (const std::exception& e) + { + fail(e.what()); + return false; + } + if (!ok) + { + fail("the buffer stream refused a returned buffer"); + return false; + } + if (m_outstanding > 0) + --m_outstanding; + return true; + } + + void clear_latest() + { + std::optional dropped; + { + std::lock_guard lock(m_state_mutex); + dropped = std::move(m_latest); + m_latest.reset(); + } + } + + FrameLease make_lease(uint32_t slot) + { + // The owner outlives its leases (see the class comment), so `this` is valid when a lease is dropped. A + // rejected post means a buffer came back twice: that breaks the pool count, so it fails the owner. + return FrameLease( + [this, keep = m_keepalive.lock(), slot]() + { + if (!m_mailbox.post(slot)) + fail("a buffer was returned twice"); + }); + } + + void fail(const std::string& why) + { + std::lock_guard lock(m_state_mutex); + if (m_error.empty()) + m_error = why; + } + + void finish_stop(bool ok) + { + { + std::lock_guard lock(m_state_mutex); + m_stop_done = true; + m_stop_ok = ok; + } + m_state_cv.notify_all(); + } + + BufferPort& m_port; + const size_t m_pool_size; + const std::chrono::nanoseconds m_wait; + BufferReturnMailbox m_mailbox; + std::weak_ptr m_keepalive; + std::atomic m_command{ Command::Run }; + bool m_started = false; + std::thread m_thread; + + size_t m_outstanding = 0; ///< owner thread only: buffers acquired and not yet released + bool m_acquiring = true; ///< owner thread only: false once the stream has failed + + mutable std::mutex m_state_mutex; ///< guards the members below + std::condition_variable m_state_cv; + std::optional m_latest; + std::string m_error; + bool m_stop_done = false; + bool m_stop_ok = false; + bool m_closed_clean = true; ///< set by the owner thread as it ends; true if no thread ever ran +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/camera_backend.cpp b/src/plugins/sensing/core/camera_backend.cpp new file mode 100644 index 0000000000..6703d197c6 --- /dev/null +++ b/src/plugins/sensing/core/camera_backend.cpp @@ -0,0 +1,93 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "camera_backend.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +std::optional parse_camera_backend(const std::string& text) +{ + if (text == "auto") + return CameraBackend::Auto; + if (text == "argus") + return CameraBackend::Argus; + if (text == "sipl") + return CameraBackend::Sipl; + return std::nullopt; +} + +const char* camera_backend_name(CameraBackend backend) +{ + switch (backend) + { + case CameraBackend::Auto: + return "auto"; + case CameraBackend::Argus: + return "argus"; + case CameraBackend::Sipl: + return "sipl"; + } + return "unknown"; +} + +namespace +{ + +std::string describe(const BackendProbe& probe) +{ + const std::string reason = probe.detail.empty() ? "no reason given" : probe.detail; + return std::string(camera_backend_name(probe.backend)) + + (probe.available ? " (found no matching cameras: " : " (unavailable: ") + reason + ")"; +} + +} // namespace + +CameraBackend select_camera_backend(CameraBackend requested, const std::vector& probes) +{ + if (requested != CameraBackend::Auto) + { + const auto probe = + std::find_if(probes.begin(), probes.end(), [&](const BackendProbe& p) { return p.backend == requested; }); + if (probe == probes.end()) + throw std::runtime_error(std::string("--camera-backend=") + camera_backend_name(requested) + + ": this build does not include that backend"); + if (!probe->available || !probe->matches) + throw std::runtime_error(std::string("--camera-backend=") + camera_backend_name(requested) + ": " + + describe(*probe)); + return requested; + } + + std::vector matching; + for (const auto& probe : probes) + { + if (probe.available && probe.matches) + matching.push_back(probe.backend); + } + if (matching.size() == 1) + return matching.front(); + + std::string message; + if (matching.empty()) + { + message = "no camera backend matches the configured rig:"; + for (const auto& probe : probes) + message += " " + describe(probe); + message += ". Name the backend with --camera-backend=argus or --camera-backend=sipl"; + } + else + { + message = + "both camera backends match the configured rig; choose one with --camera-backend=argus or " + "--camera-backend=sipl"; + } + throw std::runtime_error(message); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/camera_backend.hpp b/src/plugins/sensing/core/camera_backend.hpp new file mode 100644 index 0000000000..154914fe21 --- /dev/null +++ b/src/plugins/sensing/core/camera_backend.hpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Choosing the camera backend at startup. This is the decision only: probing the hardware is each backend's job, +// and its result arrives here as a BackendProbe. No vendor SDK is involved, so the rules are testable alone. + +#pragma once + +#include "camera_capture.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// What one backend found when asked whether it can open the configured rig. +struct BackendProbe +{ + CameraBackend backend = CameraBackend::Sipl; + /// The backend is built in and its SDK loaded; false means it cannot run here at all. + bool available = false; + /// The backend found the configured cameras. Counts only when `available` is also true. + bool matches = false; + /// Why not, for the message when this backend was wanted: a missing library, no device, a bad configuration. + std::string detail; +}; + +/// "auto", "argus" or "sipl"; nullopt for anything else. +std::optional parse_camera_backend(const std::string& text); + +const char* camera_backend_name(CameraBackend backend); + +/** + * @brief The backend to open. + * + * An explicit request must match or the call throws with that backend's own reason: it never falls back to the + * other one. Auto selects the only matching backend; none matching, or both matching, throws and names the + * explicit override, because guessing which camera to open could take the wrong rig. + * + * @throws std::runtime_error with the reason. + */ +CameraBackend select_camera_backend(CameraBackend requested, const std::vector& probes); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/camera_capture.hpp b/src/plugins/sensing/core/camera_capture.hpp new file mode 100644 index 0000000000..ab396eb95a --- /dev/null +++ b/src/plugins/sensing/core/camera_capture.hpp @@ -0,0 +1,151 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// The capture boundary of the sender: what a camera backend (SIPL, Argus) hands to the encoder. No vendor SDK +// type appears here, so the sender can select a backend at startup without including either SDK. + +#pragma once + +#include "core/sensor_clock.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +enum class CameraBackend +{ + Auto, + Argus, + Sipl, +}; + +/// Which pixel representation a backend's frames use; fixed per backend, so the encoder input can be chosen once. +enum class PixelKind +{ + CudaRgba, + DmaBufNv12, +}; + +/// One camera as a backend reports it. `id` is the backend's own index (a SIPL pipeline index, an Argus device +/// index); the two are unrelated, so configuration names cameras by logical stream, not by id. +struct CameraInfo +{ + uint32_t id = 0; + std::string name; + uint32_t width = 0; + uint32_t height = 0; + double fps = 0.0; + PixelKind pixels = PixelKind::CudaRgba; +}; + +/// RGBA8 in GPU memory, as the SIPL backend converts it. Borrowed: the backend recycles the buffer, so a consumer +/// copies it out before its next acquire_latest() for the same camera, and the frame's lease is empty. +struct CudaRgbaPixels +{ + uintptr_t device_ptr = 0; + size_t pitch_bytes = 0; +}; + +/// NV12 in a DMA-BUF allocated through NvBufSurface, as an Argus buffer stream writes it. Leased: valid until the +/// frame's lease is released. The surface layout (block-linear or pitch-linear) is whatever the allocation chose and +/// is understood only by the Jetson hardware encoder, which takes the fd directly; no CPU or CUDA consumer may +/// assume a layout. The producer has finished writing when the frame is returned. +struct DmaBufNv12Pixels +{ + int fd = -1; +}; + +/// Returns a frame's buffer to its backend when destroyed or reset. Move-only: exactly one holder owns the buffer, +/// and the backend must outlive every lease it issued. The release callback must not throw (it runs from a +/// destructor), and it runs on whichever thread drops the lease, so it must be thread-safe. +class FrameLease +{ +public: + FrameLease() = default; + explicit FrameLease(std::function release) : m_release(std::move(release)) + { + } + FrameLease(FrameLease&& other) noexcept : m_release(std::exchange(other.m_release, nullptr)) + { + } + FrameLease& operator=(FrameLease&& other) noexcept + { + if (this != &other) + { + reset(); + m_release = std::exchange(other.m_release, nullptr); + } + return *this; + } + FrameLease(const FrameLease&) = delete; + FrameLease& operator=(const FrameLease&) = delete; + ~FrameLease() + { + reset(); + } + + void reset() + { + if (m_release) + { + std::exchange(m_release, nullptr)(); + } + } + +private: + std::function m_release; +}; + +/// One captured frame. Pixels that are leased stay valid until `lease` is destroyed or reset, so a consumer that +/// needs them longer (an encoder holding an input buffer) keeps the lease; borrowed pixels (see CudaRgbaPixels) +/// are valid only until the next acquire_latest() for the same camera. +struct CaptureFrame +{ + uint32_t camera_id = 0; + uint64_t sequence = 0; + uint32_t width = 0; + uint32_t height = 0; + /// CLOCK_MONOTONIC and CLOCK_REALTIME read together when the backend had the frame ready: processing time, + /// not the sensor's capture time. + uint64_t monotonic_ns = 0; + uint64_t unix_ns = 0; + /// The backend's own sensor timestamp, unscaled and in its own clock domain; never convert it to another + /// domain here. Zero when the backend reports none. + uint64_t sensor_time = 0; + /// What `sensor_time` counts; the claim is checked at run time by SensorClockMap, not trusted. + SensorTimeDomain sensor_domain = SensorTimeDomain::None; + /// CLOCK_MONOTONIC ns of the sensor's capture event, set by the video output when it stamps capture + /// time; zero otherwise. + uint64_t capture_monotonic_ns = 0; + std::variant pixels; + FrameLease lease; +}; + +/// A backend instance owns its whole rig: SIPL hands out one process-wide object, so there is one instance per +/// backend, never one per camera. +class CameraCapture +{ +public: + virtual ~CameraCapture() = default; + + virtual const std::vector& cameras() const = 0; + virtual void start() = 0; + /// Stops capture. Leased frames already handed out stay valid until their leases are released; borrowed frames + /// (see CudaRgbaPixels) are invalid once capture stops. + virtual void stop() = 0; + /// The newest frame of `camera_id` not yet taken, or nullopt when none has arrived. Never blocks. + virtual std::optional acquire_latest(uint32_t camera_id) = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/capture_factory.cpp b/src/plugins/sensing/core/capture_factory.cpp new file mode 100644 index 0000000000..a74813d9ce --- /dev/null +++ b/src/plugins/sensing/core/capture_factory.cpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "capture_factory.hpp" + +#ifdef SENSING_HAS_SIPL +# include "sipl_capture.hpp" +#endif + +#ifdef SENSING_HAS_ARGUS +# include "argus_capture.hpp" +#endif + +#include + +namespace plugins +{ +namespace sensing +{ + +std::unique_ptr make_capture(CameraBackend backend, + const SensingConfig& config, + const std::vector& camera_ids) +{ +#ifdef SENSING_HAS_SIPL + if (backend == CameraBackend::Sipl) + { + SiplConfig sipl{}; + sipl.platform_config_json = config.platform_config_json; + sipl.platform_config_name = config.platform_config_name; + sipl.link_masks = config.link_masks; + sipl.nito_dir = config.nito_dir; + sipl.gpu_id = config.gpu_id; + sipl.full_range = config.full_range; + sipl.swap_uv = config.swap_uv; + sipl.isp0_buffers = config.isp0_buffers; + return std::make_unique(sipl); + } +#endif +#ifdef SENSING_HAS_ARGUS + if (backend == CameraBackend::Argus) + { + ArgusConfig argus; + argus.devices = camera_ids; + argus.sensor_mode = config.argus_sensor_mode; + return std::make_unique(argus); + } +#endif + (void)camera_ids; + throw std::runtime_error("this build does not include the requested camera backend"); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/capture_factory.hpp b/src/plugins/sensing/core/capture_factory.hpp new file mode 100644 index 0000000000..9ffa1bb2f8 --- /dev/null +++ b/src/plugins/sensing/core/capture_factory.hpp @@ -0,0 +1,25 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "camera_capture.hpp" +#include "sensing_types.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// Opens the chosen backend for the given camera ids (SIPL pipeline indices, or Argus device indices). +/// @throws std::runtime_error if the backend is not built in or cannot open the cameras. +std::unique_ptr make_capture(CameraBackend backend, + const SensingConfig& config, + const std::vector& camera_ids); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/jetson_encoder.cpp b/src/plugins/sensing/core/jetson_encoder.cpp new file mode 100644 index 0000000000..a9f22fe58b --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.cpp @@ -0,0 +1,469 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "jetson_encoder.hpp" + +#include "rgba_to_nv12.cuh" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +constexpr uint32_t kOutputBuffers = kEncoderInputBuffers; +constexpr uint32_t kCaptureBuffers = 6; + +void check(int ret, const char* what) +{ + if (ret < 0) + throw std::runtime_error(std::string("JetsonEncoder: ") + what + " failed"); +} + +void check_cuda(cudaError_t err, const char* what) +{ + if (err != cudaSuccess) + throw std::runtime_error(std::string("JetsonEncoder: ") + what + ": " + cudaGetErrorString(err)); +} + +// Asks the driver to return each input buffer's timestamp on its encoded unit. Must precede plane setup. +bool enable_copy_timestamp(NvVideoEncoder& enc) +{ +#ifdef V4L2_CID_MPEG_VIDEOENC_COPY_TIMESTAMP + struct v4l2_ext_control control; + struct v4l2_ext_controls controls; + std::memset(&control, 0, sizeof(control)); + std::memset(&controls, 0, sizeof(controls)); + control.id = V4L2_CID_MPEG_VIDEOENC_COPY_TIMESTAMP; + control.value = 1; + controls.ctrl_class = V4L2_CTRL_CLASS_MPEG; + controls.count = 1; + controls.controls = &control; + return enc.setExtControls(controls) >= 0; +#else + (void)enc; + return false; +#endif +} + +} // namespace + +struct JetsonEncoder::Impl +{ + EncoderConfig config; + NvVideoEncoder* encoder = nullptr; + + // SENSING_ENCODER_TIMESTAMP_PROBE=1 logs the first units' returned stamps beside the last stamp submitted, to + // show whether the driver copies the OUTPUT stamp to the unit. Diagnostic only: nothing consumes the result. + const bool timestamp_probe = std::getenv("SENSING_ENCODER_TIMESTAMP_PROBE") != nullptr; + std::atomic last_submitted_us{ 0 }; + std::atomic probe_logged{ 0 }; + + // DMA-BUF input: a wrapper per output buffer carrying the fd the encoder reads, and the lease that keeps the + // capture buffer alive until the plane returns that index. Members destroy after ~Impl deleted the encoder. + struct DmaInput : NvBuffer + { + DmaInput() : NvBuffer(0, 0) + { + } + }; + std::vector> dma_inputs; + std::vector dma_leases; + + // NV12 staging in device memory; the V4L2 output plane is host-mapped, so + // each frame costs one device-to-host copy per plane. + uint8_t* d_y = nullptr; + uint8_t* d_uv = nullptr; + size_t d_y_pitch = 0; + size_t d_uv_pitch = 0; + + std::mutex mutex; + std::deque ready; + bool eos_reached = false; + /// Why the capture thread stopped early; guarded by mutex. + std::string error; + bool eos_sent = false; // driving thread only + + ~Impl() + { + if (d_y) + cudaFree(d_y); + if (d_uv) + cudaFree(d_uv); + delete encoder; + } + + // The capture thread stops after a failure; poll() reports it so the sender does not look healthy. + void fail(const char* what) + { + std::lock_guard lock(mutex); + if (error.empty()) + error = what; + } + + // Fills v4l2_buf with a free output buffer (and `buffer` when the plane owns one). Until every output buffer + // has been queued once, index i is free by construction; after that a dequeue is what frees one. False: none + // is free yet, an ordinary dropped frame, or the device failed, which poll() reports. + bool take_input(uint32_t& queued, struct v4l2_buffer& v4l2_buf, NvBuffer** buffer) + { + if (queued < encoder->output_plane.getNumBuffers()) + { + if (buffer) + *buffer = encoder->output_plane.getNthBuffer(queued); + v4l2_buf.index = queued; + ++queued; + return true; + } + if (encoder->output_plane.dqBuffer(v4l2_buf, buffer, nullptr, 0) >= 0) + return true; + // The plane logs every failure but EAGAIN, which is the non-blocking "no input buffer free yet": drop this + // frame rather than block. Any other errno is the device failing. + const int err = errno; + if (err != EAGAIN && err != EINTR) + fail((std::string("dequeueing an input buffer failed: ") + std::strerror(err)).c_str()); + return false; + } + + // Capture-plane dequeue callback; runs on the encoder's own thread. + static bool capture_dq(struct v4l2_buffer* v4l2_buf, NvBuffer* buffer, NvBuffer* /*shared*/, void* arg) + { + auto* self = static_cast(arg); + + if (!v4l2_buf) + { + self->fail("the capture plane returned no buffer"); + return false; + } + + if (buffer && buffer->planes[0].bytesused > 0) + { + const auto* data = static_cast(buffer->planes[0].data); + // V4L2 M2M encoders may copy the OUTPUT buffer's timestamp onto the CAPTURE buffer, but the Orin driver + // observed here returns its own frame-rate numbering instead: treat this stamp as the unit's own. + EncodedUnit unit; + unit.data.assign(data, data + buffer->planes[0].bytesused); + unit.timestamp_ns = static_cast(v4l2_buf->timestamp.tv_sec) * 1000000000LL + + static_cast(v4l2_buf->timestamp.tv_usec) * 1000LL; + unit.keyframe = (v4l2_buf->flags & V4L2_BUF_FLAG_KEYFRAME) != 0; + if (self->timestamp_probe && self->probe_logged.fetch_add(1) < 30) + { + isaaccapture::Logger::get("isaaccapture.plugins.sensing.JetsonEncoder") + ->info("[ts-probe] unit stamp_us={} last_submitted_us={}", unit.timestamp_ns / 1000, + self->last_submitted_us.load()); + } + std::lock_guard lock(self->mutex); + self->ready.push_back(std::move(unit)); + } + + // A zero-length unit marks end of stream; stop the thread rather than + // re-queueing, otherwise the dq thread spins on a finished encoder. + if (buffer && buffer->planes[0].bytesused == 0) + { + std::lock_guard lock(self->mutex); + self->eos_reached = true; + return false; + } + + if (self->encoder->capture_plane.qBuffer(*v4l2_buf, nullptr) < 0) + { + self->fail("requeueing a capture buffer failed"); + return false; + } + + return true; + } +}; + +JetsonEncoder::JetsonEncoder(const EncoderConfig& config) : m_impl(std::make_unique()) +{ + if (config.width == 0 || config.height == 0) + throw std::runtime_error("JetsonEncoder: width/height must be non-zero"); + if ((config.width % 2) != 0 || (config.height % 2) != 0) + throw std::runtime_error("JetsonEncoder: width/height must be even for NV12"); + + m_impl->config = config; + + m_impl->encoder = NvVideoEncoder::createVideoEncoder("enc0"); + if (!m_impl->encoder) + throw std::runtime_error("JetsonEncoder: createVideoEncoder failed (is /dev/v4l2-nvenc present?)"); + + auto* enc = m_impl->encoder; + + // Capture format must be set before the output format. + const uint32_t bitstream_size = config.width * config.height * 3 / 2; + check(enc->setCapturePlaneFormat(V4L2_PIX_FMT_H264, config.width, config.height, bitstream_size), + "setCapturePlaneFormat"); + check(enc->setOutputPlaneFormat(V4L2_PIX_FMT_NV12M, config.width, config.height), "setOutputPlaneFormat"); + + check(enc->setBitrate(config.bitrate_bps), "setBitrate"); + check(enc->setProfile(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH), "setProfile"); + + // Level is not inferred reliably, and 2560x1984@60 is 1,190,400 macroblocks + // per second against Level 5.1's ceiling of 983,040 -- 21% over. An unset + // or too-low level encodes fine here and then gets refused downstream. + check(enc->setLevel(V4L2_MPEG_VIDEO_H264_LEVEL_5_2), "setLevel"); + + if (config.peak_bitrate_bps > config.bitrate_bps) + { + check(enc->setRateControlMode(V4L2_MPEG_VIDEO_BITRATE_MODE_VBR), "setRateControlMode"); + check(enc->setPeakBitrate(config.peak_bitrate_bps), "setPeakBitrate"); + } + else + { + check(enc->setRateControlMode(V4L2_MPEG_VIDEO_BITRATE_MODE_CBR), "setRateControlMode"); + } + check(enc->setFrameRate(config.fps ? config.fps : 30, 1), "setFrameRate"); + + const uint32_t gop = config.gop ? config.gop : (config.fps ? config.fps * 5 : 150); + check(enc->setIDRInterval(gop), "setIDRInterval"); + check(enc->setIFrameInterval(gop), "setIFrameInterval"); + + // Low-latency shape: no B-frames, SPS/PPS on every IDR so a receiver can + // join mid-stream, and the max-performance clock preset. + check(enc->setNumBFrames(0), "setNumBFrames"); + check(enc->setInsertSpsPpsAtIdrEnabled(true), "setInsertSpsPpsAtIdrEnabled"); + // Without a VUI the SPS carries no reorder/buffering limits, so a receiver's decoder (NVDEC) holds back about a + // dozen pictures. Non-fatal: a stream without it is valid, only slower to display. + if (enc->setInsertVuiEnabled(true) < 0) + isaaccapture::Logger::get("isaaccapture.plugins.sensing.JetsonEncoder") + ->warn("setInsertVuiEnabled failed; the SPS will have no VUI"); + check(enc->setMaxPerfMode(1), "setMaxPerfMode"); + // Without this the driver numbers units from its own frame clock, so a unit cannot be tied to its capture by + // stamp. Non-fatal: the producer then reports unmatched units and sends them without a capture time. + if (!enable_copy_timestamp(*enc)) + isaaccapture::Logger::get("isaaccapture.plugins.sensing.JetsonEncoder") + ->warn("the encoder will not copy input timestamps; encoded units cannot be matched to captures"); + // A separate control from the max-performance clocks: it trades quality for encoder throughput. + // FAST suits live streams; four native-size images at 30 fps may need ULTRAFAST, to be measured. + check(enc->setHWPresetType(V4L2_ENC_HW_PRESET_FAST), "setHWPresetType"); + + if (config.input == PixelKind::DmaBufNv12) + { + check(enc->output_plane.setupPlane(V4L2_MEMORY_DMABUF, kOutputBuffers, false, false), "output setupPlane"); + for (uint32_t i = 0; i < kOutputBuffers; ++i) + m_impl->dma_inputs.push_back(std::make_unique()); + m_impl->dma_leases.resize(kOutputBuffers); + } + else + { + check(enc->output_plane.setupPlane(V4L2_MEMORY_MMAP, kOutputBuffers, true, false), "output setupPlane"); + } + check(enc->capture_plane.setupPlane(V4L2_MEMORY_MMAP, kCaptureBuffers, true, false), "capture setupPlane"); + + check(enc->output_plane.setStreamStatus(true), "output setStreamStatus"); + check(enc->capture_plane.setStreamStatus(true), "capture setStreamStatus"); + + enc->capture_plane.setDQThreadCallback(&Impl::capture_dq); + enc->capture_plane.startDQThread(m_impl.get()); + + // Prime the capture plane so the encoder always has somewhere to write. + for (uint32_t i = 0; i < enc->capture_plane.getNumBuffers(); ++i) + { + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.index = i; + v4l2_buf.m.planes = planes; + check(enc->capture_plane.qBuffer(v4l2_buf, nullptr), "capture qBuffer"); + } + + if (config.input == PixelKind::CudaRgba) + { + check_cuda( + cudaMallocPitch(reinterpret_cast(&m_impl->d_y), &m_impl->d_y_pitch, config.width, config.height), + "cudaMallocPitch(Y)"); + check_cuda(cudaMallocPitch( + reinterpret_cast(&m_impl->d_uv), &m_impl->d_uv_pitch, config.width, config.height / 2), + "cudaMallocPitch(UV)"); + } +} + +JetsonEncoder::~JetsonEncoder() +{ + if (m_impl && m_impl->encoder) + { + m_impl->encoder->capture_plane.stopDQThread(); + m_impl->encoder->capture_plane.waitForDQThread(1000); + } +} + +bool JetsonEncoder::submit(uintptr_t rgba_device_ptr, std::size_t row_pitch_bytes, int64_t timestamp_ns) +{ + auto* enc = m_impl->encoder; + const auto& config = m_impl->config; + + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.m.planes = planes; + + if (config.input != PixelKind::CudaRgba) + throw std::logic_error("JetsonEncoder: submit() needs a CUDA RGBA encoder; use submit_dmabuf()"); + NvBuffer* buffer = nullptr; + if (!m_impl->take_input(m_queued, v4l2_buf, &buffer)) + return false; + + // An input buffer is ours now, so convert: a frame that would be dropped costs no conversion. A + // failure from here is terminal for the encoder, as a failed qBuffer is. + launch_rgba_to_nv12(reinterpret_cast(rgba_device_ptr), static_cast(row_pitch_bytes), m_impl->d_y, + static_cast(m_impl->d_y_pitch), m_impl->d_uv, static_cast(m_impl->d_uv_pitch), + static_cast(config.width), static_cast(config.height), config.full_range, nullptr); + check_cuda(cudaGetLastError(), "rgba_to_nv12 launch"); + check_cuda(cudaStreamSynchronize(nullptr), "rgba_to_nv12 sync"); + + check_cuda(cudaMemcpy2D(buffer->planes[0].data, buffer->planes[0].fmt.stride, m_impl->d_y, m_impl->d_y_pitch, + config.width, config.height, cudaMemcpyDeviceToHost), + "cudaMemcpy2D(Y)"); + check_cuda(cudaMemcpy2D(buffer->planes[1].data, buffer->planes[1].fmt.stride, m_impl->d_uv, m_impl->d_uv_pitch, + config.width, config.height / 2, cudaMemcpyDeviceToHost), + "cudaMemcpy2D(UV)"); + + buffer->planes[0].bytesused = buffer->planes[0].fmt.stride * config.height; + buffer->planes[1].bytesused = buffer->planes[1].fmt.stride * (config.height / 2); + v4l2_buf.m.planes[0].bytesused = buffer->planes[0].bytesused; + v4l2_buf.m.planes[1].bytesused = buffer->planes[1].bytesused; + + // Microsecond granularity is all a timeval carries, well below the frame interval. + v4l2_buf.timestamp.tv_sec = static_cast(timestamp_ns / 1000000000LL); + v4l2_buf.timestamp.tv_usec = static_cast((timestamp_ns % 1000000000LL) / 1000LL); + m_impl->last_submitted_us.store(timestamp_ns / 1000); + + check(enc->output_plane.qBuffer(v4l2_buf, nullptr), "output qBuffer"); + return true; +} + +bool JetsonEncoder::submit_dmabuf(int dmabuf_fd, int64_t timestamp_ns, FrameLease lease) +{ + if (m_impl->config.input != PixelKind::DmaBufNv12) + throw std::logic_error("JetsonEncoder: submit_dmabuf() needs an encoder created for DMA-BUF input"); + + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.m.planes = planes; + + // A dropped frame returns here, and `lease` releases the capture buffer as it goes out of scope. + if (!m_impl->take_input(m_queued, v4l2_buf, nullptr)) + return false; + + // The encoder is finished with whatever this index carried before: give that capture buffer back. + const uint32_t index = v4l2_buf.index; + m_impl->dma_leases[index].reset(); + + // Unverified contract: the output plane is configured NV12M (two planes) but only plane 0's fd is set, as the + // repository's Argus example does for the single-fd NvBufSurface Argus fills. Not run here; confirm on the target. + auto& input = *m_impl->dma_inputs[index]; + input.planes[0].fd = dmabuf_fd; + input.planes[0].bytesused = 1; + v4l2_buf.m.planes[0].bytesused = 1; + v4l2_buf.timestamp.tv_sec = static_cast(timestamp_ns / 1000000000LL); + v4l2_buf.timestamp.tv_usec = static_cast((timestamp_ns % 1000000000LL) / 1000LL); + m_impl->last_submitted_us.store(timestamp_ns / 1000); + + check(m_impl->encoder->output_plane.qBuffer(v4l2_buf, &input), "output qBuffer"); + m_impl->dma_leases[index] = std::move(lease); + return true; +} + +EncodedUnit JetsonEncoder::poll() +{ + std::lock_guard lock(m_impl->mutex); + if (m_impl->ready.empty()) + { + if (!m_impl->error.empty()) + throw std::runtime_error("JetsonEncoder: " + m_impl->error); + return {}; + } + + auto out = std::move(m_impl->ready.front()); + m_impl->ready.pop_front(); + return out; +} + +std::vector JetsonEncoder::drain() +{ + auto* enc = m_impl->encoder; + + // A zero-length output buffer is the EOS marker for the V4L2 encoder. It is sent once: a second + // drain only returns what is left. + if (!m_impl->eos_sent) + { + m_impl->eos_sent = true; + struct v4l2_buffer v4l2_buf; + struct v4l2_plane planes[MAX_PLANES]; + std::memset(&v4l2_buf, 0, sizeof(v4l2_buf)); + std::memset(planes, 0, sizeof(planes)); + v4l2_buf.m.planes = planes; + + NvBuffer* buffer = nullptr; + if (enc->output_plane.dqBuffer(v4l2_buf, &buffer, nullptr, 10) >= 0) + { + if (m_impl->config.input == PixelKind::DmaBufNv12) + { + m_impl->dma_leases[v4l2_buf.index].reset(); + v4l2_buf.m.planes[0].m.fd = -1; + } + v4l2_buf.m.planes[0].bytesused = 0; + v4l2_buf.m.planes[1].bytesused = 0; + enc->output_plane.qBuffer(v4l2_buf, nullptr); + } + + enc->capture_plane.waitForDQThread(2000); + } + + std::lock_guard lock(m_impl->mutex); + std::vector units( + std::make_move_iterator(m_impl->ready.begin()), std::make_move_iterator(m_impl->ready.end())); + m_impl->ready.clear(); + return units; +} + +EncodedUnit JetsonEncoder::end_of_stream() +{ + // The tail is concatenated, so it carries the stamp of the last unit in it. + EncodedUnit out; + for (auto& chunk : drain()) + { + out.data.insert(out.data.end(), chunk.data.begin(), chunk.data.end()); + out.timestamp_ns = chunk.timestamp_ns; + } + return out; +} + +void JetsonEncoder::request_keyframe() +{ + check(m_impl->encoder->forceIDR(), "forceIDR"); +} + +bool JetsonEncoder::set_bitrate(uint32_t bitrate_bps) +{ + return m_impl->encoder->setBitrate(bitrate_bps) >= 0; +} + +const char* JetsonEncoder::backend_name() +{ + return "Jetson V4L2 NVENC"; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/jetson_encoder.hpp b/src/plugins/sensing/core/jetson_encoder.hpp new file mode 100644 index 0000000000..834f240084 --- /dev/null +++ b/src/plugins/sensing/core/jetson_encoder.hpp @@ -0,0 +1,127 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "camera_capture.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// Input buffers the encoder holds; with DMA-BUF input each one pins a capture buffer until its index is reused. +constexpr uint32_t kEncoderInputBuffers = 6; + +struct EncoderConfig +{ + uint32_t width = 0; + uint32_t height = 0; + uint32_t bitrate_bps = 40'000'000; + /// VBR ceiling; 0 keeps CBR. The H.264 output is a recording, not a + /// fixed-rate transport, so CBR would pad static scenes and starve motion. + uint32_t peak_bitrate_bps = 60'000'000; + uint32_t fps = 30; + /// IDR period in frames; 0 defers to fps*5. + uint32_t gop = 0; + bool full_range = false; + /// What submit()/submit_dmabuf() will be given; fixed for the encoder's lifetime. + PixelKind input = PixelKind::CudaRgba; +}; + +/// One encoded Annex-B unit. +struct EncodedUnit +{ + std::vector data; + /// The encoder's own frame-rate numbering, not the stamp given to submit(). + int64_t timestamp_ns = 0; + /// The unit is an IDR (with its parameter sets), decodable on its own. Not set on the + /// concatenated tail from end_of_stream(). + bool keyframe = false; + + bool empty() const + { + return data.empty(); + } +}; + +/** + * @brief H.264 encoder on the Jetson V4L2 M2M engine (/dev/v4l2-nvenc). + * + * Jetson has no libnvidia-encode — the NVIDIA Video Codec SDK is dGPU-only — + * so this wraps NvVideoEncoder from the Jetson Multimedia API instead. + * + * One thread must drive an encoder: submit(), poll(), request_keyframe() and set_bitrate() are not + * safe to call concurrently. + * + * Submission is asynchronous: the encoder needs several input frames before it + * emits the first bitstream unit, so submit() never blocks waiting for output + * and poll() drains whatever a capture-plane thread has completed. + */ +class JetsonEncoder +{ +public: + explicit JetsonEncoder(const EncoderConfig& config); + ~JetsonEncoder(); + + JetsonEncoder(const JetsonEncoder&) = delete; + JetsonEncoder& operator=(const JetsonEncoder&) = delete; + JetsonEncoder(JetsonEncoder&&) = delete; + JetsonEncoder& operator=(JetsonEncoder&&) = delete; + + /** + * @brief Convert one GPU-resident RGBA8 frame to NV12 and queue it. + * @param rgba_device_ptr Device pointer to a HxWx4 RGBA8 buffer. + * @param row_pitch_bytes Byte stride between rows. + * @param timestamp_ns Stamp set on the V4L2 output buffer. On the verified Orin driver the unit + * poll() returns does not carry it back (units are numbered from the encoder's own frame rate), so + * callers pair units with submissions by order and must not match on this value. + * @return false when no input buffer is free (frame dropped). + */ + bool submit(uintptr_t rgba_device_ptr, std::size_t row_pitch_bytes, int64_t timestamp_ns); + + /** + * @brief Queue one NV12 DMA-BUF frame for encoding, with no copy. Only for `input == DmaBufNv12`. + * @param dmabuf_fd The NvBufSurface fd the encoder reads. + * @param lease Keeps the capture buffer alive: held until the encoder's input plane returns this buffer, and + * released at once if the frame is dropped. + * @return false when no input buffer is free (frame dropped). + */ + bool submit_dmabuf(int dmabuf_fd, int64_t timestamp_ns, FrameLease lease); + + /** @brief Take any completed unit; empty during encoder warmup. */ + EncodedUnit poll(); + + /** @brief Signal EOS and drain the remaining bitstream, concatenated into one unit. */ + EncodedUnit end_of_stream(); + + /** @brief Signal EOS and drain the remaining bitstream, one unit per frame. */ + std::vector drain(); + + /** @brief Make the next submitted frame an IDR, preceded by SPS/PPS. */ + void request_keyframe(); + + /** + * @brief Change the target bitrate of the running stream. + * @return false when this backend cannot change it while encoding. + */ + bool set_bitrate(uint32_t bitrate_bps); + + /** @brief Names the encoder behind this class, for diagnostics. */ + static const char* backend_name(); + +private: + struct Impl; + std::unique_ptr m_impl; + + /// Output buffers queued so far; below the buffer count they are free by + /// construction and need no dequeue first. + uint32_t m_queued = 0; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_box_downscale.cu b/src/plugins/sensing/core/rgba_box_downscale.cu new file mode 100644 index 0000000000..cc169b3b92 --- /dev/null +++ b/src/plugins/sensing/core/rgba_box_downscale.cu @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rgba_box_downscale.cuh" + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +__global__ void box_downscale( + const uint8_t* src, int src_pitch, uint8_t* dst, int dst_pitch, int width, int height, int divisor) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= width || y >= height) + return; + + int sum[4] = { 0, 0, 0, 0 }; + for (int j = 0; j < divisor; ++j) + { + const uint8_t* row = + src + static_cast(y * divisor + j) * src_pitch + static_cast(x * divisor) * 4; + for (int i = 0; i < divisor; ++i) + { + for (int c = 0; c < 4; ++c) + sum[c] += row[i * 4 + c]; + } + } + + const int count = divisor * divisor; + uint8_t* out = dst + static_cast(y) * dst_pitch + static_cast(x) * 4; + for (int c = 0; c < 4; ++c) + out[c] = static_cast((sum[c] + count / 2) / count); +} + +} // namespace + +void launch_rgba_box_downscale(const uint8_t* src, + int src_pitch, + uint8_t* dst, + int dst_pitch, + int dst_width, + int dst_height, + int divisor, + cudaStream_t stream) +{ + const dim3 block(16, 16); + const dim3 grid((dst_width + block.x - 1) / block.x, (dst_height + block.y - 1) / block.y); + box_downscale<<>>(src, src_pitch, dst, dst_pitch, dst_width, dst_height, divisor); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_box_downscale.cuh b/src/plugins/sensing/core/rgba_box_downscale.cuh new file mode 100644 index 0000000000..4a96e37a80 --- /dev/null +++ b/src/plugins/sensing/core/rgba_box_downscale.cuh @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Reduce RGBA8 by an integer divisor, averaging each divisor x divisor block. + * + * The aspect ratio is kept exactly: the source size must divide evenly, so nothing is stretched, + * cropped or rounded. + * + * @param dst_width src_width / divisor. + * @param dst_height src_height / divisor. + */ +void launch_rgba_box_downscale(const uint8_t* src, + int src_pitch, + uint8_t* dst, + int dst_pitch, + int dst_width, + int dst_height, + int divisor, + cudaStream_t stream); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_to_nv12.cu b/src/plugins/sensing/core/rgba_to_nv12.cu new file mode 100644 index 0000000000..2c0ca5ec6c --- /dev/null +++ b/src/plugins/sensing/core/rgba_to_nv12.cu @@ -0,0 +1,103 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rgba_to_nv12.cuh" + +namespace +{ + +__device__ __forceinline__ uint8_t clamp_u8(float v) +{ + return static_cast(fminf(fmaxf(v, 0.0f), 255.0f) + 0.5f); +} + +// BT.601 RGB->YCbCr, matching the coefficients yuv_to_rgba.cu inverts on the +// capture side so a round trip through RGBA is close to identity. +__device__ __forceinline__ float rgb_to_y(float r, float g, float b, bool full_range) +{ + return full_range ? (0.299f * r + 0.587f * g + 0.114f * b) : (16.0f + 0.257f * r + 0.504f * g + 0.098f * b); +} + +__device__ __forceinline__ float rgb_to_u(float r, float g, float b, bool full_range) +{ + return full_range ? (128.0f - 0.168736f * r - 0.331264f * g + 0.5f * b) : + (128.0f - 0.148f * r - 0.291f * g + 0.439f * b); +} + +__device__ __forceinline__ float rgb_to_v(float r, float g, float b, bool full_range) +{ + return full_range ? (128.0f + 0.5f * r - 0.418688f * g - 0.081312f * b) : + (128.0f + 0.439f * r - 0.368f * g - 0.071f * b); +} + +__global__ void rgba_to_nv12_kernel(const uint8_t* __restrict__ rgba, + int rgba_pitch, + uint8_t* __restrict__ y_plane, + int y_pitch, + uint8_t* __restrict__ uv_plane, + int uv_pitch, + int width, + int height, + bool full_range) +{ + // One thread per 2x2 block: writes four luma samples and one chroma pair. + const int bx = blockIdx.x * blockDim.x + threadIdx.x; + const int by = blockIdx.y * blockDim.y + threadIdx.y; + const int x = bx * 2; + const int y = by * 2; + if (x >= width || y >= height) + return; + + for (int dy = 0; dy < 2; ++dy) + { + for (int dx = 0; dx < 2; ++dx) + { + const int px = x + dx; + const int py = y + dy; + if (px >= width || py >= height) + continue; + + const uint8_t* pixel = rgba + static_cast(py) * rgba_pitch + static_cast(px) * 4; + const float r = pixel[0]; + const float g = pixel[1]; + const float b = pixel[2]; + y_plane[static_cast(py) * y_pitch + px] = clamp_u8(rgb_to_y(r, g, b, full_range)); + } + } + + const uint8_t* top_left = rgba + static_cast(y) * rgba_pitch + static_cast(x) * 4; + const float r = top_left[0]; + const float g = top_left[1]; + const float b = top_left[2]; + + uint8_t* uv = uv_plane + static_cast(by) * uv_pitch + static_cast(bx) * 2; + uv[0] = clamp_u8(rgb_to_u(r, g, b, full_range)); + uv[1] = clamp_u8(rgb_to_v(r, g, b, full_range)); +} + +} // namespace + +namespace plugins +{ +namespace sensing +{ + +void launch_rgba_to_nv12(const uint8_t* rgba, + int rgba_pitch, + uint8_t* y_plane, + int y_pitch, + uint8_t* uv_plane, + int uv_pitch, + int width, + int height, + bool full_range, + cudaStream_t stream) +{ + const dim3 block(16, 16); + const dim3 grid((width / 2 + block.x - 1) / block.x, (height / 2 + block.y - 1) / block.y); + rgba_to_nv12_kernel<<>>( + rgba, rgba_pitch, y_plane, y_pitch, uv_plane, uv_pitch, width, height, full_range); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/rgba_to_nv12.cuh b/src/plugins/sensing/core/rgba_to_nv12.cuh new file mode 100644 index 0000000000..a296be6181 --- /dev/null +++ b/src/plugins/sensing/core/rgba_to_nv12.cuh @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Convert packed RGBA8 to NV12 (Y plane + interleaved UV plane). + * + * Chroma is point-sampled from the top-left pixel of each 2x2 block rather + * than averaged: the ISP already delivered 4:2:0, so the RGBA the + * capture path produced was upsampled from it and averaging would only blur + * chroma that was never independent. Width and height must be even. + * + * @param full_range true for [0,255] luma, false for broadcast [16,235]. + */ +void launch_rgba_to_nv12(const uint8_t* rgba, + int rgba_pitch, + uint8_t* y_plane, + int y_pitch, + uint8_t* uv_plane, + int uv_pitch, + int width, + int height, + bool full_range, + cudaStream_t stream); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_camera.cpp b/src/plugins/sensing/core/sensing_camera.cpp new file mode 100644 index 0000000000..1f92993105 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.cpp @@ -0,0 +1,203 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "sensing_camera.hpp" + +#include "video_output.hpp" + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +VideoOutputConfig make_output_config(const SensingConfig& config, const StreamConfig& stream, const CameraInfo& sensor) +{ + VideoOutputConfig output; + output.source_width = sensor.width; + output.source_height = sensor.height; + output.scale_div = config.encode.scale_div; + output.fps = config.encode.fps; + output.gpu_id = config.gpu_id; + output.bitrate_bps = config.encode.bitrate_bps; + output.peak_bitrate_bps = config.encode.bitrate_bps + config.encode.bitrate_bps / 2; + output.input = sensor.pixels; + output.gop = config.gop; + output.full_range = config.full_range; + output.timestamp_basis = config.encode.timestamp_basis; + output.correlation = config.encode.correlation; + output.direct_host = stream.direct_host; + output.direct_port = stream.direct_port; + return output; +} + +} // namespace + +SensingCamera::SensingCamera(const SensingConfig& config, + std::unique_ptr camera, + const std::vector& streams) + : m_config(config) +{ + if (streams.empty()) + throw std::runtime_error("SensingCamera: no streams requested"); + + m_camera = std::move(camera); + if (!m_camera) + throw std::runtime_error("SensingCamera: no camera backend"); + const auto& sensors = m_camera->cameras(); + + // Everything that needs no hardware is checked first, so a bad name or divisor fails before the cameras start. + std::set tracks; + for (const auto& stream_config : streams) + { + if (!tracks.insert(stream_config.name).second) + throw std::runtime_error("stream name '" + stream_config.name + "' is used twice"); + const auto found = std::find_if( + sensors.begin(), sensors.end(), [&](const CameraInfo& s) { return s.id == stream_config.sensor_id; }); + if (found == sensors.end()) + { + std::ostringstream oss; + oss << "sensor=" << stream_config.sensor_id << " is not a pipeline in '" << config.platform_config_name + << "'. Available:"; + for (const auto& s : sensors) + oss << ' ' << s.id; + oss << ". Note this is the backend's own camera index (a SIPL pipeline index is not the GMSL link index)."; + throw std::runtime_error(oss.str()); + } + VideoOutput::validate_geometry(found->width, found->height, config.encode.scale_div); + if (found->pixels == PixelKind::DmaBufNv12 && config.encode.scale_div != 1) + throw std::runtime_error("sensor " + std::to_string(found->id) + + " delivers DMA-BUF frames, which cannot be reduced: use --scale-div=1"); + } + + m_streams.reserve(streams.size()); + for (const auto& stream_config : streams) + { + const auto found = std::find_if( + sensors.begin(), sensors.end(), [&](const CameraInfo& s) { return s.id == stream_config.sensor_id; }); + const VideoOutputConfig output = make_output_config(config, stream_config, *found); + const uint32_t width = found->width / output.scale_div; + const uint32_t height = found->height / output.scale_div; + const double max_fps = std::min(output.fps, found->fps); + + Stream stream; + stream.sensor_id = found->id; + stream.track = stream_config.name; + stream.output = std::make_unique(output); + m_streams.push_back(std::move(stream)); + + std::cout << "Sensor " << found->id << " (" << found->name << ") -> stream '" << stream_config.name + << "': " << width << "x" << height << " @ up to " << max_fps << " fps, direct link " + << (stream_config.direct_host.empty() ? + std::string("not set yet") : + stream_config.direct_host + ":" + std::to_string(stream_config.direct_port)) + << std::endl; + } + + m_camera->start(); +} + +SensingCamera::~SensingCamera() +{ + if (m_camera) + m_camera->stop(); +} + +void SensingCamera::update() +{ + for (auto& stream : m_streams) + { + auto view = m_camera->acquire_latest(stream.sensor_id); + if (view.has_value() && (!stream.have_last_sequence || view->sequence != stream.last_sequence)) + { + if (stream.have_last_sequence && view->sequence > stream.last_sequence + 1) + stream.missed_captures += view->sequence - stream.last_sequence - 1; + stream.last_sequence = view->sequence; + stream.have_last_sequence = true; + ++stream.frame_count; + + // A GPU snapshot only; the conversion and the network are on the output's own thread. + stream.output->submit(*view); + } + + if (stream.output->failed()) + throw std::runtime_error("sensor " + std::to_string(stream.sensor_id) + ": " + stream.output->failure()); + } +} + +std::vector SensingCamera::figures() const +{ + std::vector out; + for (const auto& stream : m_streams) + { + StreamFigures figures; + figures.track = stream.track; + figures.sensor_id = stream.sensor_id; + figures.frames = stream.frame_count; + figures.published = stream.output->stats().published; + figures.direct = stream.output->direct_status(); + out.push_back(std::move(figures)); + } + return out; +} + +bool SensingCamera::set_direct_target(const std::string& track, const std::string& host, uint16_t port) +{ + for (auto& stream : m_streams) + { + if (stream.track == track) + { + stream.output->set_direct_target(host, port); + return true; + } + } + return false; +} + +void SensingCamera::flush() +{ + for (auto& stream : m_streams) + stream.output->flush(); +} + +void SensingCamera::print_stats() const +{ + for (const auto& stream : m_streams) + { + const auto s = stream.output->stats(); + std::cout << " sensor " << stream.sensor_id << ": " << stream.frame_count << " frames"; + if (stream.missed_captures) + std::cout << " | " << stream.missed_captures << " captures missed"; + std::cout << " | encoded " << s.published << ", " << s.paced_out << " paced out, " << s.replaced << " replaced"; + if (s.encoder_busy) + std::cout << ", " << s.encoder_busy << " encoder busy"; + if (s.keyframes_requested) + std::cout << ", " << s.keyframes_requested << " keyframes requested"; + if (m_config.encode.correlation == FrameCorrelation::Stamp) + std::cout << " | stamps matched " << s.matched << ", unmatched " << s.unmatched << ", encoder skipped " + << s.encoder_dropped; + const DirectLinkStatus direct = stream.output->direct_status(); + if (direct.host.empty()) + std::cout << " | direct: no destination"; + else if (direct.connected) + std::cout << " | direct: " << direct.host << ":" << direct.port << " connected"; + else + std::cout << " | direct: " << direct.host << ":" << direct.port << " NOT connected" + << (direct.error.empty() ? "" : " (" + direct.error + ")"); + const std::string encoder = stream.output->describe(); + if (!encoder.empty()) + std::cout << " (" << encoder << ")"; + std::cout << std::endl; + } +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_camera.hpp b/src/plugins/sensing/core/sensing_camera.hpp new file mode 100644 index 0000000000..e09d9ddf10 --- /dev/null +++ b/src/plugins/sensing/core/sensing_camera.hpp @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "camera_capture.hpp" +#include "sensing_types.hpp" +#include "video_output.hpp" + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Multi-sensor SENSING camera producer. + * + * One CameraCapture owns every camera of the rig (SIPL's camera object is process-wide, so there is no + * per-camera capture object to hold). Each update() polls every sensor's latest frame and hands a new frame + * to that sensor's encoder and direct link to the PC. Geometry is per sensor and comes from the capture backend. + */ +/// Live figures for one stream, for the management channel. +struct StreamFigures +{ + std::string track; + uint32_t sensor_id = 0; + uint64_t frames = 0; + uint64_t published = 0; + DirectLinkStatus direct; +}; + +class SensingCamera +{ +public: + /// `camera` is the opened backend; its camera ids are what StreamConfig::sensor_id names. + SensingCamera(const SensingConfig& config, + std::unique_ptr camera, + const std::vector& streams); + ~SensingCamera(); + + SensingCamera(const SensingCamera&) = delete; + SensingCamera& operator=(const SensingCamera&) = delete; + SensingCamera(SensingCamera&&) = delete; + SensingCamera& operator=(SensingCamera&&) = delete; + + /** @brief Poll every sensor and submit newly captured frames. Throws once a stream can no longer work. */ + void update(); + + /** @brief Flush each stream's encoder. */ + void flush(); + + /** @brief Per-stream figures. Call from the thread that calls update(). */ + std::vector figures() const; + + /** + * @brief Send stream `track` to host:port from now on (empty host stops the direct link). + * @return false if no stream has that track name. Thread-safe against update(). + */ + bool set_direct_target(const std::string& track, const std::string& host, uint16_t port); + + /** @brief Print per-sensor frame counts to stdout. */ + void print_stats() const; + +private: + struct Stream + { + uint32_t sensor_id = 0; + std::string track; + std::unique_ptr output; + /// SIPL publish counter of the frame already handled; skips re-reads. + uint64_t last_sequence = 0; + bool have_last_sequence = false; + uint64_t frame_count = 0; + /// Captures the poll loop never saw, from gaps in the SIPL sequence. + uint64_t missed_captures = 0; + }; + + SensingConfig m_config; + std::unique_ptr m_camera; + std::vector m_streams; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensing_types.hpp b/src/plugins/sensing/core/sensing_types.hpp new file mode 100644 index 0000000000..032d5db09f --- /dev/null +++ b/src/plugins/sensing/core/sensing_types.hpp @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Plain configuration types, deliberately free of the SIPL and NvSci headers, which define X11-style +// macros (Success, Status, None) that collide with other libraries'. + +#pragma once + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// One captured sensor and the stream that carries it to the PC. +struct StreamConfig +{ + /// Send every encoded unit of this stream to the PC video stage at host:port (AVS1 over TCP); empty means no + /// destination yet (one can be set over the management channel). Each stream needs its own port on the PC. + std::string direct_host; + uint16_t direct_port = 0; + /// The selected backend's own camera index (SIPL pipeline or Argus device), validated at startup. + uint32_t sensor_id = 0; + /// The stream's name: what the PC, the management channel and the logs call this sensor. + std::string name; +}; + +/// The clock and event a frame's application timestamp is read from. +enum class TimestampBasis +{ + /// CLOCK_MONOTONIC when the frame was ready to encode (`monotonic_us`); recordable. + FrameReadyMonotonic, + /// The sensor's capture event converted to CLOCK_MONOTONIC (`capture_monotonic_us`); unverified on hardware. + SensorCaptureMonotonic, +}; + +/// How an encoded unit is matched to the capture it came from. +enum class FrameCorrelation +{ + /// By the stamp the encoder hands back with the unit (the stamp it was given with the input). A unit whose stamp + /// matches no submitted frame carries no capture timestamp, and inputs the encoder never returned are counted. + Stamp, + /// By order: the encoder is assumed to return one unit per input, in order. Not verifiable; an input the encoder + /// skips shifts every later timestamp. For when the encoder does not return the stamp and approximate timing is + /// acceptable (a live preview), never for recording. + Fifo, +}; + +struct EncodeConfig +{ + /// Rate cap of the encoded stream, independent of the capture rate. + uint32_t fps = 30; + /// Output-only uniform reduction of every stream; 1 sends the sensor's native size. + uint32_t scale_div = 1; + /// Target bitrate of each stream's hardware encoder; the VBR ceiling is half as much again. + uint32_t bitrate_bps = 8'000'000; + /// What each frame's application timestamp is; the receiving side's streams file must declare the same basis. + TimestampBasis timestamp_basis = TimestampBasis::FrameReadyMonotonic; + FrameCorrelation correlation = FrameCorrelation::Stamp; +}; + +struct SensingConfig +{ + /// Vendor platform config JSON. Required -- SIPL cannot enumerate without one. + std::string platform_config_json; + /// Named config inside it. + std::string platform_config_name = "SHW5G_2"; + /// One mask per deserializer, in transport order. + std::vector link_masks{ 0x0000, 0x1100 }; + std::string nito_dir = "/var/nvidia/nvcam/settings/sipl"; + int gpu_id = 0; + + /// IDR period in frames; 0 defers to the encoder default of fps*5. + uint32_t gop = 0; + + bool full_range = false; + /// Defaults off: set_sipl_buf_attributes() requests the ISP0 surface order explicitly. + bool swap_uv = false; + /// ISP0 buffers per sensor, ~6 MB each at 2560x1984 NV12. + uint32_t isp0_buffers = 6; + + /// Argus only: the sensor mode index applied to every camera. + uint32_t argus_sensor_mode = 0; + + EncodeConfig encode; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensor_clock.cpp b/src/plugins/sensing/core/sensor_clock.cpp new file mode 100644 index 0000000000..4b3a1ab619 --- /dev/null +++ b/src/plugins/sensing/core/sensor_clock.cpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "sensor_clock.hpp" + +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +int64_t clock_ns(clockid_t id) +{ + timespec ts{}; + clock_gettime(id, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000LL + ts.tv_nsec; +} + +#if defined(__aarch64__) +int64_t read_counter() +{ + uint64_t v; + asm volatile("isb; mrs %0, cntvct_el0" : "=r"(v)::"memory"); + return static_cast(v); +} +#endif + +/// The narrowest of several back-to-back reads: a descheduled read has a wide bracket and is discarded. +template +ClockPair narrowest_pair(ReadSource read_source) +{ + constexpr int kAttempts = 8; + ClockPair best{}; + best.bracket_ns = INT64_MAX; + for (int i = 0; i < kAttempts; ++i) + { + const int64_t before = clock_ns(CLOCK_MONOTONIC); + const int64_t source = read_source(); + const int64_t after = clock_ns(CLOCK_MONOTONIC); + if (after - before < best.bracket_ns) + best = { source, before + (after - before) / 2, after - before }; + } + return best; +} + +} // namespace + +ClockPair read_counter_pair() +{ +#if defined(__aarch64__) + return narrowest_pair(read_counter); +#else + throw std::runtime_error("the ARM generic counter can only be read on aarch64"); +#endif +} + +int64_t read_counter_hz() +{ +#if defined(__aarch64__) + uint64_t hz; + asm volatile("mrs %0, cntfrq_el0" : "=r"(hz)); + return static_cast(hz); +#else + throw std::runtime_error("the ARM generic counter can only be read on aarch64"); +#endif +} + +ClockPair read_raw_pair() +{ + return narrowest_pair([] { return clock_ns(CLOCK_MONOTONIC_RAW); }); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sensor_clock.hpp b/src/plugins/sensing/core/sensor_clock.hpp new file mode 100644 index 0000000000..0872f7aeba --- /dev/null +++ b/src/plugins/sensing/core/sensor_clock.hpp @@ -0,0 +1,136 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// What a backend's `CaptureFrame::sensor_time` counts. +enum class SensorTimeDomain +{ + /// The backend reports no sensor time. + None, + /// SIPL `frameCaptureTSC`: ticks of the ARM generic counter (assumed; checked at run time by SensorClockMap). + GenericCounterTicks, + /// Argus `getSensorTimestamp()`: nanoseconds on CLOCK_MONOTONIC_RAW (assumed; checked at run time). + MonotonicRawNs, +}; + +/// A source clock reading paired with CLOCK_MONOTONIC, read back to back. +struct ClockPair +{ + /// The source clock's value (counter ticks or raw ns) at the midpoint of the bracket. + int64_t source = 0; + /// CLOCK_MONOTONIC ns at the same midpoint. + int64_t monotonic_ns = 0; + /// Width of the bracket the pair was read in; the pair is no more certain than this. + int64_t bracket_ns = 0; +}; + +/// Reads the ARM generic counter (CNTVCT_EL0) and CLOCK_MONOTONIC back to back, keeping the narrowest of a few +/// attempts. Throws where the counter is not readable (not aarch64). +ClockPair read_counter_pair(); +/// The generic counter's frequency in Hz (CNTFRQ_EL0). +int64_t read_counter_hz(); +/// Reads CLOCK_MONOTONIC_RAW and CLOCK_MONOTONIC back to back, keeping the narrowest of a few attempts. +ClockPair read_raw_pair(); + +/** + * @brief Places a backend's sensor timestamp on this machine's CLOCK_MONOTONIC. + * + * One anchor pair (source clock, CLOCK_MONOTONIC) is read through a supported clock API and refreshed at a low + * rate, never per frame; a frame's capture time is the anchor plus the source-clock difference. The source's units + * are an assumption, not a fact (see SensorTimeDomain), so every conversion is checked: a capture time that is in + * the future of the frame's ready time, or more than `max_age_ns` before it, means the assumed domain or rate is + * wrong and the conversion throws. There is no fallback to a host-side stamp. + * + * Not thread-safe; one instance per capture thread. + */ +class SensorClockMap +{ +public: + /// Reads one pair; injectable so the mapping is testable off-target. + using PairReader = std::function; + + struct Config + { + SensorTimeDomain domain = SensorTimeDomain::None; + /// Counter frequency in Hz; required for GenericCounterTicks. + int64_t counter_hz = 0; + /// Anchor refresh period. + int64_t refresh_ns = 1'000'000'000; + /// Largest plausible gap between capture and the frame being ready. + int64_t max_age_ns = 2'000'000'000; + }; + + SensorClockMap(Config config, PairReader reader) : m_config(config), m_reader(std::move(reader)) + { + if (m_config.domain == SensorTimeDomain::None) + throw std::invalid_argument("SensorClockMap needs a sensor time domain"); + if (m_config.domain == SensorTimeDomain::GenericCounterTicks && m_config.counter_hz <= 0) + throw std::invalid_argument("SensorClockMap needs the counter frequency for counter ticks"); + } + + /** + * @brief CLOCK_MONOTONIC ns of the sensor's capture event. + * @param sensor_time The backend's raw sensor timestamp. + * @param ready_monotonic_ns CLOCK_MONOTONIC when the frame was ready (the plausibility reference). + * @param now_monotonic_ns CLOCK_MONOTONIC now, to decide when the anchor is due. + * @throws std::runtime_error when the value is not plausible for the assumed domain. + */ + int64_t to_monotonic_ns(uint64_t sensor_time, int64_t ready_monotonic_ns, int64_t now_monotonic_ns) + { + if (!m_have_anchor || now_monotonic_ns - m_anchor_taken_ns >= m_config.refresh_ns) + { + m_anchor = m_reader(); + m_have_anchor = true; + m_anchor_taken_ns = now_monotonic_ns; + } + const int64_t delta = to_ns(static_cast(sensor_time) - m_anchor.source); + const int64_t capture = m_anchor.monotonic_ns + delta; + if (capture > ready_monotonic_ns || ready_monotonic_ns - capture > m_config.max_age_ns) + { + throw std::runtime_error( + "sensor timestamp " + std::to_string(sensor_time) + " maps to " + + std::to_string(capture - ready_monotonic_ns) + + " ns relative to the frame being ready: the assumed sensor clock domain or rate is wrong"); + } + return capture; + } + + /// Uncertainty of the current anchor (its bracket); zero before the first conversion. + int64_t anchor_uncertainty_ns() const + { + return m_have_anchor ? m_anchor.bracket_ns : 0; + } + +private: + int64_t to_ns(int64_t source_delta) const + { + if (m_config.domain == SensorTimeDomain::MonotonicRawNs) + return source_delta; + // Split into whole seconds and remainder so the product cannot overflow 64 bits. + const int64_t hz = m_config.counter_hz; + const int64_t secs = source_delta / hz; + const int64_t rem = source_delta % hz; + return secs * 1'000'000'000 + rem * 1'000'000'000 / hz; + } + + Config m_config; + PairReader m_reader; + ClockPair m_anchor{}; + int64_t m_anchor_taken_ns = 0; + bool m_have_anchor = false; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_camera.cpp b/src/plugins/sensing/core/sipl_camera.cpp new file mode 100644 index 0000000000..fc9a2b93ae --- /dev/null +++ b/src/plugins/sensing/core/sipl_camera.cpp @@ -0,0 +1,1370 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// Portions adapted from NVIDIA's sipl_coe_unit_sample +// (/usr/src/jetson_multimedia_api/samples/unittest_samples/), BSD-3-Clause, +// Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. + +#include "sipl_camera.hpp" + +#include "yuv_to_rgba.cuh" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ +namespace +{ + +using nvsipl::INvSIPLCamera; +using nvsipl::INvSIPLCameraQuery; +using nvsipl::INvSIPLClient; +using nvsipl::NVSIPL_STATUS_OK; +using nvsipl::SIPLStatus; + +constexpr auto kIsp0 = INvSIPLClient::ConsumerDesc::OutputType::ISP0; +constexpr auto kIcp = INvSIPLClient::ConsumerDesc::OutputType::ICP; + +/// SIPL allocates and registers ICP capture buffers unconditionally, even when +/// only an ISP output is consumed -- CNvSIPLMaster::AllocateBuffers hardcodes +/// the ICP slot to true. Skip them and RegisterImages fails with a bare +/// NVSIPL_STATUS_ERROR that names nothing. +constexpr uint32_t kIcpBuffers = 6; + +uint64_t monotonic_ns() +{ + return static_cast( + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()); +} + +uint64_t unix_ns() +{ + return static_cast( + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); +} + +/// SIPL reports almost every failure as NVSIPL_STATUS_ERROR (10), which says +/// nothing on its own -- the reason only appears in its own log, above a +/// threshold that defaults to silent. SENSING_SIPL_TRACE=1..4 raises it +/// (1=error, 2=warn, 3=info, 4=debug). +void init_sipl_trace() +{ + const char* level = std::getenv("SENSING_SIPL_TRACE"); + if (level == nullptr) + { + return; + } + const int value = std::atoi(level); + if (value <= 0) + { + return; + } + auto* trace = nvsipl::INvSIPLTrace::GetInstance(); + if (trace != nullptr) + { + trace->SetLevel(static_cast(value)); + } +} + +/// The enumerator name for the statuses whose meaning is known; every other value is reported by number only. +const char* status_name(SIPLStatus status) +{ + // The order of NvSIPLCommon.hpp's SIPLStatus. + switch (static_cast(status)) + { + case 1: + return "bad argument"; + case 2: + return "not supported"; + case 3: + return "out of memory"; + case 4: + return "resource error"; + case 5: + return "timed out"; + case 6: + return "invalid state"; + case 7: + return "end of stream"; + case 8: + // Init() reports this when some cameras started and at least one did not. + return "not initialized: some cameras failed to start"; + case 9: + return "fault state"; + case 10: + return "error: no cause given"; + default: + return nullptr; + } +} + +void check_sipl(SIPLStatus status, const char* what) +{ + if (status != NVSIPL_STATUS_OK) + { + std::ostringstream oss; + oss << what << " failed with SIPLStatus " << static_cast(status); + if (const char* name = status_name(status)) + oss << " (" << name << ")"; + throw std::runtime_error(oss.str()); + } +} + +void check_sci(NvSciError err, const char* what) +{ + if (err != NvSciError_Success) + { + std::ostringstream oss; + oss << what << " failed with NvSciError " << static_cast(err); + throw std::runtime_error(oss.str()); + } +} + +void check_cuda(CUresult result, const char* what) +{ + if (result == CUDA_SUCCESS) + { + return; + } + const char* name = nullptr; + const char* text = nullptr; + cuGetErrorName(result, &name); + cuGetErrorString(result, &text); + std::ostringstream oss; + oss << what << " failed"; + if (name) + { + oss << ": " << name; + } + if (text) + { + oss << " (" << text << ")"; + } + throw std::runtime_error(oss.str()); +} + +cudaTextureObject_t texture_for_array(CUarray array); + +void check_runtime(cudaError_t result, const char* what) +{ + if (result != cudaSuccess) + { + std::ostringstream oss; + oss << what << " failed: " << cudaGetErrorString(result); + throw std::runtime_error(oss.str()); + } +} + +cudaTextureObject_t texture_for_array(CUarray array) +{ + cudaResourceDesc resource_desc{}; + resource_desc.resType = cudaResourceTypeArray; + resource_desc.res.array.array = reinterpret_cast(array); + + cudaTextureDesc texture_desc{}; + texture_desc.addressMode[0] = cudaAddressModeClamp; + texture_desc.addressMode[1] = cudaAddressModeClamp; + texture_desc.filterMode = cudaFilterModePoint; + texture_desc.readMode = cudaReadModeElementType; + texture_desc.normalizedCoords = 0; + + cudaTextureObject_t texture = 0; + check_runtime(cudaCreateTextureObject(&texture, &resource_desc, &texture_desc, nullptr), "cudaCreateTextureObject"); + return texture; +} + +/** + * @brief Buffer attributes for the ISP0 output. + * + * Block-linear and REC709_ER are what the ISP produces, not what we would + * prefer. Neither is negotiable: GetImageAttributes rejects an attribute list + * asking for pitch-linear or BT.601 with NVSIPL_STATUS_BAD_ARGUMENT. So the + * conversion adapts to the ISP rather than the other way round -- convert() + * takes the CUDA-array path, and yuv_to_rgba.cu is called with bt709=true. + * `nvsipl_camera -v 3` prints the same thing: "YUV 420 SEMI-PLANAR UINT8 BL + * REC_709ER". + */ +NvSciError set_sipl_buf_attributes(NvSciBufAttrList attr_list) +{ + NvSciBufType buf_type = NvSciBufType_Image; + NvSciBufAttrValAccessPerm access_perm = NvSciBufAccessPerm_ReadWrite; + // Must be true, and it is not about us wanting CPU access. NvmmImageFill- + // NvSciBufAttrList sets NeedCpuAccess=true on the NvBufSurface list, so + // reconciliation ORs it in; if the producer list says false, SIPL's + // GetVerifiedBufferAttributes sees a producer/reconciled mismatch and + // RegisterImages fails with a bare NVSIPL_STATUS_ERROR. + bool cpu_access = true; + bool cpu_cache = true; + NvSciBufSurfMemLayout mem_layout = NvSciSurfMemLayout_SemiPlanar; + NvSciBufAttrValImageLayoutType layout_type = NvSciBufImage_BlockLinearType; + NvSciBufSurfType surf_type = NvSciSurfType_YUV; + NvSciBufSurfSampleType sample_type = NvSciSurfSampleType_420; + NvSciBufSurfBPC surf_bpc = NvSciSurfBPC_8; + NvSciBufSurfComponentOrder comp_order = NvSciSurfComponentOrder_YUV; + NvSciBufAttrValColorStd color_std[] = { NvSciColorStd_REC709_ER }; + + NvSciBufAttrKeyValuePair kvp[] = { + { NvSciBufGeneralAttrKey_Types, &buf_type, sizeof(buf_type) }, + { NvSciBufGeneralAttrKey_RequiredPerm, &access_perm, sizeof(access_perm) }, + { NvSciBufGeneralAttrKey_NeedCpuAccess, &cpu_access, sizeof(cpu_access) }, + { NvSciBufGeneralAttrKey_EnableCpuCache, &cpu_cache, sizeof(cpu_cache) }, + { NvSciBufImageAttrKey_Layout, &layout_type, sizeof(layout_type) }, + { NvSciBufImageAttrKey_SurfType, &surf_type, sizeof(surf_type) }, + { NvSciBufImageAttrKey_SurfMemLayout, &mem_layout, sizeof(mem_layout) }, + { NvSciBufImageAttrKey_SurfSampleType, &sample_type, sizeof(sample_type) }, + { NvSciBufImageAttrKey_SurfBPC, &surf_bpc, sizeof(surf_bpc) }, + { NvSciBufImageAttrKey_SurfComponentOrder, &comp_order, sizeof(comp_order) }, + { NvSciBufImageAttrKey_SurfColorStd, &color_std, sizeof(color_std) }, + }; + + return NvSciBufAttrListSetAttrs(attr_list, kvp, sizeof(kvp) / sizeof(kvp[0])); +} + + +/// Fill a second attribute list with the NvBufSurface attributes matching the +/// SIPL list, so the allocated buffer satisfies both consumers. Without it the +/// buffer is allocated for SIPL alone and RegisterImages rejects it with +/// NVSIPL_STATUS_INVALID_STATE -- a status whose only documented meaning is +/// "pipeline had seen an init error", which sends you looking in the wrong +/// place entirely. +/// +/// Adapted from SetNvBufNvSciBufAttributes in NVIDIA's NvNvSciBufHelper.cpp +/// (BSD-3-Clause). Its colour table has exactly two entries, both REC709_ER; +/// that is the whole set this ISP path can produce. +NvSciError set_nvbuf_attributes(NvSciBufAttrList nvbuf_attrs, NvSciBufAttrList sipl_attrs, int gpu_id) +{ + NvSciBufAttrKeyValuePair img[] = { + { NvSciBufImageAttrKey_SurfWidthBase, nullptr, 0 }, { NvSciBufImageAttrKey_SurfHeightBase, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfSampleType, nullptr, 0 }, { NvSciBufImageAttrKey_SurfColorStd, nullptr, 0 }, + { NvSciBufImageAttrKey_SurfMemLayout, nullptr, 0 }, { NvSciBufImageAttrKey_SurfBPC, nullptr, 0 }, + { NvSciBufImageAttrKey_Layout, nullptr, 0 }, + }; + const NvSciError err = NvSciBufAttrListGetAttrs(sipl_attrs, img, sizeof(img) / sizeof(img[0])); + if (err != NvSciError_Success) + { + return err; + } + + const auto width = *static_cast(img[0].value); + const auto height = *static_cast(img[1].value); + const auto sample = *static_cast(img[2].value); + const auto color = *static_cast(img[3].value); + const auto mem = *static_cast(img[4].value); + const auto bpc = *static_cast(img[5].value); + const auto layout = *static_cast(img[6].value); + + NvBufSurfaceColorFormat pixfmt; + if (color == NvSciColorStd_REC709_ER && sample == NvSciSurfSampleType_420 && bpc == NvSciSurfBPC_8 && + mem == NvSciSurfMemLayout_SemiPlanar) + { + pixfmt = NVBUF_COLOR_FORMAT_NV12_709_ER; + } + else if (color == NvSciColorStd_REC709_ER && sample == NvSciSurfSampleType_420 && bpc == NvSciSurfBPC_8 && + mem == NvSciSurfMemLayout_Planar) + { + pixfmt = NVBUF_COLOR_FORMAT_YUV420_709_ER; + } + else + { + std::cerr << "[sipl] no NvBufSurface colour format for colourStd=" << static_cast(color) + << " sample=" << static_cast(sample) << " bpc=" << static_cast(bpc) + << " memLayout=" << static_cast(mem) << std::endl; + return NvSciError_BadParameter; + } + + NvmmImageParams params{}; + params.gpuId = static_cast(gpu_id); + params.width = width; + params.height = height; + params.colorFormat = pixfmt; + params.layout = (layout == NvSciBufImage_PitchLinearType) ? NVBUF_LAYOUT_PITCH : NVBUF_LAYOUT_BLOCK_LINEAR; + params.displayscanformat = NVBUF_DISPLAYSCANFORMAT_PROGRESSIVE; + params.isProtected = false; + + return (NvmmImageFillNvSciBufAttrList(¶ms, nvbuf_attrs) == 0) ? NvSciError_Success : NvSciError_BadParameter; +} + +/// Read back what reconciliation settled on, and report whether it is extended +/// range. _ER is full range (0-255), _SR is studio range (16-235); decoding one +/// as the other subtracts a black level that is not there and crushes shadows, +/// which on a dim scene loses most of the image. Returns true for full range. +bool check_color_std(NvSciBufAttrList reconciled) +{ + NvSciBufAttrKeyValuePair query{ NvSciBufImageAttrKey_SurfColorStd, nullptr, 0 }; + if (NvSciBufAttrListGetAttrs(reconciled, &query, 1) != NvSciError_Success || query.value == nullptr) + { + std::cerr << "[sipl] warning: could not read back SurfColorStd; assuming BT.709 extended range" << std::endl; + return true; + } + const auto* value = static_cast(query.value); + if (*value != NvSciColorStd_REC709_ER && *value != NvSciColorStd_REC709_SR) + { + std::ostringstream oss; + oss << "ISP0 reconciled to colour standard " << static_cast(*value) + << "; yuv_to_rgba.cu implements BT.601 and BT.709 only, and decoding anything else as" + " 709 would be a silent hue error"; + throw std::runtime_error(oss.str()); + } + return *value == NvSciColorStd_REC709_ER; +} + +std::vector load_nito(const std::string& dir, const std::string& module) +{ + const std::string path = dir + "/" + module + ".nito"; + std::ifstream in(path, std::ios::binary | std::ios::ate); + if (!in) + { + throw std::runtime_error("cannot open NITO file " + path + + " (the ISP has no tuning; run the vendor install.sh on the host)"); + } + const auto size = static_cast(in.tellg()); + in.seekg(0); + std::vector blob(static_cast(size)); + if (!in.read(reinterpret_cast(blob.data()), size)) + { + throw std::runtime_error("short read on NITO file " + path); + } + return blob; +} + +/// Walk a SensorSystemConfig into flat SensorInfo records. The nesting is +/// module -> variant -> sensorConfigs -> variant, and `id` is the pipeline +/// index SetPipelineCfg wants. +std::vector flatten(const nvsipl::sensorconfig::SensorSystemConfig& cfg) +{ + std::vector out; + for (const auto& module : cfg.modules) + { + std::visit( + [&](const auto& mod) + { + for (const auto& sensor_variant : mod.sensorConfigs) + { + std::visit( + [&](const auto& sensor) + { + SensorInfo info; + info.id = sensor.id; + info.name = module.name; + if (!sensor.vcInfoList.empty()) + { + const auto& vc = sensor.vcInfoList.front(); + info.width = vc.resolution.width; + info.height = vc.resolution.height; + info.fps = static_cast(vc.fps); + } + out.push_back(std::move(info)); + }, + sensor_variant); + } + }, + module.moduleType); + } + return out; +} + +} // namespace + +// ============================================================================= +// Impl +// ============================================================================= + +struct SiplCamera::Impl +{ + /// One ISP0 buffer, imported into CUDA once at registration. Nothing here + /// is re-created per frame -- that is the whole point of registering. + struct Slot + { + NvSciBufObj buf = nullptr; + NvBufSurface* surface = nullptr; + CUgraphicsResource resource = nullptr; + CUeglFrame egl_frame{}; + }; + + struct DeviceBuffer + { + uint8_t* ptr = nullptr; + size_t pitch = 0; + }; + + struct Sensor + { + SensorInfo info; + /// Event thread only: notifications in a row that mean the camera is not delivering. + uint32_t consecutive_capture_timeouts = 0; + uint32_t consecutive_control_failures = 0; + nvsipl::NvSIPLPipelineConfiguration pipeline_config{}; + nvsipl::NvSIPLPipelineQueues queues{}; + + /// From the reconciled surface, not from config: the ISP decides. + bool full_range = true; + NvSciBufAttrList buf_attrs = nullptr; + std::vector buf_objects; + /// ICP pool. Never read -- allocated only because SIPL requires it. + NvSciBufAttrList icp_attrs = nullptr; + std::vector icp_buf_objects; + std::vector slots; + NvSciSyncObj sync_obj = nullptr; + + /// Triple-buffered RGBA8 mailbox. A reader leases the published slot + /// until its next latest() call, so the producer never overwrites it. + std::array buffers{}; + mutable std::mutex publish_mutex; + int publish_idx = -1; + int lease_idx = -1; + uint64_t published_sequence = 0; + uint64_t consumed_sequence = 0; + uint64_t published_timestamp_ns = 0; + uint64_t published_unix_ns = 0; + uint64_t published_capture_tsc = 0; + + std::thread frame_thread; + std::thread event_thread; + }; + + explicit Impl(const SiplConfig& cfg) : config(cfg) + { + } + + SiplConfig config; + + NvSciBufModule sci_buf_module = nullptr; + NvSciSyncModule sci_sync_module = nullptr; + NvSciSyncCpuWaitContext cpu_wait_context = nullptr; + + CUdevice cu_device = 0; + CUcontext cu_context = nullptr; + bool cu_context_retained = false; + CUstream convert_stream = nullptr; + + std::unique_ptr camera; + std::unique_ptr query_api; + std::unique_ptr system_config; + + std::vector sensor_infos; + std::vector> sensors; + + std::atomic running{ false }; + std::atomic failed{ false }; + mutable std::mutex error_mutex; + std::string failure_message; + + Sensor* find(uint32_t sensor_id) + { + for (auto& s : sensors) + { + if (s->info.id == sensor_id) + { + return s.get(); + } + } + return nullptr; + } + + void set_failure(const std::string& message) + { + std::lock_guard guard(error_mutex); + if (failure_message.empty()) + { + failure_message = message; + } + failed.store(true); + } + + void throw_if_failed() const + { + if (!failed.load()) + { + return; + } + std::lock_guard guard(error_mutex); + throw std::runtime_error(failure_message.empty() ? "SIPL capture failed" : failure_message); + } + + void init_cuda(); + void init_nvsci(); + void configure(); + void allocate_buffers(Sensor& sensor); + void allocate_icp_buffers(Sensor& sensor); + void register_buffers(Sensor& sensor); + void allocate_sync(Sensor& sensor); + void register_sync(Sensor& sensor); + void register_nito(Sensor& sensor); + void frame_loop(Sensor& sensor); + void event_loop(Sensor& sensor); + void convert(Sensor& sensor, const Slot& slot, uint32_t write_idx); + void publish(Sensor& sensor, uint32_t write_idx, uint64_t timestamp_ns, uint64_t unix_stamp_ns, uint64_t capture_tsc); + uint32_t pick_write_index(const Sensor& sensor) const; + void cleanup(); +}; + +void SiplCamera::Impl::init_cuda() +{ + check_cuda(cuInit(0), "cuInit"); + check_cuda(cuDeviceGet(&cu_device, config.gpu_id), "cuDeviceGet"); + check_cuda(cuDevicePrimaryCtxRetain(&cu_context, cu_device), "cuDevicePrimaryCtxRetain"); + cu_context_retained = true; + check_cuda(cuCtxSetCurrent(cu_context), "cuCtxSetCurrent"); + check_cuda(cuStreamCreate(&convert_stream, CU_STREAM_NON_BLOCKING), "cuStreamCreate"); +} + +void SiplCamera::Impl::init_nvsci() +{ + check_sci(NvSciBufModuleOpen(&sci_buf_module), "NvSciBufModuleOpen"); + check_sci(NvSciSyncModuleOpen(&sci_sync_module), "NvSciSyncModuleOpen"); + check_sci(NvSciSyncCpuWaitContextAlloc(sci_sync_module, &cpu_wait_context), "NvSciSyncCpuWaitContextAlloc"); +} + +void SiplCamera::Impl::configure() +{ + init_sipl_trace(); + query_api = INvSIPLCameraQuery::GetInstance(); + if (!query_api) + { + throw std::runtime_error("INvSIPLCameraQuery::GetInstance returned null"); + } + // Loads the UDDF driver .so libs out of /usr/lib/nvsipl_drv. + check_sipl(query_api->ParseDatabase(), "ParseDatabase"); + check_sipl(query_api->ParseJsonFile(config.platform_config_json), "ParseJsonFile"); + + system_config = std::make_unique(); + check_sipl(query_api->GetSensorSystemConfig(config.platform_config_name, *system_config), "GetSensorSystemConfig"); + if (config.link_masks.empty()) + { + throw std::runtime_error("link masks are required for a GMSL platform config"); + } + check_sipl(query_api->ApplyMask(*system_config, config.link_masks), "ApplyMask"); + if (system_config->modules.empty()) + { + throw std::runtime_error("no modules left in '" + config.platform_config_name + + "' after applying the link masks -- check the mask against the config"); + } + + sensor_infos = flatten(*system_config); + if (sensor_infos.empty()) + { + throw std::runtime_error("platform config '" + config.platform_config_name + "' declares no sensors"); + } + + camera = INvSIPLCamera::GetInstance(); + if (!camera) + { + throw std::runtime_error("INvSIPLCamera::GetInstance returned null"); + } + check_sipl(camera->SetPlatformCfg(*system_config), "SetPlatformCfg"); + + for (const auto& info : sensor_infos) + { + auto sensor = std::make_unique(); + sensor->info = info; + // ISP0 only. ICP would double the capture bandwidth for a raw stream + // nothing here consumes, and ISP1/ISP2 are a downscale we do not want. + sensor->pipeline_config.captureOutputRequested = false; + sensor->pipeline_config.isp0OutputRequested = true; + sensor->pipeline_config.isp1OutputRequested = false; + sensor->pipeline_config.isp2OutputRequested = false; + // Defaults to false, i.e. subframe ENABLED, which needs a sliceCount we + // do not set. NVIDIA's own samples disable it unless asked for. + sensor->pipeline_config.disableSubframe = true; + sensor->pipeline_config.bufferCfg.maxIsp0BufferCount = config.isp0_buffers; + check_sipl(camera->SetPipelineCfg(info.id, sensor->pipeline_config, sensor->queues), "SetPipelineCfg"); + sensors.push_back(std::move(sensor)); + } + + // Order is not ours to choose. Everything that *describes* a pipeline -- + // GetImageAttributes and FillNvSciSyncAttrList -- must happen before Init(); + // everything that *binds* a resource to it -- RegisterImages and + // RegisterNvSciSyncObj -- must happen after. Get it wrong and Init() + // still returns OK, but the pipeline is left in an error state and the + // first Register* call returns NVSIPL_STATUS_INVALID_STATE with no clue + // why. Verified against a -v 4 trace of NVIDIA's own nvsipl_camera. + for (auto& sensor : sensors) + { + allocate_icp_buffers(*sensor); + allocate_buffers(*sensor); + allocate_sync(*sensor); + } + + check_sipl(camera->Init(), "INvSIPLCamera::Init"); + + for (auto& sensor : sensors) + { + register_buffers(*sensor); + register_sync(*sensor); + register_nito(*sensor); + } +} + +void SiplCamera::Impl::allocate_buffers(Sensor& sensor) +{ + NvSciBufAttrList sipl_attrs = nullptr; + NvSciBufAttrList nvbuf_attrs = nullptr; + NvSciBufAttrList conflict = nullptr; + check_sci(NvSciBufAttrListCreate(sci_buf_module, &sipl_attrs), "NvSciBufAttrListCreate sipl"); + check_sci(NvSciBufAttrListCreate(sci_buf_module, &nvbuf_attrs), "NvSciBufAttrListCreate nvbuf"); + check_sci(set_sipl_buf_attributes(sipl_attrs), "NvSciBufAttrListSetAttrs"); + check_sipl(camera->GetImageAttributes(sensor.info.id, kIsp0, sipl_attrs), "GetImageAttributes"); + + // Both consumers must be described before allocation: SIPL writes the + // surface, NvBufSurface reads it on the way to CUDA. Reconciling the SIPL + // list alone yields a buffer RegisterImages will not accept. + check_sci(set_nvbuf_attributes(nvbuf_attrs, sipl_attrs, config.gpu_id), "set_nvbuf_attributes"); + + NvSciBufAttrList unreconciled[] = { sipl_attrs, nvbuf_attrs }; + check_sci(NvSciBufAttrListReconcile(unreconciled, 2U, &sensor.buf_attrs, &conflict), "NvSciBufAttrListReconcile"); + NvSciBufAttrListFree(sipl_attrs); + NvSciBufAttrListFree(nvbuf_attrs); + if (conflict) + { + NvSciBufAttrListFree(conflict); + } + sensor.full_range = check_color_std(sensor.buf_attrs); + + sensor.slots.resize(config.isp0_buffers); + for (uint32_t i = 0; i < config.isp0_buffers; ++i) + { + Slot& slot = sensor.slots[i]; + check_sci(NvSciBufObjAlloc(sensor.buf_attrs, &slot.buf), "NvSciBufObjAlloc"); + sensor.buf_objects.push_back(slot.buf); + + if (NvmmNvSciBufToNvBufSurface(slot.buf, &slot.surface) != 0 || slot.surface == nullptr) + { + throw std::runtime_error("NvmmNvSciBufToNvBufSurface failed"); + } + slot.surface->numFilled = 1; + } + + // RGBA8 destination slots. Allocated from the reported geometry, never from + // a CLI flag -- see "Geometry comes from the query" in the plan. + for (auto& dest : sensor.buffers) + { + void* ptr = nullptr; + size_t pitch = 0; + check_runtime(cudaMallocPitch(&ptr, &pitch, static_cast(sensor.info.width) * 4, sensor.info.height), + "cudaMallocPitch"); + dest.ptr = static_cast(ptr); + dest.pitch = pitch; + } +} + +void SiplCamera::Impl::allocate_icp_buffers(Sensor& sensor) +{ + NvSciBufAttrList attrs = nullptr; + NvSciBufAttrList conflict = nullptr; + check_sci(NvSciBufAttrListCreate(sci_buf_module, &attrs), "NvSciBufAttrListCreate icp"); + + // Only the type and permissions: GetImageAttributes fills the raw format in. + NvSciBufType buf_type = NvSciBufType_Image; + NvSciBufAttrValAccessPerm perm = NvSciBufAccessPerm_ReadWrite; + NvSciBufAttrKeyValuePair kvp[] = { + { NvSciBufGeneralAttrKey_Types, &buf_type, sizeof(buf_type) }, + { NvSciBufGeneralAttrKey_RequiredPerm, &perm, sizeof(perm) }, + }; + check_sci(NvSciBufAttrListSetAttrs(attrs, kvp, 2), "NvSciBufAttrListSetAttrs icp"); + check_sipl(camera->GetImageAttributes(sensor.info.id, kIcp, attrs), "GetImageAttributes ICP"); + + NvSciBufAttrList unreconciled[] = { attrs }; + check_sci(NvSciBufAttrListReconcile(unreconciled, 1U, &sensor.icp_attrs, &conflict), "NvSciBufAttrListReconcile icp"); + NvSciBufAttrListFree(attrs); + if (conflict) + { + NvSciBufAttrListFree(conflict); + } + + for (uint32_t i = 0; i < kIcpBuffers; ++i) + { + NvSciBufObj obj = nullptr; + check_sci(NvSciBufObjAlloc(sensor.icp_attrs, &obj), "NvSciBufObjAlloc icp"); + sensor.icp_buf_objects.push_back(obj); + } +} + +void SiplCamera::Impl::register_buffers(Sensor& sensor) +{ + check_sipl(camera->RegisterImages(sensor.info.id, kIcp, sensor.icp_buf_objects), "RegisterImages ICP"); + check_sipl(camera->RegisterImages(sensor.info.id, kIsp0, sensor.buf_objects), "RegisterImages ISP0"); + + // Import into CUDA only once SIPL owns the buffers. Doing it at allocation + // time hands SIPL a surface that already has an EGL image bound to it. + // Registering per frame instead would put an EGL round trip on the 60 Hz + // path for no benefit. + for (auto& slot : sensor.slots) + { + if (NvBufSurfaceMapEglImage(slot.surface, 0) != 0) + { + throw std::runtime_error("NvBufSurfaceMapEglImage failed"); + } + check_cuda(cuGraphicsEGLRegisterImage(&slot.resource, slot.surface->surfaceList[0].mappedAddr.eglImage, + CU_GRAPHICS_REGISTER_FLAGS_READ_ONLY), + "cuGraphicsEGLRegisterImage"); + check_cuda(cuGraphicsResourceGetMappedEglFrame(&slot.egl_frame, slot.resource, 0, 0), + "cuGraphicsResourceGetMappedEglFrame"); + } +} + +void SiplCamera::Impl::allocate_sync(Sensor& sensor) +{ + NvSciSyncAttrList signaler = nullptr; + NvSciSyncAttrList waiter = nullptr; + NvSciSyncAttrList reconciled = nullptr; + NvSciSyncAttrList conflict = nullptr; + + check_sci(NvSciSyncAttrListCreate(sci_sync_module, &signaler), "NvSciSyncAttrListCreate signaler"); + check_sci(NvSciSyncAttrListCreate(sci_sync_module, &waiter), "NvSciSyncAttrListCreate waiter"); + + // We wait on the CPU before handing the surface to the conversion kernel. + NvSciSyncAccessPerm perm = NvSciSyncAccessPerm_WaitOnly; + bool cpu_waiter = true; + NvSciSyncAttrKeyValuePair kvp[] = { + { NvSciSyncAttrKey_NeedCpuAccess, &cpu_waiter, sizeof(cpu_waiter) }, + { NvSciSyncAttrKey_RequiredPerm, &perm, sizeof(perm) }, + }; + check_sci(NvSciSyncAttrListSetAttrs(waiter, kvp, 2), "NvSciSyncAttrListSetAttrs"); + check_sipl( + camera->FillNvSciSyncAttrList(sensor.info.id, kIsp0, signaler, nvsipl::SIPL_SIGNALER), "FillNvSciSyncAttrList"); + + NvSciSyncAttrList unreconciled[] = { signaler, waiter }; + check_sci(NvSciSyncAttrListReconcile(unreconciled, 2U, &reconciled, &conflict), "NvSciSyncAttrListReconcile"); + check_sci(NvSciSyncObjAlloc(reconciled, &sensor.sync_obj), "NvSciSyncObjAlloc"); + NvSciSyncAttrListFree(signaler); + NvSciSyncAttrListFree(waiter); + if (reconciled) + { + NvSciSyncAttrListFree(reconciled); + } + if (conflict) + { + NvSciSyncAttrListFree(conflict); + } +} + +void SiplCamera::Impl::register_sync(Sensor& sensor) +{ + check_sipl(camera->RegisterNvSciSyncObj(sensor.info.id, kIsp0, nvsipl::NVSIPL_EOFSYNCOBJ, sensor.sync_obj), + "RegisterNvSciSyncObj"); +} + +void SiplCamera::Impl::register_nito(Sensor& sensor) +{ + auto blob = load_nito(config.nito_dir, sensor.info.name); + nvsipl::ISiplControlAuto* auto_control = nullptr; + check_sipl(camera->RegisterAutoControlPlugin(sensor.info.id, nvsipl::NV_PLUGIN, auto_control, blob), + "RegisterAutoControlPlugin"); +} + +void SiplCamera::Impl::convert(Sensor& sensor, const Slot& slot, uint32_t write_idx) +{ + const CUeglFrame& frame = slot.egl_frame; + DeviceBuffer& dest = sensor.buffers[write_idx]; + + if (frame.planeCount < 2) + { + throw std::runtime_error("ISP0 surface has fewer than two planes"); + } + + // Requested SemiPlanar YUV, so this is NV12 unless swap_uv flips it, and + // REC709_ER, so the kernel gets bt709=true. + const YuvLayout layout = config.swap_uv ? YuvLayout::YVU420SemiPlanar : YuvLayout::YUV420SemiPlanar; + constexpr bool kBt709 = true; + + if (frame.frameType == CU_EGL_FRAME_TYPE_PITCH) + { + const auto* y_plane = static_cast(frame.frame.pPitch[0]); + const auto* uv_plane = static_cast(frame.frame.pPitch[1]); + launch_yuv420_pitch_to_rgba(y_plane, uv_plane, nullptr, static_cast(frame.pitch), + static_cast(frame.pitch), 0, sensor.info.width, sensor.info.height, dest.ptr, + static_cast(dest.pitch), layout, sensor.full_range, kBt709, + reinterpret_cast(convert_stream)); + } + else if (frame.frameType == CU_EGL_FRAME_TYPE_ARRAY) + { + // Block-linear: CUDA exposes the planes as arrays, so the kernel reads + // them through texture objects. Those are built per frame because the + // CUarray handles come from the per-frame CUeglFrame; the expensive + // part, cuGraphicsEGLRegisterImage, is done once per buffer. + cudaTextureObject_t y_tex = texture_for_array(frame.frame.pArray[0]); + cudaTextureObject_t uv_tex = texture_for_array(frame.frame.pArray[1]); + launch_yuv420_array_to_rgba(y_tex, uv_tex, 0, sensor.info.width, sensor.info.height, dest.ptr, + static_cast(dest.pitch), layout, sensor.full_range, kBt709, + reinterpret_cast(convert_stream)); + check_cuda(cuStreamSynchronize(convert_stream), "cuStreamSynchronize"); + check_runtime(cudaDestroyTextureObject(y_tex), "cudaDestroyTextureObject"); + check_runtime(cudaDestroyTextureObject(uv_tex), "cudaDestroyTextureObject"); + check_runtime(cudaGetLastError(), "SIPL YUV to RGBA kernel"); + return; + } + else + { + throw std::runtime_error("unsupported CUDA EGL frame type from ISP0"); + } + + check_cuda(cuStreamSynchronize(convert_stream), "cuStreamSynchronize"); + check_runtime(cudaGetLastError(), "SIPL YUV to RGBA kernel"); +} + +void SiplCamera::Impl::frame_loop(Sensor& sensor) +{ + try + { + check_cuda(cuCtxSetCurrent(cu_context), "cuCtxSetCurrent frame_loop"); + while (running.load()) + { + INvSIPLClient::INvSIPLBuffer* buffer = nullptr; + const SIPLStatus status = sensor.queues.isp0CompletionQueue->Get(buffer, config.frame_timeout_ms * 1000U); + if (status == nvsipl::NVSIPL_STATUS_TIMED_OUT) + { + continue; + } + if (status == nvsipl::NVSIPL_STATUS_EOF) + { + break; + } + check_sipl(status, "isp0CompletionQueue->Get"); + if (!buffer) + { + continue; + } + + auto* nvm = dynamic_cast(buffer); + if (!nvm) + { + buffer->Release(); + throw std::runtime_error("ISP0 buffer is not an INvSIPLNvMBuffer"); + } + + // The ISP is still writing until this fence clears. + NvSciSyncFence fence = NvSciSyncFenceInitializer; + check_sipl(nvm->GetEOFNvSciSyncFence(&fence), "GetEOFNvSciSyncFence"); + const NvSciError wait = NvSciSyncFenceWait(&fence, cpu_wait_context, config.frame_timeout_ms * 1000UL); + NvSciSyncFenceClear(&fence); + check_sci(wait, "NvSciSyncFenceWait"); + + const NvSciBufObj arrived = nvm->GetNvSciBufImage(); + const Slot* slot = nullptr; + for (const auto& candidate : sensor.slots) + { + if (candidate.buf == arrived) + { + slot = &candidate; + break; + } + } + if (!slot) + { + buffer->Release(); + throw std::runtime_error("ISP0 delivered a buffer this process never registered"); + } + + // The TSC is what makes the two eyes pairable; do not substitute a + // host clock, which carries this sensor's own queueing jitter. + const auto& meta = nvm->GetImageData(); + const uint64_t capture_tsc = meta.frameCaptureTSC; + // SENSING_TSC_PROBE=1 logs, twice a second, the capture TSC beside the host clocks read at dequeue. + // Uncalibrated samples: the counter's units are unconfirmed, so it is logged beside the host clocks and + // never subtracted from them. The environment is read once, so a disabled probe costs a branch per frame. + static const bool tsc_probe = std::getenv("SENSING_TSC_PROBE") != nullptr; + if (tsc_probe) + { + static thread_local uint64_t tsc_probe_n = 0; + if ((tsc_probe_n++ % 30) == 0) + { + const auto clock_ns = [](clockid_t id) + { + timespec ts{}; + clock_gettime(id, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000LL + ts.tv_nsec; + }; + const int64_t raw_ns = clock_ns(CLOCK_MONOTONIC_RAW); + isaaccapture::Logger::get("isaaccapture.plugins.sensing.SiplCamera") + ->info("[tsc] sensor {} tsc={} monotonic_raw_ns={} monotonic_ns={} realtime_ns={}", + sensor.info.id, capture_tsc, raw_ns, clock_ns(CLOCK_MONOTONIC), clock_ns(CLOCK_REALTIME)); + } + } + // SENSING_AE_PROBE=1 logs the sensor's exposure and gain once a + // second. AE failures are invisible in the frame rate and hard to + // judge by eye, and this is the only place the numbers surface. + if (std::getenv("SENSING_AE_PROBE") != nullptr) + { + static thread_local uint64_t probe_n = 0; + if ((probe_n++ % 60) == 0) + { + std::cerr << "[ae] sensor " << sensor.info.id << " numExp=" << meta.numExposures + << " expValid=" << static_cast(meta.sensorExpInfo.expTimeValid) + << " exp[0]=" << meta.sensorExpInfo.exposureTime[0] << "s" + << " gainValid=" << static_cast(meta.sensorExpInfo.gainValid) + << " gain[0]=" << meta.sensorExpInfo.sensorGain[0] << std::endl; + } + } + + const uint32_t write_idx = pick_write_index(sensor); + convert(sensor, *slot, write_idx); + if (std::getenv("SENSING_LUMA_PROBE") != nullptr) + { + static thread_local uint64_t luma_n = 0; + if ((luma_n++ % 60) == 0) + { + // Mean over one row out of every 64, straight off the RGBA + // the consumer receives -- the only number that says what + // the operator actually sees. + const auto& dst = sensor.buffers[write_idx]; + const uint32_t step = 64; + std::vector row(dst.pitch); + double sum = 0.0; + uint64_t n = 0; + for (uint32_t y = 0; y < sensor.info.height; y += step) + { + if (cudaMemcpy(row.data(), dst.ptr + static_cast(y) * dst.pitch, dst.pitch, + cudaMemcpyDeviceToHost) != cudaSuccess) + break; + for (uint32_t x = 0; x < sensor.info.width; ++x) + { + sum += 0.2126 * row[x * 4 + 0] + 0.7152 * row[x * 4 + 1] + 0.0722 * row[x * 4 + 2]; + ++n; + } + } + if (n) + std::cerr << "[luma] sensor " << sensor.info.id << " mean=" << (sum / n) << "/255" << std::endl; + } + } + buffer->Release(); + // Both clocks are read back to back, so the pair names one instant. + const uint64_t stamp_ns = monotonic_ns(); + publish(sensor, write_idx, stamp_ns, unix_ns(), capture_tsc); + } + } + catch (const std::exception& e) + { + std::ostringstream oss; + oss << "SIPL sensor " << sensor.info.id << " capture error: " << e.what(); + std::cerr << "[sipl] " << oss.str() << std::endl; + set_failure(oss.str()); + running.store(false); + } +} + +namespace +{ + +/// What a pipeline or device-block notification means for streaming. Values are those of +/// NvSIPLPipelineNotifier::NotificationType in the SIPL reference; the four device-block errors that were already +/// matched by name keep their enumerators, the rest are matched by value so an older header still compiles. +struct NotificationKind +{ + bool fatal = false; + const char* text = nullptr; ///< words the producer's fault table recognises; null for notifications to ignore +}; + +NotificationKind describe_notification(int type) +{ + switch (type) + { + case 200: + return { true, "camera input stream failure (bad input stream)" }; + case 201: + return { true, "camera input stream failure (capture failure)" }; + case 203: + return { true, "image processing failure" }; + case 204: + return { true, "auto control processing failure" }; + case 220: + case 221: + return { true, "interrupt localization failure" }; + case 202: + return { false, "embedded data parse failure" }; + case 205: + return { false, "sensor control write failure" }; + case 210: + return { false, "recoverable image processing failure" }; + case 100: + return { false, "frame dropped" }; + case 101: + return { false, "frame sequence discontinuity" }; + case 102: + return { false, "capture timeout" }; + default: + return {}; + } +} + +} // namespace + +void SiplCamera::Impl::event_loop(Sensor& sensor) +{ + // A GMSL link drop arrives here and nowhere else. Without this drain the + // symptom is a silent stall on the frame queue. + while (running.load()) + { + nvsipl::NvSIPLPipelineNotifier::NotificationData data{}; + const SIPLStatus status = sensor.queues.notificationQueue->Get(data, config.frame_timeout_ms * 1000U); + if (status != NVSIPL_STATUS_OK) + { + continue; + } + using Notif = nvsipl::NvSIPLPipelineNotifier; + switch (data.eNotifType) + { + case Notif::NOTIF_ERROR_DESERIALIZER_FAILURE: + case Notif::NOTIF_ERROR_SERIALIZER_FAILURE: + case Notif::NOTIF_ERROR_SENSOR_FAILURE: + case Notif::NOTIF_ERROR_INTERNAL_FAILURE: + { + // The kind is named in words the producer's fault table recognises: a lost link and a failing sensor + // call for different checks. + const char* kind = data.eNotifType == Notif::NOTIF_ERROR_DESERIALIZER_FAILURE ? "deserializer failure" : + data.eNotifType == Notif::NOTIF_ERROR_SERIALIZER_FAILURE ? "serializer failure" : + data.eNotifType == Notif::NOTIF_ERROR_SENSOR_FAILURE ? "sensor failure" : + "internal failure"; + std::ostringstream oss; + oss << "SIPL sensor " << sensor.info.id << " pipeline error: " << kind << " (notification " + << static_cast(data.eNotifType) << ")"; + std::cerr << "[sipl] " << oss.str() << std::endl; + set_failure(oss.str()); + running.store(false); + break; + } + default: + { + const int type = static_cast(data.eNotifType); + if (type == 0) // ICP processing done: the camera is delivering + { + sensor.consecutive_capture_timeouts = 0; + sensor.consecutive_control_failures = 0; + break; + } + const NotificationKind kind = describe_notification(type); + if (kind.text == nullptr) + break; + std::ostringstream oss; + oss << "SIPL sensor " << sensor.info.id << ": " << kind.text << " (notification " << type << ")"; + if (kind.fatal) + { + std::cerr << "[sipl] " << oss.str() << std::endl; + set_failure(oss.str()); + running.store(false); + break; + } + // A warning once is routine; the same one repeating means the camera has stopped delivering. + if (type == 102 && ++sensor.consecutive_capture_timeouts >= 3) + { + std::ostringstream failure; + failure << "SIPL sensor " << sensor.info.id + << ": no frame for 3 capture timeouts in a row (the camera stopped sending)"; + std::cerr << "[sipl] " << failure.str() << std::endl; + set_failure(failure.str()); + running.store(false); + } + else if (type == 205 && ++sensor.consecutive_control_failures >= 10) + { + std::ostringstream failure; + failure << "SIPL sensor " << sensor.info.id + << ": sensor control write failure repeated 10 times (its control bus is not answering)"; + std::cerr << "[sipl] " << failure.str() << std::endl; + set_failure(failure.str()); + running.store(false); + } + break; + } + } + } +} + +void SiplCamera::Impl::publish( + Sensor& sensor, uint32_t write_idx, uint64_t timestamp_ns, uint64_t unix_stamp_ns, uint64_t capture_tsc) +{ + std::lock_guard guard(sensor.publish_mutex); + sensor.publish_idx = static_cast(write_idx); + sensor.published_timestamp_ns = timestamp_ns; + sensor.published_unix_ns = unix_stamp_ns; + sensor.published_capture_tsc = capture_tsc; + ++sensor.published_sequence; +} + +uint32_t SiplCamera::Impl::pick_write_index(const Sensor& sensor) const +{ + std::lock_guard guard(sensor.publish_mutex); + if (sensor.publish_idx < 0) + { + return 0; + } + // Skip the published slot and any slot still leased to a reader. With three + // slots one is always free, so this cannot spin. + uint32_t idx = static_cast((sensor.publish_idx + 1) % 3); + if (static_cast(idx) == sensor.lease_idx) + { + idx = (idx + 1) % 3; + } + return idx; +} + +void SiplCamera::Impl::cleanup() +{ + running.store(false); + for (auto& sensor : sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + if (camera) + { + camera->Stop(); + camera->Deinit(); + } + + for (auto& sensor : sensors) + { + for (auto& slot : sensor->slots) + { + if (slot.resource) + { + cuGraphicsUnregisterResource(slot.resource); + } + if (slot.surface) + { + NvBufSurfaceUnMapEglImage(slot.surface, 0); + NvBufSurfaceDestroy(slot.surface); + } + if (slot.buf) + { + NvSciBufObjFree(slot.buf); + } + } + sensor->slots.clear(); + sensor->buf_objects.clear(); + for (auto& obj : sensor->icp_buf_objects) + { + NvSciBufObjFree(obj); + } + sensor->icp_buf_objects.clear(); + if (sensor->icp_attrs) + { + NvSciBufAttrListFree(sensor->icp_attrs); + sensor->icp_attrs = nullptr; + } + if (sensor->buf_attrs) + { + NvSciBufAttrListFree(sensor->buf_attrs); + sensor->buf_attrs = nullptr; + } + if (sensor->sync_obj) + { + NvSciSyncObjFree(sensor->sync_obj); + sensor->sync_obj = nullptr; + } + for (auto& dest : sensor->buffers) + { + if (dest.ptr) + { + cudaFree(dest.ptr); + dest.ptr = nullptr; + } + } + } + + camera.reset(); + query_api.reset(); + + if (cpu_wait_context) + { + NvSciSyncCpuWaitContextFree(cpu_wait_context); + cpu_wait_context = nullptr; + } + if (sci_sync_module) + { + NvSciSyncModuleClose(sci_sync_module); + sci_sync_module = nullptr; + } + if (sci_buf_module) + { + NvSciBufModuleClose(sci_buf_module); + sci_buf_module = nullptr; + } + if (convert_stream) + { + cuStreamDestroy(convert_stream); + convert_stream = nullptr; + } + if (cu_context_retained) + { + cuDevicePrimaryCtxRelease(cu_device); + cu_context_retained = false; + cu_context = nullptr; + } +} + +// ============================================================================= +// SiplCamera +// ============================================================================= + +SiplCamera::SiplCamera(const SiplConfig& config) : m_impl(std::make_unique(config)) +{ + if (config.platform_config_json.empty() || config.platform_config_name.empty()) + { + throw std::invalid_argument("SiplConfig needs both platform_config_json and platform_config_name"); + } + try + { + m_impl->init_cuda(); + m_impl->init_nvsci(); + m_impl->configure(); + } + catch (...) + { + m_impl->cleanup(); + throw; + } +} + +SiplCamera::~SiplCamera() +{ + m_impl->cleanup(); +} + +std::vector SiplCamera::query(const std::string& platform_config_json, + const std::string& platform_config_name, + const std::vector& link_masks) +{ + auto api = INvSIPLCameraQuery::GetInstance(); + if (!api) + { + throw std::runtime_error("INvSIPLCameraQuery::GetInstance returned null"); + } + check_sipl(api->ParseDatabase(), "ParseDatabase"); + check_sipl(api->ParseJsonFile(platform_config_json), "ParseJsonFile"); + + nvsipl::sensorconfig::SensorSystemConfig cfg; + check_sipl(api->GetSensorSystemConfig(platform_config_name, cfg), "GetSensorSystemConfig"); + if (!link_masks.empty()) + { + check_sipl(api->ApplyMask(cfg, link_masks), "ApplyMask"); + } + return flatten(cfg); +} + +const std::vector& SiplCamera::sensors() const +{ + return m_impl->sensor_infos; +} + +void SiplCamera::start() +{ + if (m_impl->running.load()) + { + return; + } + m_impl->running.store(true); + for (auto& sensor : m_impl->sensors) + { + sensor->frame_thread = std::thread([this, s = sensor.get()] { m_impl->frame_loop(*s); }); + sensor->event_thread = std::thread([this, s = sensor.get()] { m_impl->event_loop(*s); }); + } + try + { + check_sipl(m_impl->camera->Start(), "INvSIPLCamera::Start"); + } + catch (...) + { + m_impl->running.store(false); + for (auto& sensor : m_impl->sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + throw; + } +} + +void SiplCamera::stop() +{ + m_impl->running.store(false); + for (auto& sensor : m_impl->sensors) + { + if (sensor->frame_thread.joinable()) + { + sensor->frame_thread.join(); + } + if (sensor->event_thread.joinable()) + { + sensor->event_thread.join(); + } + } + if (m_impl->camera) + { + m_impl->camera->Stop(); + } +} + +std::optional SiplCamera::latest(uint32_t sensor_id) +{ + m_impl->throw_if_failed(); + Impl::Sensor* sensor = m_impl->find(sensor_id); + if (!sensor) + { + throw std::runtime_error("no SIPL pipeline with index " + std::to_string(sensor_id)); + } + + std::lock_guard guard(sensor->publish_mutex); + if (sensor->publish_idx < 0 || sensor->consumed_sequence == sensor->published_sequence) + { + return std::nullopt; + } + sensor->consumed_sequence = sensor->published_sequence; + + // The caller now holds this slot; pick_write_index() skips it until the + // next latest() moves the lease on. + sensor->lease_idx = sensor->publish_idx; + + FrameView view; + view.ptr = reinterpret_cast(sensor->buffers[sensor->publish_idx].ptr); + view.pitch = sensor->buffers[sensor->publish_idx].pitch; + view.width = sensor->info.width; + view.height = sensor->info.height; + view.timestamp_ns = sensor->published_timestamp_ns; + view.unix_ns = sensor->published_unix_ns; + view.capture_tsc = sensor->published_capture_tsc; + view.sequence = sensor->published_sequence; + return view; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_camera.hpp b/src/plugins/sensing/core/sipl_camera.hpp new file mode 100644 index 0000000000..52e645f52c --- /dev/null +++ b/src/plugins/sensing/core/sipl_camera.hpp @@ -0,0 +1,119 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// SIPL capture for the SENSING SG8A carrier: every sensor in one platform +// config, ISP0 YUV out, converted to RGBA8 on the GPU. +// +// One instance owns every sensor. This is not a style choice -- +// INvSIPLCamera::GetInstance() hands out a single process-wide object, so a +// per-sensor camera class is not expressible. Do not reintroduce one. +// +// The SIPL and NvSci headers are pimpl'd away because they drag in NvMedia and +// X11-style macros (Success, Status, None) that collide with other libraries'; keeping them out of +// this header is what lets other files include it freely. + +#pragma once + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct SiplConfig +{ + /// Vendor platform config, e.g. /query/sg8a_agth_g2a/shw5g.json. + std::string platform_config_json; + /// Named config inside it, e.g. "SHW5G_2". + std::string platform_config_name; + /// One mask per deserializer, in transport order. Required for GMSL. + std::vector link_masks; + /// Directory holding .nito; the ISP has no tuning without it. + std::string nito_dir = "/var/nvidia/nvcam/settings/sipl"; + int gpu_id = 0; + /// Overrides the range implied by the reconciled colour standard. + bool full_range = false; + /// Swap the chroma planes. Defaults off: the ISP0 surface order is + /// requested explicitly, so it does not need discovering from the frame. + bool swap_uv = false; + /// ISP0 buffers per sensor. Six is the SIPL default; more absorbs a slow + /// consumer at the cost of 6 MB each at 2560x1984 NV12. + uint32_t isp0_buffers = 6; + uint32_t frame_timeout_ms = 1000; +}; + +/// One sensor as the platform config describes it. `id` is the SIPL pipeline +/// index, which is NOT the GMSL link index, the CSI virtual channel, or the +/// JSON sensorInfo.id -- for SHW5G_2 those are 2 and 3 while this is 0 and 1. +struct SensorInfo +{ + uint32_t id = 0; + std::string name; + uint32_t width = 0; + uint32_t height = 0; + double fps = 0.0; +}; + +/** + * @brief Borrowed view of the most recently converted frame. + * + * `ptr` addresses one of three producer-owned slots and stays valid until the + * second subsequent latest() call for the same sensor, at which point the + * producer may overwrite it. Copy out before then; do not cache a FrameView + * across update() ticks. + */ +struct FrameView +{ + uintptr_t ptr = 0; + size_t pitch = 0; + uint32_t width = 0; + uint32_t height = 0; + /// CLOCK_MONOTONIC, stamped at YUV->RGBA conversion. Published as the frame's + /// transport timestamp; the encoder does not return it, so units pair with captures by order. + uint64_t timestamp_ns = 0; + /// CLOCK_REALTIME (Unix time) read at the same instant as timestamp_ns: processing time, after capture and + /// conversion, so later than the capture event. Published as the frame's application timestamp; it is + /// only as accurate as the two hosts' clock synchronisation and is not a capture time. + uint64_t unix_ns = 0; + /// SIPL's raw frame capture counter, shared by every sensor on the rig, so it pairs the two eyes. Its units + /// are unscaled ticks until the SIPL headers confirm otherwise; it is not nanoseconds or a host time. + uint64_t capture_tsc = 0; + uint64_t sequence = 0; +}; + +class SiplCamera +{ +public: + explicit SiplCamera(const SiplConfig& config); + ~SiplCamera(); + + SiplCamera(const SiplCamera&) = delete; + SiplCamera& operator=(const SiplCamera&) = delete; + + /// Enumerate a platform config without touching the hardware. Cheap: the + /// query API only parses the driver database and the JSON. + static std::vector query(const std::string& platform_config_json, + const std::string& platform_config_name, + const std::vector& link_masks); + + /// Sensors this instance configured, in pipeline-index order. + const std::vector& sensors() const; + + void start(); + void stop(); + + /// Non-blocking mailbox read. nullopt when no new frame has arrived. + std::optional latest(uint32_t sensor_id); + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_capture.cpp b/src/plugins/sensing/core/sipl_capture.cpp new file mode 100644 index 0000000000..38d6fff994 --- /dev/null +++ b/src/plugins/sensing/core/sipl_capture.cpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "sipl_capture.hpp" + +namespace plugins +{ +namespace sensing +{ + +SiplCapture::SiplCapture(const SiplConfig& config) : m_camera(config) +{ + for (const auto& sensor : m_camera.sensors()) + m_cameras.push_back({ sensor.id, sensor.name, sensor.width, sensor.height, sensor.fps }); +} + +const std::vector& SiplCapture::cameras() const +{ + return m_cameras; +} + +void SiplCapture::start() +{ + m_camera.start(); +} + +void SiplCapture::stop() +{ + m_camera.stop(); +} + +std::optional SiplCapture::acquire_latest(uint32_t camera_id) +{ + const auto view = m_camera.latest(camera_id); + if (!view) + return std::nullopt; + + CaptureFrame frame; + frame.camera_id = camera_id; + frame.sequence = view->sequence; + frame.width = view->width; + frame.height = view->height; + frame.monotonic_ns = view->timestamp_ns; + frame.unix_ns = view->unix_ns; + frame.sensor_time = view->capture_tsc; + frame.sensor_domain = SensorTimeDomain::GenericCounterTicks; + frame.pixels = CudaRgbaPixels{ view->ptr, view->pitch }; + return frame; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/sipl_capture.hpp b/src/plugins/sensing/core/sipl_capture.hpp new file mode 100644 index 0000000000..dd4f4a86db --- /dev/null +++ b/src/plugins/sensing/core/sipl_capture.hpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// The SIPL backend behind the sender's capture interface. A thin adapter over SiplCamera: frames are the +// borrowed CUDA RGBA views SiplCamera already produces, passed on without a copy. + +#pragma once + +#include "camera_capture.hpp" +#include "sipl_camera.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +class SiplCapture : public CameraCapture +{ +public: + explicit SiplCapture(const SiplConfig& config); + + const std::vector& cameras() const override; + void start() override; + void stop() override; + std::optional acquire_latest(uint32_t camera_id) override; + +private: + SiplCamera m_camera; + std::vector m_cameras; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/video_output.cpp b/src/plugins/sensing/core/video_output.cpp new file mode 100644 index 0000000000..47562a368c --- /dev/null +++ b/src/plugins/sensing/core/video_output.cpp @@ -0,0 +1,666 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "video_output.hpp" + +#include "jetson_encoder.hpp" +#include "rgba_box_downscale.cuh" +#include "sensor_clock.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +void check_cuda(cudaError_t err, const char* what) +{ + if (err != cudaSuccess) + throw std::runtime_error(std::string("Video output: ") + what + ": " + cudaGetErrorString(err)); +} + +// Congestion control never drives the encoder below this. +constexpr uint32_t kMinBitrateBps = 100'000; +/// Forced keyframes (the PC asking, or a dropped link recovering) are at least this far apart. +constexpr std::chrono::milliseconds kMinIdrInterval{ 1000 }; +/// How often the link is judged, how far the bitrate falls back, and how long it must stay calm before it rises. +constexpr std::chrono::milliseconds kAdaptInterval{ 1000 }; +constexpr uint32_t kFloorBitrateBps = 1'500'000; +constexpr uint32_t kCalmBeforeRaise = 5; +// A target is applied only when it differs from the current one by this fraction, and this long after +// the last change. + +} // namespace + +struct VideoOutput::Impl +{ + struct Slot + { + uint8_t* ptr = nullptr; + size_t pitch = 0; + uint64_t timestamp_ns = 0; + uint64_t capture_monotonic_ns = 0; + uint64_t capture_tsc = 0; + uint64_t sequence = 0; + }; + + explicit Impl(const VideoOutputConfig& cfg) + : config(cfg), + correlator(cfg.correlation == FrameCorrelation::Stamp ? video_stage::FrameCorrelator::Mode::Stamp : + video_stage::FrameCorrelator::Mode::Fifo) + { + validate_geometry(config.source_width, config.source_height, config.scale_div); + if (config.fps == 0) + throw std::runtime_error("Video output: the publication rate must be at least 1 fps"); + out_width = config.source_width / config.scale_div; + out_height = config.source_height / config.scale_div; + if (config.input == PixelKind::DmaBufNv12 && config.scale_div != 1) + throw std::runtime_error("Video output: DMA-BUF input cannot be reduced (scale_div must be 1)"); + period_ns = 1'000'000'000ull / config.fps; + // Always present: with no host the sender idles, and a management client can give it one at runtime. + { + DirectSenderConfig direct_config; + direct_config.host = config.direct_host; + direct_config.port = config.direct_port; + direct_config.width = out_width; + direct_config.height = out_height; + direct_config.fps = config.fps; + direct = std::make_unique(direct_config); + } + if (config.bitrate_bps < kMinBitrateBps) + { + throw std::runtime_error("Video output: the bitrate must be at least " + std::to_string(kMinBitrateBps) + + " bps"); + } + // Congestion control moves the target between the floor and the ceiling; the configured target + // is never below the floor, so the range is never inverted. + + // A constructor that throws never runs the destructor, so free what was allocated here. + try + { + check_cuda(cudaSetDevice(config.gpu_id), "cudaSetDevice"); + + // First, so a machine without a usable hardware encoder fails before anything is allocated. + EncoderConfig encoder_config; + encoder_config.width = out_width; + encoder_config.height = out_height; + encoder_config.bitrate_bps = config.bitrate_bps; + current_bitrate = config.bitrate_bps; + encoder_config.peak_bitrate_bps = config.peak_bitrate_bps; + encoder_config.fps = config.fps; + encoder_config.gop = config.gop; + encoder_config.full_range = config.full_range; + encoder_config.input = config.input; + encoder = std::make_unique(encoder_config); + + for (auto& slot : slots) + { + if (config.input != PixelKind::CudaRgba) + break; // DMA-BUF frames are not copied + check_cuda(cudaMallocPitch(reinterpret_cast(&slot.ptr), &slot.pitch, + static_cast(config.source_width) * 4, config.source_height), + "cudaMallocPitch(snapshot)"); + } + if (config.scale_div > 1) + { + check_cuda(cudaMallocPitch(reinterpret_cast(&d_scaled), &scaled_pitch, + static_cast(out_width) * 4, out_height), + "cudaMallocPitch(reduced)"); + } + check_cuda(cudaStreamCreateWithFlags(&snapshot_stream, cudaStreamNonBlocking), "cudaStreamCreate"); + check_cuda(cudaStreamCreateWithFlags(&work_stream, cudaStreamNonBlocking), "cudaStreamCreate"); + + worker = std::thread([this] { run(); }); + } + catch (...) + { + release(); + throw; + } + } + + ~Impl() + { + { + std::lock_guard lock(mutex); + stopping = true; + } + wake.notify_all(); + if (worker.joinable()) + worker.join(); + release(); + } + + void release() + { + for (auto& slot : slots) + cudaFree(slot.ptr); + cudaFree(d_scaled); + if (snapshot_stream) + cudaStreamDestroy(snapshot_stream); + if (work_stream) + cudaStreamDestroy(work_stream); + } + + void fail(const std::string& message) + { + std::lock_guard lock(failure_mutex); + if (!failed_flag.load()) + { + failure_message = message; + failed_flag.store(true); + } + } + + // Polling thread. + void submit(CaptureFrame& frame) + { + const bool dma = config.input == PixelKind::DmaBufNv12; + const auto* rgba = std::get_if(&frame.pixels); + if (dma ? !std::holds_alternative(frame.pixels) : rgba == nullptr) + { + fail("Video output: the frame's pixel type does not match the encoder input"); + return; + } + ++offered; + if (failed_flag.load()) + return; + + // Pace on the clock the frame is stamped with: the sensor's capture time when that is the published basis, + // otherwise the time the camera backend handed the frame over. Either way it is not the poll time, so poll + // jitter does not drop a frame that is due. + uint64_t pace_ns = frame.monotonic_ns; + if (config.timestamp_basis == TimestampBasis::SensorCaptureMonotonic) + { + try + { + frame.capture_monotonic_ns = capture_time_ns(frame); + } + catch (const std::exception& e) + { + fail(e.what()); + return; + } + pace_ns = frame.capture_monotonic_ns; + } + // A clock that steps back would otherwise hold every frame until it caught up: restart pacing instead. + if (have_last && pace_ns < last_accepted_ns) + have_last = false; + // A tenth of a period of slack lets 60 fps capture feed a 30 fps track every second capture. + if (have_last && pace_ns < last_accepted_ns + period_ns * 9 / 10) + { + ++paced_out; + return; + } + + if (dma) + { + const uint64_t accepted_ns = pace_ns; + // A frame still waiting is replaced by this newer one. Its lease is released after the lock is dropped: + // that is a camera API call, and must not run inside the section the worker waits on. + std::optional replaced_frame; + { + std::lock_guard lock(mutex); + if (pending_dma) + ++replaced; + replaced_frame = std::move(pending_dma); + pending_dma = std::move(frame); + } + replaced_frame.reset(); + ++snapshots; + have_last = true; + last_accepted_ns = accepted_ns; + wake.notify_one(); + return; + } + + try + { + std::lock_guard lock(mutex); + int target = -1; + for (int i = 0; i < 2; ++i) + { + if (i != inflight && i != pending) + { + target = i; + break; + } + } + if (target < 0) + { + // The worker holds one slot and the other waits: the newer capture takes the waiting one. + target = pending; + } + // A capture still waiting is overwritten by this newer one. + if (pending >= 0) + ++replaced; + + // Complete before returning: the camera may rewrite its slot at the second following poll. + Slot& slot = slots[target]; + check_cuda(cudaMemcpy2DAsync(slot.ptr, slot.pitch, reinterpret_cast(rgba->device_ptr), + rgba->pitch_bytes, static_cast(config.source_width) * 4, + config.source_height, cudaMemcpyDeviceToDevice, snapshot_stream), + "snapshot copy"); + check_cuda(cudaStreamSynchronize(snapshot_stream), "snapshot sync"); + + slot.timestamp_ns = frame.monotonic_ns; + slot.capture_monotonic_ns = frame.capture_monotonic_ns; + slot.capture_tsc = frame.sensor_time; + slot.sequence = frame.sequence; + pending = target; + } + catch (const std::exception& e) + { + fail(e.what()); + return; + } + ++snapshots; + have_last = true; + last_accepted_ns = pace_ns; + wake.notify_one(); + } + + // Worker thread. + void run() + { + try + { + check_cuda(cudaSetDevice(config.gpu_id), "cudaSetDevice(worker)"); + for (;;) + { + int index = -1; + std::optional dma; + bool flush_now = false; + { + std::unique_lock lock(mutex); + // Frames in the encoder come out a little later: poll for them soon, and otherwise + // wake now and then to pick up the receiving side's requests. + const auto wait = correlator.empty() ? std::chrono::milliseconds(100) : std::chrono::milliseconds(2); + wake.wait_for(lock, wait, [&] { return stopping || pending >= 0 || pending_dma || flush_requested; }); + if (stopping) + return; + index = pending; + pending = -1; + inflight = index; + dma = std::move(pending_dma); + pending_dma.reset(); + flush_now = flush_requested; + } + if (dma) + { + encode_dma(*dma); + dma.reset(); + } + if (index >= 0) + { + encode(slots[index]); + std::lock_guard lock(mutex); + inflight = -1; + } + if (flush_now) + { + for (auto& unit : encoder->drain()) + publish_unit(unit); + std::lock_guard lock(mutex); + flush_requested = false; + flushed.notify_all(); + } + else + { + for (auto unit = encoder->poll(); !unit.empty(); unit = encoder->poll()) + publish_unit(unit); + } + } + } + catch (const std::exception& e) + { + fail(e.what()); + std::lock_guard lock(mutex); + flushed.notify_all(); + } + } + + // A keyframe is several times the size of an ordinary frame. A link that is struggling makes the PC ask for + // keyframes over and over, and each one adds to the load that caused the request, so forced keyframes are spaced. + bool idr_allowed() + { + const auto now = std::chrono::steady_clock::now(); + if (now - last_forced_idr < kMinIdrInterval) + return false; + last_forced_idr = now; + return true; + } + + // Worker thread: follow the link. Frames the direct sender had to drop mean the network could not carry the + // stream, so lower the bitrate; after a calm stretch raise it again, never above the configured target. + void adapt_to_link() + { + const auto now = std::chrono::steady_clock::now(); + if (now - last_adapt < kAdaptInterval) + return; + last_adapt = now; + const uint64_t dropped = direct->dropped(); + // Drops while nothing is connected say the destination is absent, not that the network is slow. + const bool congested = dropped != last_dropped && direct->connected(); + last_dropped = dropped; + + uint32_t target = current_bitrate; + if (congested) + { + calm_intervals = 0; + target = std::max(kFloorBitrateBps, static_cast(current_bitrate * 0.7)); + } + else if (++calm_intervals >= kCalmBeforeRaise) + { + calm_intervals = 0; + target = std::min(config.bitrate_bps, static_cast(current_bitrate * 1.15)); + } + target = std::min(target, config.bitrate_bps); + if (target == current_bitrate || !encoder->set_bitrate(target)) + return; + isaaccapture::Logger::get("isaaccapture.plugins.sensing.VideoOutput") + ->info("link {}: video bitrate {} -> {} kbit/s", congested ? "congested" : "recovering", + current_bitrate / 1000, target / 1000); + current_bitrate = target; + } + + // Worker thread. + void encode(const Slot& slot) + { + const uint8_t* rgba = slot.ptr; + size_t pitch = slot.pitch; + if (config.scale_div > 1) + { + launch_rgba_box_downscale(rgba, static_cast(pitch), d_scaled, static_cast(scaled_pitch), + static_cast(out_width), static_cast(out_height), + static_cast(config.scale_div), work_stream); + check_cuda(cudaGetLastError(), "reduction kernel"); + // The encoder reads on the default stream, which a non-blocking stream does not order against. + check_cuda(cudaStreamSynchronize(work_stream), "reduction sync"); + rgba = d_scaled; + pitch = scaled_pitch; + } + + if (direct && direct->take_keyframe_request()) + { + ++keyframes_requested; + keyframe_wanted = true; + } + adapt_to_link(); + if (keyframe_wanted && idr_allowed()) + { + encoder->request_keyframe(); + keyframe_wanted = false; + } + const int64_t stamp_ns = static_cast(slot.timestamp_ns); + if (!encoder->submit(reinterpret_cast(rgba), pitch, stamp_ns)) + { + // No input buffer: dropped before encoding, so no reference is lost. A keyframe already + // requested of the encoder applies to the next frame it takes. + ++encoder_busy; + return; + } + track_in_flight(static_cast(slot.sequence), slot.capture_tsc, + application_stamp_us(slot.timestamp_ns, slot.capture_monotonic_ns), stamp_ns); + } + + // Worker thread: a leased DMA-BUF frame goes to the encoder as it is, and its lease stays with the encoder. + void encode_dma(CaptureFrame& frame) + { + if (direct && direct->take_keyframe_request()) + { + ++keyframes_requested; + keyframe_wanted = true; + } + adapt_to_link(); + if (keyframe_wanted && idr_allowed()) + { + encoder->request_keyframe(); + keyframe_wanted = false; + } + const int fd = std::get(frame.pixels).fd; + if (!encoder->submit_dmabuf(fd, static_cast(frame.monotonic_ns), std::move(frame.lease))) + { + ++encoder_busy; + return; + } + track_in_flight(static_cast(frame.sequence), frame.sensor_time, + application_stamp_us(frame.monotonic_ns, frame.capture_monotonic_ns), + static_cast(frame.monotonic_ns)); + } + + /// Polling thread: the frame's capture event on CLOCK_MONOTONIC. The sensor clock's anchor is built from the + /// first frame's declared domain. + uint64_t capture_time_ns(const CaptureFrame& frame) + { + if (frame.sensor_domain == SensorTimeDomain::None || frame.sensor_time == 0) + throw std::runtime_error("Video output: the camera backend reports no sensor timestamp to publish"); + if (!clock_map) + { + SensorClockMap::Config map; + map.domain = frame.sensor_domain; + if (frame.sensor_domain == SensorTimeDomain::GenericCounterTicks) + map.counter_hz = read_counter_hz(); + const bool counter = frame.sensor_domain == SensorTimeDomain::GenericCounterTicks; + clock_map.emplace(map, counter ? SensorClockMap::PairReader(read_counter_pair) : + SensorClockMap::PairReader(read_raw_pair)); + } + struct timespec now + { + }; + clock_gettime(CLOCK_MONOTONIC, &now); + const int64_t now_ns = static_cast(now.tv_sec) * 1'000'000'000LL + now.tv_nsec; + return static_cast( + clock_map->to_monotonic_ns(frame.sensor_time, static_cast(frame.monotonic_ns), now_ns)); + } + + /// The application timestamp published with a frame, in microseconds. + uint64_t application_stamp_us(uint64_t ready_monotonic_ns, uint64_t capture_monotonic_ns) const + { + switch (config.timestamp_basis) + { + case TimestampBasis::FrameReadyMonotonic: + return ready_monotonic_ns / 1000; + case TimestampBasis::SensorCaptureMonotonic: + return capture_monotonic_ns / 1000; + } + throw std::logic_error("unknown timestamp basis"); + } + + // Worker thread: remember the capture a submitted frame came from, for the unit that comes back. + void track_in_flight(uint32_t sequence, uint64_t capture_tsc, uint64_t stamp_us, int64_t submitted_ns) + { + correlator.submitted({ sequence, capture_tsc, stamp_us, submitted_ns / 1000 }); + } + + // Worker thread: one encoded unit becomes one published frame, with the sequence and capture time + // of the capture it came from. + void publish_unit(const EncodedUnit& unit) + { + const int64_t stamp_us = unit.timestamp_ns / 1000; + // Which capture is this unit? See FrameCorrelator: by the stamp the encoder hands back, so a frame it never + // returned is detected, and a unit that matches nothing is sent without a capture time instead of an assumed + // one. Fifo is an explicit choice for approximate timing only. + const auto result = correlator.returned(stamp_us); + if (!result) + throw std::runtime_error("Video output: the encoder returned a unit with no frame in flight (unit stamp_us=" + + std::to_string(stamp_us) + ")"); + const bool identified = result->identified; + const video_stage::InFlightFrame& meta = result->frame; + encoder_dropped += result->skipped; + if (config.correlation == FrameCorrelation::Stamp) + ++(identified ? matched : unmatched); + + EncodedVideoFrame frame; + frame.data = unit.data.data(); + frame.size = unit.data.size(); + frame.keyframe = unit.keyframe; + frame.timestamp_us = static_cast(stamp_us); + // Zero identity and time when the unit could not be tied to a capture: the receiver treats that as unknown. + frame.frame_id = identified ? meta.sequence : 0; + frame.capture_tsc = identified ? meta.capture_tsc : 0; + frame.user_timestamp_us = identified ? meta.stamp_us : 0; + // The direct link keeps its own queue and keyframe bookkeeping and never blocks this thread. + direct->submit(frame); + ++published; + } + + std::unique_ptr direct; ///< optional second destination: the PC video stage + VideoOutputConfig config; + uint32_t out_width = 0; + uint32_t out_height = 0; + uint64_t period_ns = 0; + + + std::unique_ptr encoder; + + // Worker thread only. + std::optional clock_map; ///< Polling thread only. + video_stage::FrameCorrelator correlator; ///< Submitted to the encoder, no unit back yet. + bool keyframe_wanted = false; + std::chrono::steady_clock::time_point last_forced_idr{}; + std::chrono::steady_clock::time_point last_adapt = std::chrono::steady_clock::now(); + uint64_t last_dropped = 0; + uint32_t calm_intervals = 0; + uint32_t current_bitrate = 0; + + Slot slots[2]; + /// DMA-BUF input only: the newest accepted frame, waiting for the worker. Guarded by `mutex`. + std::optional pending_dma; + uint8_t* d_scaled = nullptr; + size_t scaled_pitch = 0; + cudaStream_t snapshot_stream = nullptr; + cudaStream_t work_stream = nullptr; + + // Guards pending, inflight, stopping and the snapshot slots' contents. + std::mutex mutex; + std::condition_variable wake; + int pending = -1; + int inflight = -1; + bool stopping = false; + bool flush_requested = false; + std::condition_variable flushed; + std::thread worker; + + // Polling thread only. + bool have_last = false; + uint64_t last_accepted_ns = 0; + + std::atomic offered{ 0 }; + std::atomic paced_out{ 0 }; + std::atomic replaced{ 0 }; + std::atomic snapshots{ 0 }; + std::atomic published{ 0 }; + std::atomic matched{ 0 }; + std::atomic unmatched{ 0 }; + std::atomic encoder_dropped{ 0 }; + std::atomic encoder_busy{ 0 }; + std::atomic keyframes_requested{ 0 }; + + std::mutex failure_mutex; + std::string failure_message; + std::atomic failed_flag{ false }; +}; + +void VideoOutput::validate_geometry(uint32_t width, uint32_t height, uint32_t divisor) +{ + if (divisor == 0) + throw std::runtime_error("Video output: the scale divisor must be at least 1"); + if (width == 0 || height == 0) + throw std::runtime_error("Video output: the source size is empty"); + if (width % divisor != 0 || height % divisor != 0) + { + throw std::runtime_error("Video output: divisor " + std::to_string(divisor) + " does not divide " + + std::to_string(width) + "x" + std::to_string(height) + " exactly"); + } + if ((width / divisor) % 2 != 0 || (height / divisor) % 2 != 0) + { + throw std::runtime_error("Video output: " + std::to_string(width / divisor) + "x" + + std::to_string(height / divisor) + " is not even; NV12 needs even dimensions"); + } +} + +VideoOutput::VideoOutput(const VideoOutputConfig& config) : m_impl(std::make_unique(config)) +{ +} + +VideoOutput::~VideoOutput() = default; + +void VideoOutput::submit(CaptureFrame& frame) +{ + m_impl->submit(frame); +} + +VideoOutputStats VideoOutput::stats() const +{ + VideoOutputStats s; + s.offered = m_impl->offered.load(); + s.paced_out = m_impl->paced_out.load(); + s.replaced = m_impl->replaced.load(); + s.snapshots = m_impl->snapshots.load(); + s.published = m_impl->published.load(); + s.matched = m_impl->matched.load(); + s.unmatched = m_impl->unmatched.load(); + s.encoder_dropped = m_impl->encoder_dropped.load(); + s.encoder_busy = m_impl->encoder_busy.load(); + s.keyframes_requested = m_impl->keyframes_requested.load(); + return s; +} + +bool VideoOutput::failed() const +{ + return m_impl->failed_flag.load(); +} + +std::string VideoOutput::failure() const +{ + std::lock_guard lock(m_impl->failure_mutex); + return m_impl->failure_message; +} + +void VideoOutput::set_direct_target(const std::string& host, uint16_t port) +{ + m_impl->direct->set_target(host, port); +} + +DirectLinkStatus VideoOutput::direct_status() const +{ + return { m_impl->direct->host(), m_impl->direct->port(), m_impl->direct->connected(), m_impl->direct->last_error() }; +} + +std::string VideoOutput::describe() +{ + std::string text = std::string(JetsonEncoder::backend_name()) + ", H.264 to the PC video stage"; + return text; +} + +void VideoOutput::flush() +{ + std::unique_lock lock(m_impl->mutex); + m_impl->flush_requested = true; + m_impl->wake.notify_one(); + m_impl->flushed.wait(lock, [&] { return !m_impl->flush_requested || m_impl->failed_flag.load(); }); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/video_output.hpp b/src/plugins/sensing/core/video_output.hpp new file mode 100644 index 0000000000..595c4578c3 --- /dev/null +++ b/src/plugins/sensing/core/video_output.hpp @@ -0,0 +1,131 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "camera_capture.hpp" +#include "sensing_types.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct VideoOutputConfig +{ + uint32_t source_width = 0; + uint32_t source_height = 0; + /// Uniform output-only reduction; 1 publishes the sensor's native size. + uint32_t scale_div = 1; + /// The frames submit() will be given; DMA-BUF input needs scale_div 1 (no CPU or CUDA copy exists to reduce). + PixelKind input = PixelKind::CudaRgba; + /// Publication rate cap, independent of the capture rate. + uint32_t fps = 30; + int gpu_id = 0; + /// Hardware encoder settings: the target bitrate and the VBR ceiling. + uint32_t bitrate_bps = 8'000'000; + uint32_t peak_bitrate_bps = 0; + /// IDR period in frames; 0 defers to the encoder's default. + uint32_t gop = 0; + bool full_range = false; + /// Also send every encoded unit to the PC video stage at this address (host:port); empty disables it. + std::string direct_host; + uint16_t direct_port = 0; + /// What each frame's application timestamp is. SensorCaptureMonotonic fails the output on a frame whose sensor + /// time cannot be placed on CLOCK_MONOTONIC. + TimestampBasis timestamp_basis = TimestampBasis::FrameReadyMonotonic; + FrameCorrelation correlation = FrameCorrelation::Stamp; +}; + +/// Where this stream's encoded video also goes (the PC video stage), and whether that link is up. +struct DirectLinkStatus +{ + std::string host; + uint16_t port = 0; + bool connected = false; + std::string error; +}; + +struct VideoOutputStats +{ + uint64_t offered = 0; ///< Captures handed to submit(). + uint64_t paced_out = 0; ///< Skipped to hold the publication rate. + uint64_t replaced = 0; ///< Snapshots overwritten by a newer capture before the worker took them. + uint64_t snapshots = 0; ///< GPU snapshots taken. + uint64_t published = 0; ///< Encoded units handed to the direct link (not proof the PC received them). + uint64_t encoder_busy = 0; ///< Snapshots the encoder had no input buffer for, dropped before encoding. + uint64_t keyframes_requested = 0; ///< Keyframes the PC asked for. + uint64_t matched = 0; ///< Units matched to their capture by the stamp the encoder returned. + uint64_t unmatched = 0; ///< Units whose returned stamp matched no submitted frame: sent without a capture time. + uint64_t encoder_dropped = 0; ///< Submitted frames the encoder never returned a unit for. +}; + +/** + * @brief Encodes one sensor's frames on hardware and hands them to the direct link to the PC. + * + * submit() runs on the caller's polling loop. A pixel-buffer frame (SIPL) is copied into a snapshot in GPU memory + * before returning, because the camera may overwrite its slot at the second following poll. A DMA-BUF frame (Argus) + * is not copied: it goes to the encoder as it is, and its lease returns the buffer to the camera when the encoder is + * done with it. Everything else -- the optional reduction, the hardware encode and the send queue -- happens on this + * output's own worker thread, so a slow network cannot hold up capture or the other sensors. + * + * Two snapshot slots bound the memory: one the worker is encoding, one waiting. A newer capture replaces the waiting + * one; the worker never waits for it. The encoder is the JetsonEncoder class: the Jetson V4L2 hardware engine. There + * is no software encoder: if the hardware encoder cannot start, construction fails. + * + * The PC's keyframe requests (and a dropped or reconnected link) are applied before the next frame is encoded. + */ +class VideoOutput +{ +public: + /// @throws std::runtime_error if the geometry is unusable (see validate_geometry), the hardware + /// encoder cannot start, or CUDA fails. + explicit VideoOutput(const VideoOutputConfig& config); + + /// Stops the worker; a snapshot not yet published is dropped. + ~VideoOutput(); + + VideoOutput(const VideoOutput&) = delete; + VideoOutput& operator=(const VideoOutput&) = delete; + + /// Never blocks on the network. A frame sooner than the publication rate allows is skipped. Borrowed CUDA + /// RGBA pixels are copied out before this returns; a leased DMA-BUF frame's lease is moved into the output and + /// released once the encoder is finished with it (or at once if the frame is dropped). + void submit(CaptureFrame& frame); + + VideoOutputStats stats() const; + + /// Send the encoded video to host:port from now on, or stop the direct link when host is empty. Thread-safe. + void set_direct_target(const std::string& host, uint16_t port); + DirectLinkStatus direct_status() const; + + bool failed() const; + std::string failure() const; + + /// Names the hardware encoder and the transport's view of the stream, for diagnostics. + std::string describe(); + + /** + * Encodes what is waiting, drains the encoder and publishes every remaining unit with its own + * stamp. Call once, at the end of the stream: the encoder cannot take frames afterwards. + */ + void flush(); + + /** + * Output size after dividing: both source dimensions must divide exactly and leave positive, + * even results. Nothing is rounded or stretched. + * @throws std::runtime_error naming the problem. + */ + static void validate_geometry(uint32_t width, uint32_t height, uint32_t divisor); + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/yuv_to_rgba.cu b/src/plugins/sensing/core/yuv_to_rgba.cu new file mode 100644 index 0000000000..7186e46ae8 --- /dev/null +++ b/src/plugins/sensing/core/yuv_to_rgba.cu @@ -0,0 +1,253 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// YUV420 -> RGBA8 conversion for the SIPL ISP0 output. +// +// BT.601 and BT.709, selected per call. SIPL's ISP0 output on this rig is +// REC709_ER and the request is not negotiable -- GetImageAttributes rejects a +// BT.601 attribute list with NVSIPL_STATUS_BAD_ARGUMENT -- so the 709 matrix is +// the one the SIPL path actually uses. rgba_to_nv12.cu on the encode side is +// still 601, so an encode round trip is not identity; that only matters if the +// H.264 output is ever compared against the RGBA it came from. + +#include "yuv_to_rgba.cuh" + +namespace +{ + +__device__ __forceinline__ unsigned char clamp_u8(float v) +{ + return static_cast(v < 0.f ? 0.f : (v > 255.f ? 255.f : v)); +} + +__device__ __forceinline__ void yuv_to_rgb( + int Y, int Cb, int Cr, bool full_range, bool bt709, unsigned char& r, unsigned char& g, unsigned char& b) +{ + // Full range uses Y as-is; limited range expands 16..235 first. + const float yf = full_range ? static_cast(Y) : (static_cast(Y) - 16.f) * 1.16438f; + const float u = static_cast(Cb) - 128.f; + const float v = static_cast(Cr) - 128.f; + + // Limited-range coefficients already fold in the 255/219 luma gain applied + // above, so they differ from the full-range pair by more than rounding. + float kr, kgu, kgv, kb; + if (bt709) + { + if (full_range) + { + kr = 1.5748f; + kgu = 0.18733f; + kgv = 0.46813f; + kb = 1.8556f; + } + else + { + kr = 1.79274f; + kgu = 0.21325f; + kgv = 0.53291f; + kb = 2.11240f; + } + } + else + { + if (full_range) + { + kr = 1.402f; + kgu = 0.344136f; + kgv = 0.714136f; + kb = 1.772f; + } + else + { + kr = 1.596f; + kgu = 0.391f; + kgv = 0.813f; + kb = 2.018f; + } + } + + r = clamp_u8(yf + kr * v); + g = clamp_u8(yf - kgu * u - kgv * v); + b = clamp_u8(yf + kb * u); +} + +__global__ void yuv420_pitch_to_rgba_kernel(const uint8_t* __restrict__ y_plane, + const uint8_t* __restrict__ uv_or_u_plane, + const uint8_t* __restrict__ v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* __restrict__ rgba_out, + int rgba_row_bytes, + int layout_value, + int full_range_value, + int bt709_value) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= width || y >= height) + { + return; + } + + const auto layout = static_cast(layout_value); + const int Y = y_plane[y * y_pitch + x]; + int Cb = 128; + int Cr = 128; + + if (layout == plugins::sensing::YuvLayout::YUV420SemiPlanar || layout == plugins::sensing::YuvLayout::YVU420SemiPlanar) + { + const int uv_x = x & ~1; + const int uv_y = y >> 1; + const uint8_t a = uv_or_u_plane[uv_y * uv_pitch + uv_x + 0]; + const uint8_t b = uv_or_u_plane[uv_y * uv_pitch + uv_x + 1]; + if (layout == plugins::sensing::YuvLayout::YUV420SemiPlanar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + else + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uint8_t a = uv_or_u_plane[uv_y * uv_pitch + uv_x]; + const uint8_t b = v_plane[uv_y * v_pitch + uv_x]; + if (layout == plugins::sensing::YuvLayout::YUV420Planar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + + const int idx = y * rgba_row_bytes + x * 4; + yuv_to_rgb( + Y, Cb, Cr, full_range_value != 0, bt709_value != 0, rgba_out[idx + 0], rgba_out[idx + 1], rgba_out[idx + 2]); + rgba_out[idx + 3] = 255; +} + +__global__ void yuv420_array_to_rgba_kernel(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* __restrict__ rgba_out, + int rgba_row_bytes, + int layout_value, + int full_range_value, + int bt709_value) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= width || y >= height) + { + return; + } + + const auto layout = static_cast(layout_value); + const int Y = tex2D(y_tex, x, y); + int Cb = 128; + int Cr = 128; + + if (layout == plugins::sensing::YuvLayout::YUV420SemiPlanar || layout == plugins::sensing::YuvLayout::YVU420SemiPlanar) + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uchar2 uv = tex2D(uv_or_u_tex, uv_x, uv_y); + if (layout == plugins::sensing::YuvLayout::YUV420SemiPlanar) + { + Cb = uv.x; + Cr = uv.y; + } + else + { + Cr = uv.x; + Cb = uv.y; + } + } + else + { + const int uv_x = x >> 1; + const int uv_y = y >> 1; + const uint8_t a = tex2D(uv_or_u_tex, uv_x, uv_y); + const uint8_t b = tex2D(v_tex, uv_x, uv_y); + if (layout == plugins::sensing::YuvLayout::YUV420Planar) + { + Cb = a; + Cr = b; + } + else + { + Cr = a; + Cb = b; + } + } + + const int idx = y * rgba_row_bytes + x * 4; + yuv_to_rgb( + Y, Cb, Cr, full_range_value != 0, bt709_value != 0, rgba_out[idx + 0], rgba_out[idx + 1], rgba_out[idx + 2]); + rgba_out[idx + 3] = 255; +} + +} // namespace + +namespace plugins +{ +namespace sensing +{ + +void launch_yuv420_pitch_to_rgba(const uint8_t* y_plane, + const uint8_t* uv_or_u_plane, + const uint8_t* v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + bool bt709, + cudaStream_t stream) +{ + const dim3 block(16, 16, 1); + const dim3 grid((width + 15) / 16, (height + 15) / 16, 1); + yuv420_pitch_to_rgba_kernel<<>>(y_plane, uv_or_u_plane, v_plane, y_pitch, uv_pitch, v_pitch, + width, height, rgba_out, rgba_row_bytes, + static_cast(layout), full_range ? 1 : 0, bt709 ? 1 : 0); +} + +void launch_yuv420_array_to_rgba(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + bool bt709, + cudaStream_t stream) +{ + const dim3 block(16, 16, 1); + const dim3 grid((width + 15) / 16, (height + 15) / 16, 1); + yuv420_array_to_rgba_kernel<<>>(y_tex, uv_or_u_tex, v_tex, width, height, rgba_out, + rgba_row_bytes, static_cast(layout), + full_range ? 1 : 0, bt709 ? 1 : 0); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/core/yuv_to_rgba.cuh b/src/plugins/sensing/core/yuv_to_rgba.cuh new file mode 100644 index 0000000000..3645202521 --- /dev/null +++ b/src/plugins/sensing/core/yuv_to_rgba.cuh @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +enum class YuvLayout +{ + YUV420Planar, + YVU420Planar, + YUV420SemiPlanar, + YVU420SemiPlanar, +}; + +void launch_yuv420_pitch_to_rgba(const uint8_t* y_plane, + const uint8_t* uv_or_u_plane, + const uint8_t* v_plane, + int y_pitch, + int uv_pitch, + int v_pitch, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + /// BT.709 matrix instead of BT.601. + bool bt709, + cudaStream_t stream); + +void launch_yuv420_array_to_rgba(cudaTextureObject_t y_tex, + cudaTextureObject_t uv_or_u_tex, + cudaTextureObject_t v_tex, + int width, + int height, + uint8_t* rgba_out, + int rgba_row_bytes, + YuvLayout layout, + bool full_range, + /// BT.709 matrix instead of BT.601. + bool bt709, + cudaStream_t stream); + +} // namespace sensing +} // namespace plugins diff --git a/src/plugins/sensing/images/setup-camera-front.jpg b/src/plugins/sensing/images/setup-camera-front.jpg new file mode 100644 index 0000000000..2221a644aa --- /dev/null +++ b/src/plugins/sensing/images/setup-camera-front.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85edd8d44afd7e59075f6f23e7fa9942137f71de5b1537ac791a65b86622a927 +size 94547 diff --git a/src/plugins/sensing/images/setup-camera-front.jpg.license b/src/plugins/sensing/images/setup-camera-front.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-camera-front.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-camera-rear.jpg b/src/plugins/sensing/images/setup-camera-rear.jpg new file mode 100644 index 0000000000..3e1d73a2fc --- /dev/null +++ b/src/plugins/sensing/images/setup-camera-rear.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b1e11a13cd8a0d691e7bfe36d65262b566e12ff72613b2c08bfd789f4a6f4fe8 +size 69846 diff --git a/src/plugins/sensing/images/setup-camera-rear.jpg.license b/src/plugins/sensing/images/setup-camera-rear.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-camera-rear.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-carrier-front-panel.jpg b/src/plugins/sensing/images/setup-carrier-front-panel.jpg new file mode 100644 index 0000000000..18fc6b8b78 --- /dev/null +++ b/src/plugins/sensing/images/setup-carrier-front-panel.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c98de586d8da78d945ad1f309655c58a1422bda4be985c444eedf25c1f0e7c9c +size 73457 diff --git a/src/plugins/sensing/images/setup-carrier-front-panel.jpg.license b/src/plugins/sensing/images/setup-carrier-front-panel.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-carrier-front-panel.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg b/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg new file mode 100644 index 0000000000..f544282a2f --- /dev/null +++ b/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aeeabae35f8242468b5c6748edfd8cae4327286cf81d7fcb8abfd087a6ebfd47 +size 94271 diff --git a/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg.license b/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-carrier-inside-devkit.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-harness-connectors.jpg b/src/plugins/sensing/images/setup-harness-connectors.jpg new file mode 100644 index 0000000000..8788fa6f41 --- /dev/null +++ b/src/plugins/sensing/images/setup-harness-connectors.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c8e93a67d2f1611e1aa1bb2b6df8798c0877d48fdf866481a60d7cba96fd9bca +size 76646 diff --git a/src/plugins/sensing/images/setup-harness-connectors.jpg.license b/src/plugins/sensing/images/setup-harness-connectors.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-harness-connectors.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-harness-in-cn1.jpg b/src/plugins/sensing/images/setup-harness-in-cn1.jpg new file mode 100644 index 0000000000..2d213040cf --- /dev/null +++ b/src/plugins/sensing/images/setup-harness-in-cn1.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:06899102d2d4d4cb76bdb525ca431e6c52239e5810e9634397453ebfb672ebbe +size 71799 diff --git a/src/plugins/sensing/images/setup-harness-in-cn1.jpg.license b/src/plugins/sensing/images/setup-harness-in-cn1.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-harness-in-cn1.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/images/setup-parts-overview.jpg b/src/plugins/sensing/images/setup-parts-overview.jpg new file mode 100644 index 0000000000..9302543e5d --- /dev/null +++ b/src/plugins/sensing/images/setup-parts-overview.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:93776cd44ea1a86e67086120bd34b7c34860ee376c52bf84c71785503d536b49 +size 133909 diff --git a/src/plugins/sensing/images/setup-parts-overview.jpg.license b/src/plugins/sensing/images/setup-parts-overview.jpg.license new file mode 100644 index 0000000000..817df24424 --- /dev/null +++ b/src/plugins/sensing/images/setup-parts-overview.jpg.license @@ -0,0 +1,2 @@ +SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/src/plugins/sensing/plugin.yaml b/src/plugins/sensing/plugin.yaml new file mode 100644 index 0000000000..c1df9fea39 --- /dev/null +++ b/src/plugins/sensing/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: sensing_camera +description: "SENSING GMSL camera plugin (SIPL capture, hardware H.264, direct streaming to the PC video stage)" +command: "./camera_plugin_sensing" +version: "1.0.0" +devices: + - path: "/camera" + type: "camera" + description: "Camera video stream" diff --git a/src/plugins/sensing/tests/CMakeLists.txt b/src/plugins/sensing/tests/CMakeLists.txt new file mode 100644 index 0000000000..885f61383b --- /dev/null +++ b/src/plugins/sensing/tests/CMakeLists.txt @@ -0,0 +1,38 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Backend selection and the frame lease are pure C++; compile camera_backend.cpp directly so the test links +# neither sensing_core (CUDA, SIPL, the Multimedia API) nor any vendor SDK. +add_executable(sensing_backend_tests + test_camera_backend.cpp + ../core/camera_backend.cpp +) + +# Reaches ../core headers as "core/..." without adding the plugin's own core/ include path. +target_include_directories(sensing_backend_tests PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) + +target_link_libraries(sensing_backend_tests PRIVATE Catch2::Catch2WithMain) + +catch_discover_tests(sensing_backend_tests) + +# The buffer owner is header-only and SDK-free, tested against a fake port. +add_executable(sensing_buffer_owner_tests + test_buffer_owner.cpp +) + +target_include_directories(sensing_buffer_owner_tests PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) + +target_link_libraries(sensing_buffer_owner_tests PRIVATE Catch2::Catch2WithMain Threads::Threads) + +catch_discover_tests(sensing_buffer_owner_tests) + +# The sensor-clock mapping is header-only and injectable, so it needs no SDK. +add_executable(sensing_sensor_clock_tests + test_sensor_clock.cpp +) + +target_include_directories(sensing_sensor_clock_tests PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) + +target_link_libraries(sensing_sensor_clock_tests PRIVATE Catch2::Catch2WithMain) + +catch_discover_tests(sensing_sensor_clock_tests) diff --git a/src/plugins/sensing/tests/test_buffer_owner.cpp b/src/plugins/sensing/tests/test_buffer_owner.cpp new file mode 100644 index 0000000000..e5fa043184 --- /dev/null +++ b/src/plugins/sensing/tests/test_buffer_owner.cpp @@ -0,0 +1,1020 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "core/buffer_owner.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Catch::Matchers::ContainsSubstring; +using plugins::sensing::BufferOwner; +using plugins::sensing::BufferPort; +using plugins::sensing::BufferReturnMailbox; +using plugins::sensing::CaptureFrame; +using plugins::sensing::DmaBufNv12Pixels; + +namespace +{ + +using namespace std::chrono_literals; + +constexpr auto kWait = 5ms; +constexpr auto kDeadline = 5s; +constexpr size_t kMaxSlots = 16; + +template +bool wait_until(Pred pred, + std::chrono::milliseconds limit = std::chrono::duration_cast(kDeadline)) +{ + const auto end = std::chrono::steady_clock::now() + limit; + while (!pred()) + { + if (std::chrono::steady_clock::now() >= end) + return false; + std::this_thread::sleep_for(1ms); + } + return true; +} + +/// A bounded pool of slots standing in for the stream. Frames are produced while `budget` is positive and a slot +/// is free; otherwise acquire sleeps the requested timeout and reports Timeout. +class FakePort : public BufferPort +{ +public: + explicit FakePort(size_t pool_size) : m_pool_size(pool_size) + { + } + + Acquired acquire(uint64_t timeout_ns) override + { + Guard guard(*this); + Acquired out; + int slot = -1; + bool produce = false; + { + std::lock_guard lock(m_mutex); + ++m_acquire_calls; + if (m_throw_on_acquire) + throw std::runtime_error("acquire threw"); + for (size_t i = 0; i < m_pool_size; ++i) + { + if (!m_busy[i]) + { + slot = static_cast(i); + break; + } + } + if (slot >= 0 && m_budget > 0) + { + --m_budget; + m_busy[slot] = true; + ++m_acquired[slot]; + ++m_acquired_total; + m_events.push_back({ Event::Kind::Acquire, static_cast(slot) }); + produce = true; + if (m_fail_next) + { + m_fail_next = false; + out.result = Result::Failed; + out.holds_buffer = true; + out.slot = static_cast(slot); + out.error = "stream exploded"; + return out; + } + } + } + if (!produce) + { + std::this_thread::sleep_for(std::chrono::nanoseconds(timeout_ns)); + out.result = Result::Timeout; + return out; + } + std::this_thread::sleep_for(200us); + out.result = Result::Frame; + out.holds_buffer = true; + out.slot = static_cast(slot); + out.frame.sequence = m_sequence.fetch_add(1) + 1; + out.frame.pixels = DmaBufNv12Pixels{ slot }; + return out; + } + + bool release(uint32_t slot) override + { + Guard guard(*this); + std::lock_guard lock(m_mutex); + ++m_release_calls; + m_events.push_back({ Event::Kind::Release, slot }); + if (m_throw_on_release) + throw std::runtime_error("release threw"); + if (!m_release_ok) + return false; // refused: the buffer stays out and is not counted as released + if (slot >= m_pool_size || !m_busy[slot]) + { + ++m_bad_releases; + return true; + } + m_busy[slot] = false; + ++m_released[slot]; + ++m_released_total; + return true; + } + + bool stop_capture() override + { + Guard guard(*this); + std::lock_guard lock(m_mutex); + ++m_stop_calls; + m_events.push_back({ Event::Kind::StopBegin, 0 }); + return m_stop_ok; + } + + bool wait_idle() override + { + Guard guard(*this); + std::lock_guard lock(m_mutex); + ++m_idle_calls; + m_events.push_back({ Event::Kind::WaitIdle, 0 }); + if (m_throw_on_idle) + throw std::runtime_error("wait_idle threw"); + return m_idle_ok; + } + + void set_budget(int64_t n) + { + std::lock_guard lock(m_mutex); + m_budget = n; + } + void set_throw_on_acquire(bool v) + { + std::lock_guard lock(m_mutex); + m_throw_on_acquire = v; + } + void set_fail_next(bool v) + { + std::lock_guard lock(m_mutex); + m_fail_next = v; + } + void set_release_ok(bool v) + { + std::lock_guard lock(m_mutex); + m_release_ok = v; + } + void set_throw_on_release(bool v) + { + std::lock_guard lock(m_mutex); + m_throw_on_release = v; + } + void set_stop_ok(bool v) + { + std::lock_guard lock(m_mutex); + m_stop_ok = v; + } + void set_idle_ok(bool v) + { + std::lock_guard lock(m_mutex); + m_idle_ok = v; + } + void set_throw_on_idle(bool v) + { + std::lock_guard lock(m_mutex); + m_throw_on_idle = v; + } + int idle_calls() const + { + std::lock_guard lock(m_mutex); + return m_idle_calls; + } + + uint64_t acquire_calls() const + { + std::lock_guard lock(m_mutex); + return m_acquire_calls; + } + uint64_t acquired_total() const + { + std::lock_guard lock(m_mutex); + return m_acquired_total; + } + uint64_t released_total() const + { + std::lock_guard lock(m_mutex); + return m_released_total; + } + uint64_t release_calls() const + { + std::lock_guard lock(m_mutex); + return m_release_calls; + } + uint64_t bad_releases() const + { + std::lock_guard lock(m_mutex); + return m_bad_releases; + } + /// Port calls in the order they happened, across all slots. + struct Event + { + enum class Kind + { + Acquire, + Release, + StopBegin, + WaitIdle, + }; + Kind kind; + uint32_t slot; + }; + std::vector events() const + { + std::lock_guard lock(m_mutex); + return m_events; + } + int stop_calls() const + { + std::lock_guard lock(m_mutex); + return m_stop_calls; + } + uint64_t acquired(size_t slot) const + { + std::lock_guard lock(m_mutex); + return m_acquired[slot]; + } + uint64_t released(size_t slot) const + { + std::lock_guard lock(m_mutex); + return m_released[slot]; + } + int max_in_flight() const + { + return m_max_in_flight.load(); + } + std::set threads() const + { + std::lock_guard lock(m_threads_mutex); + return m_threads; + } + + /// Every acquired buffer was released exactly once, slot by slot. + bool balanced() const + { + std::lock_guard lock(m_mutex); + for (size_t i = 0; i < m_pool_size; ++i) + { + if (m_acquired[i] != m_released[i] || m_busy[i]) + return false; + } + return m_bad_releases == 0; + } + +private: + /// Records the calling thread and how many calls are in flight at once. + struct Guard + { + explicit Guard(FakePort& port) : p(port) + { + const int now = ++p.m_in_flight; + int seen = p.m_max_in_flight.load(); + while (now > seen && !p.m_max_in_flight.compare_exchange_weak(seen, now)) + { + } + std::lock_guard lock(p.m_threads_mutex); + p.m_threads.insert(std::this_thread::get_id()); + } + ~Guard() + { + --p.m_in_flight; + } + FakePort& p; + }; + + const size_t m_pool_size; + mutable std::mutex m_mutex; + std::array m_busy{}; + std::array m_acquired{}; + std::array m_released{}; + int64_t m_budget = 0; + bool m_throw_on_acquire = false; + bool m_fail_next = false; + bool m_release_ok = true; + bool m_stop_ok = true; + bool m_throw_on_release = false; + bool m_idle_ok = true; + bool m_throw_on_idle = false; + int m_idle_calls = 0; + std::vector m_events; + uint64_t m_acquire_calls = 0; + uint64_t m_acquired_total = 0; + uint64_t m_released_total = 0; + uint64_t m_release_calls = 0; + uint64_t m_bad_releases = 0; + int m_stop_calls = 0; + std::atomic m_sequence{ 0 }; + + std::atomic m_in_flight{ 0 }; + std::atomic m_max_in_flight{ 0 }; + mutable std::mutex m_threads_mutex; + std::set m_threads; +}; + +constexpr int64_t kUnlimited = INT64_MAX / 2; + +/// Polls the owner until `count` frames have been taken, holding them (and their leases). +std::vector take_frames(BufferOwner& owner, size_t count) +{ + std::vector held; + wait_until( + [&] + { + if (auto frame = owner.take_latest()) + held.push_back(std::move(*frame)); + return held.size() >= count; + }); + return held; +} + +bool take_throws(BufferOwner& owner, std::string* what = nullptr) +{ + return wait_until( + [&] + { + try + { + owner.take_latest(); + } + catch (const std::runtime_error& e) + { + if (what) + *what = e.what(); + return true; + } + return false; + }); +} + +} // namespace + +TEST_CASE("every port call runs on one owner thread, never two at once", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + + std::atomic stop{ false }; + std::vector droppers; + for (int t = 0; t < 4; ++t) + { + droppers.emplace_back( + [&] + { + while (!stop) + { + // The temporary's lease goes back on this thread, not the owner's. + owner.take_latest(); + std::this_thread::sleep_for(100us); + } + }); + } + REQUIRE(wait_until([&] { return port.released_total() > 100; })); + owner.request_stop(); + CHECK(owner.wait_stopped(kDeadline)); + stop = true; + for (auto& t : droppers) + t.join(); + owner.close(); + + const auto threads = port.threads(); + REQUIRE(threads.size() == 1); + CHECK(*threads.begin() != std::this_thread::get_id()); + CHECK(port.max_in_flight() == 1); +} + +TEST_CASE("concurrent lease returns release every slot exactly once", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + + std::atomic stop{ false }; + std::vector consumers; + for (int t = 0; t < 6; ++t) + { + consumers.emplace_back( + [&] + { + std::vector held; + while (!stop) + { + if (auto frame = owner.take_latest()) + { + held.push_back(std::move(*frame)); + if (held.size() >= 2) + held.clear(); // drops leases in bulk + } + std::this_thread::sleep_for(50us); + } + }); + } + std::this_thread::sleep_for(300ms); + port.set_budget(0); + stop = true; + for (auto& t : consumers) + t.join(); + owner.close(); + + CHECK(port.acquired_total() > 50); + CHECK(port.acquired_total() == port.released_total()); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); + CHECK(port.max_in_flight() == 1); +} + +TEST_CASE("a newer frame replaces an older one and the replaced buffer is released once", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(5); + BufferOwner owner(port, 8, kWait); + owner.start(); + + REQUIRE(wait_until([&] { return port.acquired_total() == 5; })); + REQUIRE(wait_until([&] { return port.released_total() == 4; })); + + auto frame = owner.take_latest(); + REQUIRE(frame.has_value()); + CHECK(frame->sequence == 5); + CHECK(port.released_total() == 4); + CHECK_FALSE(owner.take_latest().has_value()); + + frame.reset(); + REQUIRE(wait_until([&] { return port.released_total() == 5; })); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); + owner.close(); +} + +TEST_CASE("an exhausted pool stops acquiring and resumes when a lease returns", "[buffer_owner]") +{ + constexpr size_t kPool = 3; + FakePort port(kPool); + port.set_budget(kUnlimited); + BufferOwner owner(port, kPool, kWait); + owner.start(); + + std::vector held = take_frames(owner, kPool); + REQUIRE(held.size() == kPool); + + // Let any in-flight acquire finish, then the count must stay put. + std::this_thread::sleep_for(50ms); + const uint64_t settled = port.acquire_calls(); + std::this_thread::sleep_for(150ms); + CHECK(port.acquire_calls() == settled); + CHECK_FALSE(owner.take_latest().has_value()); + + held.pop_back(); + REQUIRE(wait_until([&] { return port.acquire_calls() > settled; })); + auto next = take_frames(owner, 1); + CHECK(next.size() == 1); + + held.clear(); + next.clear(); + owner.close(); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); +} + +TEST_CASE("stop clears the latest frame; held leases still return afterwards", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + + std::vector held = take_frames(owner, 2); + REQUIRE(held.size() == 2); + const uint64_t acquired_before_stop = port.acquired_total(); + REQUIRE(acquired_before_stop >= 2); + + owner.request_stop(); + REQUIRE(owner.wait_stopped(kDeadline)); + CHECK(port.stop_calls() == 1); + CHECK_FALSE(owner.take_latest().has_value()); + + // Nothing is acquired once stopped. + const uint64_t acquired_at_stop = port.acquired_total(); + std::this_thread::sleep_for(50ms); + CHECK(port.acquired_total() == acquired_at_stop); + + // Every acquired buffer comes back while the owner keeps draining, before close(). + const size_t held_count = held.size(); + held.clear(); + REQUIRE(wait_until([&] { return port.released_total() == port.acquired_total(); })); + CHECK(held_count == 2); + CHECK(port.stop_calls() == 1); + + owner.close(); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); +} + +TEST_CASE("close releases every buffer posted before it and is idempotent", "[buffer_owner]") +{ + SECTION("buffers posted just before close") + { + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + std::vector held = take_frames(owner, 4); + REQUIRE(held.size() == 4); + port.set_budget(0); + held.clear(); + owner.close(); + CHECK(port.balanced()); + owner.close(); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); + } + SECTION("destructor without start") + { + FakePort port(4); + { + BufferOwner owner(port, 4, kWait); + } + CHECK(port.acquire_calls() == 0); + CHECK(port.threads().empty()); + } + SECTION("close without start") + { + FakePort port(4); + BufferOwner owner(port, 4, kWait); + owner.close(); + owner.close(); + CHECK(port.threads().empty()); + } + SECTION("destructor without close") + { + FakePort port(4); + port.set_budget(kUnlimited); + { + BufferOwner owner(port, 4, kWait); + owner.start(); + REQUIRE(wait_until([&] { return port.acquired_total() >= 2; })); + } + CHECK(port.balanced()); + } + SECTION("stop without start completes") + { + FakePort port(4); + BufferOwner owner(port, 4, kWait); + owner.request_stop(); + CHECK(owner.wait_stopped(kDeadline)); + CHECK(port.stop_calls() == 0); + } + SECTION("starting twice throws") + { + FakePort port(4); + BufferOwner owner(port, 4, kWait); + owner.start(); + CHECK_THROWS_AS(owner.start(), std::logic_error); + owner.close(); + } +} + +TEST_CASE("failures surface from take_latest and never lose a buffer", "[buffer_owner]") +{ + SECTION("acquire failed while holding a buffer") + { + FakePort port(4); + port.set_fail_next(true); + port.set_budget(1); + BufferOwner owner(port, 4, kWait); + owner.start(); + std::string what; + REQUIRE(take_throws(owner, &what)); + CHECK_THAT(what, ContainsSubstring("stream exploded")); + REQUIRE(wait_until([&] { return port.released_total() == 1; })); + owner.close(); + CHECK(port.released_total() == 1); + CHECK(port.balanced()); + } + SECTION("release refused") + { + FakePort port(4); + port.set_release_ok(false); + port.set_budget(2); // the second frame replaces, and so releases, the first + BufferOwner owner(port, 4, kWait); + owner.start(); + std::string what; + REQUIRE(take_throws(owner, &what)); + CHECK_THAT(what, ContainsSubstring("refused")); + owner.close(); + } + SECTION("stop_capture refused") + { + FakePort port(4); + port.set_stop_ok(false); + BufferOwner owner(port, 4, kWait); + owner.start(); + owner.request_stop(); + CHECK_FALSE(owner.wait_stopped(kDeadline)); + CHECK(port.stop_calls() == 1); + owner.close(); + } + SECTION("wait_stopped times out when nobody asked to stop") + { + FakePort port(4); + BufferOwner owner(port, 4, kWait); + owner.start(); + CHECK_FALSE(owner.wait_stopped(20ms)); + owner.close(); + } + SECTION("a throwing acquire does not terminate the process") + { + FakePort port(4); + port.set_throw_on_acquire(true); + BufferOwner owner(port, 4, kWait); + owner.start(); + std::string what; + REQUIRE(take_throws(owner, &what)); + CHECK_THAT(what, ContainsSubstring("acquire threw")); + owner.request_stop(); + CHECK(owner.wait_stopped(kDeadline)); + owner.close(); + } +} + +TEST_CASE("a lease keeps its keepalive object alive; the owner holds it only weakly", "[buffer_owner]") +{ + struct Probe + { + explicit Probe(std::atomic& flag) : destroyed(flag) + { + } + ~Probe() + { + destroyed = true; + } + std::atomic& destroyed; + }; + + SECTION("with no leases the owner does not keep it") + { + FakePort port(4); + BufferOwner owner(port, 4, kWait); + std::atomic destroyed{ false }; + std::shared_ptr keep = std::make_shared(destroyed); + owner.set_keepalive(keep); + owner.start(); + keep.reset(); + CHECK(destroyed); + owner.close(); + } + SECTION("a held lease keeps it until dropped") + { + FakePort port(4); + port.set_budget(1); + BufferOwner owner(port, 4, kWait); + std::atomic destroyed{ false }; + std::shared_ptr keep = std::make_shared(destroyed); + owner.set_keepalive(keep); + owner.start(); + + std::vector held = take_frames(owner, 1); + REQUIRE(held.size() == 1); + keep.reset(); + CHECK_FALSE(destroyed); + + held.clear(); + CHECK(destroyed); + REQUIRE(wait_until([&] { return port.released_total() == 1; })); + owner.close(); + CHECK(port.balanced()); + } +} + +TEST_CASE("the mailbox rejects a post beyond its capacity", "[buffer_owner]") +{ + BufferReturnMailbox mailbox(2); + CHECK(mailbox.post(0)); + CHECK(mailbox.post(1)); + CHECK_FALSE(mailbox.post(1)); + CHECK(mailbox.rejected() == 1); + std::vector out; + mailbox.take(out); + CHECK(out == std::vector{ 0, 1 }); +} + +TEST_CASE("stop returns the held frame's buffer before stop_capture begins", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + + // The consumer never takes a frame, so the latest one is always held by the owner. + REQUIRE(wait_until([&] { return port.released_total() >= 3; })); + owner.request_stop(); + REQUIRE(owner.wait_stopped(kDeadline)); + + const auto events = port.events(); + size_t stop_at = events.size(); + for (size_t i = 0; i < events.size(); ++i) + { + if (events[i].kind == FakePort::Event::Kind::StopBegin) + { + stop_at = i; + break; + } + } + REQUIRE(stop_at < events.size()); + + // The last buffer acquired before the stop is the latest frame. + bool found_acquire = false; + uint32_t latest_slot = 0; + for (size_t i = 0; i < stop_at; ++i) + { + if (events[i].kind == FakePort::Event::Kind::Acquire) + { + found_acquire = true; + latest_slot = events[i].slot; + } + } + REQUIRE(found_acquire); + size_t acquired_before = 0; + size_t released_before = 0; + bool latest_released_first = false; + size_t last_acquire = 0; + for (size_t i = 0; i < stop_at; ++i) + if (events[i].kind == FakePort::Event::Kind::Acquire) + last_acquire = i; + for (size_t i = 0; i < stop_at; ++i) + { + acquired_before += events[i].kind == FakePort::Event::Kind::Acquire; + released_before += events[i].kind == FakePort::Event::Kind::Release; + if (i > last_acquire && events[i].kind == FakePort::Event::Kind::Release && events[i].slot == latest_slot) + latest_released_first = true; + } + CHECK(latest_released_first); + CHECK(released_before == acquired_before); + for (size_t i = stop_at + 1; i < events.size(); ++i) + CHECK(events[i].kind != FakePort::Event::Kind::Acquire); + + owner.close(); + CHECK(port.balanced()); + CHECK(owner.rejected_returns() == 0); +} + +TEST_CASE("a refused or throwing release keeps the buffer out and ends acquisition", "[buffer_owner]") +{ + constexpr size_t kPool = 4; + FakePort port(kPool); + port.set_budget(kUnlimited); + + std::string expected; + SECTION("refused") + { + port.set_release_ok(false); + expected = "refused"; + } + SECTION("throws") + { + port.set_throw_on_release(true); + expected = "release threw"; + } + + BufferOwner owner(port, kPool, kWait); + owner.start(); + std::string what; + REQUIRE(take_throws(owner, &what)); + CHECK_THAT(what, ContainsSubstring(expected)); + + // Let anything in flight finish; after that nothing more is acquired or released. + std::this_thread::sleep_for(50ms); + const uint64_t acquires = port.acquire_calls(); + const uint64_t acquired = port.acquired_total(); + const uint64_t releases = port.release_calls(); + std::this_thread::sleep_for(150ms); + CHECK(port.acquire_calls() == acquires); + CHECK(port.release_calls() == releases); + CHECK(acquired <= kPool); + for (size_t i = 0; i < kPool; ++i) + CHECK(port.acquired(i) <= 1); // the refused slot is never handed out again + CHECK(port.release_calls() == 1); // the refused buffer is not retried + CHECK(port.released_total() == 0); // nothing the stream refused counts as returned + CHECK_THROWS_AS(owner.take_latest(), std::runtime_error); + + owner.request_stop(); + CHECK(owner.wait_stopped(kDeadline)); + owner.close(); + CHECK(port.acquire_calls() == acquires); + CHECK(port.acquired_total() == acquired); + CHECK(port.released_total() == 0); + for (size_t i = 0; i < kPool; ++i) + CHECK(port.acquired(i) <= 1); + CHECK_FALSE(owner.closed_cleanly()); // the unresolved buffers are still out + CHECK(port.idle_calls() == 1); +} + +TEST_CASE("a refused release with a full pool does not over-acquire", "[buffer_owner]") +{ + // The stream has more slots than the owner's pool, so only the owner's own count can stop it. + constexpr size_t kPool = 2; + FakePort port(8); + port.set_budget(kUnlimited); + port.set_release_ok(false); + BufferOwner owner(port, kPool, kWait); + owner.start(); + REQUIRE(take_throws(owner)); + std::this_thread::sleep_for(50ms); + const uint64_t acquires = port.acquire_calls(); + std::this_thread::sleep_for(100ms); + CHECK(port.acquire_calls() == acquires); + CHECK(port.acquired_total() <= kPool); + owner.close(); + CHECK_FALSE(owner.closed_cleanly()); +} + +namespace +{ + +/// Index of the single event of `kind`, or events.size() if there is none or more than one. +size_t only_event(const std::vector& events, FakePort::Event::Kind kind) +{ + size_t found = events.size(); + for (size_t i = 0; i < events.size(); ++i) + { + if (events[i].kind == kind) + { + if (found != events.size()) + return events.size(); + found = i; + } + } + return found; +} + +} // namespace + +TEST_CASE("only close waits for the stream to go idle, after every buffer is back", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + + // Frames an encoder still holds when stop is requested; they come back before close(). + std::vector held = take_frames(owner, 2); + REQUIRE(held.size() == 2); + + owner.request_stop(); + REQUIRE(owner.wait_stopped(kDeadline)); + CHECK(port.idle_calls() == 0); + + held.clear(); + owner.close(); + + CHECK(port.idle_calls() == 1); + CHECK(owner.closed_cleanly()); + CHECK(port.balanced()); + CHECK(port.max_in_flight() == 1); + CHECK(port.threads().size() == 1); + CHECK_FALSE(owner.take_latest().has_value()); + + const auto events = port.events(); + const size_t stop_at = only_event(events, FakePort::Event::Kind::StopBegin); + const size_t idle_at = only_event(events, FakePort::Event::Kind::WaitIdle); + REQUIRE(stop_at < events.size()); + REQUIRE(idle_at < events.size()); + CHECK(stop_at < idle_at); + size_t acquires = 0; + size_t releases = 0; + for (size_t i = 0; i < events.size(); ++i) + { + acquires += events[i].kind == FakePort::Event::Kind::Acquire; + if (events[i].kind == FakePort::Event::Kind::Release) + { + ++releases; + CHECK(i < idle_at); + } + if (i > stop_at) + CHECK(events[i].kind != FakePort::Event::Kind::Acquire); + } + CHECK(acquires == releases); + CHECK(idle_at == events.size() - 1); +} + +TEST_CASE("close with leases never dropped ends the owner and reports it unclean", "[buffer_owner]") +{ + FakePort port(8); + port.set_budget(kUnlimited); + BufferOwner owner(port, 8, kWait); + owner.start(); + std::vector held = take_frames(owner, 2); + REQUIRE(held.size() == 2); + + SECTION("after a stop request") + { + owner.request_stop(); + REQUIRE(owner.wait_stopped(kDeadline)); + owner.close(); + CHECK(port.idle_calls() == 1); + } + SECTION("without a stop request") + { + owner.close(); + CHECK(port.idle_calls() == 0); + } + CHECK_FALSE(owner.closed_cleanly()); + CHECK(port.released_total() + 2 == port.acquired_total()); + held.clear(); // lands in a mailbox nobody drains; must be harmless + CHECK(owner.rejected_returns() == 0); +} + +TEST_CASE("the outcome of wait_idle decides closed_cleanly", "[buffer_owner]") +{ + FakePort port(4); + port.set_budget(kUnlimited); + bool expect_clean = false; + std::string expected; + SECTION("returns false") + { + port.set_idle_ok(false); + expected = "did not go idle"; + } + SECTION("throws") + { + port.set_throw_on_idle(true); + expected = "wait_idle threw"; + } + SECTION("returns true") + { + expect_clean = true; + } + + BufferOwner owner(port, 4, kWait); + owner.start(); + REQUIRE(wait_until([&] { return port.acquired_total() >= 2; })); + owner.request_stop(); + REQUIRE(owner.wait_stopped(kDeadline)); + owner.close(); + + CHECK(port.idle_calls() == 1); + CHECK(port.balanced()); + CHECK(owner.closed_cleanly() == expect_clean); + if (expect_clean) + { + CHECK_NOTHROW(owner.take_latest()); + } + else + { + std::string what; + try + { + owner.take_latest(); + } + catch (const std::runtime_error& e) + { + what = e.what(); + } + CHECK_THAT(what, ContainsSubstring(expected)); + } +} + +TEST_CASE("close without a stop request does not wait for idle", "[buffer_owner]") +{ + FakePort port(4); + port.set_budget(kUnlimited); + BufferOwner owner(port, 4, kWait); + owner.start(); + REQUIRE(wait_until([&] { return port.acquired_total() >= 2; })); + owner.close(); + CHECK(port.idle_calls() == 0); + CHECK(port.stop_calls() == 0); + CHECK(owner.closed_cleanly()); + CHECK(port.balanced()); +} + +TEST_CASE("an owner that never started is cleanly closed", "[buffer_owner]") +{ + FakePort port(4); + BufferOwner owner(port, 4, kWait); + CHECK(owner.closed_cleanly()); + owner.close(); + owner.close(); + CHECK(owner.closed_cleanly()); + CHECK(port.idle_calls() == 0); + CHECK(port.threads().empty()); +} diff --git a/src/plugins/sensing/tests/test_camera_backend.cpp b/src/plugins/sensing/tests/test_camera_backend.cpp new file mode 100644 index 0000000000..5a28ba89f6 --- /dev/null +++ b/src/plugins/sensing/tests/test_camera_backend.cpp @@ -0,0 +1,167 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "core/camera_backend.hpp" + +#include +#include + +#include +#include +#include +#include + +using Catch::Matchers::ContainsSubstring; +using plugins::sensing::BackendProbe; +using plugins::sensing::CameraBackend; +using plugins::sensing::FrameLease; +using plugins::sensing::parse_camera_backend; +using plugins::sensing::select_camera_backend; + +namespace +{ + +BackendProbe probe(CameraBackend backend, bool available, bool matches, std::string detail = "") +{ + BackendProbe p; + p.backend = backend; + p.available = available; + p.matches = matches; + p.detail = std::move(detail); + return p; +} + +std::string error_of(CameraBackend requested, const std::vector& probes) +{ + try + { + select_camera_backend(requested, probes); + } + catch (const std::runtime_error& e) + { + return e.what(); + } + FAIL("select_camera_backend did not throw"); + return {}; +} + +} // namespace + +TEST_CASE("parse_camera_backend accepts the three names and nothing else", "[sensing][backend]") +{ + CHECK(parse_camera_backend("auto") == CameraBackend::Auto); + CHECK(parse_camera_backend("argus") == CameraBackend::Argus); + CHECK(parse_camera_backend("sipl") == CameraBackend::Sipl); + CHECK_FALSE(parse_camera_backend("garbage")); + CHECK_FALSE(parse_camera_backend("")); + CHECK_FALSE(parse_camera_backend("SIPL")); +} + +TEST_CASE("Auto picks the only matching backend", "[sensing][backend]") +{ + CHECK(select_camera_backend(CameraBackend::Auto, { probe(CameraBackend::Argus, true, true), + probe(CameraBackend::Sipl, true, false, "no pipelines") }) == + CameraBackend::Argus); + CHECK(select_camera_backend(CameraBackend::Auto, { probe(CameraBackend::Argus, false, false, "no libargus"), + probe(CameraBackend::Sipl, true, true) }) == CameraBackend::Sipl); +} + +TEST_CASE("Auto with no match throws and names the override", "[sensing][backend]") +{ + const auto message = error_of(CameraBackend::Auto, { probe(CameraBackend::Argus, false, false, "no libargus"), + probe(CameraBackend::Sipl, true, false, "no pipelines") }); + CHECK_THAT(message, ContainsSubstring("--camera-backend")); + CHECK_THAT(message, ContainsSubstring("no libargus")); + CHECK_THAT(message, ContainsSubstring("no pipelines")); +} + +TEST_CASE("Auto with both matching throws and names the override", "[sensing][backend]") +{ + const auto message = error_of( + CameraBackend::Auto, { probe(CameraBackend::Argus, true, true), probe(CameraBackend::Sipl, true, true) }); + CHECK_THAT(message, ContainsSubstring("both")); + CHECK_THAT(message, ContainsSubstring("--camera-backend")); +} + +TEST_CASE("An explicit request that matches is returned", "[sensing][backend]") +{ + const std::vector both = { probe(CameraBackend::Argus, true, true), + probe(CameraBackend::Sipl, true, true) }; + CHECK(select_camera_backend(CameraBackend::Argus, both) == CameraBackend::Argus); + CHECK(select_camera_backend(CameraBackend::Sipl, both) == CameraBackend::Sipl); +} + +TEST_CASE("An explicit request that does not match throws with its own detail and never falls back", "[sensing][backend]") +{ + const auto message = error_of(CameraBackend::Sipl, { probe(CameraBackend::Argus, true, true), + probe(CameraBackend::Sipl, true, false, "sipl found nothing") }); + CHECK_THAT(message, ContainsSubstring("sipl found nothing")); + CHECK_THAT(message, ContainsSubstring("--camera-backend=sipl")); +} + +TEST_CASE("A probe that matches but is not available is never selected", "[sensing][backend]") +{ + const std::vector probes = { probe(CameraBackend::Argus, false, true, "argus unavailable") }; + CHECK_THROWS_AS(select_camera_backend(CameraBackend::Auto, probes), std::runtime_error); + CHECK_THROWS_AS(select_camera_backend(CameraBackend::Argus, probes), std::runtime_error); +} + +TEST_CASE("An explicit request for a backend missing from the probes throws", "[sensing][backend]") +{ + const auto message = error_of(CameraBackend::Argus, { probe(CameraBackend::Sipl, true, true) }); + CHECK_THAT(message, ContainsSubstring("--camera-backend=argus")); +} + +TEST_CASE("FrameLease releases once on destruction", "[sensing][lease]") +{ + int released = 0; + { + FrameLease lease([&] { ++released; }); + CHECK(released == 0); + } + CHECK(released == 1); +} + +TEST_CASE("A moved-from FrameLease does not release", "[sensing][lease]") +{ + int released = 0; + { + FrameLease a([&] { ++released; }); + { + FrameLease b(std::move(a)); + } + CHECK(released == 1); + } + CHECK(released == 1); +} + +TEST_CASE("FrameLease reset releases once, then not again", "[sensing][lease]") +{ + int released = 0; + { + FrameLease lease([&] { ++released; }); + lease.reset(); + CHECK(released == 1); + lease.reset(); + CHECK(released == 1); + } + CHECK(released == 1); +} + +TEST_CASE("FrameLease move-assign releases the previous target first", "[sensing][lease]") +{ + std::vector order; + { + FrameLease a([&] { order.push_back(1); }); + FrameLease b([&] { order.push_back(2); }); + a = std::move(b); + REQUIRE(order == std::vector{ 1 }); + } + CHECK(order == std::vector{ 1, 2 }); +} + +TEST_CASE("A default FrameLease releases nothing", "[sensing][lease]") +{ + FrameLease lease; + lease.reset(); +} diff --git a/src/plugins/sensing/tests/test_sensor_clock.cpp b/src/plugins/sensing/tests/test_sensor_clock.cpp new file mode 100644 index 0000000000..753213c9ed --- /dev/null +++ b/src/plugins/sensing/tests/test_sensor_clock.cpp @@ -0,0 +1,95 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "core/sensor_clock.hpp" + +#include + +using plugins::sensing::ClockPair; +using plugins::sensing::SensorClockMap; +using plugins::sensing::SensorTimeDomain; + +namespace +{ +constexpr int64_t kHz = 31'250'000; // 32 ns per tick +constexpr int64_t kMs = 1'000'000; + +SensorClockMap counter_map(int& reads, int64_t tick0 = 1'000'000, int64_t mono0 = 50'000 * kMs) +{ + SensorClockMap::Config cfg; + cfg.domain = SensorTimeDomain::GenericCounterTicks; + cfg.counter_hz = kHz; + return SensorClockMap(cfg, + [&reads, tick0, mono0] + { + ++reads; + return ClockPair{ tick0, mono0, 1000 }; + }); +} +} // namespace + +TEST_CASE("counter ticks convert through the anchor at the counter rate") +{ + int reads = 0; + auto map = counter_map(reads); + // 100 ms after the anchor = 3,125,000 ticks. + const int64_t ready = 50'200 * kMs; + REQUIRE(map.to_monotonic_ns(1'000'000 + 3'125'000, ready, ready) == 50'100 * kMs); +} + +TEST_CASE("a capture before the anchor converts with a negative delta") +{ + int reads = 0; + auto map = counter_map(reads); + const int64_t ready = 50'010 * kMs; + // 3,125 ticks before the anchor = 100 us. + REQUIRE(map.to_monotonic_ns(1'000'000 - 3'125, ready, ready) == 50'000 * kMs - 100'000); +} + +TEST_CASE("raw nanoseconds convert one to one") +{ + SensorClockMap::Config cfg; + cfg.domain = SensorTimeDomain::MonotonicRawNs; + SensorClockMap map(cfg, [] { return ClockPair{ 7'000 * kMs, 7'002 * kMs, 100 }; }); + REQUIRE(map.to_monotonic_ns(7'010 * kMs, 7'050 * kMs, 7'050 * kMs) == 7'012 * kMs); +} + +TEST_CASE("the anchor is refreshed at the configured rate, not per frame") +{ + int reads = 0; + auto map = counter_map(reads); + const int64_t t = 50'100 * kMs; + for (int i = 0; i < 30; ++i) + map.to_monotonic_ns(1'000'000 + 3'125'000, t, t + i * 10 * kMs); + REQUIRE(reads == 1); + map.to_monotonic_ns(1'000'000 + 3'125'000, 51'200 * kMs, 51'200 * kMs); + REQUIRE(reads == 2); +} + +TEST_CASE("a capture in the future of the ready time is rejected") +{ + int reads = 0; + auto map = counter_map(reads); + REQUIRE_THROWS_AS(map.to_monotonic_ns(1'000'000 + 3'125'000, 50'050 * kMs, 50'050 * kMs), std::runtime_error); +} + +TEST_CASE("a capture implausibly old is rejected, as with the wrong rate") +{ + int reads = 0; + auto map = counter_map(reads); + // Treating nanoseconds as ticks puts the capture far from the anchor. + REQUIRE_THROWS_AS(map.to_monotonic_ns(1'000'000 + 400'000'000, 50'100 * kMs, 50'100 * kMs), std::runtime_error); + int more = 0; + auto old_map = counter_map(more, 200'000'000); + // 199,999,999 ticks before the anchor is about 6.4 s. + REQUIRE_THROWS_AS(old_map.to_monotonic_ns(1, 50'100 * kMs, 50'100 * kMs), std::runtime_error); +} + +TEST_CASE("a domain is required, and counter ticks need a rate") +{ + SensorClockMap::Config cfg; + auto reader = [] { return ClockPair{}; }; + REQUIRE_THROWS_AS(SensorClockMap(cfg, reader), std::invalid_argument); + cfg.domain = SensorTimeDomain::GenericCounterTicks; + REQUIRE_THROWS_AS(SensorClockMap(cfg, reader), std::invalid_argument); +} diff --git a/src/sensing_control/CMakeLists.txt b/src/sensing_control/CMakeLists.txt new file mode 100644 index 0000000000..997b64f33b --- /dev/null +++ b/src/sensing_control/CMakeLists.txt @@ -0,0 +1,54 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The camera producer's management channel: an authenticated WebSocket server, the request protocol, the status the +# producer reports, and plain-language camera fault messages. No CUDA or SIPL, so it builds and is tested on a PC. +include(FetchContent) +FetchContent_Declare( + nlohmann_json + GIT_REPOSITORY https://github.com/nlohmann/json.git + GIT_TAG v3.11.3 + GIT_SHALLOW TRUE +) +set(JSON_BuildTests OFF CACHE INTERNAL "") +isaac_teleop_third_party_scope_begin() +FetchContent_MakeAvailable(nlohmann_json) +isaac_teleop_third_party_scope_end() + +add_library(sensing_control STATIC + websocket.cpp + producer_status.cpp + camera_fault.cpp + control_handler.cpp + control_server.cpp + producer_supervisor.cpp + driver_log_capture.cpp + inc/sensing_control/websocket.hpp + inc/sensing_control/producer_status.hpp + inc/sensing_control/camera_fault.hpp + inc/sensing_control/control_handler.hpp + inc/sensing_control/control_server.hpp + inc/sensing_control/producer_supervisor.hpp + inc/sensing_control/driver_log_capture.hpp +) +add_library(sensing::control ALIAS sensing_control) + +set_target_properties(sensing_control PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(sensing_control + INTERFACE + $ +) + +target_link_libraries(sensing_control + PUBLIC + log_bridge::log_bridge_core + PRIVATE + nlohmann_json::nlohmann_json +) + +add_subdirectory(mock_producer) + +if(BUILD_TESTING) + add_subdirectory(tests) +endif() diff --git a/src/sensing_control/camera_fault.cpp b/src/sensing_control/camera_fault.cpp new file mode 100644 index 0000000000..26226c9b6a --- /dev/null +++ b/src/sensing_control/camera_fault.cpp @@ -0,0 +1,182 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/camera_fault.hpp" + +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +struct Signature +{ + /// A fragment of the text the component was seen to produce for that situation. + std::string_view needle; + std::string_view code; + ProducerState state; + std::string_view message; +}; + +constexpr ProducerState kCamera = ProducerState::CameraFault; +constexpr ProducerState kOther = ProducerState::Error; + +constexpr std::string_view kPowerAndCables = + "Check that each camera's power cable is connected and that its GMSL cable is seated at both ends."; + +// Most specific first; the first needle found wins. Extend this table, not the logic, when a new failure is seen. +// Messages say what to check; the original text is appended by diagnose_failure(). +constexpr Signature kSignatures[] = { + // --- What the capture driver itself says about an absent camera (its log lines are attached to the failure) -- + { "OPEN_CIRCUIT", "camera_power_missing", kCamera, + "A camera's power switch reports an open circuit: the camera is not drawing power. Connect the camera's power " + "cable." }, + { "CIRCUIT_OPEN", "camera_power_missing", kCamera, + "A camera's power switch reports an open circuit: the camera is not drawing power. Connect the camera's power " + "cable." }, + { "lock timeout", "camera_link_not_locked", kCamera, + "A camera's GMSL link did not lock. The camera is unpowered or its GMSL cable is not connected. Check its power " + "cable and that its GMSL cable is seated at both ends." }, + { "DES_LOCK_ERR", "camera_link_not_locked", kCamera, + "A camera's GMSL link lost lock. The camera lost power or its GMSL cable came loose. Check its power cable and " + "that its GMSL cable is seated at both ends." }, + { "CAMERA_DETECT", "camera_not_detected", kCamera, + "A camera was not detected. It is unpowered or its GMSL cable is not connected. Check its power cable and that " + "its GMSL cable is seated at both ends." }, + { "Parse failed after", "camera_not_responding", kCamera, + "A camera stopped answering while it was being set up. It is unpowered or its GMSL cable is not connected. Check " + "its power cable and that its GMSL cable is seated at both ends." }, + { "SIPLStatus 8", "camera_partly_started", kCamera, + "Some cameras started and at least one did not. Check the power cable and GMSL cable of the camera that is " + "missing." }, + // --- A camera is gone or never came up --------------------------------------------------------------------- + { "sensor control write failure", "camera_control_bus_failed", kCamera, + "A camera's control bus is not answering (settings cannot be written to the sensor). The camera lost power or " + "its GMSL cable came loose. Check its power cable and GMSL cable." }, + { "input stream failure", "camera_stream_failed", kCamera, + "A camera's video stream is corrupt or missing. Check its GMSL cable and connectors for damage or a loose " + "fit, and its power." }, + { "image processing failure", "camera_isp_failed", kCamera, + "The camera image pipeline failed. Check the camera's cables; if it persists, restart the producer." }, + { "interrupt localization failure", "camera_hardware_fault", kCamera, + "The camera hardware reported a fault that could not be traced. Power-cycle the cameras." }, + { "deserializer failure", "camera_link_failed", kCamera, + "A GMSL deserializer reported a failure. A camera lost power or its GMSL cable came loose. Check each camera's " + "power cable and GMSL cable." }, + { "serializer failure", "camera_link_failed", kCamera, + "A camera's serializer stopped responding. The camera lost power or its GMSL cable came loose. Check its power " + "cable and GMSL cable." }, + { "sensor failure", "camera_sensor_failed", kCamera, + "A camera's image sensor reported a failure. Check the camera's power and cables; if it persists, power-cycle " + "the camera." }, + { "link lock", "camera_link_not_locked", kCamera, + "A camera's GMSL link did not lock. Check each camera's power cable and that its GMSL cable is seated at both " + "ends." }, + { "Failed to detect", "camera_not_detected", kCamera, + "A camera was not detected. Check each camera's power cable and that its GMSL cable is seated at both ends." }, + { "no frame for", "camera_no_frames", kCamera, + "A camera stopped delivering frames. Check its power and GMSL cable; the producer restarts it automatically." }, + { "INvSIPLCamera::Init failed", "camera_start_failed", kCamera, + "The camera system could not start. The usual cause is a camera with no power or no GMSL link. Check each " + "camera's power cable and GMSL cable." }, + { "INvSIPLCamera::SetPlatformCfg failed", "camera_config_rejected", kCamera, + "The camera system rejected the platform configuration. Check --platform-config, --config-name and " + "--link-masks against the installed camera drivers." }, + { "INvSIPLCamera::Start failed", "camera_start_failed", kCamera, + "The cameras initialised but would not start streaming. Check each camera's power and GMSL cable and try " + "again; if it persists, power-cycle the cameras." }, + { "SIPLStatus 10", "camera_start_failed", kCamera, + "The camera system reported an error. The usual cause is a camera with no power or no GMSL link. Check each " + "camera's power cable and GMSL cable." }, + { "pipeline error", "camera_pipeline_error", kCamera, + "A camera pipeline reported an error while streaming. Check each camera's power cable and GMSL cable." }, + { "capture error", "camera_capture_error", kCamera, + "Capturing from a camera failed. Check each camera's power cable and GMSL cable." }, + { "no modules left in", "camera_link_mask_mismatch", kCamera, + "No camera matches the configured link masks. Check --link-masks and --config-name against how the cameras " + "are wired." }, + { "declares no sensors", "camera_config_empty", kCamera, + "The platform configuration names no cameras. Check --platform-config and --config-name." }, + { "no SIPL pipeline with index", "camera_not_in_config", kCamera, + "A requested sensor is not in the platform configuration. Run --list-sensors for the valid numbers." }, + { "link masks are required", "camera_config_missing", kCamera, + "The camera configuration needs --link-masks (one mask per deserializer)." }, + { "cannot open NITO file", "camera_tuning_missing", kCamera, + "The camera's image-processing tuning file is missing. Install the camera tuning files (see --nito-dir)." }, + { "short read on NITO file", "camera_tuning_invalid", kCamera, + "The camera's image-processing tuning file is truncated or corrupt. Reinstall the camera tuning files." }, + { "Argus: no camera provider", "camera_service_down", kCamera, + "The Argus camera service is not running. Start it with: sudo systemctl start nvargus-daemon." }, + { "Argus: no camera device", "camera_not_detected", kCamera, + "The requested Argus camera was not found. Check the camera's cable and power and the --add-stream sensor " + "number." }, + { "Argus: no camera", "camera_not_detected", kCamera, + "The requested Argus camera was not found. Check the camera's cable and power and the --add-stream sensor " + "number." }, + { "Argus:", "camera_error", kCamera, + "The Argus camera stack reported an error. Check the camera's cable and power; the Argus service " + "(nvargus-daemon) may need a restart." }, + + // --- Frames could not reach the GPU or the encoder ---------------------------------------------------------- + { "NvBufSurfaceMapEglImage", "gpu_buffer_error", kCamera, + "Camera frames could not be mapped for the GPU. This is usually the graphics environment: the producer must " + "not run with DISPLAY set to a remote X server, and the Tegra EGL libraries must be installed." }, + { "NvmmNvSciBufToNvBufSurface", "gpu_buffer_error", kCamera, + "Camera frames could not be converted for the GPU. Check that the Jetson multimedia libraries match the " + "installed L4T release." }, + { "unsupported CUDA EGL frame type", "gpu_buffer_error", kCamera, + "The camera delivered a frame format the GPU conversion does not support. Check the camera mode and L4T " + "release." }, + { "CUDA", "gpu_error", kOther, + "A CUDA call failed. Check that no other process is using the GPU and that the driver matches the installed " + "L4T release." }, + { "createVideoEncoder failed", "encoder_unavailable", kOther, + "The hardware video encoder is not available (/dev/v4l2-nvenc is missing or the driver is not loaded). Check " + "the L4T installation." }, + { "JetsonEncoder", "encoder_error", kOther, + "The hardware video encoder reported an error. Another process may be holding the encoder; if it persists, " + "reduce --bitrate or the stream count." }, + + { "bad_alloc", "out_of_memory", kOther, + "The producer ran out of memory. Close other programs on the Orin or reduce the number of streams." }, + { "Permission denied", "permission_denied", kOther, + "The producer is not allowed to use a device or file. Check that its service user is in the video group." }, + { "No such file or directory", "file_missing", kOther, + "A file or device the producer needs is missing. Check the paths in its configuration." }, + { "Device or resource busy", "device_busy", kOther, + "A camera or encoder device is in use by another process. Stop any other camera program (for example a " + "second copy of this producer) and try again." }, +}; + +} // namespace + +Diagnosis diagnose_failure(std::string_view what) +{ + for (const auto& signature : kSignatures) + { + if (what.find(signature.needle) != std::string_view::npos) + { + return { { std::string(signature.code), std::string(signature.message) + + " The producer retries automatically. (" + std::string(what) + + ")" }, + signature.state }; + } + } + // Unknown text: still a code and the original words, and a camera fault because a start-up failure that nothing + // else explains is most often the cameras. + return { { "startup_failed", + "The producer could not start. The producer retries automatically. (" + std::string(what) + ")" }, + ProducerState::CameraFault }; +} + +Fault diagnose_camera_failure(std::string_view what) +{ + return diagnose_failure(what).fault; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/control_handler.cpp b/src/sensing_control/control_handler.cpp new file mode 100644 index 0000000000..8b939607bd --- /dev/null +++ b/src/sensing_control/control_handler.cpp @@ -0,0 +1,153 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/control_handler.hpp" + +#include +#include + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +using Json = nlohmann::json; + +Json error_object(const std::string& code, const std::string& message) +{ + return Json{ { "code", code }, { "message", message } }; +} + +std::string failure(const Json& id, const std::string& code, const std::string& message) +{ + return Json{ { "id", id }, { "ok", false }, { "error", error_object(code, message) } }.dump(); +} + +/// An IP literal or a plain host name; anything else (spaces, slashes, a URL) is refused with a reason. +bool valid_host(const std::string& host) +{ + if (host.empty() || host.size() > 253) + return false; + unsigned char buffer[sizeof(in6_addr)]; + if (::inet_pton(AF_INET, host.c_str(), buffer) == 1 || ::inet_pton(AF_INET6, host.c_str(), buffer) == 1) + return true; + return std::all_of(host.begin(), host.end(), [](unsigned char c) { return std::isalnum(c) || c == '.' || c == '-'; }) && + host.front() != '-' && host.front() != '.'; +} + +} // namespace + +std::string status_json(const StatusSnapshot& snapshot) +{ + Json streams = Json::array(); + for (const auto& stream : snapshot.streams) + { + streams.push_back(Json{ + { "stream", stream.track }, + { "sensor", stream.sensor_id }, + { "direct", Json{ { "host", stream.direct_host }, + { "port", stream.direct_port }, + { "connected", stream.direct_connected }, + { "error", stream.direct_error } } }, + { "frames", stream.frames }, + { "published", stream.published }, + }); + } + Json out{ + { "state", to_string(snapshot.state) }, + { "release", snapshot.release }, + { "uptime_seconds", snapshot.uptime_seconds }, + { "streams", streams }, + }; + if (snapshot.fault) + out["fault"] = error_object(snapshot.fault->code, snapshot.fault->message); + return out.dump(); +} + +std::string state_event(const StatusSnapshot& snapshot) +{ + return Json{ + { "type", "event" }, { "event", "state_changed" }, { "status", Json::parse(status_json(snapshot)) } + }.dump(); +} + +std::string error_event(const Fault& fault) +{ + return Json{ { "type", "event" }, { "event", "error" }, { "error", error_object(fault.code, fault.message) } }.dump(); +} + +ControlHandler::ControlHandler(const ProducerStatus& status, ControlActions& actions) + : m_status(status), m_actions(actions) +{ +} + +std::string ControlHandler::handle(const std::string& text) const +{ + Json request = Json::parse(text, nullptr, /*allow_exceptions=*/false); + if (request.is_discarded() || !request.is_object()) + return failure(nullptr, "bad_json", "the message must be one JSON object"); + + const Json id = request.contains("id") ? request["id"] : Json(nullptr); + if (!request.contains("type") || !request["type"].is_string()) + return failure(id, "missing_type", "the request needs a string \"type\""); + const std::string type = request["type"]; + + if (type == "ping") + return Json{ { "id", id }, { "ok", true }, { "type", "pong" } }.dump(); + + if (type == "get_status") + { + return Json{ + { "id", id }, { "ok", true }, { "type", "status" }, { "status", Json::parse(status_json(m_status.snapshot())) } + }.dump(); + } + + if (type == "set_direct_target") + { + if (!request.contains("stream") || !request["stream"].is_string()) + return failure(id, "missing_stream", "set_direct_target needs a string \"stream\" (a track name)"); + if (!request.contains("host") || !request["host"].is_string()) + return failure(id, "missing_host", "set_direct_target needs a string \"host\" (\"\" turns the link off)"); + const std::string stream = request["stream"]; + const std::string host = request["host"]; + + std::uint16_t port = 0; + if (!host.empty()) + { + if (!valid_host(host)) + return failure(id, "invalid_host", "\"host\" must be an IP address or a host name, not \"" + host + "\""); + if (!request.contains("port") || !request["port"].is_number_integer() || request["port"] < 1 || + request["port"] > 65535) + return failure(id, "invalid_port", "\"port\" must be an integer from 1 to 65535"); + port = request["port"].get(); + } + + const auto known = m_status.snapshot().streams; + if (std::none_of(known.begin(), known.end(), [&](const StreamStatus& s) { return s.track == stream; })) + { + std::string names; + for (const auto& s : known) + names += (names.empty() ? "" : ", ") + s.track; + return failure(id, "unknown_stream", "no stream named \"" + stream + "\"; the streams are: " + names); + } + + const ActionResult result = m_actions.set_direct_target(stream, host, port); + if (!result.ok) + return failure(id, result.code, result.message); + return Json{ + { "id", id }, { "ok", true }, { "type", "status" }, { "status", Json::parse(status_json(m_status.snapshot())) } + }.dump(); + } + + return failure( + id, "unknown_type", "unknown request type \"" + type + "\"; supported: ping, get_status, set_direct_target"); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/control_server.cpp b/src/sensing_control/control_server.cpp new file mode 100644 index 0000000000..36b95b37c9 --- /dev/null +++ b/src/sensing_control/control_server.cpp @@ -0,0 +1,377 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/control_server.hpp" + +#include "inc/sensing_control/websocket.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +std::shared_ptr logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.sensing.ControlServer"); +} + +constexpr std::size_t kMaxHandshakeBytes = 8 * 1024; +/// Room beyond one maximum message for the frame header, the mask and a following control frame. +constexpr std::size_t kFrameHeadroom = 4096; +constexpr std::size_t kMaxOutputBytes = 1 << 20; +/// Frames handled per client per poll round, so one chatty peer cannot hold the server thread. +constexpr int kMaxFramesPerRound = 16; +constexpr auto kHandshakeTimeout = std::chrono::seconds(5); + +void set_non_blocking(int fd) +{ + ::fcntl(fd, F_SETFL, ::fcntl(fd, F_GETFL, 0) | O_NONBLOCK); +} + +} // namespace + +std::string read_secret_file(const std::string& path) +{ + std::ifstream file(path); + if (!file) + throw std::runtime_error("cannot read " + path); + std::string secret; + std::getline(file, secret); + const auto last = secret.find_last_not_of(" \t\r\n"); + secret.erase(last == std::string::npos ? 0 : last + 1); + const auto first = secret.find_first_not_of(" \t"); + secret.erase(0, first == std::string::npos ? secret.size() : first); + if (secret.empty()) + throw std::runtime_error(path + " is empty"); + return secret; +} + +struct ControlServer::Client +{ + int fd = -1; + bool open = false; ///< the handshake completed + bool closing = false; ///< flush the output, then drop the connection + std::string input; + std::string output; + std::string message; ///< a fragmented text message being assembled + bool fragmenting = false; ///< a text message was started without its final fragment + std::string peer; + std::chrono::steady_clock::time_point accepted = std::chrono::steady_clock::now(); +}; + +ControlServer::ControlServer(const ControlServerConfig& config, MessageHandler on_message) + : m_config(config), m_on_message(std::move(on_message)) +{ + if (m_config.token.empty()) + throw std::runtime_error("the management channel needs a token: it can redirect camera video"); + + m_listen_fd = ::socket(AF_INET, SOCK_STREAM, 0); + if (m_listen_fd < 0) + throw std::runtime_error(std::string("management channel: cannot create a socket: ") + std::strerror(errno)); + const int one = 1; + ::setsockopt(m_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(m_config.port); + if (::inet_pton(AF_INET, m_config.bind_address.c_str(), &address.sin_addr) != 1) + { + ::close(m_listen_fd); + throw std::runtime_error("management channel: \"" + m_config.bind_address + "\" is not an IPv4 address"); + } + if (::bind(m_listen_fd, reinterpret_cast(&address), sizeof(address)) != 0 || ::listen(m_listen_fd, 16) != 0) + { + const std::string why = std::strerror(errno); + ::close(m_listen_fd); + throw std::runtime_error("management channel: cannot listen on " + m_config.bind_address + ":" + + std::to_string(m_config.port) + ": " + why); + } + socklen_t length = sizeof(address); + ::getsockname(m_listen_fd, reinterpret_cast(&address), &length); + m_port = ntohs(address.sin_port); + set_non_blocking(m_listen_fd); + m_wake_fd = ::eventfd(0, EFD_NONBLOCK); + + logger()->info("management channel listening on {}:{}", m_config.bind_address, m_port); + m_thread = std::thread(&ControlServer::run, this); +} + +ControlServer::~ControlServer() +{ + m_stop = true; + wake(); + if (m_thread.joinable()) + m_thread.join(); + ::close(m_listen_fd); + ::close(m_wake_fd); +} + +void ControlServer::wake() +{ + const std::uint64_t one = 1; + [[maybe_unused]] const ssize_t written = ::write(m_wake_fd, &one, sizeof(one)); +} + +void ControlServer::broadcast(const std::string& text) +{ + { + std::lock_guard lock(m_mutex); + // Notifications are advisory: if nobody drains them, keep only the newest. + if (m_broadcasts.size() >= 64) + m_broadcasts.erase(m_broadcasts.begin()); + m_broadcasts.push_back(text); + } + wake(); +} + +void ControlServer::run() +{ + std::vector> clients; + + const auto drop = [&](std::size_t index, const char* why) + { + logger()->info("management client {} disconnected: {}", clients[index]->peer, why); + ::close(clients[index]->fd); + clients.erase(clients.begin() + static_cast(index)); + }; + const auto refuse = [&](Client& client, const websocket::Handshake& handshake) + { + client.output += websocket::handshake_response(handshake); + client.closing = true; + }; + + while (!m_stop) + { + std::vector fds; + fds.push_back({ m_listen_fd, POLLIN, 0 }); + fds.push_back({ m_wake_fd, POLLIN, 0 }); + for (const auto& client : clients) + fds.push_back({ client->fd, static_cast(POLLIN | (client->output.empty() ? 0 : POLLOUT)), 0 }); + + const int ready = ::poll(fds.data(), fds.size(), 500); + if (ready < 0 && errno != EINTR) + { + logger()->error("management channel poll failed: {}", std::strerror(errno)); + break; + } + + if (fds[1].revents & POLLIN) + { + std::uint64_t counter = 0; + [[maybe_unused]] const ssize_t got = ::read(m_wake_fd, &counter, sizeof(counter)); + std::vector pending; + { + std::lock_guard lock(m_mutex); + pending.swap(m_broadcasts); + } + for (const auto& text : pending) + for (auto& client : clients) + if (client->open && !client->closing) + client->output += websocket::encode_frame(websocket::kText, text); + } + + if (fds[0].revents & POLLIN) + { + while (true) + { + sockaddr_in peer{}; + socklen_t length = sizeof(peer); + const int fd = ::accept(m_listen_fd, reinterpret_cast(&peer), &length); + if (fd < 0) + break; + if (clients.size() >= m_config.max_clients) + { + ::close(fd); + continue; + } + set_non_blocking(fd); + const int one = 1; + ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + auto client = std::make_unique(); + client->fd = fd; + char text[INET_ADDRSTRLEN] = {}; + ::inet_ntop(AF_INET, &peer.sin_addr, text, sizeof(text)); + client->peer = std::string(text) + ":" + std::to_string(ntohs(peer.sin_port)); + clients.push_back(std::move(client)); + } + } + + // The clients that were polled are the first ones in the list; those accepted just now wait for the next turn. + const std::size_t polled = fds.size() - 2; + for (std::size_t i = polled; i-- > 0;) + { + Client& client = *clients[i]; + const short events = fds[i + 2].revents; + bool dead = (events & (POLLERR | POLLNVAL)) != 0; + + if (!dead && (events & (POLLIN | POLLHUP))) + { + // Read only what the protocol could still accept: the rest waits in the kernel (TCP backpressure) until + // the frames already buffered are handled. A peer that streams without end therefore cannot grow this + // buffer or keep the thread here. + const std::size_t input_cap = + client.open ? m_config.max_message_bytes + kFrameHeadroom : kMaxHandshakeBytes + 1; + char buffer[4096]; + while (client.input.size() < input_cap) + { + const ssize_t n = ::recv(client.fd, buffer, sizeof(buffer), 0); + if (n > 0) + { + client.input.append(buffer, static_cast(n)); + continue; + } + if (n == 0 || (errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR)) + dead = true; + break; + } + } + + // Handshake: one HTTP request, then WebSocket frames. + if (!dead && !client.open && !client.closing) + { + const std::size_t end = client.input.find("\r\n\r\n"); + if (end != std::string::npos) + { + websocket::Handshake handshake = websocket::parse_handshake(client.input.substr(0, end + 4)); + client.input.erase(0, end + 4); + if (handshake.ok && !websocket::constant_time_equal(handshake.token, m_config.token)) + { + handshake.ok = false; + handshake.status = 401; + handshake.reason = "a valid token is required"; + logger()->warn("management client {} refused: missing or wrong token", client.peer); + } + if (handshake.ok) + { + client.open = true; + m_clients.fetch_add(1); + client.output += websocket::handshake_response(handshake); + logger()->info("management client {} connected", client.peer); + } + else + { + refuse(client, handshake); + } + } + else if (client.input.size() > kMaxHandshakeBytes) + { + websocket::Handshake bad; + bad.reason = "request too large"; + refuse(client, bad); + } + else if (std::chrono::steady_clock::now() - client.accepted > kHandshakeTimeout) + { + dead = true; + } + } + + for (int handled = 0; !dead && client.open && !client.closing && handled < kMaxFramesPerRound; ++handled) + { + if (client.output.size() > kMaxOutputBytes) + { + dead = true; // a client that does not read its replies is cut off rather than buffered for + break; + } + const auto parsed = websocket::parse_frame(client.input, m_config.max_message_bytes); + if (parsed.status == websocket::ParseStatus::NeedMore) + break; + if (parsed.status == websocket::ParseStatus::Error) + { + client.output += websocket::encode_close(parsed.close_code, parsed.error); + client.closing = true; + break; + } + client.input.erase(0, parsed.consumed); + const auto& frame = parsed.frame; + if (frame.opcode == websocket::kPing) + { + client.output += websocket::encode_frame(websocket::kPong, frame.payload); + } + else if (frame.opcode == websocket::kClose) + { + client.output += websocket::encode_close(1000, ""); + client.closing = true; + } + else if (frame.opcode == websocket::kText || frame.opcode == websocket::kContinuation) + { + const bool continuation = frame.opcode == websocket::kContinuation; + // A message is one text frame, or a text frame without FIN followed by continuations ending + // with FIN. Anything else (an orphan continuation, a new message inside an unfinished one) is a + // protocol error. + if (continuation != client.fragmenting) + { + client.output += websocket::encode_close(1002, "bad fragmentation"); + client.closing = true; + continue; + } + client.message += frame.payload; + client.fragmenting = !frame.fin; + if (client.message.size() > m_config.max_message_bytes) + { + client.output += websocket::encode_close(1009, "message too big"); + client.closing = true; + } + else if (frame.fin) + { + const std::string request = std::move(client.message); + client.message.clear(); + client.output += websocket::encode_frame(websocket::kText, m_on_message(request)); + } + } + else if (frame.opcode == websocket::kBinary) + { + client.output += websocket::encode_close(1003, "text messages only"); + client.closing = true; + } + } + + if (!dead && !client.output.empty()) + { + const ssize_t n = ::send(client.fd, client.output.data(), client.output.size(), MSG_NOSIGNAL); + if (n > 0) + client.output.erase(0, static_cast(n)); + else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) + dead = true; + if (client.output.size() > kMaxOutputBytes) + dead = true; // a client that does not read is cut off rather than buffered for + } + + if (dead || (client.closing && client.output.empty())) + { + if (client.open) + m_clients.fetch_sub(1); + drop(i, dead ? "connection ended" : "closed"); + } + } + } + + for (const auto& client : clients) + { + if (client->open) + m_clients.fetch_sub(1); + ::close(client->fd); + } +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/driver_log_capture.cpp b/src/sensing_control/driver_log_capture.cpp new file mode 100644 index 0000000000..c220c0a148 --- /dev/null +++ b/src/sensing_control/driver_log_capture.cpp @@ -0,0 +1,122 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/driver_log_capture.hpp" + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +bool looks_like_failure(const std::string& line) +{ + static const char* const kWords[] = { "ERROR", "error", "fail", "Fail", "timeout", + "OPEN_CIRCUIT", "CIRCUIT_OPEN", "DES_LOCK_ERR", "CAMERA_DETECT" }; + return std::any_of( + std::begin(kWords), std::end(kWords), [&](const char* word) { return line.find(word) != std::string::npos; }); +} + +/// Drop the driver's own prefix noise ("[TID=1234] Module_id 30 Severity 2 : ") so the useful words lead. +std::string trim_prefix(const std::string& line) +{ + const auto at = line.find(" : "); + return (line.find("Severity") != std::string::npos && at != std::string::npos) ? line.substr(at + 3) : line; +} + +} // namespace + +DriverLogCapture::DriverLogCapture() +{ + std::fflush(stdout); + std::fflush(stderr); + char path[] = "/tmp/sensing-driver-log-XXXXXX"; + m_file = ::mkstemp(path); + if (m_file < 0) + return; // no capture: start-up still works, only the driver's words are lost + ::unlink(path); // the file lives only as long as the descriptor + m_saved_out = ::dup(STDOUT_FILENO); + m_saved_err = ::dup(STDERR_FILENO); + if (m_saved_out < 0 || m_saved_err < 0 || ::dup2(m_file, STDOUT_FILENO) < 0 || ::dup2(m_file, STDERR_FILENO) < 0) + { + if (m_saved_out >= 0) + ::dup2(m_saved_out, STDOUT_FILENO); + if (m_saved_err >= 0) + ::dup2(m_saved_err, STDERR_FILENO); + return; + } + m_active = true; +} + +DriverLogCapture::~DriverLogCapture() +{ + finish(); + if (m_saved_out >= 0) + ::close(m_saved_out); + if (m_saved_err >= 0) + ::close(m_saved_err); + if (m_file >= 0) + ::close(m_file); +} + +std::string DriverLogCapture::finish() +{ + if (!m_active) + return {}; + m_active = false; + std::fflush(stdout); + std::fflush(stderr); + ::dup2(m_saved_out, STDOUT_FILENO); + ::dup2(m_saved_err, STDERR_FILENO); + + std::string text; + ::lseek(m_file, 0, SEEK_SET); + char buffer[4096]; + ssize_t n = 0; + while ((n = ::read(m_file, buffer, sizeof(buffer))) > 0) + text.append(buffer, static_cast(n)); + // The journal keeps the driver's lines exactly as if nothing had intervened. + for (std::size_t done = 0; done < text.size();) + { + const ssize_t w = ::write(STDERR_FILENO, text.data() + done, text.size() - done); + if (w <= 0) + break; + done += static_cast(w); + } + return interesting_lines(text); +} + +std::string DriverLogCapture::interesting_lines(const std::string& text) +{ + std::vector lines; + std::istringstream stream(text); + std::string line; + while (std::getline(stream, line)) + { + if (looks_like_failure(line)) + { + std::string cleaned = trim_prefix(line); + if (lines.empty() || lines.back() != cleaned) + lines.push_back(std::move(cleaned)); + } + } + // Keep the first lines: the cause comes first and the rest is the teardown that follows it. + std::string joined; + for (std::size_t i = 0; i < lines.size() && i < 6; ++i) + joined += (joined.empty() ? "" : "; ") + lines[i]; + if (joined.size() > 600) + joined.resize(600); + return joined; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/camera_fault.hpp b/src/sensing_control/inc/sensing_control/camera_fault.hpp new file mode 100644 index 0000000000..48a1ae1fd3 --- /dev/null +++ b/src/sensing_control/inc/sensing_control/camera_fault.hpp @@ -0,0 +1,38 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "producer_status.hpp" + +#include + +namespace plugins +{ +namespace sensing +{ + +/// A failure explained for an operator, and the state the producer is in because of it. +struct Diagnosis +{ + Fault fault; + ProducerState state = ProducerState::Error; +}; + +/** + * @brief Turn the text of a failure into what an operator should check. + * + * The producer's own components (the SIPL and Argus capture backends, the Jetson encoder) + * report failures as exception text, and SIPL as bare status codes ("Init failed with SIPLStatus 10"). A camera that + * is unpowered or unplugged produces the same failure as several other conditions, so the message names the physical + * checks first and keeps the original text for anyone who needs it. Camera failures put the producer in + * CameraFault; encoder and other failures in Error. Text nothing matches still gets a code and the + * original text, never silence. + */ +Diagnosis diagnose_failure(std::string_view what); + +/// The fault part of diagnose_failure(). +Fault diagnose_camera_failure(std::string_view what); + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/control_handler.hpp b/src/sensing_control/inc/sensing_control/control_handler.hpp new file mode 100644 index 0000000000..bcb51a6240 --- /dev/null +++ b/src/sensing_control/inc/sensing_control/control_handler.hpp @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "producer_status.hpp" + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// The outcome of an action the channel asked the producer to take. +struct ActionResult +{ + bool ok = true; + std::string code; + std::string message; +}; + +/// What a management client can change. The producer implements it; the handler only validates and forwards. +class ControlActions +{ +public: + virtual ~ControlActions() = default; + + /// Send stream `track` to host:port from now on, or stop the direct link when `host` is empty. + virtual ActionResult set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) = 0; +}; + +/** + * @brief The management protocol: one JSON object per WebSocket text message. + * + * request {"id": 7, "type": "get_status"} + * {"id": 8, "type": "set_direct_target", "stream": "sensing-left", "host": "192.168.1.205", "port": 5600} + * {"id": 9, "type": "ping"} + * reply {"id": 7, "ok": true, "type": "status", "status": {...}} + * {"id": 8, "ok": false, "error": {"code": "invalid_host", "message": "..."}} + * events {"type": "event", "event": "state_changed", "status": {...}} + * {"type": "event", "event": "error", "error": {"code": "...", "message": "..."}} + * + * Every failure is answered with a code and a sentence; nothing is dropped silently. + */ +class ControlHandler +{ +public: + ControlHandler(const ProducerStatus& status, ControlActions& actions); + + /// The reply to one received text message. + std::string handle(const std::string& text) const; + +private: + const ProducerStatus& m_status; + ControlActions& m_actions; +}; + +/// The status as a JSON object text. +std::string status_json(const StatusSnapshot& snapshot); + +/// A pushed notification that the state changed. +std::string state_event(const StatusSnapshot& snapshot); + +/// A pushed notification of a failure. +std::string error_event(const Fault& fault); + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/control_server.hpp b/src/sensing_control/inc/sensing_control/control_server.hpp new file mode 100644 index 0000000000..4b791809a4 --- /dev/null +++ b/src/sensing_control/inc/sensing_control/control_server.hpp @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +struct ControlServerConfig +{ + std::string bind_address = "0.0.0.0"; + /// 0 picks a free port (tests); port() reports it. + std::uint16_t port = 5610; + /// Shared secret every client must present (`Authorization: Bearer` or `?token=`). Required: a channel that can + /// redirect camera video is never offered without one. + std::string token; + std::size_t max_clients = 8; + std::size_t max_message_bytes = 64 * 1024; +}; + +/// The first line of a secret file with surrounding whitespace removed; throws naming the file if it is unreadable or +/// empty. The secret is read from a file so it never appears in a process listing. +std::string read_secret_file(const std::string& path); + +/** + * @brief An authenticated WebSocket server on its own thread. + * + * Each received text message goes to `on_message` and its return value is sent back to that client; the callback + * runs on the server thread and must not block. broadcast() pushes a text message to every connected client from + * any thread. A client that sends a bad handshake, a wrong token, an oversized or malformed message, or that stops + * reading is disconnected with a reason where the protocol allows one. + */ +class ControlServer +{ +public: + using MessageHandler = std::function; + + /// Starts listening. Throws std::runtime_error with the reason if the token is empty or the port cannot be bound. + ControlServer(const ControlServerConfig& config, MessageHandler on_message); + ~ControlServer(); + + ControlServer(const ControlServer&) = delete; + ControlServer& operator=(const ControlServer&) = delete; + + std::uint16_t port() const + { + return m_port; + } + + /// Queue a text message for every connected client. + void broadcast(const std::string& text); + + /// Clients that have completed the handshake. + std::size_t client_count() const + { + return m_clients.load(); + } + +private: + struct Client; + void run(); + void wake(); + + ControlServerConfig m_config; + MessageHandler m_on_message; + int m_listen_fd = -1; + int m_wake_fd = -1; + std::uint16_t m_port = 0; + std::atomic m_stop{ false }; + std::atomic m_clients{ 0 }; + std::mutex m_mutex; + std::vector m_broadcasts; ///< guarded by m_mutex + std::thread m_thread; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/driver_log_capture.hpp b/src/sensing_control/inc/sensing_control/driver_log_capture.hpp new file mode 100644 index 0000000000..55d8f3af03 --- /dev/null +++ b/src/sensing_control/inc/sensing_control/driver_log_capture.hpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugins +{ +namespace sensing +{ + +/** + * @brief Collects what native code prints to stdout and stderr while it runs, for the length of one scope. + * + * The camera driver explains why a camera failed only in its own log lines ("lock timeout", "OPEN_CIRCUIT", an I2C + * NACK); the status code it returns says nothing. This redirects descriptors 1 and 2 into a temporary file for the + * scope, and on finish() restores them, writes what was captured back to them so the service journal still has it, + * and returns the lines that matter for a failure. The descriptors are restored on every path, including + * destruction without finish(). Use it only around a start-up call, never for the process lifetime. + */ +class DriverLogCapture +{ +public: + DriverLogCapture(); + ~DriverLogCapture(); + + DriverLogCapture(const DriverLogCapture&) = delete; + DriverLogCapture& operator=(const DriverLogCapture&) = delete; + + /// Restore the descriptors, replay the captured text, and return the captured lines that look like failures. + std::string finish(); + + /// The failure-looking lines of `text` (an error, failure, timeout, NACK, open circuit), newest last, joined with + /// "; " and limited in length. + static std::string interesting_lines(const std::string& text); + +private: + int m_saved_out = -1; + int m_saved_err = -1; + int m_file = -1; + bool m_active = false; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/producer_status.hpp b/src/sensing_control/inc/sensing_control/producer_status.hpp new file mode 100644 index 0000000000..d02b17cbb6 --- /dev/null +++ b/src/sensing_control/inc/sensing_control/producer_status.hpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +enum class ProducerState +{ + /// Opening the cameras and joining the room. + Starting, + /// Cameras are capturing and tracks are published. + Streaming, + /// The cameras could not be started; the producer retries and says why. + CameraFault, + /// Another failure (for example the encoder or a stalled stream); the producer retries. + Error, + Stopping, +}; + +const char* to_string(ProducerState state); + +/// A failure as an operator should read it: a stable code and a sentence that says what to check. +struct Fault +{ + std::string code; + std::string message; +}; + +struct StreamStatus +{ + std::string track; + std::uint32_t sensor_id = 0; + /// Where this stream's encoded video is also sent (the PC video stage); empty host means no direct link. + std::string direct_host; + std::uint16_t direct_port = 0; + bool direct_connected = false; + /// Why the direct link is not up (for example "cannot connect to 192.168.1.5:5600: Connection refused"). + std::string direct_error; + std::uint64_t frames = 0; + std::uint64_t published = 0; +}; + +struct StatusSnapshot +{ + ProducerState state = ProducerState::Starting; + std::optional fault; + std::vector streams; + std::string release; + std::uint64_t uptime_seconds = 0; +}; + +/// What the management channel reports. Written by the producer's main loop, read by the channel's thread. +class ProducerStatus +{ +public: + explicit ProducerStatus(std::string release); + + /// Set the state; a fault is kept only for the states that have one. + void set_state(ProducerState state, std::optional fault = std::nullopt); + + /// Replace the per-stream figures (the configured streams stay listed while the cameras are down). + void set_streams(std::vector streams); + + /// Change one stream's direct-link target; false if no stream has that track name. + bool set_direct_target(const std::string& track, const std::string& host, std::uint16_t port); + + StatusSnapshot snapshot() const; + +private: + mutable std::mutex m_mutex; + std::string m_release; + ProducerState m_state = ProducerState::Starting; + std::optional m_fault; + std::vector m_streams; + std::chrono::steady_clock::time_point m_started = std::chrono::steady_clock::now(); +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/producer_supervisor.hpp b/src/sensing_control/inc/sensing_control/producer_supervisor.hpp new file mode 100644 index 0000000000..32da05889b --- /dev/null +++ b/src/sensing_control/inc/sensing_control/producer_supervisor.hpp @@ -0,0 +1,143 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "control_handler.hpp" +#include "producer_status.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +/// Where a stream's encoded video goes (the PC video stage); chosen at startup or at runtime over the management +/// channel. +struct DirectTarget +{ + std::string host; + std::uint16_t port = 0; +}; + +using DirectTargets = std::map; ///< by track name + +/// A start-up failure the factory has already explained: the state to report and the fault to show. +class StartupFailure : public std::runtime_error +{ +public: + StartupFailure(Fault fault, ProducerState state) + : std::runtime_error(fault.message), m_fault(std::move(fault)), m_state(state) + { + } + const Fault& fault() const + { + return m_fault; + } + ProducerState state() const + { + return m_state; + } + +private: + Fault m_fault; + ProducerState m_state; +}; + +/// One running capture-and-publish pipeline. The real producer and the mock producer each provide one. +class CameraSession +{ +public: + virtual ~CameraSession() = default; + + /// Poll for frames and hand them on. Throws once the session can no longer work (room lost, camera gone). + virtual void update() = 0; + virtual void flush() = 0; + /// Current per-stream figures, including each direct link's target and whether it is connected. + virtual std::vector streams() const = 0; + virtual void set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) = 0; +}; + +/// Builds a session, applying `targets` over the configured direct links. Throws with the failure's text. +using SessionFactory = std::function(const DirectTargets& targets)>; + +struct SupervisorConfig +{ + /// How long to wait before trying to start the cameras again after a failure. + std::chrono::milliseconds retry_interval{ 5000 }; + std::chrono::milliseconds poll_interval{ 15 }; + /// How often the figures in the status are refreshed from the running session. + std::chrono::milliseconds status_interval{ 1000 }; + /// A running session whose stream delivers no new frame for this long is failed and restarted. Zero disables it. + std::chrono::milliseconds stall_timeout{ 5000 }; +}; + +/** + * @brief Keeps the producer running and reporting. + * + * A camera that cannot be started (no power, no link) is a state to report and retry, not a reason to exit: the + * management channel stays up, says why in plain words, and the stream comes back when the cable is fixed. + * + * One thread, the one in run(), owns the session: it creates, updates, retargets, flushes and destroys it. The + * channel's thread only records what was asked (set_direct_target) and run() applies it between updates, so a slow + * camera start never blocks a command and a command never races a session that is being built or torn down. Direct + * targets are remembered, so a restarted session starts with the latest. + */ +class ProducerSupervisor : public ControlActions +{ +public: + /// `publish` receives each pushed notification as JSON text (the channel's broadcast); it may be empty. + ProducerSupervisor(ProducerStatus& status, + std::vector configured, + SessionFactory factory, + std::function publish, + const SupervisorConfig& config = {}); + + /// Run until `stop()` returns true. + void run(const std::function& stop); + + ActionResult set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) override; + +private: + void sleep_unless_stopped(std::chrono::milliseconds duration, const std::function& stop) const; + void announce(ProducerState state, std::optional fault); + /// Compare the session's figures with the last ones: report new direct-link errors, and return a description of a + /// stalled stream (empty when every stream is delivering). + std::string watch(const std::vector& streams, std::chrono::steady_clock::time_point now); + + ProducerStatus& m_status; + std::vector m_configured; + SessionFactory m_factory; + std::function m_publish; + SupervisorConfig m_config; + + /// Per stream: the last frame count seen, when it last changed, and the direct-link error last reported. + struct Watch + { + std::uint64_t frames = 0; + std::chrono::steady_clock::time_point changed; + std::string direct_error; + }; + std::map m_watch; ///< main thread only + + /// Applies the targets that changed since the session was built, on the owning thread. + void apply_pending(CameraSession& session); + + std::mutex m_targets_mutex; ///< guards m_targets and m_pending + DirectTargets m_targets; ///< what the operator last chose, by track + std::set m_pending; ///< tracks whose target changed since the running session last heard of it +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/inc/sensing_control/websocket.hpp b/src/sensing_control/inc/sensing_control/websocket.hpp new file mode 100644 index 0000000000..faa85f278f --- /dev/null +++ b/src/sensing_control/inc/sensing_control/websocket.hpp @@ -0,0 +1,94 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ +namespace websocket +{ + +/// RFC 6455 opcodes this server understands. +enum Opcode : std::uint8_t +{ + kContinuation = 0x0, + kText = 0x1, + kBinary = 0x2, + kClose = 0x8, + kPing = 0x9, + kPong = 0xA, +}; + +/// SHA-1 of `data` as 20 raw bytes; used only for the opening handshake, never for security. +std::string sha1(std::string_view data); + +std::string base64_encode(std::string_view data); + +/// The Sec-WebSocket-Accept value for a client's Sec-WebSocket-Key. +std::string accept_key(std::string_view client_key); + +/// A parsed client upgrade request. +struct Handshake +{ + bool ok = false; + /// HTTP status to answer with when !ok (400, 401, 404 or 426), and a plain-text reason. + int status = 400; + std::string reason; + std::string key; + /// The bearer token from `Authorization: Bearer ...` or the `token=` query parameter; empty when absent. + std::string token; + std::string path; +}; + +/// Parse the request head (everything up to and including the blank line). +Handshake parse_handshake(std::string_view head); + +/// The server's answer to a successful or failed handshake. +std::string handshake_response(const Handshake& handshake); + +/// Compare two secrets without leaking where they differ. +bool constant_time_equal(std::string_view a, std::string_view b); + +struct Frame +{ + bool fin = true; + std::uint8_t opcode = kText; + std::string payload; +}; + +enum class ParseStatus +{ + NeedMore, + Complete, + Error, +}; + +struct ParseResult +{ + ParseStatus status = ParseStatus::NeedMore; + std::size_t consumed = 0; + Frame frame; + /// WebSocket close code to send when status is Error (1002 protocol error, 1009 too big). + std::uint16_t close_code = 1002; + std::string error; +}; + +/// Parse one client frame from the front of `buffer`. Client frames must be masked. +ParseResult parse_frame(std::string_view buffer, std::size_t max_payload); + +/// Encode one unmasked server frame. +std::string encode_frame(std::uint8_t opcode, std::string_view payload, bool fin = true); + +/// A close frame carrying `code` and a short reason. +std::string encode_close(std::uint16_t code, std::string_view reason); + +} // namespace websocket +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/mock_producer/CMakeLists.txt b/src/sensing_control/mock_producer/CMakeLists.txt new file mode 100644 index 0000000000..0dc2d10b0c --- /dev/null +++ b/src/sensing_control/mock_producer/CMakeLists.txt @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# A camera producer without cameras: it plays H.264 files through the same management channel, supervisor and direct +# sender as camera_plugin_sensing, so the PC side can be exercised with no Orin. +add_executable(sensing_mock_producer main.cpp) + +target_link_libraries(sensing_mock_producer + PRIVATE + sensing::control + video_stage::sender + video_stage::wire +) diff --git a/src/sensing_control/mock_producer/main.cpp b/src/sensing_control/mock_producer/main.cpp new file mode 100644 index 0000000000..1d2db0e116 --- /dev/null +++ b/src/sensing_control/mock_producer/main.cpp @@ -0,0 +1,310 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// A camera producer without cameras. It runs the same ControlServer, ControlHandler, ProducerSupervisor and +// DirectSender as camera_plugin_sensing and differs only in where frames come from: H.264 files looped at a fixed rate. +// +// sensing_mock_producer --control-token-file=token.txt --stream=sensing-left=left.h264,direct=127.0.0.1:5600 +// --stream=sensing-right=right.h264 +// +// --simulate-fault-seconds=N makes the first N seconds behave like a camera with no power, so the fault reporting and +// the retry can be seen without unplugging anything. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::sensing; + +namespace +{ + +std::atomic g_stop{ false }; + +void on_signal(int) +{ + g_stop = true; +} + +struct StreamArgs +{ + std::string track; + std::string file; + std::string direct_host; + std::uint16_t direct_port = 0; +}; + +/// "TRACK=FILE[,direct=HOST:PORT]" +StreamArgs parse_stream(const std::string& text) +{ + StreamArgs out; + const auto eq = text.find('='); + if (eq == std::string::npos || eq == 0) + throw std::runtime_error("--stream must look like TRACK=FILE[,direct=HOST:PORT]: " + text); + out.track = text.substr(0, eq); + std::string rest = text.substr(eq + 1); + const auto comma = rest.find(",direct="); + if (comma != std::string::npos) + { + const std::string target = rest.substr(comma + 8); + rest = rest.substr(0, comma); + const auto colon = target.rfind(':'); + if (colon == std::string::npos) + throw std::runtime_error("direct= must be HOST:PORT: " + target); + out.direct_host = target.substr(0, colon); + out.direct_port = static_cast(std::stoul(target.substr(colon + 1))); + } + out.file = rest; + return out; +} + +std::uint64_t monotonic_us() +{ + return static_cast( + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count()); +} + +/// One looped file and the direct sender that carries it. +struct MockStream +{ + std::string track; + std::vector bytes; + std::vector units; + std::size_t cursor = 0; + std::uint32_t frame_id = 0; + std::uint64_t frames = 0; + std::unique_ptr sender; +}; + +class MockSession : public CameraSession +{ +public: + MockSession(const std::vector& streams, const DirectTargets& targets, std::uint32_t fps) + : m_period(std::chrono::microseconds(1'000'000 / fps)), m_next(std::chrono::steady_clock::now()) + { + for (const auto& args : streams) + { + auto stream = std::make_unique(); + stream->track = args.track; + std::ifstream file(args.file, std::ios::binary); + if (!file) + throw std::runtime_error("cannot read " + args.file); + stream->bytes.assign(std::istreambuf_iterator(file), std::istreambuf_iterator()); + stream->units = video_stage::split_access_units(stream->bytes.data(), stream->bytes.size()); + if (stream->units.empty()) + throw std::runtime_error(args.file + " has no H.264 access units"); + + DirectSenderConfig config; + config.fps = fps; + const auto found = targets.find(args.track); + if (found != targets.end()) + { + config.host = found->second.host; + config.port = found->second.port; + } + stream->sender = std::make_unique(config); + m_streams.push_back(std::move(stream)); + } + } + + void update() override + { + const auto now = std::chrono::steady_clock::now(); + while (now >= m_next) + { + m_next += m_period; + for (auto& stream : m_streams) + emit(*stream); + } + } + + void flush() override + { + } + + std::vector streams() const override + { + std::vector out; + for (const auto& stream : m_streams) + { + StreamStatus entry; + entry.track = stream->track; + entry.frames = stream->frames; + entry.published = stream->frames; + entry.direct_host = stream->sender->host(); + entry.direct_port = stream->sender->port(); + entry.direct_connected = stream->sender->connected(); + entry.direct_error = stream->sender->last_error(); + out.push_back(std::move(entry)); + } + return out; + } + + void set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) override + { + for (auto& stream : m_streams) + if (stream->track == track) + stream->sender->set_target(host, port); + } + +private: + void emit(MockStream& stream) + { + // A keyframe request is answered at the file's next IDR, as an encoder would answer it. + if (stream.sender->take_keyframe_request()) + { + for (std::size_t k = 0; k < stream.units.size(); ++k) + { + const std::size_t index = (stream.cursor + k) % stream.units.size(); + if (stream.units[index].idr) + { + stream.cursor = index; + break; + } + } + } + const auto& unit = stream.units[stream.cursor]; + stream.cursor = (stream.cursor + 1) % stream.units.size(); + + EncodedVideoFrame frame; + frame.data = stream.bytes.data() + unit.offset; + frame.size = unit.size; + frame.keyframe = unit.idr; + frame.timestamp_us = monotonic_us(); + frame.user_timestamp_us = frame.timestamp_us; + frame.frame_id = ++stream.frame_id; + stream.sender->submit(frame); + ++stream.frames; + } + + std::chrono::microseconds m_period; + std::chrono::steady_clock::time_point m_next; + std::vector> m_streams; +}; + +void print_usage(const char* program) +{ + std::cout << "Usage: " << program << " --control-token-file=PATH --stream=TRACK=FILE[,direct=HOST:PORT] ...\n" + << " --control-port=N management channel port (default: 5610; 0 picks a free one)\n" + << " --control-bind=ADDR IPv4 address to listen on (default: 127.0.0.1)\n" + << " --fps=N frames per second per stream (default: 30)\n" + << " --retry-seconds=N wait between start attempts (default: 2)\n" + << " --simulate-fault-seconds=N behave like an unpowered camera for the first N seconds\n" + << " --release=ID release id to report (default: mock)\n"; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + std::vector streams; + ControlServerConfig control; + control.bind_address = "127.0.0.1"; + std::string token_file; + std::uint32_t fps = 30; + std::uint32_t retry_seconds = 2; + std::uint32_t fault_seconds = 0; + std::string release = "mock"; + + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + const auto value = [&](const char* name) { return arg.substr(std::string(name).size()); }; + if (arg == "--help" || arg == "-h") + { + print_usage(argv[0]); + return 0; + } + else if (arg.rfind("--stream=", 0) == 0) + streams.push_back(parse_stream(value("--stream="))); + else if (arg.rfind("--control-token-file=", 0) == 0) + token_file = value("--control-token-file="); + else if (arg.rfind("--control-port=", 0) == 0) + control.port = static_cast(std::stoul(value("--control-port="))); + else if (arg.rfind("--control-bind=", 0) == 0) + control.bind_address = value("--control-bind="); + else if (arg.rfind("--fps=", 0) == 0) + fps = static_cast(std::stoul(value("--fps="))); + else if (arg.rfind("--retry-seconds=", 0) == 0) + retry_seconds = static_cast(std::stoul(value("--retry-seconds="))); + else if (arg.rfind("--simulate-fault-seconds=", 0) == 0) + fault_seconds = static_cast(std::stoul(value("--simulate-fault-seconds="))); + else if (arg.rfind("--release=", 0) == 0) + release = value("--release="); + else + { + std::cerr << "Unknown option: " << arg << std::endl; + print_usage(argv[0]); + return 1; + } + } + if (streams.empty() || token_file.empty() || fps == 0) + { + print_usage(argv[0]); + return 1; + } + control.token = read_secret_file(token_file); + + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + ProducerStatus status(release); + std::vector configured; + for (const auto& args : streams) + { + StreamStatus entry; + entry.track = args.track; + entry.direct_host = args.direct_host; + entry.direct_port = args.direct_port; + configured.push_back(entry); + } + + const auto started = std::chrono::steady_clock::now(); + const SessionFactory factory = [&](const DirectTargets& targets) -> std::unique_ptr + { + if (std::chrono::steady_clock::now() - started < std::chrono::seconds(fault_seconds)) + throw std::runtime_error("INvSIPLCamera::Init failed with SIPLStatus 10"); + return std::make_unique(streams, targets, fps); + }; + + std::unique_ptr server; + SupervisorConfig supervisor_config; + supervisor_config.retry_interval = std::chrono::seconds(retry_seconds); + supervisor_config.poll_interval = std::chrono::milliseconds(2); + ProducerSupervisor supervisor( + status, configured, factory, + [&server](const std::string& text) + { + if (server) + server->broadcast(text); + }, + supervisor_config); + ControlHandler handler(status, supervisor); + server = + std::make_unique(control, [&handler](const std::string& text) { return handler.handle(text); }); + std::cout << "Mock producer: management channel on " << control.bind_address << ":" << server->port() << std::endl; + + supervisor.run([] { return g_stop.load(); }); + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} diff --git a/src/sensing_control/producer_status.cpp b/src/sensing_control/producer_status.cpp new file mode 100644 index 0000000000..bea614320f --- /dev/null +++ b/src/sensing_control/producer_status.cpp @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/producer_status.hpp" + +#include + +namespace plugins +{ +namespace sensing +{ + +const char* to_string(ProducerState state) +{ + switch (state) + { + case ProducerState::Starting: + return "starting"; + case ProducerState::Streaming: + return "streaming"; + case ProducerState::CameraFault: + return "camera_fault"; + case ProducerState::Error: + return "error"; + case ProducerState::Stopping: + return "stopping"; + } + return "unknown"; +} + +ProducerStatus::ProducerStatus(std::string release) : m_release(std::move(release)) +{ +} + +void ProducerStatus::set_state(ProducerState state, std::optional fault) +{ + std::lock_guard lock(m_mutex); + m_state = state; + // Only the failing states carry a fault; leaving one clears it. + m_fault = (state == ProducerState::CameraFault || state == ProducerState::Error) ? std::move(fault) : std::nullopt; +} + +void ProducerStatus::set_streams(std::vector streams) +{ + std::lock_guard lock(m_mutex); + m_streams = std::move(streams); +} + +bool ProducerStatus::set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) +{ + std::lock_guard lock(m_mutex); + for (auto& stream : m_streams) + { + if (stream.track == track) + { + stream.direct_host = host; + stream.direct_port = host.empty() ? 0 : port; + stream.direct_connected = false; + return true; + } + } + return false; +} + +StatusSnapshot ProducerStatus::snapshot() const +{ + std::lock_guard lock(m_mutex); + StatusSnapshot out; + out.state = m_state; + out.fault = m_fault; + out.streams = m_streams; + out.release = m_release; + out.uptime_seconds = static_cast( + std::chrono::duration_cast(std::chrono::steady_clock::now() - m_started).count()); + return out; +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/producer_supervisor.cpp b/src/sensing_control/producer_supervisor.cpp new file mode 100644 index 0000000000..38087cabc5 --- /dev/null +++ b/src/sensing_control/producer_supervisor.cpp @@ -0,0 +1,236 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/producer_supervisor.hpp" + +#include "inc/sensing_control/camera_fault.hpp" + +#include + +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +std::shared_ptr logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.sensing.ProducerSupervisor"); +} + +} // namespace + +ProducerSupervisor::ProducerSupervisor(ProducerStatus& status, + std::vector configured, + SessionFactory factory, + std::function publish, + const SupervisorConfig& config) + : m_status(status), + m_configured(std::move(configured)), + m_factory(std::move(factory)), + m_publish(std::move(publish)), + m_config(config) +{ + // The configured direct targets are the starting point; a runtime change replaces them. + for (const auto& stream : m_configured) + m_targets[stream.track] = { stream.direct_host, stream.direct_port }; + m_status.set_streams(m_configured); +} + +void ProducerSupervisor::announce(ProducerState state, std::optional fault) +{ + m_status.set_state(state, fault); + if (fault) + logger()->error("{}: {}", fault->code, fault->message); + if (m_publish) + { + const auto snapshot = m_status.snapshot(); + if (fault) + m_publish(error_event(*fault)); + m_publish(state_event(snapshot)); + } +} + +std::string ProducerSupervisor::watch(const std::vector& streams, std::chrono::steady_clock::time_point now) +{ + std::string stalled; + for (const auto& stream : streams) + { + auto& entry = m_watch[stream.track]; + if (stream.frames != entry.frames) + { + entry.frames = stream.frames; + entry.changed = now; + } + else if (m_config.stall_timeout.count() > 0 && now - entry.changed >= m_config.stall_timeout) + { + const auto seconds = std::chrono::duration_cast(now - entry.changed).count(); + stalled += (stalled.empty() ? "" : "; ") + std::string("no frame for ") + std::to_string(seconds) + + " s from sensor " + std::to_string(stream.sensor_id) + " (stream " + stream.track + ")"; + } + + // A direct link that cannot reach the PC is reported once per distinct reason, not every second. + if (stream.direct_error != entry.direct_error) + { + entry.direct_error = stream.direct_error; + if (!stream.direct_error.empty() && m_publish) + { + m_publish(error_event( + { "direct_link_failed", "Stream " + stream.track + ": " + stream.direct_error + + ". Check that the PC worker is running and listening on that port, " + "and that the address is the PC's (set it with set_direct_target)." })); + } + } + } + return stalled; +} + +void ProducerSupervisor::sleep_unless_stopped(std::chrono::milliseconds duration, const std::function& stop) const +{ + const auto until = std::chrono::steady_clock::now() + duration; + while (!stop() && std::chrono::steady_clock::now() < until) + std::this_thread::sleep_for(std::chrono::milliseconds(50)); +} + +ActionResult ProducerSupervisor::set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) +{ + // Only records the request: the thread that owns the session applies it (see apply_pending). + if (!m_status.set_direct_target(track, host, port)) + return { false, "unknown_stream", "no stream named \"" + track + "\"" }; + { + std::lock_guard lock(m_targets_mutex); + m_targets[track] = { host, host.empty() ? std::uint16_t(0) : port }; + m_pending.insert(track); + } + logger()->info( + "stream {} will send to {}", track, host.empty() ? std::string("nowhere") : host + ":" + std::to_string(port)); + if (m_publish) + m_publish(state_event(m_status.snapshot())); + return {}; +} + +void ProducerSupervisor::apply_pending(CameraSession& session) +{ + DirectTargets changed; + { + std::lock_guard lock(m_targets_mutex); + for (const auto& track : m_pending) + changed[track] = m_targets[track]; + m_pending.clear(); + } + for (const auto& [track, target] : changed) + session.set_direct_target(track, target.host, target.port); +} + +void ProducerSupervisor::run(const std::function& stop) +{ + // A fault that persists across retries is announced once, not on every attempt. + std::optional last_fault; + bool first = true; + while (!stop()) + { + if (first || !last_fault) + announce(ProducerState::Starting, std::nullopt); + first = false; + + std::unique_ptr session; + DirectTargets targets; + { + // The session is built with everything chosen so far; only later changes are pending. + std::lock_guard lock(m_targets_mutex); + targets = m_targets; + m_pending.clear(); + } + try + { + session = m_factory(targets); + } + catch (const std::exception& e) + { + // The factory explains the failures it understands; anything else is read as a camera failure. + const auto* explained = dynamic_cast(&e); + const Diagnosis diagnosis = + explained ? Diagnosis{ explained->fault(), explained->state() } : diagnose_failure(e.what()); + Fault fault = diagnosis.fault; + const ProducerState state = diagnosis.state; + if (!last_fault || last_fault->message != fault.message) + announce(state, fault); + last_fault = std::move(fault); + sleep_unless_stopped(m_config.retry_interval, stop); + continue; + } + + last_fault.reset(); + announce(ProducerState::Streaming, std::nullopt); + + std::optional failure; + ProducerState failure_state = ProducerState::Error; + m_watch.clear(); + for (const auto& stream : m_configured) + { + m_watch[stream.track].changed = std::chrono::steady_clock::now(); + } + auto next_status = std::chrono::steady_clock::now(); + while (!stop() && !failure) + { + try + { + // A change that arrived while the cameras were starting is applied before the first update. + apply_pending(*session); + session->update(); + if (std::chrono::steady_clock::now() >= next_status) + { + next_status += m_config.status_interval; + // The session's own figures, including where each stream really sends. + auto streams = session->streams(); + const std::string stalled = watch(streams, std::chrono::steady_clock::now()); + m_status.set_streams(std::move(streams)); + if (!stalled.empty()) + throw std::runtime_error(stalled); + } + } + catch (const std::exception& e) + { + failure_state = ProducerState::Error; + Diagnosis diagnosis = diagnose_failure(e.what()); + if (diagnosis.fault.code == "startup_failed") + { + // A session that was running and failed for an unrecognised reason did not "fail to start". + diagnosis = { { "stream_failed", std::string("The stream stopped. The producer restarts it " + "automatically. (") + + e.what() + ")" }, + ProducerState::Error }; + } + failure = diagnosis.fault; + failure_state = diagnosis.state; + } + if (!failure) + std::this_thread::sleep_for(m_config.poll_interval); + } + + try + { + session->flush(); + } + catch (const std::exception& e) + { + logger()->warn("flush on shutdown failed: {}", e.what()); + } + session.reset(); + if (failure) + { + announce(failure_state, failure); + last_fault.reset(); + sleep_unless_stopped(m_config.retry_interval, stop); + } + } + announce(ProducerState::Stopping, std::nullopt); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/sensing_control/tests/CMakeLists.txt b/src/sensing_control/tests/CMakeLists.txt new file mode 100644 index 0000000000..026f4919ab --- /dev/null +++ b/src/sensing_control/tests/CMakeLists.txt @@ -0,0 +1,34 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(sensing_control_tests + test_websocket.cpp + test_camera_fault.cpp + test_driver_log_capture.cpp + test_producer_status.cpp + test_control_handler.cpp + test_producer_supervisor.cpp + test_control_server.cpp +) + +target_link_libraries(sensing_control_tests + PRIVATE + sensing::control + nlohmann_json::nlohmann_json + Catch2::Catch2WithMain +) + +catch_discover_tests(sensing_control_tests) + +# The mock producer end to end: needs the built binaries, so it is skipped (not failed) when they are absent. +find_package(Python3 COMPONENTS Interpreter QUIET) +if(Python3_Interpreter_FOUND) + add_test( + NAME sensing_mock_producer_e2e + COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test_mock_producer_e2e.py + ) + set_tests_properties(sensing_mock_producer_e2e PROPERTIES + ENVIRONMENT "SENSING_MOCK_PRODUCER=$;VIDEO_STAGE_WORKER=$" + TIMEOUT 120 + ) +endif() diff --git a/src/sensing_control/tests/test_camera_fault.cpp b/src/sensing_control/tests/test_camera_fault.cpp new file mode 100644 index 0000000000..df04c26b15 --- /dev/null +++ b/src/sensing_control/tests/test_camera_fault.cpp @@ -0,0 +1,181 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include + +using plugins::sensing::diagnose_camera_failure; +using plugins::sensing::diagnose_failure; +using plugins::sensing::ProducerState; + +namespace +{ + +bool contains(const std::string& text, const std::string& part) +{ + return text.find(part) != std::string::npos; +} + +bool ends_with(const std::string& text, const std::string& suffix) +{ + return text.size() >= suffix.size() && text.compare(text.size() - suffix.size(), suffix.size(), suffix) == 0; +} + +struct Expectation +{ + std::string text; + std::string code; + ProducerState state; +}; + +constexpr ProducerState kCamera = ProducerState::CameraFault; +constexpr ProducerState kError = ProducerState::Error; + +// One representative text per signature in camera_fault.cpp, in table order. +const std::vector& expectations() +{ + static const std::vector table = { + { "Severity 2 : OPEN_CIRCUIT on GPIO 4", "camera_power_missing", kCamera }, + { "fault: CIRCUIT_OPEN_DISCONNECTION", "camera_power_missing", kCamera }, + { "deserializer lock timeout on link 1", "camera_link_not_locked", kCamera }, + { "DES_LOCK_ERR reported", "camera_link_not_locked", kCamera }, + { "CAMERA_DETECT failed for module 2", "camera_not_detected", kCamera }, + { "Parse failed after 3 retries", "camera_not_responding", kCamera }, + { "Init returned SIPLStatus 8", "camera_partly_started", kCamera }, + { "sensor control write failure on sensor 3", "camera_control_bus_failed", kCamera }, + { "input stream failure on pipeline 0", "camera_stream_failed", kCamera }, + { "image processing failure on pipeline 1", "camera_isp_failed", kCamera }, + { "interrupt localization failure", "camera_hardware_fault", kCamera }, + { "deserializer failure on link 0", "camera_link_failed", kCamera }, + { "serializer failure on link 0", "camera_link_failed", kCamera }, + { "sensor failure on sensor 2", "camera_sensor_failed", kCamera }, + { "link lock lost on deserializer 1", "camera_link_not_locked", kCamera }, + { "Failed to detect camera on link 2", "camera_not_detected", kCamera }, + { "no frame for 5 s from sensor 2 (stream sensing-left)", "camera_no_frames", kCamera }, + { "INvSIPLCamera::Init failed", "camera_start_failed", kCamera }, + { "INvSIPLCamera::SetPlatformCfg failed", "camera_config_rejected", kCamera }, + { "INvSIPLCamera::Start failed", "camera_start_failed", kCamera }, + { "Init failed with SIPLStatus 10", "camera_start_failed", kCamera }, + { "pipeline error on sensor 1", "camera_pipeline_error", kCamera }, + { "capture error on sensor 1", "camera_capture_error", kCamera }, + { "no modules left in the link masks", "camera_link_mask_mismatch", kCamera }, + { "configuration declares no sensors", "camera_config_empty", kCamera }, + { "no SIPL pipeline with index 9", "camera_not_in_config", kCamera }, + { "link masks are required", "camera_config_missing", kCamera }, + { "cannot open NITO file /var/nito/x.nito", "camera_tuning_missing", kCamera }, + { "short read on NITO file", "camera_tuning_invalid", kCamera }, + { "Argus: no camera provider", "camera_service_down", kCamera }, + { "Argus: no camera device 2", "camera_not_detected", kCamera }, + { "Argus: no camera at index 3", "camera_not_detected", kCamera }, + { "Argus: session failed", "camera_error", kCamera }, + { "NvBufSurfaceMapEglImage failed", "gpu_buffer_error", kCamera }, + { "NvmmNvSciBufToNvBufSurface failed", "gpu_buffer_error", kCamera }, + { "unsupported CUDA EGL frame type 7", "gpu_buffer_error", kCamera }, + { "CUDA error 700 in cudaMemcpy", "gpu_error", kError }, + { "createVideoEncoder failed", "encoder_unavailable", kError }, + { "JetsonEncoder: dequeue timed out", "encoder_error", kError }, + { "std::bad_alloc", "out_of_memory", kError }, + { "open /dev/video0: Permission denied", "permission_denied", kError }, + { "open /dev/foo: No such file or directory", "file_missing", kError }, + { "open /dev/video1: Device or resource busy", "device_busy", kError }, + }; + return table; +} + +} // namespace + +TEST_CASE("every signature maps its text to the intended code and state", "[camera_fault]") +{ + for (const auto& e : expectations()) + { + INFO("text: " << e.text); + const auto d = diagnose_failure(e.text); + CHECK(d.fault.code == e.code); + CHECK(d.state == e.state); + CHECK_FALSE(d.fault.message.empty()); + CHECK(ends_with(d.fault.message, "(" + e.text + ")")); + CHECK(diagnose_camera_failure(e.text).code == e.code); + } +} + +TEST_CASE("known capture failures are explained and keep the original text", "[camera_fault]") +{ + const std::string sipl = "INvSIPLCamera::Init failed with SIPLStatus 10"; + const auto a = diagnose_failure(sipl); + CHECK(a.fault.code == "camera_start_failed"); + CHECK(a.state == ProducerState::CameraFault); + CHECK(contains(a.fault.message, "power cable")); + CHECK(contains(a.fault.message, "retries automatically")); + CHECK(contains(a.fault.message, sipl)); +} + +TEST_CASE("the first matching signature wins", "[camera_fault]") +{ + // Both the generic SIPL status and the driver's own explanation: the explanation is listed first. + CHECK(diagnose_failure("Init failed with SIPLStatus 10; OPEN_CIRCUIT").fault.code == "camera_power_missing"); + CHECK(diagnose_failure("SIPLStatus 10 CIRCUIT_OPEN_DISCONNECTION").fault.code == "camera_power_missing"); + CHECK(diagnose_failure("link lock failed, SIPLStatus 10").fault.code == "camera_link_not_locked"); + // A driver NACK line attached to an otherwise unknown failure does not change the mapping. + CHECK(diagnose_failure("something unknown; I2C: NACK ... errno=121").fault.code == "startup_failed"); + // The camera signature still wins when it is mentioned beside a publishing word. + CHECK(diagnose_failure("Init failed with SIPLStatus 10; LiveKit: cannot join the room").fault.code == + "camera_start_failed"); + // "Argus:" alone is the last Argus entry, not the first. + CHECK(diagnose_failure("Argus: no camera provider").fault.code == "camera_service_down"); + // Camera signatures are listed before the generic CUDA one. + CHECK(diagnose_failure("unsupported CUDA EGL frame type 3").state == ProducerState::CameraFault); +} + +TEST_CASE("an I2C NACK alone is not a signature", "[camera_fault]") +{ + // The driver prints NACK lines on healthy starts too. + for (const char* text : { "I2C: NACK", "CAMI2C_ERROR_BUS_NACK on 0x1a", "I2C: NACK from address 0x36 errno=121" }) + { + INFO(text); + CHECK(diagnose_failure(text).fault.code == "startup_failed"); + } +} + +TEST_CASE("no signature mentions LiveKit any more", "[camera_fault]") +{ + // The Orin streams over its direct links only, so LiveKit texts are not diagnosed specially. + for (const char* text : { "LiveKit: cannot join the room", "LiveKit: cannot publish track sensing-left", + "LiveKit: no local participant", "LiveKit: disconnected" }) + { + INFO(text); + const auto d = diagnose_failure(text); + CHECK(d.fault.code == "startup_failed"); + CHECK(d.state == ProducerState::CameraFault); + CHECK_FALSE(contains(d.fault.message, "livekit-url")); + CHECK_FALSE(contains(d.fault.message, "livekit-token")); + CHECK_FALSE(contains(d.fault.message, "--livekit")); + CHECK(ends_with(d.fault.message, std::string("(") + text + ")")); + } +} + +TEST_CASE("the signature advice names no retired --livekit flag", "[camera_fault]") +{ + for (const auto& e : expectations()) + { + INFO(e.text); + CHECK_FALSE(contains(diagnose_failure(e.text).fault.message, "--livekit")); + } + CHECK(contains(diagnose_failure("JetsonEncoder: dequeue timed out").fault.message, "--bitrate")); +} + +TEST_CASE("an unknown failure is a startup_failed camera fault with the original text", "[camera_fault]") +{ + const std::string text = "something entirely new (errno 5)"; + const auto d = diagnose_failure(text); + CHECK(d.fault.code == "startup_failed"); + CHECK(d.state == ProducerState::CameraFault); + CHECK(contains(d.fault.message, "retries automatically")); + CHECK(ends_with(d.fault.message, "(" + text + ")")); + + const auto empty = diagnose_failure(""); + CHECK(empty.fault.code == "startup_failed"); + CHECK(ends_with(empty.fault.message, "()")); +} diff --git a/src/sensing_control/tests/test_control_handler.cpp b/src/sensing_control/tests/test_control_handler.cpp new file mode 100644 index 0000000000..b9fc884f48 --- /dev/null +++ b/src/sensing_control/tests/test_control_handler.cpp @@ -0,0 +1,280 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +using namespace plugins::sensing; +using Json = nlohmann::json; + +namespace +{ + +class FakeActions : public ControlActions +{ +public: + ActionResult set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) override + { + ++calls; + last_track = track; + last_host = host; + last_port = port; + return result; + } + + int calls = 0; + std::string last_track; + std::string last_host; + std::uint16_t last_port = 0; + ActionResult result; +}; + +struct Fixture +{ + Fixture() : status("test-release"), handler(status, actions) + { + StreamStatus left; + left.track = "sensing-left"; + left.sensor_id = 3; + StreamStatus right; + right.track = "sensing-right"; + status.set_streams({ left, right }); + } + + Json call(const Json& request) + { + return Json::parse(handler.handle(request.dump())); + } + + ProducerStatus status; + FakeActions actions; + ControlHandler handler; +}; + +} // namespace + +TEST_CASE("ping answers pong and echoes the id", "[control_handler]") +{ + Fixture f; + const auto reply = f.call({ { "id", 7 }, { "type", "ping" } }); + CHECK(reply["ok"] == true); + CHECK(reply["type"] == "pong"); + CHECK(reply["id"] == 7); +} + +TEST_CASE("a string id is echoed and a missing id is null", "[control_handler]") +{ + Fixture f; + CHECK(f.call({ { "id", "abc" }, { "type", "ping" } })["id"] == "abc"); + CHECK(f.call({ { "type", "ping" } })["id"].is_null()); +} + +TEST_CASE("get_status reports the state, release and streams", "[control_handler]") +{ + Fixture f; + f.status.set_state(ProducerState::CameraFault, Fault{ "camera_error", "check the power" }); + const auto reply = f.call({ { "id", 1 }, { "type", "get_status" } }); + REQUIRE(reply["ok"] == true); + CHECK(reply["type"] == "status"); + const auto& status = reply["status"]; + CHECK(status["state"] == "camera_fault"); + CHECK(status["release"] == "test-release"); + CHECK(status["fault"]["code"] == "camera_error"); + CHECK(status["fault"]["message"] == "check the power"); + REQUIRE(status["streams"].size() == 2); + CHECK(status["streams"][0]["stream"] == "sensing-left"); + CHECK(status["streams"][0]["sensor"] == 3); + CHECK(status["streams"][0]["direct"]["connected"] == false); +} + +TEST_CASE("get_status reports why a direct link is down", "[control_handler]") +{ + Fixture f; + StreamStatus left; + left.track = "sensing-left"; + left.direct_host = "10.0.0.5"; + left.direct_port = 5600; + left.direct_error = "cannot connect to 10.0.0.5:5600: Connection refused"; + StreamStatus right; + right.track = "sensing-right"; + f.status.set_streams({ left, right }); + const auto status = f.call({ { "type", "get_status" } })["status"]; + CHECK(status["streams"][0]["direct"]["error"] == "cannot connect to 10.0.0.5:5600: Connection refused"); + CHECK(status["streams"][0]["direct"]["connected"] == false); + // A link with no error still carries the key, empty. + REQUIRE(status["streams"][1]["direct"].contains("error")); + CHECK(status["streams"][1]["direct"]["error"] == ""); +} + +TEST_CASE("the status has no fault key when healthy", "[control_handler]") +{ + Fixture f; + f.status.set_state(ProducerState::Streaming); + CHECK_FALSE(f.call({ { "type", "get_status" } })["status"].contains("fault")); +} + +TEST_CASE("malformed requests are answered with a code, never dropped", "[control_handler]") +{ + Fixture f; + + SECTION("bad_json") + { + for (const char* text : { "not json", "", "[1,2]", "42", "\"str\"" }) + { + const auto reply = Json::parse(f.handler.handle(text)); + CHECK(reply["ok"] == false); + CHECK(reply["error"]["code"] == "bad_json"); + CHECK_FALSE(reply["error"]["message"].get().empty()); + } + } + SECTION("missing_type") + { + CHECK(f.call({ { "id", 4 } })["error"]["code"] == "missing_type"); + const auto reply = f.call({ { "id", 5 }, { "type", 3 } }); + CHECK(reply["error"]["code"] == "missing_type"); + CHECK(reply["id"] == 5); + } + SECTION("unknown_type") + { + const auto reply = f.call({ { "id", 6 }, { "type", "reboot" } }); + CHECK(reply["ok"] == false); + CHECK(reply["error"]["code"] == "unknown_type"); + CHECK(reply["error"]["message"].get().find("reboot") != std::string::npos); + CHECK(reply["id"] == 6); + } + CHECK(f.actions.calls == 0); +} + +TEST_CASE("set_direct_target validation", "[control_handler]") +{ + Fixture f; + const Json good = { { "id", 9 }, + { "type", "set_direct_target" }, + { "stream", "sensing-left" }, + { "host", "192.168.1.205" }, + { "port", 5600 } }; + + const auto code_of = [&](Json request) { return f.call(request)["error"]["code"]; }; + + SECTION("missing_stream") + { + Json r = good; + r.erase("stream"); + CHECK(code_of(r) == "missing_stream"); + r["stream"] = 5; + CHECK(code_of(r) == "missing_stream"); + } + SECTION("missing_host") + { + Json r = good; + r.erase("host"); + CHECK(code_of(r) == "missing_host"); + r["host"] = 5; + CHECK(code_of(r) == "missing_host"); + } + SECTION("invalid_host") + { + for (const char* host : { "http://x", "a b", "a/b", "-bad", ".bad", "host:5600", "a_b" }) + { + Json r = good; + r["host"] = host; + CHECK(code_of(r) == "invalid_host"); + } + Json r = good; + r["host"] = std::string(254, 'a'); + CHECK(code_of(r) == "invalid_host"); + } + SECTION("invalid_port") + { + Json r = good; + r.erase("port"); + CHECK(code_of(r) == "invalid_port"); + for (const Json& port : { Json(0), Json(-1), Json(65536), Json("5600"), Json(5600.5), Json(nullptr) }) + { + r["port"] = port; + CHECK(code_of(r) == "invalid_port"); + } + } + SECTION("unknown_stream lists the real ones") + { + Json r = good; + r["stream"] = "sensing-up"; + const auto reply = f.call(r); + CHECK(reply["error"]["code"] == "unknown_stream"); + const std::string message = reply["error"]["message"]; + CHECK(message.find("sensing-up") != std::string::npos); + CHECK(message.find("sensing-left") != std::string::npos); + CHECK(message.find("sensing-right") != std::string::npos); + } + CHECK(f.actions.calls == 0); +} + +TEST_CASE("set_direct_target forwards to the action", "[control_handler]") +{ + Fixture f; + + SECTION("success returns the status and echoes the id") + { + const auto reply = f.call({ { "id", 11 }, + { "type", "set_direct_target" }, + { "stream", "sensing-right" }, + { "host", "pc.example" }, + { "port", 65535 } }); + CHECK(reply["ok"] == true); + CHECK(reply["id"] == 11); + CHECK(reply["type"] == "status"); + CHECK(f.actions.calls == 1); + CHECK(f.actions.last_track == "sensing-right"); + CHECK(f.actions.last_host == "pc.example"); + CHECK(f.actions.last_port == 65535); + } + SECTION("IPv4 and IPv6 literals are accepted") + { + for (const char* host : { "127.0.0.1", "::1", "fe80::1" }) + { + const auto reply = f.call( + { { "type", "set_direct_target" }, { "stream", "sensing-left" }, { "host", host }, { "port", 1 } }); + CHECK(reply["ok"] == true); + CHECK(f.actions.last_host == host); + } + } + SECTION("an empty host disables and needs no port") + { + const auto reply = + f.call({ { "id", 12 }, { "type", "set_direct_target" }, { "stream", "sensing-left" }, { "host", "" } }); + CHECK(reply["ok"] == true); + CHECK(f.actions.calls == 1); + CHECK(f.actions.last_host.empty()); + CHECK(f.actions.last_port == 0); + } + SECTION("an action failure is passed through with its own code") + { + f.actions.result = { false, "busy", "the sender is restarting" }; + const auto reply = f.call({ { "id", 13 }, + { "type", "set_direct_target" }, + { "stream", "sensing-left" }, + { "host", "10.1.1.1" }, + { "port", 5600 } }); + CHECK(reply["ok"] == false); + CHECK(reply["id"] == 13); + CHECK(reply["error"]["code"] == "busy"); + CHECK(reply["error"]["message"] == "the sender is restarting"); + } +} + +TEST_CASE("events", "[control_handler]") +{ + ProducerStatus status("r"); + status.set_state(ProducerState::Error, Fault{ "stream_failed", "gone" }); + const auto state = Json::parse(state_event(status.snapshot())); + CHECK(state["type"] == "event"); + CHECK(state["event"] == "state_changed"); + CHECK(state["status"]["state"] == "error"); + CHECK(state["status"]["fault"]["code"] == "stream_failed"); + + const auto error = Json::parse(error_event(Fault{ "c", "m" })); + CHECK(error["event"] == "error"); + CHECK(error["error"]["code"] == "c"); + CHECK(error["error"]["message"] == "m"); +} diff --git a/src/sensing_control/tests/test_control_server.cpp b/src/sensing_control/tests/test_control_server.cpp new file mode 100644 index 0000000000..e8ca1c2e7a --- /dev/null +++ b/src/sensing_control/tests/test_control_server.cpp @@ -0,0 +1,764 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::sensing; +using namespace std::chrono_literals; +namespace ws = plugins::sensing::websocket; + +namespace +{ + +constexpr const char* kToken = "test-token-123"; + +/// A received server frame (server frames are unmasked). +struct ServerFrame +{ + std::uint8_t opcode = 0; + std::string payload; +}; + +/// A bare TCP client that speaks just enough WebSocket to drive the server. +class RawClient +{ +public: + explicit RawClient(std::uint16_t port) + { + m_fd = ::socket(AF_INET, SOCK_STREAM, 0); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + REQUIRE(::connect(m_fd, reinterpret_cast(&addr), sizeof(addr)) == 0); + } + ~RawClient() + { + if (m_fd >= 0) + ::close(m_fd); + } + RawClient(const RawClient&) = delete; + RawClient& operator=(const RawClient&) = delete; + + void send_raw(const std::string& bytes) + { + std::size_t done = 0; + while (done < bytes.size()) + { + const ssize_t n = ::send(m_fd, bytes.data() + done, bytes.size() - done, MSG_NOSIGNAL); + REQUIRE(n > 0); + done += static_cast(n); + } + } + + static std::string upgrade(const std::string& target, const std::string& auth_header) + { + return "GET " + target + + " HTTP/1.1\r\nHost: x\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n" + "Sec-WebSocket-Key: dGhlIHNhbXBsZSBub25jZQ==\r\nSec-WebSocket-Version: 13\r\n" + + auth_header + "\r\n"; + } + + /// Handshake with the right token; returns the response head. + std::string handshake(const std::string& token = kToken) + { + send_raw(upgrade("/", "Authorization: Bearer " + token + "\r\n")); + return read_head(); + } + + /// Reads up to the end of an HTTP head; for a refusal also reads the body that follows. Empty on EOF/timeout. + std::string read_head() + { + while (m_buffer.find("\r\n\r\n") == std::string::npos) + if (!fill(3000)) + break; + const auto end = m_buffer.find("\r\n\r\n"); + if (end == std::string::npos) + return {}; + std::string head = m_buffer.substr(0, end + 4); + m_buffer.erase(0, end + 4); + return head; + } + + /// Reads whatever remains until the peer closes (or the timeout), for HTTP bodies. + std::string read_until_eof(int timeout_ms = 3000) + { + while (fill(timeout_ms)) + { + } + std::string out = std::move(m_buffer); + m_buffer.clear(); + return out; + } + + /// Sends as much as the peer accepts; false once the peer has closed or stopped reading for `timeout_ms`. + bool try_send_raw(const std::string& bytes, int timeout_ms = 3000) + { + std::size_t done = 0; + while (done < bytes.size()) + { + pollfd pfd{ m_fd, POLLOUT, 0 }; + if (::poll(&pfd, 1, timeout_ms) <= 0) + return false; + const ssize_t n = ::send(m_fd, bytes.data() + done, bytes.size() - done, MSG_NOSIGNAL | MSG_DONTWAIT); + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR)) + continue; + if (n <= 0) + return false; + done += static_cast(n); + } + return true; + } + + void send_frame(std::uint8_t opcode, const std::string& payload, bool fin = true, bool masked = true) + { + send_raw(build_frame(opcode, payload, fin, masked)); + } + + static std::string build_frame(std::uint8_t opcode, const std::string& payload, bool fin = true, bool masked = true) + { + const unsigned char mask[4] = { 1, 2, 3, 4 }; + std::string out; + out.push_back(static_cast((fin ? 0x80 : 0) | opcode)); + const std::uint8_t mask_bit = masked ? 0x80 : 0; + if (payload.size() < 126) + { + out.push_back(static_cast(mask_bit | payload.size())); + } + else if (payload.size() <= 0xFFFF) + { + out.push_back(static_cast(mask_bit | 126)); + out.push_back(static_cast(payload.size() >> 8)); + out.push_back(static_cast(payload.size() & 0xFF)); + } + else + { + out.push_back(static_cast(mask_bit | 127)); + for (int shift = 56; shift >= 0; shift -= 8) + out.push_back(static_cast((static_cast(payload.size()) >> shift) & 0xFF)); + } + if (masked) + out.append(reinterpret_cast(mask), 4); + for (std::size_t i = 0; i < payload.size(); ++i) + out.push_back(static_cast(masked ? (static_cast(payload[i]) ^ mask[i % 4]) : payload[i])); + return out; + } + + /// The next server frame, or nullopt on EOF/timeout. + std::optional read_frame(int timeout_ms = 3000) + { + while (true) + { + if (m_buffer.size() >= 2) + { + const auto b = [&](std::size_t i) { return static_cast(m_buffer[i]); }; + std::uint64_t length = b(1) & 0x7F; + std::size_t header = 2; + if (length == 126 && m_buffer.size() >= 4) + { + length = (std::uint64_t(b(2)) << 8) | b(3); + header = 4; + } + else if (length == 127 && m_buffer.size() >= 10) + { + length = 0; + for (int i = 0; i < 8; ++i) + length = (length << 8) | b(2 + i); + header = 10; + } + const bool header_known = + !((b(1) & 0x7F) == 126 && m_buffer.size() < 4) && !((b(1) & 0x7F) == 127 && m_buffer.size() < 10); + if (header_known && m_buffer.size() >= header + length) + { + ServerFrame frame; + frame.opcode = b(0) & 0x0F; + frame.payload = m_buffer.substr(header, static_cast(length)); + m_buffer.erase(0, header + static_cast(length)); + return frame; + } + } + if (!fill(timeout_ms)) + return std::nullopt; + } + } + + /// The close code of the next frame, which must be a close frame; 0 otherwise. + std::uint16_t read_close_code(int timeout_ms = 3000) + { + const auto frame = read_frame(timeout_ms); + if (!frame || frame->opcode != ws::kClose || frame->payload.size() < 2) + return 0; + return static_cast((static_cast(frame->payload[0]) << 8) | + static_cast(frame->payload[1])); + } + + /// True if the peer closes the TCP connection (EOF or reset) within the timeout. + bool peer_closes(int timeout_ms = 3000) + { + const auto until = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < until) + { + pollfd pfd{ m_fd, POLLIN, 0 }; + if (::poll(&pfd, 1, 100) > 0) + { + char buffer[4096]; + const ssize_t n = ::recv(m_fd, buffer, sizeof(buffer), 0); + if (n <= 0) + return true; + } + } + return false; + } + +private: + bool fill(int timeout_ms) + { + pollfd pfd{ m_fd, POLLIN, 0 }; + if (::poll(&pfd, 1, timeout_ms) <= 0) + return false; + char buffer[4096]; + const ssize_t n = ::recv(m_fd, buffer, sizeof(buffer), 0); + if (n <= 0) + return false; + m_buffer.append(buffer, static_cast(n)); + return true; + } + + int m_fd = -1; + std::string m_buffer; +}; + +ControlServerConfig config() +{ + ControlServerConfig c; + c.bind_address = "127.0.0.1"; + c.port = 0; + c.token = kToken; + return c; +} + +std::unique_ptr make_server(ControlServerConfig c = config()) +{ + return std::make_unique(c, [](const std::string& text) { return "echo:" + text; }); +} + +bool wait_clients(const ControlServer& server, std::size_t count) +{ + const auto until = std::chrono::steady_clock::now() + 3s; + while (std::chrono::steady_clock::now() < until) + { + if (server.client_count() == count) + return true; + std::this_thread::sleep_for(5ms); + } + return server.client_count() == count; +} + +} // namespace + +TEST_CASE("an empty token is refused at construction", "[control_server]") +{ + ControlServerConfig c = config(); + c.token.clear(); + CHECK_THROWS_AS(make_server(c), std::runtime_error); +} + +TEST_CASE("a bind address that is not IPv4 is refused", "[control_server]") +{ + ControlServerConfig c = config(); + c.bind_address = "not-an-address"; + CHECK_THROWS_AS(make_server(c), std::runtime_error); +} + +TEST_CASE("a port already in use is refused with the reason", "[control_server]") +{ + auto first = make_server(); + ControlServerConfig c = config(); + c.port = first->port(); + // SO_REUSEADDR does not allow two listeners on the same port. + try + { + make_server(c); + FAIL("expected the second bind to fail"); + } + catch (const std::runtime_error& e) + { + CHECK(std::string(e.what()).find("cannot listen") != std::string::npos); + } +} + +TEST_CASE("read_secret_file takes the first line, trimmed", "[control_server]") +{ + const auto dir = std::filesystem::temp_directory_path(); + const auto path = dir / ("sensing_control_secret_" + std::to_string(::getpid()) + ".txt"); + { + std::ofstream(path) << " hunter2 \t\r\nsecond line\n"; + } + CHECK(read_secret_file(path.string()) == "hunter2"); + { + std::ofstream(path) << " \n"; + } + CHECK_THROWS_AS(read_secret_file(path.string()), std::runtime_error); + std::filesystem::remove(path); + CHECK_THROWS_AS(read_secret_file(path.string()), std::runtime_error); +} + +TEST_CASE("a good handshake is accepted with the RFC accept key", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + const std::string head = client.handshake(); + CHECK(head.rfind("HTTP/1.1 101", 0) == 0); + CHECK(head.find("s3pPLMBiTxaQ9kYGzzhZRbK+xOo=") != std::string::npos); + CHECK(wait_clients(*server, 1)); +} + +TEST_CASE("the token may be sent as a query parameter", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + client.send_raw(RawClient::upgrade(std::string("/?token=") + kToken, "")); + CHECK(client.read_head().rfind("HTTP/1.1 101", 0) == 0); +} + +TEST_CASE("a wrong or absent token is 401 and the connection ends", "[control_server]") +{ + auto server = make_server(); + + SECTION("wrong token") + { + RawClient client(server->port()); + const std::string head = client.handshake("wrong-token"); + CHECK(head.rfind("HTTP/1.1 401", 0) == 0); + CHECK(client.peer_closes()); + } + SECTION("same length, one byte off") + { + RawClient client(server->port()); + std::string almost = kToken; + almost.back() ^= 1; + CHECK(client.handshake(almost).rfind("HTTP/1.1 401", 0) == 0); + } + SECTION("no token") + { + RawClient client(server->port()); + client.send_raw(RawClient::upgrade("/", "")); + CHECK(client.read_head().rfind("HTTP/1.1 401", 0) == 0); + } + CHECK(server->client_count() == 0); +} + +TEST_CASE("a plain HTTP GET is 426", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + client.send_raw("GET / HTTP/1.1\r\nHost: x\r\n\r\n"); + const std::string head = client.read_head(); + CHECK(head.rfind("HTTP/1.1 426", 0) == 0); + CHECK(head.find("Upgrade: websocket") != std::string::npos); + CHECK(client.read_until_eof().find("WebSocket endpoint") != std::string::npos); +} + +TEST_CASE("a malformed request line is 400", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + client.send_raw("POST / HTTP/1.1\r\n\r\n"); + CHECK(client.read_head().rfind("HTTP/1.1 400", 0) == 0); +} + +TEST_CASE("an oversized handshake is refused", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + client.send_raw("GET / HTTP/1.1\r\nX-Pad: " + std::string(16 * 1024, 'a')); + CHECK(client.read_head().rfind("HTTP/1.1 400", 0) == 0); +} + +TEST_CASE("ping frames are answered with pong carrying the payload", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kPing, "beat"); + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->opcode == ws::kPong); + CHECK(frame->payload == "beat"); +} + +TEST_CASE("a text message goes through the handler and the reply comes back", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + + SECTION("a single frame") + { + client.send_frame(ws::kText, "hello"); + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->opcode == ws::kText); + CHECK(frame->payload == "echo:hello"); + } + SECTION("fragments are reassembled") + { + client.send_frame(ws::kText, "he", /*fin=*/false); + client.send_frame(ws::kContinuation, "ll", /*fin=*/false); + client.send_frame(ws::kContinuation, "o", /*fin=*/true); + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->payload == "echo:hello"); + } + SECTION("a message that needs a 16-bit length is served") + { + const std::string big(5000, 'z'); + client.send_frame(ws::kText, big); + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->payload == "echo:" + big); + } + SECTION("several messages in order") + { + for (int i = 0; i < 5; ++i) + client.send_frame(ws::kText, std::to_string(i)); + for (int i = 0; i < 5; ++i) + { + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->payload == "echo:" + std::to_string(i)); + } + } + SECTION("a client close frame is answered with close 1000 and the connection ends") + { + client.send_frame(ws::kClose, std::string("\x03\xe8", 2)); + CHECK(client.read_close_code() == 1000); + CHECK(client.peer_closes()); + CHECK(wait_clients(*server, 0)); + } +} + +TEST_CASE("broadcast reaches every connected client", "[control_server]") +{ + auto server = make_server(); + RawClient a(server->port()); + RawClient b(server->port()); + REQUIRE(a.handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(b.handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(wait_clients(*server, 2)); + + server->broadcast("{\"event\":1}"); + server->broadcast("{\"event\":2}"); + for (RawClient* client : { &a, &b }) + { + const auto first = client->read_frame(); + const auto second = client->read_frame(); + REQUIRE(first); + REQUIRE(second); + CHECK(first->opcode == ws::kText); + CHECK(first->payload == "{\"event\":1}"); + CHECK(second->payload == "{\"event\":2}"); + } +} + +TEST_CASE("a client that has not handshaken does not receive broadcasts", "[control_server]") +{ + auto server = make_server(); + RawClient pending(server->port()); + RawClient open(server->port()); + REQUIRE(open.handshake().rfind("HTTP/1.1 101", 0) == 0); + server->broadcast("x"); + REQUIRE(open.read_frame()); + CHECK_FALSE(pending.read_frame(300)); +} + +TEST_CASE("an oversized message closes the connection with 1009", "[control_server]") +{ + ControlServerConfig c = config(); + c.max_message_bytes = 256; + auto server = make_server(c); + + SECTION("one frame") + { + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, std::string(1000, 'a')); + CHECK(client.read_close_code() == 1009); + CHECK(client.peer_closes()); + } + SECTION("fragments that add up to too much") + { + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, std::string(200, 'a'), /*fin=*/false); + client.send_frame(ws::kContinuation, std::string(200, 'a'), /*fin=*/false); + CHECK(client.read_close_code() == 1009); + } + SECTION("a message exactly at the limit is served") + { + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, std::string(256, 'a')); + const auto frame = client.read_frame(); + REQUIRE(frame); + CHECK(frame->opcode == ws::kText); + } +} + +TEST_CASE("a binary frame closes the connection with 1003", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kBinary, "\x01\x02"); + CHECK(client.read_close_code() == 1003); + CHECK(client.peer_closes()); +} + +TEST_CASE("an unmasked client frame closes the connection with 1002", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, "hi", true, /*masked=*/false); + CHECK(client.read_close_code() == 1002); + CHECK(client.peer_closes()); + CHECK(wait_clients(*server, 0)); +} + +TEST_CASE("max_clients is enforced and a slot is reusable", "[control_server]") +{ + ControlServerConfig c = config(); + c.max_clients = 2; + auto server = make_server(c); + + auto first = std::make_unique(server->port()); + RawClient second(server->port()); + REQUIRE(first->handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(second.handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(wait_clients(*server, 2)); + + { + RawClient third(server->port()); + third.send_raw(RawClient::upgrade("/", std::string("Authorization: Bearer ") + kToken + "\r\n")); + CHECK(third.peer_closes()); + } + CHECK(server->client_count() == 2); + + first.reset(); // a departing client frees a slot + REQUIRE(wait_clients(*server, 1)); + RawClient fourth(server->port()); + CHECK(fourth.handshake().rfind("HTTP/1.1 101", 0) == 0); +} + +TEST_CASE("destroying the server is bounded with idle and half-open clients", "[control_server]") +{ + auto server = make_server(); + RawClient idle(server->port()); + REQUIRE(idle.handshake().rfind("HTTP/1.1 101", 0) == 0); + RawClient half_open(server->port()); // connected, never sends the request + RawClient partial(server->port()); + partial.send_raw("GET / HTTP/1.1\r\nHost: x\r\n"); // an unfinished head + REQUIRE(wait_clients(*server, 1)); + std::this_thread::sleep_for(100ms); + + const auto start = std::chrono::steady_clock::now(); + server.reset(); + CHECK(std::chrono::steady_clock::now() - start < 2s); +} + +TEST_CASE("destroying an idle server is prompt", "[control_server]") +{ + auto server = make_server(); + const auto start = std::chrono::steady_clock::now(); + server.reset(); + CHECK(std::chrono::steady_clock::now() - start < 1s); +} + +TEST_CASE("a client that never reads is cut off instead of buffered for", "[control_server]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(wait_clients(*server, 1)); + + // Far more than the 1 MiB output cap plus the socket buffers; the client reads none of it. + const std::string chunk(32 * 1024, 'x'); + for (int i = 0; i < 600; ++i) + { + server->broadcast(chunk); + if (i % 16 == 0) + std::this_thread::sleep_for(10ms); + } + CHECK(wait_clients(*server, 0)); +} + +TEST_CASE("an orphan continuation frame closes the connection with 1002", "[control_server][fragmentation]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kContinuation, "tail", /*fin=*/true); + CHECK(client.read_close_code() == 1002); + CHECK(client.peer_closes()); + CHECK(wait_clients(*server, 0)); +} + +TEST_CASE("a new text frame inside an unfinished message closes the connection with 1002", + "[control_server][fragmentation]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, "he", /*fin=*/false); + client.send_frame(ws::kText, "other", /*fin=*/true); + CHECK(client.read_close_code() == 1002); + CHECK(client.peer_closes()); +} + +TEST_CASE("a continuation after a finished message is an orphan", "[control_server][fragmentation]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + client.send_frame(ws::kText, "one"); + const auto reply = client.read_frame(); + REQUIRE(reply); + CHECK(reply->payload == "echo:one"); + client.send_frame(ws::kContinuation, "x"); + CHECK(client.read_close_code() == 1002); +} + +TEST_CASE("fragmented messages work back to back, with a ping between fragments", "[control_server][fragmentation]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + for (int round = 0; round < 2; ++round) + { + client.send_frame(ws::kText, "ab", /*fin=*/false); + client.send_frame(ws::kPing, "p"); + client.send_frame(ws::kContinuation, "cd", /*fin=*/true); + const auto pong = client.read_frame(); + REQUIRE(pong); + CHECK(pong->opcode == ws::kPong); + const auto reply = client.read_frame(); + REQUIRE(reply); + CHECK(reply->payload == "echo:abcd"); + } +} + +TEST_CASE("garbage beyond the message limit is cut off and the server stays responsive", "[control_server][limits]") +{ + ControlServerConfig c = config(); + c.max_message_bytes = 1024; + auto server = make_server(c); + + RawClient flooder(server->port()); + REQUIRE(flooder.handshake().rfind("HTTP/1.1 101", 0) == 0); + RawClient other(server->port()); + REQUIRE(other.handshake().rfind("HTTP/1.1 101", 0) == 0); + + // A masked text frame header declaring 1000 bytes, then far more than limit plus headroom of filler without a + // complete valid frame boundary. + std::string flood = std::string("\x81\xFE\x03\xE8", 4) + std::string(4, '\0') + std::string(256 * 1024, 'a'); + const bool all_sent = flooder.try_send_raw(flood); + INFO("all_sent=" << all_sent); + // Either the connection is closed (with or without a close frame) or the sender is held back; never served. + // The leading valid frame may be answered; the filler after it ends in a close, not in service. + std::optional frame; + bool closed_by_frame = false; + while ((frame = flooder.read_frame(3000))) + { + if (frame->opcode == ws::kClose) + { + closed_by_frame = true; + break; + } + CHECK(frame->opcode == ws::kText); + } + INFO("closed_by_frame=" << closed_by_frame); + CHECK(flooder.peer_closes(3000)); + + other.send_frame(ws::kText, "still here"); + const auto reply = other.read_frame(); + REQUIRE(reply); + CHECK(reply->payload == "echo:still here"); + CHECK(wait_clients(*server, 1)); +} + +TEST_CASE("handshake garbage without a terminator is limited and others are served", "[control_server][limits]") +{ + auto server = make_server(); + RawClient flooder(server->port()); + flooder.try_send_raw(std::string(256 * 1024, 'g')); + CHECK(flooder.read_head().rfind("HTTP/1.1 400", 0) == 0); + + RawClient other(server->port()); + REQUIRE(other.handshake().rfind("HTTP/1.1 101", 0) == 0); +} + +TEST_CASE("pipelined requests in one write are all answered", "[control_server][limits]") +{ + auto server = make_server(); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + + // More than the 16 frames a client may have handled per poll round: the rest follow on later rounds. + constexpr int kRequests = 40; + std::string burst; + for (int i = 0; i < kRequests; ++i) + burst += RawClient::build_frame(ws::kText, std::to_string(i)); + client.send_raw(burst); + + const auto begin = std::chrono::steady_clock::now(); + for (int i = 0; i < kRequests; ++i) + { + const auto frame = client.read_frame(2000); + REQUIRE(frame); + CHECK(frame->payload == "echo:" + std::to_string(i)); + } + CHECK(std::chrono::steady_clock::now() - begin < 2s); +} + +TEST_CASE("a client that pipelines requests with large replies and never reads is cut off", "[control_server][limits]") +{ + ControlServerConfig c = config(); + auto server = make_server(c); + RawClient client(server->port()); + REQUIRE(client.handshake().rfind("HTTP/1.1 101", 0) == 0); + RawClient other(server->port()); + REQUIRE(other.handshake().rfind("HTTP/1.1 101", 0) == 0); + REQUIRE(wait_clients(*server, 2)); + + // Each echo is about 60 KiB; the client never reads, so the replies pile up past the output cap. + const std::string request = RawClient::build_frame(ws::kText, std::string(60 * 1024, 'r')); + for (int i = 0; i < 200; ++i) + { + if (!client.try_send_raw(request, 500)) + break; + } + CHECK(wait_clients(*server, 1)); + + other.send_frame(ws::kText, "alive"); + const auto reply = other.read_frame(); + REQUIRE(reply); + CHECK(reply->payload == "echo:alive"); +} diff --git a/src/sensing_control/tests/test_driver_log_capture.cpp b/src/sensing_control/tests/test_driver_log_capture.cpp new file mode 100644 index 0000000000..7bf90a8ff6 --- /dev/null +++ b/src/sensing_control/tests/test_driver_log_capture.cpp @@ -0,0 +1,233 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include +#include + +using plugins::sensing::DriverLogCapture; + +namespace +{ + +bool contains(const std::string& text, const std::string& part) +{ + return text.find(part) != std::string::npos; +} + +std::size_t count_of(const std::string& text, const std::string& part) +{ + std::size_t n = 0; + for (auto at = text.find(part); at != std::string::npos; at = text.find(part, at + 1)) + ++n; + return n; +} + +/// Points fd 2 at a scratch file for the lifetime of the object, so a test can see what the code under test replayed. +class StderrSink +{ +public: + StderrSink() + { + std::fflush(stderr); + char path[] = "/tmp/driver-log-sink-XXXXXX"; + m_file = ::mkstemp(path); + ::unlink(path); + m_saved = ::dup(STDERR_FILENO); + ::dup2(m_file, STDERR_FILENO); + } + + ~StderrSink() + { + restore(); + ::close(m_saved); + ::close(m_file); + } + + std::string restore_and_read() + { + restore(); + std::string text; + ::lseek(m_file, 0, SEEK_SET); + char buffer[4096]; + ssize_t n = 0; + while ((n = ::read(m_file, buffer, sizeof(buffer))) > 0) + text.append(buffer, static_cast(n)); + return text; + } + +private: + void restore() + { + if (!m_restored) + { + std::fflush(stderr); + ::dup2(m_saved, STDERR_FILENO); + m_restored = true; + } + } + + int m_file = -1; + int m_saved = -1; + bool m_restored = false; +}; + +/// What fd `fd` points at (the /proc link text), so two descriptors can be compared. +std::string target_of(int fd) +{ + char target[256] = {}; + const ssize_t n = ::readlink(("/proc/self/fd/" + std::to_string(fd)).c_str(), target, sizeof(target) - 1); + return n < 0 ? std::string() : std::string(target, static_cast(n)); +} + +bool same_file(int fd, int reference) +{ + const auto a = target_of(fd); + return !a.empty() && a == target_of(reference); +} + +void raw_write(int fd, const std::string& text) +{ + REQUIRE(::write(fd, text.data(), text.size()) == static_cast(text.size())); +} + +} // namespace + +TEST_CASE("output on fd 1 and fd 2 inside the scope is captured and replayed to stderr", "[driver_log_capture]") +{ + StderrSink sink; + std::string interesting; + { + DriverLogCapture capture; + raw_write(STDOUT_FILENO, "out: CAMERA_DETECT failed\n"); + raw_write(STDERR_FILENO, "err: lock timeout on link 1\n"); + raw_write(STDERR_FILENO, "harmless chatter\n"); + interesting = capture.finish(); + } + const std::string replayed = sink.restore_and_read(); + + CHECK(contains(interesting, "CAMERA_DETECT failed")); + CHECK(contains(interesting, "lock timeout on link 1")); + CHECK_FALSE(contains(interesting, "harmless chatter")); + // Everything, interesting or not, reaches the journal. + CHECK(contains(replayed, "out: CAMERA_DETECT failed")); + CHECK(contains(replayed, "err: lock timeout on link 1")); + CHECK(contains(replayed, "harmless chatter")); +} + +TEST_CASE("the descriptors are restored after finish and a second finish is empty", "[driver_log_capture]") +{ + StderrSink sink; + const int probe_out = ::dup(STDOUT_FILENO); + const int probe_err = ::dup(STDERR_FILENO); + { + DriverLogCapture capture; + CHECK_FALSE(same_file(STDOUT_FILENO, probe_out)); + capture.finish(); + CHECK(same_file(STDOUT_FILENO, probe_out)); + CHECK(same_file(STDERR_FILENO, probe_err)); + // Text written after finish() goes to the restored stderr, not into a capture. + raw_write(STDERR_FILENO, "after finish\n"); + CHECK(capture.finish().empty()); + } + const std::string replayed = sink.restore_and_read(); + CHECK(count_of(replayed, "after finish") == 1); + ::close(probe_out); + ::close(probe_err); +} + +TEST_CASE("the descriptors are restored when the scope ends without finish", "[driver_log_capture]") +{ + StderrSink sink; + const int probe_out = ::dup(STDOUT_FILENO); + const int probe_err = ::dup(STDERR_FILENO); + { + DriverLogCapture capture; + raw_write(STDERR_FILENO, "lost without finish: error\n"); + CHECK_FALSE(same_file(STDERR_FILENO, probe_err)); + } + CHECK(same_file(STDOUT_FILENO, probe_out)); + CHECK(same_file(STDERR_FILENO, probe_err)); + raw_write(STDERR_FILENO, "after destruction\n"); + const std::string replayed = sink.restore_and_read(); + // The destructor replays what was captured, and later output is not swallowed. + CHECK(contains(replayed, "lost without finish: error")); + CHECK(count_of(replayed, "after destruction") == 1); + ::close(probe_out); + ::close(probe_err); +} + +TEST_CASE("interesting_lines picks failure lines", "[driver_log_capture]") +{ + // Fewer matching lines than the six-line limit per call, so each word is seen. + const std::vector> groups = { + { "an ERROR happened", "operation failed", "Fail to open", "request timeout" }, + { "OPEN_CIRCUIT", "CIRCUIT_OPEN_DISCONNECTION", "DES_LOCK_ERR", "CAMERA_DETECT" } + }; + for (const auto& group : groups) + { + std::string text = "starting up\n"; + for (const auto& line : group) + text += line + "\n"; + text += "all good here\n"; + const auto out = DriverLogCapture::interesting_lines(text); + for (const auto& part : group) + { + INFO(part); + CHECK(contains(out, part)); + } + CHECK_FALSE(contains(out, "starting up")); + CHECK_FALSE(contains(out, "all good here")); + } + CHECK(DriverLogCapture::interesting_lines("").empty()); +} + +TEST_CASE("interesting_lines strips the Severity prefix and joins with semicolons", "[driver_log_capture]") +{ + const std::string text = + "[TID=1234] Module_id 30 Severity 2 : lock timeout on link 1\n" + "[TID=1234] Module_id 30 Severity 2 : open failed\n"; + CHECK(DriverLogCapture::interesting_lines(text) == "lock timeout on link 1; open failed"); + + // A line with " : " but no Severity keeps its text. + CHECK(DriverLogCapture::interesting_lines("camera : error 5\n") == "camera : error 5"); +} + +TEST_CASE("interesting_lines ignores NACK lines", "[driver_log_capture]") +{ + // The driver prints these on healthy starts. + CHECK(DriverLogCapture::interesting_lines("I2C: NACK ... errno=121\nbus NACK at 0x1a\n").empty()); +} + +TEST_CASE("interesting_lines collapses consecutive duplicates", "[driver_log_capture]") +{ + CHECK(DriverLogCapture::interesting_lines("error one\nerror one\nerror one\nerror two\n") == "error one; error two"); +} + +TEST_CASE("interesting_lines keeps the first six lines and at most 600 characters", "[driver_log_capture]") +{ + std::string text; + for (int i = 1; i <= 9; ++i) + text += "error line " + std::to_string(i) + "\n"; + const auto six = DriverLogCapture::interesting_lines(text); + CHECK(contains(six, "error line 1")); + CHECK(contains(six, "error line 6")); + CHECK_FALSE(contains(six, "error line 7")); + CHECK(count_of(six, "; ") == 5); + + const std::string long_line = "error " + std::string(1000, 'x') + "\n"; + CHECK(DriverLogCapture::interesting_lines(long_line).size() == 600); +} + +TEST_CASE("interesting_lines does not match block or clock", "[driver_log_capture]") +{ + CHECK(DriverLogCapture::interesting_lines("block 3 ready\nclock running\nblocked on mutex\n").empty()); + // The substring rule is about "lock", not "block": "lock timeout" style words need the failure word. + CHECK(DriverLogCapture::interesting_lines("pll lock acquired\n").empty()); +} diff --git a/src/sensing_control/tests/test_mock_producer_e2e.py b/src/sensing_control/tests/test_mock_producer_e2e.py new file mode 100755 index 0000000000..13d91df9bb --- /dev/null +++ b/src/sensing_control/tests/test_mock_producer_e2e.py @@ -0,0 +1,429 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""sensing_mock_producer end to end, through the management channel and a real video_stage_worker. + +Binaries come from SENSING_MOCK_PRODUCER and VIDEO_STAGE_WORKER, else tmp/build-vs under the repository root; the +input stream from SENSING_E2E_INPUT, else tmp/vs/orin_like.h264. The test is skipped when any of them is missing. +The worker writes to files (no LiveKit), on ports 5730 and 5731. +""" + +from __future__ import annotations + +import os +import shutil +import signal +import subprocess +import sys +import tempfile +import time +import unittest +from pathlib import Path + +HERE = Path(__file__).resolve().parent +REPO = HERE.parents[2] +sys.path.insert(0, str(HERE.parent / "tools")) + +import orin_control # noqa: E402 + +LEFT_PORT, RIGHT_PORT = 5730, 5731 +CLOSED_PORT = 5799 # nothing listens here + + +def _binary(env: str, default: str) -> Path: + return Path(os.environ.get(env) or REPO / "tmp" / "build-vs" / default) + + +PRODUCER = _binary( + "SENSING_MOCK_PRODUCER", "src/sensing_control/mock_producer/sensing_mock_producer" +) +WORKER = _binary("VIDEO_STAGE_WORKER", "src/video_stage/app/video_stage_worker") +INPUT = Path( + os.environ.get("SENSING_E2E_INPUT") or REPO / "tmp" / "vs" / "orin_like.h264" +) + + +def _missing() -> str | None: + for what, path in ( + ("sensing_mock_producer", PRODUCER), + ("video_stage_worker", WORKER), + ("input stream", INPUT), + ): + if not path.is_file(): + return f"{what} not found at {path} (set SENSING_MOCK_PRODUCER, VIDEO_STAGE_WORKER, SENSING_E2E_INPUT)" + return None + + +def _wait(predicate, timeout: float, interval: float = 0.1): + """Poll until predicate() returns something truthy; return it, or None on timeout.""" + deadline = time.monotonic() + timeout + while True: + value = predicate() + if value: + return value + if time.monotonic() >= deadline: + return None + time.sleep(interval) + + +def _size(path: Path) -> int: + return path.stat().st_size if path.exists() else 0 + + +@unittest.skipIf(_missing() is not None, _missing() or "") +class MockProducerEndToEnd(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + cls.work = Path(tempfile.mkdtemp(prefix="sensing_e2e_", dir=REPO / "tmp")) + cls.token = "e2e-token-" + os.urandom(8).hex() + token_file = cls.work / "token.txt" + token_file.write_text(cls.token + "\n") + token_file.chmod(0o600) + cls.token_file = token_file + cls.out = { + "left": cls.work / "e2e_left.h264", + "right": cls.work / "e2e_right.h264", + } + cls.procs: list[subprocess.Popen] = [] + cls.started = time.monotonic() + + producer_log = (cls.work / "producer.log").open("w") + cls.producer = subprocess.Popen( + [ + str(PRODUCER), + "--control-port=0", + f"--control-token-file={token_file}", + f"--stream=sensing-left={INPUT}", + f"--stream=sensing-right={INPUT}", + "--simulate-fault-seconds=3", + "--retry-seconds=1", + ], + stdout=producer_log, + stderr=subprocess.STDOUT, + ) + cls.procs.append(cls.producer) + + def announced_port() -> int | None: + for line in ( + (cls.work / "producer.log").read_text(errors="replace").splitlines() + ): + if "management channel on" in line: + return int(line.rsplit(":", 1)[1]) + return None + + port = _wait(announced_port, 10) + if port is None: + cls.tearDownClass() + raise RuntimeError( + "the producer did not announce its management channel:\n" + + cls.log("producer.log") + ) + cls.port = port + + worker_log = (cls.work / "worker.log").open("w") + cls.worker = subprocess.Popen( + [ + str(WORKER), + f"--stream={LEFT_PORT}:file={cls.out['left']}", + f"--stream={RIGHT_PORT}:file={cls.out['right']}", + ], + stdout=worker_log, + stderr=subprocess.STDOUT, + start_new_session=True, + ) + cls.procs.append(cls.worker) + + @classmethod + def tearDownClass(cls) -> None: + for proc in cls.procs: + if proc.poll() is None: + proc.send_signal(signal.SIGINT) + for proc in cls.procs: + try: + proc.wait(timeout=10) + except subprocess.TimeoutExpired: + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait() + + @classmethod + def log(cls, name: str) -> str: + path = cls.work / name + return path.read_text(errors="replace")[-3000:] if path.exists() else "" + + # -- helpers ---------------------------------------------------------------------------------------------- + + def connect(self, token: str | None = None) -> orin_control.Connection: + return orin_control.Connection("127.0.0.1", self.port, token or self.token) + + def request(self, message: dict) -> dict: + conn = self.connect() + try: + return conn.request(message) + finally: + conn._sock.close() + + def status(self) -> dict: + reply = self.request({"type": "get_status"}) + self.assertTrue(reply["ok"], reply) + return reply["status"] + + def set_target(self, stream: str, host: str, port: int = 0) -> dict: + return self.request( + {"type": "set_direct_target", "stream": stream, "host": host, "port": port} + ) + + def wait_streaming(self) -> dict: + status = _wait(lambda: (s := self.status())["state"] == "streaming" and s, 30) + self.assertIsNotNone( + status, "the producer never reached streaming:\n" + self.log("producer.log") + ) + return status + + def stream_of(self, status: dict, name: str) -> dict: + return next(s for s in status["streams"] if s["stream"] == name) + + def wait_stable(self, path: Path) -> int: + """The size once it stops changing.""" + last = [-1] + + def stable() -> int | None: + size = _size(path) + if size == last[0]: + return size or None + last[0] = size + time.sleep(0.5) + return None + + value = _wait(stable, 15, interval=0.0) + self.assertIsNotNone(value, f"{path.name} never stopped growing") + return value + + # -- the scenario, in order ------------------------------------------------------------------------------- + + def test_1_fault_then_streaming(self) -> None: + # The simulated unpowered camera is reported in plain words first. + fault = _wait(lambda: (s := self.status())["state"] == "camera_fault" and s, 3) + if fault is None and time.monotonic() - self.started > 3: + self.skipTest("the simulated fault window had already passed") + self.assertIsNotNone( + fault, "never saw camera_fault:\n" + self.log("producer.log") + ) + self.assertIn("power cable", fault["fault"]["message"]) + self.assertEqual(fault["fault"]["code"], "camera_start_failed") + self.assertEqual( + [s["stream"] for s in fault["streams"]], ["sensing-left", "sensing-right"] + ) + + streaming = self.wait_streaming() + self.assertNotIn("fault", streaming) + self.assertGreaterEqual( + time.monotonic() - self.started, + 2.5, + "the fault window was shorter than asked", + ) + + def test_2_set_target_streams_to_the_worker(self) -> None: + self.wait_streaming() + left = self.out["left"] + reply = self.set_target("sensing-left", "127.0.0.1", LEFT_PORT) + self.assertTrue(reply["ok"], reply) + target = self.stream_of(reply["status"], "sensing-left")["direct"] + self.assertEqual((target["host"], target["port"]), ("127.0.0.1", LEFT_PORT)) + + grew = _wait(lambda: _size(left) > 100_000, 20) + self.assertTrue( + grew, "the worker's file did not grow:\n" + self.log("worker.log") + ) + before = _size(left) + self.assertTrue( + _wait(lambda: _size(left) > before + 100_000, 10), + "the file stopped growing", + ) + + connected = _wait( + lambda: self.stream_of(self.status(), "sensing-left")["direct"][ + "connected" + ], + 10, + ) + self.assertTrue( + connected, "the status never reported the direct link as connected" + ) + self.assertFalse( + self.stream_of(self.status(), "sensing-right")["direct"]["host"] + ) + + if shutil.which("ffprobe"): + probe = subprocess.run( + [ + "ffprobe", + "-v", + "error", + "-count_frames", + "-select_streams", + "v:0", + "-show_entries", + "stream=nb_read_frames", + "-of", + "csv=p=0", + str(left), + ], + capture_output=True, + check=False, + text=True, + timeout=60, + ) + self.assertEqual(probe.returncode, 0, probe.stderr) + self.assertGreater(int(probe.stdout.strip().strip(",") or 0), 0) + + def test_3_second_stream_is_independent(self) -> None: + self.wait_streaming() + right = self.out["right"] + self.assertTrue(self.set_target("sensing-right", "127.0.0.1", RIGHT_PORT)["ok"]) + self.assertTrue( + _wait(lambda: _size(right) > 100_000, 20), self.log("worker.log") + ) + + def test_4_empty_host_stops_the_link(self) -> None: + self.wait_streaming() + left = self.out["left"] + reply = self.set_target("sensing-left", "") + self.assertTrue(reply["ok"], reply) + direct = self.stream_of(reply["status"], "sensing-left")["direct"] + self.assertEqual((direct["host"], direct["port"]), ("", 0)) + + settled = self.wait_stable(left) + time.sleep(1.5) + self.assertEqual( + _size(left), settled, "the file kept growing after the link was turned off" + ) + self.assertFalse( + self.stream_of(self.status(), "sensing-left")["direct"]["connected"] + ) + # The other stream is unaffected. + right = self.out["right"] + before = _size(right) + self.assertTrue( + _wait(lambda: _size(right) > before, 10), "the right stream stopped too" + ) + + def test_5_errors(self) -> None: + self.wait_streaming() + with self.assertRaises(orin_control.ChannelError) as refused: + self.connect(token="not-the-token") + self.assertIn("401", str(refused.exception)) + + reply = self.set_target("sensing-up", "127.0.0.1", LEFT_PORT) + self.assertFalse(reply["ok"]) + self.assertEqual(reply["error"]["code"], "unknown_stream") + self.assertIn("sensing-left", reply["error"]["message"]) + + self.assertEqual( + self.set_target("sensing-left", "bad host", 1)["error"]["code"], + "invalid_host", + ) + self.assertEqual( + self.set_target("sensing-left", "127.0.0.1", 0)["error"]["code"], + "invalid_port", + ) + + # The command-line tool agrees. + cli = [ + sys.executable, + str(HERE.parent / "tools" / "orin_control.py"), + f"127.0.0.1:{self.port}", + "--token-file", + str(self.token_file), + ] + ok = subprocess.run( + cli + ["status"], capture_output=True, text=True, timeout=20, check=False + ) + self.assertEqual(ok.returncode, 0, ok.stderr) + self.assertIn("state: streaming", ok.stdout) + bad = subprocess.run( + cli + ["set-target", "nope", "127.0.0.1", "5730"], + capture_output=True, + check=False, + text=True, + timeout=20, + ) + self.assertEqual(bad.returncode, 1) + self.assertIn("unknown_stream", bad.stderr) + + def test_6_unreachable_target_reports_the_error(self) -> None: + status = self.wait_streaming() + # Every stream carries the key, empty while nothing is wrong. + for stream in status["streams"]: + self.assertIn("error", stream["direct"]) + self.assertEqual(self.stream_of(status, "sensing-right")["direct"]["error"], "") + + watcher = self.connect() + reply = self.set_target("sensing-left", "127.0.0.1", CLOSED_PORT) + self.assertTrue(reply["ok"], reply) + self.assertIn( + "error", self.stream_of(reply["status"], "sensing-left")["direct"] + ) + + # The first error to appear after the retarget must already name the new destination. + wanted = f"cannot connect to 127.0.0.1:{CLOSED_PORT}" + + def error_text() -> str | None: + error = self.stream_of(self.status(), "sensing-left")["direct"]["error"] + return error or None + + error = _wait(error_text, 15) + self.assertIsNotNone( + error, "direct.error never appeared:\n" + self.log("producer.log") + ) + self.assertTrue(error.startswith(wanted), error) + direct = self.stream_of(self.status(), "sensing-left")["direct"] + self.assertFalse(direct["connected"]) + self.assertEqual((direct["host"], direct["port"]), ("127.0.0.1", CLOSED_PORT)) + # The other stream is unaffected. + self.assertEqual( + self.stream_of(self.status(), "sensing-right")["direct"]["error"], "" + ) + # The failure is also pushed once to connected clients as an error event. + events = [] + deadline = time.monotonic() + 10 + while time.monotonic() < deadline: + try: + message = watcher.receive(timeout=1.0) + except (TimeoutError, OSError): + continue + if message.get("event") == "error": + events.append(message) + if message["error"]["code"] == "direct_link_failed": + break + watcher._sock.close() + direct_events = [ + e for e in events if e["error"]["code"] == "direct_link_failed" + ] + self.assertEqual(len(direct_events), 1, events) + self.assertIn("sensing-left", direct_events[0]["error"]["message"]) + self.assertIn("cannot connect to", direct_events[0]["error"]["message"]) + + # Turning the link off clears the error. + self.assertTrue(self.set_target("sensing-left", "")["ok"]) + self.assertTrue( + _wait( + lambda: ( + self.stream_of(self.status(), "sensing-left")["direct"]["error"] + == "" + ), + 5, + ), + "the error was not cleared", + ) + + def test_9_producer_stops_on_sigint(self) -> None: + self.producer.send_signal(signal.SIGINT) + self.assertEqual(self.producer.wait(timeout=15), 0, self.log("producer.log")) + + +if __name__ == "__main__": + unittest.main(verbosity=2) diff --git a/src/sensing_control/tests/test_producer_status.cpp b/src/sensing_control/tests/test_producer_status.cpp new file mode 100644 index 0000000000..d7655c956b --- /dev/null +++ b/src/sensing_control/tests/test_producer_status.cpp @@ -0,0 +1,84 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +using namespace plugins::sensing; + +namespace +{ +StreamStatus stream(const std::string& track) +{ + StreamStatus s; + s.track = track; + s.direct_host = "10.0.0.1"; + s.direct_port = 1111; + s.direct_connected = true; + return s; +} +} // namespace + +TEST_CASE("a new status is Starting with the release and no fault", "[producer_status]") +{ + ProducerStatus status("rel-1"); + const auto s = status.snapshot(); + CHECK(s.state == ProducerState::Starting); + CHECK_FALSE(s.fault.has_value()); + CHECK(s.release == "rel-1"); + CHECK(s.streams.empty()); +} + +TEST_CASE("state names", "[producer_status]") +{ + CHECK(std::string(to_string(ProducerState::Starting)) == "starting"); + CHECK(std::string(to_string(ProducerState::Streaming)) == "streaming"); + CHECK(std::string(to_string(ProducerState::CameraFault)) == "camera_fault"); + CHECK(std::string(to_string(ProducerState::Error)) == "error"); + CHECK(std::string(to_string(ProducerState::Stopping)) == "stopping"); +} + +TEST_CASE("a fault is kept only for the failing states and cleared on leaving them", "[producer_status]") +{ + ProducerStatus status("r"); + const Fault fault{ "code", "message" }; + + status.set_state(ProducerState::CameraFault, fault); + REQUIRE(status.snapshot().fault.has_value()); + CHECK(status.snapshot().fault->code == "code"); + + status.set_state(ProducerState::Error, fault); + CHECK(status.snapshot().fault.has_value()); + + status.set_state(ProducerState::Streaming, fault); // a fault passed with a healthy state is ignored + CHECK(status.snapshot().state == ProducerState::Streaming); + CHECK_FALSE(status.snapshot().fault.has_value()); + + status.set_state(ProducerState::CameraFault, fault); + status.set_state(ProducerState::Stopping); + CHECK_FALSE(status.snapshot().fault.has_value()); + + status.set_state(ProducerState::CameraFault); // a failing state with no fault stays empty + CHECK_FALSE(status.snapshot().fault.has_value()); +} + +TEST_CASE("set_direct_target changes one stream and reports unknown names", "[producer_status]") +{ + ProducerStatus status("r"); + status.set_streams({ stream("left"), stream("right") }); + + CHECK(status.set_direct_target("left", "192.168.1.2", 5600)); + auto s = status.snapshot(); + CHECK(s.streams[0].direct_host == "192.168.1.2"); + CHECK(s.streams[0].direct_port == 5600); + CHECK_FALSE(s.streams[0].direct_connected); + CHECK(s.streams[1].direct_host == "10.0.0.1"); + CHECK(s.streams[1].direct_connected); + + CHECK(status.set_direct_target("left", "", 5600)); // an empty host clears the port too + s = status.snapshot(); + CHECK(s.streams[0].direct_host.empty()); + CHECK(s.streams[0].direct_port == 0); + + CHECK_FALSE(status.set_direct_target("nope", "h", 1)); +} diff --git a/src/sensing_control/tests/test_producer_supervisor.cpp b/src/sensing_control/tests/test_producer_supervisor.cpp new file mode 100644 index 0000000000..cb71b14aa1 --- /dev/null +++ b/src/sensing_control/tests/test_producer_supervisor.cpp @@ -0,0 +1,780 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::sensing; +using namespace std::chrono_literals; +using Json = nlohmann::json; + +namespace +{ + +/// Everything the fake factory and its sessions record, shared with the test thread. +struct World +{ + std::mutex mutex; + int factory_calls = 0; + int fail_factory_until_call = 0; ///< calls up to and including this number throw + std::string factory_failure = "INvSIPLCamera::Init failed with SIPLStatus 10"; + std::function on_factory; ///< may throw; runs under no lock + std::vector targets_seen; + int sessions_created = 0; + int update_calls = 0; + int throw_update_after = -1; ///< a session's update() throws on its N+1th call; -1 never + int flushes = 0; + std::vector> retargets; + std::vector retarget_threads; ///< the thread each retarget was called on + std::vector sequence; ///< "update" and "retarget:::" in call order + std::vector published; + std::string update_failure = "room lost"; ///< the text a throwing update() carries + std::uint64_t frames = 5; ///< what every stream reports + bool advance_frames = false; ///< every streams() call adds a frame + std::map direct_errors; ///< per track +}; + +class FakeSession : public CameraSession +{ +public: + FakeSession(World& world, DirectTargets targets) : m_world(world), m_targets(std::move(targets)) + { + } + void update() override + { + std::lock_guard lock(m_world.mutex); + ++m_world.update_calls; + m_world.sequence.emplace_back("update"); + if (m_world.throw_update_after >= 0 && ++m_updates > m_world.throw_update_after) + throw std::runtime_error(m_world.update_failure); + } + void flush() override + { + std::lock_guard lock(m_world.mutex); + ++m_world.flushes; + } + std::vector streams() const override + { + std::lock_guard lock(m_world.mutex); + std::vector out; + for (const auto& target : m_targets) + { + StreamStatus s; + s.track = target.first; + if (m_world.advance_frames) + ++m_world.frames; + s.frames = m_world.frames; + const auto error = m_world.direct_errors.find(target.first); + if (error != m_world.direct_errors.end()) + s.direct_error = error->second; + // The session's own view of where it sends, as the real producer reports it. + s.direct_host = target.second.host; + s.direct_port = target.second.port; + out.push_back(s); + } + return out; + } + void set_direct_target(const std::string& track, const std::string& host, std::uint16_t port) override + { + std::lock_guard lock(m_world.mutex); + m_world.retargets.emplace_back(track, host, port); + m_world.retarget_threads.push_back(std::this_thread::get_id()); + m_world.sequence.push_back("retarget:" + track + ":" + host + ":" + std::to_string(port)); + m_targets[track] = { host, port }; + } + +private: + World& m_world; + DirectTargets m_targets; + int m_updates = 0; +}; + +struct Harness +{ + explicit Harness(std::vector configured = default_streams(), + bool with_publish = true, + std::chrono::milliseconds stall_timeout = 0ms) + : status("rel") + { + SupervisorConfig config; + config.stall_timeout = stall_timeout; + config.retry_interval = 100ms; + config.poll_interval = 2ms; + config.status_interval = 20ms; + supervisor = std::make_unique( + status, std::move(configured), + [this](const DirectTargets& targets) -> std::unique_ptr + { + std::function hook; + { + std::lock_guard lock(world.mutex); + ++world.factory_calls; + world.targets_seen.push_back(targets); + hook = world.on_factory; + if (world.factory_calls <= world.fail_factory_until_call) + throw std::runtime_error(world.factory_failure); + ++world.sessions_created; + } + if (hook) + hook(world); + return std::make_unique(world, targets); + }, + with_publish ? std::function( + [this](const std::string& text) + { + std::lock_guard lock(world.mutex); + world.published.push_back(text); + }) : + std::function(), + config); + } + + ~Harness() + { + stop_and_join(); + } + + static std::vector default_streams() + { + StreamStatus a; + a.track = "a"; + StreamStatus b; + b.track = "b"; + b.direct_host = "cfg-host"; + b.direct_port = 4000; + return { a, b }; + } + + void start() + { + thread = std::thread( + [this] + { + run_thread_id = std::this_thread::get_id(); + supervisor->run([this] { return stop.load(); }); + }); + } + + void stop_and_join() + { + stop = true; + if (thread.joinable()) + thread.join(); + } + + template + bool wait_for(Pred pred, std::chrono::milliseconds limit = 5s) + { + const auto until = std::chrono::steady_clock::now() + limit; + while (std::chrono::steady_clock::now() < until) + { + if (pred()) + return true; + std::this_thread::sleep_for(5ms); + } + return pred(); + } + + int sessions() + { + std::lock_guard lock(world.mutex); + return world.sessions_created; + } + + int updates() + { + std::lock_guard lock(world.mutex); + return world.update_calls; + } + + bool state_is(ProducerState state) + { + return status.snapshot().state == state; + } + + int count_events(const std::string& event, const std::string& code = "") + { + std::lock_guard lock(world.mutex); + int n = 0; + for (const auto& text : world.published) + { + const auto j = Json::parse(text); + if (j["event"] == event && (code.empty() || j["error"]["code"] == code)) + ++n; + } + return n; + } + + World world; + ProducerStatus status; + std::unique_ptr supervisor; + std::atomic stop{ false }; + std::thread thread; + std::atomic run_thread_id{}; +}; + +} // namespace + +TEST_CASE("a camera that will not start is a reported state, announced once, then recovers", "[supervisor]") +{ + Harness h; + h.world.fail_factory_until_call = 4; + h.start(); + + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::CameraFault); })); + { + const auto s = h.status.snapshot(); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "camera_start_failed"); + CHECK(s.fault->message.find("power cable") != std::string::npos); + CHECK(s.fault->message.find("SIPLStatus 10") != std::string::npos); + CHECK(s.streams.size() == 2); // the configured streams stay listed while the cameras are down + } + + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + CHECK_FALSE(h.status.snapshot().fault.has_value()); + CHECK(h.world.factory_calls == 5); // four failures, then one success + + // Four identical failures produced exactly one announcement. + CHECK(h.count_events("error", "camera_start_failed") == 1); + CHECK(h.count_events("error") == 1); + h.stop_and_join(); +} + +TEST_CASE("a different failure message is announced again", "[supervisor]") +{ + Harness h; + h.world.fail_factory_until_call = 1000; + h.start(); + REQUIRE(h.wait_for([&] { return h.count_events("error") == 1; })); + { + std::lock_guard lock(h.world.mutex); + h.world.factory_failure = "brand new problem"; + } + REQUIRE(h.wait_for([&] { return h.count_events("error") == 2; })); + CHECK(h.status.snapshot().fault->code == "startup_failed"); + h.stop_and_join(); +} + +TEST_CASE("a StartupFailure supplies its own fault and state", "[supervisor]") +{ + // A factory that has already explained its failure. + SupervisorConfig config; + config.retry_interval = 100ms; + config.poll_interval = 2ms; + std::atomic calls{ 0 }; + ProducerStatus status("r"); + ProducerSupervisor supervisor( + status, Harness::default_streams(), + [&](const DirectTargets&) -> std::unique_ptr + { + ++calls; + throw StartupFailure(Fault{ "room_unreachable", "Cannot reach the PC video stage." }, ProducerState::Error); + }, + nullptr, config); + std::atomic stop{ false }; + std::thread thread([&] { supervisor.run([&] { return stop.load(); }); }); + const auto until = std::chrono::steady_clock::now() + 5s; + while (status.snapshot().state != ProducerState::Error && std::chrono::steady_clock::now() < until) + std::this_thread::sleep_for(5ms); + const auto s = status.snapshot(); + stop = true; + thread.join(); + CHECK(s.state == ProducerState::Error); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "room_unreachable"); + CHECK(s.fault->message == "Cannot reach the PC video stage."); + CHECK(calls.load() >= 1); +} + +TEST_CASE("update() throwing reports Error and the session is restarted", "[supervisor]") +{ + Harness h; + h.world.throw_update_after = 3; + h.world.update_failure = "room lost"; + h.start(); + + REQUIRE(h.wait_for([&] { return h.count_events("error", "stream_failed") >= 1; })); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Error); })); + const auto s = h.status.snapshot(); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "stream_failed"); + CHECK(s.fault->message.find("room lost") != std::string::npos); + + // After the retry interval a fresh session starts and streams again. + REQUIRE(h.wait_for([&] { return h.sessions() >= 2; })); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming) || h.state_is(ProducerState::Error); })); + h.stop_and_join(); + CHECK(h.world.flushes >= 1); +} + +TEST_CASE("an unrecognised update() failure is reported as stream_failed with its text", "[supervisor]") +{ + Harness h; + h.world.throw_update_after = 3; + h.world.update_failure = "something odd"; + h.start(); + + REQUIRE(h.wait_for([&] { return h.count_events("error", "stream_failed") >= 1; })); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Error); })); + const auto s = h.status.snapshot(); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "stream_failed"); + CHECK(s.fault->message.find("something odd") != std::string::npos); + CHECK(h.count_events("error", "startup_failed") == 0); + h.stop_and_join(); +} + +TEST_CASE("a restarted session keeps targets set at runtime", "[supervisor]") +{ + Harness h; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + + SECTION("set while streaming reaches the live session and the next factory call") + { + const auto result = h.supervisor->set_direct_target("a", "10.9.8.7", 5730); + CHECK(result.ok); + // The running thread applies it between updates, so it arrives shortly after the call returns. + REQUIRE(h.wait_for( + [&] + { + std::lock_guard lock(h.world.mutex); + return !h.world.retargets.empty(); + })); + { + std::lock_guard lock(h.world.mutex); + REQUIRE(h.world.retargets.size() == 1); + CHECK(std::get<0>(h.world.retargets[0]) == "a"); + CHECK(std::get<1>(h.world.retargets[0]) == "10.9.8.7"); + CHECK(std::get<2>(h.world.retargets[0]) == 5730); + } + // The status shows what the session reports after it applied the change. + REQUIRE(h.wait_for( + [&] + { + const auto s = h.status.snapshot(); + return !s.streams.empty() && s.streams[0].direct_host == "10.9.8.7" && + s.streams[0].direct_port == 5730 && s.streams[0].frames == 5; + })); + // A broadcast notified the change. + CHECK(h.count_events("state_changed") >= 1); + + // Make the session fail; the next factory call must see the remembered target. + { + std::lock_guard lock(h.world.mutex); + h.world.throw_update_after = 0; + } + REQUIRE(h.wait_for([&] { return h.sessions() >= 2; }, 10s)); + std::lock_guard lock(h.world.mutex); + const auto& last = h.world.targets_seen.back(); + REQUIRE(last.count("a") == 1); + CHECK(last.at("a").host == "10.9.8.7"); + CHECK(last.at("a").port == 5730); + CHECK(last.at("b").host == "cfg-host"); // untouched streams keep their configured target + CHECK(last.at("b").port == 4000); + } + SECTION("an empty host disables the link and is remembered as such") + { + CHECK(h.supervisor->set_direct_target("b", "", 1234).ok); + REQUIRE(h.wait_for( + [&] + { + std::lock_guard lock(h.world.mutex); + return !h.world.retargets.empty(); + })); + { + std::lock_guard lock(h.world.mutex); + h.world.throw_update_after = 0; + } + REQUIRE(h.wait_for([&] { return h.sessions() >= 2; }, 10s)); + std::lock_guard lock(h.world.mutex); + CHECK(h.world.targets_seen.back().at("b").host.empty()); + CHECK(h.world.targets_seen.back().at("b").port == 0); + } + SECTION("an unknown stream is rejected and changes nothing") + { + const auto result = h.supervisor->set_direct_target("nope", "h", 1); + CHECK_FALSE(result.ok); + CHECK(result.code == "unknown_stream"); + std::lock_guard lock(h.world.mutex); + CHECK(h.world.retargets.empty()); + } + h.stop_and_join(); +} + +TEST_CASE("a target set while the cameras are down is used when they come up", "[supervisor]") +{ + Harness h; + h.world.fail_factory_until_call = 1000; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::CameraFault); })); + + CHECK(h.supervisor->set_direct_target("a", "10.0.0.5", 5731).ok); + { + std::lock_guard lock(h.world.mutex); + CHECK(h.world.retargets.empty()); // there was no session to retarget + h.world.fail_factory_until_call = 0; + } + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + std::lock_guard lock(h.world.mutex); + CHECK(h.world.targets_seen.back().at("a").host == "10.0.0.5"); + CHECK(h.world.targets_seen.back().at("a").port == 5731); +} + +TEST_CASE("the stop flag ends run() with Stopping, even during a retry wait", "[supervisor]") +{ + SECTION("while streaming") + { + Harness h; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + h.stop_and_join(); + CHECK(h.state_is(ProducerState::Stopping)); + CHECK(h.world.flushes == 1); + } + SECTION("while the cameras are down") + { + Harness h; + h.world.fail_factory_until_call = 1000; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::CameraFault); })); + const auto begin = std::chrono::steady_clock::now(); + h.stop_and_join(); + CHECK(std::chrono::steady_clock::now() - begin < 2s); + CHECK(h.state_is(ProducerState::Stopping)); + CHECK_FALSE(h.status.snapshot().fault.has_value()); + } +} + +TEST_CASE("an empty publish callback is allowed", "[supervisor]") +{ + Harness h(Harness::default_streams(), /*with_publish=*/false); + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + CHECK(h.supervisor->set_direct_target("a", "h", 1).ok); + h.stop_and_join(); +} + +TEST_CASE("a factory failure is diagnosed and sets the state from the diagnosis", "[supervisor]") +{ + SECTION("an unrecognised startup failure is a generic CameraFault") + { + Harness h; + h.world.fail_factory_until_call = 1000; + h.world.factory_failure = "LiveKit: cannot join the room"; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::CameraFault); })); + const auto s = h.status.snapshot(); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "startup_failed"); + CHECK(s.fault->message.find("LiveKit: cannot join the room") != std::string::npos); + CHECK(h.count_events("error", "startup_failed") == 1); + h.stop_and_join(); + } + SECTION("a driver line about an open circuit is a CameraFault") + { + Harness h; + h.world.fail_factory_until_call = 1000; + h.world.factory_failure = "INvSIPLCamera::Init failed with SIPLStatus 10 [Severity 2 : OPEN_CIRCUIT]"; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::CameraFault); })); + const auto s = h.status.snapshot(); + REQUIRE(s.fault.has_value()); + CHECK(s.fault->code == "camera_power_missing"); + h.stop_and_join(); + } +} + +TEST_CASE("a stream whose frames stop is failed and the session restarted", "[supervisor][stall]") +{ + Harness h(Harness::default_streams(), true, 300ms); + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + // The fake session's frame count never moves, so the watchdog fires. + REQUIRE(h.wait_for([&] { return h.count_events("error", "camera_no_frames") >= 1; })); + { + const auto s = h.status.snapshot(); + // The text names the stall; camera_fault.cpp files "no frame for" under CameraFault. + if (s.fault.has_value() && s.fault->code == "camera_no_frames") + { + CHECK(s.fault->message.find("no frame for") != std::string::npos); + CHECK(s.fault->message.find("(stream ") != std::string::npos); + CHECK(s.fault->message.find("from sensor") != std::string::npos); + } + } + REQUIRE(h.wait_for([&] { return h.sessions() >= 2; })); + h.stop_and_join(); + CHECK(h.world.flushes >= 1); +} + +TEST_CASE("the stall state and text are those of the diagnosis", "[supervisor][stall]") +{ + Harness h(Harness::default_streams(), true, 300ms); + h.start(); + REQUIRE(h.wait_for([&] { return h.count_events("error", "camera_no_frames") >= 1; })); + const auto event = [&] + { + std::lock_guard lock(h.world.mutex); + for (const auto& text : h.world.published) + { + const auto j = Json::parse(text); + if (j["event"] == "error" && j["error"]["code"] == "camera_no_frames") + return j; + } + return Json(); + }(); + CHECK(event["error"]["message"].get().find("no frame for") != std::string::npos); + h.stop_and_join(); +} + +TEST_CASE("frames that keep advancing are not a stall", "[supervisor][stall]") +{ + Harness h(Harness::default_streams(), true, 300ms); + h.world.advance_frames = true; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + std::this_thread::sleep_for(1200ms); + CHECK(h.state_is(ProducerState::Streaming)); + CHECK(h.count_events("error") == 0); + h.stop_and_join(); + CHECK(h.world.sessions_created == 1); +} + +TEST_CASE("a stall timeout of zero disables the watchdog", "[supervisor][stall]") +{ + Harness h(Harness::default_streams(), true, 0ms); + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + std::this_thread::sleep_for(1200ms); // frames never move + CHECK(h.state_is(ProducerState::Streaming)); + CHECK(h.count_events("error") == 0); + h.stop_and_join(); + CHECK(h.world.sessions_created == 1); +} + +TEST_CASE("a direct-link error is published once per distinct reason", "[supervisor][direct_error]") +{ + Harness h; + h.world.advance_frames = true; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + CHECK(h.count_events("error", "direct_link_failed") == 0); + + { + std::lock_guard lock(h.world.mutex); + h.world.direct_errors["a"] = "cannot connect to 10.0.0.5:5600: Connection refused"; + } + REQUIRE(h.wait_for([&] { return h.count_events("error", "direct_link_failed") == 1; })); + // Many status refreshes later, still one. + std::this_thread::sleep_for(300ms); + CHECK(h.count_events("error", "direct_link_failed") == 1); + + { + std::lock_guard lock(h.world.mutex); + h.world.direct_errors["a"] = "10.0.0.5:5600 closed the control connection"; + } + REQUIRE(h.wait_for([&] { return h.count_events("error", "direct_link_failed") == 2; })); + + // Clearing publishes nothing. + { + std::lock_guard lock(h.world.mutex); + h.world.direct_errors["a"] = ""; + } + std::this_thread::sleep_for(300ms); + CHECK(h.count_events("error", "direct_link_failed") == 2); + + // The same reason coming back after it cleared is a new occurrence. + { + std::lock_guard lock(h.world.mutex); + h.world.direct_errors["a"] = "10.0.0.5:5600 closed the control connection"; + } + REQUIRE(h.wait_for([&] { return h.count_events("error", "direct_link_failed") == 3; })); + + // It names the stream and the reason, and does not change the producer state. + { + std::lock_guard lock(h.world.mutex); + const auto j = Json::parse(h.world.published.back()); + const auto message = j["error"]["message"].get(); + CHECK(message.find("Stream a: ") != std::string::npos); + CHECK(message.find("closed the control connection") != std::string::npos); + } + CHECK(h.state_is(ProducerState::Streaming)); + h.stop_and_join(); +} + +TEST_CASE("direct-link errors of different streams are independent", "[supervisor][direct_error]") +{ + Harness h; + h.world.advance_frames = true; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + { + std::lock_guard lock(h.world.mutex); + h.world.direct_errors["a"] = "same reason"; + h.world.direct_errors["b"] = "same reason"; + } + REQUIRE(h.wait_for([&] { return h.count_events("error", "direct_link_failed") == 2; })); + h.stop_and_join(); +} + +namespace +{ + +/// A factory hook that parks the first call until released, so the test can act while a session is being built. +struct FactoryGate +{ + explicit FactoryGate(World& world) : released(release.get_future().share()) + { + world.on_factory = [this](World&) + { + if (!entered_once.exchange(true)) + entered.set_value(); + released.wait(); + }; + } + std::promise entered_promise; + std::promise release; + std::shared_future released; + std::atomic entered_once{ false }; + std::promise& entered = entered_promise; +}; + +} // namespace + +TEST_CASE("a change made while the factory runs is applied before the session's first update", "[supervisor][threading]") +{ + Harness h; + FactoryGate gate(h.world); + auto entered = gate.entered_promise.get_future(); + h.start(); + REQUIRE(entered.wait_for(5s) == std::future_status::ready); + + // The factory is blocked: the session does not exist yet. The call must return at once. + const auto begin = std::chrono::steady_clock::now(); + CHECK(h.supervisor->set_direct_target("a", "10.1.1.1", 5741).ok); + CHECK(std::chrono::steady_clock::now() - begin < 1s); + { + std::lock_guard lock(h.world.mutex); + CHECK(h.world.retargets.empty()); + REQUIRE(h.world.targets_seen.size() == 1); + CHECK(h.world.targets_seen[0].at("a").host.empty()); // chosen after the factory took its snapshot + } + gate.release.set_value(); + + REQUIRE(h.wait_for([&] { return h.updates() >= 3; })); + std::lock_guard lock(h.world.mutex); + REQUIRE(h.world.retargets.size() == 1); + CHECK(std::get<0>(h.world.retargets[0]) == "a"); + CHECK(std::get<1>(h.world.retargets[0]) == "10.1.1.1"); + CHECK(std::get<2>(h.world.retargets[0]) == 5741); + REQUIRE(h.world.sequence.size() >= 2); + CHECK(h.world.sequence[0] == "retarget:a:10.1.1.1:5741"); // before the first update + CHECK(h.world.sequence[1] == "update"); +} + +TEST_CASE("a target chosen before the factory runs is passed to it and not applied again", "[supervisor][threading]") +{ + Harness h; + CHECK(h.supervisor->set_direct_target("a", "10.2.2.2", 5742).ok); + h.start(); + REQUIRE(h.wait_for([&] { return h.updates() >= 5; })); + h.stop_and_join(); + std::lock_guard lock(h.world.mutex); + REQUIRE(h.world.targets_seen.size() >= 1); + CHECK(h.world.targets_seen[0].at("a").host == "10.2.2.2"); + CHECK(h.world.targets_seen[0].at("a").port == 5742); + CHECK(h.world.retargets.empty()); +} + +TEST_CASE("set_direct_target never calls into the session from the caller's thread", "[supervisor][threading]") +{ + Harness h; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + for (int i = 0; i < 5; ++i) + CHECK(h.supervisor->set_direct_target("a", "10.3.3." + std::to_string(i), 5743).ok); + REQUIRE(h.wait_for( + [&] + { + std::lock_guard lock(h.world.mutex); + return !h.world.retargets.empty() && std::get<1>(h.world.retargets.back()) == "10.3.3.4"; + })); + h.stop_and_join(); + std::lock_guard lock(h.world.mutex); + REQUIRE_FALSE(h.world.retarget_threads.empty()); + for (const auto& id : h.world.retarget_threads) + { + CHECK(id == h.run_thread_id.load()); + CHECK(id != std::this_thread::get_id()); + } +} + +TEST_CASE("changes to the same track coalesce to the last", "[supervisor][threading]") +{ + Harness h; + FactoryGate gate(h.world); + auto entered = gate.entered_promise.get_future(); + h.start(); + REQUIRE(entered.wait_for(5s) == std::future_status::ready); + + CHECK(h.supervisor->set_direct_target("a", "10.4.4.1", 1).ok); + CHECK(h.supervisor->set_direct_target("a", "10.4.4.2", 2).ok); + CHECK(h.supervisor->set_direct_target("a", "10.4.4.3", 3).ok); + CHECK(h.supervisor->set_direct_target("b", "10.4.4.9", 9).ok); + gate.release.set_value(); + + REQUIRE(h.wait_for([&] { return h.updates() >= 3; })); + h.stop_and_join(); + std::lock_guard lock(h.world.mutex); + REQUIRE(h.world.retargets.size() == 2); // one per track, not one per call + int a_calls = 0; + for (const auto& r : h.world.retargets) + { + if (std::get<0>(r) == "a") + { + ++a_calls; + CHECK(std::get<1>(r) == "10.4.4.3"); + CHECK(std::get<2>(r) == 3); + } + } + CHECK(a_calls == 1); +} + +TEST_CASE("the status shows the session's reported host and port", "[supervisor][threading]") +{ + Harness h; + h.start(); + REQUIRE(h.wait_for([&] { return h.state_is(ProducerState::Streaming); })); + CHECK(h.supervisor->set_direct_target("a", "10.5.5.5", 5745).ok); + REQUIRE(h.wait_for( + [&] + { + const auto s = h.status.snapshot(); + for (const auto& stream : s.streams) + if (stream.track == "a") + return stream.direct_host == "10.5.5.5" && stream.direct_port == 5745; + return false; + })); + const auto s = h.status.snapshot(); + for (const auto& stream : s.streams) + { + if (stream.track == "b") + { + CHECK(stream.direct_host == "cfg-host"); + CHECK(stream.direct_port == 4000); + } + } +} diff --git a/src/sensing_control/tests/test_websocket.cpp b/src/sensing_control/tests/test_websocket.cpp new file mode 100644 index 0000000000..e06c864887 --- /dev/null +++ b/src/sensing_control/tests/test_websocket.cpp @@ -0,0 +1,317 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include + +using namespace plugins::sensing::websocket; + +namespace +{ + +std::string hex(const std::string& bytes) +{ + static constexpr char kDigits[] = "0123456789abcdef"; + std::string out; + for (const unsigned char c : bytes) + { + out.push_back(kDigits[c >> 4]); + out.push_back(kDigits[c & 0x0F]); + } + return out; +} + +/// A client frame: masked, as RFC 6455 requires. +std::string client_frame(std::uint8_t opcode, const std::string& payload, bool fin = true, bool masked = true) +{ + const unsigned char mask[4] = { 0x37, 0xfa, 0x21, 0x3d }; + std::string out; + out.push_back(static_cast((fin ? 0x80 : 0) | opcode)); + const std::uint8_t mask_bit = masked ? 0x80 : 0; + if (payload.size() < 126) + { + out.push_back(static_cast(mask_bit | payload.size())); + } + else if (payload.size() <= 0xFFFF) + { + out.push_back(static_cast(mask_bit | 126)); + out.push_back(static_cast(payload.size() >> 8)); + out.push_back(static_cast(payload.size() & 0xFF)); + } + else + { + out.push_back(static_cast(mask_bit | 127)); + for (int shift = 56; shift >= 0; shift -= 8) + out.push_back(static_cast((static_cast(payload.size()) >> shift) & 0xFF)); + } + if (masked) + out.append(reinterpret_cast(mask), 4); + for (std::size_t i = 0; i < payload.size(); ++i) + out.push_back(static_cast(masked ? (static_cast(payload[i]) ^ mask[i % 4]) : payload[i])); + return out; +} + +std::string upgrade_request(const std::string& target = "/", + const std::string& extra_headers = "", + const std::string& version = "13") +{ + return "GET " + target + + " HTTP/1.1\r\nHost: x\r\nUpgrade: websocket\r\nConnection: keep-alive, Upgrade\r\n" + "Sec-WebSocket-Key: dGhlIHNhbXBsZSBub25jZQ==\r\nSec-WebSocket-Version: " + + version + "\r\n" + extra_headers + "\r\n"; +} + +} // namespace + +TEST_CASE("sha1 matches the standard test vectors", "[websocket]") +{ + CHECK(hex(sha1("abc")) == "a9993e364706816aba3e25717850c26c9cd0d89d"); + CHECK(hex(sha1("")) == "da39a3ee5e6b4b0d3255bfef95601890afd80709"); + // Two blocks after padding. + CHECK(hex(sha1("abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq")) == + "84983e441c3bd26ebaae4aa1f95129e5e54670f1"); +} + +TEST_CASE("base64 and the RFC 6455 accept key", "[websocket]") +{ + CHECK(base64_encode("") == ""); + CHECK(base64_encode("f") == "Zg=="); + CHECK(base64_encode("fo") == "Zm8="); + CHECK(base64_encode("foo") == "Zm9v"); + CHECK(accept_key("dGhlIHNhbXBsZSBub25jZQ==") == "s3pPLMBiTxaQ9kYGzzhZRbK+xOo="); +} + +TEST_CASE("constant_time_equal", "[websocket]") +{ + CHECK(constant_time_equal("secret", "secret")); + CHECK(constant_time_equal("", "")); + CHECK_FALSE(constant_time_equal("secret", "secreT")); + CHECK_FALSE(constant_time_equal("secret", "secre")); + CHECK_FALSE(constant_time_equal("", "x")); +} + +TEST_CASE("parse_frame decodes the three length forms", "[websocket]") +{ + SECTION("short") + { + const auto r = parse_frame(client_frame(kText, "hello"), 1 << 20); + REQUIRE(r.status == ParseStatus::Complete); + CHECK(r.frame.fin); + CHECK(r.frame.opcode == kText); + CHECK(r.frame.payload == "hello"); + CHECK(r.consumed == 2 + 4 + 5); + } + SECTION("125 is the last short form, 126 uses 16 bits") + { + const std::string p125(125, 'a'); + const std::string p126(126, 'b'); + const auto a = parse_frame(client_frame(kText, p125), 1 << 20); + REQUIRE(a.status == ParseStatus::Complete); + CHECK(a.consumed == 2 + 4 + 125); + const auto b = parse_frame(client_frame(kText, p126), 1 << 20); + REQUIRE(b.status == ParseStatus::Complete); + CHECK(b.frame.payload == p126); + CHECK(b.consumed == 4 + 4 + 126); + } + SECTION("65535 is the 16-bit maximum, 65536 uses 64 bits") + { + const std::string p16(65535, 'c'); + const std::string p64(65536, 'd'); + const auto a = parse_frame(client_frame(kBinary, p16), 1 << 20); + REQUIRE(a.status == ParseStatus::Complete); + CHECK(a.consumed == 4 + 4 + 65535); + const auto b = parse_frame(client_frame(kBinary, p64), 1 << 20); + REQUIRE(b.status == ParseStatus::Complete); + CHECK(b.frame.payload == p64); + CHECK(b.consumed == 10 + 4 + 65536); + } +} + +TEST_CASE("parse_frame waits for more bytes and leaves trailing bytes", "[websocket]") +{ + const std::string frame = client_frame(kText, std::string(300, 'x')); + for (const std::size_t cut : { std::size_t(0), std::size_t(1), std::size_t(3), std::size_t(5), frame.size() - 1 }) + CHECK(parse_frame(std::string_view(frame).substr(0, cut), 1 << 20).status == ParseStatus::NeedMore); + + const std::string two = client_frame(kText, "one") + client_frame(kText, "two"); + const auto first = parse_frame(two, 1 << 20); + REQUIRE(first.status == ParseStatus::Complete); + CHECK(first.frame.payload == "one"); + const auto second = parse_frame(std::string_view(two).substr(first.consumed), 1 << 20); + REQUIRE(second.status == ParseStatus::Complete); + CHECK(second.frame.payload == "two"); +} + +TEST_CASE("parse_frame reports fragments through fin and continuation", "[websocket]") +{ + const auto first = parse_frame(client_frame(kText, "hel", /*fin=*/false), 1 << 20); + REQUIRE(first.status == ParseStatus::Complete); + CHECK_FALSE(first.frame.fin); + CHECK(first.frame.opcode == kText); + const auto last = parse_frame(client_frame(kContinuation, "lo", /*fin=*/true), 1 << 20); + REQUIRE(last.status == ParseStatus::Complete); + CHECK(last.frame.fin); + CHECK(last.frame.opcode == kContinuation); +} + +TEST_CASE("parse_frame errors", "[websocket]") +{ + SECTION("an unmasked client frame is a protocol error") + { + const auto r = parse_frame(client_frame(kText, "hi", true, /*masked=*/false), 1 << 20); + CHECK(r.status == ParseStatus::Error); + CHECK(r.close_code == 1002); + } + SECTION("reserved bits") + { + std::string frame = client_frame(kText, "hi"); + frame[0] = static_cast(frame[0] | 0x40); + const auto r = parse_frame(frame, 1 << 20); + CHECK(r.status == ParseStatus::Error); + CHECK(r.close_code == 1002); + } + SECTION("an oversized payload is 1009, decided from the header alone") + { + const std::string frame = client_frame(kText, std::string(200, 'a')); + const auto r = parse_frame(std::string_view(frame).substr(0, 4), 100); + CHECK(r.status == ParseStatus::Error); + CHECK(r.close_code == 1009); + const auto exact = parse_frame(client_frame(kText, std::string(100, 'a')), 100); + CHECK(exact.status == ParseStatus::Complete); + } + SECTION("a 64-bit length beyond the limit is 1009 without waiting for the payload") + { + std::string header; + header.push_back(static_cast(0x81)); + header.push_back(static_cast(0x80 | 127)); + header.append(std::string("\x00\x00\x00\x01\x00\x00\x00\x00", 8)); + const auto r = parse_frame(header, 1 << 20); + CHECK(r.status == ParseStatus::Error); + CHECK(r.close_code == 1009); + } + SECTION("control frames may not be fragmented or longer than 125 bytes") + { + CHECK(parse_frame(client_frame(kPing, "x", /*fin=*/false), 1 << 20).status == ParseStatus::Error); + CHECK(parse_frame(client_frame(kClose, std::string(126, 'x')), 1 << 20).status == ParseStatus::Error); + CHECK(parse_frame(client_frame(kPing, std::string(125, 'x')), 1 << 20).status == ParseStatus::Complete); + } +} + +TEST_CASE("encode_frame produces unmasked server frames", "[websocket]") +{ + CHECK(encode_frame(kText, "Hello") == std::string("\x81\x05Hello")); + CHECK(encode_frame(kText, "a", /*fin=*/false) == std::string("\x01\x01" + "a")); + + const std::string mid = encode_frame(kText, std::string(126, 'a')); + CHECK(mid.size() == 4 + 126); + CHECK(static_cast(mid[1]) == 126); + CHECK(static_cast(mid[2]) == 0); + CHECK(static_cast(mid[3]) == 126); + + const std::string big = encode_frame(kBinary, std::string(70000, 'a')); + CHECK(big.size() == 10 + 70000); + CHECK(static_cast(big[1]) == 127); + CHECK(static_cast(big[7]) == 1); + CHECK(static_cast(big[8]) == 0x11); + CHECK(static_cast(big[9]) == 0x70); + + const std::string close = encode_close(1009, "big"); + CHECK(close == std::string("\x88\x05\x03\xf1" + "big", + 7)); + CHECK(encode_close(1000, std::string(500, 'r')).size() == 2 + 2 + 120); +} + +TEST_CASE("parse_handshake accepts a valid upgrade", "[websocket]") +{ + const auto h = parse_handshake(upgrade_request("/chan", "Authorization: Bearer s3cret\r\n")); + REQUIRE(h.ok); + CHECK(h.key == "dGhlIHNhbXBsZSBub25jZQ=="); + CHECK(h.token == "s3cret"); + CHECK(h.path == "/chan"); + + const std::string response = handshake_response(h); + CHECK(response.rfind("HTTP/1.1 101 Switching Protocols\r\n", 0) == 0); + CHECK(response.find("Sec-WebSocket-Accept: s3pPLMBiTxaQ9kYGzzhZRbK+xOo=\r\n") != std::string::npos); +} + +TEST_CASE("parse_handshake finds the token in the header or the query", "[websocket]") +{ + SECTION("header names and the Bearer scheme are case-insensitive") + { + const auto h = parse_handshake(upgrade_request("/", "AUTHORIZATION: bearer abc123\r\n")); + REQUIRE(h.ok); + CHECK(h.token == "abc123"); + } + SECTION("query parameter") + { + const auto h = parse_handshake(upgrade_request("/?x=1&token=qtok&y=2")); + REQUIRE(h.ok); + CHECK(h.token == "qtok"); + CHECK(h.path == "/"); + } + SECTION("the header wins over the query") + { + const auto h = parse_handshake(upgrade_request("/?token=q", "Authorization: Bearer h\r\n")); + REQUIRE(h.ok); + CHECK(h.token == "h"); + } + SECTION("no token leaves it empty and the handshake still parses") + { + const auto h = parse_handshake(upgrade_request()); + REQUIRE(h.ok); + CHECK(h.token.empty()); + } +} + +TEST_CASE("parse_handshake refuses what is not a WebSocket upgrade", "[websocket]") +{ + SECTION("a plain GET is 426 with an Upgrade header in the answer") + { + const auto h = parse_handshake("GET / HTTP/1.1\r\nHost: x\r\n\r\n"); + CHECK_FALSE(h.ok); + CHECK(h.status == 426); + const std::string response = handshake_response(h); + CHECK(response.rfind("HTTP/1.1 426 Upgrade Required\r\n", 0) == 0); + CHECK(response.find("Upgrade: websocket\r\n") != std::string::npos); + CHECK(response.find("Content-Length: " + std::to_string(h.reason.size())) != std::string::npos); + } + SECTION("a missing key is 426") + { + const auto h = parse_handshake( + "GET / HTTP/1.1\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n" + "Sec-WebSocket-Version: 13\r\n\r\n"); + CHECK(h.status == 426); + CHECK_FALSE(h.ok); + } + SECTION("a wrong version is 426 and says so") + { + const auto h = parse_handshake(upgrade_request("/", "", "8")); + CHECK_FALSE(h.ok); + CHECK(h.status == 426); + CHECK(h.reason.find("13") != std::string::npos); + } + SECTION("bad request lines are 400") + { + CHECK(parse_handshake("POST / HTTP/1.1\r\n\r\n").status == 400); + CHECK(parse_handshake("GET / HTTP/1.0\r\n\r\n").status == 400); + CHECK(parse_handshake("GET\r\n\r\n").status == 400); + CHECK(parse_handshake("garbage").status == 400); + CHECK_FALSE(parse_handshake("GET / HTTP/1.0\r\n\r\n").ok); + } + SECTION("status phrases") + { + Handshake h; + h.status = 401; + h.reason = "no"; + CHECK(handshake_response(h).rfind("HTTP/1.1 401 Unauthorized\r\n", 0) == 0); + h.status = 404; + CHECK(handshake_response(h).rfind("HTTP/1.1 404 Not Found\r\n", 0) == 0); + h.status = 400; + CHECK(handshake_response(h).rfind("HTTP/1.1 400 Bad Request\r\n", 0) == 0); + } +} diff --git a/src/sensing_control/tools/orin_control.py b/src/sensing_control/tools/orin_control.py new file mode 100755 index 0000000000..f313dfe3ca --- /dev/null +++ b/src/sensing_control/tools/orin_control.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Talk to a camera producer's management channel (camera_plugin_sensing or sensing_mock_producer). + + orin_control.py HOST[:PORT] --token-file token.txt status + orin_control.py HOST[:PORT] --token-file token.txt set-target sensing-left 192.168.1.205 5600 + orin_control.py HOST[:PORT] --token-file token.txt set-target sensing-left "" # stop the direct link + orin_control.py HOST[:PORT] --token-file token.txt watch # print pushed events + +Standard library only. The token goes in the Authorization header, never on the command line or in the URL. +""" + +from __future__ import annotations + +import argparse +import base64 +import hashlib +import json +import os +import socket +import struct +import sys +from pathlib import Path + +DEFAULT_PORT = 5610 +GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + + +class ChannelError(Exception): + """The channel could not be used; the message says why.""" + + +def recv_exact(sock: socket.socket, count: int) -> bytes: + data = b"" + while len(data) < count: + chunk = sock.recv(count - len(data)) + if not chunk: + raise ChannelError("the producer closed the connection") + data += chunk + return data + + +class Connection: + def __init__(self, host: str, port: int, token: str, timeout: float = 5.0) -> None: + try: + self._sock = socket.create_connection((host, port), timeout=timeout) + except OSError as exc: + raise ChannelError(f"cannot connect to {host}:{port}: {exc}") from exc + key = base64.b64encode(os.urandom(16)).decode() + request = ( + f"GET / HTTP/1.1\r\nHost: {host}:{port}\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n" + f"Sec-WebSocket-Key: {key}\r\nSec-WebSocket-Version: 13\r\nAuthorization: Bearer {token}\r\n\r\n" + ) + self._sock.sendall(request.encode()) + head = b"" + while b"\r\n\r\n" not in head: + chunk = self._sock.recv(4096) + if not chunk: + raise ChannelError( + "the producer closed the connection during the handshake" + ) + head += chunk + status_line = head.split(b"\r\n", 1)[0].decode(errors="replace") + if " 101 " not in status_line: + body = head.split(b"\r\n\r\n", 1)[1].decode(errors="replace").strip() + raise ChannelError( + f"the producer refused the connection ({status_line}): {body}" + ) + expected = base64.b64encode( + hashlib.sha1((key + GUID).encode()).digest() + ).decode() + if ( + f"sec-websocket-accept: {expected}".lower() + not in head.decode(errors="replace").lower() + ): + raise ChannelError( + "the server's handshake answer is wrong; this is not a management channel" + ) + self._pending = head.split(b"\r\n\r\n", 1)[1] + + def send(self, message: dict) -> None: + payload = json.dumps(message).encode() + mask = os.urandom(4) + header = bytes([0x81]) + if len(payload) < 126: + header += bytes([0x80 | len(payload)]) + elif len(payload) < 65536: + header += bytes([0x80 | 126]) + struct.pack(">H", len(payload)) + else: + header += bytes([0x80 | 127]) + struct.pack(">Q", len(payload)) + masked = bytes(b ^ mask[i % 4] for i, b in enumerate(payload)) + self._sock.sendall(header + mask + masked) + + def _read(self, count: int) -> bytes: + take = self._pending[:count] + self._pending = self._pending[count:] + return take + ( + recv_exact(self._sock, count - len(take)) if len(take) < count else b"" + ) + + def receive(self, timeout: float | None = None) -> dict: + self._sock.settimeout(timeout) + while True: + first, second = self._read(2) + opcode, length = first & 0x0F, second & 0x7F + if length == 126: + (length,) = struct.unpack(">H", self._read(2)) + elif length == 127: + (length,) = struct.unpack(">Q", self._read(8)) + payload = self._read(length) + if opcode == 0x8: + raise ChannelError( + "the producer closed the channel: " + + payload[2:].decode(errors="replace") + ) + if opcode == 0x1: + return json.loads(payload) + # Pings and other frames are ignored; the server sends none that need an answer. + + def request(self, message: dict) -> dict: + message = {"id": 1, **message} + self.send(message) + while True: + reply = self.receive(timeout=10) + if reply.get("id") == message["id"]: + return reply + + +def describe(status: dict) -> str: + lines = [ + f"state: {status['state']} release: {status['release']} up {status['uptime_seconds']} s" + ] + fault = status.get("fault") + if fault: + lines.append(f"PROBLEM [{fault['code']}]: {fault['message']}") + for stream in status["streams"]: + direct = stream["direct"] + target = f"{direct['host']}:{direct['port']}" if direct["host"] else "off" + link = "connected" if direct["connected"] else "not connected" + lines.append( + f" {stream['stream']}: sensor {stream['sensor']}, {stream['frames']} frames, " + f"direct link {target} ({link if direct['host'] else '-'})" + ) + return "\n".join(lines) + + +def main(argv: list[str]) -> int: + ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + ap.add_argument( + "target", help=f"producer HOST or HOST:PORT (default port {DEFAULT_PORT})" + ) + ap.add_argument( + "--token-file", required=True, type=Path, help="file holding the shared secret" + ) + sub = ap.add_subparsers(dest="command", required=True) + sub.add_parser("status", help="print the producer's state") + sub.add_parser("watch", help="print pushed events until interrupted") + setter = sub.add_parser( + "set-target", + help="send a stream's video to HOST PORT (empty HOST turns it off)", + ) + setter.add_argument("stream") + setter.add_argument("host") + setter.add_argument("port", nargs="?", type=int, default=0) + args = ap.parse_args(argv) + + host, _, port = args.target.partition(":") + try: + token = args.token_file.read_text().strip() + if not token: + raise ChannelError(f"{args.token_file} is empty") + conn = Connection(host, int(port) if port else DEFAULT_PORT, token) + if args.command == "status": + reply = conn.request({"type": "get_status"}) + elif args.command == "set-target": + reply = conn.request( + { + "type": "set_direct_target", + "stream": args.stream, + "host": args.host, + "port": args.port, + } + ) + else: + print(describe(conn.request({"type": "get_status"})["status"])) + while True: + print(json.dumps(conn.receive())) + if not reply["ok"]: + print( + f"Error [{reply['error']['code']}]: {reply['error']['message']}", + file=sys.stderr, + ) + return 1 + print(describe(reply["status"])) + return 0 + except (ChannelError, OSError, ValueError) as exc: + print(f"Error: {exc}", file=sys.stderr) + return 1 + except KeyboardInterrupt: + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv[1:])) diff --git a/src/sensing_control/websocket.cpp b/src/sensing_control/websocket.cpp new file mode 100644 index 0000000000..efb77d09c4 --- /dev/null +++ b/src/sensing_control/websocket.cpp @@ -0,0 +1,369 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/sensing_control/websocket.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ +namespace websocket +{ + +namespace +{ + +constexpr std::string_view kGuid = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"; + +std::uint32_t rotl(std::uint32_t value, int bits) +{ + return (value << bits) | (value >> (32 - bits)); +} + +std::string lower(std::string_view text) +{ + std::string out(text); + std::transform(out.begin(), out.end(), out.begin(), [](unsigned char c) { return std::tolower(c); }); + return out; +} + +std::string trim(std::string_view text) +{ + std::size_t begin = 0; + std::size_t end = text.size(); + while (begin < end && (text[begin] == ' ' || text[begin] == '\t')) + ++begin; + while (end > begin && (text[end - 1] == ' ' || text[end - 1] == '\t' || text[end - 1] == '\r')) + --end; + return std::string(text.substr(begin, end - begin)); +} + +/// The value of one `name=value` pair of a query string, percent-decoding nothing: tokens are URL-safe. +std::string query_value(std::string_view query, std::string_view name) +{ + std::size_t pos = 0; + while (pos <= query.size()) + { + const std::size_t amp = query.find('&', pos); + const std::string_view pair = query.substr(pos, amp == std::string_view::npos ? amp : amp - pos); + const std::size_t eq = pair.find('='); + if (eq != std::string_view::npos && pair.substr(0, eq) == name) + return std::string(pair.substr(eq + 1)); + if (amp == std::string_view::npos) + break; + pos = amp + 1; + } + return {}; +} + +} // namespace + +std::string sha1(std::string_view data) +{ + std::array h = { 0x67452301, 0xEFCDAB89, 0x98BADCFE, 0x10325476, 0xC3D2E1F0 }; + + std::string message(data); + const std::uint64_t bit_length = static_cast(data.size()) * 8; + message.push_back(static_cast(0x80)); + while (message.size() % 64 != 56) + message.push_back('\0'); + for (int shift = 56; shift >= 0; shift -= 8) + message.push_back(static_cast((bit_length >> shift) & 0xFF)); + + for (std::size_t offset = 0; offset < message.size(); offset += 64) + { + std::array w{}; + for (int i = 0; i < 16; ++i) + { + const auto* p = reinterpret_cast(message.data() + offset + 4 * i); + w[i] = (std::uint32_t(p[0]) << 24) | (std::uint32_t(p[1]) << 16) | (std::uint32_t(p[2]) << 8) | p[3]; + } + for (int i = 16; i < 80; ++i) + w[i] = rotl(w[i - 3] ^ w[i - 8] ^ w[i - 14] ^ w[i - 16], 1); + + std::uint32_t a = h[0], b = h[1], c = h[2], d = h[3], e = h[4]; + for (int i = 0; i < 80; ++i) + { + std::uint32_t f = 0; + std::uint32_t k = 0; + if (i < 20) + { + f = (b & c) | (~b & d); + k = 0x5A827999; + } + else if (i < 40) + { + f = b ^ c ^ d; + k = 0x6ED9EBA1; + } + else if (i < 60) + { + f = (b & c) | (b & d) | (c & d); + k = 0x8F1BBCDC; + } + else + { + f = b ^ c ^ d; + k = 0xCA62C1D6; + } + const std::uint32_t temp = rotl(a, 5) + f + e + k + w[i]; + e = d; + d = c; + c = rotl(b, 30); + b = a; + a = temp; + } + h[0] += a; + h[1] += b; + h[2] += c; + h[3] += d; + h[4] += e; + } + + std::string digest; + for (const std::uint32_t word : h) + for (int shift = 24; shift >= 0; shift -= 8) + digest.push_back(static_cast((word >> shift) & 0xFF)); + return digest; +} + +std::string base64_encode(std::string_view data) +{ + static constexpr char kAlphabet[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + std::string out; + out.reserve((data.size() + 2) / 3 * 4); + for (std::size_t i = 0; i < data.size(); i += 3) + { + const std::uint32_t b0 = static_cast(data[i]); + const std::uint32_t b1 = i + 1 < data.size() ? static_cast(data[i + 1]) : 0; + const std::uint32_t b2 = i + 2 < data.size() ? static_cast(data[i + 2]) : 0; + const std::uint32_t triple = (b0 << 16) | (b1 << 8) | b2; + out.push_back(kAlphabet[(triple >> 18) & 0x3F]); + out.push_back(kAlphabet[(triple >> 12) & 0x3F]); + out.push_back(i + 1 < data.size() ? kAlphabet[(triple >> 6) & 0x3F] : '='); + out.push_back(i + 2 < data.size() ? kAlphabet[triple & 0x3F] : '='); + } + return out; +} + +std::string accept_key(std::string_view client_key) +{ + std::string joined(client_key); + joined.append(kGuid); + return base64_encode(sha1(joined)); +} + +bool constant_time_equal(std::string_view a, std::string_view b) +{ + // Length is not secret here: it is compared first, and only the content is hidden. + if (a.size() != b.size()) + return false; + unsigned char difference = 0; + for (std::size_t i = 0; i < a.size(); ++i) + difference |= static_cast(a[i]) ^ static_cast(b[i]); + return difference == 0; +} + +Handshake parse_handshake(std::string_view head) +{ + Handshake result; + const std::size_t line_end = head.find("\r\n"); + if (line_end == std::string_view::npos) + { + result.reason = "malformed request"; + return result; + } + const std::string_view request_line = head.substr(0, line_end); + // "GET HTTP/1.1" + const std::size_t first_space = request_line.find(' '); + const std::size_t second_space = request_line.rfind(' '); + if (first_space == std::string_view::npos || second_space == first_space || + request_line.substr(0, first_space) != "GET" || request_line.substr(second_space + 1) != "HTTP/1.1") + { + result.reason = "expected a GET request over HTTP/1.1"; + return result; + } + const std::string_view target = request_line.substr(first_space + 1, second_space - first_space - 1); + const std::size_t question = target.find('?'); + result.path = std::string(target.substr(0, question)); + const std::string_view query = question == std::string_view::npos ? std::string_view() : target.substr(question + 1); + + bool upgrade = false; + bool connection_upgrade = false; + std::string version; + std::string bearer; + std::size_t pos = line_end + 2; + while (pos < head.size()) + { + const std::size_t end = head.find("\r\n", pos); + const std::string_view line = head.substr(pos, end == std::string_view::npos ? end : end - pos); + if (line.empty()) + break; + const std::size_t colon = line.find(':'); + if (colon != std::string_view::npos) + { + const std::string name = lower(trim(line.substr(0, colon))); + const std::string value = trim(line.substr(colon + 1)); + if (name == "upgrade") + upgrade = lower(value) == "websocket"; + else if (name == "connection") + connection_upgrade = lower(value).find("upgrade") != std::string::npos; + else if (name == "sec-websocket-key") + result.key = value; + else if (name == "sec-websocket-version") + version = value; + else if (name == "authorization" && lower(value).rfind("bearer ", 0) == 0) + bearer = trim(std::string_view(value).substr(7)); + } + if (end == std::string_view::npos) + break; + pos = end + 2; + } + + if (!upgrade || !connection_upgrade || result.key.empty()) + { + result.status = 426; + result.reason = "this is a WebSocket endpoint"; + return result; + } + if (version != "13") + { + result.status = 426; + result.reason = "only WebSocket version 13 is supported"; + return result; + } + result.token = bearer.empty() ? query_value(query, "token") : bearer; + result.ok = true; + return result; +} + +std::string handshake_response(const Handshake& handshake) +{ + if (handshake.ok) + { + return "HTTP/1.1 101 Switching Protocols\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n" + "Sec-WebSocket-Accept: " + + accept_key(handshake.key) + "\r\n\r\n"; + } + const char* phrase = handshake.status == 401 ? "Unauthorized" : + handshake.status == 404 ? "Not Found" : + handshake.status == 426 ? "Upgrade Required" : + "Bad Request"; + std::string response = "HTTP/1.1 " + std::to_string(handshake.status) + " " + phrase + "\r\n"; + if (handshake.status == 426) + response += "Upgrade: websocket\r\n"; + response += "Content-Type: text/plain\r\nContent-Length: " + std::to_string(handshake.reason.size()) + + "\r\nConnection: close\r\n\r\n" + handshake.reason; + return response; +} + +ParseResult parse_frame(std::string_view buffer, std::size_t max_payload) +{ + ParseResult result; + if (buffer.size() < 2) + return result; + const auto byte = [&](std::size_t i) { return static_cast(buffer[i]); }; + + const unsigned char b0 = byte(0); + const unsigned char b1 = byte(1); + if ((b0 & 0x70) != 0) + { + result.status = ParseStatus::Error; + result.error = "reserved bits set"; + return result; + } + if ((b1 & 0x80) == 0) + { + result.status = ParseStatus::Error; + result.error = "client frames must be masked"; + return result; + } + const std::uint8_t opcode = b0 & 0x0F; + const bool control = (opcode & 0x08) != 0; + + std::uint64_t length = b1 & 0x7F; + std::size_t header = 2; + if (length == 126) + { + if (buffer.size() < 4) + return result; + length = (std::uint64_t(byte(2)) << 8) | byte(3); + header = 4; + } + else if (length == 127) + { + if (buffer.size() < 10) + return result; + length = 0; + for (int i = 0; i < 8; ++i) + length = (length << 8) | byte(2 + i); + header = 10; + } + if (control && (length > 125 || (b0 & 0x80) == 0)) + { + result.status = ParseStatus::Error; + result.error = "invalid control frame"; + return result; + } + if (length > max_payload) + { + result.status = ParseStatus::Error; + result.close_code = 1009; + result.error = "message too big"; + return result; + } + const std::size_t total = header + 4 + static_cast(length); + if (buffer.size() < total) + return result; + + const unsigned char mask[4] = { byte(header), byte(header + 1), byte(header + 2), byte(header + 3) }; + result.frame.fin = (b0 & 0x80) != 0; + result.frame.opcode = opcode; + result.frame.payload.resize(static_cast(length)); + for (std::size_t i = 0; i < length; ++i) + result.frame.payload[i] = static_cast(byte(header + 4 + i) ^ mask[i % 4]); + result.consumed = total; + result.status = ParseStatus::Complete; + return result; +} + +std::string encode_frame(std::uint8_t opcode, std::string_view payload, bool fin) +{ + std::string out; + out.push_back(static_cast((fin ? 0x80 : 0x00) | (opcode & 0x0F))); + if (payload.size() < 126) + { + out.push_back(static_cast(payload.size())); + } + else if (payload.size() <= 0xFFFF) + { + out.push_back(126); + out.push_back(static_cast(payload.size() >> 8)); + out.push_back(static_cast(payload.size() & 0xFF)); + } + else + { + out.push_back(127); + for (int shift = 56; shift >= 0; shift -= 8) + out.push_back(static_cast((static_cast(payload.size()) >> shift) & 0xFF)); + } + out.append(payload); + return out; +} + +std::string encode_close(std::uint16_t code, std::string_view reason) +{ + std::string payload; + payload.push_back(static_cast(code >> 8)); + payload.push_back(static_cast(code & 0xFF)); + payload.append(reason.substr(0, 120)); + return encode_frame(kClose, payload); +} + +} // namespace websocket +} // namespace sensing +} // namespace plugins diff --git a/src/video_stage/CMakeLists.txt b/src/video_stage/CMakeLists.txt new file mode 100644 index 0000000000..1315f9fe27 --- /dev/null +++ b/src/video_stage/CMakeLists.txt @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# ============================================================================== +# Video stage: GPU processing of camera video on the PC (NVDEC -> GPU operation -> NVENC) +# ============================================================================== +# core/ video_stage_core -- the GPU operation interface and the example sine-wave warp +# tests/ video_stage_tests -- Catch2 tests; need a CUDA-capable GPU +# +# Configure with: +# cmake -B build -DBUILD_VIDEO_STAGE=ON +# ============================================================================== + +enable_language(CUDA) + +if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + # Real code for the common desktop architectures plus PTX for 90, which newer GPUs JIT-compile. + set(CMAKE_CUDA_ARCHITECTURES "80-real;86-real;89-real;90") +endif() + +find_package(CUDAToolkit REQUIRED) + +# NVDEC/NVENC API headers (MIT licence) with run-time loaders for libnvcuvid and libnvidia-encode, pinned by hash. +include(FetchContent) +FetchContent_Declare( + nv_codec_headers + URL https://github.com/FFmpeg/nv-codec-headers/archive/refs/tags/n12.2.72.0.tar.gz + URL_HASH SHA256=dbeaec433d93b850714760282f1d0992b1254fc3b5a6cb7d76fc1340a1e47563 +) +FetchContent_GetProperties(nv_codec_headers) +if(NOT nv_codec_headers_POPULATED) + FetchContent_Populate(nv_codec_headers) +endif() +add_library(video_stage_nv_codec_headers INTERFACE) +target_include_directories(video_stage_nv_codec_headers SYSTEM INTERFACE "${nv_codec_headers_SOURCE_DIR}/include") + +add_subdirectory(core) +add_subdirectory(wire) +add_subdirectory(sender) +add_subdirectory(codec) +add_subdirectory(app) + +if(BUILD_TESTING) + add_subdirectory(tests) + add_subdirectory(wire/tests) +endif() diff --git a/src/video_stage/README.md b/src/video_stage/README.md new file mode 100644 index 0000000000..ade1811f8e --- /dev/null +++ b/src/video_stage/README.md @@ -0,0 +1,112 @@ + + +# Video stage + +GPU processing of camera video on the PC: decoded frames stay in GPU memory from the hardware decoder, through one +GPU operation, to the hardware encoder. This directory holds the operation interface and the example operation. The +decoder, encoder and transport sit beside it and do not change when the operation is replaced. + +## The operation contract + +`core/inc/video_stage/gpu_operation.hpp` defines `GpuOperation`: NV12 in, NV12 out, fixed geometry. + +- `prepare(input_geometry)` runs once, outside the frame loop. Build and upload anything per-geometry here (a + rectification map, for example). Nothing in `enqueue` allocates or uploads. +- `enqueue(in, out, stream)` only queues kernels on `stream` and returns. It does not synchronise the device or the + stream. After it returns, the caller records a CUDA event on `stream`; whoever reads `out` waits on that event. +- `in` and `out` are different surfaces (the operation is not in-place). `out` has `output_geometry(in.geometry)` and + keeps the input's color description. Both are `Nv12View`s: device pointers, byte pitches, size. +- Frame identity and timestamps are not the operation's concern; they travel with the frame. + +## The example: `SineWarp` + +`core/sine_warp.cu` is a horizontal sine ripple, `source_x = x + A*sin(2*pi*y/wavelength + phase)`, +`source_y = y`, sampled bilinearly, with the border fill for the declared range (Y=16 limited, 0 full; chroma 128). +Amplitude 0 is an identity. It is an example, not calibrated rectification: the warped picture does not keep the +source camera's projection. + +The stage assumes the decoder's NV12 uses the H.264 default chroma siting described below; a stream that signals a +different siting must be rejected at the decoder, not warped with the wrong sample positions. + +Chroma is not sampled at the luma coordinate divided by two. A chroma sample of H.264 NV12 sits on an even luma column +and half way between two luma rows, so the kernel maps that luma-space position, then converts back to chroma-plane +coordinates. Keep that when substituting an operation. + +## Substituting the fisheye rectification + +Replace the mapping, keep the rest: a class deriving from `GpuOperation` whose `prepare` generates the destination-to- +source maps once (on the CPU, uploaded once) and whose `enqueue` launches a kernel that reads `map_x/map_y` instead of +computing the sine. Sample luma at the map position and chroma at the position derived from the same map with the +siting above. `output_geometry` returns the rectified output size. Nothing else in the stage changes. + +## The worker and the data path + +```text +Orin producer --DATA (AVS1 records, H.264)--> PC worker: NVDEC -> GpuOperation -> NVENC --> LiveKit track + <--CTRL (keyframe requests, clock exchange)-- +``` + +By default the worker does no processing: `--operation=none` forwards the Orin's encoded access units to LiveKit untouched, +with no decode, encode or GPU, for the lowest latency and no quality loss. `--operation=sine-warp` turns on the GPU +stage below (decode, the example warp, re-encode); it is a test of the stage and the template for the rectification. + +`video_stage_worker` serves one Orin camera per `--stream=PORT:TRACK`; all tracks are published under one LiveKit +participant, as the Orin producer does for its cameras. The recorder and the web page subscribe to the processed tracks +exactly as they subscribe to the Orin's own, so they are unchanged. + +- **Direct input.** The Orin producer connects to each stream's port (`--add-stream=sensor=0,livekit=NAME,direct=HOST:PORT`) + and sends every encoded access unit as an AVS1 record over TCP (`wire/`; codec 2 is H.264). It reconnects by itself, + starts at a keyframe, and drops frames up to the next keyframe rather than block when the link cannot keep up. Use a + wired link. +- **Control link.** A second connection from the Orin carries the PC's keyframe requests and a four-timestamp clock + exchange (the same bytes the recorder uses over LiveKit). The worker converts each Orin frame-ready stamp onto its own + monotonic clock before publishing, and answers the recorder's and the page's clock sync with that clock, so they map one + clock. Video is never withheld because of timing: while the Orin-to-PC mapping is missing or less certain than + `--max-upstream-uncertainty-ms` (5 by default) the frames are still published, without a capture timestamp, so the page keeps + its last picture and shows no latency figure, and a recording that needs a timestamp stops and says so. When the Orin's + clock jumps, the track is unpublished and published again at the next keyframe. The uncertainty the recorder and the page + report covers only the worker-to-them leg: it excludes the Orin-to-PC mapping, which this budget bounds. +- **Fixed rate.** The worker publishes what its source encodes (or what it encodes itself in GPU mode) at that rate; a + downstream bitrate request is logged once and not acted on. The Orin producer adapts its own bitrate to its direct link. +- **Fixed geometry.** A stream's picture size is fixed for the run; a change ends the stream with an error. +- **Identity.** Each picture carries a key through NVDEC and NVENC, and the source frame number and stamp are looked up by + that key, so metadata follows its own frame through loss. A stage that cannot keep up discards its queued records and + resumes at the next keyframe; it never drops compressed frames one by one. + +Declare the processed tracks like any other camera in the CaptureServer streams file, with the worker's participant as the +publisher and the monotonic basis the worker publishes: + +```yaml +livekit: + url: ws://PC_HOST:7880 + room: sensing-preview + publisher: stage-worker # the identity in the worker's token + timestamp_basis: monotonic_us + streams: + - {name: Head left, track: processed-left, width: 2560, height: 1984, fps: 30} + - {name: Head right, track: processed-right, width: 2560, height: 1984, fps: 30} +``` + +```bash +video_stage_worker --stream=5600:processed-left --stream=5601:processed-right \ + --livekit-url=ws://127.0.0.1:7880 --livekit-token-file=worker.token +# a file sink instead of LiveKit, to check a run: +video_stage_worker --stream=5600:file=out.h264 --once --operation=sine-warp +``` + +`tests/tools/mock_orin_sender.py` stands in for the Orin: it mirrors the producer's direct sender (connection and hello order, +retry, keyframe start, bounded queue with drop-to-keyframe, keyframe requests, clock answers on its own offset clock, source +frame ids that step by 2) and feeds it Jetson-style H.264 (no AUDs, IDR every 5 s, constant bitrate). One `--port` per camera. + +## Build + +```bash +cmake -B build -DBUILD_VIDEO_STAGE=ON -DBUILD_PLUGINS=OFF -DBUILD_VIZ=OFF -DBUILD_EXAMPLES=OFF \ + -DBUILD_PYTHON_BINDINGS=OFF +cmake --build build --target video_stage_tests && ctest --test-dir build -R video_stage +``` + +The tests need a CUDA GPU. diff --git a/src/video_stage/app/CMakeLists.txt b/src/video_stage/app/CMakeLists.txt new file mode 100644 index 0000000000..801a8389ab --- /dev/null +++ b/src/video_stage/app/CMakeLists.txt @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The PC worker: receives AVS1 access units from the Orin, decodes on NVDEC, applies the GPU operation, re-encodes on +# NVENC and hands the access units to a sink. +add_executable(video_stage_worker + main.cpp + control_channel.cpp + livekit_output.cpp + pipeline.cpp + stream_worker.cpp + tcp_receiver.cpp +) + +target_link_libraries(video_stage_worker + PRIVATE + video_stage::core + video_stage::codec + video_stage::wire + sensing::livekit +) diff --git a/src/video_stage/app/control_channel.cpp b/src/video_stage/app/control_channel.cpp new file mode 100644 index 0000000000..fa0034712f --- /dev/null +++ b/src/video_stage/app/control_channel.cpp @@ -0,0 +1,114 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "control_channel.hpp" + +#include "pipeline.hpp" + +#include + +#include +#include + +namespace video_stage +{ + +namespace cs = plugins::sensing; + +ControlChannel::ControlChannel(TcpReceiver& link, uint64_t first_epoch) : m_link(link), m_epoch(first_epoch) +{ + m_reader = std::thread(&ControlChannel::read_loop, this); + m_requester = std::thread(&ControlChannel::request_loop, this); +} + +ControlChannel::~ControlChannel() +{ + { + std::lock_guard lock(m_wait_mutex); + m_stop = true; + } + m_wait.notify_all(); + m_link.shutdown_session(); // unblocks the reader + if (m_requester.joinable()) + m_requester.join(); + if (m_reader.joinable()) + m_reader.join(); +} + +std::optional ControlChannel::mapping() const +{ + std::lock_guard lock(m_mutex); + const auto mapping = m_estimator.mapping(monotonic_ns()); + if (!mapping) + return std::nullopt; + return UpstreamMapping{ *mapping, m_epoch }; +} + +void ControlChannel::request_loop() +{ + uint32_t sequence = 0; + while (true) + { + cs::ClockSyncRequest request; + request.sequence = ++sequence; + request.t1 = monotonic_ns(); + { + std::lock_guard lock(m_mutex); + m_outstanding[m_next_outstanding] = { request.sequence, request.t1, false }; + m_next_outstanding = (m_next_outstanding + 1) % kOutstanding; + } + if (!m_link.send_control(cs::build_clock_sync_request(request).data(), cs::kClockSyncRequestSize)) + return; + std::unique_lock lock(m_wait_mutex); + if (m_wait.wait_for(lock, std::chrono::seconds(1), [this] { return m_stop; })) + return; + } +} + +void ControlChannel::read_loop() +{ + while (true) + { + uint8_t first = 0; + if (!m_link.recv_control(&first, 1)) + return; + const int64_t t4 = monotonic_ns(); // stamped before anything else runs + if (first != 'R') + continue; // the Orin sends nothing else; ignore rather than guess + std::vector reply(cs::kClockSyncReplySize); + reply[0] = first; + if (!m_link.recv_control(reply.data() + 1, reply.size() - 1)) + return; + const auto parsed = cs::parse_clock_sync_reply(reply); + if (!parsed) + continue; + const Exchange exchange{ parsed->t1, parsed->t2, parsed->t3, t4 }; + std::lock_guard lock(m_mutex); + Outstanding* match = nullptr; + for (Outstanding& o : m_outstanding) + { + if (!o.used && o.sequence == parsed->sequence && o.t1 == parsed->t1) + match = &o; + } + if (match == nullptr) + { + ++m_rejected; // not the answer to a request of this session + continue; + } + match->used = true; + if (!m_estimator.add(exchange).empty()) + { + ++m_rejected; + continue; + } + ++m_exchanges; + if (m_estimator.discontinuity()) + { + ++m_discontinuities; + ++m_epoch; + m_estimator.clear_discontinuity(); + } + } +} + +} // namespace video_stage diff --git a/src/video_stage/app/control_channel.hpp b/src/video_stage/app/control_channel.hpp new file mode 100644 index 0000000000..0a6d7d821f --- /dev/null +++ b/src/video_stage/app/control_channel.hpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tcp_receiver.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// The Orin-to-PC mapping and the epoch it belongs to. The epoch increases whenever the Orin's clock was found to have +/// jumped relative to this one; frames mapped under different epochs must not be part of one synchronized take. +struct UpstreamMapping +{ + ClockMapping mapping; + uint64_t epoch = 0; +}; + +/** + * @brief The PC's half of the Orin clock exchange, over the control connection. + * + * Once a second it sends a four-timestamp request (the same wire the recorder and the browser use over LiveKit) and + * feeds each reply to a ClockEstimator, so the Orin's monotonic stamps can be placed on this machine's monotonic clock. + * A reader thread stamps t4 the instant a reply's first byte arrives. On a discontinuity the estimator restarts from + * the new exchange; frames mapped in the interval before it was noticed carry the old offset. + */ +class ControlChannel +{ +public: + /// `first_epoch` differs for every session of a stream, so an epoch is never reused across a sender restart. + ControlChannel(TcpReceiver& link, uint64_t first_epoch); + ~ControlChannel(); + + ControlChannel(const ControlChannel&) = delete; + ControlChannel& operator=(const ControlChannel&) = delete; + + /// The current Orin-to-PC mapping with its uncertainty aged to now; nullopt before the first reply or when stale. + std::optional mapping() const; + + uint64_t exchanges() const + { + return m_exchanges.load(); + } + uint64_t rejected() const + { + return m_rejected.load(); + } + uint64_t discontinuities() const + { + return m_discontinuities.load(); + } + +private: + void request_loop(); + void read_loop(); + + TcpReceiver& m_link; + mutable std::mutex m_mutex; + ClockEstimator m_estimator; + std::atomic m_exchanges{ 0 }; + std::atomic m_rejected{ 0 }; + std::atomic m_discontinuities{ 0 }; + // Requests sent and not yet answered (sequence, t1): a reply is used only if it answers one of these. + struct Outstanding + { + uint32_t sequence = 0; + int64_t t1 = 0; + bool used = true; + }; + static constexpr size_t kOutstanding = 4; + Outstanding m_outstanding[kOutstanding]{}; + size_t m_next_outstanding = 0; + uint64_t m_epoch; + std::mutex m_wait_mutex; + std::condition_variable m_wait; + bool m_stop = false; + std::thread m_requester; + std::thread m_reader; +}; + +} // namespace video_stage diff --git a/src/video_stage/app/livekit_output.cpp b/src/video_stage/app/livekit_output.cpp new file mode 100644 index 0000000000..24c2cdc293 --- /dev/null +++ b/src/video_stage/app/livekit_output.cpp @@ -0,0 +1,163 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "livekit_output.hpp" + +#include +#include + +namespace video_stage +{ + +namespace sensing = plugins::sensing; + +SharedLiveKit::SharedLiveKit(const std::string& url, const std::string& token) +{ + m_session = sensing::LiveKitSession::connect(url, token); +} + +std::unique_ptr SharedLiveKit::publish(const sensing::PublishSpec& spec) +{ + std::lock_guard lock(m_mutex); + return m_session->publish_video(spec); +} + +bool SharedLiveKit::failed() const +{ + return m_session->failed(); +} + +std::string SharedLiveKit::failure() const +{ + return m_session->failure(); +} + +LiveKitOutput::LiveKitOutput(SharedLiveKit& livekit, + const LiveKitOutputConfig& config, + MappingSource mapping, + std::function request_idr) + : m_livekit(livekit), m_config(config), m_mapping(std::move(mapping)), m_request_idr(std::move(request_idr)) +{ +} + +void LiveKitOutput::break_publication() +{ + m_track.reset(); // destroying the sink unpublishes the track + m_awaiting_keyframe = true; + m_idr_requested_for_gap = false; +} + +void LiveKitOutput::end_session() +{ + break_publication(); +} + +void LiveKitOutput::write(const EncodedPacket& packet, const FrameMeta& meta, const Geometry& geometry) +{ + const int64_t write_start_ns = monotonic_ns(); + if (m_livekit.failed()) + throw std::runtime_error("LiveKit room lost: " + m_livekit.failure()); + + // A subscriber's keyframe or rate request reaches the encoder through the pipeline. + if (m_track) + { + const auto feedback = m_track->take_feedback(); + if (feedback.keyframe_requested) + m_request_idr(); + // This worker runs at the rate its source encodes; it does not adapt it to downstream congestion. Say so once + // instead of silently discarding the request. + if (feedback.target_bitrate_bps && !m_rate_feedback_noted) + { + m_rate_feedback_noted = true; + std::cerr << "stream " << m_config.track_name << ": downstream asked for " + << *feedback.target_bitrate_bps / 1000 + << " kbit/s; this worker forwards its source's rate and does not adapt it" << std::endl; + } + } + + // Video keeps flowing whatever the timing quality: a viewer that loses the picture every time the link's clock + // mapping wobbles is worse than one that sees video without a latency figure. What a frame may claim about time + // depends on the mapping: a qualified one carries the Orin's capture time on this PC's clock; otherwise the frame + // carries no timestamp, and a consumer that needs one (a recording) says so instead of using a guessed time. + const auto upstream = m_mapping(); + bool qualified = false; + if (meta.source_timestamp_ns == 0) + ++m_no_source_stamp; // the sender could not tie this unit to a capture: no capture time to map + else if (!upstream) + ++m_unmapped; + else if (upstream->mapping.uncertainty_ns > m_config.max_upstream_uncertainty_ns) + ++m_over_budget; + else + qualified = true; + + // This milestone publishes one fixed picture size: a change is refused rather than reconfiguring the track. + if (m_track && !(geometry == m_track_geometry)) + throw std::runtime_error("the stream's picture size changed; publishing a different size is not supported"); + // The Orin's clock jumped: end this publication so consumers see a break instead of a silent offset change. + if (upstream) + { + if (m_track && upstream->epoch != m_epoch) + { + ++m_republished; + break_publication(); + } + m_epoch = upstream->epoch; + } + + if (m_awaiting_keyframe && !packet.keyframe) + { + // Resume at a keyframe: ask for one once per gap instead of waiting out the encoder's own interval. + if (!m_idr_requested_for_gap) + { + m_idr_requested_for_gap = true; + m_request_idr(); + } + return; + } + if (!m_track) + { + sensing::PublishSpec spec; + spec.name = m_config.track_name; + spec.width = geometry.width; + spec.height = geometry.height; + spec.max_bitrate_bps = m_config.max_bitrate_bps; + spec.max_fps = m_config.fps; + m_track = m_livekit.publish(spec); + m_track_geometry = geometry; + } + m_awaiting_keyframe = false; + + // Without a qualified mapping the transport timestamp is the arrival time here and the frame has no capture time. + const int64_t pc_ns = + qualified ? upstream->mapping.to_local_ns(static_cast(meta.source_timestamp_ns)) : meta.received_ns; + sensing::EncodedVideoFrame frame; + frame.data = packet.data.data(); + frame.size = packet.data.size(); + frame.keyframe = packet.keyframe; + frame.timestamp_us = static_cast(pc_ns / 1000); + frame.user_timestamp_us = qualified ? frame.timestamp_us : 0; + frame.frame_id = static_cast(meta.frame_number); + const int64_t submit_start_ns = monotonic_ns(); + const bool accepted = m_track->submit(frame); + const int64_t now_ns = monotonic_ns(); + if (accepted) + { + ++m_published; + m_intake_to_write_ns.add(write_start_ns - meta.received_ns); + m_submit_ns.add(now_ns - submit_start_ns); + if (qualified) + { + m_uplink_ns.add(meta.received_ns - pc_ns); + m_source_to_publish_ns.add(now_ns - pc_ns); + m_upstream_uncertainty_ns = upstream->mapping.uncertainty_ns; + } + return; + } + // The SDK refused the frame, which can break the reference chain: continue from a keyframe. + ++m_refused; + m_awaiting_keyframe = true; + m_idr_requested_for_gap = true; + m_request_idr(); +} + +} // namespace video_stage diff --git a/src/video_stage/app/livekit_output.hpp b/src/video_stage/app/livekit_output.hpp new file mode 100644 index 0000000000..725359e2cd --- /dev/null +++ b/src/video_stage/app/livekit_output.hpp @@ -0,0 +1,157 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "control_channel.hpp" +#include "pipeline.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// One LiveKit participant shared by every stream of the worker, as the Orin producer shares one for its cameras: one +/// identity, one room, one track per stream. Publishing is serialised because streams run on their own threads. +class SharedLiveKit +{ +public: + SharedLiveKit(const std::string& url, const std::string& token); + + std::unique_ptr publish(const plugins::sensing::PublishSpec& spec); + bool failed() const; + std::string failure() const; + +private: + plugins::sensing::LiveKitSdkReference m_sdk; + std::unique_ptr m_session; + std::mutex m_mutex; +}; + +struct LiveKitOutputConfig +{ + std::string track_name; + uint64_t max_bitrate_bps = 12'000'000; + double fps = 30.0; + /// Frames are withheld while the Orin-to-PC mapping is less certain than this. The recorder's and the page's own + /// uncertainty covers only the leg from this worker to them; this leg is on top of it. + int64_t max_upstream_uncertainty_ns = 5'000'000; +}; + +/** + * @brief Publishes the processed access units as an ordinary LiveKit camera track. + * + * The recorder and the web page subscribe to it exactly as they do to the Orin's own track, so it follows the same + * conventions: per-frame application timestamp and frame id in the packet trailer, and the publisher's own + * monotonic clock answered on the clock-sync topic. The Orin's frame-ready stamp is converted onto this machine's + * monotonic clock through the control-link mapping before publication, so consumers map one clock, never two. + * + * Timing breaks are explicit, because consumers cannot see this leg: a frame with no mapping, or a mapping worse than + * the budget, is withheld; and when the mapping's epoch changes (the Orin's clock jumped) the track is unpublished and + * published again at the next keyframe, which a recorder reports as a lost track instead of silently continuing a + * synchronized take through the jump. The picture size is fixed per stream: a change is an error. + */ +class LiveKitOutput final : public PacketSink +{ +public: + /// Where the Orin-to-PC clock mapping comes from; empty when there is none (no session, or a stale mapping). + using MappingSource = std::function()>; + + /// `request_idr` is called when the track needs a keyframe (a subscriber joined, a frame was refused, or a gap). + LiveKitOutput(SharedLiveKit& livekit, + const LiveKitOutputConfig& config, + MappingSource mapping, + std::function request_idr); + + void write(const EncodedPacket& packet, const FrameMeta& meta, const Geometry& geometry) override; + + /// The source session ended: unpublish, so a sender restart is never carried through as one continuous take. The + /// next session publishes a fresh track at its first keyframe. + void end_session(); + + uint64_t published() const + { + return m_published.load(); + } + /// Frames the sender sent without a capture time (it could not tie the unit to a capture). + uint64_t no_source_stamp() const + { + return m_no_source_stamp.load(); + } + uint64_t unmapped() const + { + return m_unmapped.load(); + } + uint64_t refused() const + { + return m_refused.load(); + } + uint64_t republished() const + { + return m_republished.load(); + } + uint64_t over_budget() const + { + return m_over_budget.load(); + } + /// Orin frame-ready to arrival at this PC (Orin encode, its queue and the link), per published frame. + const DurationStats& uplink_ns() const + { + return m_uplink_ns; + } + /// Orin frame-ready to the moment this worker hands the frame to LiveKit: everything before the SFU and the page. + const DurationStats& source_to_publish_ns() const + { + return m_source_to_publish_ns; + } + /// From the intake of the record to the start of this publish call: the worker's own time (queue, decode, warp, + /// encode). + const DurationStats& intake_to_write_ns() const + { + return m_intake_to_write_ns; + } + /// The LiveKit SDK call that takes the encoded frame. + const DurationStats& submit_ns() const + { + return m_submit_ns; + } + int64_t upstream_uncertainty_ns() const + { + return m_upstream_uncertainty_ns.load(); + } + +private: + void break_publication(); ///< unpublish; the next keyframe publishes a fresh track + + SharedLiveKit& m_livekit; + LiveKitOutputConfig m_config; + MappingSource m_mapping; + std::function m_request_idr; + std::unique_ptr m_track; + Geometry m_track_geometry; + uint64_t m_epoch = 0; + bool m_awaiting_keyframe = true; + bool m_idr_requested_for_gap = false; + bool m_rate_feedback_noted = false; + // Counted on the output's thread and read by the statistics thread. + std::atomic m_published{ 0 }; + std::atomic m_unmapped{ 0 }; + std::atomic m_no_source_stamp{ 0 }; + std::atomic m_refused{ 0 }; + std::atomic m_republished{ 0 }; + std::atomic m_over_budget{ 0 }; + DurationStats m_intake_to_write_ns; + DurationStats m_submit_ns; + DurationStats m_uplink_ns; + DurationStats m_source_to_publish_ns; + std::atomic m_upstream_uncertainty_ns{ 0 }; +}; + +} // namespace video_stage diff --git a/src/video_stage/app/main.cpp b/src/video_stage/app/main.cpp new file mode 100644 index 0000000000..518477e184 --- /dev/null +++ b/src/video_stage/app/main.cpp @@ -0,0 +1,197 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "livekit_output.hpp" +#include "stream_worker.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace video_stage; + +namespace +{ + +std::atomic g_stop{ false }; + +void on_signal(int) +{ + g_stop = true; +} + +void usage(const char* program) +{ + std::cout + << "Usage: " << program << " --stream=PORT:TRACK ... --livekit-url=URL --livekit-token-file=PATH\n" + << " " << program << " --stream=PORT:file=PATH.h264 ...\n" + << " [--operation=none|sine-warp] [--amplitude=PX] [--bitrate=BPS] [--idr-period=N] [--max-upstream-uncertainty-ms=MS] [--stats-interval=S] [--once]\n" + << "One --stream per camera. Each listens on PORT for the Orin's direct connection (AVS1 H.264 records plus a\n" + << "control link) and publishes the access units as LiveKit track TRACK (all tracks under one participant) or\n" + << "writes them to a file. By default (--operation=none) the Orin's access units are forwarded untouched, with no\n" + << "decode, encode or GPU. --operation=sine-warp decodes on NVDEC, applies the example sine warp and re-encodes on\n" + << "NVENC, as a test of the GPU stage. Serves one Orin\n" + << "session after another unless --once. Prints per-stage percentiles at the end.\n"; +} + +std::string read_token(const std::string& path) +{ + std::ifstream in(path); + std::string token; + if (!in || !std::getline(in, token) || token.empty()) + throw std::runtime_error("cannot read a token from " + path); + return token; +} + +// "PORT:TRACK" or "PORT:file=PATH". +StreamSpec parse_stream(const std::string& text) +{ + const auto colon = text.find(':'); + if (colon == std::string::npos || colon == 0 || colon + 1 == text.size()) + throw std::runtime_error("--stream must be PORT:TRACK or PORT:file=PATH, got '" + text + "'"); + StreamSpec spec; + spec.port = static_cast(std::stoul(text.substr(0, colon))); + const std::string rest = text.substr(colon + 1); + if (rest.rfind("file=", 0) == 0) + spec.file = rest.substr(5); + else + spec.track = rest; + return spec; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + std::vector specs; + std::string livekit_url; + std::string token_file; + StreamSettings settings; + int stats_interval_s = 5; + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + if (arg.rfind("--stream=", 0) == 0) + specs.push_back(parse_stream(arg.substr(9))); + else if (arg.rfind("--livekit-url=", 0) == 0) + livekit_url = arg.substr(14); + else if (arg.rfind("--livekit-token-file=", 0) == 0) + token_file = arg.substr(21); + else if (arg.rfind("--stats-interval=", 0) == 0) + stats_interval_s = std::stoi(arg.substr(17)); + else if (arg == "--once") + settings.once = true; + else if (arg.rfind("--operation=", 0) == 0) + { + const std::string name = arg.substr(12); + if (name == "none") + settings.operation = Operation::None; + else if (name == "sine-warp") + settings.operation = Operation::SineWarp; + else + { + std::cerr << "--operation must be none or sine-warp\n"; + return 1; + } + } + else if (arg.rfind("--amplitude=", 0) == 0) + settings.warp.amplitude_px = std::stof(arg.substr(12)); + else if (arg.rfind("--bitrate=", 0) == 0) + { + settings.pipeline.bitrate_bps = static_cast(std::stoul(arg.substr(10))); + settings.livekit.max_bitrate_bps = settings.pipeline.bitrate_bps; + } + else if (arg.rfind("--idr-period=", 0) == 0) + settings.pipeline.idr_period = static_cast(std::stoul(arg.substr(13))); + else if (arg.rfind("--max-upstream-uncertainty-ms=", 0) == 0) + settings.livekit.max_upstream_uncertainty_ns = static_cast(std::stod(arg.substr(30)) * 1e6); + else + { + usage(argv[0]); + return arg == "--help" ? 0 : 1; + } + } + if (specs.empty()) + { + usage(argv[0]); + return 1; + } + bool any_livekit = false; + for (const StreamSpec& s : specs) + any_livekit = any_livekit || s.file.empty(); + if (any_livekit && (livekit_url.empty() || token_file.empty())) + { + std::cerr << "a --stream with a track needs --livekit-url and --livekit-token-file\n"; + return 1; + } + + // Only an operation that uses the GPU needs a CUDA context; the default forwards the Orin's access units untouched. + std::unique_ptr context; + if (settings.operation != Operation::None) + { + context = std::make_unique(); + std::cout << "GPU: " << context->device_name() << "\n"; + } + // One LiveKit participant for every stream, as the Orin producer joins once for all its cameras. + std::unique_ptr livekit; + if (any_livekit) + livekit = std::make_unique(livekit_url, read_token(token_file)); + + std::vector> workers; + for (const StreamSpec& spec : specs) + workers.push_back( + std::make_unique(context.get(), spec, settings, spec.file.empty() ? livekit.get() : nullptr)); + + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + for (auto& w : workers) + w->start(); + + // With --once the streams end by themselves; otherwise run until interrupted. + std::atomic finished{ false }; + std::thread waiter( + [&] + { + auto next_stats = std::chrono::steady_clock::now() + std::chrono::seconds(stats_interval_s); + while (!g_stop && !finished) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (stats_interval_s > 0 && std::chrono::steady_clock::now() >= next_stats) + { + next_stats += std::chrono::seconds(stats_interval_s); + for (const auto& w : workers) + w->print_live(std::cout); + std::cout.flush(); + } + } + if (g_stop) + { + for (auto& w : workers) + w->request_stop(); + } + }); + bool ok = true; + for (auto& w : workers) + ok = w->join() && ok; + finished = true; + waiter.join(); + + for (const auto& w : workers) + w->print_summary(std::cout); + return ok ? 0 : 1; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} diff --git a/src/video_stage/app/pipeline.cpp b/src/video_stage/app/pipeline.cpp new file mode 100644 index 0000000000..8bfce4efc7 --- /dev/null +++ b/src/video_stage/app/pipeline.cpp @@ -0,0 +1,244 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "pipeline.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +int64_t monotonic_ns() +{ + timespec ts{}; + clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000LL + ts.tv_nsec; +} + +namespace +{ +constexpr size_t kMetaRing = 64; +} + +struct Pipeline::Impl +{ + CudaContext& context; + GpuOperation& operation; + PacketSink& sink; + PipelineConfig config; + cudaStream_t stream = nullptr; + + std::array meta{}; + uint64_t next_key = 0; + bool have_previous_frame = false; + uint64_t previous_frame = 0; + bool need_idr = true; + std::atomic idr_requested{ false }; + + Geometry input_geometry; + Geometry output_geometry; + std::unique_ptr encoder; + std::unique_ptr decoder; + PipelineStats stats; + int64_t decode_start_ns = 0; + std::string last_sps; + uint32_t source_fps = 0; ///< the frame rate the source declared in its last record + + Impl(CudaContext& ctx, GpuOperation& op, PacketSink& s, const PipelineConfig& cfg) + : context(ctx), operation(op), sink(s), config(cfg) + { + context.make_current(); + if (cudaStreamCreateWithFlags(&stream, cudaStreamNonBlocking) != cudaSuccess) + throw std::runtime_error("Pipeline: cannot create the CUDA stream"); + decoder = std::make_unique( + context, stream, [this](DecodedFrame&& f) { on_frame(std::move(f)); }, + [this](const StreamFormat& f) { on_format(f); }); + } + + // A new source session: nothing the previous one left in the codec may reach the new one. The decoder is rebuilt + // (its parser and any pictures it still holds are dropped with it), the encoder is released so the new session's + // first sequence header creates a matching one that opens with an IDR, and the identity ring forgets every key. + void reset_session() + { + context.make_current(); + cudaStreamSynchronize(stream); + decoder.reset(); + encoder.reset(); + decoder = std::make_unique( + context, stream, [this](DecodedFrame&& f) { on_frame(std::move(f)); }, + [this](const StreamFormat& f) { on_format(f); }); + meta.fill(FrameMeta{}); + next_key = 0; + input_geometry = Geometry{}; + output_geometry = Geometry{}; + last_sps.clear(); + have_previous_frame = false; + previous_frame = 0; + need_idr = true; + idr_requested = false; + } + + ~Impl() + { + decoder.reset(); + encoder.reset(); + context.make_current(); + cudaStreamDestroy(stream); + } + + // The input geometry changed: prepare the operation and a matching encoder before the next picture. The first + // output after a change is an IDR. + void on_format(const StreamFormat& format) + { + const Geometry g = format.geometry; + input_geometry = g; + operation.prepare(g); + output_geometry = operation.output_geometry(g); + EncoderConfig enc; + enc.geometry = output_geometry; + enc.full_range = format.color.full_range; // the encoder signals the range the pictures actually have + enc.bitrate_bps = config.bitrate_bps; + enc.idr_period = config.idr_period; + enc.fps_numerator = source_fps != 0 ? source_fps : config.fps_numerator; + encoder.reset(); + encoder = std::make_unique(context, enc); + need_idr = true; + } + + void on_frame(DecodedFrame&& frame) + { + ++stats.pictures; + const int64_t decoded_ns = monotonic_ns(); + const FrameMeta m = meta[frame.key % kMetaRing]; + stats.decode_ns.add(decoded_ns - m.submitted_ns); + stats.decode_lag.add(static_cast(next_key - 1 - frame.key)); + if (m.key != frame.key) + { + ++stats.metadata_lost; + return; // identity of this picture is unknown: never publish it under another frame's metadata + } + + NvencEncoder::Surface surface; + if (!encoder || !encoder->acquire(surface)) + { + ++stats.dropped_no_surface; + return; + } + EncodedPacket packet; + try + { + surface.view.color = frame.view.color; + operation.enqueue(frame.view, surface.view, stream); + const bool idr = need_idr || idr_requested.exchange(false); + packet = encoder->encode(surface, stream, frame.key, idr); + need_idr = false; + } + catch (...) + { + // Exceptional path only: launches already queued may still read the decoder surface or write the encoder's + // one. Drain this stream, then hand the encoder surface back; the decoder lease releases on unwind. + cudaStreamSynchronize(stream); + encoder->release(surface); + throw; + } + frame.lease.reset(); // the encoder has consumed the output; the decoder surface can be reused + const int64_t done_ns = monotonic_ns(); + stats.process_encode_ns.add(done_ns - decoded_ns); + stats.receive_to_output_ns.add(done_ns - m.received_ns); + const FrameMeta& out_meta = meta[packet.key % kMetaRing]; + if (out_meta.key != packet.key) + { + ++stats.metadata_lost; + return; + } + sink.write(packet, out_meta, output_geometry); + } +}; + +Pipeline::Pipeline(CudaContext& context, GpuOperation& operation, PacketSink& sink, const PipelineConfig& config) + : m_impl(std::make_unique(context, operation, sink, config)) +{ +} + +Pipeline::~Pipeline() = default; + +void Pipeline::process(const avs1::Record& record, int64_t received_ns) +{ + Impl& p = *m_impl; + ++p.stats.records; + const uint64_t frame = record.sample.frame_number; + // Frame ids need not be consecutive (the camera captures faster than it publishes); they must increase. + if (p.have_previous_frame && frame <= p.previous_frame) + ++p.stats.gaps; + p.previous_frame = frame; + p.have_previous_frame = true; + + const uint64_t key = p.next_key++; + FrameMeta& slot = p.meta[key % kMetaRing]; + slot.frame_number = frame; + slot.source_timestamp_ns = record.sample.frame_timestamp_ns; + slot.received_ns = received_ns; + slot.key = key; + p.source_fps = record.sample.fps_numerator; + p.decode_start_ns = monotonic_ns(); + slot.submitted_ns = p.decode_start_ns; + p.stats.queue_age_ns.add(p.decode_start_ns - received_ns); + + // A sequence header that does not say "no reordering" makes NVDEC hold back pictures; say it when the source's + // contract is that it has no B-frames, and report what the stream declared. + const uint8_t* payload = record.payload; + size_t payload_size = record.payload_size; + std::vector rewritten; + if (const auto sps = h264::parse_sps(payload, payload_size)) + { + // The operation samples chroma at the default siting; a stream that declares another would be warped with + // its colour misplaced, so it is refused here rather than processed. + if (sps->chroma_loc_present && (sps->chroma_loc_top != 0 || sps->chroma_loc_bottom != 0)) + throw std::runtime_error("the stream declares chroma sample locations (" + + std::to_string(sps->chroma_loc_top) + ", " + + std::to_string(sps->chroma_loc_bottom) + "); only the default siting is supported"); + const std::string described = sps->describe(); + if (described != p.last_sps) + { + p.last_sps = described; + std::cerr << "source SPS: " << described << std::endl; + } + const bool needs = !sps->bitstream_restriction || sps->max_num_reorder_frames != 0 || + sps->max_dec_frame_buffering > std::max(sps->max_num_ref_frames, 1); + if (p.config.source_has_no_b_frames && needs) + { + if (auto fixed = h264::declare_no_reordering(payload, payload_size)) + { + rewritten = std::move(*fixed); + payload = rewritten.data(); + payload_size = rewritten.size(); + } + } + } + p.decoder->decode(payload, payload_size, key); +} + +void Pipeline::reset_session() +{ + m_impl->reset_session(); +} + +void Pipeline::request_idr() +{ + m_impl->idr_requested = true; +} + +const PipelineStats& Pipeline::stats() const +{ + return m_impl->stats; +} + +} // namespace video_stage diff --git a/src/video_stage/app/pipeline.hpp b/src/video_stage/app/pipeline.hpp new file mode 100644 index 0000000000..09ca5da258 --- /dev/null +++ b/src/video_stage/app/pipeline.hpp @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "stats.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// Source identity carried with a picture from the access unit that produced it to the packet that leaves. +struct FrameMeta +{ + uint64_t frame_number = 0; + uint64_t source_timestamp_ns = 0; + int64_t received_ns = 0; ///< CLOCK_MONOTONIC when the intake finished reading the record + uint64_t key = ~uint64_t{ 0 }; ///< the key this entry was written for; a mismatch means the entry was overwritten + int64_t submitted_ns = 0; ///< when the access unit was handed to the decoder +}; + +/// Where finished access units go. Called on the pipeline thread; it must not block. +class PacketSink +{ +public: + virtual ~PacketSink() = default; + virtual void write(const EncodedPacket& packet, const FrameMeta& meta, const Geometry& geometry) = 0; +}; + +struct PipelineConfig +{ + /// The source never emits B-frames (the Orin producer's encoder contract). A decoder that cannot see that holds + /// back pictures to be safe, so the sequence header is rewritten to declare it. + bool source_has_no_b_frames = true; + uint32_t bitrate_bps = 12'000'000; + uint32_t idr_period = 30; + uint32_t fps_numerator = 30; +}; + +struct PipelineStats +{ + // Written by the processing thread and read by the statistics thread, so the counters are atomic. + std::atomic records{ 0 }; + std::atomic pictures{ 0 }; + std::atomic dropped_no_surface{ 0 }; + std::atomic gaps{ 0 }; ///< records whose frame number did not increase + std::atomic metadata_lost{ 0 }; ///< pictures whose metadata entry had been overwritten + DurationStats queue_age_ns; ///< from intake to the start of processing + DurationStats decode_ns; ///< from handing an access unit to the decoder to its picture appearing + DurationStats decode_lag; ///< records handed to the decoder after this picture's own record, before it appeared + ///< (frames) + DurationStats process_encode_ns; + DurationStats receive_to_output_ns; +}; + +/** + * @brief Decode -> GPU operation -> encode on one thread, frame by frame. + * + * Pictures stay in GPU memory from NVDEC to NVENC. Identity travels by key: each access unit's key indexes a small + * ring of FrameMeta, and NVDEC and NVENC hand the same key back, so metadata follows its own frame through any loss. + * A picture arriving while every encoder input surface is busy is dropped and counted, never queued. + */ +class Pipeline +{ +public: + Pipeline(CudaContext& context, GpuOperation& operation, PacketSink& sink, const PipelineConfig& config); + ~Pipeline(); + + /// Process one received record. Throws std::runtime_error on a decoder or encoder failure. + void process(const avs1::Record& record, int64_t received_ns); + + /// A new source session begins: forget the previous one's frame numbering and start the encoder at an IDR. Call + /// between sessions, from the thread that calls process(). + void reset_session(); + + /// Make the next encoded picture an IDR (a subscriber joined or the receiving side asked). Thread-safe. + void request_idr(); + + const PipelineStats& stats() const; + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +int64_t monotonic_ns(); + +} // namespace video_stage diff --git a/src/video_stage/app/stats.hpp b/src/video_stage/app/stats.hpp new file mode 100644 index 0000000000..1be197b6c3 --- /dev/null +++ b/src/video_stage/app/stats.hpp @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +namespace video_stage +{ + +/// Bounded per-stage duration samples, summarised as percentiles at shutdown; nothing is logged per frame. +class DurationStats +{ +public: + explicit DurationStats(size_t capacity = 1U << 16) : m_capacity(capacity) + { + m_samples.reserve(capacity); + } + + void add(int64_t ns) + { + std::lock_guard lock(m_mutex); + if (m_samples.size() < m_capacity) + m_samples.push_back(ns); + else + ++m_overflow; + } + + size_t count() const + { + std::lock_guard lock(m_mutex); + return m_samples.size(); + } + size_t overflow() const + { + return m_overflow; + } + + /// The q-th percentile (0..100) in nanoseconds; 0 when empty. + int64_t percentile(double q) const + { + std::vector sorted; + { + std::lock_guard lock(m_mutex); + sorted = m_samples; + } + if (sorted.empty()) + return 0; + std::sort(sorted.begin(), sorted.end()); + const size_t index = + std::min(sorted.size() - 1, static_cast(q / 100.0 * static_cast(sorted.size()))); + return sorted[index]; + } + +private: + size_t m_capacity; + size_t m_overflow = 0; + mutable std::mutex m_mutex; + std::vector m_samples; +}; + +} // namespace video_stage diff --git a/src/video_stage/app/stream_worker.cpp b/src/video_stage/app/stream_worker.cpp new file mode 100644 index 0000000000..59ec5190ba --- /dev/null +++ b/src/video_stage/app/stream_worker.cpp @@ -0,0 +1,465 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "stream_worker.hpp" + +#include "control_channel.hpp" +#include "tcp_receiver.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +namespace +{ + +// Writes the processed access units as one Annex-B stream, so a run can be checked with ffprobe or a player. +class FileSink final : public PacketSink +{ +public: + explicit FileSink(const std::string& path) : m_out(path, std::ios::binary) + { + if (!m_out) + throw std::runtime_error("cannot open " + path); + } + void write(const EncodedPacket& packet, const FrameMeta&, const Geometry&) override + { + m_out.write(reinterpret_cast(packet.data.data()), static_cast(packet.data.size())); + if (!m_out) + throw std::runtime_error("writing the output file failed"); + } + +private: + std::ofstream m_out; +}; + +// Records waiting for the processing thread. Bounded by count: past that, the stage is not keeping up. +class RecordQueue +{ +public: + struct Item + { + std::vector bytes; + int64_t arrived_ns = 0; + }; + + explicit RecordQueue(size_t capacity) : m_capacity(capacity) + { + } + + /// False when full. + bool push(Item&& item) + { + { + std::lock_guard lock(m_mutex); + if (m_items.size() >= m_capacity) + return false; + m_items.push_back(std::move(item)); + } + m_wake.notify_one(); + return true; + } + + /// Discards everything waiting; returns how many records. + size_t clear() + { + std::lock_guard lock(m_mutex); + const size_t n = m_items.size(); + m_items.clear(); + return n; + } + + void close() + { + { + std::lock_guard lock(m_mutex); + m_closed = true; + } + m_wake.notify_all(); + } + + /// Blocks for the next record; false once closed and empty. + bool pop(Item& out) + { + std::unique_lock lock(m_mutex); + m_wake.wait(lock, [this] { return m_closed || !m_items.empty(); }); + if (m_items.empty()) + return false; + out = std::move(m_items.front()); + m_items.pop_front(); + return true; + } + +private: + size_t m_capacity; + std::mutex m_mutex; + std::condition_variable m_wake; + std::deque m_items; + bool m_closed = false; +}; + +// What a stream without a GPU pipeline reports for the pipeline's own stages: nothing. +const PipelineStats& no_pipeline_stats() +{ + static const PipelineStats empty; + return empty; +} + +void print_stage(std::ostream& out, const char* name, const DurationStats& s) +{ + char line[160]; + std::snprintf(line, sizeof(line), " %-22s n=%zu p50 %.2f ms p95 %.2f ms p99 %.2f ms\n", name, s.count(), + s.percentile(50) / 1e6, s.percentile(95) / 1e6, s.percentile(99) / 1e6); + out << line; +} + +} // namespace + +struct StreamWorker::Impl +{ + CudaContext* context; + StreamSpec spec; + StreamSettings settings; + SharedLiveKit* livekit; + + std::unique_ptr operation; ///< null for Operation::None + std::unique_ptr receiver; + std::atomic active_control{ nullptr }; + std::unique_ptr sink; + LiveKitOutput* livekit_output = nullptr; + std::unique_ptr pipeline; + + std::thread thread; + std::atomic stop{ false }; + std::atomic ok{ true }; + std::atomic overflows{ 0 }; + std::atomic discarded{ 0 }; + std::atomic sessions{ 0 }; + uint64_t epoch_base = 0; ///< advances for every session so a control channel's epoch is never reused + std::atomic clock_exchanges{ 0 }; + std::atomic clock_rejected{ 0 }; + std::atomic clock_discontinuities{ 0 }; + DurationStats backlog_bytes; + // Pass-through state (Operation::None): the picture size from the last sequence header, and a count. + Geometry passthrough_geometry; + std::atomic passthrough_records{ 0 }; + + Impl(CudaContext* ctx, const StreamSpec& s, const StreamSettings& st, SharedLiveKit* lk) + : context(ctx), spec(s), settings(st), livekit(lk) + { + if (settings.operation == Operation::SineWarp) + { + if (context == nullptr) + throw std::runtime_error("the sine warp needs a GPU context"); + operation = std::make_unique(settings.warp); + } + if (livekit != nullptr) + { + LiveKitOutputConfig out = settings.livekit; + out.track_name = spec.track; + auto output = std::make_unique( + *livekit, out, + [this]() -> std::optional + { + ControlChannel* c = active_control.load(); + return c ? c->mapping() : std::nullopt; + }, + [this] + { + // A subscriber needs a keyframe: ask this stage's encoder, or, with nothing in between, the Orin's. + if (pipeline) + pipeline->request_idr(); + else + receiver->request_keyframe(); + }); + livekit_output = output.get(); + sink = std::move(output); + } + else + { + sink = std::make_unique(spec.file); + } + if (operation) + pipeline = std::make_unique(*context, *operation, *sink, settings.pipeline); + receiver = std::make_unique(spec.port); + } + + // Operation::None: the access unit goes to the sink untouched. The picture size comes from the stream's own + // sequence header; nothing is published before the first one. + void forward(const avs1::Record& record, int64_t arrived_ns) + { + const bool keyframe = avs1::contains_idr(record.payload, record.payload_size); + if (keyframe) + { + if (const auto sps = h264::parse_sps(record.payload, record.payload_size)) + passthrough_geometry = Geometry{ sps->width, sps->height }; + } + if (passthrough_geometry.width == 0) + return; + FrameMeta meta; + meta.frame_number = record.sample.frame_number; + meta.source_timestamp_ns = record.sample.frame_timestamp_ns; + meta.received_ns = arrived_ns; + const uint64_t index = passthrough_records.fetch_add(1); + meta.key = index; + EncodedPacket packet; + packet.data.assign(record.payload, record.payload + record.payload_size); + packet.keyframe = keyframe; + packet.key = index; + sink->write(packet, meta, passthrough_geometry); + } + + // Runs one Orin session to its end. Intake and processing are separate threads, so reading from the socket never + // waits for the GPU stages and the time a record spends waiting is measured. Compressed records are not dropped + // one by one: if the queue overflows, everything waiting is discarded and the stream resumes at the next keyframe, + // which is asked of the sender. A link or decoder error ends the session (reported), after the queued records are + // processed. + bool run_session() + { + constexpr size_t kQueueCapacity = 4; + RecordQueue queue(kQueueCapacity); + std::atomic session_ok{ true }; + std::thread intake( + [&] + { + try + { + bool catching_up = false; + std::vector bytes; + while (receiver->read_record(bytes)) + { + const int64_t arrived_ns = monotonic_ns(); + backlog_bytes.add(static_cast(receiver->pending_bytes())); + std::string error; + const auto record = avs1::parse(bytes.data(), bytes.size(), error); + if (!record) + throw std::runtime_error("bad AVS1 record: " + error); + if (catching_up) + { + if (!avs1::contains_idr(record->payload, record->payload_size)) + { + ++discarded; + continue; + } + catching_up = false; + } + if (!queue.push({ std::move(bytes), arrived_ns })) + { + discarded += queue.clear() + 1; + ++overflows; + catching_up = true; + receiver->request_keyframe(); + } + bytes = {}; + } + } + catch (const std::exception& e) + { + std::cerr << "stream " << spec.port << " intake: " << e.what() << std::endl; + session_ok = false; + } + queue.close(); + }); + + try + { + RecordQueue::Item item; + while (queue.pop(item)) + { + std::string error; + const auto record = avs1::parse(item.bytes.data(), item.bytes.size(), error); + if (!record) + throw std::runtime_error("bad AVS1 record: " + error); + if (pipeline) + pipeline->process(*record, item.arrived_ns); + else + forward(*record, item.arrived_ns); + } + } + catch (const std::exception& e) + { + std::cerr << "stream " << spec.port << " processing: " << e.what() << std::endl; + session_ok = false; + queue.close(); + receiver->shutdown_session(); + } + intake.join(); + return session_ok.load(); + } + + void run() + { + try + { + do + { + std::cout << "stream " << spec.port << ": listening" << std::endl; + receiver->accept_session(); + if (stop) + break; + std::cout << "stream " << spec.port << ": Orin session connected" << std::endl; + ++sessions; + { + // A new session starts clean: nothing of the previous sender's publication or numbering carries + // over. + if (livekit_output) + livekit_output->end_session(); + if (pipeline) + pipeline->reset_session(); + passthrough_geometry = Geometry{}; + epoch_base += 1'000'000; + ControlChannel control(*receiver, epoch_base + 1); + active_control = &control; + if (!run_session()) + ok = false; + active_control = nullptr; + if (livekit_output) + livekit_output->end_session(); + clock_exchanges += control.exchanges(); + clock_rejected += control.rejected(); + clock_discontinuities += control.discontinuities(); + } + std::cout << "stream " << spec.port << ": Orin session ended" << std::endl; + } while (!settings.once && !stop); + } + catch (const std::exception& e) + { + if (!stop) + { + std::cerr << "stream " << spec.port << ": " << e.what() << std::endl; + ok = false; + } + } + } +}; + +StreamWorker::StreamWorker(CudaContext* context, + const StreamSpec& spec, + const StreamSettings& settings, + SharedLiveKit* livekit) + : m_impl(std::make_unique(context, spec, settings, livekit)) +{ +} + +StreamWorker::~StreamWorker() +{ + request_stop(); + join(); +} + +void StreamWorker::start() +{ + m_impl->thread = std::thread([this] { m_impl->run(); }); +} + +void StreamWorker::request_stop() +{ + m_impl->stop = true; + m_impl->receiver->stop_listening(); + m_impl->receiver->shutdown_session(); +} + +bool StreamWorker::join() +{ + if (m_impl->thread.joinable()) + m_impl->thread.join(); + return m_impl->ok.load(); +} + +void StreamWorker::print_live(std::ostream& out) const +{ + const Impl& d = *m_impl; + const PipelineStats& s = d.pipeline ? d.pipeline->stats() : no_pipeline_stats(); + char line[512]; + int n = std::snprintf( + line, sizeof(line), + "[%u] queue age p50 %.1f p99 %.1f ms | backlog p99 %lld B | decoder lag p50 %lld p99 %lld frames | " + "submit->picture p50 %.1f p99 %.1f ms | warp+encode p50 %.1f p99 %.1f ms", + static_cast(d.spec.port), s.queue_age_ns.percentile(50) / 1e6, s.queue_age_ns.percentile(99) / 1e6, + static_cast(d.backlog_bytes.percentile(99)), static_cast(s.decode_lag.percentile(50)), + static_cast(s.decode_lag.percentile(99)), s.decode_ns.percentile(50) / 1e6, + s.decode_ns.percentile(99) / 1e6, s.process_encode_ns.percentile(50) / 1e6, + s.process_encode_ns.percentile(99) / 1e6); + if (d.livekit_output && n > 0 && n < static_cast(sizeof(line))) + { + std::snprintf( + line + n, sizeof(line) - static_cast(n), + " | intake->write p50 %.1f p99 %.1f ms | submit p50 %.1f p99 %.1f ms | Orin->PC p50 %.1f p99 %.1f ms | Orin->publish p50 %.1f p99 %.1f ms | mapping +-%.1f ms | published %llu", + d.livekit_output->intake_to_write_ns().percentile(50) / 1e6, + d.livekit_output->intake_to_write_ns().percentile(99) / 1e6, + d.livekit_output->submit_ns().percentile(50) / 1e6, d.livekit_output->submit_ns().percentile(99) / 1e6, + d.livekit_output->uplink_ns().percentile(50) / 1e6, d.livekit_output->uplink_ns().percentile(99) / 1e6, + d.livekit_output->source_to_publish_ns().percentile(50) / 1e6, + d.livekit_output->source_to_publish_ns().percentile(99) / 1e6, + d.livekit_output->upstream_uncertainty_ns() / 1e6, + static_cast(d.livekit_output->published())); + } + out << line << "\n"; +} + +void StreamWorker::print_summary(std::ostream& out) const +{ + const Impl& d = *m_impl; + const PipelineStats& s = d.pipeline ? d.pipeline->stats() : no_pipeline_stats(); + char line[256]; + std::snprintf( + line, sizeof(line), + "stream %u: sessions %llu, records %llu, pictures %llu, dropped (no surface) %llu, gaps %llu, metadata lost %llu\n" + " overflows %llu, records discarded %llu, clock exchanges %llu (rejected %llu, discontinuities %llu)\n", + static_cast(d.spec.port), static_cast(d.sessions.load()), + static_cast(d.pipeline ? s.records.load() : d.passthrough_records.load()), + static_cast(d.pipeline ? s.pictures.load() : d.passthrough_records.load()), + static_cast(s.dropped_no_surface.load()), static_cast(s.gaps.load()), + static_cast(s.metadata_lost.load()), static_cast(d.overflows.load()), + static_cast(d.discarded.load()), static_cast(d.clock_exchanges.load()), + static_cast(d.clock_rejected.load()), + static_cast(d.clock_discontinuities.load())); + out << line; + if (d.livekit_output) + { + std::snprintf( + line, sizeof(line), + " published %llu (frames sent without a capture time: no source stamp %llu, unmapped %llu, over budget " + "%llu), refused %llu, republished %llu\n", + static_cast(d.livekit_output->published()), + static_cast(d.livekit_output->no_source_stamp()), + static_cast(d.livekit_output->unmapped()), + static_cast(d.livekit_output->over_budget()), + static_cast(d.livekit_output->refused()), + static_cast(d.livekit_output->republished())); + out << line; + } + std::snprintf(line, sizeof(line), " socket backlog p50 %lld B p99 %lld B\n", + static_cast(d.backlog_bytes.percentile(50)), + static_cast(d.backlog_bytes.percentile(99))); + out << line; + if (d.livekit_output) + { + print_stage(out, "Orin -> PC arrival", d.livekit_output->uplink_ns()); + print_stage(out, "Orin -> publish", d.livekit_output->source_to_publish_ns()); + } + print_stage(out, "queue age", s.queue_age_ns); + { + char lag[160]; + std::snprintf(lag, sizeof(lag), " decoder lag (frames) p50 %lld p99 %lld max %lld\n", + static_cast(s.decode_lag.percentile(50)), + static_cast(s.decode_lag.percentile(99)), + static_cast(s.decode_lag.percentile(100))); + out << lag; + } + print_stage(out, "decode", s.decode_ns); + print_stage(out, "warp + encode", s.process_encode_ns); + print_stage(out, "receive -> output", s.receive_to_output_ns); +} + +} // namespace video_stage diff --git a/src/video_stage/app/stream_worker.hpp b/src/video_stage/app/stream_worker.hpp new file mode 100644 index 0000000000..14f74a99a8 --- /dev/null +++ b/src/video_stage/app/stream_worker.hpp @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "livekit_output.hpp" +#include "pipeline.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// One camera's stream: where the Orin connects, and where the processed video goes. +struct StreamSpec +{ + uint16_t port = 0; + /// LiveKit track name; used when a LiveKit session is given. + std::string track; + /// Annex-B output file; used instead of LiveKit when set (for checking a run). + std::string file; +}; + +/// What happens to each picture between the Orin and LiveKit. +enum class Operation +{ + /// Nothing: the Orin's encoded access units go to LiveKit as they are. No decode, no encode, no GPU; the lowest + /// latency and no quality loss. The default. + None, + /// Decode on NVDEC, apply the example sine-wave warp, re-encode on NVENC. For testing the GPU stage and as the + /// template for the fisheye rectification. + SineWarp, +}; + +struct StreamSettings +{ + Operation operation = Operation::None; + PipelineConfig pipeline; + SineWarpParams warp; + LiveKitOutputConfig livekit; + bool once = false; ///< serve one Orin session, then finish +}; + +/** + * @brief One stream's thread: accept an Orin session, run it to its end, accept the next. + * + * Owns that stream's receiver, control channel, optional GPU pipeline and output. Streams share the CUDA context and, + * when publishing, one LiveKit participant, and otherwise run independently so one camera's trouble stays its own. + */ +class StreamWorker +{ +public: + /// `context` is needed only for an operation that uses the GPU (null for Operation::None). + StreamWorker(CudaContext* context, const StreamSpec& spec, const StreamSettings& settings, SharedLiveKit* livekit); + ~StreamWorker(); + + StreamWorker(const StreamWorker&) = delete; + StreamWorker& operator=(const StreamWorker&) = delete; + + void start(); + /// Ends the wait for an Orin and shuts down any running session. + void request_stop(); + /// Waits for the thread; false if any session of this stream failed. + bool join(); + + void print_summary(std::ostream& out) const; + /// One compact line of the current percentiles; safe to call while the stream runs. + void print_live(std::ostream& out) const; + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace video_stage diff --git a/src/video_stage/app/tcp_receiver.cpp b/src/video_stage/app/tcp_receiver.cpp new file mode 100644 index 0000000000..91ab361d4d --- /dev/null +++ b/src/video_stage/app/tcp_receiver.cpp @@ -0,0 +1,241 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "tcp_receiver.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +namespace +{ + +// Reads exactly `size` bytes. Returns false only for end of stream before the first byte when `allow_eof`. +bool read_exact(int fd, uint8_t* out, size_t size, bool allow_eof) +{ + size_t done = 0; + while (done < size) + { + const ssize_t n = ::recv(fd, out + done, size - done, 0); + if (n == 0) + { + if (allow_eof && done == 0) + return false; + throw std::runtime_error("TcpReceiver: the connection ended inside a record"); + } + if (n < 0) + { + if (errno == EINTR) + continue; + // A peer that exits with unread data resets the connection: at a record boundary that is the end of the + // stream. + if ((errno == ECONNRESET || errno == EPIPE) && allow_eof && done == 0) + return false; + throw std::runtime_error(std::string("TcpReceiver: recv failed: ") + std::strerror(errno)); + } + done += static_cast(n); + } + return true; +} + +} // namespace + +TcpReceiver::TcpReceiver(uint16_t port) +{ + m_listen = ::socket(AF_INET, SOCK_STREAM, 0); + if (m_listen < 0) + throw std::runtime_error("TcpReceiver: cannot create a socket"); + const int one = 1; + ::setsockopt(m_listen, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(port); + if (::bind(m_listen, reinterpret_cast(&addr), sizeof(addr)) < 0 || ::listen(m_listen, 1) < 0) + { + const std::string why = std::strerror(errno); + ::close(m_listen); + throw std::runtime_error("TcpReceiver: cannot listen on port " + std::to_string(port) + ": " + why); + } +} + +int TcpReceiver::connection_fd() const +{ + std::lock_guard lock(m_fd_mutex); + return m_connection; +} + +int TcpReceiver::control_fd() const +{ + std::lock_guard lock(m_fd_mutex); + return m_control; +} + +TcpReceiver::~TcpReceiver() +{ + if (m_connection >= 0) + ::close(m_connection); + if (m_control >= 0) + ::close(m_control); + if (m_listen >= 0) + ::close(m_listen); +} + +void TcpReceiver::accept_session() +{ + { + // Retire the earlier session under the lock a concurrent shutdown_session() takes, so it never sees a + // descriptor that is being closed. + std::lock_guard lock(m_fd_mutex); + for (int* fd : { &m_connection, &m_control }) + { + if (*fd >= 0) + ::close(*fd); + *fd = -1; + } + } + // Only this thread publishes descriptors, so it may read them here without the lock. + while (m_connection < 0 || m_control < 0) + { + const int fd = ::accept(m_listen, nullptr, nullptr); + if (fd < 0) + throw std::runtime_error(std::string("TcpReceiver: accept failed: ") + std::strerror(errno)); + const int one = 1; + ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + // Notice a link that died (Wi-Fi roam, unplugged cable) in seconds, as the sender does. + ::setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &one, sizeof(one)); + const int idle = 5, interval = 2, count = 3; + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &idle, sizeof(idle)); + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &interval, sizeof(interval)); + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &count, sizeof(count)); + const unsigned user_timeout_ms = 5000; + ::setsockopt(fd, IPPROTO_TCP, TCP_USER_TIMEOUT, &user_timeout_ms, sizeof(user_timeout_ms)); + // A peer that connects and never finishes its hello must not hold the worker: bound the wait, and let a stop + // reach this socket through m_pending. + timeval timeout{ 2, 0 }; + ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + { + std::lock_guard lock(m_fd_mutex); + m_pending = fd; + } + uint8_t hello[4]; + bool have_hello = false; + try + { + have_hello = read_exact(fd, hello, sizeof(hello), true); + } + catch (const std::exception&) + { + have_hello = false; + } + timeval none{ 0, 0 }; + ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &none, sizeof(none)); + { + // Hand the socket over (or close it) in the same critical section that stops advertising it as pending. + std::lock_guard lock(m_fd_mutex); + m_pending = -1; + if (have_hello && std::memcmp(hello, "DATA", 4) == 0 && m_connection < 0) + m_connection = fd; + else if (have_hello && std::memcmp(hello, "CTRL", 4) == 0 && m_control < 0) + m_control = fd; + else + ::close(fd); // no hello, an unknown one, or a duplicate + } + } +} + +void TcpReceiver::stop_listening() +{ + if (m_listen >= 0) + ::shutdown(m_listen, SHUT_RDWR); // makes a blocked accept() fail + std::lock_guard lock(m_fd_mutex); + if (m_pending >= 0) + ::shutdown(m_pending, SHUT_RDWR); // and a hello being read +} + +void TcpReceiver::shutdown_session() +{ + std::lock_guard lock(m_fd_mutex); + if (m_pending >= 0) + ::shutdown(m_pending, SHUT_RDWR); + for (const int fd : { m_connection, m_control }) + { + if (fd >= 0) + ::shutdown(fd, SHUT_RDWR); + } +} + +bool TcpReceiver::send_control(const uint8_t* data, size_t size) +{ + std::lock_guard lock(m_control_send); + const int fd = control_fd(); + size_t done = 0; + while (done < size) + { + const ssize_t n = ::send(fd, data + done, size - done, MSG_NOSIGNAL); + if (n < 0) + { + if (errno == EINTR) + continue; + return false; + } + done += static_cast(n); + } + return true; +} + +bool TcpReceiver::recv_control(uint8_t* out, size_t size) +{ + try + { + return read_exact(control_fd(), out, size, true); + } + catch (const std::exception&) + { + return false; + } +} + +size_t TcpReceiver::pending_bytes() const +{ + int pending = 0; + const int fd = connection_fd(); + if (fd < 0 || ::ioctl(fd, FIONREAD, &pending) < 0) + return 0; + return static_cast(pending); +} + +void TcpReceiver::request_keyframe() +{ + // Best effort: if the link is down the sender asks for a keyframe itself when it reconnects. + const uint8_t request = 'K'; + (void)send_control(&request, 1); +} + +bool TcpReceiver::read_record(std::vector& record) +{ + const int fd = connection_fd(); + uint8_t prefix[4]; + if (!read_exact(fd, prefix, 4, true)) + return false; + const uint32_t length = avs1::read_length(prefix); + if (length == 0 || length > avs1::kMaxRecordBytes) + throw std::runtime_error("TcpReceiver: invalid record length " + std::to_string(length)); + record.resize(length); + read_exact(fd, record.data(), length, false); + return true; +} + +} // namespace video_stage diff --git a/src/video_stage/app/tcp_receiver.hpp b/src/video_stage/app/tcp_receiver.hpp new file mode 100644 index 0000000000..e6abeed772 --- /dev/null +++ b/src/video_stage/app/tcp_receiver.hpp @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace video_stage +{ + +/// One listening TCP socket and one accepted connection that delivers length-prefixed AVS1 records. +class TcpReceiver +{ +public: + /// Listens on `port` (all interfaces). Throws std::runtime_error on failure. + explicit TcpReceiver(uint16_t port); + ~TcpReceiver(); + + TcpReceiver(const TcpReceiver&) = delete; + TcpReceiver& operator=(const TcpReceiver&) = delete; + + /// Blocks until the sender has opened both connections, each announced by a 4-byte hello: "DATA" carries the + /// records and "CTRL" the small control messages (keyframe requests and the clock exchange). TCP_NODELAY on both. + /// Any earlier session is closed: call this only after shutdown_session() and after the threads that read or + /// write the earlier session have stopped, since they use a copy of its descriptors. + void accept_session(); + + /// Ends a blocked accept_session() (it then throws or returns without a session) and stops accepting. + void stop_listening(); + + /// Unblocks every thread waiting in a read or write on this session. + void shutdown_session(); + + /// Control link: write all of `data` / read exactly `size` bytes. send_control is safe from several threads; + /// recv_control returns false when the link ended. + bool send_control(const uint8_t* data, size_t size); + bool recv_control(uint8_t* out, size_t size); + + /// Bytes already received and waiting in the socket (how far behind the reader is). + size_t pending_bytes() const; + + /// Asks the sender for a keyframe: one byte back on the connection. Safe to call from another thread. + void request_keyframe(); + + /// Reads one record's bytes (after the length prefix) into `record`. False on a clean end of stream; throws on a + /// truncated or oversized record. + bool read_record(std::vector& record); + +private: + /// The session descriptors have one owner at a time: this lock covers publishing a new one, retiring the old and + /// shutting them down from another thread. Blocking reads and writes happen outside it, on a copy. + int connection_fd() const; + int control_fd() const; + + int m_listen = -1; + mutable std::mutex m_fd_mutex; + int m_connection = -1; ///< DATA; guarded by m_fd_mutex + int m_control = -1; ///< CTRL; guarded by m_fd_mutex + int m_pending = -1; ///< an accepted socket whose hello is being read, visible to shutdown; guarded by m_fd_mutex + std::mutex m_control_send; +}; + +} // namespace video_stage diff --git a/src/video_stage/codec/CMakeLists.txt b/src/video_stage/codec/CMakeLists.txt new file mode 100644 index 0000000000..e755cc46e6 --- /dev/null +++ b/src/video_stage/codec/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# NVDEC decoder and NVENC encoder adapters over a shared CUDA context. +add_library(video_stage_codec STATIC + cuda_context.cpp + nvdec_decoder.cpp + nvenc_encoder.cpp +) +add_library(video_stage::codec ALIAS video_stage_codec) + +set_target_properties(video_stage_codec PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(video_stage_codec + INTERFACE + $ +) + +target_link_libraries(video_stage_codec + PUBLIC + video_stage::core + CUDA::cudart + CUDA::cuda_driver + PRIVATE + video_stage_nv_codec_headers + ${CMAKE_DL_LIBS} +) diff --git a/src/video_stage/codec/cuda_context.cpp b/src/video_stage/codec/cuda_context.cpp new file mode 100644 index 0000000000..714165b063 --- /dev/null +++ b/src/video_stage/codec/cuda_context.cpp @@ -0,0 +1,50 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/cuda_context.hpp" + +#include + +namespace video_stage +{ + +void check_cu(CUresult result, const char* what) +{ + if (result == CUDA_SUCCESS) + return; + const char* message = nullptr; + cuGetErrorString(result, &message); + throw std::runtime_error(std::string(what) + ": " + (message ? message : "unknown CUDA driver error")); +} + +CudaContext::CudaContext(int device_index) +{ + check_cu(cuInit(0), "cuInit"); + check_cu(cuDeviceGet(&m_device, device_index), "cuDeviceGet"); + char name[128] = {}; + check_cu(cuDeviceGetName(name, sizeof(name), m_device), "cuDeviceGetName"); + m_name = name; + check_cu(cuDevicePrimaryCtxRetain(&m_context, m_device), "cuDevicePrimaryCtxRetain"); + make_current(); + // Bind the runtime API to the same primary context. + if (cudaSetDevice(device_index) != cudaSuccess) + throw std::runtime_error("cudaSetDevice failed"); +} + +CudaContext::~CudaContext() +{ + if (m_context) + cuDevicePrimaryCtxRelease(m_device); +} + +void CudaContext::make_current_noexcept() const noexcept +{ + cuCtxSetCurrent(m_context); +} + +void CudaContext::make_current() const +{ + check_cu(cuCtxSetCurrent(m_context), "cuCtxSetCurrent"); +} + +} // namespace video_stage diff --git a/src/video_stage/codec/inc/video_stage/cuda_context.hpp b/src/video_stage/codec/inc/video_stage/cuda_context.hpp new file mode 100644 index 0000000000..5410a3e939 --- /dev/null +++ b/src/video_stage/codec/inc/video_stage/cuda_context.hpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace video_stage +{ + +/// Throws std::runtime_error naming `what` when a CUDA driver call failed. +void check_cu(CUresult result, const char* what); + +/** + * @brief The device's primary CUDA context, shared by decode, the GPU operation and encode. + * + * The runtime API (kernels, streams) and the driver API (NVDEC, NVENC) both use the primary context, so surfaces + * are visible to all of them. Every thread that touches CUDA calls make_current() once. + */ +class CudaContext +{ +public: + explicit CudaContext(int device_index = 0); + ~CudaContext(); + + CudaContext(const CudaContext&) = delete; + CudaContext& operator=(const CudaContext&) = delete; + + void make_current() const; + /// For cleanup paths that must not throw: the result is ignored. + void make_current_noexcept() const noexcept; + CUcontext handle() const + { + return m_context; + } + const std::string& device_name() const + { + return m_name; + } + +private: + CUdevice m_device = 0; + CUcontext m_context = nullptr; + std::string m_name; +}; + +} // namespace video_stage diff --git a/src/video_stage/codec/inc/video_stage/nvdec_decoder.hpp b/src/video_stage/codec/inc/video_stage/nvdec_decoder.hpp new file mode 100644 index 0000000000..fc41280e12 --- /dev/null +++ b/src/video_stage/codec/inc/video_stage/nvdec_decoder.hpp @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "cuda_context.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// What a decoded stream looks like: its size and its quantisation range. A change of either at a sequence header +/// recreates the decoder and is reported to the consumer. +struct StreamFormat +{ + Geometry geometry; + ColorInfo color; + + bool operator==(const StreamFormat& other) const + { + return geometry == other.geometry && color.full_range == other.color.full_range; + } +}; + +/// A decoded picture in device memory, still mapped to the decoder's output surface. Keep the lease until the work +/// that reads `view` has completed; releasing it returns the surface to the decoder. `key` is the value given with +/// the access unit that produced the picture. +struct DecodedFrame +{ + Nv12View view; + uint64_t key = 0; + std::shared_ptr lease; +}; + +/** + * @brief H.264 access units to NV12 device surfaces on NVDEC. + * + * decode() feeds one complete Annex-B access unit and returns after the parser has called back. With display delay 0 + * and no B-frames, a picture is delivered from the call that supplied its access unit. Pictures arrive in display + * order through the callback, on the calling thread. Every picture carries the key of its own access unit, so + * identity survives any reordering and a lost access unit cannot shift later keys. + * + * A change of coded size or format in the stream recreates the decoder at that access unit; on_format_change() + * tells the consumer, which must reprepare its operation for the new geometry before the next picture. + */ +class NvdecDecoder +{ +public: + using FrameCallback = std::function; + using FormatCallback = std::function; + + /// `stream` is the CUDA stream the decoded surfaces are made ready on; work enqueued on it afterwards is ordered + /// after the decode. + NvdecDecoder(CudaContext& context, cudaStream_t stream, FrameCallback on_frame, FormatCallback on_format_change); + ~NvdecDecoder(); + + NvdecDecoder(const NvdecDecoder&) = delete; + NvdecDecoder& operator=(const NvdecDecoder&) = delete; + + /// Throws std::runtime_error if the decoder rejects the data. + void decode(const uint8_t* access_unit, size_t size, uint64_t key); + + /// Pictures delivered so far and access units that produced none; the difference shows decoder-side loss. + uint64_t pictures() const; + uint64_t access_units() const; + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace video_stage diff --git a/src/video_stage/codec/inc/video_stage/nvenc_encoder.hpp b/src/video_stage/codec/inc/video_stage/nvenc_encoder.hpp new file mode 100644 index 0000000000..c63e05fcb2 --- /dev/null +++ b/src/video_stage/codec/inc/video_stage/nvenc_encoder.hpp @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "cuda_context.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +struct EncoderConfig +{ + Geometry geometry; + uint32_t fps_numerator = 30; + uint32_t fps_denominator = 1; + uint32_t bitrate_bps = 12'000'000; + /// Frames between IDRs; a stream recovers from loss only at an IDR, so keep it short. + uint32_t idr_period = 30; + bool full_range = false; + /// Reusable input surfaces, registered with the encoder once at startup. + uint32_t input_surfaces = 3; +}; + +/// One encoded access unit and the key of the input it came from. +struct EncodedPacket +{ + std::vector data; + uint64_t key = 0; + bool keyframe = false; +}; + +/** + * @brief NV12 device surfaces to H.264 access units on NVENC, tuned for low latency. + * + * The encoder owns a small pool of CUDA surfaces registered once; the GPU operation writes its output straight + * into one, so there is no copy and no per-frame registration. Low-latency settings: the ultra-low-latency tuning, + * no B-frames, no look-ahead, constant bitrate with a one-frame buffer, parameter sets repeated before every IDR. + * encode() returns the access unit for the picture it was given. + */ +class NvencEncoder +{ +public: + /// A pooled input surface. Write the picture into `view`, then pass the same `slot` to encode(). + struct Surface + { + Nv12View view; + uint32_t slot = 0; + }; + + NvencEncoder(CudaContext& context, const EncoderConfig& config); + ~NvencEncoder(); + + NvencEncoder(const NvencEncoder&) = delete; + NvencEncoder& operator=(const NvencEncoder&) = delete; + + /// The next free input surface, or nullopt-equivalent `false` when all are in flight. + bool acquire(Surface& out); + + /// Give back a surface acquired but not encoded (an error path). Idempotent, and encode() releases its own. + void release(const Surface& surface); + + /// Encode the picture written to `surface` after `stream` has completed the work that wrote it, and release the + /// surface. `key` comes back on the packet. `force_idr` makes this picture an IDR with parameter sets. + /// Throws std::runtime_error on an encoder error. + EncodedPacket encode(const Surface& surface, cudaStream_t stream, uint64_t key, bool force_idr); + +private: + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace video_stage diff --git a/src/video_stage/codec/nvdec_decoder.cpp b/src/video_stage/codec/nvdec_decoder.cpp new file mode 100644 index 0000000000..c10117afe7 --- /dev/null +++ b/src/video_stage/codec/nvdec_decoder.cpp @@ -0,0 +1,231 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/nvdec_decoder.hpp" + +#include + +#include +#include +#include +#include + +namespace video_stage +{ + +namespace +{ + +void check_cu_v(CUresult r, const char* what) +{ + check_cu(r, what); +} + +} // namespace + +struct NvdecDecoder::Impl +{ + CudaContext& context; + cudaStream_t stream; + FrameCallback on_frame; + FormatCallback on_format_change; + + CuvidFunctions* cvdl = nullptr; + CUvideoparser parser = nullptr; + CUvideodecoder decoder = nullptr; + CUVIDEOFORMAT format{}; + StreamFormat stream_format; + uint64_t pictures = 0; + uint64_t access_units = 0; + // Set by the callbacks so decode() can surface an error thrown inside the parser's C callbacks. + std::string callback_error; + + Impl(CudaContext& ctx, cudaStream_t s, FrameCallback f, FormatCallback fc) + : context(ctx), stream(s), on_frame(std::move(f)), on_format_change(std::move(fc)) + { + if (cuvid_load_functions(&cvdl, nullptr) < 0 || cvdl == nullptr) + throw std::runtime_error("NvdecDecoder: libnvcuvid is not available"); + context.make_current(); + CUVIDPARSERPARAMS params{}; + params.CodecType = cudaVideoCodec_H264; + // The parser only needs a minimum here; the sequence callback reports the real requirement. + params.ulMaxNumDecodeSurfaces = 1; + params.ulClockRate = 0; + params.ulErrorThreshold = 100; + params.ulMaxDisplayDelay = 0; // output as soon as decoded; the stream has no B-frames + params.pUserData = this; + params.pfnSequenceCallback = &Impl::sequence_cb; + params.pfnDecodePicture = &Impl::decode_cb; + params.pfnDisplayPicture = &Impl::display_cb; + check_cu_v(cvdl->cuvidCreateVideoParser(&parser, ¶ms), "cuvidCreateVideoParser"); + } + + ~Impl() + { + context.make_current(); + if (parser) + cvdl->cuvidDestroyVideoParser(parser); + destroy_decoder(); + cuvid_free_functions(&cvdl); + } + + void destroy_decoder() + { + if (decoder) + { + cvdl->cuvidDestroyDecoder(decoder); + decoder = nullptr; + } + } + + static int CUDAAPI sequence_cb(void* user, CUVIDEOFORMAT* fmt) + { + auto* self = static_cast(user); + try + { + return self->on_sequence(fmt); + } + catch (const std::exception& e) + { + self->callback_error = e.what(); + return 0; // 0 = failure: the parser stops + } + } + + int on_sequence(CUVIDEOFORMAT* fmt) + { + if (fmt->codec != cudaVideoCodec_H264 || fmt->chroma_format != cudaVideoChromaFormat_420 || + fmt->bit_depth_luma_minus8 != 0) + throw std::runtime_error("NvdecDecoder: only 8-bit 4:2:0 H.264 is supported"); + const unsigned width = static_cast(fmt->display_area.right - fmt->display_area.left); + const unsigned height = static_cast(fmt->display_area.bottom - fmt->display_area.top); + StreamFormat next; + next.geometry = Geometry{ width, height }; + next.color.full_range = fmt->video_signal_description.video_full_range_flag != 0; + const bool same = decoder != nullptr && format.coded_width == fmt->coded_width && + format.coded_height == fmt->coded_height && stream_format == next && + format.min_num_decode_surfaces == fmt->min_num_decode_surfaces; + if (same) + return static_cast(format.min_num_decode_surfaces); + + destroy_decoder(); + CUVIDDECODECREATEINFO info{}; + info.ulWidth = fmt->coded_width; + info.ulHeight = fmt->coded_height; + info.ulMaxWidth = fmt->coded_width; + info.ulMaxHeight = fmt->coded_height; + // The codec's own minimum plus margin so a consumer holding a surface does not stall the parser. + info.ulNumDecodeSurfaces = fmt->min_num_decode_surfaces + 2; + info.CodecType = fmt->codec; + info.ChromaFormat = fmt->chroma_format; + info.bitDepthMinus8 = fmt->bit_depth_luma_minus8; + info.ulCreationFlags = cudaVideoCreate_PreferCUVID; + info.OutputFormat = cudaVideoSurfaceFormat_NV12; + info.DeinterlaceMode = cudaVideoDeinterlaceMode_Weave; + info.ulTargetWidth = width; + info.ulTargetHeight = height; + info.display_area.left = static_cast(fmt->display_area.left); + info.display_area.top = static_cast(fmt->display_area.top); + info.display_area.right = static_cast(fmt->display_area.right); + info.display_area.bottom = static_cast(fmt->display_area.bottom); + info.ulNumOutputSurfaces = 3; + context.make_current(); + check_cu_v(cvdl->cuvidCreateDecoder(&decoder, &info), "cuvidCreateDecoder"); + format = *fmt; + stream_format = next; + if (on_format_change) + on_format_change(next); + return static_cast(info.ulNumDecodeSurfaces); + } + + static int CUDAAPI decode_cb(void* user, CUVIDPICPARAMS* pic) + { + auto* self = static_cast(user); + if (self->cvdl->cuvidDecodePicture(self->decoder, pic) != CUDA_SUCCESS) + { + self->callback_error = "cuvidDecodePicture failed"; + return 0; + } + return 1; + } + + static int CUDAAPI display_cb(void* user, CUVIDPARSERDISPINFO* disp) + { + auto* self = static_cast(user); + try + { + self->on_display(disp); + return 1; + } + catch (const std::exception& e) + { + self->callback_error = e.what(); + return 0; + } + } + + void on_display(CUVIDPARSERDISPINFO* disp) + { + CUVIDPROCPARAMS proc{}; + proc.progressive_frame = disp->progressive_frame; + proc.second_field = disp->repeat_first_field + 1; + proc.top_field_first = disp->top_field_first; + proc.unpaired_field = disp->repeat_first_field < 0; + proc.output_stream = stream; + CUdeviceptr dptr = 0; + unsigned pitch = 0; + check_cu_v(cvdl->cuvidMapVideoFrame(decoder, disp->picture_index, &dptr, &pitch, &proc), "cuvidMapVideoFrame"); + + DecodedFrame frame; + frame.key = static_cast(disp->timestamp); + frame.view.geometry = stream_format.geometry; + frame.view.y = reinterpret_cast(dptr); + // The decoder's NV12 output surface holds the chroma plane right after the luma plane, `target height` rows on. + frame.view.uv = frame.view.y + static_cast(pitch) * stream_format.geometry.height; + frame.view.y_pitch = pitch; + frame.view.uv_pitch = pitch; + frame.view.color = stream_format.color; + CuvidFunctions* cvdl_copy = cvdl; + CUvideodecoder dec = decoder; + frame.lease = std::shared_ptr(reinterpret_cast(dptr), [cvdl_copy, dec](void* p) + { cvdl_copy->cuvidUnmapVideoFrame(dec, reinterpret_cast(p)); }); + ++pictures; + on_frame(std::move(frame)); + } +}; + +NvdecDecoder::NvdecDecoder(CudaContext& context, cudaStream_t stream, FrameCallback on_frame, FormatCallback on_format_change) + : m_impl(std::make_unique(context, stream, std::move(on_frame), std::move(on_format_change))) +{ +} + +NvdecDecoder::~NvdecDecoder() = default; + +void NvdecDecoder::decode(const uint8_t* access_unit, size_t size, uint64_t key) +{ + Impl& d = *m_impl; + d.context.make_current(); + d.callback_error.clear(); + CUVIDSOURCEDATAPACKET packet{}; + packet.payload = access_unit; + packet.payload_size = size; + packet.flags = CUVID_PKT_TIMESTAMP | CUVID_PKT_ENDOFPICTURE; + packet.timestamp = static_cast(key); + ++d.access_units; + const CUresult r = d.cvdl->cuvidParseVideoData(d.parser, &packet); + if (!d.callback_error.empty()) + throw std::runtime_error("NvdecDecoder: " + d.callback_error); + check_cu_v(r, "cuvidParseVideoData"); +} + +uint64_t NvdecDecoder::pictures() const +{ + return m_impl->pictures; +} + +uint64_t NvdecDecoder::access_units() const +{ + return m_impl->access_units; +} + +} // namespace video_stage diff --git a/src/video_stage/codec/nvenc_encoder.cpp b/src/video_stage/codec/nvenc_encoder.cpp new file mode 100644 index 0000000000..bd19ce55c6 --- /dev/null +++ b/src/video_stage/codec/nvenc_encoder.cpp @@ -0,0 +1,284 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/nvenc_encoder.hpp" + +#include + +#include +#include +#include + +namespace video_stage +{ + +namespace +{ + +void check_nv(NVENCSTATUS status, const char* what) +{ + if (status != NV_ENC_SUCCESS) + throw std::runtime_error(std::string("NvencEncoder: ") + what + " failed with status " + std::to_string(status)); +} + +} // namespace + +struct NvencEncoder::Impl +{ + CudaContext& context; + EncoderConfig config; + + NvencFunctions* nvenc = nullptr; + NV_ENCODE_API_FUNCTION_LIST fn{}; + void* encoder = nullptr; + NV_ENC_OUTPUT_PTR bitstream = nullptr; + + struct Slot + { + CUdeviceptr ptr = 0; + size_t pitch = 0; + NV_ENC_REGISTERED_PTR registered = nullptr; + bool in_use = false; + }; + std::vector slots; + + Impl(CudaContext& ctx, const EncoderConfig& cfg) : context(ctx), config(cfg) + { + if (config.geometry.width == 0 || config.geometry.height == 0 || (config.geometry.width % 2) != 0 || + (config.geometry.height % 2) != 0) + throw std::invalid_argument("NvencEncoder: NV12 needs non-zero even dimensions"); + try + { + context.make_current(); + if (nvenc_load_functions(&nvenc, nullptr) < 0 || nvenc == nullptr) + throw std::runtime_error("NvencEncoder: libnvidia-encode is not available"); + fn.version = NV_ENCODE_API_FUNCTION_LIST_VER; + check_nv(nvenc->NvEncodeAPICreateInstance(&fn), "NvEncodeAPICreateInstance"); + + NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS open{}; + open.version = NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS_VER; + open.deviceType = NV_ENC_DEVICE_TYPE_CUDA; + open.device = context.handle(); + open.apiVersion = NVENCAPI_VERSION; + check_nv(fn.nvEncOpenEncodeSessionEx(&open, &encoder), "nvEncOpenEncodeSessionEx"); + + configure(); + allocate_surfaces(); + + NV_ENC_CREATE_BITSTREAM_BUFFER out{}; + out.version = NV_ENC_CREATE_BITSTREAM_BUFFER_VER; + check_nv(fn.nvEncCreateBitstreamBuffer(encoder, &out), "nvEncCreateBitstreamBuffer"); + bitstream = out.bitstreamBuffer; + } + catch (...) + { + // A constructor that throws does not run the destructor, so a failure part way releases what exists. + shutdown(); + throw; + } + } + + ~Impl() + { + shutdown(); + } + + // Releases the session, the registered surfaces and the loaded library; safe to call on a partly built object. + void shutdown() noexcept + { + context.make_current_noexcept(); + if (encoder) + { + for (Slot& s : slots) + { + if (s.registered) + fn.nvEncUnregisterResource(encoder, s.registered); + if (s.ptr) + cuMemFree(s.ptr); + s = Slot{}; + } + if (bitstream) + fn.nvEncDestroyBitstreamBuffer(encoder, bitstream); + fn.nvEncDestroyEncoder(encoder); + encoder = nullptr; + bitstream = nullptr; + } + if (nvenc) + nvenc_free_functions(&nvenc); + } + + void configure() + { + const GUID codec = NV_ENC_CODEC_H264_GUID; + const GUID preset = NV_ENC_PRESET_P1_GUID; + const NV_ENC_TUNING_INFO tuning = NV_ENC_TUNING_INFO_ULTRA_LOW_LATENCY; + + NV_ENC_PRESET_CONFIG preset_config{}; + preset_config.version = NV_ENC_PRESET_CONFIG_VER; + preset_config.presetCfg.version = NV_ENC_CONFIG_VER; + check_nv(fn.nvEncGetEncodePresetConfigEx(encoder, codec, preset, tuning, &preset_config), + "nvEncGetEncodePresetConfigEx"); + + NV_ENC_CONFIG enc_config = preset_config.presetCfg; + enc_config.version = NV_ENC_CONFIG_VER; + enc_config.gopLength = config.idr_period; + enc_config.frameIntervalP = 1; // no B-frames + enc_config.rcParams.rateControlMode = NV_ENC_PARAMS_RC_CBR; + enc_config.rcParams.averageBitRate = config.bitrate_bps; + enc_config.rcParams.maxBitRate = config.bitrate_bps; + // A buffer of one frame's worth of bits keeps the output close to the instantaneous rate. + const uint32_t frame_bits = static_cast(static_cast(config.bitrate_bps) * + config.fps_denominator / config.fps_numerator); + enc_config.rcParams.vbvBufferSize = frame_bits; + enc_config.rcParams.vbvInitialDelay = frame_bits; + enc_config.rcParams.lookaheadDepth = 0; + NV_ENC_CONFIG_H264& h264 = enc_config.encodeCodecConfig.h264Config; + h264.idrPeriod = config.idr_period; + h264.repeatSPSPPS = 1; + h264.chromaFormatIDC = 1; + h264.h264VUIParameters.videoSignalTypePresentFlag = 1; + h264.h264VUIParameters.videoFormat = NV_ENC_VUI_VIDEO_FORMAT_UNSPECIFIED; + h264.h264VUIParameters.videoFullRangeFlag = config.full_range ? 1 : 0; + + NV_ENC_INITIALIZE_PARAMS init{}; + init.version = NV_ENC_INITIALIZE_PARAMS_VER; + init.encodeGUID = codec; + init.presetGUID = preset; + init.tuningInfo = tuning; + init.encodeWidth = config.geometry.width; + init.encodeHeight = config.geometry.height; + init.darWidth = config.geometry.width; + init.darHeight = config.geometry.height; + init.maxEncodeWidth = config.geometry.width; + init.maxEncodeHeight = config.geometry.height; + init.frameRateNum = config.fps_numerator; + init.frameRateDen = config.fps_denominator; + init.enablePTD = 1; + init.encodeConfig = &enc_config; + check_nv(fn.nvEncInitializeEncoder(encoder, &init), "nvEncInitializeEncoder"); + } + + void allocate_surfaces() + { + slots.resize(config.input_surfaces); + const size_t rows = static_cast(config.geometry.height) * 3 / 2; + for (Slot& s : slots) + { + // One allocation: luma rows then chroma rows, the layout NVENC reads for NV12. + check_cu(cuMemAllocPitch(&s.ptr, &s.pitch, config.geometry.width, rows, 16), "cuMemAllocPitch"); + NV_ENC_REGISTER_RESOURCE reg{}; + reg.version = NV_ENC_REGISTER_RESOURCE_VER; + reg.resourceType = NV_ENC_INPUT_RESOURCE_TYPE_CUDADEVICEPTR; + reg.width = config.geometry.width; + reg.height = config.geometry.height; + reg.pitch = static_cast(s.pitch); + reg.resourceToRegister = reinterpret_cast(s.ptr); + reg.bufferFormat = NV_ENC_BUFFER_FORMAT_NV12; + reg.bufferUsage = NV_ENC_INPUT_IMAGE; + check_nv(fn.nvEncRegisterResource(encoder, ®), "nvEncRegisterResource"); + s.registered = reg.registeredResource; + } + } +}; + +NvencEncoder::NvencEncoder(CudaContext& context, const EncoderConfig& config) + : m_impl(std::make_unique(context, config)) +{ +} + +NvencEncoder::~NvencEncoder() = default; + +void NvencEncoder::release(const Surface& surface) +{ + m_impl->slots.at(surface.slot).in_use = false; +} + +bool NvencEncoder::acquire(Surface& out) +{ + Impl& e = *m_impl; + for (uint32_t i = 0; i < e.slots.size(); ++i) + { + Impl::Slot& s = e.slots[i]; + if (s.in_use) + continue; + s.in_use = true; + out.slot = i; + out.view.geometry = e.config.geometry; + out.view.color.full_range = e.config.full_range; + out.view.y = reinterpret_cast(s.ptr); + out.view.y_pitch = s.pitch; + out.view.uv = reinterpret_cast(s.ptr) + s.pitch * e.config.geometry.height; + out.view.uv_pitch = s.pitch; + return true; + } + return false; +} + +EncodedPacket NvencEncoder::encode(const Surface& surface, cudaStream_t stream, uint64_t key, bool force_idr) +{ + Impl& e = *m_impl; + Impl::Slot& slot = e.slots.at(surface.slot); + EncodedPacket packet; + try + { + e.context.make_current(); + // NVENC reads the surface directly; wait for this stream's work (the warp) to finish writing it. + if (cudaStreamSynchronize(stream) != cudaSuccess) + throw std::runtime_error("NvencEncoder: the GPU work feeding the encoder failed"); + + NV_ENC_MAP_INPUT_RESOURCE map{}; + map.version = NV_ENC_MAP_INPUT_RESOURCE_VER; + map.registeredResource = slot.registered; + check_nv(e.fn.nvEncMapInputResource(e.encoder, &map), "nvEncMapInputResource"); + // Unmap on every exit path: a registered surface that stays mapped cannot be reused or freed. + struct MapGuard + { + Impl& impl; + NV_ENC_INPUT_PTR mapped; + ~MapGuard() + { + impl.fn.nvEncUnmapInputResource(impl.encoder, mapped); + } + } map_guard{ e, map.mappedResource }; + + NV_ENC_PIC_PARAMS pic{}; + pic.version = NV_ENC_PIC_PARAMS_VER; + pic.inputBuffer = map.mappedResource; + pic.bufferFmt = map.mappedBufferFmt; + pic.inputWidth = e.config.geometry.width; + pic.inputHeight = e.config.geometry.height; + pic.inputPitch = static_cast(slot.pitch); + pic.outputBitstream = e.bitstream; + pic.pictureStruct = NV_ENC_PIC_STRUCT_FRAME; + pic.inputTimeStamp = key; + if (force_idr) + pic.encodePicFlags = NV_ENC_PIC_FLAG_FORCEIDR | NV_ENC_PIC_FLAG_OUTPUT_SPSPPS; + check_nv(e.fn.nvEncEncodePicture(e.encoder, &pic), "nvEncEncodePicture"); + + NV_ENC_LOCK_BITSTREAM lock{}; + lock.version = NV_ENC_LOCK_BITSTREAM_VER; + lock.outputBitstream = e.bitstream; + check_nv(e.fn.nvEncLockBitstream(e.encoder, &lock), "nvEncLockBitstream"); + struct LockGuard + { + Impl& impl; + ~LockGuard() + { + impl.fn.nvEncUnlockBitstream(impl.encoder, impl.bitstream); + } + } lock_guard{ e }; + const auto* bytes = static_cast(lock.bitstreamBufferPtr); + packet.data.assign(bytes, bytes + lock.bitstreamSizeInBytes); + packet.key = lock.outputTimeStamp; + packet.keyframe = lock.pictureType == NV_ENC_PIC_TYPE_IDR; + } + catch (...) + { + slot.in_use = false; + throw; + } + slot.in_use = false; + return packet; +} + +} // namespace video_stage diff --git a/src/video_stage/core/CMakeLists.txt b/src/video_stage/core/CMakeLists.txt new file mode 100644 index 0000000000..b21ee8c1f4 --- /dev/null +++ b/src/video_stage/core/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(video_stage_core STATIC + sine_warp.cu +) +add_library(video_stage::core ALIAS video_stage_core) + +set_target_properties(video_stage_core PROPERTIES + CUDA_SEPARABLE_COMPILATION OFF + POSITION_INDEPENDENT_CODE ON +) + +target_include_directories(video_stage_core + INTERFACE + $ +) + +target_link_libraries(video_stage_core + PUBLIC + CUDA::cudart +) diff --git a/src/video_stage/core/inc/video_stage/gpu_operation.hpp b/src/video_stage/core/inc/video_stage/gpu_operation.hpp new file mode 100644 index 0000000000..62f8008718 --- /dev/null +++ b/src/video_stage/core/inc/video_stage/gpu_operation.hpp @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "nv12.hpp" + +#include + +namespace video_stage +{ + +/** + * @brief One per-frame GPU image operation: NV12 in, NV12 out, fixed geometry. + * + * Contract: + * - prepare() runs once per input geometry, outside the frame loop. It may allocate and upload; enqueue() may not. + * - enqueue() only queues work on `stream` and returns. It never synchronises the device or the stream, and + * allocates nothing. When it returns, the output is not yet written: the caller records a CUDA event on + * `stream` after the call, and whoever consumes `out` waits on that event. + * - `in` and `out` are distinct surfaces; the operation is not in-place. `in` must stay valid until the work + * completes. `out` has output_geometry() and carries the input's color description. + * - Frame identity, timestamps and metadata are not an operation's business: they travel with the frame. + * - If enqueue() throws, earlier launches of that call may already be in flight on `stream`. The caller drains + * the stream (or keeps the surfaces leased) before it recycles `in` or `out`. + */ +class GpuOperation +{ +public: + virtual ~GpuOperation() = default; + + /// The geometry this operation produces for `input`. Constant for a given operation and input. + virtual Geometry output_geometry(const Geometry& input) const = 0; + + /// Prepare resources for frames of this input geometry. Throws std::invalid_argument if unsupported. + virtual void prepare(const Geometry& input) = 0; + + /// Queue the operation on `stream`. Throws std::logic_error if not prepared for in.geometry. + virtual void enqueue(const Nv12View& in, const Nv12View& out, cudaStream_t stream) = 0; +}; + +} // namespace video_stage diff --git a/src/video_stage/core/inc/video_stage/nv12.hpp b/src/video_stage/core/inc/video_stage/nv12.hpp new file mode 100644 index 0000000000..d5eb1cb14d --- /dev/null +++ b/src/video_stage/core/inc/video_stage/nv12.hpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace video_stage +{ + +/// Image size in luma pixels. Both dimensions are even, as NV12 requires. +struct Geometry +{ + uint32_t width = 0; + uint32_t height = 0; + + bool operator==(const Geometry& other) const + { + return width == other.width && height == other.height; + } +}; + +/// Color description carried unchanged from input to output. Only the quantisation range matters to an +/// operation here: it decides the value of a border fill (black is Y=16 limited-range, Y=0 full-range). +struct ColorInfo +{ + bool full_range = false; +}; + +/// A device-memory NV12 image: a luma plane followed by one interleaved chroma (U,V) plane at half +/// resolution in both directions. Pitches are in bytes and may exceed the row size. +/// +/// A view does not own its memory. The owner decides how long the planes stay valid; an operation only +/// reads or writes them on the stream it is given. +struct Nv12View +{ + uint8_t* y = nullptr; + uint8_t* uv = nullptr; + size_t y_pitch = 0; + size_t uv_pitch = 0; + Geometry geometry; + ColorInfo color; +}; + +} // namespace video_stage diff --git a/src/video_stage/core/inc/video_stage/sine_warp.hpp b/src/video_stage/core/inc/video_stage/sine_warp.hpp new file mode 100644 index 0000000000..76f6719441 --- /dev/null +++ b/src/video_stage/core/inc/video_stage/sine_warp.hpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "gpu_operation.hpp" + +namespace video_stage +{ + +/// Parameters of the example warp. The defaults are a visible but modest ripple. +struct SineWarpParams +{ + /// Peak horizontal displacement in luma pixels. Zero makes the operation an identity. + float amplitude_px = 8.0F; + /// Rows per full sine period. + float wavelength_px = 128.0F; + /// Phase in radians, applied at row 0. + float phase_rad = 0.0F; +}; + +/** + * @brief EXAMPLE operation: a horizontal sine ripple, not calibrated rectification. + * + * Destination-to-source mapping, in luma coordinates: + * source_x = x + amplitude * sin(2*pi * y / wavelength + phase), source_y = y + * Output pixel (x, y) is the bilinear sample of the input at (source_x, source_y). A source position outside + * the image gives the border fill (black for the declared range). Output size and color are unchanged. The warped + * picture no longer has the source camera's calibrated projection. + * + * Chroma is sampled from its own plane at the position the same mapping gives for the chroma sample's luma-space + * location, with the H.264 default siting: chroma sits on even luma columns and half way between luma rows. A + * replacement operation (the fisheye remap) keeps the same enqueue contract and swaps only the mapping. + */ +class SineWarp final : public GpuOperation +{ +public: + explicit SineWarp(const SineWarpParams& params = {}); + + Geometry output_geometry(const Geometry& input) const override; + void prepare(const Geometry& input) override; + void enqueue(const Nv12View& in, const Nv12View& out, cudaStream_t stream) override; + +private: + SineWarpParams m_params; + Geometry m_prepared; +}; + +} // namespace video_stage diff --git a/src/video_stage/core/sine_warp.cu b/src/video_stage/core/sine_warp.cu new file mode 100644 index 0000000000..d039512155 --- /dev/null +++ b/src/video_stage/core/sine_warp.cu @@ -0,0 +1,151 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/sine_warp.hpp" + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +namespace +{ + +constexpr float kTwoPi = 6.28318530717958647692F; +constexpr unsigned kBlockX = 32; +constexpr unsigned kBlockY = 8; + +struct WarpArgs +{ + float amplitude; + float cycles_per_row; // 1 / wavelength + float phase; +}; + +__device__ __forceinline__ float displaced_x(float x, float y, const WarpArgs& a) +{ + return x + a.amplitude * sinf(kTwoPi * y * a.cycles_per_row + a.phase); +} + +// Bilinear sample of one byte channel. `stride` is the byte distance between neighbouring samples of the channel +// (1 for luma, 2 for interleaved chroma). The caller has checked that (sx, sy) lies inside [0,w-1]x[0,h-1]. +__device__ __forceinline__ float bilinear(const uint8_t* plane, size_t pitch, int stride, int w, int h, float sx, float sy) +{ + const int x0 = static_cast(floorf(sx)); + const int y0 = static_cast(floorf(sy)); + const int x1 = min(x0 + 1, w - 1); + const int y1 = min(y0 + 1, h - 1); + const float fx = sx - static_cast(x0); + const float fy = sy - static_cast(y0); + const uint8_t* r0 = plane + static_cast(y0) * pitch; + const uint8_t* r1 = plane + static_cast(y1) * pitch; + const float top = (1.0F - fx) * r0[x0 * stride] + fx * r0[x1 * stride]; + const float bottom = (1.0F - fx) * r1[x0 * stride] + fx * r1[x1 * stride]; + return (1.0F - fy) * top + fy * bottom; +} + +__device__ __forceinline__ uint8_t to_byte(float v) +{ + return static_cast(fminf(fmaxf(v + 0.5F, 0.0F), 255.0F)); +} + +__global__ void warp_luma( + const uint8_t* in, size_t in_pitch, uint8_t* out, size_t out_pitch, int w, int h, WarpArgs args, uint8_t fill) +{ + const int x = blockIdx.x * blockDim.x + threadIdx.x; + const int y = blockIdx.y * blockDim.y + threadIdx.y; + if (x >= w || y >= h) + return; + const float sx = displaced_x(static_cast(x), static_cast(y), args); + const float sy = static_cast(y); + const bool inside = sx >= 0.0F && sx <= static_cast(w - 1); + out[static_cast(y) * out_pitch + x] = inside ? to_byte(bilinear(in, in_pitch, 1, w, h, sx, sy)) : fill; +} + +// Chroma sample (cx, cy) sits at luma position (2*cx, 2*cy + 0.5): co-sited with even luma columns, half way between +// luma rows. The mapping is applied there and the result is converted back to chroma-plane coordinates. +__global__ void warp_chroma(const uint8_t* in, size_t in_pitch, uint8_t* out, size_t out_pitch, int cw, int ch, WarpArgs args) +{ + const int cx = blockIdx.x * blockDim.x + threadIdx.x; + const int cy = blockIdx.y * blockDim.y + threadIdx.y; + if (cx >= cw || cy >= ch) + return; + const float luma_x = 2.0F * static_cast(cx); + const float luma_y = 2.0F * static_cast(cy) + 0.5F; + const float src_luma_x = displaced_x(luma_x, luma_y, args); + const float scx = 0.5F * src_luma_x; + const float scy = 0.5F * (luma_y - 0.5F); + uint8_t* dst = out + static_cast(cy) * out_pitch + 2 * cx; + // A positive predicate: a NaN coordinate fails it and takes the fill path instead of reaching the sampler. + if (!(scx >= 0.0F && scx <= static_cast(cw - 1))) + { + dst[0] = 128; + dst[1] = 128; + return; + } + dst[0] = to_byte(bilinear(in, in_pitch, 2, cw, ch, scx, scy)); + dst[1] = to_byte(bilinear(in + 1, in_pitch, 2, cw, ch, scx, scy)); +} + +void check(cudaError_t status, const char* what) +{ + if (status != cudaSuccess) + throw std::runtime_error(std::string("SineWarp: ") + what + ": " + cudaGetErrorString(status)); +} + +} // namespace + +SineWarp::SineWarp(const SineWarpParams& params) : m_params(params) +{ + const bool finite = + std::isfinite(params.amplitude_px) && std::isfinite(params.wavelength_px) && std::isfinite(params.phase_rad); + if (!finite || !(params.wavelength_px > 0.0F) || !std::isfinite(1.0F / params.wavelength_px)) + throw std::invalid_argument( + "SineWarp: amplitude, phase and wavelength must be finite and the wavelength positive"); +} + +Geometry SineWarp::output_geometry(const Geometry& input) const +{ + return input; +} + +void SineWarp::prepare(const Geometry& input) +{ + if (input.width == 0 || input.height == 0 || (input.width % 2) != 0 || (input.height % 2) != 0) + throw std::invalid_argument("SineWarp: NV12 needs non-zero even dimensions"); + m_prepared = input; +} + +void SineWarp::enqueue(const Nv12View& in, const Nv12View& out, cudaStream_t stream) +{ + if (m_prepared.width == 0 || !(in.geometry == m_prepared)) + throw std::logic_error("SineWarp: not prepared for this input geometry"); + if (!(out.geometry == output_geometry(in.geometry))) + throw std::invalid_argument("SineWarp: output geometry does not match"); + const auto bad = [](const Nv12View& v) + { return v.y == nullptr || v.uv == nullptr || v.y_pitch < v.geometry.width || v.uv_pitch < v.geometry.width; }; + if (bad(in) || bad(out)) + throw std::invalid_argument("SineWarp: surface has a null plane or a pitch below the row size"); + + const int w = static_cast(in.geometry.width); + const int h = static_cast(in.geometry.height); + const WarpArgs args{ m_params.amplitude_px, 1.0F / m_params.wavelength_px, m_params.phase_rad }; + const uint8_t fill = in.color.full_range ? 0 : 16; + + const dim3 block(kBlockX, kBlockY); + const dim3 luma_grid((w + kBlockX - 1) / kBlockX, (h + kBlockY - 1) / kBlockY); + warp_luma<<>>(in.y, in.y_pitch, out.y, out.y_pitch, w, h, args, fill); + check(cudaGetLastError(), "luma launch"); + + const int cw = w / 2; + const int ch = h / 2; + const dim3 chroma_grid((cw + kBlockX - 1) / kBlockX, (ch + kBlockY - 1) / kBlockY); + warp_chroma<<>>(in.uv, in.uv_pitch, out.uv, out.uv_pitch, cw, ch, args); + check(cudaGetLastError(), "chroma launch"); +} + +} // namespace video_stage diff --git a/src/video_stage/sender/CMakeLists.txt b/src/video_stage/sender/CMakeLists.txt new file mode 100644 index 0000000000..71332d739e --- /dev/null +++ b/src/video_stage/sender/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The Orin-side sender of AVS1 records to the PC video stage: plain POSIX sockets and threads, no CUDA, so the camera +# producer, the mock producer and the tests all link the same code. +add_library(video_stage_sender STATIC + direct_sender.cpp + inc/video_stage/direct_sender.hpp +) +add_library(video_stage::sender ALIAS video_stage_sender) + +set_target_properties(video_stage_sender PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(video_stage_sender + INTERFACE + $ +) + +target_link_libraries(video_stage_sender + PUBLIC + video_stage::wire + log_bridge::log_bridge_core +) + +if(BUILD_TESTING) + add_subdirectory(tests) +endif() diff --git a/src/video_stage/sender/direct_sender.cpp b/src/video_stage/sender/direct_sender.cpp new file mode 100644 index 0000000000..1797215edd --- /dev/null +++ b/src/video_stage/sender/direct_sender.cpp @@ -0,0 +1,486 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/direct_sender.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +namespace +{ + +std::shared_ptr logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.sensing.DirectSender"); +} + +bool recv_exact(int fd, uint8_t* out, std::size_t size) +{ + std::size_t done = 0; + while (done < size) + { + const ssize_t n = ::recv(fd, out + done, size - done, 0); + if (n == 0) + return false; + if (n < 0) + { + if (errno == EINTR) + continue; + return false; + } + done += static_cast(n); + } + return true; +} + +bool send_all(int fd, const uint8_t* data, std::size_t size) +{ + while (size > 0) + { + const ssize_t n = ::send(fd, data, size, MSG_NOSIGNAL); + if (n < 0) + { + if (errno == EINTR) + continue; + return false; + } + data += n; + size -= static_cast(n); + } + return true; +} + +} // namespace + +DirectSender::DirectSender(const DirectSenderConfig& config) + : m_config(config), m_host(config.host), m_port(config.port), m_has_target(!config.host.empty()) +{ + m_thread = std::thread(&DirectSender::run, this); +} + +DirectSender::~DirectSender() +{ + { + std::lock_guard lock(m_mutex); + m_stop = true; + } + m_stopping = true; + m_wake.notify_all(); + // Unblock a connect, send or recv that the worker thread is sitting in; it then sees m_stopping and exits. + { + std::lock_guard lock(m_fd_mutex); + for (const int fd : m_fds) + if (fd >= 0) + ::shutdown(fd, SHUT_RDWR); + } + if (m_thread.joinable()) + m_thread.join(); +} + +void DirectSender::flush_stale_locked() +{ + m_dropped += m_queue.size(); + m_queue.clear(); + m_queued_bytes = 0; + m_need_keyframe = true; + m_keyframe_requested = true; +} + +void DirectSender::set_target(const std::string& host, uint16_t port) +{ + { + std::lock_guard lock(m_target_mutex); + m_host = host; + m_port = host.empty() ? 0 : port; + m_has_target = !host.empty(); + ++m_target_generation; + } + set_error({}); + { + // Whatever waits was meant for the old destination, and the new one needs a keyframe first. + std::lock_guard lock(m_mutex); + m_queue.clear(); + m_queued_bytes = 0; + m_need_keyframe = true; + m_keyframe_requested = true; + } + { + // A connect, send or receive in progress is for the old destination: cut it so the worker moves on. + std::lock_guard lock(m_fd_mutex); + for (const int fd : m_fds) + if (fd >= 0) + ::shutdown(fd, SHUT_RDWR); + } + m_wake.notify_all(); +} + +std::string DirectSender::last_error() const +{ + std::lock_guard lock(m_error_mutex); + return m_last_error; +} + +void DirectSender::set_error(const std::string& error) +{ + std::lock_guard lock(m_error_mutex); + m_last_error = error; +} + +std::string DirectSender::host() const +{ + std::lock_guard lock(m_target_mutex); + return m_host; +} + +uint16_t DirectSender::port() const +{ + std::lock_guard lock(m_target_mutex); + return m_port; +} + +void DirectSender::current_target(std::string& host, uint16_t& port, uint64_t& generation) const +{ + std::lock_guard lock(m_target_mutex); + host = m_host; + port = m_port; + generation = m_target_generation; +} + +void DirectSender::submit(const EncodedVideoFrame& frame) +{ + // With no destination there is nothing to queue, and nothing to ask the encoder for. + if (!m_has_target.load()) + return; + std::lock_guard lock(m_mutex); + if (!m_queue.empty() && std::chrono::steady_clock::now() - m_queue.front().enqueued > m_config.max_queue_age) + flush_stale_locked(); + if (m_need_keyframe) + { + if (!frame.keyframe) + return; // a delta frame cannot start a stream; the keyframe request is already pending + m_need_keyframe = false; + } + if (m_queued_bytes + frame.size > m_config.max_queued_bytes) + { + // Dropping any frame breaks the chain after it: discard up to the next keyframe, and ask for one. + ++m_dropped; + m_need_keyframe = true; + m_keyframe_requested = true; + return; + } + video_stage::avs1::Sample sample; + sample.width = m_config.width; + sample.height = m_config.height; + sample.fps_numerator = m_config.fps; + sample.bundle_sequence = ++m_sequence; + sample.session_id = 1; + sample.frame_number = frame.frame_id; + sample.frame_timestamp_ns = frame.user_timestamp_us * 1000; + std::vector record; + video_stage::avs1::encode(sample, frame.data, frame.size, record); + m_queued_bytes += record.size(); + m_queue.push_back({ std::move(record), std::chrono::steady_clock::now() }); + m_wake.notify_one(); +} + +bool DirectSender::take_keyframe_request() +{ + return m_keyframe_requested.exchange(false); +} + +bool DirectSender::connect_once( + int& fd, int slot, const std::string& host, uint16_t port, uint64_t generation, std::string& why) +{ + addrinfo hints{}; + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + addrinfo* found = nullptr; + if (::getaddrinfo(host.c_str(), std::to_string(port).c_str(), &hints, &found) != 0) + { + why = "cannot resolve the address"; + return false; + } + fd = -1; + for (addrinfo* a = found; a != nullptr && fd < 0 && !m_stopping; a = a->ai_next) + { + const int s = ::socket(a->ai_family, a->ai_socktype, a->ai_protocol); + if (s < 0) + continue; + ::fcntl(s, F_SETFL, ::fcntl(s, F_GETFL, 0) | O_NONBLOCK); + // Set before connect so the handshake itself is bounded, and publish so the destructor can cancel it. + const unsigned user_timeout_ms = 5000; + ::setsockopt(s, IPPROTO_TCP, TCP_USER_TIMEOUT, &user_timeout_ms, sizeof(user_timeout_ms)); + { + std::lock_guard lock(m_fd_mutex); + m_fds[slot] = s; + } + bool ok = false; + int rc = ::connect(s, a->ai_addr, a->ai_addrlen); + if (rc == 0) + { + ok = true; + } + else if (errno == EINPROGRESS) + { + // Poll in short slices so a stop request is noticed within 100 ms. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (!m_stopping && m_target_generation.load() == generation) + { + pollfd pfd{ s, POLLOUT, 0 }; + const int n = ::poll(&pfd, 1, 100); + if (n < 0 && errno != EINTR) + { + why = std::strerror(errno); + break; + } + if (n > 0) + { + int err = 0; + socklen_t len = sizeof(err); + ::getsockopt(s, SOL_SOCKET, SO_ERROR, &err, &len); + if (err == 0) + ok = true; + else + why = std::strerror(err); + break; + } + if (std::chrono::steady_clock::now() >= deadline) + { + why = "connection timed out"; + break; + } + } + } + else + { + why = std::strerror(errno); + } + if (ok && !m_stopping && m_target_generation.load() == generation) + { + ::fcntl(s, F_SETFL, ::fcntl(s, F_GETFL, 0) & ~O_NONBLOCK); + fd = s; + break; + } + { + std::lock_guard lock(m_fd_mutex); + m_fds[slot] = -1; + ::close(s); + } + } + ::freeaddrinfo(found); + if (fd < 0) + { + if (m_stopping) + why = "stopping"; + return false; + } + // The link may be wired or Wi-Fi, so make it behave under both: send small packets at once, keep the kernel's send + // buffer small so a backlog stays in our queue (where its age is bounded) instead of hiding in the kernel, and + // notice a dead link in seconds, not minutes. + const int one = 1; + ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + const int send_buffer = 256 * 1024; + ::setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &send_buffer, sizeof(send_buffer)); + ::setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &one, sizeof(one)); + const int idle = 5, interval = 2, count = 3; + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &idle, sizeof(idle)); + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &interval, sizeof(interval)); + ::setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &count, sizeof(count)); + return true; +} + +// The control connection carries only small messages from the PC: a keyframe request ('K') and clock-exchange +// requests ('Q', answered with 'R'). This thread blocks in recv so t2 is stamped when a request actually arrives, not +// when the record sender next looks at the socket; it ends when the connection does. +void DirectSender::control_loop(int fd) +{ + m_control_ended = false; + run_control(fd); + m_control_ended = true; +} + +void DirectSender::run_control(int fd) +{ + while (true) + { + uint8_t type = 0; + if (!recv_exact(fd, &type, 1)) + return; + const int64_t t2 = monotonic_ns(); + if (type == 'K') + { + m_keyframe_requested = true; + } + else if (type == 'Q') + { + std::vector request(kClockSyncRequestSize); + request[0] = type; + if (!recv_exact(fd, request.data() + 1, request.size() - 1)) + return; + const auto parsed = parse_clock_sync_request(request); + if (!parsed) + continue; + std::vector reply = build_clock_sync_reply(*parsed, t2, 0); + set_clock_sync_reply_t3(reply, monotonic_ns()); + if (!send_all(fd, reply.data(), reply.size())) + return; + } + // Anything else is ignored: the PC sends nothing more. + } +} + +void DirectSender::run() +{ + int data_fd = -1; + int control_fd = -1; + bool logged_failure = false; + std::chrono::steady_clock::time_point last_failure_log; + std::thread control; + uint64_t connected_generation = 0; + const auto close_connection = [&] + { + if (control_fd >= 0) + ::shutdown(control_fd, SHUT_RDWR); // unblocks the control thread + if (control.joinable()) + control.join(); + // Close under the lock the destructor shuts down under, so it never touches a closed or reused descriptor. + std::lock_guard lock(m_fd_mutex); + if (data_fd >= 0) + ::close(data_fd); + if (control_fd >= 0) + ::close(control_fd); + data_fd = control_fd = -1; + m_fds[0] = m_fds[1] = -1; + m_connected = false; + }; + while (true) + { + { + std::unique_lock lock(m_mutex); + if (m_stop) + break; + } + std::string host; + uint16_t port = 0; + uint64_t generation = 0; + current_target(host, port, generation); + if (data_fd >= 0 && generation != connected_generation) + close_connection(); // retargeted: leave the old destination + if (host.empty()) + { + // No destination: idle until one is set. + std::unique_lock lock(m_mutex); + m_wake.wait_for(lock, std::chrono::milliseconds(200), [this] { return m_stop; }); + continue; + } + if (data_fd < 0) + { + std::string why = "the hello could not be sent"; + const bool up = connect_once(data_fd, 0, host, port, generation, why) && + send_all(data_fd, reinterpret_cast("DATA"), 4) && + connect_once(control_fd, 1, host, port, generation, why) && + send_all(control_fd, reinterpret_cast("CTRL"), 4); + if (!up) + { + if (m_stopping) + { + close_connection(); + break; + } + if (m_target_generation.load() != generation) + { + close_connection(); // retargeted while connecting: the failure belongs to the old destination + continue; + } + // Say so on the first failure and then every 30 s: a producer that cannot reach the PC is otherwise + // indistinguishable from one that never tried. + const auto now = std::chrono::steady_clock::now(); + if (!logged_failure || now - last_failure_log >= std::chrono::seconds(30)) + { + logger()->warn("cannot connect to {}:{}: {}; retrying every second", host, port, why); + logged_failure = true; + last_failure_log = now; + } + set_error("cannot connect to " + host + ":" + std::to_string(port) + ": " + why); + close_connection(); + std::unique_lock lock(m_mutex); + m_wake.wait_for(lock, std::chrono::seconds(1), [this] { return m_stop; }); + continue; + } + logged_failure = false; + connected_generation = generation; + m_connected = true; + set_error({}); + logger()->info("connected to {}:{}", host, port); + control = std::thread(&DirectSender::control_loop, this, control_fd); + // The PC needs a keyframe first: drop anything queued before the connection and ask for one. + std::lock_guard lock(m_mutex); + m_queue.clear(); + m_queued_bytes = 0; + m_need_keyframe = true; + m_keyframe_requested = true; + } + std::vector record; + { + std::unique_lock lock(m_mutex); + // Wake periodically so a control connection that ended is noticed even when no frames are arriving. + m_wake.wait_for(lock, std::chrono::milliseconds(200), [this] { return m_stop || !m_queue.empty(); }); + if (m_stop) + break; + if (m_queue.empty()) + { + if (!control_alive()) + { + // Only if this is still the destination: a retarget cuts the old connection on purpose. + if (m_target_generation.load() == generation) + set_error(host + ":" + std::to_string(port) + " closed the control connection"); + logger()->warn("{}:{} closed the control connection; reconnecting", host, port); + lock.unlock(); + close_connection(); + } + continue; + } + if (std::chrono::steady_clock::now() - m_queue.front().enqueued > m_config.max_queue_age) + { + // The link could not keep up: what waits is already late, so drop it rather than send it. + flush_stale_locked(); + continue; + } + record = std::move(m_queue.front().bytes); + m_queue.pop_front(); + m_queued_bytes -= record.size(); + } + if (send_all(data_fd, record.data(), record.size())) + { + ++m_sent; + continue; + } + if (m_target_generation.load() == generation) + { + logger()->warn("send to {}:{} failed; reconnecting", host, port); + set_error("sending to " + host + ":" + std::to_string(port) + " failed"); + } + close_connection(); + } + close_connection(); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/video_stage/sender/inc/video_stage/direct_sender.hpp b/src/video_stage/sender/inc/video_stage/direct_sender.hpp new file mode 100644 index 0000000000..339083c7f7 --- /dev/null +++ b/src/video_stage/sender/inc/video_stage/direct_sender.hpp @@ -0,0 +1,134 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +using video_stage::EncodedVideoFrame; + +struct DirectSenderConfig +{ + std::string host; + uint16_t port = 0; + uint32_t width = 0; + uint32_t height = 0; + uint32_t fps = 30; + /// Encoded bytes allowed to wait for the network. A frame that does not fit is dropped, and so is everything after + /// it up to the next keyframe, because those frames reference it. + std::size_t max_queued_bytes = 16U << 20; + /// A unit that has waited this long for the network is stale. On a slow or lossy link (Wi-Fi, a congested switch) a + /// backlog is worse than a gap: when the oldest queued unit is older than this, the whole queue is dropped and the + /// stream resumes at the next keyframe, so the PC never receives video that is already late. + std::chrono::milliseconds max_queue_age{ 250 }; +}; + +/** + * @brief Sends each encoded access unit to the PC over TCP as an AVS1 record, beside the LiveKit publication. + * + * submit() never blocks the encoder thread: it copies the unit into a bounded queue that a sender thread drains. The + * connection starts at a keyframe. When the link is down or the queue overflowed, units are discarded until the next + * keyframe and a keyframe is requested; take_keyframe_request() hands that request to the encoder's owner. The PC + * connects nothing back: this side connects and reconnects, so the worker can be restarted freely. + */ +class DirectSender +{ +public: + explicit DirectSender(const DirectSenderConfig& config); + ~DirectSender(); + + DirectSender(const DirectSender&) = delete; + DirectSender& operator=(const DirectSender&) = delete; + + /// Offer one unit. `frame.user_timestamp_us` and `frame.frame_id` travel as the record's stamp and frame number. + void submit(const EncodedVideoFrame& frame); + + /// True once after a (re)connection or a drop that needs a fresh keyframe. + bool take_keyframe_request(); + + /// Send to host:port from now on (empty host stops sending). Whatever is queued is discarded, the current + /// connection is cut, and the new destination starts at a keyframe. Safe to call from any thread. + void set_target(const std::string& host, std::uint16_t port); + std::string host() const; + /// Why the link is not up (the last connect or send failure), empty while it is up or not yet tried. + std::string last_error() const; + std::uint16_t port() const; + /// Both connections to the current destination are up. + bool connected() const + { + return m_connected.load(); + } + + std::uint64_t sent() const + { + return m_sent.load(); + } + std::uint64_t dropped() const + { + return m_dropped.load(); + } + +private: + void run(); + void control_loop(int fd); + void run_control(int fd); + /// Connects without blocking past a stop request; \p slot (0 data, 1 control) publishes the fd to the destructor. + bool connect_once( + int& fd, int slot, const std::string& host, std::uint16_t port, std::uint64_t generation, std::string& why); + void set_error(const std::string& error); + void current_target(std::string& host, std::uint16_t& port, std::uint64_t& generation) const; + /// Drops every queued unit and waits for a keyframe, asking the encoder for one. Caller holds m_mutex. + void flush_stale_locked(); + bool control_alive() const + { + return !m_control_ended.load(); + } + + DirectSenderConfig m_config; + mutable std::mutex m_target_mutex; + std::string m_host; ///< guarded by m_target_mutex + std::uint16_t m_port = 0; ///< guarded by m_target_mutex + std::atomic m_has_target{ false }; + std::atomic m_target_generation{ 0 }; + std::atomic m_connected{ false }; + mutable std::mutex m_error_mutex; + std::string m_last_error; ///< guarded by m_error_mutex + std::thread m_thread; + std::mutex m_mutex; + std::condition_variable m_wake; + struct Queued + { + std::vector bytes; + std::chrono::steady_clock::time_point enqueued; + }; + std::deque m_queue; + std::size_t m_queued_bytes = 0; + bool m_need_keyframe = true; ///< guarded by m_mutex + bool m_stop = false; ///< guarded by m_mutex + std::uint64_t m_sequence = 0; + std::atomic m_stopping{ false }; + std::mutex m_fd_mutex; + int m_fds[2] = { -1, -1 }; ///< live sockets, guarded by m_fd_mutex + std::atomic m_keyframe_requested{ false }; + std::atomic m_control_ended{ false }; + std::atomic m_sent{ 0 }; + std::atomic m_dropped{ 0 }; +}; + +} // namespace sensing +} // namespace plugins diff --git a/src/video_stage/sender/tests/CMakeLists.txt b/src/video_stage/sender/tests/CMakeLists.txt new file mode 100644 index 0000000000..4435988773 --- /dev/null +++ b/src/video_stage/sender/tests/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(video_stage_sender_tests + test_direct_sender.cpp + test_direct_sender_target.cpp +) + +target_link_libraries(video_stage_sender_tests PRIVATE video_stage::sender Catch2::Catch2WithMain) + +catch_discover_tests(video_stage_sender_tests) diff --git a/src/video_stage/sender/tests/test_direct_sender.cpp b/src/video_stage/sender/tests/test_direct_sender.cpp new file mode 100644 index 0000000000..6fd07ced39 --- /dev/null +++ b/src/video_stage/sender/tests/test_direct_sender.cpp @@ -0,0 +1,168 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Destroying a DirectSender must be bounded whatever its worker thread is blocked in: a connect that never +// completes, a send to a peer that does not read, or an idle connection. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using plugins::sensing::DirectSender; +using plugins::sensing::DirectSenderConfig; +using plugins::sensing::EncodedVideoFrame; + +namespace +{ + +using Clock = std::chrono::steady_clock; +constexpr auto kBound = std::chrono::seconds(2); + +class Listener +{ +public: + explicit Listener(int backlog) + { + m_fd = ::socket(AF_INET, SOCK_STREAM, 0); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + REQUIRE(::bind(m_fd, reinterpret_cast(&addr), sizeof(addr)) == 0); + REQUIRE(::listen(m_fd, backlog) == 0); + socklen_t len = sizeof(addr); + ::getsockname(m_fd, reinterpret_cast(&addr), &len); + m_port = ntohs(addr.sin_port); + } + + ~Listener() + { + ::close(m_fd); + for (const int fd : m_accepted) + ::close(fd); + } + + uint16_t port() const + { + return m_port; + } + + /// Accepts \p count connections and reads their 4-byte hellos, then never reads again. + bool accept_and_stall(int count) + { + for (int i = 0; i < count; ++i) + { + pollfd pfd{ m_fd, POLLIN, 0 }; + if (::poll(&pfd, 1, 5000) <= 0) + return false; + const int c = ::accept(m_fd, nullptr, nullptr); + if (c < 0) + return false; + m_accepted.push_back(c); + char hello[4]; + if (::recv(c, hello, sizeof(hello), MSG_WAITALL) != 4) + return false; + } + return true; + } + + /// Fills the accept queue with half-open clients so later SYNs are dropped and a connect hangs. + void saturate(std::vector& clients) + { + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + addr.sin_port = htons(m_port); + for (int i = 0; i < 8; ++i) + { + const int c = ::socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0); + ::connect(c, reinterpret_cast(&addr), sizeof(addr)); + clients.push_back(c); + } + } + +private: + int m_fd = -1; + uint16_t m_port = 0; + std::vector m_accepted; +}; + +DirectSenderConfig config_for(uint16_t port) +{ + DirectSenderConfig config; + config.host = "127.0.0.1"; + config.port = port; + config.width = 64; + config.height = 48; + config.max_queued_bytes = 512U << 20; + config.max_queue_age = std::chrono::hours(1); + return config; +} + +std::chrono::milliseconds time_destruction(std::unique_ptr sender) +{ + const auto start = Clock::now(); + sender.reset(); + return std::chrono::duration_cast(Clock::now() - start); +} + +} // namespace + +TEST_CASE("destroying a sender whose connect never completes is bounded", "[direct_sender]") +{ + Listener listener(0); + std::vector clients; + listener.saturate(clients); + + auto sender = std::make_unique(config_for(listener.port())); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); // let the worker reach its connect + + CHECK(time_destruction(std::move(sender)) < kBound); + for (const int fd : clients) + ::close(fd); +} + +TEST_CASE("destroying a sender blocked in send to a peer that does not read is bounded", "[direct_sender]") +{ + Listener listener(8); + auto sender = std::make_unique(config_for(listener.port())); + REQUIRE(listener.accept_and_stall(2)); + + // Far more than the socket buffers hold, so the worker ends up blocked in send(). + const std::vector unit(1U << 20, 0x65); + constexpr int kUnits = 100; + for (int i = 0; i < kUnits; ++i) + { + EncodedVideoFrame frame; + frame.data = unit.data(); + frame.size = unit.size(); + frame.keyframe = (i == 0); + frame.frame_id = static_cast(i); + sender->submit(frame); + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + REQUIRE(sender->sent() < kUnits); + + CHECK(time_destruction(std::move(sender)) < kBound); +} + +TEST_CASE("destroying a connected, idle sender is bounded", "[direct_sender]") +{ + Listener listener(8); + auto sender = std::make_unique(config_for(listener.port())); + REQUIRE(listener.accept_and_stall(2)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + CHECK(time_destruction(std::move(sender)) < kBound); +} diff --git a/src/video_stage/sender/tests/test_direct_sender_target.cpp b/src/video_stage/sender/tests/test_direct_sender_target.cpp new file mode 100644 index 0000000000..876584bac6 --- /dev/null +++ b/src/video_stage/sender/tests/test_direct_sender_target.cpp @@ -0,0 +1,548 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Retargeting a DirectSender at run time: the connection moves, the new destination starts at a keyframe, an empty +// host idles, and none of it can hang the caller or the destructor. + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using plugins::sensing::DirectSender; +using plugins::sensing::DirectSenderConfig; +using plugins::sensing::EncodedVideoFrame; +using namespace std::chrono_literals; + +namespace +{ + +using Clock = std::chrono::steady_clock; + +/// A loopback receiver that accepts the sender's DATA and CTRL connections and reads AVS1 records from DATA. +class Peer +{ +public: + explicit Peer(int backlog = 8) + { + m_listen = ::socket(AF_INET, SOCK_STREAM, 0); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + REQUIRE(::bind(m_listen, reinterpret_cast(&addr), sizeof(addr)) == 0); + REQUIRE(::listen(m_listen, backlog) == 0); + socklen_t len = sizeof(addr); + ::getsockname(m_listen, reinterpret_cast(&addr), &len); + m_port = ntohs(addr.sin_port); + } + + ~Peer() + { + ::close(m_listen); + close_connections(); + } + + std::uint16_t port() const + { + return m_port; + } + + /// Accepts the DATA and CTRL connections; true when both hellos arrived. + bool accept_pair(int timeout_ms = 5000) + { + for (int i = 0; i < 2; ++i) + { + pollfd pfd{ m_listen, POLLIN, 0 }; + if (::poll(&pfd, 1, timeout_ms) <= 0) + return false; + const int fd = ::accept(m_listen, nullptr, nullptr); + if (fd < 0) + return false; + char hello[4]; + if (!read_exact(fd, reinterpret_cast(hello), 4, timeout_ms)) + { + ::close(fd); + return false; + } + const std::string tag(hello, 4); + if (tag == "DATA") + m_data = fd; + else if (tag == "CTRL") + m_control = fd; + else + { + ::close(fd); + return false; + } + } + return m_data >= 0 && m_control >= 0; + } + + /// One AVS1 record's bytes after the length prefix; nullopt on EOF or timeout. + std::optional> read_record(int timeout_ms = 5000) + { + std::uint8_t prefix[4]; + if (m_data < 0 || !read_exact(m_data, prefix, 4, timeout_ms)) + return std::nullopt; + std::vector body(video_stage::avs1::read_length(prefix)); + if (!read_exact(m_data, body.data(), body.size(), timeout_ms)) + return std::nullopt; + return body; + } + + /// True if the DATA connection was closed by the sender within the timeout (records still in flight are drained). + bool data_closed(int timeout_ms = 5000) + { + const auto until = Clock::now() + std::chrono::milliseconds(timeout_ms); + while (Clock::now() < until) + { + pollfd pfd{ m_data, POLLIN, 0 }; + if (::poll(&pfd, 1, 100) > 0) + { + std::uint8_t buffer[65536]; + const ssize_t n = ::recv(m_data, buffer, sizeof(buffer), 0); + if (n <= 0) + return true; + } + } + return false; + } + + /// Whether any new connection arrives within the timeout. + bool any_connection(int timeout_ms) + { + pollfd pfd{ m_listen, POLLIN, 0 }; + return ::poll(&pfd, 1, timeout_ms) > 0; + } + + /// The first record of the DATA stream must carry an IDR: the PC cannot start at a delta frame. + bool first_record_is_keyframe() + { + const auto body = read_record(); + if (!body) + return false; + std::string error; + const auto record = video_stage::avs1::parse(body->data(), body->size(), error); + return record && video_stage::avs1::contains_idr(record->payload, record->payload_size); + } + + void close_connections() + { + for (int* fd : { &m_data, &m_control }) + { + if (*fd >= 0) + ::close(*fd); + *fd = -1; + } + } + + /// Fills the accept queue with half-open clients so later SYNs are dropped and a connect hangs. + void saturate(std::vector& clients) + { + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + addr.sin_port = htons(m_port); + for (int i = 0; i < 8; ++i) + { + const int c = ::socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0); + ::connect(c, reinterpret_cast(&addr), sizeof(addr)); + clients.push_back(c); + } + } + +private: + static bool read_exact(int fd, std::uint8_t* out, std::size_t size, int timeout_ms) + { + std::size_t done = 0; + while (done < size) + { + pollfd pfd{ fd, POLLIN, 0 }; + if (::poll(&pfd, 1, timeout_ms) <= 0) + return false; + const ssize_t n = ::recv(fd, out + done, size - done, 0); + if (n <= 0) + return false; + done += static_cast(n); + } + return true; + } + + int m_listen = -1; + int m_data = -1; + int m_control = -1; + std::uint16_t m_port = 0; +}; + +/// Plays the encoder: a unit every few milliseconds, a keyframe every tenth and whenever the sender asks. +class Feeder +{ +public: + Feeder(DirectSender& sender, std::size_t unit_bytes = 2000) : m_sender(sender), m_unit_bytes(unit_bytes) + { + m_thread = std::thread( + [this] + { + std::uint32_t id = 0; + bool force_key = true; + while (!m_stop) + { + force_key |= m_sender.take_keyframe_request(); + const bool key = force_key || (id % 10 == 0); + force_key = false; + std::vector unit = { 0, 0, 0, 1, static_cast(key ? 0x65 : 0x41), 0x88 }; + unit.resize(m_unit_bytes, 0x5a); + EncodedVideoFrame frame; + frame.data = unit.data(); + frame.size = unit.size(); + frame.keyframe = key; + frame.frame_id = ++id; + frame.user_timestamp_us = id * 1000ULL; + m_sender.submit(frame); + std::this_thread::sleep_for(5ms); + } + }); + } + ~Feeder() + { + m_stop = true; + m_thread.join(); + } + +private: + DirectSender& m_sender; + std::size_t m_unit_bytes; + std::atomic m_stop{ false }; + std::thread m_thread; +}; + +DirectSenderConfig config_for(const std::string& host, std::uint16_t port) +{ + DirectSenderConfig config; + config.host = host; + config.port = port; + config.width = 64; + config.height = 48; + config.max_queued_bytes = 512U << 20; + config.max_queue_age = std::chrono::hours(1); + return config; +} + +template +bool wait_for(Pred pred, std::chrono::milliseconds limit = 5s) +{ + const auto until = Clock::now() + limit; + while (Clock::now() < until) + { + if (pred()) + return true; + std::this_thread::sleep_for(5ms); + } + return pred(); +} + +} // namespace + +TEST_CASE("the configured target is reported by host() and port()", "[direct_sender_target]") +{ + Peer a; + DirectSender sender(config_for("127.0.0.1", a.port())); + CHECK(sender.host() == "127.0.0.1"); + CHECK(sender.port() == a.port()); + REQUIRE(a.accept_pair()); + CHECK(wait_for([&] { return sender.connected(); })); +} + +TEST_CASE("retargeting moves the connection and the new peer starts at a keyframe", "[direct_sender_target]") +{ + Peer a; + Peer b; + DirectSender sender(config_for("127.0.0.1", a.port())); + Feeder feeder(sender); + + REQUIRE(a.accept_pair()); + CHECK(a.first_record_is_keyframe()); + REQUIRE(wait_for([&] { return sender.connected(); })); + + sender.set_target("127.0.0.1", b.port()); + CHECK(sender.host() == "127.0.0.1"); + CHECK(sender.port() == b.port()); + + REQUIRE(b.accept_pair()); + CHECK(b.first_record_is_keyframe()); + CHECK(a.data_closed()); // the old destination was left + REQUIRE(wait_for([&] { return sender.connected(); })); + + // The new peer keeps receiving, and the old one gets nothing more and no reconnect. + for (int i = 0; i < 5; ++i) + CHECK(b.read_record()); + CHECK_FALSE(a.any_connection(300)); +} + +TEST_CASE("setting the same peer again reconnects it from a keyframe", "[direct_sender_target]") +{ + Peer a; + DirectSender sender(config_for("127.0.0.1", a.port())); + Feeder feeder(sender); + REQUIRE(a.accept_pair()); + REQUIRE(a.first_record_is_keyframe()); + + sender.set_target("127.0.0.1", a.port()); + REQUIRE(a.data_closed()); + a.close_connections(); + REQUIRE(a.accept_pair()); + CHECK(a.first_record_is_keyframe()); +} + +TEST_CASE("an empty host idles the sender", "[direct_sender_target]") +{ + Peer a; + DirectSender sender(config_for("127.0.0.1", a.port())); + Feeder feeder(sender); + REQUIRE(a.accept_pair()); + REQUIRE(wait_for([&] { return sender.connected(); })); + + sender.set_target("", 1234); + CHECK(sender.host().empty()); + CHECK(sender.port() == 0); + CHECK(wait_for([&] { return !sender.connected(); })); + CHECK(a.data_closed()); + + // Idle: no reconnect, and submitted units are not queued or counted as sent. + CHECK_FALSE(a.any_connection(500)); + const auto sent = sender.sent(); + std::this_thread::sleep_for(200ms); + CHECK(sender.sent() == sent); + CHECK_FALSE(sender.connected()); + + // A target set later wakes it again. + Peer b; + sender.set_target("127.0.0.1", b.port()); + REQUIRE(b.accept_pair()); + CHECK(b.first_record_is_keyframe()); + CHECK(wait_for([&] { return sender.connected(); })); +} + +TEST_CASE("a sender created without a target idles until one is set", "[direct_sender_target]") +{ + DirectSender sender(config_for("", 0)); + Feeder feeder(sender); + std::this_thread::sleep_for(300ms); + CHECK_FALSE(sender.connected()); + CHECK(sender.sent() == 0); + CHECK(sender.host().empty()); + + Peer a; + sender.set_target("127.0.0.1", a.port()); + REQUIRE(a.accept_pair()); + CHECK(a.first_record_is_keyframe()); +} + +TEST_CASE("set_target while a send is blocked does not hang and the new peer gets video", "[direct_sender_target]") +{ + Peer stalled; + Peer b; + DirectSender sender(config_for("127.0.0.1", stalled.port())); + REQUIRE(stalled.accept_pair()); + + // Units far larger than the socket buffers, to a peer that never reads: the worker ends up blocked in send(). + const std::vector unit = [] + { + std::vector bytes(1U << 20, 0x5a); + bytes[0] = 0; + bytes[1] = 0; + bytes[2] = 0; + bytes[3] = 1; + bytes[4] = 0x65; + return bytes; + }(); + for (int i = 0; i < 100; ++i) + { + EncodedVideoFrame frame; + frame.data = unit.data(); + frame.size = unit.size(); + frame.keyframe = (i == 0); + frame.frame_id = static_cast(i); + sender.submit(frame); + } + std::this_thread::sleep_for(500ms); + REQUIRE(sender.sent() < 100); + + const auto start = Clock::now(); + sender.set_target("127.0.0.1", b.port()); + CHECK(Clock::now() - start < 1s); + + Feeder feeder(sender); + REQUIRE(b.accept_pair()); + CHECK(b.first_record_is_keyframe()); +} + +TEST_CASE("destruction is bounded after a retarget", "[direct_sender_target]") +{ + const auto time_destruction = [](std::unique_ptr sender) + { + const auto start = Clock::now(); + sender.reset(); + return Clock::now() - start; + }; + + SECTION("to a peer that is connected") + { + Peer a; + Peer b; + auto sender = std::make_unique(config_for("127.0.0.1", a.port())); + REQUIRE(a.accept_pair()); + sender->set_target("127.0.0.1", b.port()); + REQUIRE(b.accept_pair()); + std::this_thread::sleep_for(200ms); + CHECK(time_destruction(std::move(sender)) < 2s); + } + SECTION("to a peer whose connect never completes") + { + Peer a; + Peer hung(0); + std::vector clients; + hung.saturate(clients); + auto sender = std::make_unique(config_for("127.0.0.1", a.port())); + REQUIRE(a.accept_pair()); + sender->set_target("127.0.0.1", hung.port()); + std::this_thread::sleep_for(500ms); // let the worker reach its connect + CHECK(time_destruction(std::move(sender)) < 2s); + for (const int fd : clients) + ::close(fd); + } + SECTION("while idle after an empty host") + { + Peer a; + auto sender = std::make_unique(config_for("127.0.0.1", a.port())); + REQUIRE(a.accept_pair()); + sender->set_target("", 0); + std::this_thread::sleep_for(300ms); + CHECK(time_destruction(std::move(sender)) < 2s); + } +} + +TEST_CASE("retargeting away from a connect that never completes is prompt", "[direct_sender_target]") +{ + Peer hung(0); + std::vector clients; + hung.saturate(clients); + Peer b; + DirectSender sender(config_for("127.0.0.1", hung.port())); + Feeder feeder(sender); + std::this_thread::sleep_for(500ms); + + sender.set_target("127.0.0.1", b.port()); + REQUIRE(b.accept_pair(3000)); // well inside the 5 s connect timeout + CHECK(b.first_record_is_keyframe()); + for (const int fd : clients) + ::close(fd); +} + +namespace +{ + +/// A loopback port nothing listens on: bound once to learn a free number, then released. +std::uint16_t closed_port() +{ + const int s = ::socket(AF_INET, SOCK_STREAM, 0); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + REQUIRE(::bind(s, reinterpret_cast(&addr), sizeof(addr)) == 0); + socklen_t len = sizeof(addr); + ::getsockname(s, reinterpret_cast(&addr), &len); + ::close(s); + return ntohs(addr.sin_port); +} + +bool starts_with(const std::string& text, const std::string& prefix) +{ + return text.compare(0, prefix.size(), prefix) == 0; +} + +} // namespace + +TEST_CASE("a connect to a closed port is reported by last_error", "[direct_sender_target][last_error]") +{ + const auto port = closed_port(); + DirectSender sender(config_for("127.0.0.1", port)); + REQUIRE(wait_for([&] { return !sender.last_error().empty(); })); + const auto error = sender.last_error(); + CHECK(starts_with(error, "cannot connect to 127.0.0.1:" + std::to_string(port) + ": ")); + CHECK_FALSE(sender.connected()); +} + +TEST_CASE("a successful connection leaves last_error empty", "[direct_sender_target][last_error]") +{ + Peer a; + DirectSender sender(config_for("127.0.0.1", a.port())); + REQUIRE(a.accept_pair()); + REQUIRE(wait_for([&] { return sender.connected(); })); + CHECK(sender.last_error().empty()); +} + +TEST_CASE("set_target to a reachable peer clears a connect error", "[direct_sender_target][last_error]") +{ + Peer a; + DirectSender sender(config_for("127.0.0.1", closed_port())); + REQUIRE(wait_for([&] { return !sender.last_error().empty(); })); + + sender.set_target("127.0.0.1", a.port()); + REQUIRE(a.accept_pair()); + REQUIRE(wait_for([&] { return sender.connected(); })); + CHECK(sender.last_error().empty()); +} + +TEST_CASE("set_target to an empty host clears last_error and it stays clear", "[direct_sender_target][last_error]") +{ + DirectSender sender(config_for("127.0.0.1", closed_port())); + REQUIRE(wait_for([&] { return !sender.last_error().empty(); })); + + sender.set_target("", 0); + CHECK(sender.last_error().empty()); + std::this_thread::sleep_for(300ms); + CHECK(sender.last_error().empty()); +} + +TEST_CASE("set_target to another closed port reports the new destination", "[direct_sender_target][last_error]") +{ + DirectSender sender(config_for("127.0.0.1", closed_port())); + REQUIRE(wait_for([&] { return !sender.last_error().empty(); })); + + const auto second = closed_port(); + sender.set_target("127.0.0.1", second); + REQUIRE(wait_for( + [&] { return starts_with(sender.last_error(), "cannot connect to 127.0.0.1:" + std::to_string(second)); })); +} + +TEST_CASE("a retarget away from a peer that then closes leaves no stale control error", + "[direct_sender_target][last_error]") +{ + Peer a; + Peer b; + DirectSender sender(config_for("127.0.0.1", a.port())); + REQUIRE(a.accept_pair()); + REQUIRE(wait_for([&] { return sender.connected(); })); + + sender.set_target("127.0.0.1", b.port()); + a.close_connections(); + REQUIRE(b.accept_pair()); + REQUIRE(wait_for([&] { return sender.connected(); })); + std::this_thread::sleep_for(500ms); + CHECK(sender.last_error().find("closed the control connection") == std::string::npos); + CHECK(sender.last_error().empty()); +} diff --git a/src/video_stage/tests/CMakeLists.txt b/src/video_stage/tests/CMakeLists.txt new file mode 100644 index 0000000000..002197097e --- /dev/null +++ b/src/video_stage/tests/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(video_stage_tests + test_sine_warp.cpp +) + +target_link_libraries(video_stage_tests PRIVATE video_stage::core Catch2::Catch2WithMain) + +catch_discover_tests(video_stage_tests) + +# The receiver is part of the worker executable, not a library, so its source is compiled in directly. It needs +# only POSIX sockets and the AVS1 codec: no CUDA device. +add_executable(video_stage_tcp_receiver_tests + test_tcp_receiver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../app/tcp_receiver.cpp +) + +target_include_directories(video_stage_tcp_receiver_tests PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../app) +target_link_libraries(video_stage_tcp_receiver_tests PRIVATE video_stage::wire Catch2::Catch2WithMain) + +catch_discover_tests(video_stage_tcp_receiver_tests) diff --git a/src/video_stage/tests/test_sine_warp.cpp b/src/video_stage/tests/test_sine_warp.cpp new file mode 100644 index 0000000000..49e4f089af --- /dev/null +++ b/src/video_stage/tests/test_sine_warp.cpp @@ -0,0 +1,540 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using video_stage::ColorInfo; +using video_stage::Geometry; +using video_stage::Nv12View; +using video_stage::SineWarp; +using video_stage::SineWarpParams; + +namespace +{ + +constexpr uint8_t kPadding = 0xAB; +constexpr float kTwoPi = 6.28318530717958647692F; + +void cuda_ok(cudaError_t status) +{ + if (status != cudaSuccess) + FAIL("CUDA error: " << cudaGetErrorString(status)); +} + +bool have_cuda_device() +{ + int count = 0; + return cudaGetDeviceCount(&count) == cudaSuccess && count > 0; +} + +// Host-side NV12 image with tightly packed rows. +struct HostNv12 +{ + uint32_t w = 0; + uint32_t h = 0; + std::vector y; // w * h + std::vector uv; // w * (h / 2), interleaved U,V +}; + +HostNv12 make_image(uint32_t w, uint32_t h) +{ + HostNv12 img; + img.w = w; + img.h = h; + img.y.resize(static_cast(w) * h); + img.uv.resize(static_cast(w) * (h / 2)); + for (uint32_t y = 0; y < h; ++y) + for (uint32_t x = 0; x < w; ++x) + img.y[y * w + x] = static_cast(20 + (3 * x + 2 * y + ((x * y) % 7)) % 200); + for (uint32_t cy = 0; cy < h / 2; ++cy) + for (uint32_t cx = 0; cx < w / 2; ++cx) + { + img.uv[cy * w + 2 * cx] = static_cast(30 + (5 * cx + cy) % 190); // U + img.uv[cy * w + 2 * cx + 1] = static_cast(220 - (2 * cx + 4 * cy) % 180); // V + } + return img; +} + +// Device NV12 surface with pitch deliberately larger than the row size. +class DeviceSurface +{ +public: + DeviceSurface(uint32_t w, uint32_t h, bool full_range = false) : m_w(w), m_h(h) + { + cuda_ok(cudaMallocPitch(reinterpret_cast(&m_y), &m_y_pitch, w + 64, h)); + cuda_ok(cudaMallocPitch(reinterpret_cast(&m_uv), &m_uv_pitch, w + 64, h / 2)); + REQUIRE(m_y_pitch > w); + REQUIRE(m_uv_pitch > w); + cuda_ok(cudaMemset2D(m_y, m_y_pitch, kPadding, m_y_pitch, h)); + cuda_ok(cudaMemset2D(m_uv, m_uv_pitch, kPadding, m_uv_pitch, h / 2)); + m_color.full_range = full_range; + } + ~DeviceSurface() + { + cudaFree(m_y); + cudaFree(m_uv); + } + DeviceSurface(const DeviceSurface&) = delete; + DeviceSurface& operator=(const DeviceSurface&) = delete; + + void upload(const HostNv12& img) + { + cuda_ok(cudaMemcpy2D(m_y, m_y_pitch, img.y.data(), m_w, m_w, m_h, cudaMemcpyHostToDevice)); + cuda_ok(cudaMemcpy2D(m_uv, m_uv_pitch, img.uv.data(), m_w, m_w, m_h / 2, cudaMemcpyHostToDevice)); + } + + // Copies the visible area out and checks that the row padding was left untouched. + HostNv12 download() const + { + HostNv12 img; + img.w = m_w; + img.h = m_h; + img.y.resize(static_cast(m_w) * m_h); + img.uv.resize(static_cast(m_w) * (m_h / 2)); + cuda_ok(cudaMemcpy2D(img.y.data(), m_w, m_y, m_y_pitch, m_w, m_h, cudaMemcpyDeviceToHost)); + cuda_ok(cudaMemcpy2D(img.uv.data(), m_w, m_uv, m_uv_pitch, m_w, m_h / 2, cudaMemcpyDeviceToHost)); + check_padding(m_y, m_y_pitch, m_h); + check_padding(m_uv, m_uv_pitch, m_h / 2); + return img; + } + + Nv12View view() const + { + Nv12View v; + v.y = m_y; + v.uv = m_uv; + v.y_pitch = m_y_pitch; + v.uv_pitch = m_uv_pitch; + v.geometry = { m_w, m_h }; + v.color = m_color; + return v; + } + +private: + void check_padding(const uint8_t* plane, size_t pitch, uint32_t rows) const + { + std::vector pad(pitch - m_w); + for (uint32_t r = 0; r < rows; ++r) + { + cuda_ok(cudaMemcpy(pad.data(), plane + r * pitch + m_w, pad.size(), cudaMemcpyDeviceToHost)); + for (uint8_t b : pad) + REQUIRE(b == kPadding); + } + } + + uint32_t m_w, m_h; + uint8_t* m_y = nullptr; + uint8_t* m_uv = nullptr; + size_t m_y_pitch = 0, m_uv_pitch = 0; + ColorInfo m_color; +}; + +// CPU reference of the documented mapping. +float bilinear(const std::vector& p, int stride, int ofs, int row_bytes, int w, int h, float sx, float sy) +{ + const int x0 = static_cast(std::floor(sx)); + const int y0 = static_cast(std::floor(sy)); + const int x1 = std::min(x0 + 1, w - 1); + const int y1 = std::min(y0 + 1, h - 1); + const float fx = sx - static_cast(x0); + const float fy = sy - static_cast(y0); + auto at = [&](int x, int y) { return static_cast(p[y * row_bytes + x * stride + ofs]); }; + const float top = (1.0F - fx) * at(x0, y0) + fx * at(x1, y0); + const float bottom = (1.0F - fx) * at(x0, y1) + fx * at(x1, y1); + return (1.0F - fy) * top + fy * bottom; +} + +uint8_t to_byte(float v) +{ + return static_cast(std::min(std::max(v + 0.5F, 0.0F), 255.0F)); +} + +float displaced_x(float x, float y, const SineWarpParams& p) +{ + return x + p.amplitude_px * std::sin(kTwoPi * y * (1.0F / p.wavelength_px) + p.phase_rad); +} + +struct Reference +{ + HostNv12 img; + std::vector luma_filled; // per luma pixel + std::vector chroma_filled; // per chroma sample pair +}; + +Reference reference_warp(const HostNv12& in, const SineWarpParams& p, bool full_range) +{ + const int w = static_cast(in.w); + const int h = static_cast(in.h); + const int cw = w / 2; + const int ch = h / 2; + Reference ref; + ref.img.w = in.w; + ref.img.h = in.h; + ref.img.y.resize(in.y.size()); + ref.img.uv.resize(in.uv.size()); + ref.luma_filled.assign(in.y.size(), false); + ref.chroma_filled.assign(static_cast(cw) * ch, false); + const uint8_t fill = full_range ? 0 : 16; + for (int y = 0; y < h; ++y) + for (int x = 0; x < w; ++x) + { + const float sx = displaced_x(static_cast(x), static_cast(y), p); + if (sx >= 0.0F && sx <= static_cast(w - 1)) + ref.img.y[y * w + x] = to_byte(bilinear(in.y, 1, 0, w, w, h, sx, static_cast(y))); + else + { + ref.img.y[y * w + x] = fill; + ref.luma_filled[y * w + x] = true; + } + } + for (int cy = 0; cy < ch; ++cy) + for (int cx = 0; cx < cw; ++cx) + { + const float luma_y = 2.0F * static_cast(cy) + 0.5F; + const float scx = 0.5F * displaced_x(2.0F * static_cast(cx), luma_y, p); + const float scy = 0.5F * (luma_y - 0.5F); + if (scx < 0.0F || scx > static_cast(cw - 1)) + { + ref.img.uv[cy * w + 2 * cx] = 128; + ref.img.uv[cy * w + 2 * cx + 1] = 128; + ref.chroma_filled[cy * cw + cx] = true; + } + else + { + ref.img.uv[cy * w + 2 * cx] = to_byte(bilinear(in.uv, 2, 0, w, cw, ch, scx, scy)); + ref.img.uv[cy * w + 2 * cx + 1] = to_byte(bilinear(in.uv, 2, 1, w, cw, ch, scx, scy)); + } + } + return ref; +} + +void run_once(SineWarp& op, const DeviceSurface& in, const DeviceSurface& out) +{ + cudaStream_t s = nullptr; + cuda_ok(cudaStreamCreate(&s)); + op.enqueue(in.view(), out.view(), s); + cuda_ok(cudaStreamSynchronize(s)); + cuda_ok(cudaStreamDestroy(s)); +} + +// Compares two images within `tol` levels; returns the worst difference. +int max_diff(const std::vector& a, const std::vector& b) +{ + REQUIRE(a.size() == b.size()); + int worst = 0; + for (size_t i = 0; i < a.size(); ++i) + worst = std::max(worst, std::abs(static_cast(a[i]) - static_cast(b[i]))); + return worst; +} + +void check_against_reference(uint32_t w, uint32_t h, const SineWarpParams& params, bool full_range = false) +{ + const HostNv12 src = make_image(w, h); + DeviceSurface in(w, h, full_range), out(w, h, full_range); + in.upload(src); + SineWarp op(params); + op.prepare({ w, h }); + run_once(op, in, out); + const HostNv12 got = out.download(); + const Reference ref = reference_warp(src, params, full_range); + CHECK(max_diff(got.y, ref.img.y) <= 1); + CHECK(max_diff(got.uv, ref.img.uv) <= 1); + // Border-filled pixels are exact. + for (size_t i = 0; i < ref.luma_filled.size(); ++i) + if (ref.luma_filled[i]) + REQUIRE(got.y[i] == (full_range ? 0 : 16)); + const size_t cw = w / 2; + for (size_t i = 0; i < ref.chroma_filled.size(); ++i) + if (ref.chroma_filled[i]) + { + const size_t row = i / cw, col = i % cw; + REQUIRE(got.uv[row * w + 2 * col] == 128); + REQUIRE(got.uv[row * w + 2 * col + 1] == 128); + } +} + +struct Size +{ + uint32_t w, h; +}; + +} // namespace + +#define REQUIRE_CUDA_OR_SKIP() \ + do \ + { \ + if (!have_cuda_device()) \ + SKIP("No CUDA device available"); \ + } while (false) + +TEST_CASE("SineWarp zero amplitude is a bit-exact identity", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + for (const Size sz : { Size{ 64, 48 }, Size{ 130, 66 } }) + { + SECTION(std::to_string(sz.w) + "x" + std::to_string(sz.h)) + { + const HostNv12 src = make_image(sz.w, sz.h); + DeviceSurface in(sz.w, sz.h), out(sz.w, sz.h); + in.upload(src); + SineWarp op(SineWarpParams{ 0.0F, 37.0F, 0.7F }); + op.prepare({ sz.w, sz.h }); + run_once(op, in, out); + const HostNv12 got = out.download(); + CHECK(got.y == src.y); + CHECK(got.uv == src.uv); + } + } +} + +TEST_CASE("SineWarp matches the CPU reference", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + for (const Size sz : { Size{ 64, 48 }, Size{ 130, 66 } }) + { + SECTION("default-ish parameters " + std::to_string(sz.w) + "x" + std::to_string(sz.h)) + { + check_against_reference(sz.w, sz.h, SineWarpParams{ 5.0F, 32.0F, 0.0F }); + } + SECTION("phase and wavelength " + std::to_string(sz.w) + "x" + std::to_string(sz.h)) + { + check_against_reference(sz.w, sz.h, SineWarpParams{ 3.5F, 21.0F, 1.1F }); + } + SECTION("large amplitude leaves the image " + std::to_string(sz.w) + "x" + std::to_string(sz.h)) + { + check_against_reference(sz.w, sz.h, SineWarpParams{ 40.0F, 24.0F, 0.4F }); + } + SECTION("full-range border fill " + std::to_string(sz.w) + "x" + std::to_string(sz.h)) + { + check_against_reference(sz.w, sz.h, SineWarpParams{ 40.0F, 24.0F, 0.4F }, true); + } + } +} + +TEST_CASE("SineWarp large amplitude actually produces border fill", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + const SineWarpParams params{ 40.0F, 24.0F, 0.4F }; + const HostNv12 src = make_image(64, 48); + const Reference ref = reference_warp(src, params, false); + CHECK(std::count(ref.luma_filled.begin(), ref.luma_filled.end(), true) > 0); + CHECK(std::count(ref.chroma_filled.begin(), ref.chroma_filled.end(), true) > 0); +} + +TEST_CASE("SineWarp keeps constant chroma constant in the interior", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + const uint32_t w = 130, h = 66; + HostNv12 src = make_image(w, h); + for (uint32_t cy = 0; cy < h / 2; ++cy) + for (uint32_t cx = 0; cx < w / 2; ++cx) + { + src.uv[cy * w + 2 * cx] = 90; + src.uv[cy * w + 2 * cx + 1] = 200; + } + const SineWarpParams params{ 4.0F, 30.0F, 0.3F }; + DeviceSurface in(w, h), out(w, h); + in.upload(src); + SineWarp op(params); + op.prepare({ w, h }); + run_once(op, in, out); + const HostNv12 got = out.download(); + const Reference ref = reference_warp(src, params, false); + size_t interior = 0; + for (uint32_t cy = 0; cy < h / 2; ++cy) + for (uint32_t cx = 0; cx < w / 2; ++cx) + { + if (ref.chroma_filled[cy * (w / 2) + cx]) + continue; + ++interior; + REQUIRE(got.uv[cy * w + 2 * cx] == 90); + REQUIRE(got.uv[cy * w + 2 * cx + 1] == 200); + } + CHECK(interior > 0); +} + +TEST_CASE("SineWarp chroma follows the siting, not luma/2", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + // Vertical chroma edge: U steps from 40 to 220 at chroma column 16. Compared with the reference the output + // pins the half-luma-row siting of the displacement. + const uint32_t w = 64, h = 48; + HostNv12 src = make_image(w, h); + for (uint32_t cy = 0; cy < h / 2; ++cy) + for (uint32_t cx = 0; cx < w / 2; ++cx) + { + src.uv[cy * w + 2 * cx] = cx < 16 ? 40 : 220; + src.uv[cy * w + 2 * cx + 1] = 128; + } + const SineWarpParams params{ 6.0F, 16.0F, 0.0F }; + DeviceSurface in(w, h), out(w, h); + in.upload(src); + SineWarp op(params); + op.prepare({ w, h }); + run_once(op, in, out); + const HostNv12 got = out.download(); + const Reference ref = reference_warp(src, params, false); + CHECK(max_diff(got.uv, ref.img.uv) <= 1); + + // The naive mapping (sample at luma_y = 2*cy, no half-row offset) must differ somewhere on this edge. + SineWarpParams p = params; + int naive_worst = 0; + for (uint32_t cy = 0; cy < h / 2; ++cy) + for (uint32_t cx = 0; cx < w / 2; ++cx) + { + const float scx = 0.5F * displaced_x(2.0F * static_cast(cx), 2.0F * static_cast(cy), p); + if (scx < 0.0F || scx > static_cast(w / 2 - 1)) + continue; + const uint8_t naive = to_byte(bilinear(src.uv, 2, 0, w, w / 2, h / 2, scx, static_cast(cy))); + naive_worst = + std::max(naive_worst, std::abs(static_cast(naive) - static_cast(got.uv[cy * w + 2 * cx]))); + } + CHECK(naive_worst > 1); +} + +TEST_CASE("SineWarp contract violations throw", "[video_stage][gpu]") +{ + SECTION("non-positive wavelength throws in the constructor") + { + CHECK_THROWS_AS(SineWarp(SineWarpParams{ 1.0F, 0.0F, 0.0F }), std::invalid_argument); + CHECK_THROWS_AS(SineWarp(SineWarpParams{ 1.0F, -5.0F, 0.0F }), std::invalid_argument); + } + SECTION("prepare rejects odd or zero dimensions") + { + SineWarp op; + CHECK_THROWS_AS(op.prepare({ 0, 48 }), std::invalid_argument); + CHECK_THROWS_AS(op.prepare({ 64, 0 }), std::invalid_argument); + CHECK_THROWS_AS(op.prepare({ 63, 48 }), std::invalid_argument); + CHECK_THROWS_AS(op.prepare({ 64, 47 }), std::invalid_argument); + } + + REQUIRE_CUDA_OR_SKIP(); + DeviceSurface in(64, 48), out(64, 48); + SineWarp op; + + SECTION("enqueue before prepare throws std::logic_error") + { + CHECK_THROWS_AS(op.enqueue(in.view(), out.view(), nullptr), std::logic_error); + } + SECTION("enqueue with a different input geometry throws std::logic_error") + { + op.prepare({ 128, 96 }); + CHECK_THROWS_AS(op.enqueue(in.view(), out.view(), nullptr), std::logic_error); + } + + op.prepare({ 64, 48 }); + + SECTION("mismatched output geometry") + { + DeviceSurface other(32, 48); + CHECK_THROWS_AS(op.enqueue(in.view(), other.view(), nullptr), std::invalid_argument); + } + SECTION("null planes") + { + Nv12View bad = out.view(); + bad.y = nullptr; + CHECK_THROWS_AS(op.enqueue(in.view(), bad, nullptr), std::invalid_argument); + bad = out.view(); + bad.uv = nullptr; + CHECK_THROWS_AS(op.enqueue(in.view(), bad, nullptr), std::invalid_argument); + bad = in.view(); + bad.y = nullptr; + CHECK_THROWS_AS(op.enqueue(bad, out.view(), nullptr), std::invalid_argument); + } + SECTION("pitch below the row size") + { + Nv12View bad = out.view(); + bad.y_pitch = 63; + CHECK_THROWS_AS(op.enqueue(in.view(), bad, nullptr), std::invalid_argument); + bad = out.view(); + bad.uv_pitch = 63; + CHECK_THROWS_AS(op.enqueue(in.view(), bad, nullptr), std::invalid_argument); + bad = in.view(); + bad.y_pitch = 10; + CHECK_THROWS_AS(op.enqueue(bad, out.view(), nullptr), std::invalid_argument); + } +} + +TEST_CASE("SineWarp enqueue is asynchronous and correct after the caller syncs", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + const SineWarpParams params{ 5.0F, 32.0F, 0.2F }; + const HostNv12 src = make_image(130, 66); + DeviceSurface in(130, 66), out(130, 66); + in.upload(src); + SineWarp op(params); + op.prepare({ 130, 66 }); + cudaStream_t s = nullptr; + cuda_ok(cudaStreamCreate(&s)); + cudaEvent_t done = nullptr; + cuda_ok(cudaEventCreate(&done)); + op.enqueue(in.view(), out.view(), s); + cuda_ok(cudaEventRecord(done, s)); + cuda_ok(cudaEventSynchronize(done)); + const HostNv12 got = out.download(); + const Reference ref = reference_warp(src, params, false); + CHECK(max_diff(got.y, ref.img.y) <= 1); + CHECK(max_diff(got.uv, ref.img.uv) <= 1); + cuda_ok(cudaEventDestroy(done)); + cuda_ok(cudaStreamDestroy(s)); +} + +TEST_CASE("SineWarp prepared operation is reusable across surfaces and streams", "[video_stage][gpu]") +{ + REQUIRE_CUDA_OR_SKIP(); + const SineWarpParams params{ 5.0F, 32.0F, 0.2F }; + const uint32_t w = 130, h = 66; + const HostNv12 a = make_image(w, h); + HostNv12 b = make_image(w, h); + for (auto& v : b.y) + v = static_cast(255 - v); + for (auto& v : b.uv) + v = static_cast(255 - v); + + DeviceSurface in_a(w, h), in_b(w, h), out_a(w, h), out_b(w, h); + in_a.upload(a); + in_b.upload(b); + SineWarp op(params); + op.prepare({ w, h }); + const Reference ref_a = reference_warp(a, params, false); + const Reference ref_b = reference_warp(b, params, false); + + SECTION("two surfaces back-to-back on one stream") + { + cudaStream_t s = nullptr; + cuda_ok(cudaStreamCreate(&s)); + op.enqueue(in_a.view(), out_a.view(), s); + op.enqueue(in_b.view(), out_b.view(), s); + cuda_ok(cudaStreamSynchronize(s)); + cuda_ok(cudaStreamDestroy(s)); + } + SECTION("two surfaces on two streams") + { + cudaStream_t s1 = nullptr, s2 = nullptr; + cuda_ok(cudaStreamCreate(&s1)); + cuda_ok(cudaStreamCreate(&s2)); + op.enqueue(in_a.view(), out_a.view(), s1); + op.enqueue(in_b.view(), out_b.view(), s2); + cuda_ok(cudaStreamSynchronize(s1)); + cuda_ok(cudaStreamSynchronize(s2)); + cuda_ok(cudaStreamDestroy(s1)); + cuda_ok(cudaStreamDestroy(s2)); + } + const HostNv12 got_a = out_a.download(); + const HostNv12 got_b = out_b.download(); + CHECK(max_diff(got_a.y, ref_a.img.y) <= 1); + CHECK(max_diff(got_a.uv, ref_a.img.uv) <= 1); + CHECK(max_diff(got_b.y, ref_b.img.y) <= 1); + CHECK(max_diff(got_b.uv, ref_b.img.uv) <= 1); +} diff --git a/src/video_stage/tests/test_tcp_receiver.cpp b/src/video_stage/tests/test_tcp_receiver.cpp new file mode 100644 index 0000000000..8c4fe9d8ed --- /dev/null +++ b/src/video_stage/tests/test_tcp_receiver.cpp @@ -0,0 +1,210 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using video_stage::TcpReceiver; + +namespace +{ + +constexpr uint16_t kStopPort = 5741; +constexpr uint16_t kRacePort = 5742; +constexpr uint16_t kSessionPort = 5743; + +int connect_to(uint16_t port) +{ + const int fd = ::socket(AF_INET, SOCK_STREAM, 0); + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + if (fd < 0 || ::connect(fd, reinterpret_cast(&addr), sizeof(addr)) != 0) + { + if (fd >= 0) + ::close(fd); + return -1; + } + return fd; +} + +/// Opens a DATA and a CTRL connection with their hellos; the descriptors are closed on destruction. +struct Sender +{ + explicit Sender(uint16_t port) + { + data = connect_to(port); + if (data >= 0) + (void)::send(data, "DATA", 4, MSG_NOSIGNAL); + control = connect_to(port); + if (control >= 0) + (void)::send(control, "CTRL", 4, MSG_NOSIGNAL); + } + ~Sender() + { + for (const int fd : { data, control }) + if (fd >= 0) + ::close(fd); + } + Sender(const Sender&) = delete; + Sender& operator=(const Sender&) = delete; + int data = -1; + int control = -1; +}; + +} // namespace + +TEST_CASE("a session is accepted and carries a record and control bytes", "[tcp_receiver]") +{ + TcpReceiver receiver(kSessionPort); + Sender sender(kSessionPort); + REQUIRE(sender.data >= 0); + REQUIRE(sender.control >= 0); + receiver.accept_session(); + + const uint8_t record[] = { 0, 0, 0, 3, 'a', 'b', 'c' }; // 4-byte length prefix, then the record + REQUIRE(::send(sender.data, record, sizeof(record), MSG_NOSIGNAL) == static_cast(sizeof(record))); + std::vector out; + REQUIRE(receiver.read_record(out)); + CHECK(out == std::vector{ 'a', 'b', 'c' }); + + const uint8_t ping[] = { 7 }; + REQUIRE(receiver.send_control(ping, 1)); + uint8_t got = 0; + REQUIRE(::recv(sender.control, &got, 1, 0) == 1); + CHECK(got == 7); + receiver.shutdown_session(); + CHECK_FALSE(receiver.read_record(out)); // a shut-down session ends cleanly +} + +TEST_CASE("stop_listening ends a blocked accept_session", "[tcp_receiver]") +{ + TcpReceiver receiver(kStopPort); + auto accepted = std::async(std::launch::async, + [&] + { + try + { + receiver.accept_session(); + return false; + } + catch (const std::exception&) + { + return true; + } + }); + std::this_thread::sleep_for(200ms); + REQUIRE(accepted.wait_for(0ms) == std::future_status::timeout); // blocked, with no sender + receiver.stop_listening(); + REQUIRE(accepted.wait_for(3s) == std::future_status::ready); + CHECK(accepted.get()); // it threw instead of returning a session +} + +TEST_CASE("stop_listening ends an accept blocked on a silent connection", "[tcp_receiver]") +{ + TcpReceiver receiver(kStopPort + 10); + const int silent = connect_to(kStopPort + 10); // connects, never sends its hello + REQUIRE(silent >= 0); + auto accepted = std::async(std::launch::async, + [&] + { + try + { + receiver.accept_session(); + } + catch (const std::exception&) + { + } + }); + std::this_thread::sleep_for(300ms); // the hello read is now pending (bounded at 2 s) + const auto begin = std::chrono::steady_clock::now(); + receiver.stop_listening(); + REQUIRE(accepted.wait_for(1500ms) == std::future_status::ready); + CHECK(std::chrono::steady_clock::now() - begin < 1500ms); + ::close(silent); +} + +TEST_CASE("shutdown_session from another thread while accept_session replaces descriptors", "[tcp_receiver][race]") +{ + TcpReceiver receiver(kRacePort); + + for (int round = 0; round < 200; ++round) + { + std::atomic done{ false }; + + std::thread accepting( + [&] + { + try + { + receiver.accept_session(); + } + catch (const std::exception&) + { + } + done = true; + }); + + // A sender that keeps offering a DATA/CTRL pair until a session is up; a shutdown may eat a hello in flight. + std::thread client( + [&] + { + while (!done) + { + Sender sender(kRacePort); + for (int i = 0; i < 20 && !done; ++i) + std::this_thread::sleep_for(1ms); + } + }); + + std::thread shutting( + [&] + { + while (!done) + { + receiver.shutdown_session(); + std::this_thread::yield(); + } + }); + + // Users of the session on other threads: none may touch a descriptor that is being replaced. + std::thread user( + [&] + { + uint8_t byte = 0; + std::vector record; + while (!done) + { + (void)receiver.pending_bytes(); + (void)receiver.send_control(&byte, 1); + std::this_thread::yield(); + } + }); + + const auto deadline = std::chrono::steady_clock::now() + 15s; + while (!done && std::chrono::steady_clock::now() < deadline) + std::this_thread::sleep_for(1ms); + if (!done) + receiver.stop_listening(); // do not hang the suite if a session never completes + accepting.join(); + client.join(); + shutting.join(); + user.join(); + REQUIRE(std::chrono::steady_clock::now() < deadline); + receiver.shutdown_session(); // end any reader before the next round retires the descriptors + } +} diff --git a/src/video_stage/tests/tools/mock_orin_sender.py b/src/video_stage/tests/tools/mock_orin_sender.py new file mode 100755 index 0000000000..aca8de5689 --- /dev/null +++ b/src/video_stage/tests/tools/mock_orin_sender.py @@ -0,0 +1,471 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""A mock of the Orin producer's direct sender, behaving as the real one does. + +It mirrors DirectSender in src/plugins/sensing/core/direct_sender.cpp and what feeds it: + - the stream: H.264 as the Jetson encoder emits it (no AUD, SPS+PPS before every IDR, no B-frames, constant bitrate, + an IDR every fps*5 frames), made once with ffmpeg if the source file is missing; + - frames paced at the publication rate, with source frame ids that step by 2 (60 fps capture, every second capture + published) and a stamp on the Orin's own monotonic clock (offset from this machine's, optionally drifting), as the + frame-ready time of each unit; + - two TCP connections opened by the Orin side, "DATA" then "CTRL" (4-byte hellos), retried every second; on every + (re)connect the queue is cleared and the stream starts at a keyframe; + - a bounded queue: a unit that does not fit is dropped, and so is everything after it until the next keyframe; + - a control thread blocked in recv: 'K' makes the encoder produce a keyframe at the next unit, 'Q' is answered with 'R' + with t2 stamped when the first byte arrives and t3 just before the reply is sent, on the Orin's clock. +One sender per --port, all in one process on one clock, like the producer's cameras. Runs until stopped or --duration. +""" + +from __future__ import annotations + +import argparse +import collections +import socket +import struct +import subprocess +import sys +import threading +import time +from pathlib import Path + +HEADER = struct.Struct("!4sHHIHHQQQQIIQ") +STREAM = struct.Struct("!HHBBHIIIIQHHI") +SAMPLE = struct.Struct("!HBBHHIIQQQQQQII") +CODEC_H264 = 2 +BITRATE = "8M" # the producer's default target + + +# ---------------------------------------------------------------------------------------------------------------------- +# The stream +# ---------------------------------------------------------------------------------------------------------------------- + + +def make_stream( + path: Path, width: int, height: int, fps: int, seconds: int = 12 +) -> None: + """Encode a test pattern the way the Jetson encoder does: IDR period fps*5, no B-frames, CBR, no AUDs.""" + gop = fps * 5 + cmd = [ + "ffmpeg", "-y", "-loglevel", "error", "-f", "lavfi", "-i", f"testsrc2=size={width}x{height}:rate={fps}", + "-t", str(seconds), "-c:v", "libx264", "-preset", "veryfast", "-profile:v", "high", "-bf", "0", "-g", str(gop), + "-x264opts", f"keyint={gop}:min-keyint={gop}:scenecut=0:aud=0:repeat-headers=1:nal-hrd=cbr", + "-b:v", BITRATE, "-maxrate", BITRATE, "-bufsize", "267k", "-pix_fmt", "yuv420p", "-f", "h264", str(path), + ] # fmt: skip + subprocess.run(cmd, check=True) + + +_TRAILING_NALS = (10, 11, 12) + + +def _nals(data: bytes) -> list[tuple[int, int, int]]: + """(offset of the start code, NAL type, offset of the NAL header byte) for every NAL unit.""" + out = [] + i = 0 + while True: + j = data.find(b"\x00\x00\x01", i) + if j < 0 or j + 3 >= len(data): + break + start = j - 1 if j > 0 and data[j - 1] == 0 else j + out.append((start, data[j + 3] & 0x1F, j + 3)) + i = j + 3 + return out + + +def _first_mb_in_slice(data: bytes, header_at: int) -> int: + """The slice's first_mb_in_slice, an Exp-Golomb code right after the NAL header byte.""" + bits = int.from_bytes(data[header_at + 1 : header_at + 5], "big") + zeros = 0 + while zeros < 15 and not (bits >> (31 - zeros)) & 1: + zeros += 1 + code = (bits >> (32 - (2 * zeros + 1))) & ((1 << (2 * zeros + 1)) - 1) + return code - 1 + + +def split_access_units(data: bytes) -> list[tuple[bytes, bool]]: + """Split Annex-B data into (access unit, is IDR). A unit begins at the first NAL after a picture's slices, or at a + picture whose first slice follows another picture directly; later slices of one picture stay with it.""" + starts = [] + previous_vcl = False + for position, (start, nal_type, header) in enumerate(_nals(data)): + vcl = nal_type in (1, 5) + if position == 0: + begin = True + elif vcl: + begin = previous_vcl and _first_mb_in_slice(data, header) == 0 + else: + begin = previous_vcl and nal_type not in _TRAILING_NALS + if begin: + starts.append(start) + # End-of-sequence, end-of-stream and filler data (constant-bitrate padding) belong to the picture before them. + previous_vcl = vcl or (previous_vcl and nal_type in _TRAILING_NALS) + starts.append(len(data)) + units = [] + for a, b in zip(starts, starts[1:]): + au = data[a:b] + units.append((au, any(t == 5 for _, t, _ in _nals(au)))) + return units + + +def record( + au: bytes, + sequence: int, + frame: int, + stamp_ns: int, + width: int, + height: int, + fps: int, +) -> bytes: + descriptor_bytes = STREAM.size + SAMPLE.size + header = HEADER.pack( + b"AVS1", + 1, + 0, + HEADER.size, + 1, + 1, + sequence, + 1, + 0, + 0, + descriptor_bytes, + len(au), + 0, + ) + stream = STREAM.pack( + 0, 1, CODEC_H264, 1, 0, width, height, fps, 1, sequence, 0, 1, 0 + ) + sample = SAMPLE.pack( + 0, 0, 0, 0x0005, 0, 0, 0, frame, 0, stamp_ns, 0, 0, 0, 0, len(au) + ) + body = header + stream + sample + au + return struct.pack("!I", len(body)) + body + + +# ---------------------------------------------------------------------------------------------------------------------- +# The Orin's clock +# ---------------------------------------------------------------------------------------------------------------------- + + +class OrinClock: + """Monotonic time on a machine with another boot time, optionally running a little fast or slow.""" + + def __init__(self, offset_ns: int, drift_ppm: float) -> None: + self._t0 = time.monotonic_ns() + self._offset = offset_ns + self._drift = drift_ppm * 1e-6 + + def now_ns(self) -> int: + local = time.monotonic_ns() + return local + self._offset + int((local - self._t0) * self._drift) + + +# ---------------------------------------------------------------------------------------------------------------------- +# DirectSender (mirrors direct_sender.cpp) +# ---------------------------------------------------------------------------------------------------------------------- + + +class DirectSender: + def __init__( + self, + host: str, + port: int, + width: int, + height: int, + fps: int, + clock: OrinClock, + max_queued_bytes: int, + ): + self.host, self.port, self.width, self.height, self.fps = ( + host, + port, + width, + height, + fps, + ) + self.clock = clock + self.max_queued_bytes = max_queued_bytes + self._cond = threading.Condition() + self._queue: collections.deque[bytes] = collections.deque() + self._queued_bytes = 0 + self._need_keyframe = True + self._stop = False + self._sequence = 0 + self._keyframe_requested = False + self.sent = 0 + self.dropped = 0 + self.keyframe_requests = 0 + self.clock_answers = 0 + self.connects = 0 + self._live: list[socket.socket] = [] + self._thread = threading.Thread(target=self._run, daemon=True) + self._thread.start() + + def stop(self) -> None: + """Cancels any connect or send in progress and waits for the worker thread; a stuck thread is an error.""" + with self._cond: + self._stop = True + self._cond.notify_all() + live = list(self._live) + for s in live: + try: + s.shutdown(socket.SHUT_RDWR) + except OSError: + pass + self._thread.join(timeout=10) + if self._thread.is_alive(): + raise RuntimeError(f"sender for port {self.port} did not stop") + + def take_keyframe_request(self) -> bool: + with self._cond: + requested, self._keyframe_requested = self._keyframe_requested, False + return requested + + def submit(self, au: bytes, keyframe: bool, frame_id: int, stamp_ns: int) -> None: + with self._cond: + if self._need_keyframe: + if not keyframe: + return # a delta frame cannot start a stream; the keyframe request is pending + self._need_keyframe = False + if self._queued_bytes + len(au) > self.max_queued_bytes: + # Dropping any frame breaks the chain after it: discard up to the next keyframe, and ask for one. + self.dropped += 1 + self._need_keyframe = True + self._keyframe_requested = True + return + self._sequence += 1 + rec = record( + au, + self._sequence, + frame_id, + stamp_ns, + self.width, + self.height, + self.fps, + ) + self._queued_bytes += len(rec) + self._queue.append(rec) + self._cond.notify() + + # --- connection -------------------------------------------------------------------------------------------------- + + def _connect(self) -> tuple[socket.socket, socket.socket] | None: + socks = [] + try: + for hello in (b"DATA", b"CTRL"): + s = socket.create_connection((self.host, self.port), timeout=3) + with self._cond: + if self._stop: + s.close() + raise OSError("stopping") + self._live.append(s) + s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + s.settimeout(None) + s.sendall(hello) + socks.append(s) + except OSError: + for s in socks: + s.close() + with self._cond: + self._live = [] + return None + return socks[0], socks[1] + + def _control(self, ctrl: socket.socket, state: dict) -> None: + """Blocks in recv so t2 is the arrival of a request, not the next time the sender looks at the socket.""" + try: + while True: + first = ctrl.recv(1) + if not first: + break + t2 = self.clock.now_ns() + if first == b"K": + with self._cond: + self.keyframe_requests += 1 + self._keyframe_requested = True + elif first == b"Q": + rest = b"" + while len(rest) < 12: + chunk = ctrl.recv(12 - len(rest)) + if not chunk: + raise OSError("closed inside a request") + rest += chunk + seq, t1 = struct.unpack(" None: + while True: + with self._cond: + if self._stop: + return + pair = self._connect() + if pair is None: + with self._cond: + if self._cond.wait_for(lambda: self._stop, timeout=1.0): + return + continue + data, ctrl = pair + self.connects += 1 + state = {"control_ended": False} + control = threading.Thread( + target=self._control, args=(ctrl, state), daemon=True + ) + control.start() + with self._cond: + # The PC needs a keyframe first: drop anything queued before the connection and ask for one. + self._queue.clear() + self._queued_bytes = 0 + self._need_keyframe = True + self._keyframe_requested = True + try: + while True: + with self._cond: + self._cond.wait_for( + lambda: self._stop or self._queue, timeout=0.2 + ) + if self._stop: + return + if not self._queue: + if state["control_ended"]: + break + continue + rec = self._queue.popleft() + self._queued_bytes -= len(rec) + data.sendall(rec) + self.sent += 1 + except OSError: + pass + finally: + for s in (ctrl, data): + try: + s.shutdown(socket.SHUT_RDWR) + except OSError: + pass + s.close() + with self._cond: + self._live = [] + control.join(timeout=2) + + +# ---------------------------------------------------------------------------------------------------------------------- + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + ap.add_argument( + "--host", default="127.0.0.1", help="the PC running video_stage_worker" + ) + ap.add_argument( + "--port", + type=int, + action="append", + required=True, + help="one per camera (repeat)", + ) + ap.add_argument("--width", type=int, default=2560) + ap.add_argument("--height", type=int, default=1984) + ap.add_argument("--fps", type=int, default=30, help="publication rate") + ap.add_argument( + "--source", + default="orin_like.h264", + help="H.264 file; made with ffmpeg if it does not exist", + ) + ap.add_argument( + "--frame-id-step", + type=int, + default=2, + help="source frame ids advance by this per unit", + ) + ap.add_argument( + "--clock-offset-ms", + type=float, + default=5000.0, + help="this clock's boot-time offset from the PC's", + ) + ap.add_argument("--clock-drift-ppm", type=float, default=0.0) + ap.add_argument("--max-queued-mb", type=float, default=16.0) + ap.add_argument( + "--duration", + type=float, + default=0.0, + help="seconds to run; 0 means until stopped", + ) + args = ap.parse_args() + + source = Path(args.source) + if not source.exists(): + print( + f"making {source} ({args.width}x{args.height}@{args.fps}, Jetson-style H.264)", + flush=True, + ) + make_stream(source, args.width, args.height, args.fps) + units = split_access_units(source.read_bytes()) + keyframes = [i for i, (_, key) in enumerate(units) if key] + if not keyframes: + sys.exit(f"{source} has no IDR access units") + + clock = OrinClock(int(args.clock_offset_ms * 1e6), args.clock_drift_ppm) + senders = [ + DirectSender( + args.host, + p, + args.width, + args.height, + args.fps, + clock, + int(args.max_queued_mb * (1 << 20)), + ) + for p in args.port + ] + period = 1.0 / args.fps + cursors = [0] * len(senders) + frame_id = 0 + started = time.monotonic() + next_tick = started + next_report = started + 5 + try: + while args.duration <= 0 or time.monotonic() - started < args.duration: + delay = next_tick - time.monotonic() + if delay > 0: + time.sleep(delay) + next_tick += period + frame_id += args.frame_id_step + stamp = clock.now_ns() + for n, s in enumerate(senders): + # A keyframe request makes that camera's encoder emit an IDR; the file's next keyframe stands in for it. + if s.take_keyframe_request(): + cursors[n] = next( + (k for k in keyframes if k >= cursors[n]), keyframes[0] + ) + au, key = units[cursors[n] % len(units)] + cursors[n] = (cursors[n] + 1) % len(units) + s.submit(au, key, frame_id, stamp) + if time.monotonic() >= next_report: + next_report += 5 + print( + " | ".join( + f"port {s.port}: sent {s.sent} dropped {s.dropped} K {s.keyframe_requests} Q {s.clock_answers}" + f" connects {s.connects}" + for s in senders + ), + flush=True, + ) + except KeyboardInterrupt: + pass + finally: + for s in senders: + s.stop() + for s in senders: + print( + f"port {s.port}: sent {s.sent}, dropped {s.dropped}, keyframe requests {s.keyframe_requests}, " + f"clock answers {s.clock_answers}, connections {s.connects}" + ) + + +if __name__ == "__main__": + main() diff --git a/src/video_stage/tests/tools/test_mock_orin_sender.py b/src/video_stage/tests/tools/test_mock_orin_sender.py new file mode 100644 index 0000000000..1eeed20c4f --- /dev/null +++ b/src/video_stage/tests/tools/test_mock_orin_sender.py @@ -0,0 +1,501 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Tests for mock_orin_sender.py. No GPU needed; the stream tests need ffmpeg. + +Run: python3 -m unittest discover -s src/video_stage/tests/tools -p 'test_*.py' +""" + +from __future__ import annotations + +import shutil +import socket +import struct +import subprocess +import sys +import tempfile +import threading +import time +import unittest +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +import mock_orin_sender as m # noqa: E402 + +START = b"\x00\x00\x00\x01" + + +def nal(nal_type: int, payload: bytes = b"\x80") -> bytes: + return START + bytes([0x60 | nal_type]) + payload + + +# first_mb_in_slice ue(v) codes: 0 -> '1', 1 -> '010', 2 -> '011', 3 -> '00100' +def slice_nal(nal_type: int, first_mb: int) -> bytes: + code = {0: 0b1, 1: 0b010, 2: 0b011, 3: 0b00100}[first_mb] + width = {0: 1, 1: 3, 2: 3, 3: 5}[first_mb] + value = (code << (8 - width)) & 0xFF + return START + bytes([0x60 | nal_type, value, 0xAA, 0xBB, 0xCC, 0xDD]) + + +class SplitterTest(unittest.TestCase): + def test_first_mb_in_slice_decoding(self) -> None: + for mb in (0, 1, 2, 3): + data = slice_nal(1, mb) + self.assertEqual(m._first_mb_in_slice(data, 4), mb) + + def test_aud_less_stream_splits_at_parameter_sets_and_slices(self) -> None: + idr = nal(7) + nal(8) + slice_nal(5, 0) + p1 = slice_nal(1, 0) + p2 = slice_nal(1, 0) + data = idr + p1 + p2 + idr + units = m.split_access_units(data) + self.assertEqual([u for u, _ in units], [idr, p1, p2, idr]) + self.assertEqual([k for _, k in units], [True, False, False, True]) + self.assertEqual(b"".join(u for u, _ in units), data) + + def test_filler_attaches_to_the_preceding_unit(self) -> None: + p1 = slice_nal(1, 0) + nal(12, b"\xff\xff") + p2 = slice_nal(1, 0) + units = m.split_access_units(p1 + p2) + self.assertEqual([u for u, _ in units], [p1, p2]) + + def test_multi_slice_picture_is_one_unit(self) -> None: + pic = slice_nal(1, 0) + slice_nal(1, 1) + slice_nal(1, 2) + nxt = slice_nal(1, 0) + units = m.split_access_units(pic + nxt) + self.assertEqual([u for u, _ in units], [pic, nxt]) + + def test_sei_before_a_picture_starts_a_new_unit(self) -> None: + a = slice_nal(1, 0) + b = nal(6) + slice_nal(1, 0) + units = m.split_access_units(a + b) + self.assertEqual([u for u, _ in units], [a, b]) + + @unittest.skipUnless( + shutil.which("ffmpeg") and shutil.which("ffprobe"), "needs ffmpeg and ffprobe" + ) + def test_unit_count_matches_ffprobe_and_units_concatenate_to_the_file(self) -> None: + with tempfile.TemporaryDirectory() as tmp: + path = Path(tmp) / "s.h264" + m.make_stream(path, 320, 240, 30, seconds=7) + data = path.read_bytes() + units = m.split_access_units(data) + self.assertEqual(b"".join(u for u, _ in units), data) + out = subprocess.run( + ["ffprobe", "-v", "error", "-count_packets", "-select_streams", "v", "-show_entries", + "stream=nb_read_packets", "-of", "csv=p=0", str(path)], + capture_output=True, text=True, check=True, + ).stdout # fmt: skip + self.assertEqual(len(units), int(out.strip())) + keys = [i for i, (_, k) in enumerate(units) if k] + self.assertEqual(keys[0], 0) + self.assertTrue(all(b == a + 150 for a, b in zip(keys, keys[1:]))) + self.assertTrue( + units[0][0].startswith(START) and units[0][0][4] & 0x1F == 7 + ) # SPS first + self.assertFalse( + any(t == 9 for u, _ in units for _, t, _ in m._nals(u)) + ) # no AUDs + + +class RecordTest(unittest.TestCase): + def test_layout(self) -> None: + au = b"\x00\x00\x00\x01\x65abc" + rec = m.record( + au, + sequence=7, + frame=14, + stamp_ns=123456789, + width=2560, + height=1984, + fps=30, + ) + (length,) = struct.unpack("!I", rec[:4]) + self.assertEqual(length, len(rec) - 4) + body = rec[4:] + header = m.HEADER.unpack(body[: m.HEADER.size]) + self.assertEqual(header[0], b"AVS1") + self.assertEqual(header[3], m.HEADER.size) + self.assertEqual(header[6], 7) # sequence + descriptors = body[ + m.HEADER.size : m.HEADER.size + m.STREAM.size + m.SAMPLE.size + ] + self.assertEqual(header[10], len(descriptors)) + self.assertEqual(header[11], len(au)) # payload bytes + stream = m.STREAM.unpack(descriptors[: m.STREAM.size]) + self.assertEqual( + (stream[2], stream[5], stream[6], stream[7]), (m.CODEC_H264, 2560, 1984, 30) + ) + sample = m.SAMPLE.unpack(descriptors[m.STREAM.size :]) + self.assertEqual(sample[7], 14) # frame number + self.assertEqual(sample[9], 123456789) # stamp + self.assertEqual(sample[-1], len(au)) + self.assertEqual(body[-len(au) :], au) + + +class FakeWorker: + """A TCP server accepting DATA then CTRL connections, like the worker's receiver.""" + + def __init__(self) -> None: + self.server = socket.socket() + self.server.bind(("127.0.0.1", 0)) + self.server.listen(8) + self.port = self.server.getsockname()[1] + self.hellos: list[bytes] = [] + self.sessions: list[tuple[socket.socket, socket.socket]] = [] + self.records: list[list[bytes]] = [] + self._threads: list[threading.Thread] = [] + self._stop = False + t = threading.Thread(target=self._accept, daemon=True) + t.start() + + def _accept(self) -> None: + while not self._stop: + try: + a, _ = self.server.accept() + b, _ = self.server.accept() + except OSError: + return + ha, hb = a.recv(4), b.recv(4) + self.hellos += [ha, hb] + data, ctrl = (a, b) if ha == b"DATA" else (b, a) + self.sessions.append((data, ctrl)) + self.records.append([]) + threading.Thread( + target=self._read, args=(data, len(self.records) - 1), daemon=True + ).start() + + def _read(self, data: socket.socket, index: int) -> None: + buf = b"" + try: + while True: + chunk = data.recv(65536) + if not chunk: + return + buf += chunk + while len(buf) >= 4: + (n,) = struct.unpack("!I", buf[:4]) + if len(buf) < 4 + n: + break + self.records[index].append(buf[4 : 4 + n]) + buf = buf[4 + n :] + except OSError: + return + + def close_session(self, index: int) -> None: + for s in self.sessions[index]: + s.close() + + def close(self) -> None: + self._stop = True + self.server.close() + for pair in self.sessions: + for s in pair: + s.close() + + +def wait_for(cond, timeout: float = 5.0) -> bool: + end = time.monotonic() + timeout + while time.monotonic() < end: + if cond(): + return True + time.sleep(0.02) + return False + + +def frame_numbers(records: list[bytes]) -> list[int]: + off = m.HEADER.size + m.STREAM.size + return [m.SAMPLE.unpack(r[off : off + m.SAMPLE.size])[7] for r in records] + + +IDR = b"\x00\x00\x00\x01\x65" + b"I" * 20 +DELTA = b"\x00\x00\x00\x01\x41" + b"P" * 20 + + +class DirectSenderTest(unittest.TestCase): + def setUp(self) -> None: + self.worker = FakeWorker() + self.clock = m.OrinClock(5_000_000_000, 0.0) + self.senders: list[m.DirectSender] = [] + + def tearDown(self) -> None: + for s in self.senders: + s.stop() + self.worker.close() + + def sender(self, max_queued: int = 1 << 20) -> m.DirectSender: + s = m.DirectSender( + "127.0.0.1", self.worker.port, 64, 48, 30, self.clock, max_queued + ) + self.senders.append(s) + return s + + def test_hello_order_is_data_then_ctrl(self) -> None: + self.sender() + self.assertTrue(wait_for(lambda: len(self.worker.hellos) == 2)) + self.assertEqual(self.worker.hellos, [b"DATA", b"CTRL"]) + + def test_deltas_are_ignored_until_a_keyframe_after_connect(self) -> None: + s = self.sender() + self.assertTrue( + wait_for( + lambda: s.connects == 1 and s._need_keyframe and s._keyframe_requested + ) + ) + s.submit(DELTA, False, 2, self.clock.now_ns()) + s.submit(DELTA, False, 4, self.clock.now_ns()) + s.submit(IDR, True, 6, self.clock.now_ns()) + s.submit(DELTA, False, 8, self.clock.now_ns()) + self.assertTrue( + wait_for( + lambda: ( + len(self.worker.records) == 1 and len(self.worker.records[0]) == 2 + ) + ) + ) + self.assertEqual(frame_numbers(self.worker.records[0]), [6, 8]) + self.assertTrue(s.take_keyframe_request()) + self.assertFalse(s.take_keyframe_request()) + + def test_reconnects_after_the_server_closes_and_restarts_at_a_keyframe( + self, + ) -> None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + s.submit(IDR, True, 2, self.clock.now_ns()) + self.assertTrue( + wait_for(lambda: self.worker.records and len(self.worker.records[0]) == 1) + ) + self.worker.close_session(0) + self.assertTrue(wait_for(lambda: s.connects == 2, timeout=8)) + self.assertEqual(self.worker.hellos, [b"DATA", b"CTRL", b"DATA", b"CTRL"]) + self.assertTrue(wait_for(lambda: s._need_keyframe and s._keyframe_requested)) + s.submit( + DELTA, False, 4, self.clock.now_ns() + ) # ignored: the new connection starts at a keyframe + s.submit(IDR, True, 6, self.clock.now_ns()) + self.assertTrue( + wait_for( + lambda: ( + len(self.worker.records) == 2 and len(self.worker.records[1]) == 1 + ) + ) + ) + self.assertEqual(frame_numbers(self.worker.records[1]), [6]) + + def test_queue_is_cleared_on_reconnect(self) -> None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + self.worker.close_session(0) + self.assertTrue(wait_for(lambda: s.connects == 2, timeout=8)) + self.assertTrue(wait_for(lambda: s._need_keyframe)) + self.assertEqual(len(s._queue), 0) + self.assertEqual(s._queued_bytes, 0) + + def test_overflow_drops_then_ignores_deltas_until_the_next_keyframe(self) -> None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + # Stop the sender thread from draining: hold the lock while submitting. + rec_size = len(m.record(IDR, 1, 2, 0, 64, 48, 30)) + s.max_queued_bytes = rec_size * 2 + 1 + with s._cond: + pass + s._queue.clear() + s._need_keyframe = False + s._queued_bytes = 0 + # Submit while holding the condition's lock so the sender thread cannot pop between submits. + with s._cond: + for i, (au, key) in enumerate( + [(IDR, True), (DELTA, False), (DELTA, False), (DELTA, False)] + ): + self._submit_locked(s, au, key, 2 * (i + 1)) + self.assertEqual( + s.dropped, 1 + ) # the third unit did not fit, the fourth was ignored + self.assertTrue(s._need_keyframe) + self.assertTrue(s._keyframe_requested) + self.assertEqual(len(s._queue), 2) + # Until the next keyframe, deltas are ignored; the keyframe is taken. + s.submit(DELTA, False, 20, self.clock.now_ns()) + self.assertEqual(s.dropped, 1) + self.assertTrue(wait_for(lambda: len(s._queue) == 0)) + s.submit(IDR, True, 22, self.clock.now_ns()) + self.assertFalse(s._need_keyframe) + + @staticmethod + def _submit_locked(s: m.DirectSender, au: bytes, key: bool, frame: int) -> None: + # Condition wraps an RLock, so submit() can re-enter it while the test holds it. + s.submit(au, key, frame, 0) + + def test_k_sets_keyframe_requested(self) -> None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + self.assertTrue( + wait_for(lambda: s.take_keyframe_request()) + ) # the connect's own request + _, ctrl = self.worker.sessions[0] + ctrl.sendall(b"K") + self.assertTrue(wait_for(lambda: s.keyframe_requests == 1)) + self.assertTrue(s.take_keyframe_request()) + + def test_q_is_answered_with_r_echoing_seq_and_t1_on_the_offset_clock(self) -> None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + _, ctrl = self.worker.sessions[0] + t1 = time.monotonic_ns() + ctrl.settimeout(5) + ctrl.sendall(struct.pack(" None: + s = self.sender() + self.assertTrue(wait_for(lambda: s.connects == 1)) + _, ctrl = self.worker.sessions[0] + ctrl.settimeout(5) + ctrl.sendall(b"K" + struct.pack(" None: + clock = m.OrinClock(0, 1000.0) # 1000 ppm fast + a_local, a = time.monotonic_ns(), clock.now_ns() + time.sleep(0.3) + b_local, b = time.monotonic_ns(), clock.now_ns() + extra = (b - a) - (b_local - a_local) + self.assertGreater(extra, 0.3e9 * 1000e-6 * 0.5) + flat = m.OrinClock(0, 0.0) + c = flat.now_ns() + time.sleep(0.1) + d = flat.now_ns() + self.assertAlmostEqual((d - c) / 1e9, 0.1, delta=0.03) + + +class StalledPeer: + """Accepts the DATA and CTRL connections, reads the hellos and then never reads again.""" + + def __init__(self) -> None: + self.server = socket.socket() + self.server.bind(("127.0.0.1", 0)) + self.server.listen(8) + self.port = self.server.getsockname()[1] + self.accepted: list[socket.socket] = [] + threading.Thread(target=self._accept, daemon=True).start() + + def _accept(self) -> None: + try: + while True: + c, _ = self.server.accept() + c.recv(4) + self.accepted.append(c) + except OSError: + return + + def close(self) -> None: + self.server.close() + for c in self.accepted: + c.close() + + +class StopTest(unittest.TestCase): + def test_stop_returns_promptly_while_a_send_is_blocked_on_a_peer_that_does_not_read( + self, + ) -> None: + peer = StalledPeer() + self.addCleanup(peer.close) + clock = m.OrinClock(0, 0.0) + s = m.DirectSender("127.0.0.1", peer.port, 64, 48, 30, clock, 512 << 20) + self.assertTrue(wait_for(lambda: s.connects == 1)) + big = b"\x00\x00\x00\x01\x65" + b"I" * (1 << 20) + s.submit(big, True, 2, 0) + for i in range(3, 80): + s.submit(big, False, 2 * i, 0) + # Loopback buffers hold a few MB; with ~80 MB queued the sender thread is stuck in sendall. + time.sleep(0.5) + self.assertGreater(len(s._queue), 0) + started = time.monotonic() + s.stop() + self.assertLess(time.monotonic() - started, 2.0) + self.assertFalse(s._thread.is_alive()) + + def test_stop_returns_promptly_when_connected_and_idle(self) -> None: + peer = StalledPeer() + self.addCleanup(peer.close) + s = m.DirectSender( + "127.0.0.1", peer.port, 64, 48, 30, m.OrinClock(0, 0.0), 1 << 20 + ) + self.assertTrue(wait_for(lambda: s.connects == 1)) + started = time.monotonic() + s.stop() + self.assertLess(time.monotonic() - started, 2.0) + + +class TwoCameraKeyframeTest(unittest.TestCase): + """Runs the mock's main() against two fake workers: a keyframe request from one camera moves only its cursor.""" + + UNITS = 200 + GOP = 40 + + def _source(self, path: Path) -> None: + data = b"" + for i in range(self.UNITS): + key = i % self.GOP == 0 + data += START + bytes([0x65 if key else 0x41, 0x80, 0xAA, i]) + path.write_bytes(data) + + @staticmethod + def _indices(records: list[bytes]) -> list[int]: + return [r[-1] for r in records] + + def test_a_keyframe_request_on_one_camera_leaves_the_other_cursor_alone( + self, + ) -> None: + a, b = FakeWorker(), FakeWorker() + self.addCleanup(a.close) + self.addCleanup(b.close) + with tempfile.TemporaryDirectory() as tmp: + src = Path(tmp) / "s.h264" + self._source(src) + proc = subprocess.Popen( + [sys.executable, str(Path(m.__file__)), "--port", str(a.port), + "--port", str(b.port), "--width", "64", "--height", "48", "--fps", "30", + "--source", str(src), "--duration", "5"], + stdout=subprocess.DEVNULL, + ) # fmt: skip + try: + self.assertTrue( + wait_for( + lambda: all( + w.records and len(w.records[0]) >= 8 for w in (a, b) + ) + ) + ) + before_a = len(a.records[0]) + a.sessions[0][1].sendall(b"K") + self.assertTrue(wait_for(lambda: len(a.records[0]) >= before_a + 10)) + proc.wait(timeout=15) + finally: + if proc.poll() is None: + proc.kill() + proc.wait() + idx_a, idx_b = self._indices(a.records[0]), self._indices(b.records[0]) + steps_a = [y - x for x, y in zip(idx_a, idx_a[1:])] + steps_b = [y - x for x, y in zip(idx_b, idx_b[1:])] + self.assertTrue(any(s > 1 for s in steps_a), f"camera A never jumped: {idx_a}") + jump = next(i for i, s in enumerate(steps_a) if s > 1) + self.assertEqual(idx_a[jump + 1] % self.GOP, 0) # it resumed at a keyframe + self.assertEqual(set(steps_b), {1}, f"camera B was disturbed: {idx_b}") + + +if __name__ == "__main__": + unittest.main() diff --git a/src/video_stage/wire/CMakeLists.txt b/src/video_stage/wire/CMakeLists.txt new file mode 100644 index 0000000000..4925e1aad6 --- /dev/null +++ b/src/video_stage/wire/CMakeLists.txt @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The AVS1 record codec: plain C++, no CUDA, so the Orin-side sender can link it too. +add_library(video_stage_wire STATIC + avs1.cpp + h264_sps.cpp + h264_access_units.cpp +) +add_library(video_stage::wire ALIAS video_stage_wire) + +set_target_properties(video_stage_wire PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(video_stage_wire + INTERFACE + $ +) diff --git a/src/video_stage/wire/avs1.cpp b/src/video_stage/wire/avs1.cpp new file mode 100644 index 0000000000..90e899c8a0 --- /dev/null +++ b/src/video_stage/wire/avs1.cpp @@ -0,0 +1,205 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/avs1.hpp" + +#include + +namespace video_stage +{ +namespace avs1 +{ + +namespace +{ + +constexpr uint16_t kStreamPresent = 0x0001; +constexpr uint16_t kSamplePresent = 0x0001; +constexpr uint16_t kSampleFrameTimestampValid = 0x0004; +constexpr uint16_t kSampleKnownFlags = kSamplePresent | 0x0002 | kSampleFrameTimestampValid | 0x0008; + +void put16(std::vector& out, uint16_t v) +{ + out.push_back(static_cast(v >> 8)); + out.push_back(static_cast(v)); +} + +void put32(std::vector& out, uint32_t v) +{ + for (int shift = 24; shift >= 0; shift -= 8) + out.push_back(static_cast(v >> shift)); +} + +void put64(std::vector& out, uint64_t v) +{ + for (int shift = 56; shift >= 0; shift -= 8) + out.push_back(static_cast(v >> shift)); +} + +uint16_t get16(const uint8_t* p) +{ + return static_cast((p[0] << 8) | p[1]); +} + +uint32_t get32(const uint8_t* p) +{ + return (static_cast(p[0]) << 24) | (static_cast(p[1]) << 16) | + (static_cast(p[2]) << 8) | p[3]; +} + +uint64_t get64(const uint8_t* p) +{ + return (static_cast(get32(p)) << 32) | get32(p + 4); +} + +std::optional fail(std::string& error, const char* why) +{ + error = why; + return std::nullopt; +} + +} // namespace + +void encode(const Sample& s, const uint8_t* payload, size_t payload_size, std::vector& out) +{ + const uint32_t descriptor_bytes = kStreamBytes + kSampleBytes; + const uint32_t record_length = kHeaderBytes + descriptor_bytes + static_cast(payload_size); + out.reserve(out.size() + 4 + record_length); + put32(out, record_length); + + // Bundle header (64 bytes). + out.insert(out.end(), { 'A', 'V', 'S', '1' }); + put16(out, kVersion); + put16(out, 0); // bundle flags: no sender wall-clock fields + put32(out, kHeaderBytes); + put16(out, 1); // stream_count + put16(out, 1); // sample_count + put64(out, s.bundle_sequence); + put64(out, s.session_id); + put64(out, 0); // sender_capture_ready_ns + put64(out, 0); // sender_tcp_enqueue_ns + put32(out, descriptor_bytes); + put32(out, static_cast(payload_size)); + put64(out, 0); // sender_capture_ready_tsc_ns + + // Stream descriptor (40 bytes). + put16(out, 0); // stream_index + put16(out, kStreamPresent); + out.push_back(static_cast(s.codec)); + out.push_back(1); // pixel_format NV12 + put16(out, 0); // rotation + put32(out, s.width); + put32(out, s.height); + put32(out, s.fps_numerator); + put32(out, 1); // fps_denominator + put64(out, s.bundle_sequence); // stream_sequence + put16(out, 0); // first_sample + put16(out, 1); // sample_count + put32(out, 0); // reserved + + // Sample descriptor (72 bytes). + put16(out, 0); // stream_index + out.push_back(0); // camera_index + out.push_back(0); // role: mono + put16(out, kSamplePresent | kSampleFrameTimestampValid); + put16(out, 0); // reserved + put32(out, 0); // device_index + put32(out, 0); // sensor_mode_index + put64(out, s.frame_number); + put64(out, 0); // sof_tsc_ns + put64(out, s.frame_timestamp_ns); + put64(out, 0); // sender_capture_ready_ns + put64(out, 0); // sender_process_submit_ns + put64(out, 0); // sender_process_done_ns + put32(out, 0); // payload_offset + put32(out, static_cast(payload_size)); + + out.insert(out.end(), payload, payload + payload_size); +} + +uint32_t read_length(const uint8_t prefix[4]) +{ + return get32(prefix); +} + +std::optional parse(const uint8_t* data, size_t size, std::string& error) +{ + constexpr size_t kFixed = kHeaderBytes + kStreamBytes + kSampleBytes; + if (size < kFixed) + return fail(error, "record shorter than its descriptors"); + if (std::memcmp(data, "AVS1", 4) != 0) + return fail(error, "bad magic"); + if (get16(data + 4) != kVersion) + return fail(error, "unsupported version"); + if (get16(data + 6) != 0) + return fail(error, "unknown bundle flags"); + if (get32(data + 8) != kHeaderBytes) + return fail(error, "bad header size"); + if (get16(data + 12) != 1 || get16(data + 14) != 1) + return fail(error, "this stage takes one stream with one sample"); + const uint32_t descriptor_bytes = get32(data + 48); + const uint32_t payload_bytes = get32(data + 52); + if (descriptor_bytes != kStreamBytes + kSampleBytes) + return fail(error, "bad descriptor size"); + if (size != kFixed + payload_bytes) + return fail(error, "payload size does not match the record length"); + if (get64(data + 32) != 0 || get64(data + 40) != 0 || get64(data + 56) != 0) + return fail(error, "sender wall-clock fields must be zero without their flags"); + + const uint8_t* stream = data + kHeaderBytes; + if (get16(stream + 2) != kStreamPresent) + return fail(error, "stream not present or unknown stream flags"); + if (stream[4] != static_cast(Codec::H264)) + return fail(error, "codec is not H.264"); + if (stream[5] != 1) + return fail(error, "pixel format is not NV12"); + if (get16(stream + 6) != 0) + return fail(error, "rotation not supported"); + if (get32(stream + 20) == 0) + return fail(error, "zero fps denominator"); + if (get16(stream + 32) != 0 || get16(stream + 34) != 1 || get32(stream + 36) != 0) + return fail(error, "bad sample indexing"); + + const uint8_t* sample = stream + kStreamBytes; + const uint16_t flags = get16(sample + 4); + if ((flags & ~kSampleKnownFlags) != 0) + return fail(error, "unknown sample flags"); + if ((flags & kSamplePresent) == 0 || (flags & kSampleFrameTimestampValid) == 0) + return fail(error, "sample must be present with a frame timestamp"); + if (get16(sample + 6) != 0) + return fail(error, "reserved sample field is nonzero"); + const uint32_t payload_offset = get32(sample + 64); + const uint32_t payload_length = get32(sample + 68); + if (payload_length == 0 || payload_offset != 0 || payload_length != payload_bytes) + return fail(error, "payload range is empty, offset, or does not cover the payload"); + + Record record; + record.sample.codec = Codec::H264; + record.sample.width = get32(stream + 8); + record.sample.height = get32(stream + 12); + record.sample.fps_numerator = get32(stream + 16); + record.sample.bundle_sequence = get64(data + 16); + record.sample.session_id = get64(data + 24); + record.sample.frame_number = get64(sample + 16); + record.sample.frame_timestamp_ns = get64(sample + 32); + record.payload = data + kFixed; + record.payload_size = payload_length; + return record; +} + +bool contains_idr(const uint8_t* au, size_t size) +{ + for (size_t i = 0; i + 3 < size; ++i) + { + if (au[i] == 0 && au[i + 1] == 0 && au[i + 2] == 1) + { + if ((au[i + 3] & 0x1F) == 5) + return true; + i += 2; + } + } + return false; +} + +} // namespace avs1 +} // namespace video_stage diff --git a/src/video_stage/wire/h264_access_units.cpp b/src/video_stage/wire/h264_access_units.cpp new file mode 100644 index 0000000000..425ac64f45 --- /dev/null +++ b/src/video_stage/wire/h264_access_units.cpp @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/h264_access_units.hpp" + +namespace video_stage +{ + +namespace +{ + +struct Nal +{ + std::size_t start; ///< offset of the start code (including a leading zero byte when it is a 4-byte code) + int type; + std::size_t header; ///< offset of the NAL header byte +}; + +std::vector find_nals(const std::uint8_t* data, std::size_t size) +{ + std::vector nals; + std::size_t i = 0; + while (i + 3 < size) + { + if (data[i] == 0 && data[i + 1] == 0 && data[i + 2] == 1) + { + const std::size_t start = (i > 0 && data[i - 1] == 0) ? i - 1 : i; + nals.push_back({ start, data[i + 3] & 0x1F, i + 3 }); + i += 3; + } + else + { + ++i; + } + } + return nals; +} + +/// first_mb_in_slice, the Exp-Golomb code right after the NAL header byte; large values read as nonzero. +std::size_t first_mb_in_slice(const std::uint8_t* data, std::size_t size, std::size_t header) +{ + std::uint32_t bits = 0; + for (std::size_t k = 0; k < 4; ++k) + bits = (bits << 8) | (header + 1 + k < size ? data[header + 1 + k] : 0); + int zeros = 0; + while (zeros < 15 && !((bits >> (31 - zeros)) & 1)) + ++zeros; + const std::uint32_t code = (bits >> (32 - (2 * zeros + 1))) & ((1u << (2 * zeros + 1)) - 1); + return code - 1; +} + +bool is_trailing(int type) +{ + return type == 10 || type == 11 || type == 12; +} + +} // namespace + +std::vector split_access_units(const std::uint8_t* data, std::size_t size) +{ + const std::vector nals = find_nals(data, size); + std::vector starts; + bool previous_vcl = false; + for (std::size_t n = 0; n < nals.size(); ++n) + { + const bool vcl = nals[n].type == 1 || nals[n].type == 5; + bool begin = false; + if (n == 0) + begin = true; + else if (vcl) + begin = previous_vcl && first_mb_in_slice(data, size, nals[n].header) == 0; + else + begin = previous_vcl && !is_trailing(nals[n].type); + if (begin) + starts.push_back(nals[n].start); + previous_vcl = vcl || (previous_vcl && is_trailing(nals[n].type)); + } + starts.push_back(size); + + std::vector units; + for (std::size_t u = 0; u + 1 < starts.size(); ++u) + { + AccessUnitRange unit{ starts[u], starts[u + 1] - starts[u], false }; + for (const auto& nal : nals) + if (nal.start >= unit.offset && nal.start < unit.offset + unit.size && nal.type == 5) + unit.idr = true; + units.push_back(unit); + } + return units; +} + +} // namespace video_stage diff --git a/src/video_stage/wire/h264_sps.cpp b/src/video_stage/wire/h264_sps.cpp new file mode 100644 index 0000000000..a917eff86e --- /dev/null +++ b/src/video_stage/wire/h264_sps.cpp @@ -0,0 +1,401 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/video_stage/h264_sps.hpp" + +#include +#include + +namespace video_stage +{ +namespace h264 +{ + +namespace +{ + +// RBSP bytes of a NAL: the payload with emulation-prevention bytes (00 00 03) removed. +std::vector unescape(const uint8_t* p, size_t n) +{ + std::vector out; + out.reserve(n); + int zeros = 0; + for (size_t i = 0; i < n; ++i) + { + if (zeros >= 2 && p[i] == 3) + { + zeros = 0; + continue; + } + out.push_back(p[i]); + zeros = p[i] == 0 ? zeros + 1 : 0; + } + return out; +} + +std::vector escape(const std::vector& rbsp) +{ + std::vector out; + out.reserve(rbsp.size() + 8); + int zeros = 0; + for (uint8_t b : rbsp) + { + if (zeros >= 2 && b <= 3) + { + out.push_back(3); + zeros = 0; + } + out.push_back(b); + zeros = b == 0 ? zeros + 1 : 0; + } + return out; +} + +class BitReader +{ +public: + explicit BitReader(const std::vector& d) : m_data(d) + { + } + bool ok() const + { + return !m_error; + } + size_t position() const + { + return m_pos; + } + uint32_t bits(int n) + { + uint32_t v = 0; + for (int i = 0; i < n; ++i) + v = (v << 1) | bit(); + return v; + } + uint32_t bit() + { + if (m_pos >= m_data.size() * 8) + { + m_error = true; + return 0; + } + const uint32_t b = (m_data[m_pos >> 3] >> (7 - (m_pos & 7))) & 1; + ++m_pos; + return b; + } + uint32_t ue() + { + int zeros = 0; + while (!bit() && ok() && zeros < 32) + ++zeros; + if (zeros >= 32) + { + m_error = true; + return 0; + } + return (1u << zeros) - 1 + bits(zeros); + } + int32_t se() + { + const uint32_t k = ue(); + return (k & 1) ? static_cast((k + 1) / 2) : -static_cast(k / 2); + } + +private: + const std::vector& m_data; + size_t m_pos = 0; + bool m_error = false; +}; + +class BitWriter +{ +public: + void bit(uint32_t b) + { + if ((m_bits & 7) == 0) + m_data.push_back(0); + if (b) + m_data.back() |= static_cast(1u << (7 - (m_bits & 7))); + ++m_bits; + } + void bits(uint32_t v, int n) + { + for (int i = n - 1; i >= 0; --i) + bit((v >> i) & 1); + } + void ue(uint32_t v) + { + const uint32_t x = v + 1; + int len = 0; + while ((x >> len) > 1) + ++len; + bits(0, len); + bits(x, len + 1); + } + void copy(const std::vector& src, size_t from, size_t to) + { + for (size_t i = from; i < to; ++i) + bit((src[i >> 3] >> (7 - (i & 7))) & 1); + } + std::vector& finish() + { + bit(1); // rbsp_stop_one_bit, then zero bits to a byte boundary + return m_data; + } + +private: + std::vector m_data; + size_t m_bits = 0; +}; + +void skip_scaling_list(BitReader& r, int size) +{ + int last = 8, next = 8; + for (int j = 0; j < size; ++j) + { + if (next != 0) + { + next = (last + r.se() + 256) % 256; + } + last = next == 0 ? last : next; + } +} + +void skip_hrd(BitReader& r) +{ + const uint32_t cpb_cnt = r.ue() + 1; + r.bits(4); + r.bits(4); + for (uint32_t i = 0; i < cpb_cnt && r.ok(); ++i) + { + r.ue(); + r.ue(); + r.bit(); + } + r.bits(5); + r.bits(5); + r.bits(5); + r.bits(5); +} + +struct Parsed +{ + SpsInfo info; + std::vector rbsp; // the SPS RBSP, header byte first + size_t vui_flag_pos = 0; // bit position of vui_parameters_present_flag + size_t restriction_flag_pos = 0; // bit position of bitstream_restriction_flag (valid when vui_present) +}; + +std::optional parse_rbsp(std::vector rbsp) +{ + if (rbsp.size() < 4) + return std::nullopt; + Parsed out; + out.rbsp = std::move(rbsp); + BitReader r(out.rbsp); + r.bits(8); // NAL header + SpsInfo& s = out.info; + s.profile_idc = r.bits(8); + r.bits(8); + s.level_idc = r.bits(8); + r.ue(); + const uint32_t high[] = { 100, 110, 122, 244, 44, 83, 86, 118, 128, 138, 139, 134, 135 }; + if (std::find(std::begin(high), std::end(high), s.profile_idc) != std::end(high)) + { + s.chroma_format_idc = r.ue(); + if (s.chroma_format_idc == 3) + r.bit(); + r.ue(); + r.ue(); + r.bit(); + if (r.bit()) + { + for (int i = 0; i < (s.chroma_format_idc != 3 ? 8 : 12) && r.ok(); ++i) + { + if (r.bit()) + skip_scaling_list(r, i < 6 ? 16 : 64); + } + } + } + r.ue(); + s.pic_order_cnt_type = r.ue(); + if (s.pic_order_cnt_type == 0) + { + r.ue(); + } + else if (s.pic_order_cnt_type == 1) + { + r.bit(); + r.se(); + r.se(); + const uint32_t cycle = r.ue(); + for (uint32_t i = 0; i < cycle && r.ok(); ++i) + r.se(); + } + s.max_num_ref_frames = r.ue(); + r.bit(); + const uint32_t w_mbs = r.ue() + 1; + const uint32_t h_map_units = r.ue() + 1; + s.frame_mbs_only = r.bit() != 0; + if (!s.frame_mbs_only) + r.bit(); + r.bit(); + uint32_t crop[4] = { 0, 0, 0, 0 }; + if (r.bit()) + { + for (uint32_t& c : crop) + c = r.ue(); + } + s.width = w_mbs * 16; + s.height = h_map_units * (s.frame_mbs_only ? 1 : 2) * 16; + const uint32_t cx = (s.chroma_format_idc == 1 || s.chroma_format_idc == 2) ? 2 : 1; + const uint32_t cy = (s.chroma_format_idc == 1 ? 2 : 1) * (s.frame_mbs_only ? 1 : 2); + s.width -= cx * (crop[0] + crop[1]); + s.height -= cy * (crop[2] + crop[3]); + out.vui_flag_pos = r.position(); + s.vui_present = r.bit() != 0; + if (s.vui_present) + { + if (r.bit()) + { + if (r.bits(8) == 255) + r.bits(32); + } + if (r.bit()) + r.bit(); + if (r.bit()) + { + r.bits(3); + s.full_range = r.bit() != 0; + if (r.bit()) + r.bits(24); + } + if (r.bit()) + { + // chroma_loc_info_present_flag: the vertical sample positions of the chroma samples (H.264 Table E-1) + s.chroma_loc_present = true; + s.chroma_loc_top = r.ue(); + s.chroma_loc_bottom = r.ue(); + } + if (r.bit()) + { + r.bits(32); + r.bits(32); + r.bit(); + } + const bool nal_hrd = r.bit() != 0; + if (nal_hrd) + skip_hrd(r); + const bool vcl_hrd = r.bit() != 0; + if (vcl_hrd) + skip_hrd(r); + if (nal_hrd || vcl_hrd) + r.bit(); + r.bit(); + out.restriction_flag_pos = r.position(); + s.bitstream_restriction = r.bit() != 0; + if (s.bitstream_restriction) + { + r.bit(); + r.ue(); + r.ue(); + r.ue(); + r.ue(); + s.max_num_reorder_frames = r.ue(); + s.max_dec_frame_buffering = r.ue(); + } + } + if (!r.ok()) + return std::nullopt; + return out; +} + +// Finds the first NAL of `type`; returns the offsets of its header byte and its end (start of the next start code). +bool find_nal(const uint8_t* d, size_t n, int type, size_t& begin, size_t& end, size_t& start_code) +{ + for (size_t i = 0; i + 3 < n; ++i) + { + if (d[i] == 0 && d[i + 1] == 0 && d[i + 2] == 1 && (d[i + 3] & 0x1F) == type) + { + begin = i + 3; + start_code = (i > 0 && d[i - 1] == 0) ? i - 1 : i; + size_t j = begin; + while (j + 2 < n && !(d[j] == 0 && d[j + 1] == 0 && (d[j + 2] == 1 || d[j + 2] == 0))) + ++j; + end = (j + 2 < n) ? j : n; + return true; + } + } + return false; +} + +} // namespace + +std::string SpsInfo::describe() const +{ + std::string s = "H.264 profile " + std::to_string(profile_idc) + " level " + std::to_string(level_idc) + ", " + + std::to_string(width) + "x" + std::to_string(height) + ", refs " + + std::to_string(max_num_ref_frames) + ", poc type " + std::to_string(pic_order_cnt_type); + if (bitstream_restriction) + s += ", bitstream_restriction: reorder " + std::to_string(max_num_reorder_frames) + ", dpb " + + std::to_string(max_dec_frame_buffering); + else + s += vui_present ? ", VUI without bitstream_restriction" : ", no VUI"; + return s; +} + +std::optional parse_sps(const uint8_t* annexb, size_t size) +{ + size_t begin = 0, end = 0, sc = 0; + if (!find_nal(annexb, size, 7, begin, end, sc)) + return std::nullopt; + auto parsed = parse_rbsp(unescape(annexb + begin, end - begin)); + if (!parsed) + return std::nullopt; + return parsed->info; +} + +std::optional> declare_no_reordering(const uint8_t* annexb, size_t size) +{ + size_t begin = 0, end = 0, sc = 0; + if (!find_nal(annexb, size, 7, begin, end, sc)) + return std::nullopt; + auto parsed = parse_rbsp(unescape(annexb + begin, end - begin)); + if (!parsed) + return std::nullopt; + const SpsInfo& info = parsed->info; + const std::vector& rbsp = parsed->rbsp; + // Drop the trailing stop bit and padding: copy the original bits up to the point the VUI tail is replaced. + BitWriter w; + if (!info.vui_present) + { + w.copy(rbsp, 0, parsed->vui_flag_pos); + w.bit(1); // vui_parameters_present_flag + // Minimal VUI: nothing present except bitstream_restriction. + for (int i = 0; i < 8; ++i) + w.bit(0); // aspect ratio, overscan, video signal, chroma loc, timing, nal hrd, vcl hrd, pic_struct + w.bit(1); // bitstream_restriction_flag + } + else + { + w.copy(rbsp, 0, parsed->restriction_flag_pos); + w.bit(1); // bitstream_restriction_flag + } + w.bit(1); // motion_vectors_over_pic_boundaries_flag + w.ue(0); // max_bytes_per_pic_denom + w.ue(1); // max_bits_per_mb_denom + w.ue(15); // log2_max_mv_length_horizontal + w.ue(15); // log2_max_mv_length_vertical + w.ue(0); // max_num_reorder_frames + w.ue(std::max(info.max_num_ref_frames, 1)); // max_dec_frame_buffering + const std::vector new_rbsp = w.finish(); + const std::vector payload = escape(new_rbsp); + std::vector out(annexb, annexb + sc); + out.insert(out.end(), { 0, 0, 0, 1 }); + out.insert(out.end(), payload.begin(), payload.end()); + out.insert(out.end(), annexb + end, annexb + size); + return out; +} + +} // namespace h264 +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/avs1.hpp b/src/video_stage/wire/inc/video_stage/avs1.hpp new file mode 100644 index 0000000000..d38f16873b --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/avs1.hpp @@ -0,0 +1,75 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +namespace video_stage +{ +namespace avs1 +{ + +/// AVS1 version 1, one mono stream with one sample per record. The layout is the repository's AVS1 wire format +/// (examples/camera_streamer/argus_sender/WIRE_FORMAT.md): a 4-byte big-endian record length, a 64-byte bundle +/// header, one 40-byte stream descriptor, one 72-byte sample descriptor, then the payload. Two things are specific +/// to this use: +/// - codec 2 is H.264: one Annex-B access unit per record, with SPS and PPS before every IDR; +/// - `frame_number` is the source frame id and `frame_timestamp_ns` carries the source stamp on the sender's own +/// monotonic clock, the same value the LiveKit track publishes. Neither is a wall-clock time. A zero stamp and a +/// zero frame number mean the sender could not tie the unit to a capture: the receiver publishes it without a +/// capture time. +constexpr uint16_t kVersion = 1; +constexpr uint32_t kHeaderBytes = 64; +constexpr uint32_t kStreamBytes = 40; +constexpr uint32_t kSampleBytes = 72; +constexpr uint32_t kMaxRecordBytes = 64U * 1024U * 1024U; + +enum class Codec : uint8_t +{ + RawNv12 = 0, + Hevc = 1, + H264 = 2, +}; + +/// What one record carries: the picture description and its identity. +struct Sample +{ + Codec codec = Codec::H264; + uint32_t width = 0; + uint32_t height = 0; + uint32_t fps_numerator = 30; + uint64_t bundle_sequence = 0; + uint64_t session_id = 0; + uint64_t frame_number = 0; + uint64_t frame_timestamp_ns = 0; +}; + +/// A parsed record. `payload` points into the buffer given to parse(); it is valid while that buffer is. +struct Record +{ + Sample sample; + const uint8_t* payload = nullptr; + size_t payload_size = 0; +}; + +/// Appends one complete record (length prefix included) to `out`. `payload` is one access unit. +void encode(const Sample& sample, const uint8_t* payload, size_t payload_size, std::vector& out); + +/// Parses the bytes after the 4-byte length prefix. Returns nullopt and sets `error` when the record is not a valid +/// version-1 mono H.264 record: bad magic or version, nonzero reserved fields, unknown flags, inconsistent presence, +/// a payload range outside the record, or an empty access unit. +std::optional parse(const uint8_t* data, size_t size, std::string& error); + +/// The record length from its 4-byte big-endian prefix. +uint32_t read_length(const uint8_t prefix[4]); + +/// True when an Annex-B access unit contains an IDR slice (NAL type 5). +bool contains_idr(const uint8_t* au, size_t size); + +} // namespace avs1 +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/clock_exchange.hpp b/src/video_stage/wire/inc/video_stage/clock_exchange.hpp new file mode 100644 index 0000000000..6d0e48dad2 --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/clock_exchange.hpp @@ -0,0 +1,152 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// One completed four-timestamp exchange: t1/t4 on the local clock, t2/t3 on the remote's. Nanoseconds. +struct Exchange +{ + int64_t t1 = 0; + int64_t t2 = 0; + int64_t t3 = 0; + int64_t t4 = 0; + + int64_t rtt_ns() const + { + return (t4 - t1) - (t3 - t2); + } + /// Remote minus local, assuming symmetric transit. + int64_t offset_ns() const + { + return ((t2 - t1) + (t3 - t4)) / 2; + } + bool well_formed() const + { + return t4 >= t1 && t3 >= t2 && rtt_ns() >= 0; + } +}; + +struct ClockMapping +{ + int64_t offset_ns = 0; ///< remote minus local + int64_t rtt_ns = 0; + /// Half the round trip plus the drift the exchange's age allows: what unknown asymmetry and rate can cause. + int64_t uncertainty_ns = 0; + int64_t accepted_at_ns = 0; + + int64_t to_local_ns(int64_t remote_ns) const + { + return remote_ns - offset_ns; + } +}; + +/** + * @brief Maps a remote monotonic clock onto this one from four-timestamp exchanges. + * + * Same rules as the Python estimator the recorder uses (egocap_web/clock_exchange.py): the best recent exchange by + * uncertainty, a mapping withheld when no exchange arrived within the stale limit, a new exchange that disagrees with + * the current mapping by more than both uncertainties plus a drift allowance flagged as a discontinuity, and the + * uncertainty aged to the time of the read. Not thread-safe; the owner serialises access. + */ +class ClockEstimator +{ +public: + static constexpr int64_t kMaxRttNs = 250'000'000; + static constexpr int64_t kStaleAfterNs = 5'000'000'000; + static constexpr size_t kWindow = 8; + static constexpr double kDriftAllowance = 200e-6; + + /// Why the exchange was rejected, or empty when accepted. + std::string add(const Exchange& e) + { + if (!e.well_formed()) + return "malformed exchange"; + if (e.rtt_ns() > kMaxRttNs) + return "round trip too long"; + const std::optional previous = m_current; + m_newest_ns = e.t4; + const ClockMapping newest = mapping_of(e, e.t4); + if (previous && disagrees(*previous, newest)) + { + m_discontinuity = true; + m_recent.clear(); + m_recent.push_back(e); + m_current = mapping_of(e, e.t4); + } + else + { + m_recent.push_back(e); + if (m_recent.size() > kWindow) + m_recent.pop_front(); + const Exchange* best = &m_recent.front(); + for (const Exchange& c : m_recent) + { + const int64_t u = uncertainty(c, e.t4); + const int64_t bu = uncertainty(*best, e.t4); + if (u < bu || (u == bu && c.t4 > best->t4)) + best = &c; + } + m_current = mapping_of(*best, e.t4); + } + return {}; + } + + /// The mapping at `now_ns` with its uncertainty aged to then, or nullopt when there is none or it is stale. + std::optional mapping(int64_t now_ns) const + { + if (!m_current || !m_newest_ns || now_ns - *m_newest_ns > kStaleAfterNs) + return std::nullopt; + ClockMapping aged = *m_current; + const int64_t age = std::max(0, now_ns - aged.accepted_at_ns); + aged.uncertainty_ns = + std::max(aged.uncertainty_ns, aged.rtt_ns / 2 + static_cast(kDriftAllowance * age)); + return aged; + } + + /// Set when the clocks jumped apart; frames already mapped keep their mapping. The owner counts the event and + /// clears it; the estimator has already restarted from the exchange that showed it. + bool discontinuity() const + { + return m_discontinuity; + } + void clear_discontinuity() + { + m_discontinuity = false; + } + +private: + static int64_t uncertainty(const Exchange& e, int64_t now_ns) + { + return e.rtt_ns() / 2 + static_cast(kDriftAllowance * std::max(0, now_ns - e.t4)); + } + + static ClockMapping mapping_of(const Exchange& e, int64_t now_ns) + { + return ClockMapping{ e.offset_ns(), e.rtt_ns(), uncertainty(e, now_ns), e.t4 }; + } + + static bool disagrees(const ClockMapping& previous, const ClockMapping& candidate) + { + const int64_t elapsed = std::max(0, candidate.accepted_at_ns - previous.accepted_at_ns); + const double allowed = static_cast(previous.uncertainty_ns + candidate.uncertainty_ns) + + kDriftAllowance * static_cast(elapsed); + const int64_t delta = candidate.offset_ns - previous.offset_ns; + return static_cast(delta < 0 ? -delta : delta) > allowed; + } + + std::deque m_recent; + std::optional m_current; + std::optional m_newest_ns; + bool m_discontinuity = false; +}; + +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/clock_sync.hpp b/src/video_stage/wire/inc/video_stage/clock_sync.hpp new file mode 100644 index 0000000000..f084e23c59 --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/clock_sync.hpp @@ -0,0 +1,118 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// The publisher's half of the PC's four-timestamp clock exchange: parse a request, build the reply. All times are +// CLOCK_MONOTONIC nanoseconds, the clock the PC maps. Wire format (little-endian), shared with +// examples/egocap/backend/egocap_web/livekit_clock_sync.py and the egocap emulator: +// request, 13 bytes: 'Q', u32 sequence, i64 t1 (the requester's clock) +// reply, 29 bytes: 'R', u32 sequence, i64 t1 (both echoed), i64 t2 (receipt), i64 t3 (reply send) +// No LiveKit type appears here, so the format is testable on its own. + +#pragma once + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace sensing +{ + +inline constexpr const char* kClockSyncTopic = "isaaccapture.clock-sync"; +inline constexpr std::size_t kClockSyncRequestSize = 13; +inline constexpr std::size_t kClockSyncReplySize = 29; +/// Byte offset of t3 in a reply, so it can be written as late as possible, just before the send. +inline constexpr std::size_t kClockSyncReplyT3Offset = 21; + +struct ClockSyncRequest +{ + std::uint32_t sequence = 0; + std::int64_t t1 = 0; +}; + +inline std::int64_t monotonic_ns() +{ + timespec ts{}; + clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * 1'000'000'000LL + ts.tv_nsec; +} + +inline void put_le(std::vector& out, std::size_t at, std::uint64_t value, std::size_t bytes) +{ + for (std::size_t i = 0; i < bytes; ++i) + out[at + i] = static_cast((value >> (8 * i)) & 0xFF); +} + +inline std::uint64_t get_le(const std::vector& in, std::size_t at, std::size_t bytes) +{ + std::uint64_t value = 0; + for (std::size_t i = 0; i < bytes; ++i) + value |= static_cast(in[at + i]) << (8 * i); + return value; +} + +/// The request in `data`, or nullopt for anything that is not exactly a request. +inline std::optional parse_clock_sync_request(const std::vector& data) +{ + if (data.size() != kClockSyncRequestSize || data[0] != 'Q') + return std::nullopt; + ClockSyncRequest request; + request.sequence = static_cast(get_le(data, 1, 4)); + request.t1 = static_cast(get_le(data, 5, 8)); + return request; +} + +inline std::vector build_clock_sync_reply(const ClockSyncRequest& request, std::int64_t t2, std::int64_t t3) +{ + std::vector reply(kClockSyncReplySize); + reply[0] = 'R'; + put_le(reply, 1, request.sequence, 4); + put_le(reply, 5, static_cast(request.t1), 8); + put_le(reply, 13, static_cast(t2), 8); + put_le(reply, kClockSyncReplyT3Offset, static_cast(t3), 8); + return reply; +} + +/// The requester's half of the same exchange, for a peer that sends requests (the PC video stage over its control +/// link). +inline std::vector build_clock_sync_request(const ClockSyncRequest& request) +{ + std::vector out(kClockSyncRequestSize); + out[0] = 'Q'; + put_le(out, 1, request.sequence, 4); + put_le(out, 5, static_cast(request.t1), 8); + return out; +} + +struct ClockSyncReply +{ + std::uint32_t sequence = 0; + std::int64_t t1 = 0; + std::int64_t t2 = 0; + std::int64_t t3 = 0; +}; + +/// The reply in `data`, or nullopt for anything that is not exactly a reply. +inline std::optional parse_clock_sync_reply(const std::vector& data) +{ + if (data.size() != kClockSyncReplySize || data[0] != 'R') + return std::nullopt; + ClockSyncReply reply; + reply.sequence = static_cast(get_le(data, 1, 4)); + reply.t1 = static_cast(get_le(data, 5, 8)); + reply.t2 = static_cast(get_le(data, 13, 8)); + reply.t3 = static_cast(get_le(data, kClockSyncReplyT3Offset, 8)); + return reply; +} + +/// Writes t3 into an already built reply. +inline void set_clock_sync_reply_t3(std::vector& reply, std::int64_t t3) +{ + put_le(reply, kClockSyncReplyT3Offset, static_cast(t3), 8); +} + +} // namespace sensing +} // namespace plugins diff --git a/src/video_stage/wire/inc/video_stage/encoded_video_frame.hpp b/src/video_stage/wire/inc/video_stage/encoded_video_frame.hpp new file mode 100644 index 0000000000..cc13bc95b7 --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/encoded_video_frame.hpp @@ -0,0 +1,32 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace video_stage +{ + +/// One complete encoded H.264 access unit, with what accompanies it. +struct EncodedVideoFrame +{ + /// Annex-B bytes: an IDR carries its SPS/PPS in front of it. + const uint8_t* data = nullptr; + size_t size = 0; + /// The unit is an IDR, decodable on its own. + bool keyframe = false; + /// CLOCK_MONOTONIC microseconds at capture: the host timeline, not sensor time. + uint64_t timestamp_us = 0; + /// The frame's application timestamp in microseconds, on the clock its stream declares. Zero means the publisher + /// cannot vouch for a time, and the frame carries none. + uint64_t user_timestamp_us = 0; + /// The camera's own frame sequence. + uint32_t frame_id = 0; + /// The sensor's capture time on its own clock, carried as user data so a consumer can pair + /// frames from different sensors. + uint64_t capture_tsc = 0; +}; + +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/frame_correlator.hpp b/src/video_stage/wire/inc/video_stage/frame_correlator.hpp new file mode 100644 index 0000000000..d18bbef440 --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/frame_correlator.hpp @@ -0,0 +1,103 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace video_stage +{ + +/// What is known about a frame that went into the encoder. +struct InFlightFrame +{ + std::uint32_t sequence = 0; + std::uint64_t capture_tsc = 0; + /// The application timestamp to publish with the frame, in microseconds. + std::uint64_t stamp_us = 0; + /// The stamp given to the encoder with the input, in microseconds: the key a returned unit is matched by. + std::int64_t submitted_us = 0; +}; + +/** + * @brief Ties each encoded unit back to the frame it came from, and says when it cannot. + * + * Stamp mode credits a unit to a frame only when the stamp the encoder hands back equals one submitted; older frames + * the encoder never returned are skipped and counted, and a unit matching nothing is reported as not identified (the + * oldest in-flight entry is dropped so the queue stays bounded). Fifo mode assumes one unit per input in order and + * cannot be verified; it is an explicit choice for approximate timing. Worker thread only. + */ +class FrameCorrelator +{ +public: + enum class Mode + { + Stamp, + Fifo, + }; + + struct Result + { + InFlightFrame frame; + bool identified = true; + /// Inputs older than the match that the encoder did not return a unit for. + std::uint64_t skipped = 0; + }; + + explicit FrameCorrelator(Mode mode, std::size_t max_in_flight = 12) : m_mode(mode), m_max(max_in_flight) + { + } + + void submitted(const InFlightFrame& frame) + { + m_in_flight.push_back(frame); + // The encoder holds a bounded number of frames, so a longer queue is stale metadata. + while (m_in_flight.size() > m_max) + m_in_flight.pop_front(); + } + + /// The frame a unit stamped `unit_stamp_us` came from; nullopt when nothing is in flight. + std::optional returned(std::int64_t unit_stamp_us) + { + if (m_in_flight.empty()) + return std::nullopt; + Result result; + if (m_mode == Mode::Stamp) + { + const auto match = std::find_if(m_in_flight.begin(), m_in_flight.end(), + [&](const InFlightFrame& f) { return f.submitted_us == unit_stamp_us; }); + if (match != m_in_flight.end()) + { + result.skipped = static_cast(std::distance(m_in_flight.begin(), match)); + result.frame = *match; + m_in_flight.erase(m_in_flight.begin(), std::next(match)); + return result; + } + result.identified = false; + } + result.frame = m_in_flight.front(); + m_in_flight.pop_front(); + return result; + } + + bool empty() const + { + return m_in_flight.empty(); + } + std::size_t in_flight() const + { + return m_in_flight.size(); + } + +private: + Mode m_mode; + std::size_t m_max; + std::deque m_in_flight; +}; + +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/h264_access_units.hpp b/src/video_stage/wire/inc/video_stage/h264_access_units.hpp new file mode 100644 index 0000000000..5acdc4960c --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/h264_access_units.hpp @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +namespace video_stage +{ + +/// One access unit of an Annex-B stream, as a range of the input. +struct AccessUnitRange +{ + std::size_t offset = 0; + std::size_t size = 0; + /// The unit contains an IDR slice. + bool idr = false; +}; + +/** + * @brief Split an Annex-B H.264 stream into access units. + * + * A unit starts at the first NAL after a picture's slices, or at a picture whose first slice follows another + * picture directly; further slices of one picture stay with it. End-of-sequence, end-of-stream and filler NALs + * belong to the picture before them. Parameter sets and SEI stay with the picture that follows. + */ +std::vector split_access_units(const std::uint8_t* data, std::size_t size); + +} // namespace video_stage diff --git a/src/video_stage/wire/inc/video_stage/h264_sps.hpp b/src/video_stage/wire/inc/video_stage/h264_sps.hpp new file mode 100644 index 0000000000..7d1b72ccce --- /dev/null +++ b/src/video_stage/wire/inc/video_stage/h264_sps.hpp @@ -0,0 +1,56 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include + +namespace video_stage +{ +namespace h264 +{ + +/// The fields of an H.264 sequence parameter set that decide how a decoder buffers pictures, and where the VUI's +/// bitstream_restriction sits so it can be rewritten. +struct SpsInfo +{ + uint32_t profile_idc = 0; + uint32_t level_idc = 0; + uint32_t chroma_format_idc = 1; + uint32_t pic_order_cnt_type = 0; + uint32_t max_num_ref_frames = 0; + uint32_t width = 0; ///< coded width in luma pixels + uint32_t height = 0; ///< coded height in luma pixels + bool frame_mbs_only = true; + bool vui_present = false; + bool full_range = false; + /// The stream states where its chroma samples sit (vertical offsets, 0 = the default left/co-sited siting that + /// the example operation assumes). Absent means the default. + bool chroma_loc_present = false; + uint32_t chroma_loc_top = 0; + uint32_t chroma_loc_bottom = 0; + bool bitstream_restriction = false; + /// Valid when bitstream_restriction is set: how many pictures may precede one in decode order and follow it in + /// display order, and how many a decoder must hold. + uint32_t max_num_reorder_frames = 0; + uint32_t max_dec_frame_buffering = 0; + + /// One line for a log. + std::string describe() const; +}; + +/// The first SPS NAL (type 7) in an Annex-B access unit, parsed; nullopt if there is none or it is not understood. +std::optional parse_sps(const uint8_t* annexb, size_t size); + +/// A copy of the access unit whose SPS declares that the stream never reorders pictures: bitstream_restriction with +/// max_num_reorder_frames 0 and max_dec_frame_buffering equal to max_num_ref_frames (at least 1). Without that, a +/// decoder must assume the worst-case DPB for the level and holds back many pictures before showing the first. +/// Only call it for a stream that really has no B-frames. Returns nullopt if there is no SPS or it cannot be rewritten. +std::optional> declare_no_reordering(const uint8_t* annexb, size_t size); + +} // namespace h264 +} // namespace video_stage diff --git a/src/video_stage/wire/tests/CMakeLists.txt b/src/video_stage/wire/tests/CMakeLists.txt new file mode 100644 index 0000000000..c3bb897bf4 --- /dev/null +++ b/src/video_stage/wire/tests/CMakeLists.txt @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(video_stage_wire_tests + test_avs1.cpp + test_clock_exchange.cpp + test_clock_sync.cpp + test_encoded_video_frame.cpp + test_frame_correlator.cpp + test_h264_access_units.cpp + test_h264_sps.cpp +) + +target_link_libraries(video_stage_wire_tests PRIVATE video_stage::wire Catch2::Catch2WithMain) + +catch_discover_tests(video_stage_wire_tests) diff --git a/src/video_stage/wire/tests/test_avs1.cpp b/src/video_stage/wire/tests/test_avs1.cpp new file mode 100644 index 0000000000..af38f27637 --- /dev/null +++ b/src/video_stage/wire/tests/test_avs1.cpp @@ -0,0 +1,316 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include + +namespace avs1 = video_stage::avs1; + +namespace +{ + +// Record offsets (after the 4-byte length prefix). +constexpr size_t kStream = 64; +constexpr size_t kSample = 64 + 40; +constexpr size_t kPayload = 64 + 40 + 72; + +uint64_t be(const std::vector& b, size_t off, size_t n) +{ + uint64_t v = 0; + for (size_t i = 0; i < n; ++i) + v = (v << 8) | b.at(off + i); + return v; +} + +void set_be(std::vector& b, size_t off, size_t n, uint64_t v) +{ + for (size_t i = 0; i < n; ++i) + b.at(off + n - 1 - i) = static_cast(v >> (8 * i)); +} + +const std::vector kAu = { 0, 0, 0, 1, 0x67, 1, 2, 0, 0, 0, 1, 0x68, 3, 0, 0, 1, 0x65, 9, 9, 9 }; + +avs1::Sample make_sample() +{ + avs1::Sample s; + s.codec = avs1::Codec::H264; + s.width = 1280; + s.height = 720; + s.fps_numerator = 60; + s.bundle_sequence = 0x0102030405060708ULL; + s.session_id = 0x1112131415161718ULL; + s.frame_number = 0x2122232425262728ULL; + s.frame_timestamp_ns = 0x3132333435363738ULL; + return s; +} + +// Encoded record without the length prefix. +std::vector valid_record(const std::vector& payload = kAu) +{ + std::vector out; + avs1::encode(make_sample(), payload.data(), payload.size(), out); + return std::vector(out.begin() + 4, out.end()); +} + +bool rejects(const std::vector& rec) +{ + std::string error; + const auto r = avs1::parse(rec.data(), rec.size(), error); + return !r.has_value() && !error.empty(); +} + +} // namespace + +TEST_CASE("AVS1 round trip keeps every field", "[video_stage_wire]") +{ + const avs1::Sample s = make_sample(); + std::vector out; + avs1::encode(s, kAu.data(), kAu.size(), out); + REQUIRE(avs1::read_length(out.data()) == out.size() - 4); + + std::string error; + const auto r = avs1::parse(out.data() + 4, out.size() - 4, error); + REQUIRE(r.has_value()); + CHECK(r->sample.codec == avs1::Codec::H264); + CHECK(r->sample.width == s.width); + CHECK(r->sample.height == s.height); + CHECK(r->sample.fps_numerator == s.fps_numerator); + CHECK(r->sample.bundle_sequence == s.bundle_sequence); + CHECK(r->sample.session_id == s.session_id); + CHECK(r->sample.frame_number == s.frame_number); + CHECK(r->sample.frame_timestamp_ns == s.frame_timestamp_ns); + REQUIRE(r->payload_size == kAu.size()); + CHECK(std::memcmp(r->payload, kAu.data(), kAu.size()) == 0); + CHECK(r->payload == out.data() + 4 + kPayload); +} + +TEST_CASE("AVS1 encode follows WIRE_FORMAT.md byte for byte", "[video_stage_wire]") +{ + const avs1::Sample s = make_sample(); + std::vector out; + avs1::encode(s, kAu.data(), kAu.size(), out); + REQUIRE(out.size() == 4 + 64 + 40 + 72 + kAu.size()); + CHECK(be(out, 0, 4) == out.size() - 4); + + const std::vector r(out.begin() + 4, out.end()); + // HEADER "!4sHHIHHQQQQIIQ" + CHECK(std::memcmp(r.data(), "AVS1", 4) == 0); + CHECK(be(r, 4, 2) == 1); + CHECK(be(r, 6, 2) == 0); + CHECK(be(r, 8, 4) == 64); + CHECK(be(r, 12, 2) == 1); + CHECK(be(r, 14, 2) == 1); + CHECK(be(r, 16, 8) == s.bundle_sequence); + CHECK(be(r, 24, 8) == s.session_id); + CHECK(be(r, 32, 8) == 0); + CHECK(be(r, 40, 8) == 0); + CHECK(be(r, 48, 4) == 112); + CHECK(be(r, 52, 4) == kAu.size()); + CHECK(be(r, 56, 8) == 0); + // STREAM "!HHBBHIIIIQHHI" + CHECK(be(r, kStream + 0, 2) == 0); + CHECK(be(r, kStream + 2, 2) == 1); + CHECK(r[kStream + 4] == 2); + CHECK(r[kStream + 5] == 1); + CHECK(be(r, kStream + 6, 2) == 0); + CHECK(be(r, kStream + 8, 4) == s.width); + CHECK(be(r, kStream + 12, 4) == s.height); + CHECK(be(r, kStream + 16, 4) == s.fps_numerator); + CHECK(be(r, kStream + 20, 4) == 1); + CHECK(be(r, kStream + 24, 8) != 0); + CHECK(be(r, kStream + 32, 2) == 0); + CHECK(be(r, kStream + 34, 2) == 1); + CHECK(be(r, kStream + 36, 4) == 0); + // SAMPLE "!HBBHHIIQQQQQQII" + CHECK(be(r, kSample + 0, 2) == 0); + CHECK(r[kSample + 2] == 0); + CHECK(r[kSample + 3] == 0); + CHECK(be(r, kSample + 4, 2) == 0x0005); + CHECK(be(r, kSample + 6, 2) == 0); + CHECK(be(r, kSample + 16, 8) == s.frame_number); + CHECK(be(r, kSample + 32, 8) == s.frame_timestamp_ns); + CHECK(be(r, kSample + 64, 4) == 0); + CHECK(be(r, kSample + 68, 4) == kAu.size()); + CHECK(std::memcmp(r.data() + kPayload, kAu.data(), kAu.size()) == 0); +} + +TEST_CASE("AVS1 read_length is big endian", "[video_stage_wire]") +{ + const uint8_t p[4] = { 0x01, 0x02, 0x03, 0x04 }; + CHECK(avs1::read_length(p) == 0x01020304U); + const uint8_t q[4] = { 0xFF, 0xFF, 0xFF, 0xFF }; + CHECK(avs1::read_length(q) == 0xFFFFFFFFU); +} + +TEST_CASE("AVS1 parse rejects malformed records", "[video_stage_wire]") +{ + const std::vector good = valid_record(); + { + std::string error; + REQUIRE(avs1::parse(good.data(), good.size(), error).has_value()); + } + + struct Mutation + { + const char* name; + size_t offset; + size_t bytes; + uint64_t value; + }; + const Mutation mutations[] = { + { "bad magic", 0, 1, 'X' }, + { "version 2", 4, 2, 2 }, + { "version 0", 4, 2, 0 }, + { "bundle flags", 6, 2, 1 }, + { "bundle flags TAI", 6, 2, 2 }, + { "header_bytes", 8, 4, 65 }, + { "stream_count 0", 12, 2, 0 }, + { "stream_count 2", 12, 2, 2 }, + { "sample_count 0", 14, 2, 0 }, + { "sample_count 2", 14, 2, 2 }, + { "descriptor_bytes small", 48, 4, 111 }, + { "descriptor_bytes large", 48, 4, 113 }, + { "payload_bytes small", 52, 4, kAu.size() - 1 }, + { "payload_bytes large", 52, 4, kAu.size() + 1 }, + { "sender field offset 32", 32, 8, 1 }, + { "sender field offset 40", 40, 8, 1 }, + { "sender field offset 56", 56, 8, 1 }, + { "stream absent", kStream + 2, 2, 0 }, + { "stream stereo flag", kStream + 2, 2, 3 }, + { "codec raw", kStream + 4, 1, 0 }, + { "codec hevc", kStream + 4, 1, 1 }, + { "codec 3", kStream + 4, 1, 3 }, + { "pixel format 0", kStream + 5, 1, 0 }, + { "pixel format 2", kStream + 5, 1, 2 }, + { "rotation 180", kStream + 6, 2, 180 }, + { "fps denominator 0", kStream + 20, 4, 0 }, + { "first_sample 1", kStream + 32, 2, 1 }, + { "stream sample_count 0", kStream + 34, 2, 0 }, + { "stream sample_count 2", kStream + 34, 2, 2 }, + { "stream reserved", kStream + 36, 4, 1 }, + { "sample unknown flag 0x10", kSample + 4, 2, 0x0015 }, + { "sample unknown flag 0x8000", kSample + 4, 2, 0x8005 }, + { "sample not present", kSample + 4, 2, 0x0004 }, + { "sample no timestamp valid", kSample + 4, 2, 0x0001 }, + { "sample flags clear", kSample + 4, 2, 0 }, + { "sample reserved", kSample + 6, 2, 1 }, + { "payload length 0", kSample + 68, 4, 0 }, + { "payload offset 1", kSample + 64, 4, 1 }, + { "payload length mismatch", kSample + 68, 4, kAu.size() - 1 }, + }; + for (const Mutation& m : mutations) + { + DYNAMIC_SECTION(m.name) + { + std::vector rec = good; + set_be(rec, m.offset, m.bytes, m.value); + CHECK(rejects(rec)); + } + } + + SECTION("optional sample flags are accepted") + { + std::vector rec = good; + set_be(rec, kSample + 4, 2, 0x000F); + std::string error; + CHECK(avs1::parse(rec.data(), rec.size(), error).has_value()); + } + SECTION("record size does not match payload_bytes") + { + std::vector rec = good; + rec.push_back(0); + CHECK(rejects(rec)); + rec = good; + rec.pop_back(); + CHECK(rejects(rec)); + } + SECTION("empty payload") + { + std::vector rec(good.begin(), good.begin() + kPayload); + set_be(rec, 52, 4, 0); + set_be(rec, kSample + 68, 4, 0); + CHECK(rejects(rec)); + } + SECTION("short record") + { + CHECK(rejects(std::vector{})); + CHECK(rejects(std::vector(good.begin(), good.begin() + 10))); + } +} + +TEST_CASE("AVS1 rejects every proper prefix of a valid record", "[video_stage_wire]") +{ + const std::vector good = valid_record(); + for (size_t n = 0; n < good.size(); ++n) + { + std::string error; + const auto r = avs1::parse(good.data(), n, error); + INFO("prefix length " << n); + REQUIRE_FALSE(r.has_value()); + REQUIRE_FALSE(error.empty()); + } +} + +TEST_CASE("AVS1 parse survives single-byte corruption anywhere", "[video_stage_wire]") +{ + const std::vector good = valid_record(); + uint32_t state = 12345; + for (int i = 0; i < 4000; ++i) + { + state = state * 1664525U + 1013904223U; + std::vector rec = good; + rec[(state >> 8) % rec.size()] ^= static_cast(1U + (state >> 24) % 255U); + std::string error; + const auto r = avs1::parse(rec.data(), rec.size(), error); + if (r) + { + REQUIRE(r->payload >= rec.data()); + REQUIRE(r->payload + r->payload_size <= rec.data() + rec.size()); + } + } +} + +TEST_CASE("AVS1 contains_idr", "[video_stage_wire]") +{ + SECTION("3-byte start code IDR") + { + const uint8_t au[] = { 0, 0, 1, 0x65, 1, 2, 3 }; + CHECK(avs1::contains_idr(au, sizeof(au))); + } + SECTION("4-byte start code IDR") + { + const uint8_t au[] = { 0, 0, 0, 1, 0x65, 1, 2, 3 }; + CHECK(avs1::contains_idr(au, sizeof(au))); + } + SECTION("SPS, PPS, IDR") + { + CHECK(avs1::contains_idr(kAu.data(), kAu.size())); + } + SECTION("IDR with other nal_ref_idc") + { + const uint8_t au[] = { 0, 0, 0, 1, 0x25, 1 }; + CHECK(avs1::contains_idr(au, sizeof(au))); + } + SECTION("SPS and PPS only") + { + const uint8_t au[] = { 0, 0, 0, 1, 0x67, 1, 2, 0, 0, 0, 1, 0x68, 3 }; + CHECK_FALSE(avs1::contains_idr(au, sizeof(au))); + } + SECTION("non-IDR slice") + { + const uint8_t au[] = { 0, 0, 0, 1, 0x41, 1, 2, 3 }; + CHECK_FALSE(avs1::contains_idr(au, sizeof(au))); + } + SECTION("empty and tiny inputs") + { + CHECK_FALSE(avs1::contains_idr(nullptr, 0)); + const uint8_t au[] = { 0, 0, 1 }; + CHECK_FALSE(avs1::contains_idr(au, sizeof(au))); + } +} diff --git a/src/video_stage/wire/tests/test_clock_exchange.cpp b/src/video_stage/wire/tests/test_clock_exchange.cpp new file mode 100644 index 0000000000..db80ccfc8b --- /dev/null +++ b/src/video_stage/wire/tests/test_clock_exchange.cpp @@ -0,0 +1,229 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include + +using video_stage::ClockEstimator; +using video_stage::Exchange; + +namespace +{ +constexpr int64_t MS = 1'000'000; +constexpr int64_t S = 1'000'000'000; + +// An exchange between a local clock and a remote clock running `offset` ahead of it. +Exchange make_exchange(int64_t t1, int64_t offset = 0, int64_t forward = MS, int64_t back = MS, int64_t processing = 0) +{ + const int64_t arrival = t1 + forward; + const int64_t departure = arrival + processing; + return Exchange{ t1, arrival + offset, departure + offset, departure + back }; +} +} // namespace + +TEST_CASE("symmetric delays recover offset, rtt excludes processing", "[Clock]") +{ + const Exchange ex = make_exchange(10 * S, 123'456'000, 3 * MS, 3 * MS, 40 * MS); + CHECK(ex.offset_ns() == 123'456'000); + CHECK(ex.rtt_ns() == 6 * MS); + + ClockEstimator est; + CHECK(est.add(ex).empty()); + const auto m = est.mapping(ex.t4); + REQUIRE(m); + CHECK(m->offset_ns == 123'456'000); + CHECK(m->rtt_ns == 6 * MS); + CHECK(m->uncertainty_ns == 3 * MS); + CHECK(m->to_local_ns(S + 123'456'000) == S); +} + +TEST_CASE("asymmetric delay error is half the difference, within uncertainty", "[Clock]") +{ + const int64_t pairs[][2] = { { 1 * MS, 9 * MS }, { 9 * MS, 1 * MS }, { 2 * MS, 2 * MS } }; + for (const auto& p : pairs) + { + const Exchange ex = make_exchange(S, 5 * MS, p[0], p[1], 7 * MS); + ClockEstimator est; + est.add(ex); + const auto m = est.mapping(ex.t4); + REQUIRE(m); + const int64_t error = m->offset_ns - 5 * MS; + CHECK(error == (p[0] - p[1]) / 2); + CHECK(std::abs(error) <= m->uncertainty_ns); + } +} + +TEST_CASE("negative offset is recovered", "[Clock]") +{ + const Exchange ex = make_exchange(20 * S, -250 * MS, 2 * MS, 2 * MS, MS); + ClockEstimator est; + est.add(ex); + CHECK(est.mapping(ex.t4)->offset_ns == -250 * MS); +} + +TEST_CASE("malformed and slow exchanges are rejected without changing the mapping", "[Clock]") +{ + const Exchange bad[] = { + make_exchange(S, 0, 200 * MS, 100 * MS), // rtt 300 ms + Exchange{ 100, 0, 0, 50 }, // t4 before t1 + Exchange{ 0, 100, 50, 1000 }, // t3 before t2 + Exchange{ 0, 0, 900, 800 }, // negative rtt + }; + for (const Exchange& b : bad) + { + ClockEstimator est; + est.add(make_exchange(S, MS)); + const auto before = est.mapping(2 * S); + REQUIRE(before); + CHECK_FALSE(est.add(b).empty()); + const auto after = est.mapping(2 * S); + REQUIRE(after); + CHECK(after->offset_ns == before->offset_ns); + CHECK(after->rtt_ns == before->rtt_ns); + CHECK(after->accepted_at_ns == before->accepted_at_ns); + CHECK(after->uncertainty_ns == before->uncertainty_ns); + CHECK_FALSE(est.discontinuity()); + } +} + +TEST_CASE("best of the window is the lowest rtt", "[Clock]") +{ + ClockEstimator est; + const int64_t delays[] = { 5 * MS, 3 * MS, 8 * MS, 2 * MS, 6 * MS }; + int64_t t = S; + for (int64_t d : delays) + { + est.add(make_exchange(t, MS, d, d)); + t += S; + } + CHECK(est.mapping(6 * S)->rtt_ns == 4 * MS); +} + +TEST_CASE("high rtt outlier does not replace the better mapping; uncertainty ages at read", "[Clock]") +{ + ClockEstimator est; + est.add(make_exchange(1 * S, 10 * MS, MS, MS)); + const auto good = est.mapping(2 * S); + REQUIRE(good); + est.add(make_exchange(2 * S, 10 * MS, 20 * MS, 20 * MS)); + const auto m = est.mapping(3 * S); + REQUIRE(m); + CHECK(m->rtt_ns == good->rtt_ns); + CHECK(m->accepted_at_ns == good->accepted_at_ns); + CHECK(m->uncertainty_ns == good->rtt_ns / 2 + static_cast(200e-6 * (3 * S - good->accepted_at_ns))); +} + +TEST_CASE("uncertainty ages between exchanges without a new one", "[Clock]") +{ + ClockEstimator est; + const Exchange ex = make_exchange(10 * S, 5 * MS, 2 * MS, 2 * MS); + est.add(ex); + const auto at_add = est.mapping(ex.t4); + REQUIRE(at_add); + CHECK(at_add->uncertainty_ns == ex.rtt_ns() / 2); + const auto later = est.mapping(ex.t4 + 3 * S); + REQUIRE(later); + CHECK(later->uncertainty_ns == ex.rtt_ns() / 2 + static_cast(200e-6 * 3 * S)); + CHECK(later->offset_ns == at_add->offset_ns); + CHECK(later->accepted_at_ns == at_add->accepted_at_ns); +} + +TEST_CASE("steady link keeps choosing the newest exchange", "[Clock]") +{ + ClockEstimator est; + for (int s = 1; s <= 30; ++s) + { + const Exchange ex = make_exchange(s * S, 7 * MS, 2 * MS, 2 * MS); + CHECK(est.add(ex).empty()); + const auto m = est.mapping(ex.t4); + REQUIRE(m); + CHECK(m->accepted_at_ns == ex.t4); + CHECK(m->uncertainty_ns == 2 * MS); + } + CHECK_FALSE(est.discontinuity()); +} + +TEST_CASE("mapping is none before any exchange and after the stale window", "[Clock]") +{ + ClockEstimator est; + CHECK_FALSE(est.mapping(0)); + const Exchange ex = make_exchange(S, MS); + est.add(ex); + CHECK(est.mapping(ex.t4)); + CHECK(est.mapping(ex.t4 + 5 * S)); + CHECK_FALSE(est.mapping(ex.t4 + 5 * S + 1)); + + // A new exchange brings it back. + est.add(make_exchange(70 * S, MS)); + CHECK(est.mapping(70 * S + 2 * MS)); +} + +TEST_CASE("rejected exchanges do not refresh staleness", "[Clock]") +{ + ClockEstimator est; + const Exchange ex = make_exchange(S, MS); + est.add(ex); + const int64_t late = ex.t4 + 5 * S + 1; + REQUIRE_FALSE(est.mapping(late)); + CHECK_FALSE(est.add(make_exchange(3 * S, MS, 200 * MS, 100 * MS)).empty()); + CHECK_FALSE(est.add(Exchange{ 6 * S, 0, 0, 5 * S }).empty()); + CHECK_FALSE(est.mapping(late)); +} + +TEST_CASE("a clock step is a discontinuity and restarts from the new exchange", "[Clock]") +{ + ClockEstimator est; + for (int s = 1; s <= 5; ++s) + est.add(make_exchange(s * S, 0, 2 * MS, 2 * MS)); + CHECK_FALSE(est.discontinuity()); + + // Same rtt as before: the step alone must flag it. + const Exchange step = make_exchange(6 * S, 50 * MS, 2 * MS, 2 * MS); + CHECK(est.add(step).empty()); + CHECK(est.discontinuity()); + const auto m = est.mapping(step.t4); + REQUIRE(m); + CHECK(m->offset_ns == 50 * MS); + CHECK(m->accepted_at_ns == step.t4); + + // Sticky until cleared; later good exchanges do not clear it. + est.add(make_exchange(7 * S, 50 * MS, MS, MS)); + CHECK(est.discontinuity()); + CHECK(est.mapping(7 * S + 2 * MS)->offset_ns == 50 * MS); + + est.clear_discontinuity(); + CHECK_FALSE(est.discontinuity()); + est.add(make_exchange(8 * S, 50 * MS, MS, MS)); + CHECK_FALSE(est.discontinuity()); +} + +TEST_CASE("a lower-rtt step does not mix old exchanges into the new mapping", "[Clock]") +{ + ClockEstimator est; + for (int s = 1; s <= 3; ++s) + est.add(make_exchange(s * S, 0, 2 * MS, 2 * MS)); + est.add(make_exchange(4 * S, 50 * MS, MS, MS)); + CHECK(est.discontinuity()); + CHECK(est.mapping(4 * S + 2 * MS)->offset_ns == 50 * MS); +} + +TEST_CASE("drift stays continuous and tracks within uncertainty plus drift", "[Clock]") +{ + ClockEstimator est; + const double ppm = 50.0; + for (int s = 1; s <= 30; ++s) + { + const int64_t t1 = s * S; + auto remote = [&](int64_t local) { return local + static_cast(local * ppm * 1e-6); }; + const Exchange ex{ t1, remote(t1 + MS), remote(t1 + MS), t1 + 2 * MS }; + CHECK(est.add(ex).empty()); + const auto m = est.mapping(ex.t4); + REQUIRE(m); + const double true_offset = ppm * 1e-6 * ex.t4; + const double drift = ppm * 1e-6 * (ex.t4 - m->accepted_at_ns); + CHECK(std::abs(m->offset_ns - true_offset) <= m->uncertainty_ns + drift + 2); + } + CHECK_FALSE(est.discontinuity()); +} diff --git a/src/video_stage/wire/tests/test_clock_sync.cpp b/src/video_stage/wire/tests/test_clock_sync.cpp new file mode 100644 index 0000000000..3e67150712 --- /dev/null +++ b/src/video_stage/wire/tests/test_clock_sync.cpp @@ -0,0 +1,134 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include + +using plugins::sensing::build_clock_sync_reply; +using plugins::sensing::ClockSyncRequest; +using plugins::sensing::kClockSyncReplySize; +using plugins::sensing::kClockSyncRequestSize; +using plugins::sensing::monotonic_ns; +using plugins::sensing::parse_clock_sync_request; +using plugins::sensing::set_clock_sync_reply_t3; + +namespace +{ + +using Bytes = std::vector; + +Bytes from_hex(const std::string& hex) +{ + Bytes out; + for (std::size_t i = 0; i + 1 < hex.size(); i += 2) + out.push_back(static_cast(std::stoul(hex.substr(i, 2), nullptr, 16))); + return out; +} + +// Golden bytes produced by egocap_web.livekit_clock_sync.encode_request and +// tools/livekit_orin_emulator.build_sync_reply for the inputs named beside each constant. +// sequence 1, t1 1, t2 2, t3 3 +const char* const kSimpleRequest = "51010000000100000000000000"; +const char* const kSimpleReply = "5201000000010000000000000002000000000000000300000000000000"; +// sequence 0xFFFFFFFF, t1 -1234567890123, t2 INT64_MAX, t3 INT64_MIN +const char* const kExtremeRequest = "51ffffffff35fb048ee0feffff"; +const char* const kExtremeReply = "52ffffffff35fb048ee0feffffffffffffffffff7f0000000000000080"; + +} // namespace + +TEST_CASE("parse_clock_sync_request accepts a well-formed request", "[clock_sync]") +{ + auto request = parse_clock_sync_request(from_hex(kSimpleRequest)); + REQUIRE(request.has_value()); + CHECK(request->sequence == 1); + CHECK(request->t1 == 1); + + request = parse_clock_sync_request(from_hex(kExtremeRequest)); + REQUIRE(request.has_value()); + CHECK(request->sequence == 0xFFFFFFFFu); + CHECK(request->t1 == -1234567890123LL); +} + +TEST_CASE("parse_clock_sync_request rejects anything else", "[clock_sync]") +{ + for (std::size_t size : { std::size_t{ 0 }, std::size_t{ 12 }, std::size_t{ 14 }, std::size_t{ 29 } }) + { + Bytes data(size, 0); + if (size > 0) + data[0] = 'Q'; + CHECK_FALSE(parse_clock_sync_request(data).has_value()); + } + + Bytes wrong_kind = from_hex(kSimpleRequest); + wrong_kind[0] = 'X'; + CHECK_FALSE(parse_clock_sync_request(wrong_kind).has_value()); + + CHECK_FALSE(parse_clock_sync_request(from_hex(kSimpleReply)).has_value()); + + Bytes reply_kind_at_request_size = from_hex(kSimpleRequest); + reply_kind_at_request_size[0] = 'R'; + CHECK_FALSE(parse_clock_sync_request(reply_kind_at_request_size).has_value()); +} + +TEST_CASE("build_clock_sync_reply lays out the documented fields", "[clock_sync]") +{ + const Bytes reply = build_clock_sync_reply(ClockSyncRequest{ 1, 1 }, 2, 3); + REQUIRE(reply.size() == kClockSyncReplySize); + CHECK(reply[0] == 'R'); + CHECK(reply == from_hex(kSimpleReply)); + + // Explicit offsets, little-endian. + CHECK(reply[1] == 1); + CHECK(reply[5] == 1); + CHECK(reply[13] == 2); + CHECK(reply[21] == 3); +} + +TEST_CASE("build_clock_sync_reply handles large and negative values", "[clock_sync]") +{ + const ClockSyncRequest request{ 0xFFFFFFFFu, -1234567890123LL }; + const Bytes reply = build_clock_sync_reply( + request, std::numeric_limits::max(), std::numeric_limits::min()); + CHECK(reply == from_hex(kExtremeReply)); +} + +TEST_CASE("clock sync request and reply round trip through parse", "[clock_sync]") +{ + const Bytes request_bytes = from_hex(kExtremeRequest); + REQUIRE(request_bytes.size() == kClockSyncRequestSize); + const auto request = parse_clock_sync_request(request_bytes); + REQUIRE(request.has_value()); + const Bytes reply = build_clock_sync_reply( + *request, std::numeric_limits::max(), std::numeric_limits::min()); + CHECK(reply == from_hex(kExtremeReply)); +} + +TEST_CASE("set_clock_sync_reply_t3 changes only the t3 bytes", "[clock_sync]") +{ + const Bytes before = build_clock_sync_reply(ClockSyncRequest{ 7, -5 }, 11, 13); + Bytes after = before; + set_clock_sync_reply_t3(after, 0x0102030405060708LL); + + REQUIRE(after.size() == before.size()); + for (std::size_t i = 0; i < before.size(); ++i) + { + if (i >= 21 && i <= 28) + CHECK(after[i] == static_cast(0x08 - (i - 21))); + else + CHECK(after[i] == before[i]); + } + CHECK(after == build_clock_sync_reply(ClockSyncRequest{ 7, -5 }, 11, 0x0102030405060708LL)); +} + +TEST_CASE("monotonic_ns does not go backwards", "[clock_sync]") +{ + const std::int64_t a = monotonic_ns(); + const std::int64_t b = monotonic_ns(); + CHECK(a > 0); + CHECK(b >= a); +} diff --git a/src/video_stage/wire/tests/test_encoded_video_frame.cpp b/src/video_stage/wire/tests/test_encoded_video_frame.cpp new file mode 100644 index 0000000000..7445a6c231 --- /dev/null +++ b/src/video_stage/wire/tests/test_encoded_video_frame.cpp @@ -0,0 +1,17 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +TEST_CASE("a default EncodedVideoFrame is empty and not a keyframe", "[EncodedVideoFrame]") +{ + const video_stage::EncodedVideoFrame frame; + CHECK(frame.data == nullptr); + CHECK(frame.size == 0); + CHECK_FALSE(frame.keyframe); + CHECK(frame.timestamp_us == 0); + CHECK(frame.user_timestamp_us == 0); + CHECK(frame.frame_id == 0); + CHECK(frame.capture_tsc == 0); +} diff --git a/src/video_stage/wire/tests/test_frame_correlator.cpp b/src/video_stage/wire/tests/test_frame_correlator.cpp new file mode 100644 index 0000000000..9fc7707171 --- /dev/null +++ b/src/video_stage/wire/tests/test_frame_correlator.cpp @@ -0,0 +1,176 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include + +using video_stage::FrameCorrelator; +using video_stage::InFlightFrame; + +namespace +{ + +InFlightFrame frame(std::uint32_t sequence, std::int64_t submitted_us) +{ + InFlightFrame f; + f.sequence = sequence; + f.capture_tsc = 1000 + sequence; + f.stamp_us = 5000 + sequence; + f.submitted_us = submitted_us; + return f; +} + +} // namespace + +TEST_CASE("stamp mode matches returned units in order", "[frame_correlator]") +{ + FrameCorrelator c(FrameCorrelator::Mode::Stamp); + for (std::uint32_t i = 0; i < 5; ++i) + c.submitted(frame(i, 100 + 33 * i)); + for (std::uint32_t i = 0; i < 5; ++i) + { + const auto r = c.returned(100 + 33 * i); + REQUIRE(r.has_value()); + CHECK(r->identified); + CHECK(r->skipped == 0); + CHECK(r->frame.sequence == i); + CHECK(r->frame.capture_tsc == 1000 + i); + } + CHECK(c.empty()); +} + +TEST_CASE("stamp mode counts a skipped input and keeps matching", "[frame_correlator]") +{ + FrameCorrelator c(FrameCorrelator::Mode::Stamp); + c.submitted(frame(0, 10)); + c.submitted(frame(1, 20)); + c.submitted(frame(2, 30)); + auto a = c.returned(10); + REQUIRE(a); + CHECK(a->frame.sequence == 0); + auto third = c.returned(30); + REQUIRE(third); + CHECK(third->identified); + CHECK(third->frame.sequence == 2); + CHECK(third->skipped == 1); + CHECK(c.empty()); + + c.submitted(frame(3, 40)); + auto later = c.returned(40); + REQUIRE(later); + CHECK(later->identified); + CHECK(later->frame.sequence == 3); + CHECK(later->skipped == 0); +} + +TEST_CASE("stamp mode reports a unit that matches nothing as not identified", "[frame_correlator]") +{ + FrameCorrelator c(FrameCorrelator::Mode::Stamp); + c.submitted(frame(0, 10)); + c.submitted(frame(1, 20)); + c.submitted(frame(2, 30)); + const auto r = c.returned(999); + REQUIRE(r); + CHECK_FALSE(r->identified); + CHECK(r->skipped == 0); + CHECK(r->frame.sequence == 0); // the oldest, removed and not credited + CHECK(c.in_flight() == 2); + + const auto next = c.returned(20); + REQUIRE(next); + CHECK(next->identified); + CHECK(next->frame.sequence == 1); + CHECK(next->skipped == 0); + const auto last = c.returned(30); + REQUIRE(last); + CHECK(last->identified); + CHECK(last->frame.sequence == 2); +} + +TEST_CASE("returned yields nothing when no frame is in flight", "[frame_correlator]") +{ + for (auto mode : { FrameCorrelator::Mode::Stamp, FrameCorrelator::Mode::Fifo }) + { + FrameCorrelator c(mode); + CHECK_FALSE(c.returned(1).has_value()); + c.submitted(frame(0, 1)); + REQUIRE(c.returned(1)); + CHECK_FALSE(c.returned(1).has_value()); + } +} + +TEST_CASE("the in-flight queue is bounded by dropping the oldest", "[frame_correlator]") +{ + FrameCorrelator c(FrameCorrelator::Mode::Stamp, 12); + for (std::uint32_t i = 0; i < 13; ++i) + c.submitted(frame(i, 100 + i)); + CHECK(c.in_flight() == 12); + + // The dropped frame's unit arrives late: it matches nothing. + const auto late = c.returned(100); + REQUIRE(late); + CHECK_FALSE(late->identified); + CHECK(late->frame.sequence == 1); // the oldest remaining was removed, not credited + + const auto next = c.returned(102); + REQUIRE(next); + CHECK(next->identified); + CHECK(next->frame.sequence == 2); + CHECK(next->skipped == 0); +} + +TEST_CASE("fifo mode takes the oldest regardless of stamp and never skips", "[frame_correlator]") +{ + FrameCorrelator c(FrameCorrelator::Mode::Fifo); + c.submitted(frame(0, 10)); + c.submitted(frame(1, 20)); + c.submitted(frame(2, 30)); + for (std::uint32_t i = 0; i < 3; ++i) + { + const auto r = c.returned(30 - 10 * static_cast(i)); // stamps deliberately out of order + REQUIRE(r); + CHECK(r->identified); + CHECK(r->skipped == 0); + CHECK(r->frame.sequence == i); + } + CHECK(c.empty()); +} + +TEST_CASE("stamp mode: random submit and return sequences with skips", "[frame_correlator]") +{ + std::mt19937 rng(12345); + for (int run = 0; run < 100; ++run) + { + FrameCorrelator c(FrameCorrelator::Mode::Stamp); + std::vector pending; // submitted, not yet returned or skipped + std::uint32_t next = 0; + std::int64_t stamp = 1000; + for (int step = 0; step < 200; ++step) + { + if (rng() % 2 == 0 && pending.size() < 12) + { + stamp += 1 + static_cast(rng() % 50); // distinct, increasing + const InFlightFrame f = frame(next++, stamp); + c.submitted(f); + pending.push_back(f); + } + else if (!pending.empty()) + { + // Return a random in-flight frame; those before it are skipped by the encoder. + const std::size_t k = rng() % pending.size(); + const auto r = c.returned(pending[k].submitted_us); + REQUIRE(r); + REQUIRE(r->identified); + CHECK(r->frame.submitted_us == pending[k].submitted_us); + CHECK(r->frame.sequence == pending[k].sequence); + CHECK(r->skipped == k); + pending.erase(pending.begin(), pending.begin() + static_cast(k) + 1); + } + } + CHECK(c.in_flight() == pending.size()); + } +} diff --git a/src/video_stage/wire/tests/test_h264_access_units.cpp b/src/video_stage/wire/tests/test_h264_access_units.cpp new file mode 100644 index 0000000000..661c693898 --- /dev/null +++ b/src/video_stage/wire/tests/test_h264_access_units.cpp @@ -0,0 +1,231 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include + +using video_stage::AccessUnitRange; +using video_stage::split_access_units; + +namespace +{ + +using Bytes = std::vector; + +const Bytes kStart4 = { 0, 0, 0, 1 }; +const Bytes kStart3 = { 0, 0, 1 }; + +/// A NAL with the given start code, header byte and payload (payloads here never contain 00 00 0x). +Bytes nal(const Bytes& start, std::uint8_t header, const Bytes& payload) +{ + Bytes out = start; + out.push_back(header); + out.insert(out.end(), payload.begin(), payload.end()); + return out; +} + +// first_mb_in_slice is the Exp-Golomb code at the start of the slice data: '1' is 0, '010' is 1, '011' is 2. +const Bytes kFirstSlice = { 0x88, 0x84, 0x21 }; +const Bytes kSecondSlice = { 0x48, 0x84, 0x21 }; +const Bytes kThirdSlice = { 0x68, 0x84, 0x21 }; + +Bytes sps(const Bytes& start = kStart4) +{ + return nal(start, 0x67, { 0x64, 0x00, 0x1f, 0xac }); +} +Bytes pps(const Bytes& start = kStart4) +{ + return nal(start, 0x68, { 0xeb, 0xc3, 0xcb }); +} +Bytes idr(const Bytes& start = kStart4, const Bytes& slice = kFirstSlice) +{ + return nal(start, 0x65, slice); +} +Bytes p_slice(const Bytes& start = kStart3, const Bytes& slice = kFirstSlice) +{ + return nal(start, 0x41, slice); +} +Bytes sei(const Bytes& start = kStart3) +{ + return nal(start, 0x06, { 0x05, 0x02, 0xaa, 0xbb, 0x80 }); +} +Bytes filler(const Bytes& start = kStart3) +{ + return nal(start, 0x0c, { 0xff, 0xff, 0x80 }); +} + +Bytes concat(std::initializer_list parts) +{ + Bytes out; + for (const auto& part : parts) + out.insert(out.end(), part.begin(), part.end()); + return out; +} + +std::vector split(const Bytes& stream) +{ + return split_access_units(stream.data(), stream.size()); +} + +} // namespace + +TEST_CASE("an empty or start-code-free input has no access units", "[h264_access_units]") +{ + CHECK(split_access_units(nullptr, 0).empty()); + CHECK(split(Bytes{}).empty()); + CHECK(split(Bytes{ 1, 2, 3, 4, 5, 6 }).empty()); + CHECK(split(Bytes{ 0, 0, 1 }).empty()); // a start code with no NAL after it +} + +TEST_CASE("parameter sets stay with the IDR that follows", "[h264_access_units]") +{ + const Bytes a = concat({ sps(), pps(), idr() }); + const Bytes b = p_slice(); + const Bytes c = p_slice(); + const auto units = split(concat({ a, b, c })); + + REQUIRE(units.size() == 3); + CHECK(units[0].offset == 0); + CHECK(units[0].size == a.size()); + CHECK(units[0].idr); + CHECK(units[1].offset == a.size()); + CHECK(units[1].size == b.size()); + CHECK_FALSE(units[1].idr); + CHECK(units[2].offset == a.size() + b.size()); + CHECK(units[2].size == c.size()); + CHECK_FALSE(units[2].idr); +} + +TEST_CASE("the units tile the stream exactly", "[h264_access_units]") +{ + const Bytes stream = concat({ sps(), pps(), idr(), p_slice(), sei(), p_slice(), p_slice() }); + const auto units = split(stream); + REQUIRE(units.size() == 4); + std::size_t expected_offset = 0; + for (const auto& unit : units) + { + CHECK(unit.offset == expected_offset); + expected_offset += unit.size; + } + CHECK(expected_offset == stream.size()); +} + +TEST_CASE("slices of one picture stay together", "[h264_access_units]") +{ + const Bytes first = + concat({ sps(), pps(), idr(kStart4, kFirstSlice), idr(kStart3, kSecondSlice), idr(kStart3, kThirdSlice) }); + const Bytes second = concat({ p_slice(kStart3, kFirstSlice), p_slice(kStart3, kSecondSlice) }); + const auto units = split(concat({ first, second })); + + REQUIRE(units.size() == 2); + CHECK(units[0].size == first.size()); + CHECK(units[0].idr); + CHECK(units[1].offset == first.size()); + CHECK(units[1].size == second.size()); + CHECK_FALSE(units[1].idr); +} + +TEST_CASE("a new first slice directly after a picture starts a new unit", "[h264_access_units]") +{ + const Bytes p1 = concat({ p_slice(kStart3, kFirstSlice), p_slice(kStart3, kSecondSlice) }); + const Bytes p2 = p_slice(kStart3, kFirstSlice); + const auto units = split(concat({ p1, p2 })); + REQUIRE(units.size() == 2); + CHECK(units[0].size == p1.size()); + CHECK(units[1].size == p2.size()); +} + +TEST_CASE("SEI and an access unit delimiter before a picture belong to that picture", "[h264_access_units]") +{ + const Bytes p0 = p_slice(); + const Bytes aud = nal(kStart3, 0x09, { 0xf0 }); + const Bytes next = concat({ aud, sei(), p_slice() }); + const auto units = split(concat({ p0, next })); + + REQUIRE(units.size() == 2); + CHECK(units[0].size == p0.size()); + CHECK(units[1].offset == p0.size()); + CHECK(units[1].size == next.size()); +} + +TEST_CASE("trailing filler, end of sequence and end of stream stay with the previous picture", "[h264_access_units]") +{ + SECTION("filler") + { + const Bytes first = concat({ p_slice(), filler() }); + const Bytes second = p_slice(); + const auto units = split(concat({ first, second })); + REQUIRE(units.size() == 2); + CHECK(units[0].size == first.size()); + CHECK(units[1].offset == first.size()); + } + SECTION("end of sequence then the next IDR's parameter sets") + { + const Bytes first = concat({ + sps(), + pps(), + idr(), + nal(kStart3, 0x0a, {}), + }); + const Bytes second = concat({ sps(), pps(), idr() }); + const auto units = split(concat({ first, second })); + REQUIRE(units.size() == 2); + CHECK(units[0].size == first.size()); + CHECK(units[1].offset == first.size()); + CHECK(units[1].idr); + } + SECTION("end of stream at the very end") + { + const Bytes stream = concat({ p_slice(), nal(kStart4, 0x0b, {}) }); + const auto units = split(stream); + REQUIRE(units.size() == 1); + CHECK(units[0].size == stream.size()); + } + SECTION("several fillers in a row") + { + const Bytes first = concat({ p_slice(), filler(), filler() }); + const auto units = split(concat({ first, p_slice() })); + REQUIRE(units.size() == 2); + CHECK(units[0].size == first.size()); + } +} + +TEST_CASE("three- and four-byte start codes both work, and a four-byte code's leading zero begins its unit", + "[h264_access_units]") +{ + const Bytes first = concat( + { nal(kStart3, 0x67, { 0x64, 0x00, 0x1f }), nal(kStart3, 0x68, { 0xeb }), nal(kStart3, 0x65, kFirstSlice) }); + const Bytes second = p_slice(kStart4); + const Bytes third = p_slice(kStart3); + const auto units = split(concat({ first, second, third })); + + REQUIRE(units.size() == 3); + CHECK(units[0].size == first.size()); + CHECK(units[1].offset == first.size()); // includes the zero byte of the 4-byte code + CHECK(units[1].size == second.size()); + CHECK(units[2].offset == first.size() + second.size()); + CHECK(units[2].size == third.size()); +} + +TEST_CASE("only an IDR slice marks a unit as a keyframe", "[h264_access_units]") +{ + const auto units = split(concat({ sps(), pps(), idr(), p_slice(), idr(kStart4), p_slice() })); + REQUIRE(units.size() == 4); + CHECK(units[0].idr); + CHECK_FALSE(units[1].idr); + CHECK(units[2].idr); + CHECK_FALSE(units[3].idr); +} + +TEST_CASE("a single NAL is one unit", "[h264_access_units]") +{ + const Bytes stream = p_slice(kStart4); + const auto units = split(stream); + REQUIRE(units.size() == 1); + CHECK(units[0].offset == 0); + CHECK(units[0].size == stream.size()); +} diff --git a/src/video_stage/wire/tests/test_h264_sps.cpp b/src/video_stage/wire/tests/test_h264_sps.cpp new file mode 100644 index 0000000000..e1917638c8 --- /dev/null +++ b/src/video_stage/wire/tests/test_h264_sps.cpp @@ -0,0 +1,353 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include + +using video_stage::h264::declare_no_reordering; +using video_stage::h264::parse_sps; +using video_stage::h264::SpsInfo; + +namespace +{ +// First SPS, PPS and the start of the first IDR slice of short ffmpeg streams (x264 unless noted), 3 frames each. +// Every SPS contains emulation-prevention bytes in its VUI timing fields. The IDR bytes are truncated: only their +// preservation matters here. +// a_high_720 +const std::vector a_high_720_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x64, 0x00, 0x1f, 0xac, 0xb2, + 0x00, 0xa0, 0x0b, 0x76, 0x02, 0x20, 0x00, 0x00, 0x03, 0x00, + 0x20, 0x00, 0x00, 0x07, 0x81, 0xe3, 0x06, 0x49 }; +const std::vector a_high_720_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xeb, 0xc3, 0xcb, 0x22, 0xc0 }; +const std::vector a_high_720_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x08, 0x7f, 0xd3, 0xd6, 0x67, 0x0b, + 0x58, 0xc9, 0x09, 0x0f, 0xd0, 0x9f, 0x5b, 0xe3, 0x26, 0xe9, 0xc3, 0xd4, + 0xb8, 0x70, 0x34, 0xe3, 0x55, 0x12, 0xc0, 0x2f, 0xf6, 0xfb, 0x3c, 0x4c, + 0x99, 0xf9, 0xe1, 0x86, 0x0b, 0xe5, 0x90, 0x5d, 0x3f, 0x68, 0xf2, 0xb6 }; +// b_high_2560 +const std::vector b_high_2560_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x64, 0x00, 0x33, 0xac, 0xb2, + 0x00, 0x50, 0x01, 0xf3, 0x60, 0x22, 0x00, 0x00, 0x03, 0x00, + 0x02, 0x00, 0x00, 0x03, 0x00, 0x78, 0x1e, 0x30, 0x64, 0x90 }; +const std::vector b_high_2560_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xeb, 0xc3, 0xcb, 0x22, 0xc0 }; +const std::vector b_high_2560_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x08, 0x7f, 0xd3, 0xd6, 0x67, 0x0b, + 0x58, 0xc9, 0x09, 0x0f, 0xd0, 0x9f, 0x5b, 0xe3, 0x26, 0xe9, 0xc3, 0xd4, + 0xb8, 0x70, 0x34, 0xe3, 0x55, 0x12, 0xce, 0x35, 0x37, 0xb3, 0xca, 0x63, + 0x32, 0x3d, 0xb7, 0x41, 0xc7, 0x2a, 0x82, 0xea, 0x04, 0xb9, 0x64, 0x3e }; +// c_base_720 +const std::vector c_base_720_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0xc0, 0x1f, 0xd9, 0x00, + 0x50, 0x05, 0xbb, 0x01, 0x10, 0x00, 0x00, 0x03, 0x00, 0x10, + 0x00, 0x00, 0x03, 0x03, 0xc0, 0xf1, 0x83, 0x24, 0x80 }; +const std::vector c_base_720_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xcb, 0x83, 0xcb, 0x20 }; +const std::vector c_base_720_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x08, 0x7c, 0x42, 0x22, 0xb0, 0x60, + 0x42, 0x1f, 0x80, 0xea, 0x40, 0x03, 0xec, 0xcc, 0xb3, 0x20, 0x00, 0x2d, + 0x66, 0x69, 0x14, 0x68, 0x3e, 0x4e, 0xff, 0x70, 0x2a, 0x0a, 0x4c, 0xb3, + 0x6e, 0x2d, 0xfe, 0x03, 0x16, 0x67, 0x22, 0x8e, 0x1f, 0x37, 0x7f, 0xb0 }; +// d_cbr_2560 +const std::vector d_cbr_2560_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x64, 0x00, 0x33, 0xac, 0xb2, 0x00, + 0x50, 0x01, 0xf3, 0x60, 0x22, 0x00, 0x00, 0x03, 0x00, 0x02, 0x00, + 0x00, 0x03, 0x00, 0x78, 0xcc, 0x00, 0x01, 0xe8, 0x48, 0x00, 0x10, + 0x4b, 0xed, 0x61, 0x80, 0x3c, 0x60, 0xc9, 0x20 }; +const std::vector d_cbr_2560_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xeb, 0xcc, 0xb2, 0x2c }; +const std::vector d_cbr_2560_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x0b, 0x7f, 0xdb, 0xe5, 0x95, 0xbd, + 0xc6, 0x03, 0x72, 0x93, 0x37, 0x67, 0xad, 0x5c, 0xfd, 0xf5, 0x21, 0xdc, + 0xcb, 0x9f, 0xfd, 0xce, 0xc4, 0xe2, 0x96, 0x1a, 0x45, 0xb2, 0x75, 0x05, + 0x88, 0xac, 0xfb, 0x5f, 0xf2, 0x45, 0x4c, 0x6d, 0x93, 0x7f, 0xac, 0x86 }; +// e_bframes +const std::vector e_bframes_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x64, 0x00, 0x1f, 0xac, 0xd9, + 0x40, 0x50, 0x05, 0xbb, 0x01, 0x10, 0x00, 0x00, 0x03, 0x00, + 0x10, 0x00, 0x00, 0x03, 0x03, 0xc0, 0xf1, 0x83, 0x19, 0x60 }; +const std::vector e_bframes_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xe9, 0x38, 0xf2, 0xc8, 0xb0 }; +const std::vector e_bframes_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x00, 0x21, 0xff, 0xd3, 0xd6, 0x67, + 0x0b, 0x58, 0xc9, 0x09, 0x0f, 0xd0, 0x9f, 0x5b, 0xe3, 0x26, 0xe9, 0xc3, + 0xd4, 0xb8, 0x70, 0x34, 0xe3, 0x55, 0x12, 0xc0, 0x2f, 0xf6, 0xfb, 0x3c, + 0x4c, 0x99, 0xf9, 0xe1, 0x86, 0x0b, 0xe5, 0x90, 0x5d, 0x3f, 0x68, 0xf2 }; +// f_full +const std::vector f_full_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x64, 0x00, 0x1f, 0xac, 0xb2, + 0x00, 0xa0, 0x0b, 0x76, 0x02, 0xd9, 0x00, 0x00, 0x03, 0x00, + 0x01, 0x00, 0x00, 0x03, 0x00, 0x3c, 0x0f, 0x18, 0x32, 0x48 }; +const std::vector f_full_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xeb, 0xc3, 0xcb, 0x22, 0xc0 }; +const std::vector f_full_idr = { 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x08, 0x7f, 0xd3, 0xd6, 0x67, 0x0b, + 0x58, 0xc9, 0x09, 0x0f, 0xd0, 0x9f, 0x5b, 0xe3, 0x26, 0xe9, 0xc3, 0xd4, + 0xb8, 0x70, 0x34, 0xe3, 0x55, 0x12, 0xc0, 0x2f, 0xf6, 0xfb, 0x3c, 0x4c, + 0x99, 0xf9, 0xe1, 0x86, 0x0b, 0xe5, 0x90, 0x5d, 0x3f, 0x68, 0xf2, 0xb6 }; +// g_nvenc +const std::vector g_nvenc_sps = { 0x00, 0x00, 0x00, 0x01, 0x67, 0x4d, 0x40, 0x1f, 0x95, 0x90, 0x05, 0x00, 0x5b, + 0xb0, 0x11, 0x00, 0x00, 0x03, 0x03, 0xe8, 0x00, 0x00, 0xea, 0x60, 0xe0, 0x00, + 0x00, 0x16, 0xe3, 0x60, 0x00, 0x07, 0xa1, 0x20, 0xbb, 0xcb, 0x82, 0x80 }; +const std::vector g_nvenc_pps = { 0x00, 0x00, 0x00, 0x01, 0x68, 0xeb, 0x8f, 0x20 }; +const std::vector g_nvenc_idr = { 0x00, 0x00, 0x00, 0x01, 0x65, 0xb8, 0x04, 0x3f, 0xad, 0x6e, 0xda, 0xc4, + 0xf1, 0x5a, 0x80, 0x2e, 0xee, 0x6f, 0x27, 0x6e, 0x50, 0x1f, 0xdd, 0x44, + 0xcf, 0xce, 0x97, 0x18, 0xd2, 0x37, 0x20, 0xd0, 0x6c, 0x94, 0xee, 0xe0, + 0xe1, 0xc6, 0x50, 0x59, 0x74, 0xda, 0x8d, 0x62, 0xfa, 0x36, 0x69, 0xbc }; + +std::vector concat(const std::vector& a, const std::vector& b, const std::vector& c) +{ + std::vector out(a); + out.insert(out.end(), b.begin(), b.end()); + out.insert(out.end(), c.begin(), c.end()); + return out; +} + +struct Case +{ + const char* name; + const std::vector* sps; + const std::vector* pps; + const std::vector* idr; + uint32_t profile, level, width, height, refs, poc; + bool vui, full_range, restriction; + uint32_t reorder, dpb; +}; + +const std::vector& cases() +{ + static const std::vector c = { + { "x264 high 1280x720", &a_high_720_sps, &a_high_720_pps, &a_high_720_idr, 100, 31, 1280, 720, 3, 2, true, + false, true, 0, 3 }, + { "x264 high 2560x1984", &b_high_2560_sps, &b_high_2560_pps, &b_high_2560_idr, 100, 51, 2560, 1984, 3, 2, true, + false, true, 0, 3 }, + { "x264 baseline 1280x720", &c_base_720_sps, &c_base_720_pps, &c_base_720_idr, 66, 31, 1280, 720, 3, 2, true, + false, true, 0, 3 }, + { "x264 high CBR (nal-hrd) 2560x1984", &d_cbr_2560_sps, &d_cbr_2560_pps, &d_cbr_2560_idr, 100, 51, 2560, 1984, + 3, 2, true, false, true, 0, 3 }, + { "x264 high with B-frames", &e_bframes_sps, &e_bframes_pps, &e_bframes_idr, 100, 31, 1280, 720, 4, 0, true, + false, true, 2, 4 }, + { "x264 full range", &f_full_sps, &f_full_pps, &f_full_idr, 100, 31, 1280, 720, 3, 2, true, true, true, 0, 3 }, + // NVENC emits a VUI without bitstream_restriction: the case the rewrite exists for. + { "h264_nvenc main", &g_nvenc_sps, &g_nvenc_pps, &g_nvenc_idr, 77, 31, 1280, 720, 3, 2, true, false, false, 0, 0 }, + }; + return c; +} +} // namespace + +TEST_CASE("parse_sps reads the fields of real SPS NAL units", "[Sps]") +{ + for (const Case& c : cases()) + { + INFO(c.name); + const auto au = concat(*c.sps, *c.pps, *c.idr); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->profile_idc == c.profile); + CHECK(info->level_idc == c.level); + CHECK(info->width == c.width); + CHECK(info->height == c.height); + CHECK(info->max_num_ref_frames == c.refs); + CHECK(info->pic_order_cnt_type == c.poc); + CHECK(info->frame_mbs_only); + CHECK(info->vui_present == c.vui); + CHECK(info->full_range == c.full_range); + CHECK(info->bitstream_restriction == c.restriction); + if (c.restriction) + { + CHECK(info->max_num_reorder_frames == c.reorder); + CHECK(info->max_dec_frame_buffering == c.dpb); + } + CHECK_FALSE(info->describe().empty()); + } +} + +TEST_CASE("declare_no_reordering sets the restriction and changes nothing else", "[Sps]") +{ + for (const Case& c : cases()) + { + INFO(c.name); + const auto au = concat(*c.sps, *c.pps, *c.idr); + const auto before = parse_sps(au.data(), au.size()); + REQUIRE(before); + const auto out = declare_no_reordering(au.data(), au.size()); + REQUIRE(out); + const auto after = parse_sps(out->data(), out->size()); + REQUIRE(after); + + CHECK(after->bitstream_restriction); + CHECK(after->max_num_reorder_frames == 0); + CHECK(after->max_dec_frame_buffering == std::max(before->max_num_ref_frames, 1)); + CHECK(after->profile_idc == before->profile_idc); + CHECK(after->level_idc == before->level_idc); + CHECK(after->chroma_format_idc == before->chroma_format_idc); + CHECK(after->pic_order_cnt_type == before->pic_order_cnt_type); + CHECK(after->max_num_ref_frames == before->max_num_ref_frames); + CHECK(after->width == before->width); + CHECK(after->height == before->height); + CHECK(after->frame_mbs_only == before->frame_mbs_only); + CHECK(after->vui_present); + CHECK(after->full_range == before->full_range); + + // Everything after the SPS (PPS, IDR) is byte-identical. + const std::size_t rest = c.pps->size() + c.idr->size(); + REQUIRE(out->size() > rest); + CHECK(std::equal(au.end() - rest, au.end(), out->end() - rest)); + // The SPS keeps its start code and header byte. + CHECK(std::equal(c.sps->begin(), c.sps->begin() + 5, out->begin())); + + // Rewriting is idempotent. + const auto twice = declare_no_reordering(out->data(), out->size()); + REQUIRE(twice); + CHECK(*twice == *out); + } +} + +TEST_CASE("an access unit with a different NAL ahead of the SPS is handled", "[Sps]") +{ + const Case& c = cases()[6]; + const std::vector aud = { 0, 0, 0, 1, 0x09, 0xf0 }; + const auto body = concat(*c.sps, *c.pps, *c.idr); + const auto au = concat(aud, body, {}); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->width == 1280); + const auto out = declare_no_reordering(au.data(), au.size()); + REQUIRE(out); + CHECK(std::equal(aud.begin(), aud.end(), out->begin())); + const auto after = parse_sps(out->data(), out->size()); + REQUIRE(after); + CHECK(after->bitstream_restriction); + CHECK(std::equal(au.end() - (c.pps->size() + c.idr->size()), au.end(), out->end() - (c.pps->size() + c.idr->size()))); +} + +TEST_CASE("a three byte start code is accepted", "[Sps]") +{ + const Case& c = cases()[0]; + std::vector au(c.sps->begin() + 1, c.sps->end()); // 00 00 01 + NAL + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->width == 1280); +} + +TEST_CASE("no SPS or a damaged SPS yields nullopt", "[Sps]") +{ + const std::vector none = { 0, 0, 0, 1, 0x68, 0xce, 0x3c, 0x80 }; + CHECK_FALSE(parse_sps(none.data(), none.size())); + CHECK_FALSE(declare_no_reordering(none.data(), none.size())); + CHECK_FALSE(parse_sps(none.data(), 0)); + + const Case& c = cases()[0]; + std::vector truncated(c.sps->begin(), c.sps->begin() + 8); + CHECK_FALSE(declare_no_reordering(truncated.data(), truncated.size())); +} + +namespace +{ +struct BitWriter +{ + std::vector bytes; + int used = 8; + void bit(unsigned v) + { + if (used == 8) + { + bytes.push_back(0); + used = 0; + } + bytes.back() = static_cast(bytes.back() | ((v & 1u) << (7 - used))); + ++used; + } + void bits(unsigned v, int n) + { + for (int i = n - 1; i >= 0; --i) + bit((v >> i) & 1u); + } + void ue(unsigned v) + { + const unsigned x = v + 1; + int len = 0; + while ((x >> len) > 1) + ++len; + bits(0, len); + bits(x, len + 1); + } +}; + +/// A baseline-profile 1280x720 SPS whose VUI carries only the chroma location field. `chroma_loc` is null for none. +std::vector synthetic_sps(const std::pair* chroma_loc) +{ + BitWriter w; + w.bits(66, 8); // profile_idc + w.bits(0, 8); // constraint flags + w.bits(31, 8); // level_idc + w.ue(0); // seq_parameter_set_id + w.ue(0); // log2_max_frame_num_minus4 + w.ue(2); // pic_order_cnt_type + w.ue(1); // max_num_ref_frames + w.bit(0); // gaps_in_frame_num_value_allowed_flag + w.ue(79); // pic_width_in_mbs_minus1 + w.ue(44); // pic_height_in_map_units_minus1 + w.bit(1); // frame_mbs_only_flag + w.bit(1); // direct_8x8_inference_flag + w.bit(0); // frame_cropping_flag + w.bit(1); // vui_parameters_present_flag + w.bit(0); // aspect_ratio_info_present_flag + w.bit(0); // overscan_info_present_flag + w.bit(0); // video_signal_type_present_flag + if (chroma_loc) + { + w.bit(1); + w.ue(chroma_loc->first); + w.ue(chroma_loc->second); + } + else + { + w.bit(0); + } + w.bit(0); // timing_info_present_flag + w.bit(0); // nal_hrd_parameters_present_flag + w.bit(0); // vcl_hrd_parameters_present_flag + w.bit(0); // pic_struct_present_flag + w.bit(0); // bitstream_restriction_flag + w.bit(1); // rbsp_stop_one_bit + std::vector out = { 0, 0, 0, 1, 0x67 }; + out.insert(out.end(), w.bytes.begin(), w.bytes.end()); + return out; +} +} // namespace + +TEST_CASE("chroma sample locations parse into chroma_loc_*", "[Sps]") +{ + const std::pair zero{ 0, 0 }; + const std::pair one{ 1, 1 }; + const std::pair mixed{ 2, 5 }; + + SECTION("absent") + { + const auto au = synthetic_sps(nullptr); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->width == 1280); + CHECK(info->height == 720); + CHECK(info->vui_present); + CHECK_FALSE(info->chroma_loc_present); + CHECK(info->chroma_loc_top == 0); + CHECK(info->chroma_loc_bottom == 0); + } + SECTION("present with (0, 0)") + { + const auto au = synthetic_sps(&zero); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->chroma_loc_present); + CHECK(info->chroma_loc_top == 0); + CHECK(info->chroma_loc_bottom == 0); + } + SECTION("present with (1, 1)") + { + const auto au = synthetic_sps(&one); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->chroma_loc_present); + CHECK(info->chroma_loc_top == 1); + CHECK(info->chroma_loc_bottom == 1); + CHECK(info->width == 1280); + } + SECTION("distinct top and bottom values") + { + const auto au = synthetic_sps(&mixed); + const auto info = parse_sps(au.data(), au.size()); + REQUIRE(info); + CHECK(info->chroma_loc_top == 2); + CHECK(info->chroma_loc_bottom == 5); + } +}