fp6-img/aports/temp/libcamera/0008-libcamera-software_isp-Add-autofocus.patch

From 832c016ad79e1280d1a2e840cc47f434cba4e382 Mon Sep 17 00:00:00 2001
From: Vasiliy Doylov <nekocwd@mainlining.org>
Date: Wed, 9 Jul 2025 16:07:14 +0300
Subject: [PATCH] libcamera: software_isp: Add autofocus

Signed-off-by: Vasiliy Doylov <nekocwd@mainlining.org>
---
 .../internal/software_isp/swisp_stats.h       |  4 ++
 src/ipa/simple/algorithms/af.cpp              | 59 ++++++++++++++++++-
 src/ipa/simple/algorithms/af.h                |  1 +
 src/ipa/simple/ipa_context.h                  |  5 ++
 src/libcamera/software_isp/swstats_cpu.cpp    | 22 +++++--
 5 files changed, 86 insertions(+), 5 deletions(-)

diff --git a/include/libcamera/internal/software_isp/swisp_stats.h b/include/libcamera/internal/software_isp/swisp_stats.h
index d9d0d9be8..c2ec20e42 100644
--- a/include/libcamera/internal/software_isp/swisp_stats.h
+++ b/include/libcamera/internal/software_isp/swisp_stats.h
@@ -43,6 +43,10 @@ struct SwIspStats {
 	 * \brief A histogram of luminance values of all the sampled pixels
 	 */
 	Histogram yHistogram;
+	/**
+	 * \brief Holds the sharpness of an image
+	 */
+	uint64_t sharpness;
 };
 
 } /* namespace libcamera */
diff --git a/src/ipa/simple/algorithms/af.cpp b/src/ipa/simple/algorithms/af.cpp
index 6197f3271..3f0f98f93 100644
--- a/src/ipa/simple/algorithms/af.cpp
+++ b/src/ipa/simple/algorithms/af.cpp
@@ -27,14 +27,17 @@ int Af::init(IPAContext &context,
 	     [[maybe_unused]] const ValueNode &tuningData)
 {
 	context.ctrlMap[&controls::LensPosition] = ControlInfo(0.0f, 100.0f, 50.0f);
+	context.ctrlMap[&controls::AfTrigger] = ControlInfo(0, 1, 0);
 	return 0;
 }
 
 int Af::configure(IPAContext &context,
 		  [[maybe_unused]] const IPAConfigInfo &configInfo)
 {
+	context.activeState.knobs.focus_sweep = std::optional<bool>();
 	context.activeState.knobs.focus_pos = std::optional<double>();
-
+	context.activeState.knobs.focus_sweep = false;
+	context.activeState.knobs.focus_pos = 0;
 	return 0;
 }
 
@@ -44,10 +47,24 @@ void Af::queueRequest([[maybe_unused]] typename Module::Context &context,
 		      const ControlList &controls)
 {
 	const auto &focus_pos = controls.get(controls::LensPosition);
+	const auto &af_trigger = controls.get(controls::AfTrigger);
 	if (focus_pos.has_value()) {
 		context.activeState.knobs.focus_pos = focus_pos;
 		LOG(IPASoftAutoFocus, Debug) << "Setting focus position to " << focus_pos.value();
 	}
+	if (af_trigger.has_value()) {
+		context.activeState.knobs.focus_sweep = af_trigger.value() == 1;
+		if(context.activeState.knobs.focus_sweep){
+			context.activeState.knobs.focus_pos = 0;
+			context.configuration.focus.focus_max_pos = 0;
+			context.configuration.focus.sharpness_max = 0;
+			context.configuration.focus.start = 0;
+			context.configuration.focus.stop = 100;
+			context.configuration.focus.step = 25;
+			LOG(IPASoftAutoFocus, Info) << "Starting focus sweep";
+		}
+	}
+
 }
 
 void Af::updateFocus([[maybe_unused]] IPAContext &context, [[maybe_unused]] IPAFrameContext &frameContext, [[maybe_unused]] double exposureMSV)
@@ -55,12 +72,52 @@ void Af::updateFocus([[maybe_unused]] IPAContext &context, [[maybe_unused]] IPAF
 	frameContext.lens.focus_pos = context.activeState.knobs.focus_pos.value_or(50.0) / 100.0 * (context.configuration.focus.focus_max - context.configuration.focus.focus_min);
 }
 
+void Af::step(double& start, double& stop, double& step, double& focus_pos, double& max_pos, uint64_t& max_sharp, uint64_t sharp, bool& sweep){
+	if(!sweep)
+		return;
+	if(focus_pos < start) {
+		focus_pos = start;
+		return;
+	}
+	if(sharp > max_sharp) {
+		max_sharp = sharp;
+		max_pos = focus_pos;
+	}
+	if(focus_pos >= stop) {
+		LOG(IPASoftAutoFocus, Info) << "Best focus on step " <<step << ": " << focus_pos;
+		start = std::clamp(max_pos - step, 0.0, 100.0);
+		stop = std::clamp(max_pos +  step, 0.0, 100.0);
+		focus_pos = start;
+		max_sharp = 0;
+		step /= 2;
+		if(step <= 0.2){
+			sweep = false;
+			LOG(IPASoftAutoFocus, Info) << "Sweep end. Best focus: " << max_pos;
+			focus_pos = max_pos;
+		}
+		return;
+	}
+
+	focus_pos += step;
+}
+
 void Af::process([[maybe_unused]] IPAContext &context,
 		 [[maybe_unused]] const uint32_t frame,
 		 [[maybe_unused]] IPAFrameContext &frameContext,
 		 [[maybe_unused]] const SwIspStats *stats,
 		 [[maybe_unused]] ControlList &metadata)
 {
+	if (stats->valid) {
+		step(context.configuration.focus.start,
+			context.configuration.focus.stop,
+			context.configuration.focus.step,
+			context.activeState.knobs.focus_pos.value(),
+			context.configuration.focus.focus_max_pos,
+			context.configuration.focus.sharpness_max,
+			stats->sharpness,
+			context.activeState.knobs.focus_sweep.value());
+	}
+
 	updateFocus(context, frameContext, 0);
 }
 
diff --git a/src/ipa/simple/algorithms/af.h b/src/ipa/simple/algorithms/af.h
index b138b4d63..4d0ea58b3 100644
--- a/src/ipa/simple/algorithms/af.h
+++ b/src/ipa/simple/algorithms/af.h
@@ -33,6 +33,7 @@ public:
 
 private:
 	void updateFocus(IPAContext &context, IPAFrameContext &frameContext, double focus);
+	void step(double& start, double& stop, double& step, double& focus_pos, double& max_pos, uint64_t& max_sharp, uint64_t sharp, bool& sweep);
 };
 
 } /* namespace ipa::soft::algorithms */
diff --git a/src/ipa/simple/ipa_context.h b/src/ipa/simple/ipa_context.h
index 2b6bd4b3a..f869d1a67 100644
--- a/src/ipa/simple/ipa_context.h
+++ b/src/ipa/simple/ipa_context.h
@@ -35,6 +35,9 @@ struct IPASessionConfiguration {
 	} black;
 	struct {
 		int32_t focus_min, focus_max;
+		double focus_max_pos;
+		uint64_t sharpness_max;
+		double start, stop, step;
 	} focus;
 };
 
@@ -65,6 +68,8 @@ struct IPAActiveState {
 		std::optional<float> saturation;
 		/* 0..100 range, 50.0 = normal */
 		std::optional<double> focus_pos;
+		/* 0..1 range, 0 = normal */
+		std::optional<bool> focus_sweep;
 	} knobs;
 };
 
diff --git a/src/libcamera/software_isp/swstats_cpu.cpp b/src/libcamera/software_isp/swstats_cpu.cpp
index 7fb77ce7d..f243992fb 100644
--- a/src/libcamera/software_isp/swstats_cpu.cpp
+++ b/src/libcamera/software_isp/swstats_cpu.cpp
@@ -177,7 +177,10 @@ static constexpr unsigned int kBlueYMul = 29; /* 0.114 * 256 */
                                           \
 	uint64_t sumR = 0;                \
 	uint64_t sumG = 0;                \
-	uint64_t sumB = 0;
+	uint64_t sumB = 0;                \
+	pixel_t r0 = 0, r1 = 0, b0 = 0,   \
+		b1 = 0, g0 = 0, g1 = 0;   \
+	uint64_t sharpness = 0;
 
 #define SWSTATS_ACCUMULATE_LINE_STATS(div) \
 	sumR += r;                         \
@@ -187,12 +190,20 @@ static constexpr unsigned int kBlueYMul = 29; /* 0.114 * 256 */
 	yVal = r * kRedYMul;               \
 	yVal += g * kGreenYMul;            \
 	yVal += b * kBlueYMul;             \
-	stats.yHistogram[yVal * SwIspStats::kYHistogramSize / (256 * 256 * (div))]++;
-
+	stats.yHistogram[yVal * SwIspStats::kYHistogramSize / (256 * 256 * (div))]++; \
+	if (r0 != 0) \
+		sharpness += abs(r - 2 * r1 + r0) * kRedYMul + abs(g - 2 * g1 + g0) * kGreenYMul + abs(b - 2 * b1 + b0) * kBlueYMul; \
+	r0 = r1; \
+	g0 = g1; \
+	b0 = b1; \
+	r1 = r; \
+	g1 = g; \
+	b1 = b;
 #define SWSTATS_FINISH_LINE_STATS() \
 	stats.sum_.r() += sumR;     \
 	stats.sum_.g() += sumG;     \
-	stats.sum_.b() += sumB;
+	stats.sum_.b() += sumB;     \
+	stats.sharpness += sharpness;
 
 void SwStatsCpu::statsBGGR8Line0(const uint8_t *src[], SwIspStats &stats)
 {
@@ -392,6 +403,7 @@ void SwStatsCpu::startFrame(uint32_t frame)
 	for (auto &s : stats_) {
 		s.sum_ = RGB<uint64_t>({ 0, 0, 0 });
 		s.yHistogram.fill(0);
+		s.sharpness = 0;
 	}
 }
 
@@ -408,8 +420,10 @@ void SwStatsCpu::finishFrame(uint32_t frame, uint32_t bufferId)
 
 	if (valid) {
 		sharedStats_->sum_ = RGB<uint64_t>({ 0, 0, 0 });
+		sharedStats_->sharpness = 0;
 		sharedStats_->yHistogram.fill(0);
 		for (const auto &s : stats_) {
+			sharedStats_->sharpness += s.sharpness;
 			sharedStats_->sum_ += s.sum_;
 			for (unsigned int j = 0; j < SwIspStats::kYHistogramSize; j++)
 				sharedStats_->yHistogram[j] += s.yHistogram[j];