path: root/src/qcam/dng_writer.cpp

/* SPDX-License-Identifier: LGPL-2.1-or-later */
/*
 * Copyright (C) 2020, Raspberry Pi (Trading) Ltd.
 *
 * dng_writer.cpp - DNG writer
 */

#include "dng_writer.h"

#include <algorithm>
#include <iostream>
#include <map>

#include <tiffio.h>

#include <libcamera/control_ids.h>
#include <libcamera/formats.h>
#include <libcamera/property_ids.h>

using namespace libcamera;

enum CFAPatternColour : uint8_t {
	CFAPatternRed = 0,
	CFAPatternGreen = 1,
	CFAPatternBlue = 2,
};

struct FormatInfo {
	uint8_t bitsPerSample;
	CFAPatternColour pattern[4];
	void (*packScanline)(void *output, const void *input,
			     unsigned int width);
	void (*thumbScanline)(const FormatInfo &info, void *output,
			      const void *input, unsigned int width,
			      unsigned int stride);
};

struct Matrix3d {
	Matrix3d()
	{
	}

	Matrix3d(float m0, float m1, float m2,
		 float m3, float m4, float m5,
		 float m6, float m7, float m8)
	{
		m[0] = m0, m[1] = m1, m[2] = m2;
		m[3] = m3, m[4] = m4, m[5] = m5;
		m[6] = m6, m[7] = m7, m[8] = m8;
	}

	Matrix3d(const Span<const float> &span)
		: Matrix3d(span[0], span[1], span[2],
			   span[3], span[4], span[5],
			   span[6], span[7], span[8])
	{
	}

	static Matrix3d diag(float diag0, float diag1, float diag2)
	{
		return Matrix3d(diag0, 0, 0, 0, diag1, 0, 0, 0, diag2);
	}

	static Matrix3d identity()
	{
		return Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1);
	}

	Matrix3d transpose() const
	{
		return { m[0], m[3], m[6], m[1], m[4], m[7], m[2], m[5], m[8] };
	}

	Matrix3d cofactors() const
	{
		return { m[4] * m[8] - m[5] * m[7],
			 -(m[3] * m[8] - m[5] * m[6]),
			 m[3] * m[7] - m[4] * m[6],
			 -(m[1] * m[8] - m[2] * m[7]),
			 m[0] * m[8] - m[2] * m[6],
			 -(m[0] * m[7] - m[1] * m[6]),
			 m[1] * m[5] - m[2] * m[4],
			 -(m[0] * m[5] - m[2] * m[3]),
			 m[0] * m[4] - m[1] * m[3] };
	}

	Matrix3d adjugate() const
	{
		return cofactors().transpose();
	}

	float determinant() const
	{
		return m[0] * (m[4] * m[8] - m[5] * m[7]) -
		       m[1] * (m[3] * m[8] - m[5] * m[6]) +
		       m[2] * (m[3] * m[7] - m[4] * m[6]);
	}

	Matrix3d inverse() const
	{
		return adjugate() * (1.0 / determinant());
	}

	Matrix3d operator*(const Matrix3d &other) const
	{
		Matrix3d result;
		for (unsigned int i = 0; i < 3; i++) {
			for (unsigned int j = 0; j < 3; j++) {
				result.m[i * 3 + j] =
					m[i * 3 + 0] * other.m[0 + j] +
					m[i * 3 + 1] * other.m[3 + j] +
					m[i * 3 + 2] * other.m[6 + j];
			}
		}
		return result;
	}

	Matrix3d operator*(float f) const
	{
		Matrix3d result;
		for (unsigned int i = 0; i < 9; i++)
			result.m[i] = m[i] * f;
		return result;
	}

	float m[9];
};

void packScanlineSBGGR10P(void *output, const void *input, unsigned int width)
{
	const uint8_t *in = static_cast<const uint8_t *>(input);
	uint8_t *out = static_cast<uint8_t *>(output);

	/* \todo Can this be made more efficient? */
	for (unsigned int x = 0; x < width; x += 4) {
		*out++ = in[0];
		*out++ = (in[4] & 0x03) << 6 | in[1] >> 2;
		*out++ = (in[1] & 0x03) << 6 | (in[4] & 0x0c) << 2 | in[2] >> 4;
		*out++ = (in[2] & 0x0f) << 4 | (in[4] & 0x30) >> 2 | in[3] >> 6;
		*out++ = (in[3] & 0x3f) << 2 | (in[4] & 0xc0) >> 6;
		in += 5;
	}
}

void packScanlineSBGGR12P(void *output, const void *input, unsigned int width)
{
	const uint8_t *in = static_cast<const uint8_t *>(input);
	uint8_t *out = static_cast<uint8_t *>(output);

	/* \todo Can this be made more efficient? */
	for (unsigned int i = 0; i < width; i += 2) {
		*out++ = in[0];
		*out++ = (in[2] & 0x0f) << 4 | in[1] >> 4;
		*out++ = (in[1] & 0x0f) << 4 | in[2] >> 4;
		in += 3;
	}
}

void thumbScanlineSBGGRxxP(const FormatInfo &info, void *output,
			   const void *input, unsigned int width,
			   unsigned int stride)
{
	const uint8_t *in = static_cast<const uint8_t *>(input);
	uint8_t *out = static_cast<uint8_t *>(output);

	/* Number of bytes corresponding to 16 pixels. */
	unsigned int skip = info.bitsPerSample * 16 / 8;

	for (unsigned int x = 0; x < width; x++) {
		uint8_t value = (in[0] + in[1] + in[stride] + in[stride + 1]) >> 2;
		*out++ = value;
		*out++ = value;
		*out++ = value;
		in += skip;
	}
}

void packScanlineIPU3(void *output, const void *input, unsigned int width)
{
	const uint8_t *in = static_cast<const uint8_t *>(input);
	uint16_t *out = static_cast<uint16_t *>(output);

	/*
	 * Upscale the 10-bit format to 16-bit as it's not trivial to pack it
	 * as 10-bit without gaps.
	 *
	 * \todo Improve packing to keep the 10-bit sample size.
	 */
	unsigned int x = 0;
	while (true) {
		for (unsigned int i = 0; i < 6; i++) {
			*out++ = (in[1] & 0x03) << 14 | (in[0] & 0xff) << 6;
			if (++x >= width)
				return;

			*out++ = (in[2] & 0x0f) << 12 | (in[1] & 0xfc) << 4;
			if (++x >= width)
				return;

			*out++ = (in[3] & 0x3f) << 10 | (in[2] & 0xf0) << 2;
			if (++x >= width)
				return;

			*out++ = (in[4] & 0xff) <<  8 | (in[3] & 0xc0) << 0;
			if (++x >= width)
				return;

			in += 5;
		}

		*out++ = (in[1] & 0x03) << 14 | (in[0] & 0xff) << 6;
		if (++x >= width)
			return;

		in += 2;
	}
}

void thumbScanlineIPU3([[maybe_unused]] const FormatInfo &info, void *output,
		       const void *input, unsigned int width,
		       unsigned int stride)
{
	uint8_t *out = static_cast<uint8_t *>(output);

	for (unsigned int x = 0; x < width; x++) {
		unsigned int pixel = x * 16;
		unsigned int block = pixel / 25;
		unsigned int pixelInBlock = pixel - block * 25;

		/*
		 * If the pixel is the last in the block cheat a little and
		 * move one pixel backward to avoid reading between two blocks
		 * and having to deal with the padding bits.
		 */
		if (pixelInBlock == 24)
			pixelInBlock--;

		const uint8_t *in = static_cast<const uint8_t *>(input)
				  + block * 32 + (pixelInBlock / 4) * 5;

		uint16_t val1, val2, val3, val4;
		switch (pixelInBlock % 4) {
		case 0:
			val1 = (in[1] & 0x03) << 14 | (in[0] & 0xff) << 6;
			val2 = (in[2] & 0x0f) << 12 | (in[1] & 0xfc) << 4;
			val3 = (in[stride + 1] & 0x03) << 14 | (in[stride + 0] & 0xff) << 6;
			val4 = (in[stride + 2] & 0x0f) << 12 | (in[stride + 1] & 0xfc) << 4;
			break;
		case 1:
			val1 = (in[2] & 0x0f) << 12 | (in[1] & 0xfc) << 4;
			val2 = (in[3] & 0x3f) << 10 | (in[2] & 0xf0) << 2;
			val3 = (in[stride + 2] & 0x0f) << 12 | (in[stride + 1] & 0xfc) << 4;
			val4 = (in[stride + 3] & 0x3f) << 10 | (in[stride + 2] & 0xf0) << 2;
			break;
		case 2:
			val1 = (in[3] & 0x3f) << 10 | (in[2] & 0xf0) << 2;
			val2 = (in[4] & 0xff) <<  8 | (in[3] & 0xc0) << 0;
			val3 = (in[stride + 3] & 0x3f) << 10 | (in[stride + 2] & 0xf0) << 2;
			val4 = (in[stride + 4] & 0xff) <<  8 | (in[stride + 3] & 0xc0) << 0;
			break;
		case 3:
			val1 = (in[4] & 0xff) <<  8 | (in[3] & 0xc0) << 0;
			val2 = (in[6] & 0x03) << 14 | (in[5] & 0xff) << 6;
			val3 = (in[stride + 4] & 0xff) <<  8 | (in[stride + 3] & 0xc0) << 0;
			val4 = (in[stride + 6] & 0x03) << 14 | (in[stride + 5] & 0xff) << 6;
			break;
		}

		uint8_t value = (val1 + val2 + val3 + val4) >> 10;
		*out++ = value;
		*out++ = value;
		*out++ = value;
	}
}

static const std::map<PixelFormat, FormatInfo> formatInfo = {
	{ formats::SBGGR10_CSI2P, {
		.bitsPerSample = 10,
		.pattern = { CFAPatternBlue, CFAPatternGreen, CFAPatternGreen, CFAPatternRed },
		.packScanline = packScanlineSBGGR10P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SGBRG10_CSI2P, {
		.bitsPerSample = 10,
		.pattern = { CFAPatternGreen, CFAPatternBlue, CFAPatternRed, CFAPatternGreen },
		.packScanline = packScanlineSBGGR10P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SGRBG10_CSI2P, {
		.bitsPerSample = 10,
		.pattern = { CFAPatternGreen, CFAPatternRed, CFAPatternBlue, CFAPatternGreen },
		.packScanline = packScanlineSBGGR10P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SRGGB10_CSI2P, {
		.bitsPerSample = 10,
		.pattern = { CFAPatternRed, CFAPatternGreen, CFAPatternGreen, CFAPatternBlue },
		.packScanline = packScanlineSBGGR10P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SBGGR12_CSI2P, {
		.bitsPerSample = 12,
		.pattern = { CFAPatternBlue, CFAPatternGreen, CFAPatternGreen, CFAPatternRed },
		.packScanline = packScanlineSBGGR12P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SGBRG12_CSI2P, {
		.bitsPerSample = 12,
		.pattern = { CFAPatternGreen, CFAPatternBlue, CFAPatternRed, CFAPatternGreen },
		.packScanline = packScanlineSBGGR12P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SGRBG12_CSI2P, {
		.bitsPerSample = 12,
		.pattern = { CFAPatternGreen, CFAPatternRed, CFAPatternBlue, CFAPatternGreen },
		.packScanline = packScanlineSBGGR12P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SRGGB12_CSI2P, {
		.bitsPerSample = 12,
		.pattern = { CFAPatternRed, CFAPatternGreen, CFAPatternGreen, CFAPatternBlue },
		.packScanline = packScanlineSBGGR12P,
		.thumbScanline = thumbScanlineSBGGRxxP,
	} },
	{ formats::SBGGR10_IPU3, {
		.bitsPerSample = 16,
		.pattern = { CFAPatternBlue, CFAPatternGreen, CFAPatternGreen, CFAPatternRed },
		.packScanline = packScanlineIPU3,
		.thumbScanline = thumbScanlineIPU3,
	} },
	{ formats::SGBRG10_IPU3, {
		.bitsPerSample = 16,
		.pattern = { CFAPatternGreen, CFAPatternBlue, CFAPatternRed, CFAPatternGreen },
		.packScanline = packScanlineIPU3,
		.thumbScanline = thumbScanlineIPU3,
	} },
	{ formats::SGRBG10_IPU3, {
		.bitsPerSample = 16,
		.pattern = { CFAPatternGreen, CFAPatternRed, CFAPatternBlue, CFAPatternGreen },
		.packScanline = packScanlineIPU3,
		.thumbScanline = thumbScanlineIPU3,
	} },
	{ formats::SRGGB10_IPU3, {
		.bitsPerSample = 16,
		.pattern = { CFAPatternRed, CFAPatternGreen, CFAPatternGreen, CFAPatternBlue },
		.packScanline = packScanlineIPU3,