From c0ec98a6b90bb55e838f2cff272503ee55db456a Mon Sep 17 00:00:00 2001
From: Daniel Semkowicz via libcamera-devel
 <libcamera-devel@lists.libcamera.org>
Date: Tue, 28 Jun 2022 11:06:56 +0200
Subject: libcamera: rkisp1: Control the lens from IPA

Check if lens are available and have ability to control the focus.
Connect IPA setLensControls() signal to the pipeline slot that controls
the lens. If lens are valid, allow changing the focus from IPA by
emitting signal to the pipeline.

Signed-off-by: Daniel Semkowicz <dse@thaumatec.com>
---
 include/libcamera/ipa/rkisp1.mojom       |  1 +
 src/ipa/rkisp1/ipa_context.h             |  4 ++++
 src/ipa/rkisp1/rkisp1.cpp                | 36 ++++++++++++++++++++++++++++++++
 src/libcamera/pipeline/rkisp1/rkisp1.cpp |  1 +
 4 files changed, 42 insertions(+)

diff --git a/include/libcamera/ipa/rkisp1.mojom b/include/libcamera/ipa/rkisp1.mojom
index e3537385..ee06a6b0 100644
--- a/include/libcamera/ipa/rkisp1.mojom
+++ b/include/libcamera/ipa/rkisp1.mojom
@@ -32,5 +32,6 @@ interface IPARkISP1Interface {
 interface IPARkISP1EventInterface {
 	paramsBufferReady(uint32 frame);
 	setSensorControls(uint32 frame, libcamera.ControlList sensorControls);
+	setLensControls(libcamera.ControlList sensorControls);
 	metadataReady(uint32 frame, libcamera.ControlList metadata);
 };
diff --git a/src/ipa/rkisp1/ipa_context.h b/src/ipa/rkisp1/ipa_context.h
index f387cace..4b199048 100644
--- a/src/ipa/rkisp1/ipa_context.h
+++ b/src/ipa/rkisp1/ipa_context.h
@@ -41,6 +41,10 @@ struct IPASessionConfiguration {
 };
 
 struct IPAFrameContext {
+	struct {
+		uint32_t focus;
+	} af;
+
 	struct {
 		uint32_t exposure;
 		double gain;
diff --git a/src/ipa/rkisp1/rkisp1.cpp b/src/ipa/rkisp1/rkisp1.cpp
index 33642094..01bb54fb 100644
--- a/src/ipa/rkisp1/rkisp1.cpp
+++ b/src/ipa/rkisp1/rkisp1.cpp
@@ -7,6 +7,7 @@
 
 #include <algorithm>
 #include <math.h>
+#include <optional>
 #include <queue>
 #include <stdint.h>
 #include <string.h>
@@ -65,6 +66,8 @@ protected:
 	std::string logPrefix() const override;
 
 private:
+	bool validateLensControls(const ControlInfoMap &lensControls);
+
 	void setControls(unsigned int frame);
 	void prepareMetadata(unsigned int frame, unsigned int aeState);
 
@@ -72,6 +75,7 @@ private:
 	std::map<unsigned int, MappedFrameBuffer> mappedBuffers_;
 
 	ControlInfoMap sensorCtrls_;
+	std::optional<ControlInfoMap> lensCtrls_;
 
 	/* Camera sensor controls. */
 	bool autoExposure_;
@@ -192,6 +196,14 @@ int IPARkISP1::configure([[maybe_unused]] const IPACameraSensorInfo &info,
 		return -EINVAL;
 	}
 
+	auto lensControls = entityControls.find(1);
+	if (lensControls != entityControls.end()) {
+		if (validateLensControls(lensControls->second))
+			lensCtrls_ = lensControls->second;
+		else
+			LOG(IPARkISP1, Error) << "Lens control validation failed.";
+	}
+
 	autoExposure_ = true;
 
 	int32_t minExposure = itExp->second.min().get<int32_t>();
@@ -235,6 +247,23 @@ int IPARkISP1::configure([[maybe_unused]] const IPACameraSensorInfo &info,
 	return 0;
 }
 
+bool IPARkISP1::validateLensControls(const ControlInfoMap &lensControls)
+{
+	static const uint32_t ctrls[] = {
+		V4L2_CID_FOCUS_ABSOLUTE,
+	};
+
+	for (auto c : ctrls) {
+		if (lensControls.find(c) == lensControls.end()) {
+			LOG(IPARkISP1, Error) << "Unable to find lens control "
+					      << utils::hex(c);
+			return false;
+		}
+	}
+
+	return true;
+}
+
 void IPARkISP1::mapBuffers(const std::vector<IPABuffer> &buffers)
 {
 	for (const IPABuffer &buffer : buffers) {
@@ -319,6 +348,13 @@ void IPARkISP1::setControls(unsigned int frame)
 	ctrls.set(V4L2_CID_ANALOGUE_GAIN, static_cast<int32_t>(gain));
 
 	setSensorControls.emit(frame, ctrls);
+
+	if (lensCtrls_) {
+		ControlList lensCtrls(*lensCtrls_);
+		lensCtrls.set(V4L2_CID_FOCUS_ABSOLUTE,
+			      static_cast<int32_t>(context_.frameContext.af.focus));
+		setLensControls.emit(lensCtrls);
+	}
 }
 
 void IPARkISP1::prepareMetadata(unsigned int frame, unsigned int aeState)
diff --git a/src/libcamera/pipeline/rkisp1/rkisp1.cpp b/src/libcamera/pipeline/rkisp1/rkisp1.cpp
index d9c740d6..d54328d5 100644
--- a/src/libcamera/pipeline/rkisp1/rkisp1.cpp
+++ b/src/libcamera/pipeline/rkisp1/rkisp1.cpp
@@ -323,6 +323,7 @@ int RkISP1CameraData::loadIPA(unsigned int hwRevision)
 		return -ENOENT;
 
 	ipa_->setSensorControls.connect(this, &RkISP1CameraData::setSensorControls);
+	ipa_->setLensControls.connect(this, &RkISP1CameraData::setLensControls);
 	ipa_->paramsBufferReady.connect(this, &RkISP1CameraData::paramFilled);
 	ipa_->metadataReady.connect(this, &RkISP1CameraData::metadataReady);
 
-- 
cgit v1.2.1