From e78832cf91a81539f6188cf38739c3547b74684b Mon Sep 17 00:00:00 2001
From: Daniel Semkowicz <dse@thaumatec.com>
Date: Thu, 19 Jan 2023 09:41:06 +0100
Subject: rkisp1: Control camera lens position from IPA

Allow control of lens position from the IPA, by setting corresponding
af fields in the IPAFrameContext structure. Controls are then passed to
the pipeline handler, which sets the lens position in CameraLens.

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

diff --git a/include/libcamera/ipa/rkisp1.mojom b/include/libcamera/ipa/rkisp1.mojom
index bf6e9141..c3ed87aa 100644
--- a/include/libcamera/ipa/rkisp1.mojom
+++ b/include/libcamera/ipa/rkisp1.mojom
@@ -39,5 +39,6 @@ interface IPARkISP1Interface {
 interface IPARkISP1EventInterface {
 	paramsBufferReady(uint32 frame);
 	setSensorControls(uint32 frame, libcamera.ControlList sensorControls);
+	setLensControls(libcamera.ControlList lensControls);
 	metadataReady(uint32 frame, libcamera.ControlList metadata);
 };
diff --git a/src/ipa/rkisp1/ipa_context.h b/src/ipa/rkisp1/ipa_context.h
index b9b20653..1fac6af9 100644
--- a/src/ipa/rkisp1/ipa_context.h
+++ b/src/ipa/rkisp1/ipa_context.h
@@ -53,6 +53,11 @@ struct IPASessionConfiguration {
 };
 
 struct IPAActiveState {
+	struct {
+		uint32_t lensPosition;
+		bool applyLensCtrls;
+	} af;
+
 	struct {
 		struct {
 			uint32_t exposure;
diff --git a/src/ipa/rkisp1/rkisp1.cpp b/src/ipa/rkisp1/rkisp1.cpp
index 9e861fc0..297161b2 100644
--- a/src/ipa/rkisp1/rkisp1.cpp
+++ b/src/ipa/rkisp1/rkisp1.cpp
@@ -270,6 +270,10 @@ int IPARkISP1::configure(const IPAConfigInfo &ipaConfig,
 			return format.colourEncoding == PixelFormatInfo::ColourEncodingRAW;
 		});
 
+	/* Lens position is unknown at the startup, so initilize the variable
+	 * that holds the current position to something out of the range. */
+	context_.activeState.af.lensPosition = std::numeric_limits<int32_t>::max();
+
 	for (auto const &a : algorithms()) {
 		Algorithm *algo = static_cast<Algorithm *>(a.get());
 
@@ -452,6 +456,14 @@ void IPARkISP1::setControls(unsigned int frame)
 	ctrls.set(V4L2_CID_ANALOGUE_GAIN, static_cast<int32_t>(gain));
 
 	setSensorControls.emit(frame, ctrls);
+
+	if (lensControls_ && context_.activeState.af.applyLensCtrls) {
+		context_.activeState.af.applyLensCtrls = false;
+		ControlList lensCtrls(*lensControls_);
+		lensCtrls.set(V4L2_CID_FOCUS_ABSOLUTE,
+			      static_cast<int32_t>(context_.activeState.af.lensPosition));
+		setLensControls.emit(lensCtrls);
+	}
 }
 
 } /* namespace ipa::rkisp1 */
diff --git a/src/libcamera/pipeline/rkisp1/rkisp1.cpp b/src/libcamera/pipeline/rkisp1/rkisp1.cpp
index e13d249b..7e0964c4 100644
--- a/src/libcamera/pipeline/rkisp1/rkisp1.cpp
+++ b/src/libcamera/pipeline/rkisp1/rkisp1.cpp
@@ -114,6 +114,7 @@ private:
 	void paramFilled(unsigned int frame);
 	void setSensorControls(unsigned int frame,
 			       const ControlList &sensorControls);
+	void setLensControls(const ControlList &lensControls);
 
 	void metadataReady(unsigned int frame, const ControlList &metadata);
 };
@@ -342,6 +343,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);
 
@@ -405,6 +407,20 @@ void RkISP1CameraData::setSensorControls([[maybe_unused]] unsigned int frame,
 	delayedCtrls_->push(sensorControls);
 }
 
+void RkISP1CameraData::setLensControls(const ControlList &lensControls)
+{
+	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>());
+}
+
 void RkISP1CameraData::metadataReady(unsigned int frame, const ControlList &metadata)
 {
 	RkISP1FrameInfo *info = frameInfo_.find(frame);
-- 
cgit v1.2.1