ipa: rkisp1: Add "Windows" Metering mode to auto focus algorithm

Allow manually setting auto focus window. Currently only one window is
enabled, but ISP allows up to three of them.

Signed-off-by: Daniel Semkowicz <dse@thaumatec.com>

3 files changed, 98 insertions, 6 deletions

diff --git a/src/ipa/rkisp1/algorithms/af.cpp b/src/ipa/rkisp1/algorithms/af.cpp
index c2a321cd..65768fc4 100644
--- a/src/ipa/rkisp1/algorithms/af.cpp
+++ b/src/ipa/rkisp1/algorithms/af.cpp
@@ -20,14 +20,32 @@ namespace libcamera::ipa::rkisp1::algorithms {
 
 LOG_DEFINE_CATEGORY(RkISP1Af)
 
+namespace {
+
+constexpr rkisp1_cif_isp_window rectangleToIspWindow(const Rectangle &rectangle)
+{
+	return rkisp1_cif_isp_window{
+		.h_offs = static_cast<uint16_t>(rectangle.x),
+		.v_offs = static_cast<uint16_t>(rectangle.y),
+		.h_size = static_cast<uint16_t>(rectangle.width),
+		.v_size = static_cast<uint16_t>(rectangle.height)
+	};
+}
+
+} /* namespace */
+
 /**
  * \copydoc libcamera::ipa::Algorithm::init
  */
 int Af::init([[maybe_unused]] IPAContext &context, const YamlObject &tuningData)
 {
 	waitFramesLens_ = tuningData["wait-frames-lens"].get<uint32_t>(1);
+	ispThreshold_ = tuningData["isp-threshold"].get<uint32_t>(128);
+	ispVarShift_ = tuningData["isp-var-shift"].get<uint32_t>(4);
 
-	LOG(RkISP1Af, Debug) << "waitFramesLens_: " << waitFramesLens_;
+	LOG(RkISP1Af, Debug) << "waitFramesLens_: " << waitFramesLens_
+			     << ", ispThreshold_: " << ispThreshold_
+			     << ", ispVarShift_: " << ispVarShift_;
 
 	return initBase(tuningData);
 }
@@ -36,8 +54,15 @@ int Af::init([[maybe_unused]] IPAContext &context, const YamlObject &tuningData)
  * \copydoc libcamera::ipa::Algorithm::configure
  */
 int Af::configure([[maybe_unused]] IPAContext &context,
-		  [[maybe_unused]] const IPACameraSensorInfo &configInfo)
+		  const IPACameraSensorInfo &configInfo)
 {
+	/* Default AF window of 3/4 size of the screen placed at the center */
+	defaultWindow_ = Rectangle(configInfo.outputSize.width / 8,
+				   configInfo.outputSize.height / 8,
+				   3 * configInfo.outputSize.width / 4,
+				   3 * configInfo.outputSize.height / 4);
+	updateCurrentWindow(defaultWindow_);
+
 	return 0;
 }
 
@@ -50,6 +75,21 @@ void Af::queueRequest([[maybe_unused]] IPAContext &context,
 		      const ControlList &controls)
 {
 	queueRequestBase(frame, controls);
+
+	for (auto const &[id, value] : controls) {
+		switch (id) {
+		case controls::AF_METERING: {
+			setMeteringMode(static_cast<controls::AfMeteringEnum>(value.get<int32_t>()));
+			break;
+		}
+		case controls::AF_WINDOWS: {
+			setWindows(value.get<Span<const Rectangle>>());
+			break;
+		}
+		default:
+			break;
+		}
+	}
 }
 
 /**
@@ -58,8 +98,23 @@ void Af::queueRequest([[maybe_unused]] IPAContext &context,
 void Af::prepare([[maybe_unused]] IPAContext &context,
 		 [[maybe_unused]] const uint32_t frame,
 		 [[maybe_unused]] IPAFrameContext &frameContext,
-		 [[maybe_unused]] rkisp1_params_cfg *params)
+		 rkisp1_params_cfg *params)
 {
+	if (updateWindow_) {
+		params->meas.afc_config.num_afm_win = 1;
+		params->meas.afc_config.thres = ispThreshold_;
+		params->meas.afc_config.var_shift = ispVarShift_;
+		params->meas.afc_config.afm_win[0] = rectangleToIspWindow(*updateWindow_);
+
+		params->module_cfg_update |= RKISP1_CIF_ISP_MODULE_AFC;
+		params->module_ens |= RKISP1_CIF_ISP_MODULE_AFC;
+		params->module_en_update |= RKISP1_CIF_ISP_MODULE_AFC;
+
+		updateWindow_.reset();
+
+		/* Wait one frame for the ISP to apply changes */
+		setFramesToSkip(1);
+	}
 }
 
 /**
@@ -88,12 +143,38 @@ void Af::process(IPAContext &context, [[maybe_unused]] const uint32_t frame,
 
 void Af::setMeteringMode([[maybe_unused]] controls::AfMeteringEnum metering)
 {
-	LOG(RkISP1Af, Error) << __FUNCTION__ << " not implemented!";
+	if (metering == meteringMode_)
+		return;
+
+	if (metering == controls::AfMeteringWindows) {
+		updateCurrentWindow(userWindow_);
+	} else {
+		updateCurrentWindow(defaultWindow_);
+	}
+
+	meteringMode_ = metering;
+}
+
+void Af::setWindows(Span<const Rectangle> windows)
+{
+	if (windows.size() != 1) {
+		LOG(RkISP1Af, Error) << "Only one AF window is supported";
+		return;
+	}
+
+	/* \todo Check if window size is valid for ISP */
+
+	LOG(RkISP1Af, Debug) << "setWindows: " << userWindow_;
+
+	userWindow_ = windows[0];
+
+	if (meteringMode_ == controls::AfMeteringWindows)
+		updateCurrentWindow(userWindow_);
 }
 
-void Af::setWindows([[maybe_unused]] Span<const Rectangle> windows)
+void Af::updateCurrentWindow(const Rectangle &window)
 {
-	LOG(RkISP1Af, Error) << __FUNCTION__ << " not implemented!";
+	updateWindow_ = window;
 }
 
 REGISTER_IPA_ALGORITHM(Af, "Af")
diff --git a/src/ipa/rkisp1/algorithms/af.h b/src/ipa/rkisp1/algorithms/af.h
index 882be952..72cc7932 100644
--- a/src/ipa/rkisp1/algorithms/af.h
+++ b/src/ipa/rkisp1/algorithms/af.h
@@ -35,6 +35,15 @@ private:
 	void setMeteringMode(controls::AfMeteringEnum metering) final;
 	void setWindows(Span<const Rectangle> windows) final;
 
+	void updateCurrentWindow(const Rectangle &window);
+
+	controls::AfMeteringEnum meteringMode_ = controls::AfMeteringAuto;
+	Rectangle defaultWindow_;
+	Rectangle userWindow_;
+	std::optional<Rectangle> updateWindow_;
+	uint32_t ispThreshold_;
+	uint32_t ispVarShift_;
+
 	/* Wait number of frames after changing lens position */
 	uint32_t waitFramesLens_;
 };
diff --git a/src/ipa/rkisp1/rkisp1.cpp b/src/ipa/rkisp1/rkisp1.cpp
index 2ad711d5..32839360 100644
--- a/src/ipa/rkisp1/rkisp1.cpp
+++ b/src/ipa/rkisp1/rkisp1.cpp
@@ -101,9 +101,11 @@ namespace {
 /* List of controls handled by the RkISP1 IPA */
 const ControlInfoMap::Map rkisp1Controls{
 	{ &controls::AeEnable, ControlInfo(false, true) },
+	{ &controls::AfMetering, ControlInfo(controls::AfMeteringValues) },
 	{ &controls::AfMode, ControlInfo(controls::AfModeValues) },
 	{ &controls::AfPause, ControlInfo(controls::AfPauseValues) },
 	{ &controls::AfTrigger, ControlInfo(controls::AfTriggerValues) },
+	{ &controls::AfWindows, ControlInfo(Rectangle{}, Rectangle(65535, 65535, 65535, 65535), Rectangle{}) },
 	{ &controls::AwbEnable, ControlInfo(false, true) },
 	{ &controls::ColourGains, ControlInfo(0.0f, 3.996f, 1.0f) },
 	{ &controls::Brightness, ControlInfo(-1.0f, 0.993f, 0.0f) },