1 files changed, 410 insertions, 0 deletions
diff --git a/src/ipa/mali-c55/algorithms/agc.cpp b/src/ipa/mali-c55/algorithms/agc.cpp
new file mode 100644
index 00000000..70667db3
--- /dev/null
+++ b/src/ipa/mali-c55/algorithms/agc.cpp
@@ -0,0 +1,410 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2024, Ideas On Board Oy
+ *
+ * agc.cpp - AGC/AEC mean-based control algorithm
+ */
+
+#include "agc.h"
+
+#include <cmath>
+
+#include <libcamera/base/log.h>
+#include <libcamera/base/utils.h>
+
+#include <libcamera/control_ids.h>
+#include <libcamera/property_ids.h>
+
+#include "libipa/colours.h"
+#include "libipa/fixedpoint.h"
+
+namespace libcamera {
+
+using namespace std::literals::chrono_literals;
+
+namespace ipa::mali_c55::algorithms {
+
+LOG_DEFINE_CATEGORY(MaliC55Agc)
+
+/*
+ * Number of histogram bins. This is only true for the specific configuration we
+ * set to the ISP; 4 separate histograms of 256 bins each. If that configuration
+ * ever changes then this constant will need updating.
+ */
+static constexpr unsigned int kNumHistogramBins = 256;
+
+/*
+ * The Mali-C55 ISP has a digital gain block which allows setting gain in Q5.8
+ * format, a range of 0.0 to (very nearly) 32.0. We clamp from 1.0 to the actual
+ * max value which is 8191 * 2^-8.
+ */
+static constexpr double kMinDigitalGain = 1.0;
+static constexpr double kMaxDigitalGain = 31.99609375;
+
+uint32_t AgcStatistics::decodeBinValue(uint16_t binVal)
+{
+	int exponent = (binVal & 0xf000) >> 12;
+	int mantissa = binVal & 0xfff;
+
+	if (!exponent)
+		return mantissa * 2;
+	else
+		return (mantissa + 4096) * std::pow(2, exponent);
+}
+
+/*
+ * We configure the ISP to give us 4 histograms of 256 bins each, with
+ * a single histogram per colour channel (R/Gr/Gb/B). The memory space
+ * containing the data is a single block containing all 4 histograms
+ * with the position of each colour's histogram within it dependent on
+ * the bayer pattern of the data input to the ISP.
+ *
+ * NOTE: The validity of this function depends on the parameters we have
+ * configured. With different skip/offset x, y values not all of the
+ * colour channels would be populated, and they may not be in the same
+ * planes as calculated here.
+ */
+int AgcStatistics::setBayerOrderIndices(BayerFormat::Order bayerOrder)
+{
+	switch (bayerOrder) {
+	case BayerFormat::Order::RGGB:
+		rIndex_ = 0;
+		grIndex_ = 1;
+		gbIndex_ = 2;
+		bIndex_ = 3;
+		break;
+	case BayerFormat::Order::GRBG:
+		grIndex_ = 0;
+		rIndex_ = 1;
+		bIndex_ = 2;
+		gbIndex_ = 3;
+		break;
+	case BayerFormat::Order::GBRG:
+		gbIndex_ = 0;
+		bIndex_ = 1;
+		rIndex_ = 2;
+		grIndex_ = 3;
+		break;
+	case BayerFormat::Order::BGGR:
+		bIndex_ = 0;
+		gbIndex_ = 1;
+		grIndex_ = 2;
+		rIndex_ = 3;
+		break;
+	default:
+		LOG(MaliC55Agc, Error)
+			<< "Invalid bayer format " << bayerOrder;
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void AgcStatistics::parseStatistics(const mali_c55_stats_buffer *stats)
+{
+	uint32_t r[256], g[256], b[256], y[256];
+
+	/*
+	 * We need to decode the bin values for each histogram from their 16-bit
+	 * compressed values to a 32-bit value. We also take the average of the
+	 * Gr/Gb values into a single green histogram.
+	 */
+	for (unsigned int i = 0; i < 256; i++) {
+		r[i] = decodeBinValue(stats->ae_1024bin_hist.bins[i + (256 * rIndex_)]);
+		g[i] = (decodeBinValue(stats->ae_1024bin_hist.bins[i + (256 * grIndex_)]) +
+			decodeBinValue(stats->ae_1024bin_hist.bins[i + (256 * gbIndex_)])) / 2;
+		b[i] = decodeBinValue(stats->ae_1024bin_hist.bins[i + (256 * bIndex_)]);
+
+		y[i] = rec601LuminanceFromRGB({ { static_cast<double>(r[i]),
+						  static_cast<double>(g[i]),
+						  static_cast<double>(b[i]) } });
+	}
+
+	rHist = Histogram(Span<uint32_t>(r, kNumHistogramBins));
+	gHist = Histogram(Span<uint32_t>(g, kNumHistogramBins));
+	bHist = Histogram(Span<uint32_t>(b, kNumHistogramBins));
+	yHist = Histogram(Span<uint32_t>(y, kNumHistogramBins));
+}
+
+Agc::Agc()
+	: AgcMeanLuminance()
+{
+}
+
+int Agc::init(IPAContext &context, const YamlObject &tuningData)
+{
+	int ret = parseTuningData(tuningData);
+	if (ret)
+		return ret;
+
+	context.ctrlMap[&controls::AeEnable] = ControlInfo(false, true);
+	context.ctrlMap[&controls::DigitalGain] = ControlInfo(
+		static_cast<float>(kMinDigitalGain),
+		static_cast<float>(kMaxDigitalGain),
+		static_cast<float>(kMinDigitalGain)
+	);
+	context.ctrlMap.merge(controls());
+
+	return 0;
+}
+
+int Agc::configure(IPAContext &context,
+		   [[maybe_unused]] const IPACameraSensorInfo &configInfo)
+{
+	int ret = statistics_.setBayerOrderIndices(context.configuration.sensor.bayerOrder);
+	if (ret)
+		return ret;
+
+	/*
+	 * Defaults; we use whatever the sensor's default exposure is and the
+	 * minimum analogue gain. AEGC is _active_ by default.
+	 */
+	context.activeState.agc.autoEnabled = true;
+	context.activeState.agc.automatic.sensorGain = context.configuration.agc.minAnalogueGain;
+	context.activeState.agc.automatic.exposure = context.configuration.agc.defaultExposure;
+	context.activeState.agc.automatic.ispGain = kMinDigitalGain;
+	context.activeState.agc.manual.sensorGain = context.configuration.agc.minAnalogueGain;
+	context.activeState.agc.manual.exposure = context.configuration.agc.defaultExposure;
+	context.activeState.agc.manual.ispGain = kMinDigitalGain;
+	context.activeState.agc.constraintMode = constraintModes().begin()->first;
+	context.activeState.agc.exposureMode = exposureModeHelpers().begin()->first;
+
+	/* \todo Run this again when FrameDurationLimits is passed in */
+	setLimits(context.configuration.agc.minShutterSpeed,
+		  context.configuration.agc.maxShutterSpeed,
+		  context.configuration.agc.minAnalogueGain,
+		  context.configuration.agc.maxAnalogueGain);
+
+	resetFrameCount();
+
+	return 0;
+}
+
+void Agc::queueRequest(IPAContext &context, const uint32_t frame,
+		       [[maybe_unused]] IPAFrameContext &frameContext,
+		       const ControlList &controls)
+{
+	auto &agc = context.activeState.agc;
+
+	const auto &constraintMode = controls.get(controls::AeConstraintMode);
+	agc.constraintMode = constraintMode.value_or(agc.constraintMode);
+
+	const auto &exposureMode = controls.get(controls::AeExposureMode);
+	agc.exposureMode = exposureMode.value_or(agc.exposureMode);
+
+	const auto &agcEnable = controls.get(controls::AeEnable);
+	if (agcEnable && *agcEnable != agc.autoEnabled) {
+		agc.autoEnabled = *agcEnable;
+
+		LOG(MaliC55Agc, Info)
+			<< (agc.autoEnabled ? "Enabling" : "Disabling")
+			<< " AGC";
+	}
+
+	/*
+	 * If the automatic exposure and gain is enabled we have no further work
+	 * to do here...
+	 */
+	if (agc.autoEnabled)
+		return;
+
+	/*
+	 * ...otherwise we need to look for exposure and gain controls and use
+	 * those to set the activeState.
+	 */
+	const auto &exposure = controls.get(controls::ExposureTime);
+	if (exposure) {
+		agc.manual.exposure = *exposure * 1.0us / context.configuration.sensor.lineDuration;
+
+		LOG(MaliC55Agc, Debug)
+			<< "Exposure set to " << agc.manual.exposure
+			<< " on request sequence " << frame;
+	}
+
+	const auto &analogueGain = controls.get(controls::AnalogueGain);
+	if (analogueGain) {
+		agc.manual.sensorGain = *analogueGain;
+
+		LOG(MaliC55Agc, Debug)
+			<< "Analogue gain set to " << agc.manual.sensorGain
+			<< " on request sequence " << frame;
+	}
+
+	const auto &digitalGain = controls.get(controls::DigitalGain);
+	if (digitalGain) {
+		agc.manual.ispGain = *digitalGain;
+
+		LOG(MaliC55Agc, Debug)
+			<< "Digital gain set to " << agc.manual.ispGain
+			<< " on request sequence " << frame;
+	}
+}
+
+size_t Agc::fillGainParamBlock(IPAContext &context, IPAFrameContext &frameContext,
+			       mali_c55_params_block block)
+{
+	IPAActiveState &activeState = context.activeState;
+	double gain;
+
+	if (activeState.agc.autoEnabled)
+		gain = activeState.agc.automatic.ispGain;
+	else
+		gain = activeState.agc.manual.ispGain;
+
+	block.header->type = MALI_C55_PARAM_BLOCK_DIGITAL_GAIN;
+	block.header->flags = MALI_C55_PARAM_BLOCK_FL_NONE;
+	block.header->size = sizeof(struct mali_c55_params_digital_gain);
+
+	block.digital_gain->gain = floatingToFixedPoint<5, 8, uint16_t, double>(gain);
+	frameContext.agc.ispGain = gain;
+
+	return block.header->size;
+}
+
+size_t Agc::fillParamsBuffer(mali_c55_params_block block,
+			     enum mali_c55_param_block_type type)
+{
+	block.header->type = type;
+	block.header->flags = MALI_C55_PARAM_BLOCK_FL_NONE;
+	block.header->size = sizeof(struct mali_c55_params_aexp_hist);
+
+	/* Collect every 3rd pixel horizontally */
+	block.aexp_hist->skip_x = 1;
+	/* Start from first column */
+	block.aexp_hist->offset_x = 0;
+	/* Collect every pixel vertically */
+	block.aexp_hist->skip_y = 0;
+	/* Start from the first row */
+	block.aexp_hist->offset_y = 0;
+	/* 1x scaling (i.e. none) */
+	block.aexp_hist->scale_bottom = 0;
+	block.aexp_hist->scale_top = 0;
+	/* Collect all Bayer planes into 4 separate histograms */
+	block.aexp_hist->plane_mode = 1;
+	/* Tap the data immediately after the digital gain block */
+	block.aexp_hist->tap_point = MALI_C55_AEXP_HIST_TAP_FS;
+
+	return block.header->size;
+}
+
+size_t Agc::fillWeightsArrayBuffer(mali_c55_params_block block,
+				   enum mali_c55_param_block_type type)
+{
+	block.header->type = type;
+	block.header->flags = MALI_C55_PARAM_BLOCK_FL_NONE;
+	block.header->size = sizeof(struct mali_c55_params_aexp_weights);
+
+	/* We use every zone - a 15x15 grid */
+	block.aexp_weights->nodes_used_horiz = 15;
+	block.aexp_weights->nodes_used_vert = 15;
+
+	/*
+	 * We uniformly weight the zones to 1 - this results in the collected
+	 * histograms containing a true pixel count, which we can then use to
+	 * approximate colour channel averages for the image.
+	 */
+	Span<uint8_t> weights{
+		block.aexp_weights->zone_weights,
+		MALI_C55_MAX_ZONES
+	};
+	std::fill(weights.begin(), weights.end(), 1);
+
+	return block.header->size;
+}
+
+void Agc::prepare(IPAContext &context, const uint32_t frame,
+		  IPAFrameContext &frameContext, mali_c55_params_buffer *params)
+{
+	mali_c55_params_block block;
+
+	block.data = &params->data[params->total_size];
+	params->total_size += fillGainParamBlock(context, frameContext, block);
+
+	if (frame > 0)
+		return;
+
+	block.data = &params->data[params->total_size];
+	params->total_size += fillParamsBuffer(block,
+					       MALI_C55_PARAM_BLOCK_AEXP_HIST);
+
+	block.data = &params->data[params->total_size];
+	params->total_size += fillWeightsArrayBuffer(block,
+						     MALI_C55_PARAM_BLOCK_AEXP_HIST_WEIGHTS);
+
+	block.data = &params->data[params->total_size];
+	params->total_size += fillParamsBuffer(block,
+					       MALI_C55_PARAM_BLOCK_AEXP_IHIST);
+
+	block.data = &params->data[params->total_size];
+	params->total_size += fillWeightsArrayBuffer(block,
+						     MALI_C55_PARAM_BLOCK_AEXP_IHIST_WEIGHTS);
+}
+
+double Agc::estimateLuminance(const double gain) const
+{
+	double rAvg = statistics_.rHist.interQuantileMean(0, 1) * gain;
+	double gAvg = statistics_.gHist.interQuantileMean(0, 1) * gain;
+	double bAvg = statistics_.bHist.interQuantileMean(0, 1) * gain;
+	double yAvg = rec601LuminanceFromRGB({ { rAvg, gAvg, bAvg } });
+
+	return yAvg / kNumHistogramBins;
+}
+
+void Agc::process(IPAContext &context,
+		  [[maybe_unused]] const uint32_t frame,
+		  IPAFrameContext &frameContext,
+		  const mali_c55_stats_buffer *stats,
+		  [[maybe_unused]] ControlList &metadata)
+{
+	IPASessionConfiguration &configuration = context.configuration;
+	IPAActiveState &activeState = context.activeState;
+
+	if (!stats) {
+		LOG(MaliC55Agc, Error) << "No statistics buffer passed to Agc";
+		return;
+	}
+
+	statistics_.parseStatistics(stats);
+	context.activeState.agc.temperatureK = estimateCCT({ { statistics_.rHist.interQuantileMean(0, 1),
+							       statistics_.gHist.interQuantileMean(0, 1),
+							       statistics_.bHist.interQuantileMean(0, 1) } });
+
+	/*
+	 * The Agc algorithm needs to know the effective exposure value that was
+	 * applied to the sensor when the statistics were collected.
+	 */
+	uint32_t exposure = frameContext.agc.exposure;
+	double analogueGain = frameContext.agc.sensorGain;
+	double digitalGain = frameContext.agc.ispGain;
+	double totalGain = analogueGain * digitalGain;
+	utils::Duration currentShutter = exposure * configuration.sensor.lineDuration;
+	utils::Duration effectiveExposureValue = currentShutter * totalGain;
+
+	utils::Duration shutterTime;
+	double aGain, dGain;
+	std::tie(shutterTime, aGain, dGain) =
+		calculateNewEv(activeState.agc.constraintMode,
+			       activeState.agc.exposureMode, statistics_.yHist,
+			       effectiveExposureValue);
+
+	dGain = std::clamp(dGain, kMinDigitalGain, kMaxDigitalGain);
+
+	LOG(MaliC55Agc, Debug)
+		<< "Divided up shutter, analogue gain and digital gain are "
+		<< shutterTime << ", " << aGain << " and " << dGain;
+
+	activeState.agc.automatic.exposure = shutterTime / configuration.sensor.lineDuration;
+	activeState.agc.automatic.sensorGain = aGain;
+	activeState.agc.automatic.ispGain = dGain;
+
+	metadata.set(controls::ExposureTime, currentShutter.get<std::micro>());
+	metadata.set(controls::AnalogueGain, frameContext.agc.sensorGain);
+	metadata.set(controls::DigitalGain, frameContext.agc.ispGain);
+	metadata.set(controls::ColourTemperature, context.activeState.agc.temperatureK);
+}
+
+REGISTER_IPA_ALGORITHM(Agc, "Agc")
+
+} /* namespace ipa::mali_c55::algorithms */
+
+} /* namespace libcamera */