From: Philipp Zabel <p.za...@pengutronix.de> The CSI can skip any out of up to 6 input frames, allowing to reduce the frame rate at the output pads by small fractions.
Signed-off-by: Philipp Zabel <p.za...@pengutronix.de> Signed-off-by: Steve Longerbeam <steve_longerb...@mentor.com> --- drivers/staging/media/imx/imx-media-csi.c | 125 ++++++++++++++++++++++++++++-- 1 file changed, 120 insertions(+), 5 deletions(-) diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c index a726744..e5105ec 100644 --- a/drivers/staging/media/imx/imx-media-csi.c +++ b/drivers/staging/media/imx/imx-media-csi.c @@ -1,13 +1,15 @@ /* * V4L2 Capture CSI Subdev for Freescale i.MX5/6 SOC * - * Copyright (c) 2014-2016 Mentor Graphics Inc. + * Copyright (c) 2014-2017 Mentor Graphics Inc. + * Copyright (C) 2017 Pengutronix, Philipp Zabel <ker...@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 * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. */ +#include <linux/gcd.h> #include <linux/interrupt.h> #include <linux/module.h> #include <linux/platform_device.h> @@ -40,6 +42,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,6 +79,7 @@ struct csi_priv { const struct imx_media_pixfmt *cc[CSI_NUM_PADS]; struct v4l2_fract frame_interval; struct v4l2_rect crop; + const struct csi_skip_desc *skip[CSI_NUM_PADS - 1]; /* active vb2 buffers to send to video dev sink */ struct imx_media_buffer *active_vb2_buf[2]; @@ -517,10 +532,12 @@ static int csi_setup(struct csi_priv *priv) struct v4l2_mbus_config sensor_mbus_cfg; struct v4l2_of_endpoint *sensor_ep; struct v4l2_mbus_framefmt if_fmt; + const struct csi_skip_desc *skip; infmt = &priv->format_mbus[CSI_SINK_PAD]; outfmt = &priv->format_mbus[priv->active_output_pad]; sensor_ep = &priv->sensor->sensor_ep; + skip = priv->skip[priv->active_output_pad - 1]; /* compose mbus_config from sensor endpoint */ sensor_mbus_cfg.type = sensor_ep->bus_type; @@ -545,6 +562,9 @@ static int csi_setup(struct csi_priv *priv) ipu_csi_set_dest(priv->csi, priv->dest); + ipu_csi_set_skip_smfc(priv->csi, skip->skip_smfc, skip->max_ratio - 1, + 0); + ipu_csi_dump(priv->csi); return 0; @@ -608,6 +628,77 @@ static void csi_stop(struct csi_priv *priv) 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) + 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. */ @@ -617,8 +708,16 @@ 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; + + if (fi->pad != CSI_SINK_PAD) + csi_apply_skip_interval(priv->skip[fi->pad - 1], &fi->interval); + mutex_unlock(&priv->lock); return 0; @@ -629,14 +728,27 @@ static int csi_s_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); - /* 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; + /* No limits on input pad */ + if (fi->pad == CSI_SINK_PAD) { + priv->frame_interval = fi->interval; + + /* Reset frame skipping ratio to 1:1 */ + priv->skip[0] = &csi_skip[0]; + priv->skip[1] = &csi_skip[0]; - priv->frame_interval = fi->interval; + goto out; + } + + /* Output pads depend on input interval, modified by frame skipping */ + priv->skip[fi->pad - 1] = csi_find_best_skip(&priv->frame_interval, + &fi->interval); +out: mutex_unlock(&priv->lock); return 0; @@ -1256,6 +1368,9 @@ static int csi_registered(struct v4l2_subdev *sd) &priv->cc[i]); if (ret) goto put_csi; + /* disable frame skipping */ + if (i != CSI_SINK_PAD) + priv->skip[i - 1] = &csi_skip[0]; } /* init default frame interval */ -- 2.7.4