[media] media: imx: csi: add frame skipping support (fb30ee79) · Commits · e / devices / android_kernel_fairphone_FP4

drivers/staging/media/imx/imx-media-csi.c

+146 −21

Original line number	Diff line number	Diff line
		@@ -2,6 +2,7 @@
		* V4L2 Capture CSI Subdev for Freescale i.MX5/6 SOC
		*
		* Copyright (c) 2014-2017 Mentor Graphics Inc.
		* Copyright (C) 2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License as published by
		@@ -9,6 +10,7 @@
		* (at your option) any later version.
		*/
		#include <linux/delay.h>
		#include <linux/gcd.h>
		#include <linux/interrupt.h>
		#include <linux/module.h>
		#include <linux/pinctrl/consumer.h>
		@@ -42,6 +44,18 @@
		#define H_ALIGN 1 /* multiple of 2 lines */
		#define S_ALIGN 1 /* multiple of 2 */

		/*
		* struct csi_skip_desc - CSI frame skipping descriptor
		* @keep - number of frames kept per max_ratio frames
		* @max_ratio - width of skip_smfc, written to MAX_RATIO bitfield
		* @skip_smfc - skip pattern written to the SKIP_SMFC bitfield
		*/
		struct csi_skip_desc {
		u8 keep;
		u8 max_ratio;
		u8 skip_smfc;
		};

		struct csi_priv {
		struct device *dev;
		struct ipu_soc *ipu;
		@@ -65,8 +79,9 @@ struct csi_priv {

		struct v4l2_mbus_framefmt format_mbus[CSI_NUM_PADS];
		const struct imx_media_pixfmt *cc[CSI_NUM_PADS];
		struct v4l2_fract frame_interval;
		struct v4l2_fract frame_interval[CSI_NUM_PADS];
		struct v4l2_rect crop;
		const struct csi_skip_desc *skip;

		/* active vb2 buffers to send to video dev sink */
		struct imx_media_buffer *active_vb2_buf[2];
		@@ -581,6 +596,10 @@ static int csi_setup(struct csi_priv *priv)

		ipu_csi_set_dest(priv->csi, priv->dest);

		if (priv->dest == IPU_CSI_DEST_IDMAC)
		ipu_csi_set_skip_smfc(priv->csi, priv->skip->skip_smfc,
		priv->skip->max_ratio - 1, 0);

		ipu_csi_dump(priv->csi);

		return 0;
		@@ -588,6 +607,7 @@ static int csi_setup(struct csi_priv *priv)

		static int csi_start(struct csi_priv *priv)
		{
		struct v4l2_fract output_fi, input_fi;
		u32 bad_frames = 0;
		int ret;

		@@ -596,10 +616,12 @@ static int csi_start(struct csi_priv *priv)
		return -EINVAL;
		}

		output_fi = &priv->frame_interval[priv->active_output_pad];
		input_fi = &priv->frame_interval[CSI_SINK_PAD];

		ret = v4l2_subdev_call(priv->sensor->sd, sensor,
		g_skip_frames, &bad_frames);
		if (!ret && bad_frames) {
		struct v4l2_fract *fi = &priv->frame_interval;
		u32 delay_usec;

		/*
		@@ -610,8 +632,8 @@ static int csi_start(struct csi_priv *priv)
		* to lose vert/horiz sync.
		*/
		delay_usec = DIV_ROUND_UP_ULL(
		(u64)USEC_PER_SEC * fi->numerator * bad_frames,
		fi->denominator);
		(u64)USEC_PER_SEC * input_fi->numerator * bad_frames,
		input_fi->denominator);
		usleep_range(delay_usec, delay_usec + 1000);
		}

		@@ -627,8 +649,7 @@ static int csi_start(struct csi_priv *priv)

		/* start the frame interval monitor */
		if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC) {
		ret = imx_media_fim_set_stream(priv->fim, &priv->frame_interval,
		true);
		ret = imx_media_fim_set_stream(priv->fim, output_fi, true);
		if (ret)
		goto idmac_stop;
		}
		@@ -643,8 +664,7 @@ static int csi_start(struct csi_priv *priv)

		fim_off:
		if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC)
		imx_media_fim_set_stream(priv->fim, &priv->frame_interval,
		false);
		imx_media_fim_set_stream(priv->fim, NULL, false);
		idmac_stop:
		if (priv->dest == IPU_CSI_DEST_IDMAC)
		csi_idmac_stop(priv);
		@@ -658,14 +678,85 @@ static void csi_stop(struct csi_priv *priv)

		/* stop the frame interval monitor */
		if (priv->fim)
		imx_media_fim_set_stream(priv->fim,
		&priv->frame_interval,
		false);
		imx_media_fim_set_stream(priv->fim, NULL, false);
		}

		ipu_csi_disable(priv->csi);
		}

		static const struct csi_skip_desc csi_skip[12] = {
		{ 1, 1, 0x00 }, /* Keep all frames */
		{ 5, 6, 0x10 }, /* Skip every sixth frame */
		{ 4, 5, 0x08 }, /* Skip every fifth frame */
		{ 3, 4, 0x04 }, /* Skip every fourth frame */
		{ 2, 3, 0x02 }, /* Skip every third frame */
		{ 3, 5, 0x0a }, /* Skip frames 1 and 3 of every 5 */
		{ 1, 2, 0x01 }, /* Skip every second frame */
		{ 2, 5, 0x0b }, /* Keep frames 1 and 4 of every 5 */
		{ 1, 3, 0x03 }, /* Keep one in three frames */
		{ 1, 4, 0x07 }, /* Keep one in four frames */
		{ 1, 5, 0x0f }, /* Keep one in five frames */
		{ 1, 6, 0x1f }, /* Keep one in six frames */
		};

		static void csi_apply_skip_interval(const struct csi_skip_desc *skip,
		struct v4l2_fract *interval)
		{
		unsigned int div;

		interval->numerator *= skip->max_ratio;
		interval->denominator *= skip->keep;

		/* Reduce fraction to lowest terms */
		div = gcd(interval->numerator, interval->denominator);
		if (div > 1) {
		interval->numerator /= div;
		interval->denominator /= div;
		}
		}

		/*
		* Find the skip pattern to produce the output frame interval closest to the
		* requested one, for the given input frame interval. Updates the output frame
		* interval to the exact value.
		*/
		static const struct csi_skip_desc csi_find_best_skip(struct v4l2_fract in,
		struct v4l2_fract *out)
		{
		const struct csi_skip_desc skip = &csi_skip[0], best_skip = skip;
		u32 min_err = UINT_MAX;
		u64 want_us;
		int i;

		/* Default to 1:1 ratio */
		if (out->numerator == 0 \|\| out->denominator == 0 \|\|
		in->numerator == 0 \|\| in->denominator == 0) {
		out = in;
		return best_skip;
		}

		want_us = div_u64((u64)USEC_PER_SEC * out->numerator, out->denominator);

		/* Find the reduction closest to the requested time per frame */
		for (i = 0; i < ARRAY_SIZE(csi_skip); i++, skip++) {
		u64 tmp, err;

		tmp = div_u64((u64)USEC_PER_SEC * in->numerator *
		skip->max_ratio, in->denominator * skip->keep);

		err = abs((s64)tmp - want_us);
		if (err < min_err) {
		min_err = err;
		best_skip = skip;
		}
		}

		out = in;
		csi_apply_skip_interval(best_skip, out);

		return best_skip;
		}

		/*
		* V4L2 subdev operations.
		*/
		@@ -675,8 +766,13 @@ static int csi_g_frame_interval(struct v4l2_subdev *sd,
		{
		struct csi_priv *priv = v4l2_get_subdevdata(sd);

		if (fi->pad >= CSI_NUM_PADS)
		return -EINVAL;

		mutex_lock(&priv->lock);
		fi->interval = priv->frame_interval;

		fi->interval = priv->frame_interval[fi->pad];

		mutex_unlock(&priv->lock);

		return 0;
		@@ -686,18 +782,44 @@ static int csi_s_frame_interval(struct v4l2_subdev *sd,
		struct v4l2_subdev_frame_interval *fi)
		{
		struct csi_priv *priv = v4l2_get_subdevdata(sd);
		struct v4l2_fract *input_fi;
		int ret = 0;

		mutex_lock(&priv->lock);

		/* Output pads mirror active input pad, no limits on input pads */
		if (fi->pad == CSI_SRC_PAD_IDMAC \|\| fi->pad == CSI_SRC_PAD_DIRECT)
		fi->interval = priv->frame_interval;
		input_fi = &priv->frame_interval[CSI_SINK_PAD];

		priv->frame_interval = fi->interval;
		switch (fi->pad) {
		case CSI_SINK_PAD:
		/* No limits on input frame interval */
		/* Reset output intervals and frame skipping ratio to 1:1 */
		priv->frame_interval[CSI_SRC_PAD_IDMAC] = fi->interval;
		priv->frame_interval[CSI_SRC_PAD_DIRECT] = fi->interval;
		priv->skip = &csi_skip[0];
		break;
		case CSI_SRC_PAD_IDMAC:
		/*
		* frame interval at IDMAC output pad depends on input
		* interval, modified by frame skipping.
		*/
		priv->skip = csi_find_best_skip(input_fi, &fi->interval);
		break;
		case CSI_SRC_PAD_DIRECT:
		/*
		* frame interval at DIRECT output pad is same as input
		* interval.
		*/
		fi->interval = *input_fi;
		break;
		default:
		ret = -EINVAL;
		goto out;
		}

		priv->frame_interval[fi->pad] = fi->interval;
		out:
		mutex_unlock(&priv->lock);

		return 0;
		return ret;
		}

		static int csi_s_stream(struct v4l2_subdev *sd, int enable)
		@@ -1334,11 +1456,14 @@ static int csi_registered(struct v4l2_subdev *sd)
		&priv->cc[i]);
		if (ret)
		goto put_csi;
		}

		/* init default frame interval */
		priv->frame_interval.numerator = 1;
		priv->frame_interval.denominator = 30;
		priv->frame_interval[i].numerator = 1;
		priv->frame_interval[i].denominator = 30;
		}

		/* disable frame skipping */
		priv->skip = &csi_skip[0];

		priv->fim = imx_media_fim_init(&priv->sd);
		if (IS_ERR(priv->fim)) {