6 files changed, 578 insertions, 0 deletions

diff --git a/src/ipa/ipu3/algorithms/af.cpp b/src/ipa/ipu3/algorithms/af.cpp
new file mode 100644
index 00000000..0170a372
--- /dev/null
+++ b/src/ipa/ipu3/algorithms/af.cpp
@@ -0,0 +1,463 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ *
+ * af.cpp - IPU3 auto focus algorithm
+ */
+
+#include "af.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <fcntl.h>
+#include <numeric>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <linux/videodev2.h>
+
+#include <libcamera/base/log.h>
+
+#include <libcamera/ipa/core_ipa_interface.h>
+
+#include "libipa/histogram.h"
+
+/**
+ * \file af.h
+ */
+
+/**
+ * \var kAfMinGridWidth
+ * \brief the minimum width of AF grid.
+ * The minimum grid horizontal dimensions.
+ */
+
+/**
+ * \var kAfMinGridHeight
+ * \brief the minimum height of AF grid.
+ * The minimum grid vertical dimensions.
+ */
+
+/**
+ * \var kAfMaxGridWidth
+ * \brief the maximum width of AF grid.
+ * The maximum grid horizontal dimensions.
+ */
+
+/**
+ * \var kAfMaxGridHeight
+ * \brief The maximum height of AF grid.
+ * The maximum grid vertical dimensions.
+ */
+
+/**
+ * \var kAfMinGridBlockWidth
+ * \brief The minimum block size of the width.
+ * The minimum value of Log2 of the width of the grid cell.
+ */
+
+/**
+ * \var kAfMinGridBlockHeight
+ * \brief The minimum block size of the height.
+ * The minimum value of Log2 of the height of the grid cell.
+ */
+
+/**
+ * \def kAfMaxGridBlockWidth
+ * \brief The maximum block size of the width.
+ * The maximum value of Log2 of the width of the grid cell.
+ */
+
+/**
+ * \var kAfMaxGridBlockHeight
+ * \brief The maximum block size of the height.
+ * The maximum value of Log2 of the height of the grid cell.
+ */
+
+/**
+ * \var kAfDefaultHeightPerSlice
+ * \brief The default number of blocks in vertical axis per slice.
+ * The number of blocks in vertical axis per slice.
+ */
+
+namespace libcamera {
+
+using namespace std::literals::chrono_literals;
+
+namespace ipa::ipu3::algorithms {
+
+LOG_DEFINE_CATEGORY(IPU3Af)
+
+/**
+ * Maximum focus steps of the VCM control
+ * \todo should be obtained from the VCM driver
+ */
+static constexpr uint32_t kMaxFocusSteps = 1023;
+
+/* Minimum focus step for searching appropriate focus */
+static constexpr uint32_t kCoarseSearchStep = 30;
+static constexpr uint32_t kFineSearchStep = 1;
+
+/* Max ratio of variance change, 0.0 < kMaxChange < 1.0 */
+static constexpr double kMaxChange = 0.5;
+
+/* The numbers of frame to be ignored, before performing focus scan. */
+static constexpr uint32_t kIgnoreFrame = 10;
+
+/* Fine scan range 0 < kFineRange < 1 */
+static constexpr double kFineRange = 0.05;
+
+/* Settings for IPU3 AF filter */
+static struct ipu3_uapi_af_filter_config afFilterConfigDefault = {
+	.y1_coeff_0 = { 0, 1, 3, 7 },
+	.y1_coeff_1 = { 11, 13, 1, 2 },
+	.y1_coeff_2 = { 8, 19, 34, 242 },
+	.y1_sign_vec = 0x7fdffbfe,
+	.y2_coeff_0 = { 0, 1, 6, 6 },
+	.y2_coeff_1 = { 13, 25, 3, 0 },
+	.y2_coeff_2 = { 25, 3, 177, 254 },
+	.y2_sign_vec = 0x4e53ca72,
+	.y_calc = { 8, 8, 8, 8 },
+	.nf = { 0, 9, 0, 9, 0 },
+};
+
+/**
+ * \class Af
+ * \brief An auto-focus algorithm based on IPU3 statistics
+ *
+ * This algorithm is used to determine the position of the lens to make a
+ * focused image. The IPU3 AF processing block computes the statistics that
+ * are composed by two types of filtered value and stores in a AF buffer.
+ * Typically, for a clear image, it has a relatively higher contrast than a
+ * blurred one. Therefore, if an image with the highest contrast can be
+ * found through the scan, the position of the len indicates to a clearest
+ * image.
+ */
+Af::Af()
+	: focus_(0), bestFocus_(0), currentVariance_(0.0), previousVariance_(0.0),
+	  coarseCompleted_(false), fineCompleted_(false)
+{
+}
+
+/**
+ * \copydoc libcamera::ipa::Algorithm::prepare
+ */
+void Af::prepare(IPAContext &context, ipu3_uapi_params *params)
+{
+	const struct ipu3_uapi_grid_config &grid = context.configuration.af.afGrid;
+	params->acc_param.af.grid_cfg = grid;
+	params->acc_param.af.filter_config = afFilterConfigDefault;
+
+	/* Enable AF processing block */
+	params->use.acc_af = 1;
+}
+
+/**
+ * \brief Configure the Af given a configInfo
+ * \param[in] context The shared IPA context
+ * \param[in] configInfo The IPA configuration data
+ * \return 0 on success, a negative error code otherwise
+ */
+int Af::configure(IPAContext &context, const IPAConfigInfo &configInfo)
+{
+	struct ipu3_uapi_grid_config &grid = context.configuration.af.afGrid;
+	grid.width = kAfMinGridWidth;
+	grid.height = kAfMinGridHeight;
+	grid.block_width_log2 = kAfMinGridBlockWidth;
+	grid.block_height_log2 = kAfMinGridBlockHeight;
+	grid.height_per_slice = kAfDefaultHeightPerSlice;
+
+	/* x_start and y start are default to BDS center */
+	grid.x_start = (configInfo.bdsOutputSize.width / 2) -
+		       (((grid.width << grid.block_width_log2) / 2));
+	grid.y_start = (configInfo.bdsOutputSize.height / 2) -
+		       (((grid.height << grid.block_height_log2) / 2));
+
+	/* x_start and y_start should be even */
+	grid.x_start = (grid.x_start / 2) * 2;
+	grid.y_start = (grid.y_start / 2) * 2;
+	grid.y_start = grid.y_start | IPU3_UAPI_GRID_Y_START_EN;
+
+	/* Initial max focus step */
+	maxStep_ = kMaxFocusSteps;
+
+	/* Initial focus value */
+	context.frameContext.af.focus = 0;
+	/* Maximum variance of the AF statistics */
+	context.frameContext.af.maxVariance = 0;
+	/* The stable AF value flag. if it is true, the AF should be in a stable state. */
+	context.frameContext.af.stable = false;
+
+	return 0;
+}
+
+/**
+ * \brief AF coarse scan
+ *
+ * Find a near focused image using a coarse step. The step is determined by coarseSearchStep.
+ *
+ * \param[in] context The shared IPA context
+ */
+void Af::afCoarseScan(IPAContext &context)
+{
+	if (coarseCompleted_)
+		return;
+
+	if (afNeedIgnoreFrame())
+		return;
+
+	if (afScan(context, kCoarseSearchStep)) {
+		coarseCompleted_ = true;
+		context.frameContext.af.maxVariance = 0;
+		focus_ = context.frameContext.af.focus -
+			 (context.frameContext.af.focus * kFineRange);
+		context.frameContext.af.focus = focus_;
+		previousVariance_ = 0;
+		maxStep_ = std::clamp(focus_ + static_cast<uint32_t>((focus_ * kFineRange)),
+				      0U, kMaxFocusSteps);
+	}
+}
+
+/**
+ * \brief AF fine scan
+ *
+ * Find an optimum lens position with moving 1 step for each search.
+ *
+ * \param[in] context The shared IPA context
+ */
+void Af::afFineScan(IPAContext &context)
+{
+	if (!coarseCompleted_)
+		return;
+
+	if (afNeedIgnoreFrame())
+		return;
+
+	if (afScan(context, kFineSearchStep)) {
+		context.frameContext.af.stable = true;
+		fineCompleted_ = true;
+	}
+}
+
+/**
+ * \brief AF reset
+ *
+ * Reset all the parameters to start over the AF process.
+ *
+ * \param[in] context The shared IPA context
+ */
+void Af::afReset(IPAContext &context)
+{
+	if (afNeedIgnoreFrame())
+		return;
+
+	context.frameContext.af.maxVariance = 0;
+	context.frameContext.af.focus = 0;
+	focus_ = 0;
+	context.frameContext.af.stable = false;
+	ignoreCounter_ = kIgnoreFrame;
+	previousVariance_ = 0.0;
+	coarseCompleted_ = false;
+	fineCompleted_ = false;
+	maxStep_ = kMaxFocusSteps;
+}
+
+/**
+ * \brief AF variance comparison.
+ * \param[in] context The IPA context
+ * \param min_step The VCM movement step.
+ *
+ * We always pick the largest variance to replace the previous one. The image
+ * with a larger variance also indicates it is a clearer image than previous
+ * one. If we find a negative derivative, we return immediately.
+ *
+ * \return True, if it finds a AF value.
+ */
+bool Af::afScan(IPAContext &context, int min_step)
+{
+	if (focus_ > maxStep_) {
+		/* If reach the max step, move lens to the position. */
+		context.frameContext.af.focus = bestFocus_;
+		return true;
+	} else {
+		/*
+		 * Find the maximum of the variance by estimating its
+		 * derivative. If the direction changes, it means we have
+		 * passed a maximum one step before.
+		 */
+		if ((currentVariance_ - context.frameContext.af.maxVariance) >=
+		    -(context.frameContext.af.maxVariance * 0.1)) {
+			/*
+			 * Positive and zero derivative:
+			 * The variance is still increasing. The focus could be
+			 * increased for the next comparison. Also, the max variance
+			 * and previous focus value are updated.
+			 */
+			bestFocus_ = focus_;
+			focus_ += min_step;
+			context.frameContext.af.focus = focus_;
+			context.frameContext.af.maxVariance = currentVariance_;
+		} else {
+			/*
+			 * Negative derivative:
+			 * The variance starts to decrease which means the maximum
+			 * variance is found. Set focus step to previous good one
+			 * then return immediately.
+			 */
+			context.frameContext.af.focus = bestFocus_;
+			return true;
+		}
+	}
+
+	previousVariance_ = currentVariance_;
+	LOG(IPU3Af, Debug) << " Previous step is "
+			   << bestFocus_
+			   << " Current step is "
+			   << focus_;
+	return false;
+}
+
+/**
+ * \brief Determine the frame to be ignored.
+ * \return Return True if the frame should be ignored, false otherwise
+ */
+bool Af::afNeedIgnoreFrame()
+{
+	if (ignoreCounter_ == 0)
+		return false;
+	else
+		ignoreCounter_--;
+	return true;
+}
+
+/**
+ * \brief Reset frame ignore counter.
+ */
+void Af::afIgnoreFrameReset()
+{
+	ignoreCounter_ = kIgnoreFrame;
+}
+
+/**
+ * \brief Estimate variance
+ * \param y_item The AF filter data set from the IPU3 statistics buffer
+ * \param len The quantity of table item entries which are valid to process
+ * \param isY1 Selects between filter Y1 or Y2 to calculate the variance
+ *
+ * Calculate the mean of the data set provided by \a y_item, and then calculate
+ * the variance of that data set from the mean.
+ *
+ * The operation can work on one of two sets of values contained within the
+ * y_item data set supplied by the IPU3. The two data sets are the results of
+ * both the Y1 and Y2 filters which are used to support coarse (Y1) and fine
+ * (Y2) calculations of the contrast.
+ *
+ * \return The variance of the values in the data set \a y_item selected by \a isY1
+ */
+double Af::afEstimateVariance(y_table_item_t *y_item, uint32_t len,
+			      bool isY1)
+{
+	uint32_t z = 0;
+	uint32_t total = 0;
+	double mean;
+	double var_sum = 0;
+
+	for (z = 0; z < len; z++) {
+		if (isY1)
+			total += y_item[z].y1_avg;
+		else
+			total += y_item[z].y2_avg;
+	}
+	mean = total / len;
+	for (z = 0; z < len; z++) {
+		if (isY1)
+			var_sum += pow((y_item[z].y1_avg - mean), 2);
+		else
+			var_sum += pow((y_item[z].y2_avg - mean), 2);
+	}
+
+	return var_sum / static_cast<double>(len);
+}
+
+/**
+ * \brief Determine out-of-focus situation.
+ * \param context The IPA context.
+ *
+ * Out-of-focus means that the variance change rate for a focused and a new
+ * variance is greater than a threshold.
+ *
+ * \return True if the variance threshold is crossed indicating lost focus,
+ *         false otherwise.
+ */
+bool Af::afIsOutOfFocus(IPAContext context)
+{
+	const uint32_t diff_var = std::abs(currentVariance_ -
+					   context.frameContext.af.maxVariance);
+	const double var_ratio = diff_var / context.frameContext.af.maxVariance;
+
+	LOG(IPU3Af, Debug) << "Variance change rate: "
+			   << var_ratio
+			   << " Current VCM step: "
+			   << context.frameContext.af.focus;
+
+	if (var_ratio > kMaxChange)
+		return true;
+	else
+		return false;
+}
+
+/**
+ * \brief Determine the max contrast image and lens position.
+ * \param[in] context The IPA context.
+ * \param[in] stats The statistics buffer of IPU3.
+ *
+ * Ideally, a clear image also has a relatively higher contrast. So, every
+ * image for each focus step should be tested to find an optimal focus step.
+ *
+ * The Hill Climbing Algorithm[1] is used to find the maximum variance of the
+ * AF statistics which is the AF output of IPU3. The focus step is increased
+ * then the variance of the AF statistics are estimated. If it finds the
+ * negative derivative we have just passed the peak, and we infer that the best
+ * focus is found.
+ *
+ * [1] Hill Climbing Algorithm, https://en.wikipedia.org/wiki/Hill_climbing
+ */
+void Af::process(IPAContext &context, const ipu3_uapi_stats_3a *stats)
+{
+	y_table_item_t y_item[IPU3_UAPI_AF_Y_TABLE_MAX_SIZE / sizeof(y_table_item_t)];
+	uint32_t afRawBufferLen;
+
+	/* Evaluate the AF buffer length */
+	afRawBufferLen = context.configuration.af.afGrid.width *
+			 context.configuration.af.afGrid.height;
+
+	memcpy(y_item, stats->af_raw_buffer.y_table,
+	       afRawBufferLen * sizeof(y_table_item_t));
+
+	/*
+	 * Calculate the mean and the variance of AF statistics for a given grid.
+	 * For coarse: y1 are used.
+	 * For fine: y2 results are used.
+	 */
+	if (coarseCompleted_)
+		currentVariance_ = afEstimateVariance(y_item, afRawBufferLen, false);
+	else
+		currentVariance_ = afEstimateVariance(y_item, afRawBufferLen, true);
+
+	if (!context.frameContext.af.stable) {
+		afCoarseScan(context);
+		afFineScan(context);
+	} else {
+		if (afIsOutOfFocus(context))
+			afReset(context);
+		else
+			afIgnoreFrameReset();
+	}
+}
+
+} /* namespace ipa::ipu3::algorithms */
+
+} /* namespace libcamera */
diff --git a/src/ipa/ipu3/algorithms/af.h b/src/ipa/ipu3/algorithms/af.h
new file mode 100644
index 00000000..13c7e0e8
--- /dev/null
+++ b/src/ipa/ipu3/algorithms/af.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ *
+ * af.h - IPU3 Af algorithm
+ */
+
+#pragma once
+
+#include <linux/intel-ipu3.h>
+
+#include <libcamera/base/utils.h>
+
+#include <libcamera/geometry.h>
+
+#include "algorithm.h"
+
+/* Static variables from repo of chromium */
+static constexpr uint8_t kAfMinGridWidth = 16;
+static constexpr uint8_t kAfMinGridHeight = 16;
+static constexpr uint8_t kAfMaxGridWidth = 32;
+static constexpr uint8_t kAfMaxGridHeight = 24;
+static constexpr uint16_t kAfMinGridBlockWidth = 4;
+static constexpr uint16_t kAfMinGridBlockHeight = 3;
+static constexpr uint16_t kAfMaxGridBlockWidth = 6;
+static constexpr uint16_t kAfMaxGridBlockHeight = 6;
+static constexpr uint16_t kAfDefaultHeightPerSlice = 2;
+
+namespace libcamera {
+
+namespace ipa::ipu3::algorithms {
+
+class Af : public Algorithm
+{
+	/* The format of y_table. From ipu3-ipa repo */
+	typedef struct __attribute__((packed)) y_table_item {
+		uint16_t y1_avg;
+		uint16_t y2_avg;
+	} y_table_item_t;
+public:
+	Af();
+	~Af() = default;
+
+	void prepare(IPAContext &context, ipu3_uapi_params *params) override;
+	int configure(IPAContext &context, const IPAConfigInfo &configInfo) override;
+	void process(IPAContext &context, const ipu3_uapi_stats_3a *stats) override;
+
+private:
+	void afCoarseScan(IPAContext &context);
+	void afFineScan(IPAContext &context);
+	bool afScan(IPAContext &context, int min_step);
+	void afReset(IPAContext &context);
+	bool afNeedIgnoreFrame();
+	void afIgnoreFrameReset();
+	double afEstimateVariance(y_table_item_t *y_item, uint32_t len,
+				  bool isY1);
+	bool afIsOutOfFocus(IPAContext context);
+
+	/* VCM step configuration. It is the current setting of the VCM step. */
+	uint32_t focus_;
+	/* The best VCM step. It is a local optimum VCM step during scanning. */
+	uint32_t bestFocus_;
+	/* Current AF statistic variance. */
+	double currentVariance_;
+	/* The frames are ignore before starting measuring. */
+	uint32_t ignoreCounter_;
+	/* It is used to determine the derivative during scanning */
+	double previousVariance_;
+	/* The designated maximum range of focus scanning. */
+	uint32_t maxStep_;
+	/* If the coarse scan completes, it is set to true. */
+	bool coarseCompleted_;
+	/* If the fine scan completes, it is set to true. */
+	bool fineCompleted_;
+};
+
+} /* namespace ipa::ipu3::algorithms */
+
+} /* namespace libcamera */
diff --git a/src/ipa/ipu3/algorithms/meson.build b/src/ipa/ipu3/algorithms/meson.build
index 4db6ae1d..b70a551c 100644
--- a/src/ipa/ipu3/algorithms/meson.build
+++ b/src/ipa/ipu3/algorithms/meson.build
@@ -1,6 +1,7 @@
 # SPDX-License-Identifier: CC0-1.0
 
 ipu3_ipa_algorithms = files([
+    'af.cpp',
     'agc.cpp',
     'awb.cpp',
     'blc.cpp',
diff --git a/src/ipa/ipu3/ipa_context.cpp b/src/ipa/ipu3/ipa_context.cpp
index 9c4ec936..1b59c918 100644
--- a/src/ipa/ipu3/ipa_context.cpp
+++ b/src/ipa/ipu3/ipa_context.cpp
@@ -70,6 +70,29 @@ namespace libcamera::ipa::ipu3 {
  */
 
 /**
+ * \var IPASessionConfiguration::af
+ * \brief AF grid configuration of the IPA
+ *
+ * \var IPASessionConfiguration::af.afGrid
+ * \brief AF scene grid configuration.
+ */
+
+/**
+ * \var IPAFrameContext::af
+ * \brief Context for the Automatic Focus algorithm
+ *
+ * \struct  IPAFrameContext::af
+ * \var IPAFrameContext::af.focus
+ * \brief Current position of the lens
+ *
+ * \var IPAFrameContext::af.maxVariance
+ * \brief The maximum variance of the current image.
+ *
+ * \var IPAFrameContext::af.stable
+ * \brief It is set to true, if the best focus is found.
+ */
+
+/**
  * \var IPASessionConfiguration::agc
  * \brief AGC parameters configuration of the IPA
  *
diff --git a/src/ipa/ipu3/ipa_context.h b/src/ipa/ipu3/ipa_context.h
index e7c49828..6b7a512e 100644
--- a/src/ipa/ipu3/ipa_context.h
+++ b/src/ipa/ipu3/ipa_context.h
@@ -26,6 +26,10 @@ struct IPASessionConfiguration {
 	} grid;
 
 	struct {
+		ipu3_uapi_grid_config afGrid;
+	} af;
+
+	struct {
 		utils::Duration minShutterSpeed;
 		utils::Duration maxShutterSpeed;
 		double minAnalogueGain;
@@ -39,6 +43,12 @@ struct IPASessionConfiguration {
 
 struct IPAFrameContext {
 	struct {
+		uint32_t focus;
+		double maxVariance;
+		bool stable;
+	} af;
+
+	struct {
 		uint32_t exposure;
 		double gain;
 	} agc;
diff --git a/src/ipa/ipu3/ipu3.cpp b/src/ipa/ipu3/ipu3.cpp
index 17324616..1c4ff2d7 100644
--- a/src/ipa/ipu3/ipu3.cpp
+++ b/src/ipa/ipu3/ipu3.cpp
@@ -30,6 +30,7 @@
 
 #include "libcamera/internal/mapped_framebuffer.h"
 
+#include "algorithms/af.h"
 #include "algorithms/agc.h"
 #include "algorithms/algorithm.h"
 #include "algorithms/awb.h"
@@ -297,6 +298,7 @@ int IPAIPU3::init(const IPASettings &settings,
 	context_.configuration.sensor.lineDuration = sensorInfo.lineLength * 1.0s / sensorInfo.pixelRate;
 
 	/* Construct our Algorithms */
+	algorithms_.push_back(std::make_unique<algorithms::Af>());
 	algorithms_.push_back(std::make_unique<algorithms::Agc>());
 	algorithms_.push_back(std::make_unique<algorithms::Awb>());
 	algorithms_.push_back(std::make_unique<algorithms::BlackLevelCorrection>());