1 files changed, 266 insertions, 170 deletions

diff --git a/src/ipa/rkisp1/algorithms/lsc.cpp b/src/ipa/rkisp1/algorithms/lsc.cpp
index 161183fc..e47aa2f0 100644
--- a/src/ipa/rkisp1/algorithms/lsc.cpp
+++ b/src/ipa/rkisp1/algorithms/lsc.cpp
@@ -16,6 +16,7 @@
 
 #include "libcamera/internal/yaml_parser.h"
 
+#include "libipa/lsc_polynomial.h"
 #include "linux/rkisp1-config.h"
 
 /**
@@ -24,6 +25,36 @@
 
 namespace libcamera {
 
+namespace ipa {
+
+constexpr int kColourTemperatureChangeThreshhold = 10;
+
+template<typename T>
+void interpolateVector(const std::vector<T> &a, const std::vector<T> &b,
+		       std::vector<T> &dest, double lambda)
+{
+	assert(a.size() == b.size());
+	dest.resize(a.size());
+	for (size_t i = 0; i < a.size(); i++) {
+		dest[i] = a[i] * (1.0 - lambda) + b[i] * lambda;
+	}
+}
+
+template<>
+void Interpolator<rkisp1::algorithms::LensShadingCorrection::Components>::
+	interpolate(const rkisp1::algorithms::LensShadingCorrection::Components &a,
+		    const rkisp1::algorithms::LensShadingCorrection::Components &b,
+		    rkisp1::algorithms::LensShadingCorrection::Components &dest,
+		    double lambda)
+{
+	interpolateVector(a.r, b.r, dest.r, lambda);
+	interpolateVector(a.gr, b.gr, dest.gr, lambda);
+	interpolateVector(a.gb, b.gb, dest.gb, lambda);
+	interpolateVector(a.b, b.b, dest.b, lambda);
+}
+
+} /* namespace ipa */
+
 namespace ipa::rkisp1::algorithms {
 
 /**
@@ -40,6 +71,200 @@ namespace ipa::rkisp1::algorithms {
 
 LOG_DEFINE_CATEGORY(RkISP1Lsc)
 
+class LscPolynomialLoader
+{
+public:
+	LscPolynomialLoader(const Size &sensorSize,
+			    const Rectangle &cropRectangle,
+			    const std::vector<double> &xSizes,
+			    const std::vector<double> &ySizes)
+		: sensorSize_(sensorSize),
+		  cropRectangle_(cropRectangle),
+		  xSizes_(xSizes),
+		  ySizes_(ySizes)
+	{
+	}
+
+	int parseLscData(const YamlObject &yamlSets,
+			 std::map<unsigned int, LensShadingCorrection::Components> &lscData)
+	{
+		const auto &sets = yamlSets.asList();
+		for (const auto &yamlSet : sets) {
+			std::optional<LscPolynomial> pr, pgr, pgb, pb;
+			uint32_t ct = yamlSet["ct"].get<uint32_t>(0);
+
+			if (lscData.count(ct)) {
+				LOG(RkISP1Lsc, Error)
+					<< "Multiple sets found for "
+					<< "color temperature " << ct;
+				return -EINVAL;
+			}
+
+			LensShadingCorrection::Components &set = lscData[ct];
+			pr = yamlSet["r"].get<LscPolynomial>();
+			pgr = yamlSet["gr"].get<LscPolynomial>();
+			pgb = yamlSet["gb"].get<LscPolynomial>();
+			pb = yamlSet["b"].get<LscPolynomial>();
+
+			if (!(pr || pgr || pgb || pb)) {
+				LOG(RkISP1Lsc, Error)
+					<< "Failed to parse polynomial for "
+					<< "colour temperature " << ct;
+				return -EINVAL;
+			}
+
+			set.ct = ct;
+			pr->setReferenceImageSize(sensorSize_);
+			pgr->setReferenceImageSize(sensorSize_);
+			pgb->setReferenceImageSize(sensorSize_);
+			pb->setReferenceImageSize(sensorSize_);
+			set.r = samplePolynomial(*pr);
+			set.gr = samplePolynomial(*pgr);
+			set.gb = samplePolynomial(*pgb);
+			set.b = samplePolynomial(*pb);
+		}
+
+		if (lscData.empty()) {
+			LOG(RkISP1Lsc, Error) << "Failed to load any sets";
+			return -EINVAL;
+		}
+
+		return 0;
+	}
+
+private:
+	/*
+	 * The lsc grid has custom spacing defined on half the range (see
+	 * parseSizes() for details). For easier handling this function converts
+	 * the spaces vector to positions and mirrors them. E.g.:
+	 *
+	 * input:   | 0.2 | 0.3 |
+	 * output: 0.0   0.2   0.5   0.8   1.0
+	 */
+	std::vector<double> sizesListToPositions(const std::vector<double> &sizes)
+	{
+		const int half = sizes.size();
+		std::vector<double> res(half * 2 + 1);
+		double x = 0.0;
+
+		res[half] = 0.5;
+		for (int i = 1; i <= half; i++) {
+			x += sizes[half - i];
+			res[half - i] = 0.5 - x;
+			res[half + i] = 0.5 + x;
+		}
+
+		return res;
+	}
+
+	std::vector<uint16_t> samplePolynomial(const LscPolynomial &poly)
+	{
+		constexpr int k = RKISP1_CIF_ISP_LSC_SAMPLES_MAX;
+
+		double m = poly.getM();
+		double x0 = cropRectangle_.x / m;
+		double y0 = cropRectangle_.y / m;
+		double w = cropRectangle_.width / m;
+		double h = cropRectangle_.height / m;
+		std::vector<uint16_t> res;
+
+		assert(xSizes_.size() * 2 + 1 == k);
+		assert(ySizes_.size() * 2 + 1 == k);
+
+		res.reserve(k * k);
+
+		std::vector<double> xPos(sizesListToPositions(xSizes_));
+		std::vector<double> yPos(sizesListToPositions(ySizes_));
+
+		for (int y = 0; y < k; y++) {
+			for (int x = 0; x < k; x++) {
+				double xp = x0 + xPos[x] * w;
+				double yp = y0 + yPos[y] * h;
+				/*
+				 * The hardware uses 2.10 fixed point format and
+				 * limits the legal values to [1..3.999]. Scale
+				 * and clamp the sampled value accordingly.
+				 */
+				int v = static_cast<int>(
+					poly.sampleAtNormalizedPixelPos(xp, yp) *
+					1024);
+				v = std::min(std::max(v, 1024), 4095);
+				res.push_back(v);
+			}
+		}
+		return res;
+	}
+
+	Size sensorSize_;
+	Rectangle cropRectangle_;
+	const std::vector<double> &xSizes_;
+	const std::vector<double> &ySizes_;
+};
+
+class LscTableLoader
+{
+public:
+	int parseLscData(const YamlObject &yamlSets,
+			 std::map<unsigned int, LensShadingCorrection::Components> &lscData)
+	{
+		const auto &sets = yamlSets.asList();
+
+		for (const auto &yamlSet : sets) {
+			uint32_t ct = yamlSet["ct"].get<uint32_t>(0);
+
+			if (lscData.count(ct)) {
+				LOG(RkISP1Lsc, Error)
+					<< "Multiple sets found for color temperature "
+					<< ct;
+				return -EINVAL;
+			}
+
+			LensShadingCorrection::Components &set = lscData[ct];
+
+			set.ct = ct;
+			set.r = parseTable(yamlSet, "r");
+			set.gr = parseTable(yamlSet, "gr");
+			set.gb = parseTable(yamlSet, "gb");
+			set.b = parseTable(yamlSet, "b");
+
+			if (set.r.empty() || set.gr.empty() ||
+			    set.gb.empty() || set.b.empty()) {
+				LOG(RkISP1Lsc, Error)
+					<< "Set for color temperature " << ct
+					<< " is missing tables";
+				return -EINVAL;
+			}
+		}
+
+		if (lscData.empty()) {
+			LOG(RkISP1Lsc, Error) << "Failed to load any sets";
+			return -EINVAL;
+		}
+
+		return 0;
+	}
+
+private:
+	std::vector<uint16_t> parseTable(const YamlObject &tuningData,
+					 const char *prop)
+	{
+		static constexpr unsigned int kLscNumSamples =
+			RKISP1_CIF_ISP_LSC_SAMPLES_MAX * RKISP1_CIF_ISP_LSC_SAMPLES_MAX;
+
+		std::vector<uint16_t> table =
+			tuningData[prop].getList<uint16_t>().value_or(std::vector<uint16_t>{});
+		if (table.size() != kLscNumSamples) {
+			LOG(RkISP1Lsc, Error)
+				<< "Invalid '" << prop << "' values: expected "
+				<< kLscNumSamples
+				<< " elements, got " << table.size();
+			return {};
+		}
+
+		return table;
+	}
+};
+
 static std::vector<double> parseSizes(const YamlObject &tuningData,
 				      const char *prop)
 {
@@ -70,28 +295,10 @@ static std::vector<double> parseSizes(const YamlObject &tuningData,
 	return sizes;
 }
 
-static std::vector<uint16_t> parseTable(const YamlObject &tuningData,
-					const char *prop)
-{
-	static constexpr unsigned int kLscNumSamples =
-		RKISP1_CIF_ISP_LSC_SAMPLES_MAX * RKISP1_CIF_ISP_LSC_SAMPLES_MAX;
-
-	std::vector<uint16_t> table =
-		tuningData[prop].getList<uint16_t>().value_or(std::vector<uint16_t>{});
-	if (table.size() != kLscNumSamples) {
-		LOG(RkISP1Lsc, Error)
-			<< "Invalid '" << prop << "' values: expected "
-			<< kLscNumSamples
-			<< " elements, got " << table.size();
-		return {};
-	}
-
-	return table;
-}
-
 LensShadingCorrection::LensShadingCorrection()
-	: lastCt_({ 0, 0 })
+	: lastAppliedCt_(0), lastAppliedQuantizedCt_(0)
 {
+	sets_.setQuantization(kColourTemperatureChangeThreshhold);
 }
 
 /**
@@ -114,38 +321,30 @@ int LensShadingCorrection::init([[maybe_unused]] IPAContext &context,
 		return -EINVAL;
 	}
 
-	const auto &sets = yamlSets.asList();
-	for (const auto &yamlSet : sets) {
-		uint32_t ct = yamlSet["ct"].get<uint32_t>(0);
-
-		if (sets_.count(ct)) {
-			LOG(RkISP1Lsc, Error)
-				<< "Multiple sets found for color temperature "
-				<< ct;
-			return -EINVAL;
-		}
-
-		Components &set = sets_[ct];
-
-		set.ct = ct;
-		set.r = parseTable(yamlSet, "r");
-		set.gr = parseTable(yamlSet, "gr");
-		set.gb = parseTable(yamlSet, "gb");
-		set.b = parseTable(yamlSet, "b");
-
-		if (set.r.empty() || set.gr.empty() ||
-		    set.gb.empty() || set.b.empty()) {
-			LOG(RkISP1Lsc, Error)
-				<< "Set for color temperature " << ct
-				<< " is missing tables";
-			return -EINVAL;
-		}
+	std::map<unsigned int, Components> lscData;
+	int res = 0;
+	std::string type = tuningData["type"].get<std::string>("table");
+	if (type == "table") {
+		LOG(RkISP1Lsc, Debug) << "Loading tabular LSC data.";
+		auto loader = LscTableLoader();
+		res = loader.parseLscData(yamlSets, lscData);
+	} else if (type == "polynomial") {
+		LOG(RkISP1Lsc, Debug) << "Loading polynomial LSC data.";
+		auto loader = LscPolynomialLoader(context.sensorInfo.activeAreaSize,
+						  context.sensorInfo.analogCrop,
+						  xSize_,
+						  ySize_);
+		res = loader.parseLscData(yamlSets, lscData);
+	} else {
+		LOG(RkISP1Lsc, Error) << "Unsupported LSC data type '"
+				      << type << "'";
+		res = -EINVAL;
 	}
 
-	if (sets_.empty()) {
-		LOG(RkISP1Lsc, Error) << "Failed to load any sets";
-		return -EINVAL;
-	}
+	if (res)
+		return res;
+
+	sets_.setData(std::move(lscData));
 
 	return 0;
 }
@@ -185,18 +384,12 @@ int LensShadingCorrection::configure(IPAContext &context,
 	return 0;
 }
 
-void LensShadingCorrection::setParameters(rkisp1_params_cfg *params)
+void LensShadingCorrection::setParameters(rkisp1_cif_isp_lsc_config &config)
 {
-	struct rkisp1_cif_isp_lsc_config &config = params->others.lsc_config;
-
 	memcpy(config.x_grad_tbl, xGrad_, sizeof(config.x_grad_tbl));
 	memcpy(config.y_grad_tbl, yGrad_, sizeof(config.y_grad_tbl));
 	memcpy(config.x_size_tbl, xSizes_, sizeof(config.x_size_tbl));
 	memcpy(config.y_size_tbl, ySizes_, sizeof(config.y_size_tbl));
-
-	params->module_en_update |= RKISP1_CIF_ISP_MODULE_LSC;
-	params->module_ens |= RKISP1_CIF_ISP_MODULE_LSC;
-	params->module_cfg_update |= RKISP1_CIF_ISP_MODULE_LSC;
 }
 
 void LensShadingCorrection::copyTable(rkisp1_cif_isp_lsc_config &config,
@@ -208,131 +401,34 @@ void LensShadingCorrection::copyTable(rkisp1_cif_isp_lsc_config &config,
 	std::copy(set.b.begin(), set.b.end(), &config.b_data_tbl[0][0]);
 }
 
-/*
- * Interpolate LSC parameters based on color temperature value.
- */
-void LensShadingCorrection::interpolateTable(rkisp1_cif_isp_lsc_config &config,
-					     const Components &set0,
-					     const Components &set1,
-					     const uint32_t ct)
-{
-	double coeff0 = (set1.ct - ct) / static_cast<double>(set1.ct - set0.ct);
-	double coeff1 = (ct - set0.ct) / static_cast<double>(set1.ct - set0.ct);
-
-	for (unsigned int i = 0; i < RKISP1_CIF_ISP_LSC_SAMPLES_MAX; ++i) {
-		for (unsigned int j = 0; j < RKISP1_CIF_ISP_LSC_SAMPLES_MAX; ++j) {
-			unsigned int sample = i * RKISP1_CIF_ISP_LSC_SAMPLES_MAX + j;
-
-			config.r_data_tbl[i][j] =
-				set0.r[sample] * coeff0 +
-				set1.r[sample] * coeff1;
-
-			config.gr_data_tbl[i][j] =
-				set0.gr[sample] * coeff0 +
-				set1.gr[sample] * coeff1;
-
-			config.gb_data_tbl[i][j] =
-				set0.gb[sample] * coeff0 +
-				set1.gb[sample] * coeff1;
-
-			config.b_data_tbl[i][j] =
-				set0.b[sample] * coeff0 +
-				set1.b[sample] * coeff1;
-		}
-	}
-}
-
 /**
  * \copydoc libcamera::ipa::Algorithm::prepare
  */
 void LensShadingCorrection::prepare(IPAContext &context,
-				    const uint32_t frame,
+				    [[maybe_unused]] const uint32_t frame,
 				    [[maybe_unused]] IPAFrameContext &frameContext,
-				    rkisp1_params_cfg *params)
+				    RkISP1Params *params)
 {
-	struct rkisp1_cif_isp_lsc_config &config = params->others.lsc_config;
-
-	/*
-	 * If there is only one set, the configuration has already been done
-	 * for first frame.
-	 */
-	if (sets_.size() == 1 && frame > 0)
-		return;
-
-	/*
-	 * If there is only one set, pick it. We can ignore lastCt_, as it will
-	 * never be relevant.
-	 */
-	if (sets_.size() == 1) {
-		setParameters(params);
-		copyTable(config, sets_.cbegin()->second);
-		return;
-	}
-
 	uint32_t ct = context.activeState.awb.temperatureK;
-	ct = std::clamp(ct, sets_.cbegin()->first, sets_.crbegin()->first);
-
-	/*
-	 * If the original is the same, then it means the same adjustment would
-	 * be made. If the adjusted is the same, then it means that it's the
-	 * same as what was actually applied. Thus in these cases we can skip
-	 * reprogramming the LSC.
-	 *
-	 * original == adjusted can only happen if an interpolation
-	 * happened, or if original has an exact entry in sets_. This means
-	 * that if original != adjusted, then original was adjusted to
-	 * the nearest available entry in sets_, resulting in adjusted.
-	 * Clearly, any ct value that is in between original and adjusted
-	 * will be adjusted to the same adjusted value, so we can skip
-	 * reprogramming the LSC table.
-	 *
-	 * We also skip updating the original value, as the last one had a
-	 * larger bound and thus a larger range of ct values that will be
-	 * adjusted to the same adjusted.
-	 */
-	if ((lastCt_.original <= ct && ct <= lastCt_.adjusted) ||
-	    (lastCt_.adjusted <= ct && ct <= lastCt_.original))
+	if (std::abs(static_cast<int>(ct) - static_cast<int>(lastAppliedCt_)) <
+	    kColourTemperatureChangeThreshhold)
 		return;
-
-	setParameters(params);
-
-	/*
-	 * The color temperature matches exactly one of the available LSC tables.
-	 */
-	if (sets_.count(ct)) {
-		copyTable(config, sets_[ct]);
-		lastCt_ = { ct, ct };
+	unsigned int quantizedCt;
+	const Components &set = sets_.getInterpolated(ct, &quantizedCt);
+	if (lastAppliedQuantizedCt_ == quantizedCt)
 		return;
-	}
 
-	/* No shortcuts left; we need to round or interpolate */
-	auto iter = sets_.upper_bound(ct);
-	const Components &set1 = iter->second;
-	const Components &set0 = (--iter)->second;
-	uint32_t ct0 = set0.ct;
-	uint32_t ct1 = set1.ct;
-	uint32_t diff0 = ct - ct0;
-	uint32_t diff1 = ct1 - ct;
-	static constexpr double kThreshold = 0.1;
-	float threshold = kThreshold * (ct1 - ct0);
-
-	if (diff0 < threshold || diff1 < threshold) {
-		const Components &set = diff0 < diff1 ? set0 : set1;
-		LOG(RkISP1Lsc, Debug) << "using LSC table for " << set.ct;
-		copyTable(config, set);
-		lastCt_ = { ct, set.ct };
-		return;
-	}
+	auto config = params->block<BlockType::Lsc>();
+	config.setEnabled(true);
+	setParameters(*config);
+	copyTable(*config, set);
+
+	lastAppliedCt_ = ct;
+	lastAppliedQuantizedCt_ = quantizedCt;
 
-	/*
-	 * ct is not within 10% of the difference between the neighbouring
-	 * color temperatures, so we need to interpolate.
-	 */
 	LOG(RkISP1Lsc, Debug)
-		<< "ct is " << ct << ", interpolating between "
-		<< ct0 << " and " << ct1;
-	interpolateTable(config, set0, set1, ct);
-	lastCt_ = { ct, ct };
+		<< "ct is " << ct << ", quantized to "
+		<< quantizedCt;
 }
 
 REGISTER_IPA_ALGORITHM(LensShadingCorrection, "LensShadingCorrection")