/* 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::<span class="hl com">/* SPDX-License-Identifier: GPL-2.0-or-later */</span>
<span class="hl com">/*</span>
<span class="hl com"> * Copyright (C) 2019, Google Inc.</span>
<span class="hl com"> *</span>
<span class="hl com"> * libcamera V4L2 API tests</span>
<span class="hl com"> */</span>

<span class="hl ppc">#include &lt;iostream&gt;</span>

<span class="hl ppc">#include &lt;linux/media-bus-format.h&gt;</span>

<span class="hl ppc">#include</span> <span class="hl pps">&quot;libcamera/internal/device_enumerator.h&quot;</span><span class="hl ppc"></span>
<span class="hl ppc">#include</span> <span class="hl pps">&quot;libcamera/internal/media_device.h&quot;</span><span class="hl ppc"></span>

<span class="hl ppc">#include</span> <span class="hl pps">&quot;v4l2_videodevice_test.h&quot;</span><span class="hl ppc"></span>

<span class="hl kwa">using namespace</span> std<span class="hl opt">;</span>
<span class="hl kwa">using namespace</span> libcamera<span class="hl opt">;</span>

<span class="hl kwb">int</span> <span class="hl kwc">V4L2VideoDeviceTest</span><span class="hl opt">::</span><span class="hl kwd">init</span><span class="hl opt">()</span>
<span class="hl opt">{</span>
	enumerator_ <span class="hl opt">=</span> <span class="hl kwc">DeviceEnumerator</span><span class="hl opt">::</span><span class="hl kwd">create</span><span class="hl opt">();</span>
	<span class="hl kwa">if</span> <span class="hl opt">(!</span>enumerator_<span class="hl opt">) {</span>
		cerr <span class="hl opt">&lt;&lt;</span> <span class="hl str">&quot;Failed to create device enumerator&quot;</span> <span class="hl opt">&lt;&lt;</span> endl<span class="hl opt">;</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>
	<span class="hl opt">}</span>

	<span class="hl kwa">if</span> <span class="hl opt">(</span>enumerator_<span class="hl opt">-&gt;</span><span class="hl kwd">enumerate</span><span class="hl opt">()) {</span>
		cerr <span class="hl opt">&lt;&lt;</span> <span class="hl str">&quot;Failed to enumerate media devices&quot;</span> <span class="hl opt">&lt;&lt;</span> endl<span class="hl opt">;</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>
	<span class="hl opt">}</span>

	DeviceMatch <span class="hl kwd">dm</span><span class="hl opt">(</span>driver_<span class="hl opt">);</span>
	dm<span class="hl opt">.</span><span class="hl kwd">add</span><span class="hl opt">(</span>entity_<span class="hl opt">);</span>

	media_ <span class="hl opt">=</span> enumerator_<span class="hl opt">-&gt;</span><span class="hl kwd">search</span><span class="hl opt">(</span>dm<span class="hl opt">);</span>
	<span class="hl kwa">if</span> <span class="hl opt">(!</span>media_<span class="hl opt">)</span>
		<span class="hl kwa">return</span> TestSkip<span class="hl opt">;</span>

	MediaEntity <span class="hl opt">*</span>entity <span class="hl opt">=</span> media_<span class="hl opt">-&gt;</span><span class="hl kwd">getEntityByName</span><span class="hl opt">(</span>entity_<span class="hl opt">);</span>
	<span class="hl kwa">if</span> <span class="hl opt">(!</span>entity<span class="hl opt">)</span>
		<span class="hl kwa">return</span> TestSkip<span class="hl opt">;</span>

	capture_ <span class="hl opt">=</span> <span class="hl kwa">new</span> <span class="hl kwd">V4L2VideoDevice</span><span class="hl opt">(</span>entity<span class="hl opt">);</span>
	<span class="hl kwa">if</span> <span class="hl opt">(!</span>capture_<span class="hl opt">)</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	<span class="hl kwa">if</span> <span class="hl opt">(!</span>media_<span class="hl opt">-&gt;</span><span class="hl kwd">acquire</span><span class="hl opt">())</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	<span class="hl kwb">int</span> ret <span class="hl opt">=</span> media_<span class="hl opt">-&gt;</span><span class="hl kwd">disableLinks</span><span class="hl opt">();</span>
	media_<span class="hl opt">-&gt;</span><span class="hl kwd">release</span><span class="hl opt">();</span>
	<span class="hl kwa">if</span> <span class="hl opt">(</span>ret<span class="hl opt">)</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	<span class="hl kwa">if</span> <span class="hl opt">(</span>capture_<span class="hl opt">-&gt;</span><span class="hl kwd">open</span><span class="hl opt">())</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	V4L2DeviceFormat format <span class="hl opt">= {};</span>
	<span class="hl kwa">if</span> <span class="hl opt">(</span>capture_<span class="hl opt">-&gt;</span><span class="hl kwd">getFormat</span><span class="hl opt">(&amp;</span>format<span class="hl opt">))</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	<span class="hl kwa">if</span> <span class="hl opt">(</span>driver_ <span class="hl opt">==</span> <span class="hl str">&quot;vimc&quot;</span><span class="hl opt">) {</span>
		sensor_ <span class="hl opt">=</span> <span class="hl kwa">new</span> <span class="hl kwd">CameraSensor</span><span class="hl opt">(</span>media_<span class="hl opt">-&gt;</span><span class="hl kwd">getEntityByName</span><span class="hl opt">(</span><span class="hl str">&quot;Sensor A&quot;</span><span class="hl opt">));</span>
		<span class="hl kwa">if</span> <span class="hl opt">(</span>sensor_<span class="hl opt">-&gt;</span><span class="hl kwd">init</span><span class="hl opt">())</span>
			<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

		debayer_ <span class="hl opt">=</span> <span class="hl kwa">new</span> <span class="hl kwd">V4L2Subdevice</span><span class="hl opt">(</span>media_<span class="hl opt">-&gt;</span><span class="hl kwd">getEntityByName</span><span class="hl opt">(</span><span class="hl str">&quot;Debayer A&quot;</span><span class="hl opt">));</span>
		<span class="hl kwa">if</span> <span class="hl opt">(</span>debayer_<span class="hl opt">-&gt;</span><span class="hl kwd">open</span><span class="hl opt">())</span>
			<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

		format<span class="hl opt">.</span>fourcc <span class="hl opt">=</span> <span class="hl kwd">V4L2PixelFormat</span><span class="hl opt">(</span>V4L2_PIX_FMT_SBGGR8<span class="hl opt">);</span>

		V4L2SubdeviceFormat subformat <span class="hl opt">= {};</span>
		subformat<span class="hl opt">.</span>mbus_code <span class="hl opt">=</span> MEDIA_BUS_FMT_SBGGR8_1X8<span class="hl opt">;</span>
		subformat<span class="hl opt">.</span>size <span class="hl opt">=</span> format<span class="hl opt">.</span>size<span class="hl opt">;</span>

		<span class="hl kwa">if</span> <span class="hl opt">(</span>sensor_<span class="hl opt">-&gt;</span><span class="hl kwd">setFormat</span><span class="hl opt">(&amp;</span>subformat<span class="hl opt">))</span>
			<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

		<span class="hl kwa">if</span> <span class="hl opt">(</span>debayer_<span class="hl opt">-&gt;</span><span class="hl kwd">setFormat</span><span class="hl opt">(</span><span class="hl num">0</span><span class="hl opt">, &amp;</span>subformat<span class="hl opt">))</span>
			<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>
	<span class="hl opt">}</span>

	format<span class="hl opt">.</span>size<span class="hl opt">.</span>width <span class="hl opt">=</span> <span class="hl num">640</span><span class="hl opt">;</span>
	format<span class="hl opt">.</span>size<span class="hl opt">.</span>height <span class="hl opt">=</span> <span class="hl num">480</span><span class="hl opt">;</span>
	<span class="hl kwa">if</span> <span class="hl opt">(</span>capture_<span class="hl opt">-&gt;</span><span class="hl kwd">setFormat</span><span class="hl opt">(&amp;</span>format<span class="hl opt">))</span>
		<span class="hl kwa">return</span> TestFail<span class="hl opt">;</span>

	<span class="hl kwa">return</span> TestPass<span class="hl opt">;</span>
<span class="hl opt">}</span>

<span class="hl kwb">void</span> <span class="hl kwc">V4L2VideoDeviceTest</span><span class="hl opt">::</span><span class="hl kwd">cleanup</span><span class="hl opt">()</span>
<span class="hl opt">{</span>
	capture_<span class="hl opt">-&gt;</span><span class="hl kwd">streamOff</span><span class="hl opt">();</span>
	capture_<span class="hl opt">-&gt;</span><span class="hl kwd">releaseBuffers</span><span class="hl opt">();</span>
	capture_<span class="hl opt">-&gt;</span><span class="hl kwd">close</span><span class="hl opt">();</span>

	<span class="hl kwa">delete</span> debayer_<span class="hl opt">;</span>
	<span class="hl kwa">delete</span> sensor_<span class="hl opt">;</span>
	<span class="hl kwa">delete</span> capture_<span class="hl opt">;</span>
<span class="hl opt">}</span>
</code></pre></td></tr></table>
</div> <!-- class=content -->
<div class='footer'>generated by <a href='https://git.zx2c4.com/cgit/about/'>cgit v1.2.1</a> (<a href='https://git-scm.com/'>git 2.18.0</a>) at 2025-02-26 06:31:24 +0000</div>
</div> <!-- id=cgit -->
</body>
</html>