ipu3: Apply auto focus and send lens controls to pipeline handler

Apply auto focus and send lens controls to pipeline handler.

Signed-off-by: Han-Lin Chen <hanlinchen@chromium.org>
Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
Reviewed-by: Umang Jain <umang.jain@ideasonboard.com>
Signed-off-by: Kieran Bingham <kieran.bingham@ideasonboard.com>

4 files changed, 28 insertions, 6 deletions

diff --git a/aiq/aiq.cpp b/aiq/aiq.cpp
index 24c61cb..52a9c07 100644
--- a/aiq/aiq.cpp
+++ b/aiq/aiq.cpp
@@ -138,8 +138,7 @@ int AIQ::setStatistics(unsigned int frame,
  * might run asycnronously, or after receipt of statistics, with the filling
  * of the parameter buffer being the only part handled when called for.
  */
-int AIQ::run2a(unsigned int frame, AiqInputParameters &params,
-	       AiqResults &results)
+int AIQ::run2a(unsigned int frame, AiqInputParameters &params, AiqResults &results)
 {
 	(void)frame;
 
diff --git a/aiq/aiq.h b/aiq/aiq.h
index fcd02d2..49eee2d 100644
--- a/aiq/aiq.h
+++ b/aiq/aiq.h
@@ -39,9 +39,7 @@ public:
 	int setStatistics(unsigned int frame,
 			  int64_t timestamp, AiqResults &results,
 			  const ipu3_uapi_stats_3a *stats);
-
-	int run2a(unsigned int frame, AiqInputParameters &params,
-		  AiqResults &results);
+	int run2a(unsigned int frame, AiqInputParameters &params, AiqResults &results);
 
 private:
 	std::string decodeError(ia_err err);
diff --git a/aiq/aiq_input_parameters.cpp b/aiq/aiq_input_parameters.cpp
index 8f60d14..7a6a1fa 100644
--- a/aiq/aiq_input_parameters.cpp
+++ b/aiq/aiq_input_parameters.cpp
@@ -166,7 +166,7 @@ void AiqInputParameters::setAeAwbAfDefaults()
 		ia_aiq_af_range_normal,
 		ia_aiq_af_metering_mode_auto,
 		ia_aiq_flash_mode_off,
-		NULL, NULL, false
+		&focusRect, &manualFocusParams, false
 	};
 
 	/* GBCE Params */
diff --git a/ipu3.cpp b/ipu3.cpp
index e2b5710..6ab86e0 100644
--- a/ipu3.cpp
+++ b/ipu3.cpp
@@ -78,6 +78,10 @@ private:
 	uint32_t gain_;
 	uint32_t minGain_;
 	uint32_t maxGain_;
+	int32_t lensPosition_;
+
+	/* Intel AF library relies on timestamp to wait for lens movement */
+	uint64_t lensMovementStartTime_;
 
 	/* Intel Library Instances. */
 	aiq::AIQ aiq_;
@@ -259,6 +263,9 @@ int IPAIPU3::configure(const IPAConfigInfo &configInfo,
 	maxGain_ = itGain->second.max().get<int32_t>();
 	gain_ = maxGain_;
 
+	lensMovementStartTime_ = 0;
+	lensPosition_ = 0;
+
 	int ret;
 
 	ret = aiq_.configure();
@@ -370,11 +377,27 @@ void IPAIPU3::runAiq([[maybe_unused]] unsigned int frame)
 	resultsHistory_.extendOne();
 	aiq::AiqResults& latestResults = resultsHistory_.latest();
 
+	/* Todo: Refactor AiqInputParameters interface to set following parameters. */
+	aiqInputParams_.afParams.lens_position = lensPosition_;
+	aiqInputParams_.afParams.lens_movement_start_timestamp = lensMovementStartTime_;
+
 	aiq_.run2a(frame, aiqInputParams_, latestResults);
 
 	exposure_ = latestResults.ae()->exposures[0].sensor_exposure->coarse_integration_time;
 	gain_ = latestResults.ae()->exposures[0].sensor_exposure->analog_gain_code_global;
 
+	/*
+	 * Af algorithm compares the timestamp of start of the lens movement and
+	 * that of the statistics generated to estimate whether next lens
+	 * position should be produced.
+	 * \todo use the lens movement start time reported by the pipeline handler.
+	 */
+	if (lensPosition_ != latestResults.af()->next_lens_position) {
+		utils::time_point time = utils::clock::now();
+		uint64_t usecs = std::chrono::duration_cast<std::chrono::microseconds>(time.time_since_epoch()).count();
+		lensMovementStartTime_ = usecs;
+	}
+	lensPosition_ = latestResults.af()->next_lens_position;
 }
 
 void IPAIPU3::fillParams(unsigned int frame, ipu3_uapi_params *params)
@@ -483,6 +506,8 @@ void IPAIPU3::setControls(unsigned int frame)
 
 	op.sensorControls = sensorCtrls;
 
+	op.lensControls.set(V4L2_CID_FOCUS_ABSOLUTE, lensPosition_);
+
 	queueFrameAction.emit(frame, op);
 }