/* SPDX-License-Identifier: GPL-2.0-or-later */ /* * Copyright (C) 2021, Ideas on Board Oy * * kms_sink.cpp - KMS Sink */ #include "kms_sink.h" #include <array> #include <algorithm> #include <assert.h> #include <iostream> #include <memory> #include <stdint.h> #include <string.h> #include <libcamera/camera.h> #include <libcamera/formats.h> #include <libcamera/framebuffer.h> #include <libcamera/stream.h> #include "drm.h" KMSSink::KMSSink(const std::string &connectorName) : connector_(nullptr), crtc_(nullptr), plane_(nullptr), mode_(nullptr) { int ret = dev_.init(); if (ret < 0) return; /* * Find the requested connector. If no specific connector is requested, * pick the first connected connector or, if no connector is connected, * the first connector with unknown status. */ for (const DRM::Connector &conn : dev_.connectors()) { if (!connectorName.empty()) { if (conn.name() != connectorName) continue; connector_ = &conn; break; } if (conn.status() == DRM::Connector::Connected) { connector_ = &conn; break; } if (!connector_ && conn.status() == DRM::Connector::Unknown) connector_ = &conn; } if (!connector_) { if (!connectorName.empty()) std::cerr << "Connector " << connectorName << " not found" << std::endl; else std::cerr << "No connected connector found" << std::endl; return; } dev_.requestComplete.connect(this, &KMSSink::requestComplete); } void KMSSink::mapBuffer(libcamera::FrameBuffer *buffer) { std::array<uint32_t, 4> strides = {}; /* \todo Should libcamera report per-plane strides ? */ unsigned int uvStrideMultiplier; switch (format_) { case libcamera::formats::NV24: case libcamera::formats::NV42: uvStrideMultiplier = 4; break; case libcamera::formats::YUV420: case libcamera::formats::YVU420: case libcamera::formats::YUV422: uvStrideMultiplier = 1; break; default: uvStrideMultiplier = 2; break; } strides[0] = stride_; for (unsigned int i = 1; i < buffer->planes().size(); ++i) strides[i] = stride_ * uvStrideMultiplier / 2; std::unique_ptr<DRM::FrameBuffer> drmBuffer = dev_.createFrameBuffer(*buffer, format_, size_, strides); if (!drmBuffer) return; buffers_.emplace(std::piecewise_construct, std::forward_as_tuple(buffer), std::forward_as_tuple(std::move(drmBuffer))); } int KMSSink::configure(const libcamera::CameraConfiguration &config) { if (!connector_) return -EINVAL; crtc_ = nullptr; plane_ = nullptr; mode_ = nullptr; const libcamera::StreamConfiguration &cfg = config.at(0); const std::vector<DRM::Mode> &modes = connector_->modes(); const auto iter = std::find_if(modes.begin(), modes.end(), [&](const DRM::Mode &mode) { return mode.hdisplay == cfg.size.width && mode.vdisplay == cfg.size.height; }); if (iter == modes.end()) { std::cerr << "No mode matching " << cfg.size.toString() << std::endl; return -EINVAL; } int ret = configurePipeline(cfg.pixelFormat); if (ret < 0) return ret; mode_ = &*iter; size_ = cfg.size; stride_ = cfg.stride; return 0; } int KMSSink::selectPipeline(const libcamera::PixelFormat &format) { /* * If the requested format has an alpha channel, also consider the X * variant. */ libcamera::PixelFormat xFormat; switch (format) { case libcamera::formats::ABGR8888: xFormat = libcamera::formats::XBGR8888; break; case libcamera::formats::ARGB8888: xFormat = libcamera::formats::XRGB8888; break; case libcamera::formats::BGRA8888: xFormat = libcamera::formats::BGRX8888; break; case libcamera::formats::RGBA8888: xFormat = libcamera::formats::RGBX8888; break; } /* * Find a CRTC and plane suitable for the request format and the * connector at the end of the pipeline. Restrict the search to primary * planes for now. */ for (const DRM::Encoder *encoder : connector_->encoders()) { for (const DRM::Crtc *crtc : encoder->possibleCrtcs()) { for (const DRM::Plane *plane : crtc->planes()) { if (plane->type() != DRM::Plane::TypePrimary) continue; if (plane->supportsFormat(format)) { crtc_ = crtc; plane_ = plane; format_ = format; return 0; } if (plane->supportsFormat(xFormat)) { crtc_ = crtc; plane_ = plane; format_ = xFormat; return 0; } } } } return -EPIPE; } int KMSSink::configurePipeline(const libcamera::PixelFormat &format) { const int ret = selectPipeline(format); if (ret) { std::cerr << "Unable to find display pipeline for format " << format.toString() << std::endl; return ret; } std::cout << "Using KMS plane " << plane_->id() << ", CRTC " << crtc_->id() << ", connector " << connector_->name() << " (" << connector_->id() << ")" << std::endl; return 0; } int KMSSink::start() { std::unique_ptr<DRM::AtomicRequest> request; int ret = FrameSink::start(); if (ret < 0) return ret; /* Disable all CRTCs and planes to start from a known valid state. */ request = std::make_unique<DRM::AtomicRequest>(&dev_); for (const DRM::Crtc &crtc : dev_.crtcs()) request->addProperty(&crtc, "ACTIVE", 0); for (const DRM::Plane &plane : dev_.planes()) { request->addProperty(&plane, "CRTC_ID", 0); request->addProperty(&plane, "FB_ID", 0); } ret = request->commit(DRM::AtomicRequest::FlagAllowModeset); if (ret < 0) { std::cerr << "Failed to disable CRTCs and planes: " << strerror(-ret) << std::endl; return ret; } return 0; } int KMSSink::stop() { /* Display pipeline. */ DRM::AtomicRequest request(&dev_); request.addProperty(connector_, "CRTC_ID", 0); request.addProperty(crtc_, "ACTIVE", 0); request.addProperty(crtc_, "MODE_ID", 0); request.addProperty(plane_, "CRTC_ID", 0); request.addProperty(plane_, "FB_ID", 0); int ret = request.commit(DRM::AtomicRequest::FlagAllowModeset); if (ret < 0) { std::cerr << "Failed to stop display pipeline: " << strerror(-ret) << std::endl; return ret; } /* Free all buffers. */ pending_.reset(); queued_.reset(); active_.reset(); buffers_.clear(); return FrameSink::stop(); } bool KMSSink::processRequest(libcamera::Request *camRequest) { /* * Perform a very crude rate adaptation by simply dropping the request * if the display queue is full. */ if (pending_) return true; libcamera::FrameBuffer *buffer = camRequest->buffers().begin()->second; auto iter = buffers_.find(buffer); if (iter == buffers_.end()) return true; DRM::FrameBuffer *drmBuffer = iter->second.get(); unsigned int flags = DRM::AtomicRequest::FlagAsync; DRM::AtomicRequest *drmRequest = new DRM::AtomicRequest(&dev_); drmRequest->addProperty(plane_, "FB_ID", drmBuffer->id()); if (!active_ && !queued_) { /* Enable the display pipeline on the first frame. */ drmRequest->addProperty(connector_, "CRTC_ID", crtc_->id()); drmRequest->addProperty(crtc_, "ACTIVE", 1); drmRequest->addProperty(crtc_, "MODE_ID", mode_->toBlob(&dev_)); drmRequest->addProperty(plane_, "CRTC_ID", crtc_->id()); drmRequest->addProperty(plane_, "SRC_X", 0 << 16); drmRequest->addProperty(plane_, "SRC_Y", 0 << 16); drmRequest->addProperty(plane_, "SRC_W", mode_->hdisplay << 16); drmRequest->addProperty(plane_, "SRC_H", mode_->vdisplay << 16); drmRequest->addProperty(plane_, "CRTC_X", 0); drmRequest->addProperty(plane_, "CRTC_Y", 0); drmRequest->addProperty(plane_, "CRTC_W", mode_->hdisplay); drmRequest->addProperty(plane_, "CRTC_H", mode_->vdisplay); flags |= DRM::AtomicRequest::FlagAllowModeset; } pending_ = std::make_unique<Request>(drmRequest, camRequest); std::lock_guard<std::mutex> lock(lock_); if (!queued_) { int ret = drmRequest->commit(flags); if (ret < 0) { std::cerr << "Failed to commit atomic request: " << strerror(-ret) << std::endl; /* \todo Implement error handling */ } queued_ = std::move(pending_); } return false; } void KMSSink::requestComplete(DRM::AtomicRequest *request) { std::lock_guard<std::mutex> lock(lock_); assert(queued_ && queued_->drmRequest_.get() == request); /* Complete the active request, if any. */ if (active_) requestProcessed.emit(active_->camRequest_); /* The queued request becomes active. */ active_ = std::move(queued_); /* Queue the pending request, if any. */ if (pending_) { pending_->drmRequest_->commit(DRM::AtomicRequest::FlagAsync); queued_ = std::move(pending_); } }