From fb30ee795576a349e86577d06e1bb4a5ce19ba52 Mon Sep 17 00:00:00 2001 From: Philipp Zabel Date: Wed, 7 Jun 2017 15:34:07 -0300 Subject: [PATCH] [media] media: imx: csi: add frame skipping support 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 Signed-off-by: Steve Longerbeam Signed-off-by: Russell King Signed-off-by: Hans Verkuil Signed-off-by: Mauro Carvalho Chehab --- drivers/staging/media/imx/imx-media-csi.c | 167 +++++++++++++++++++--- 1 file changed, 146 insertions(+), 21 deletions(-) diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c index ec4f9f88712c..e6188900f5ea 100644 --- a/drivers/staging/media/imx/imx-media-csi.c +++ b/drivers/staging/media/imx/imx-media-csi.c @@ -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 * * 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 +#include #include #include #include @@ -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[i].numerator = 1; + priv->frame_interval[i].denominator = 30; } - /* init default frame interval */ - priv->frame_interval.numerator = 1; - priv->frame_interval.denominator = 30; + /* disable frame skipping */ + priv->skip = &csi_skip[0]; priv->fim = imx_media_fim_init(&priv->sd); if (IS_ERR(priv->fim)) { -- 2.39.2