fp6-img/aports/temp/libcamera/0007-libcamera-software_isp-Add-focus-control.patch

From d3ee3b91af950deae8edd4ce4148ac11664c48e9 Mon Sep 17 00:00:00 2001
From: Vasiliy Doylov <nekocwd@mainlining.org>
Date: Mon, 17 Mar 2025 04:24:56 +0300
Subject: [PATCH] libcamera: software_isp: Add focus control

Signed-off-by: Vasiliy Doylov <nekocwd@mainlining.org>
---
 .../internal/software_isp/software_isp.h      |  4 +-
 include/libcamera/ipa/soft.mojom              |  3 +-
 src/ipa/simple/algorithms/af.cpp              | 71 +++++++++++++++++++
 src/ipa/simple/algorithms/af.h                | 40 +++++++++++
 src/ipa/simple/algorithms/meson.build         |  1 +
 src/ipa/simple/data/uncalibrated.yaml         |  1 +
 src/ipa/simple/ipa_context.h                  |  9 +++
 src/ipa/simple/soft_simple.cpp                | 18 ++++-
 src/libcamera/pipeline/simple/simple.cpp      | 30 ++++++--
 src/libcamera/software_isp/software_isp.cpp   |  4 +-
 10 files changed, 169 insertions(+), 12 deletions(-)
 create mode 100644 src/ipa/simple/algorithms/af.cpp
 create mode 100644 src/ipa/simple/algorithms/af.h

diff --git a/include/libcamera/internal/software_isp/software_isp.h b/include/libcamera/internal/software_isp/software_isp.h
index 4f72dce9b..bf2cfea43 100644
--- a/include/libcamera/internal/software_isp/software_isp.h
+++ b/include/libcamera/internal/software_isp/software_isp.h
@@ -86,11 +86,11 @@ public:
 	Signal<FrameBuffer *> outputBufferReady;
 	Signal<uint32_t, uint32_t> ispStatsReady;
 	Signal<uint32_t, const ControlList &> metadataReady;
-	Signal<const ControlList &> setSensorControls;
+	Signal<const ControlList &, const ControlList &> setSensorControls;
 
 private:
 	void saveIspParams();
-	void setSensorCtrls(const ControlList &sensorControls);
+	void setSensorCtrls(const ControlList &sensorControls, const ControlList &lensControls);
 	void statsReady(uint32_t frame, uint32_t bufferId);
 	void inputReady(FrameBuffer *input);
 	void outputReady(FrameBuffer *output);
diff --git a/include/libcamera/ipa/soft.mojom b/include/libcamera/ipa/soft.mojom
index 77328c5fd..e5767532c 100644
--- a/include/libcamera/ipa/soft.mojom
+++ b/include/libcamera/ipa/soft.mojom
@@ -10,6 +10,7 @@ import "include/libcamera/ipa/core.mojom";
 
 struct IPAConfigInfo {
 	libcamera.ControlInfoMap sensorControls;
+	libcamera.ControlInfoMap lensControls;
 };
 
 interface IPASoftInterface {
@@ -32,7 +33,7 @@ interface IPASoftInterface {
 };
 
 interface IPASoftEventInterface {
-	setSensorControls(libcamera.ControlList sensorControls);
+	setSensorControls(libcamera.ControlList sensorControls, libcamera.ControlList lensControls);
 	setIspParams();
 	metadataReady(uint32 frame, libcamera.ControlList metadata);
 };
diff --git a/src/ipa/simple/algorithms/af.cpp b/src/ipa/simple/algorithms/af.cpp
new file mode 100644
index 000000000..6197f3271
--- /dev/null
+++ b/src/ipa/simple/algorithms/af.cpp
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2025 Vasiliy Doylov <nekodevelopper@gmail.com>
+ *
+ * Auto focus
+ */
+
+#include "af.h"
+
+#include <stdint.h>
+
+#include <libcamera/base/log.h>
+
+#include "control_ids.h"
+
+namespace libcamera {
+
+LOG_DEFINE_CATEGORY(IPASoftAutoFocus)
+
+namespace ipa::soft::algorithms {
+
+Af::Af()
+{
+}
+
+int Af::init(IPAContext &context,
+	     [[maybe_unused]] const ValueNode &tuningData)
+{
+	context.ctrlMap[&controls::LensPosition] = ControlInfo(0.0f, 100.0f, 50.0f);
+	return 0;
+}
+
+int Af::configure(IPAContext &context,
+		  [[maybe_unused]] const IPAConfigInfo &configInfo)
+{
+	context.activeState.knobs.focus_pos = std::optional<double>();
+
+	return 0;
+}
+
+void Af::queueRequest([[maybe_unused]] typename Module::Context &context,
+		      [[maybe_unused]] const uint32_t frame,
+		      [[maybe_unused]] typename Module::FrameContext &frameContext,
+		      const ControlList &controls)
+{
+	const auto &focus_pos = controls.get(controls::LensPosition);
+	if (focus_pos.has_value()) {
+		context.activeState.knobs.focus_pos = focus_pos;
+		LOG(IPASoftAutoFocus, Debug) << "Setting focus position to " << focus_pos.value();
+	}
+}
+
+void Af::updateFocus([[maybe_unused]] IPAContext &context, [[maybe_unused]] IPAFrameContext &frameContext, [[maybe_unused]] double exposureMSV)
+{
+	frameContext.lens.focus_pos = context.activeState.knobs.focus_pos.value_or(50.0) / 100.0 * (context.configuration.focus.focus_max - context.configuration.focus.focus_min);
+}
+
+void Af::process([[maybe_unused]] IPAContext &context,
+		 [[maybe_unused]] const uint32_t frame,
+		 [[maybe_unused]] IPAFrameContext &frameContext,
+		 [[maybe_unused]] const SwIspStats *stats,
+		 [[maybe_unused]] ControlList &metadata)
+{
+	updateFocus(context, frameContext, 0);
+}
+
+REGISTER_IPA_ALGORITHM(Af, "Af")
+
+} /* namespace ipa::soft::algorithms */
+
+} /* namespace libcamera */
diff --git a/src/ipa/simple/algorithms/af.h b/src/ipa/simple/algorithms/af.h
new file mode 100644
index 000000000..b138b4d63
--- /dev/null
+++ b/src/ipa/simple/algorithms/af.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2025 Vasiliy Doylov <nekodevelopper@gmail.com>
+ *
+ * Auto focus
+ */
+
+#pragma once
+
+#include "algorithm.h"
+
+namespace libcamera {
+
+namespace ipa::soft::algorithms {
+
+class Af : public Algorithm
+{
+public:
+	Af();
+	~Af() = default;
+
+	int init(IPAContext &context, const ValueNode &tuningData) override;
+	int configure(IPAContext &context, const IPAConfigInfo &configInfo) override;
+	void queueRequest(typename Module::Context &context,
+			  const uint32_t frame,
+			  typename Module::FrameContext &frameContext,
+			  const ControlList &controls)
+		override;
+	void process(IPAContext &context, const uint32_t frame,
+		     IPAFrameContext &frameContext,
+		     const SwIspStats *stats,
+		     ControlList &metadata) override;
+
+private:
+	void updateFocus(IPAContext &context, IPAFrameContext &frameContext, double focus);
+};
+
+} /* namespace ipa::soft::algorithms */
+
+} /* namespace libcamera */
diff --git a/src/ipa/simple/algorithms/meson.build b/src/ipa/simple/algorithms/meson.build
index 73c637220..8950b008f 100644
--- a/src/ipa/simple/algorithms/meson.build
+++ b/src/ipa/simple/algorithms/meson.build
@@ -6,4 +6,5 @@ soft_simple_ipa_algorithms = files([
     'agc.cpp',
     'blc.cpp',
     'ccm.cpp',
+    'af.cpp',
 ])
diff --git a/src/ipa/simple/data/uncalibrated.yaml b/src/ipa/simple/data/uncalibrated.yaml
index fc90ca526..ede277d1d 100644
--- a/src/ipa/simple/data/uncalibrated.yaml
+++ b/src/ipa/simple/data/uncalibrated.yaml
@@ -16,4 +16,5 @@ algorithms:
   #                0, 0, 1]
   - Adjust:
   - Agc:
+  - Af:
 ...
diff --git a/src/ipa/simple/ipa_context.h b/src/ipa/simple/ipa_context.h
index 8ccfacb46..2b6bd4b3a 100644
--- a/src/ipa/simple/ipa_context.h
+++ b/src/ipa/simple/ipa_context.h
@@ -33,6 +33,9 @@ struct IPASessionConfiguration {
 	struct {
 		std::optional<uint8_t> level;
 	} black;
+	struct {
+		int32_t focus_min, focus_max;
+	} focus;
 };
 
 struct IPAActiveState {
@@ -60,6 +63,8 @@ struct IPAActiveState {
 		/* 0..2 range, 1.0 = normal */
 		std::optional<float> contrast;
 		std::optional<float> saturation;
+		/* 0..100 range, 50.0 = normal */
+		std::optional<double> focus_pos;
 	} knobs;
 };
 
@@ -76,6 +81,10 @@ struct IPAFrameContext : public FrameContext {
 	float gamma;
 	std::optional<float> contrast;
 	std::optional<float> saturation;
+
+	struct {
+		int32_t focus_pos;
+	} lens;
 };
 
 struct IPAContext {
diff --git a/src/ipa/simple/soft_simple.cpp b/src/ipa/simple/soft_simple.cpp
index 629e1a32d..dcf746cb2 100644
--- a/src/ipa/simple/soft_simple.cpp
+++ b/src/ipa/simple/soft_simple.cpp
@@ -78,6 +78,7 @@ private:
 	SwIspStats *stats_;
 	std::unique_ptr<CameraSensorHelper> camHelper_;
 	ControlInfoMap sensorInfoMap_;
+	ControlInfoMap lensInfoMap_;
 
 	/* Local parameter storage */
 	struct IPAContext context_;
@@ -202,6 +203,7 @@ int IPASoftSimple::init(const IPASettings &settings,
 int IPASoftSimple::configure(const IPAConfigInfo &configInfo)
 {
 	sensorInfoMap_ = configInfo.sensorControls;
+	lensInfoMap_ = configInfo.lensControls;
 
 	const ControlInfo &exposureInfo = sensorInfoMap_.find(V4L2_CID_EXPOSURE)->second;
 	const ControlInfo &gainInfo = sensorInfoMap_.find(V4L2_CID_ANALOGUE_GAIN)->second;
@@ -211,6 +213,17 @@ int IPASoftSimple::configure(const IPAConfigInfo &configInfo)
 	context_.activeState = {};
 	context_.frameContexts.clear();
 
+	if (lensInfoMap_.empty()) {
+		LOG(IPASoft, Warning) << "No camera leans found! Focus control disabled.";
+		context_.configuration.focus.focus_min = 0;
+		context_.configuration.focus.focus_max = 0;
+	} else {
+		const ControlInfo &lensInfo = lensInfoMap_.find(V4L2_CID_FOCUS_ABSOLUTE)->second;
+		context_.configuration.focus.focus_min = lensInfo.min().get<int32_t>();
+		context_.configuration.focus.focus_max = lensInfo.max().get<int32_t>();
+		LOG(IPASoft, Warning) << "Camera leans found! Focus: " << context_.configuration.focus.focus_min << "-" << context_.configuration.focus.focus_max;
+	}
+
 	context_.configuration.agc.lineDuration =
 		context_.sensorInfo.minLineLength * 1.0s / context_.sensorInfo.pixelRate;
 	context_.configuration.agc.exposureMin = exposureInfo.min().get<int32_t>();
@@ -325,7 +338,10 @@ void IPASoftSimple::processStats(const uint32_t frame,
 	ctrls.set(V4L2_CID_ANALOGUE_GAIN,
 		  static_cast<int32_t>(camHelper_ ? camHelper_->gainCode(againNew) : againNew));
 
-	setSensorControls.emit(ctrls);
+	ControlList lens_ctrls(lensInfoMap_);
+	lens_ctrls.set(V4L2_CID_FOCUS_ABSOLUTE, frameContext.lens.focus_pos);
+
+	setSensorControls.emit(ctrls, lens_ctrls);
 }
 
 std::string IPASoftSimple::logPrefix() const
diff --git a/src/libcamera/pipeline/simple/simple.cpp b/src/libcamera/pipeline/simple/simple.cpp
index b96b8b529..047800b59 100644
--- a/src/libcamera/pipeline/simple/simple.cpp
+++ b/src/libcamera/pipeline/simple/simple.cpp
@@ -33,6 +33,7 @@
 #include <libcamera/stream.h>
 
 #include "libcamera/internal/camera.h"
+#include "libcamera/internal/camera_lens.h"
 #include "libcamera/internal/camera_manager.h"
 #include "libcamera/internal/camera_sensor.h"
 #include "libcamera/internal/camera_sensor_properties.h"
@@ -48,6 +49,8 @@
 #include "libcamera/internal/v4l2_subdevice.h"
 #include "libcamera/internal/v4l2_videodevice.h"
 
+#include "libcamera/controls.h"
+
 namespace libcamera {
 
 LOG_DEFINE_CATEGORY(SimplePipeline)
@@ -371,7 +374,7 @@ private:
 
 	void ispStatsReady(uint32_t frame, uint32_t bufferId);
 	void metadataReady(uint32_t frame, const ControlList &metadata);
-	void setSensorControls(const ControlList &sensorControls);
+	void setSensorControls(const ControlList &sensorControls, const ControlList &lensControls);
 };
 
 class SimpleCameraConfiguration : public CameraConfiguration
@@ -1039,7 +1042,7 @@ void SimpleCameraData::metadataReady(uint32_t frame, const ControlList &metadata
 	tryCompleteRequest(info->request);
 }
 
-void SimpleCameraData::setSensorControls(const ControlList &sensorControls)
+void SimpleCameraData::setSensorControls(const ControlList &sensorControls, const ControlList &lensControls)
 {
 	delayedCtrls_->push(sensorControls);
 	/*
@@ -1050,10 +1053,21 @@ void SimpleCameraData::setSensorControls(const ControlList &sensorControls)
 	 * but it also bypasses delayedCtrls_, creating AGC regulation issues.
 	 * Both problems should be fixed.
 	 */
-	if (!frameStartEmitter_) {
-		ControlList ctrls(sensorControls);
-		sensor_->setControls(&ctrls);
-	}
+	if (frameStartEmitter_)
+		return;
+
+	ControlList ctrls(sensorControls);
+	sensor_->setControls(&ctrls);
+
+	CameraLens *focusLens = sensor_->focusLens();
+	if (!focusLens)
+		return;
+
+	if (!lensControls.contains(V4L2_CID_FOCUS_ABSOLUTE))
+		return;
+
+	const ControlValue &focusValue = lensControls.get(V4L2_CID_FOCUS_ABSOLUTE);
+	focusLens->setFocusPosition(focusValue.get<int32_t>());
 }
 
 /* Retrieve all source pads connected to a sink pad through active routes. */
@@ -1603,6 +1617,10 @@ int SimplePipelineHandler::configure(Camera *camera, CameraConfiguration *c)
 	} else {
 		ipa::soft::IPAConfigInfo configInfo;
 		configInfo.sensorControls = data->sensor_->controls();
+		if (data->sensor_->focusLens() != nullptr)
+			configInfo.lensControls = data->sensor_->focusLens()->controls();
+		else
+			configInfo.lensControls = ControlInfoMap();
 		return data->swIsp_->configure(inputCfg, outputCfgs, configInfo);
 	}
 }
diff --git a/src/libcamera/software_isp/software_isp.cpp b/src/libcamera/software_isp/software_isp.cpp
index c73a16ce0..2e6fc3d98 100644
--- a/src/libcamera/software_isp/software_isp.cpp
+++ b/src/libcamera/software_isp/software_isp.cpp
@@ -436,9 +436,9 @@ void SoftwareIsp::saveIspParams()
 	debayerParams_ = *sharedParams_;
 }
 
-void SoftwareIsp::setSensorCtrls(const ControlList &sensorControls)
+void SoftwareIsp::setSensorCtrls(const ControlList &sensorControls, const ControlList &lensControls)
 {
-	setSensorControls.emit(sensorControls);
+	setSensorControls.emit(sensorControls, lensControls);
 }
 
 void SoftwareIsp::statsReady(uint32_t frame, uint32_t bufferId)