omap3-isp-live: Initial commit

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

1 files changed, 259 insertions, 0 deletions

diff --git a/isp/controls.c b/isp/controls.c
new file mode 100644
index 0000000..1c46039
--- /dev/null
+++ b/isp/controls.c
@@ -0,0 +1,259 @@
+/*
+ * OMAP3 ISP library - OMAP3 ISP controls
+ *
+ * Copyright (C) 2010-2011 Ideas on board SPRL
+ *
+ * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This library is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <linux/omap3isp.h>
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "controls.h"
+#include "omap3isp.h"
+#include "omap3isp-priv.h"
+#include "subdev.h"
+
+/* -----------------------------------------------------------------------------
+ * Matrix algebra helper functions
+ */
+
+struct matrix {
+	float coeff[3][3];
+};
+
+static void matrix_zero(struct matrix *m)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < 3; ++i) {
+		for (j = 0; j < 3; ++j)
+			m->coeff[i][j] = 0.0;
+	}
+}
+
+static void matrix_invert(struct matrix *m)
+{
+	/* Invert the matrix using the transpose of the matrix of cofactors. The
+	 * Gauss-Jordan elimination would be faster in the general case, but we
+	 * know that the matrix is 3x3.
+	 */
+	const float eps = 1e-6;
+	struct matrix out;
+	unsigned int i, j;
+	float det;
+
+	out.coeff[0][0] = m->coeff[1][1] * m->coeff[2][2]
+			- m->coeff[1][2] * m->coeff[2][1];
+	out.coeff[0][1] = m->coeff[0][2] * m->coeff[2][1]
+			- m->coeff[0][1] * m->coeff[2][2];
+	out.coeff[0][2] = m->coeff[0][1] * m->coeff[1][2]
+			- m->coeff[0][2] * m->coeff[1][1];
+	out.coeff[1][0] = m->coeff[1][2] * m->coeff[2][0]
+			- m->coeff[1][0] * m->coeff[2][2];
+	out.coeff[1][1] = m->coeff[0][0] * m->coeff[2][2]
+			- m->coeff[0][2] * m->coeff[2][0];
+	out.coeff[1][2] = m->coeff[0][2] * m->coeff[1][0]
+			- m->coeff[0][0] * m->coeff[1][2];
+	out.coeff[2][0] = m->coeff[1][0] * m->coeff[2][1]
+			- m->coeff[1][1] * m->coeff[2][0];
+	out.coeff[2][1] = m->coeff[0][1] * m->coeff[2][0]
+			- m->coeff[0][0] * m->coeff[2][1];
+	out.coeff[2][2] = m->coeff[0][0] * m->coeff[1][1]
+			- m->coeff[1][0] * m->coeff[0][1];
+
+	det = m->coeff[0][0] * out.coeff[0][0] +
+	      m->coeff[0][1] * out.coeff[1][0] +
+	      m->coeff[0][2] * out.coeff[2][0];
+
+	if (det < eps)
+		return;
+
+	det = 1/det;
+
+	for (i = 0; i < 3; ++i)
+		for (j = 0; j < 3; ++j)
+			m->coeff[i][j] = out.coeff[i][j] * det;
+}
+
+static void matrix_multiply(struct matrix *a, const struct matrix *b)
+{
+	struct matrix out;
+
+	/* Compute a * b and return the result in a. */
+	out.coeff[0][0] = a->coeff[0][0] * b->coeff[0][0]
+			+ a->coeff[0][1] * b->coeff[1][0]
+			+ a->coeff[0][2] * b->coeff[2][0];
+	out.coeff[0][1] = a->coeff[0][0] * b->coeff[0][1]
+			+ a->coeff[0][1] * b->coeff[1][1]
+			+ a->coeff[0][2] * b->coeff[2][1];
+	out.coeff[0][2] = a->coeff[0][0] * b->coeff[0][2]
+			+ a->coeff[0][1] * b->coeff[1][2]
+			+ a->coeff[0][2] * b->coeff[2][2];
+	out.coeff[1][0] = a->coeff[1][0] * b->coeff[0][0]
+			+ a->coeff[1][1] * b->coeff[1][0]
+			+ a->coeff[1][2] * b->coeff[2][0];
+	out.coeff[1][1] = a->coeff[1][0] * b->coeff[0][1]
+			+ a->coeff[1][1] * b->coeff[1][1]
+			+ a->coeff[1][2] * b->coeff[2][1];
+	out.coeff[1][2] = a->coeff[1][0] * b->coeff[0][2]
+			+ a->coeff[1][1] * b->coeff[1][2]
+			+ a->coeff[1][2] * b->coeff[2][2];
+	out.coeff[2][0] = a->coeff[2][0] * b->coeff[0][0]
+			+ a->coeff[2][1] * b->coeff[1][0]
+			+ a->coeff[2][2] * b->coeff[2][0];
+	out.coeff[2][1] = a->coeff[2][0] * b->coeff[0][1]
+			+ a->coeff[2][1] * b->coeff[1][1]
+			+ a->coeff[2][2] * b->coeff[2][1];
+	out.coeff[2][2] = a->coeff[2][0] * b->coeff[0][2]
+			+ a->coeff[2][1] * b->coeff[1][2]
+			+ a->coeff[2][2] * b->coeff[2][2];
+
+	*a = out;
+}
+
+static void matrix_float_to_s10q8(__u16 out[3][3], const struct matrix *in)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < 3; ++i) {
+		for (j = 0; j < 3; ++j)
+			out[i][j] = (__u16)((__s16)(in->coeff[i][j] * 256) & 0x3ff);
+	}
+}
+
+static void matrix_float_to_s12q8(__u16 out[3][3], const struct matrix *in)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < 3; ++i) {
+		for (j = 0; j < 3; ++j)
+			out[i][j] = (__u16)((__s16)(in->coeff[i][j] * 256) & 0xfff);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Preview engine parameters configuration
+ */
+
+static const struct matrix omap3isp_preview_csc = {
+	.coeff = {
+		/* Default values. */
+		{  0.2968750,  0.5937500,  0.1093750 },
+		{ -0.1718750, -0.3281250,  0.5000000 },
+		{  0.5000000, -0.3828125, -0.0781250 },
+#if 0
+		/* Default values for fluorescent light. */
+		{  0.25781250,  0.50390625,  0.09765625 },
+		{ -0.14843750, -0.29296875,  0.43750000 },
+		{  0.43750000, -0.36718750, -0.07031250 },
+#endif
+	},
+};
+
+static const struct matrix omap3isp_preview_rgb2rgb = {
+	.coeff = {
+		/* Default values. */
+		{ 1.0, 0.0, 0.0 },
+		{ 0.0, 1.0, 0.0 },
+		{ 0.0, 0.0, 1.0 },
+#if 0
+		/* Default values for fluorescent light. */
+		{  1.88281250, -0.812500, -0.07031250 },
+		{ -0.39453125,  1.671875, -0.27734375 },
+		{ -0.12500000, -1.250000,  2.37500000 },
+#endif
+	},
+};
+
+int omap3_isp_preview_setup(struct omap3_isp_device *isp)
+{
+	struct omap3isp_prev_update_config config;
+	struct omap3isp_prev_rgbtorgb rgb2rgb;
+	struct omap3isp_prev_csc csc;
+	int ret;
+
+	memset(&config, 0, sizeof config);
+	config.update = OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV;
+	config.flag = OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV;
+	config.rgb2rgb = &rgb2rgb;
+	config.csc = &csc;
+
+	memset(&rgb2rgb, 0, sizeof rgb2rgb);
+	matrix_float_to_s12q8(rgb2rgb.matrix, &omap3isp_preview_rgb2rgb);
+
+	memset(&csc, 0, sizeof csc);
+	matrix_float_to_s10q8(csc.matrix, &omap3isp_preview_csc);
+
+	v4l2_subdev_open(isp->preview);
+
+	ret = ioctl(isp->preview->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
+	if (ret < 0)
+		return -errno;
+
+	return ret;
+}
+
+int omap3_isp_preview_set_contrast(struct omap3_isp_device *isp, unsigned int value)
+{
+	int contrast = value;
+	int ret;
+
+	ret = v4l2_subdev_set_control(isp->preview, V4L2_CID_CONTRAST, &contrast);
+	if (ret < 0)
+		return -errno;
+
+	return ret;
+}
+int omap3_isp_preview_set_saturation(struct omap3_isp_device *isp, float value)
+{
+	struct omap3isp_prev_update_config config;
+	struct omap3isp_prev_rgbtorgb rgb2rgb;
+	struct matrix saturation;
+	struct matrix gain;
+	int ret;
+
+	matrix_zero(&gain);
+	gain.coeff[0][0] = 1.0;
+	gain.coeff[1][1] = value;
+	gain.coeff[2][2] = value;
+
+	saturation = omap3isp_preview_csc;
+	matrix_invert(&saturation);
+	matrix_multiply(&saturation, &gain);
+	matrix_multiply(&saturation, &omap3isp_preview_csc);
+
+	memset(&config, 0, sizeof config);
+	config.update = OMAP3ISP_PREV_RGB2RGB;
+	config.flag = OMAP3ISP_PREV_RGB2RGB;
+	config.rgb2rgb = &rgb2rgb;
+
+	memset(&rgb2rgb, 0, sizeof rgb2rgb);
+	matrix_float_to_s12q8(rgb2rgb.matrix, &saturation);
+
+	ret = ioctl(isp->preview->fd, VIDIOC_OMAP3ISP_PRV_CFG, &config);
+	if (ret < 0)
+		return -errno;
+
+	return ret;
+}