libcamera: software_isp: Add focus control

Signed-off-by: Vasiliy Doylov <nekocwd@mainlining.org>

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);

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);
 };

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 YamlObject &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 */

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 YamlObject &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 */

src/ipa/simple/algorithms/meson.build

@@ -6,4 +6,5 @@ soft_simple_ipa_algorithms = files([
     'blc.cpp',
     'ccm.cpp',
     'lut.cpp',
+    'af.cpp',
 ])

src/ipa/simple/data/uncalibrated.yaml

@@ -16,4 +16,5 @@ algorithms:
   #                0, 0, 1]
   - Lut:
   - Agc:
+  - Af:
 ...

src/ipa/simple/ipa_context.h

@@ -34,6 +34,9 @@ struct IPASessionConfiguration {
 	struct {
 		std::optional<uint8_t> level;
 	} black;
+	struct {
+		int32_t focus_min, focus_max;
+	} focus;
 };
 
 struct IPAActiveState {
@@ -68,6 +71,8 @@ struct IPAActiveState {
 		std::optional<double> brightness;
 		/* 0..1 range, 1 = normal */
 		std::optional<bool> ae_enabled;
+		/* 0..100 range, 50.0 = normal */
+		std::optional<double> focus_pos;
 	} knobs;
 };
 
@@ -81,6 +86,10 @@ struct IPAFrameContext : public FrameContext {
 		double gain;
 	} sensor;
 
+	struct {
+		int32_t focus_pos;
+	} lens;
+
 	struct {
 		double red;
 		double blue;

src/ipa/simple/soft_simple.cpp

@@ -77,6 +77,7 @@ private:
 	SwIspStats *stats_;
 	std::unique_ptr<CameraSensorHelper> camHelper_;
 	ControlInfoMap sensorInfoMap_;
+	ControlInfoMap lensInfoMap_;
 
 	/* Local parameter storage */
 	struct IPAContext context_;
@@ -196,6 +197,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;
@@ -205,6 +207,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>();
@@ -327,7 +340,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

src/libcamera/pipeline/simple/simple.cpp

@@ -30,6 +30,7 @@
 #include <libcamera/stream.h>
 
 #include "libcamera/internal/camera.h"
+#include "libcamera/internal/camera_lens.h"
 #include "libcamera/internal/camera_sensor.h"
 #include "libcamera/internal/camera_sensor_properties.h"
 #include "libcamera/internal/converter.h"
@@ -41,6 +42,8 @@
 #include "libcamera/internal/v4l2_subdevice.h"
 #include "libcamera/internal/v4l2_videodevice.h"
 
+#include "libcamera/controls.h"
+
 namespace libcamera {
 
 LOG_DEFINE_CATEGORY(SimplePipeline)
@@ -356,7 +359,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
@@ -1002,7 +1005,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);
 	/*
@@ -1013,10 +1016,21 @@ void SimpleCameraData::setSensorControls(const ControlList &sensorControls)
 	 * but it also bypasses delayedCtrls_, creating AGC regulation issues.
 	 * Both problems should be fixed.
 	 */
-	if (!frameStartEmitter_) {
+	if (frameStartEmitter_)
-		ControlList ctrls(sensorControls);
+		return;
-		sensor_->setControls(&ctrls);
+
-	}
+	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. */
@@ -1406,6 +1420,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);
 	}
 }

src/libcamera/software_isp/software_isp.cpp

@@ -395,9 +395,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)