external_libcamera/src/cam/kms_sink.cpp

/* 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 <limits.h>
#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);

	/* Find the best mode for the stream size. */
	const std::vector<DRM::Mode> &modes = connector_->modes();

	unsigned int cfgArea = cfg.size.width * cfg.size.height;
	unsigned int bestDistance = UINT_MAX;

	for (const DRM::Mode &mode : modes) {
		unsigned int modeArea = mode.hdisplay * mode.vdisplay;
		unsigned int distance = modeArea > cfgArea ? modeArea - cfgArea
				      : cfgArea - modeArea;

		if (distance < bestDistance) {
			mode_ = &mode;
			bestDistance = distance;

			/*
			 * If the sizes match exactly, there will be no better
			 * match.
			 */
			if (distance == 0)
				break;
		}
	}

	if (!mode_) {
		std::cerr << "No modes\n";
		return -EINVAL;
	}

	int ret = configurePipeline(cfg.pixelFormat);
	if (ret < 0)
		return ret;

	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 << std::endl;

		return ret;
	}

	std::cout
		<< "Using KMS plane " << plane_->id() << ", CRTC " << crtc_->id()
		<< ", connector " << connector_->name()
		<< " (" << connector_->id() << "), mode " << mode_->hdisplay
		<< "x" << mode_->vdisplay << "@" << mode_->vrefresh << 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::testModeSet(DRM::FrameBuffer *drmBuffer,
			  const libcamera::Rectangle &src,
			  const libcamera::Rectangle &dst)
{
	DRM::AtomicRequest drmRequest{ &dev_ };

	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_, "FB_ID", drmBuffer->id());
	drmRequest.addProperty(plane_, "SRC_X", src.x << 16);
	drmRequest.addProperty(plane_, "SRC_Y", src.y << 16);
	drmRequest.addProperty(plane_, "SRC_W", src.width << 16);
	drmRequest.addProperty(plane_, "SRC_H", src.height << 16);
	drmRequest.addProperty(plane_, "CRTC_X", dst.x);
	drmRequest.addProperty(plane_, "CRTC_Y", dst.y);
	drmRequest.addProperty(plane_, "CRTC_W", dst.width);
	drmRequest.addProperty(plane_, "CRTC_H", dst.height);

	return !drmRequest.commit(DRM::AtomicRequest::FlagAllowModeset |
				  DRM::AtomicRequest::FlagTestOnly);
}

bool KMSSink::setupComposition(DRM::FrameBuffer *drmBuffer)
{
	/*
	 * Test composition options, from most to least desirable, to select the
	 * best one.
	 */
	const libcamera::Rectangle framebuffer{ size_ };
	const libcamera::Rectangle display{ 0, 0, mode_->hdisplay, mode_->vdisplay };

	/* 1. Scale the frame buffer to full screen, preserving aspect ratio. */
	libcamera::Rectangle src = framebuffer;
	libcamera::Rectangle dst = display.size().boundedToAspectRatio(framebuffer.size())
						 .centeredTo(display.center());

	if (testModeSet(drmBuffer, src, dst)) {
		std::cout << "KMS: full-screen scaled output, square pixels"
			  << std::endl;
		src_ = src;
		dst_ = dst;
		return true;
	}

	/*
	 * 2. Scale the frame buffer to full screen, without preserving aspect
	 *    ratio.
	 */
	src = framebuffer;
	dst = display;

	if (testModeSet(drmBuffer, src, dst)) {
		std::cout << "KMS: full-screen scaled output, non-square pixels"
			  << std::endl;
		src_ = src;
		dst_ = dst;
		return true;
	}

	/* 3. Center the frame buffer on the display. */
	src = display.size().centeredTo(framebuffer.center()).boundedTo(framebuffer);
	dst = framebuffer.size().centeredTo(display.center()).boundedTo(display);

	if (testModeSet(drmBuffer, src, dst)) {
		std::cout << "KMS: centered output" << std::endl;
		src_ = src;
		dst_ = dst;
		return true;
	}

	/* 4. Align the frame buffer on the top-left of the display. */
	src = framebuffer.boundedTo(display);
	dst = display.boundedTo(framebuffer);

	if (testModeSet(drmBuffer, src, dst)) {
		std::cout << "KMS: top-left aligned output" << std::endl;
		src_ = src;
		dst_ = dst;
		return true;
	}

	return false;
}

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;
	std::unique_ptr<DRM::AtomicRequest> drmRequest =
		std::make_unique<DRM::AtomicRequest>(&dev_);
	drmRequest->addProperty(plane_, "FB_ID", drmBuffer->id());

	if (!active_ && !queued_) {
		/* Enable the display pipeline on the first frame. */
		if (!setupComposition(drmBuffer)) {
			std::cerr << "Failed to setup composition" << std::endl;
			return true;
		}

		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", src_.x << 16);
		drmRequest->addProperty(plane_, "SRC_Y", src_.y << 16);
		drmRequest->addProperty(plane_, "SRC_W", src_.width << 16);
		drmRequest->addProperty(plane_, "SRC_H", src_.height << 16);
		drmRequest->addProperty(plane_, "CRTC_X", dst_.x);
		drmRequest->addProperty(plane_, "CRTC_Y", dst_.y);
		drmRequest->addProperty(plane_, "CRTC_W", dst_.width);
		drmRequest->addProperty(plane_, "CRTC_H", dst_.height);

		flags |= DRM::AtomicRequest::FlagAllowModeset;
	}

	pending_ = std::make_unique<Request>(std::move(drmRequest), camRequest);

	std::lock_guard<std::mutex> lock(lock_);

	if (!queued_) {
		int ret = pending_->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_);
	}
}