[PATCH v2 0/4] STM32 DCMI camera interface crop support

[PATCH v2 0/4] STM32 DCMI camera interface crop support

[Hugues Fruchet]

This patchset implements crop feature of Digital Camera Memory Interface
(DCMI) of STMicroelectronics STM32 SoC series, allowing user to crop
at pixel level inside sensor captured frame.

This patchset follows discussions initiated from a first submission of DCMI
crop support, see [1].

First part of patches brings few fixes and cleanup in DCMI driver
to prepare support of crop through g_/s_selection interface in latest
commit.

This has been tested on STM32F746G-DISCO + STM32F4DIS-CAM extension
running OV9655 sensor, using a modified version of yavta [2] utility
to support crop through S_SELECTION(V4L2_SEL_TGT_CROP) ioctl:
 yavta -s 480x272 -n 1 --capture=2 /dev/video0 --crop=0,0,480,272

v4l2-compliance cropping test is failed due to OV9655 sensor supporting
several discrete frame sizes and crop support added by DCMI interface [3]:
  fail: v4l2-test-formats.cpp(1266): node->frmsizes_count[pixfmt] > 1
    test Cropping: FAIL
If sensor is restricted to only a single supported resolution, test is OK.
Compliance test should be adapted to support this case.


[1] http://www.mail-archive.com/linux-media@vger.kernel.org/msg114652.html
[2] http://git.ideasonboard.org/?p=yavta.git;a=summary
[3] see v4l2-compliance test report below

===========
= history =
===========
version 2:
  - Fix remarks from Hans Verkuil on "cleanup" commit:
    http://www.mail-archive.com/linux-media@vger.kernel.org/msg116612.html
    - change "nb_of" to "num_of"
  - Drop "default format at open" commit as per Hans' remark:
    http://www.mail-archive.com/linux-media@vger.kernel.org/msg116614.html
  - Fix Kbuild warning on uninitialized ret variable:
    http://www.mail-archive.com/linux-media@vger.kernel.org/msg116398.html
  - Fix remarks from Hans Verkuil on crop commit:
    http://www.mail-archive.com/linux-media@vger.kernel.org/msg116615.html
    - refactor g_selection() code
    - revisit dcmi->do_crop true/false critera

version 1:
  - Initial version

===================
= v4l2-compliance =
===================
Below is the v4l2-compliance report for this current version of the DCMI camera interface.
v4l2-compliance has been built from v4l-utils-1.12.5.

~ # v4l2-compliance -s -f -d /dev/video0
v4l2-compliance SHA   : f5f45e17ee98a0ebad7836ade2b34ceec909d751

Driver Info:
        Driver name   : stm32-dcmi
        Card type     : STM32 Camera Memory Interface
        Bus info      : platform:dcmi
        Driver version: 4.12.0
        Capabilities  : 0x85200001
                Video Capture
                Read/Write
                Streaming
                Extended Pix Format
                Device Capabilities
        Device Caps   : 0x05200001
                Video Capture
                Read/Write
                Streaming
                Extended Pix Format

Compliance test for device /dev/video0 (not using libv4l2):

Required ioctls:
        test VIDIOC_QUERYCAP: OK

Allow for multiple opens:
        test second video open: OK
        test VIDIOC_QUERYCAP: OK
        test VIDIOC_G/S_PRIORITY: OK
        test for unlimited opens: OK

Debug ioctls:
        test VIDIOC_DBG_G/S_REGISTER: OK (Not Supported)
        test VIDIOC_LOG_STATUS: OK

Input ioctls:
        test VIDIOC_G/S_TUNER/ENUM_FREQ_BANDS: OK (Not Supported)
        test VIDIOC_G/S_FREQUENCY: OK (Not Supported)
        test VIDIOC_S_HW_FREQ_SEEK: OK (Not Supported)
        test VIDIOC_ENUMAUDIO: OK (Not Supported)
        test VIDIOC_G/S/ENUMINPUT: OK
        test VIDIOC_G/S_AUDIO: OK (Not Supported)
        Inputs: 1 Audio Inputs: 0 Tuners: 0

Output ioctls:
        test VIDIOC_G/S_MODULATOR: OK (Not Supported)
        test VIDIOC_G/S_FREQUENCY: OK (Not Supported)
        test VIDIOC_ENUMAUDOUT: OK (Not Supported)
        test VIDIOC_G/S/ENUMOUTPUT: OK (Not Supported)
        test VIDIOC_G/S_AUDOUT: OK (Not Supported)
        Outputs: 0 Audio Outputs: 0 Modulators: 0

Input/Output configuration ioctls:
        test VIDIOC_ENUM/G/S/QUERY_STD: OK (Not Supported)
        test VIDIOC_ENUM/G/S/QUERY_DV_TIMINGS: OK (Not Supported)
        test VIDIOC_DV_TIMINGS_CAP: OK (Not Supported)
        test VIDIOC_G/S_EDID: OK (Not Supported)

Test input 0:

        Control ioctls:
                test VIDIOC_QUERY_EXT_CTRL/QUERYMENU: OK
                test VIDIOC_QUERYCTRL: OK
                test VIDIOC_G/S_CTRL: OK
                test VIDIOC_G/S/TRY_EXT_CTRLS: OK
                test VIDIOC_(UN)SUBSCRIBE_EVENT/DQEVENT: OK
                test VIDIOC_G/S_JPEGCOMP: OK (Not Supported)
                Standard Controls: 2 Private Controls: 0

        Format ioctls:
                test VIDIOC_ENUM_FMT/FRAMESIZES/FRAMEINTERVALS: OK
                test VIDIOC_G/S_PARM: OK (Not Supported)
                test VIDIOC_G_FBUF: OK (Not Supported)
                test VIDIOC_G_FMT: OK
                test VIDIOC_TRY_FMT: OK
                test VIDIOC_S_FMT: OK
                test VIDIOC_G_SLICED_VBI_CAP: OK (Not Supported)
                fail: v4l2-test-formats.cpp(1266): node->frmsizes_count[pixfmt] > 1
                test Cropping: FAIL
                test Composing: OK (Not Supported)
                fail: v4l2-test-formats.cpp(1633): node->can_scale && node->frmsizes_count[v4l_format_g_pixelformat(&cur)]
                test Scaling: OK

        Codec ioctls:
                test VIDIOC_(TRY_)ENCODER_CMD: OK (Not Supported)
                test VIDIOC_G_ENC_INDEX: OK (Not Supported)
                test VIDIOC_(TRY_)DECODER_CMD: OK (Not Supported)

        Buffer ioctls:
                test VIDIOC_REQBUFS/CREATE_BUFS/QUERYBUF: OK
                test VIDIOC_EXPBUF: OK

Test input 0:

Streaming ioctls:
        test read/write: OK
        test MMAP: OK
        test USERPTR: OK (Not Supported)
        test DMABUF: Cannot test, specify --expbuf-device

Stream using all formats:
        test MMAP for Format RGBP, Frame Size 160x120:
                Crop 160x120@0x0, Stride 320, Field None: OK
                Crop 0x0@0x0, Stride 320, Field None, SelTest: OK
                Crop 160x120@0x0, Stride 320, Field None, SelTest: OK
        test MMAP for Format RGBP, Frame Size 160x120:
                Crop 160x120@0x0, Stride 320, Field None: OK
        test MMAP for Format RGBP, Frame Size 160x120:
                Crop 160x120@0x0, Stride 320, Field None: OK

Total: 51, Succeeded: 50, Failed: 1, Warnings: 0
Hugues Fruchet (4):
  [media] stm32-dcmi: catch dma submission error
  [media] stm32-dcmi: revisit control register handling
  [media] stm32-dcmi: cleanup variable/fields namings
  [media] stm32-dcmi: g_/s_selection crop support

 drivers/media/platform/stm32/stm32-dcmi.c | 491 ++++++++++++++++++++++++++----
 1 file changed, 430 insertions(+), 61 deletions(-)

-- 
1.9.1

[PATCH v2 1/4] [media] stm32-dcmi: catch dma submission error

[Hugues Fruchet]

Test cookie return by dmaengine_submit() and return error if any.

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
---
 drivers/media/platform/stm32/stm32-dcmi.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index c2168b5..7ffb2d3 100644
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -295,6 +295,10 @@ static int dcmi_start_dma(struct stm32_dcmi *dcmi,
 
 	/* Push current DMA transaction in the pending queue */
 	dcmi->dma_cookie = dmaengine_submit(desc);
+	if (dma_submit_error(dcmi->dma_cookie)) {
+		dev_err(dcmi->dev, "%s: DMA submission failed\n", __func__);
+		return -ENXIO;
+	}
 
 	dma_async_issue_pending(dcmi->dma_chan);
 
-- 
1.9.1

[PATCH v2 2/4] [media] stm32-dcmi: revisit control register handling

[Hugues Fruchet]

Simplify bits handling of DCMI_CR register.

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
---
 drivers/media/platform/stm32/stm32-dcmi.c | 14 ++++----------
 1 file changed, 4 insertions(+), 10 deletions(-)

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index 7ffb2d3..1fe2d0f 100644
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -490,7 +490,7 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
 	struct dcmi_buf *buf, *node;
-	u32 val;
+	u32 val = 0;
 	int ret;
 
 	ret = clk_enable(dcmi->mclk);
@@ -510,22 +510,16 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	spin_lock_irq(&dcmi->irqlock);
 
-	val = reg_read(dcmi->regs, DCMI_CR);
-
-	val &= ~(CR_PCKPOL | CR_HSPOL | CR_VSPOL |
-		 CR_EDM_0 | CR_EDM_1 | CR_FCRC_0 |
-		 CR_FCRC_1 | CR_JPEG | CR_ESS);
-
 	/* Set bus width */
 	switch (dcmi->bus.bus_width) {
 	case 14:
-		val &= CR_EDM_0 + CR_EDM_1;
+		val |= CR_EDM_0 | CR_EDM_1;
 		break;
 	case 12:
-		val &= CR_EDM_1;
+		val |= CR_EDM_1;
 		break;
 	case 10:
-		val &= CR_EDM_0;
+		val |= CR_EDM_0;
 		break;
 	default:
 		/* Set bus width to 8 bits by default */
-- 
1.9.1

[PATCH v2 3/4] [media] stm32-dcmi: cleanup variable/fields namings

[Hugues Fruchet]

Uniformize "pixfmt" variables to "pix".
Change "current_fmt" & "dcmi_fmt" variables to variables
with "sd_" prefix to explicitly refer to subdev format.

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
---
 drivers/media/platform/stm32/stm32-dcmi.c | 103 ++++++++++++++++--------------
 1 file changed, 54 insertions(+), 49 deletions(-)

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index 1fe2d0f..7713c10 100644
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -132,9 +132,9 @@ struct stm32_dcmi {
 	struct dcmi_graph_entity	entity;
 	struct v4l2_format		fmt;
 
-	const struct dcmi_format	**user_formats;
-	unsigned int			num_user_formats;
-	const struct dcmi_format	*current_fmt;
+	const struct dcmi_format	**sd_formats;
+	unsigned int			num_of_sd_formats;
+	const struct dcmi_format	*sd_format;
 
 	/* Protect this data structure */
 	struct mutex			lock;
@@ -684,12 +684,12 @@ static int dcmi_g_fmt_vid_cap(struct file *file, void *priv,
 static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
 						       unsigned int fourcc)
 {
-	unsigned int num_formats = dcmi->num_user_formats;
+	unsigned int num_formats = dcmi->num_of_sd_formats;
 	const struct dcmi_format *fmt;
 	unsigned int i;
 
 	for (i = 0; i < num_formats; i++) {
-		fmt = dcmi->user_formats[i];
+		fmt = dcmi->sd_formats[i];
 		if (fmt->fourcc == fourcc)
 			return fmt;
 	}
@@ -698,40 +698,42 @@ static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
 }
 
 static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
-			const struct dcmi_format **current_fmt)
+			const struct dcmi_format **sd_format)
 {
-	const struct dcmi_format *dcmi_fmt;
-	struct v4l2_pix_format *pixfmt = &f->fmt.pix;
+	const struct dcmi_format *sd_fmt;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev_pad_config pad_cfg;
 	struct v4l2_subdev_format format = {
 		.which = V4L2_SUBDEV_FORMAT_TRY,
 	};
 	int ret;
 
-	dcmi_fmt = find_format_by_fourcc(dcmi, pixfmt->pixelformat);
-	if (!dcmi_fmt) {
-		dcmi_fmt = dcmi->user_formats[dcmi->num_user_formats - 1];
-		pixfmt->pixelformat = dcmi_fmt->fourcc;
+	sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
+	if (!sd_fmt) {
+		sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
+		pix->pixelformat = sd_fmt->fourcc;
 	}
 
 	/* Limit to hardware capabilities */
-	pixfmt->width = clamp(pixfmt->width, MIN_WIDTH, MAX_WIDTH);
-	pixfmt->height = clamp(pixfmt->height, MIN_HEIGHT, MAX_HEIGHT);
+	pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
+	pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
 
-	v4l2_fill_mbus_format(&format.format, pixfmt, dcmi_fmt->mbus_code);
+	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
 			       &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	v4l2_fill_pix_format(pixfmt, &format.format);
+	/* Update pix regarding to what sensor can do */
+	v4l2_fill_pix_format(pix, &format.format);
 
-	pixfmt->field = V4L2_FIELD_NONE;
-	pixfmt->bytesperline = pixfmt->width * dcmi_fmt->bpp;
-	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
 
-	if (current_fmt)
-		*current_fmt = dcmi_fmt;
+	pix->field = V4L2_FIELD_NONE;
+	pix->bytesperline = pix->width * sd_fmt->bpp;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	if (sd_format)
+		*sd_format = sd_fmt;
 
 	return 0;
 }
@@ -741,22 +743,25 @@ static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
 	struct v4l2_subdev_format format = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
-	const struct dcmi_format *current_fmt;
+	const struct dcmi_format *sd_format;
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 	int ret;
 
-	ret = dcmi_try_fmt(dcmi, f, &current_fmt);
+	ret = dcmi_try_fmt(dcmi, f, &sd_format);
 	if (ret)
 		return ret;
 
-	v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
-			      current_fmt->mbus_code);
+	/* pix to mbus format */
+	v4l2_fill_mbus_format(mf, pix,
+			      sd_format->mbus_code);
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
 			       set_fmt, NULL, &format);
 	if (ret < 0)
 		return ret;
 
 	dcmi->fmt = *f;
-	dcmi->current_fmt = current_fmt;
+	dcmi->sd_format = sd_format;
 
 	return 0;
 }
@@ -785,10 +790,10 @@ static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
 {
 	struct stm32_dcmi *dcmi = video_drvdata(file);
 
-	if (f->index >= dcmi->num_user_formats)
+	if (f->index >= dcmi->num_of_sd_formats)
 		return -EINVAL;
 
-	f->pixelformat = dcmi->user_formats[f->index]->fourcc;
+	f->pixelformat = dcmi->sd_formats[f->index]->fourcc;
 	return 0;
 }
 
@@ -830,18 +835,18 @@ static int dcmi_enum_framesizes(struct file *file, void *fh,
 				struct v4l2_frmsizeenum *fsize)
 {
 	struct stm32_dcmi *dcmi = video_drvdata(file);
-	const struct dcmi_format *dcmi_fmt;
+	const struct dcmi_format *sd_fmt;
 	struct v4l2_subdev_frame_size_enum fse = {
 		.index = fsize->index,
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
 	int ret;
 
-	dcmi_fmt = find_format_by_fourcc(dcmi, fsize->pixel_format);
-	if (!dcmi_fmt)
+	sd_fmt = find_format_by_fourcc(dcmi, fsize->pixel_format);
+	if (!sd_fmt)
 		return -EINVAL;
 
-	fse.code = dcmi_fmt->mbus_code;
+	fse.code = sd_fmt->mbus_code;
 
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, enum_frame_size,
 			       NULL, &fse);
@@ -859,7 +864,7 @@ static int dcmi_enum_frameintervals(struct file *file, void *fh,
 				    struct v4l2_frmivalenum *fival)
 {
 	struct stm32_dcmi *dcmi = video_drvdata(file);
-	const struct dcmi_format *dcmi_fmt;
+	const struct dcmi_format *sd_fmt;
 	struct v4l2_subdev_frame_interval_enum fie = {
 		.index = fival->index,
 		.width = fival->width,
@@ -868,11 +873,11 @@ static int dcmi_enum_frameintervals(struct file *file, void *fh,
 	};
 	int ret;
 
-	dcmi_fmt = find_format_by_fourcc(dcmi, fival->pixel_format);
-	if (!dcmi_fmt)
+	sd_fmt = find_format_by_fourcc(dcmi, fival->pixel_format);
+	if (!sd_fmt)
 		return -EINVAL;
 
-	fie.code = dcmi_fmt->mbus_code;
+	fie.code = sd_fmt->mbus_code;
 
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
 			       enum_frame_interval, NULL, &fie);
@@ -994,7 +999,7 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
 			.width		= CIF_WIDTH,
 			.height		= CIF_HEIGHT,
 			.field		= V4L2_FIELD_NONE,
-			.pixelformat	= dcmi->user_formats[0]->fourcc,
+			.pixelformat	= dcmi->sd_formats[0]->fourcc,
 		},
 	};
 	int ret;
@@ -1002,7 +1007,7 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
 	ret = dcmi_try_fmt(dcmi, &f, NULL);
 	if (ret)
 		return ret;
-	dcmi->current_fmt = dcmi->user_formats[0];
+	dcmi->sd_format = dcmi->sd_formats[0];
 	dcmi->fmt = f;
 	return 0;
 }
@@ -1025,7 +1030,7 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
 
 static int dcmi_formats_init(struct stm32_dcmi *dcmi)
 {
-	const struct dcmi_format *dcmi_fmts[ARRAY_SIZE(dcmi_formats)];
+	const struct dcmi_format *sd_fmts[ARRAY_SIZE(dcmi_formats)];
 	unsigned int num_fmts = 0, i, j;
 	struct v4l2_subdev *subdev = dcmi->entity.subdev;
 	struct v4l2_subdev_mbus_code_enum mbus_code = {
@@ -1040,13 +1045,13 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)
 
 			/* Code supported, have we got this fourcc yet? */
 			for (j = 0; j < num_fmts; j++)
-				if (dcmi_fmts[j]->fourcc ==
+				if (sd_fmts[j]->fourcc ==
 						dcmi_formats[i].fourcc)
 					/* Already available */
 					break;
 			if (j == num_fmts)
 				/* New */
-				dcmi_fmts[num_fmts++] = dcmi_formats + i;
+				sd_fmts[num_fmts++] = dcmi_formats + i;
 		}
 		mbus_code.index++;
 	}
@@ -1054,18 +1059,18 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)
 	if (!num_fmts)
 		return -ENXIO;
 
-	dcmi->num_user_formats = num_fmts;
-	dcmi->user_formats = devm_kcalloc(dcmi->dev,
-					 num_fmts, sizeof(struct dcmi_format *),
-					 GFP_KERNEL);
-	if (!dcmi->user_formats) {
-		dev_err(dcmi->dev, "could not allocate memory\n");
+	dcmi->num_of_sd_formats = num_fmts;
+	dcmi->sd_formats = devm_kcalloc(dcmi->dev,
+					num_fmts, sizeof(struct dcmi_format *),
+					GFP_KERNEL);
+	if (!dcmi->sd_formats) {
+		dev_err(dcmi->dev, "Could not allocate memory\n");
 		return -ENOMEM;
 	}
 
-	memcpy(dcmi->user_formats, dcmi_fmts,
+	memcpy(dcmi->sd_formats, sd_fmts,
 	       num_fmts * sizeof(struct dcmi_format *));
-	dcmi->current_fmt = dcmi->user_formats[0];
+	dcmi->sd_format = dcmi->sd_formats[0];
 
 	return 0;
 }
-- 
1.9.1

[PATCH v2 4/4] [media] stm32-dcmi: g_/s_selection crop support

[Hugues Fruchet]

Implements g_/s_selection crop support by using DCMI crop
hardware feature.
User can first get the maximum supported resolution of the sensor
by calling g_selection(V4L2_SEL_TGT_CROP_BOUNDS).
Then user call to s_selection(V4L2_SEL_TGT_CROP) will reset sensor
to its maximum resolution and crop request is saved for later usage
in s_fmt().
Next call to s_fmt() will check if sensor can do frame size request
with crop request. If sensor supports only discrete frame sizes,
the frame size which is larger than user request is selected in
order to be able to match the crop request. Then s_fmt() resolution
user request is adjusted to match crop request resolution.

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
---
 drivers/media/platform/stm32/stm32-dcmi.c | 374 +++++++++++++++++++++++++++++-
 1 file changed, 370 insertions(+), 4 deletions(-)

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index 7713c10..35ba6f2 100644
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -33,6 +33,7 @@
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-rect.h>
 #include <media/videobuf2-dma-contig.h>
 
 #define DRV_NAME "stm32-dcmi"
@@ -107,6 +108,11 @@ struct dcmi_format {
 	u8	bpp;
 };
 
+struct dcmi_framesize {
+	u32	width;
+	u32	height;
+};
+
 struct dcmi_buf {
 	struct vb2_v4l2_buffer	vb;
 	bool			prepared;
@@ -131,10 +137,16 @@ struct stm32_dcmi {
 	struct v4l2_async_notifier	notifier;
 	struct dcmi_graph_entity	entity;
 	struct v4l2_format		fmt;
+	struct v4l2_rect		crop;
+	bool				do_crop;
 
 	const struct dcmi_format	**sd_formats;
 	unsigned int			num_of_sd_formats;
 	const struct dcmi_format	*sd_format;
+	struct dcmi_framesize		*sd_framesizes;
+	unsigned int			num_of_sd_framesizes;
+	struct dcmi_framesize		sd_framesize;
+	struct v4l2_rect		sd_bounds;
 
 	/* Protect this data structure */
 	struct mutex			lock;
@@ -325,6 +337,28 @@ static int dcmi_start_capture(struct stm32_dcmi *dcmi)
 	return 0;
 }
 
+static void dcmi_set_crop(struct stm32_dcmi *dcmi)
+{
+	u32 size, start;
+
+	/* Crop resolution */
+	size = ((dcmi->crop.height - 1) << 16) |
+		((dcmi->crop.width << 1) - 1);
+	reg_write(dcmi->regs, DCMI_CWSIZE, size);
+
+	/* Crop start point */
+	start = ((dcmi->crop.top) << 16) |
+		 ((dcmi->crop.left << 1));
+	reg_write(dcmi->regs, DCMI_CWSTRT, start);
+
+	dev_dbg(dcmi->dev, "Cropping to %ux%u@%u:%u\n",
+		dcmi->crop.width, dcmi->crop.height,
+		dcmi->crop.left, dcmi->crop.top);
+
+	/* Enable crop */
+	reg_set(dcmi->regs, DCMI_CR, CR_CROP);
+}
+
 static irqreturn_t dcmi_irq_thread(int irq, void *arg)
 {
 	struct stm32_dcmi *dcmi = arg;
@@ -540,6 +574,10 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	reg_write(dcmi->regs, DCMI_CR, val);
 
+	/* Set crop */
+	if (dcmi->do_crop)
+		dcmi_set_crop(dcmi);
+
 	/* Enable dcmi */
 	reg_set(dcmi->regs, DCMI_CR, CR_ENABLE);
 
@@ -697,10 +735,37 @@ static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
 	return NULL;
 }
 
+static void __find_outer_frame_size(struct stm32_dcmi *dcmi,
+				    struct v4l2_pix_format *pix,
+				    struct dcmi_framesize *framesize)
+{
+	struct dcmi_framesize *match = NULL;
+	unsigned int i;
+	unsigned int min_err = UINT_MAX;
+
+	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+		struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
+		int w_err = (fsize->width - pix->width);
+		int h_err = (fsize->height - pix->height);
+		int err = w_err + h_err;
+
+		if ((w_err >= 0) && (h_err >= 0) && (err < min_err)) {
+			min_err = err;
+			match = fsize;
+		}
+	}
+	if (!match)
+		match = &dcmi->sd_framesizes[0];
+
+	*framesize = *match;
+}
+
 static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
-			const struct dcmi_format **sd_format)
+			const struct dcmi_format **sd_format,
+			struct dcmi_framesize *sd_framesize)
 {
 	const struct dcmi_format *sd_fmt;
+	struct dcmi_framesize sd_fsize;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev_pad_config pad_cfg;
 	struct v4l2_subdev_format format = {
@@ -718,6 +783,17 @@ static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
 	pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
 	pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
 
+	if (dcmi->do_crop && dcmi->num_of_sd_framesizes) {
+		struct dcmi_framesize outer_sd_fsize;
+		/*
+		 * If crop is requested and sensor have discrete frame sizes,
+		 * select the frame size that is just larger than request
+		 */
+		__find_outer_frame_size(dcmi, pix, &outer_sd_fsize);
+		pix->width = outer_sd_fsize.width;
+		pix->height = outer_sd_fsize.height;
+	}
+
 	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
 			       &pad_cfg, &format);
@@ -727,6 +803,31 @@ static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
 	/* Update pix regarding to what sensor can do */
 	v4l2_fill_pix_format(pix, &format.format);
 
+	/* Save resolution that sensor can actually do */
+	sd_fsize.width = pix->width;
+	sd_fsize.height = pix->height;
+
+	if (dcmi->do_crop) {
+		struct v4l2_rect c = dcmi->crop;
+		struct v4l2_rect max_rect;
+
+		/*
+		 * Adjust crop by making the intersection between
+		 * format resolution request and crop request
+		 */
+		max_rect.top = 0;
+		max_rect.left = 0;
+		max_rect.width = pix->width;
+		max_rect.height = pix->height;
+		v4l2_rect_map_inside(&c, &max_rect);
+		c.top  = clamp_t(s32, c.top, 0, pix->height - c.height);
+		c.left = clamp_t(s32, c.left, 0, pix->width - c.width);
+		dcmi->crop = c;
+
+		/* Adjust format resolution request to crop */
+		pix->width = dcmi->crop.width;
+		pix->height = dcmi->crop.height;
+	}
 
 	pix->field = V4L2_FIELD_NONE;
 	pix->bytesperline = pix->width * sd_fmt->bpp;
@@ -734,6 +835,8 @@ static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
 
 	if (sd_format)
 		*sd_format = sd_fmt;
+	if (sd_framesize)
+		*sd_framesize = sd_fsize;
 
 	return 0;
 }
@@ -744,24 +847,41 @@ static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
 	const struct dcmi_format *sd_format;
+	struct dcmi_framesize sd_framesize;
 	struct v4l2_mbus_framefmt *mf = &format.format;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	int ret;
 
-	ret = dcmi_try_fmt(dcmi, f, &sd_format);
+	/*
+	 * Try format, fmt.width/height could have been changed
+	 * to match sensor capability or crop request
+	 * sd_format & sd_framesize will contain what subdev
+	 * can do for this request.
+	 */
+	ret = dcmi_try_fmt(dcmi, f, &sd_format, &sd_framesize);
 	if (ret)
 		return ret;
 
 	/* pix to mbus format */
 	v4l2_fill_mbus_format(mf, pix,
 			      sd_format->mbus_code);
+	mf->width = sd_framesize.width;
+	mf->height = sd_framesize.height;
+
 	ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
 			       set_fmt, NULL, &format);
 	if (ret < 0)
 		return ret;
 
+	dev_dbg(dcmi->dev, "Sensor format set to 0x%x %ux%u\n",
+		mf->code, mf->width, mf->height);
+	dev_dbg(dcmi->dev, "Buffer format set to %4.4s %ux%u\n",
+		(char *)&pix->pixelformat,
+		pix->width, pix->height);
+
 	dcmi->fmt = *f;
 	dcmi->sd_format = sd_format;
+	dcmi->sd_framesize = sd_framesize;
 
 	return 0;
 }
@@ -782,7 +902,7 @@ static int dcmi_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct stm32_dcmi *dcmi = video_drvdata(file);
 
-	return dcmi_try_fmt(dcmi, f, NULL);
+	return dcmi_try_fmt(dcmi, f, NULL, NULL);
 }
 
 static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
@@ -797,6 +917,193 @@ static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
 	return 0;
 }
 
+static int dcmi_get_sensor_format(struct stm32_dcmi *dcmi,
+				  struct v4l2_pix_format *pix)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	v4l2_fill_pix_format(pix, &fmt.format);
+
+	return 0;
+}
+
+static int dcmi_set_sensor_format(struct stm32_dcmi *dcmi,
+				  struct v4l2_pix_format *pix)
+{
+	const struct dcmi_format *sd_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_subdev_pad_config pad_cfg;
+	int ret;
+
+	sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
+	if (!sd_fmt) {
+		sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
+		pix->pixelformat = sd_fmt->fourcc;
+	}
+
+	v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
+			       &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int dcmi_get_sensor_bounds(struct stm32_dcmi *dcmi,
+				  struct v4l2_rect *r)
+{
+	struct v4l2_subdev_selection bounds = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = V4L2_SEL_TGT_CROP_BOUNDS,
+	};
+	unsigned int max_width, max_height, max_pixsize;
+	struct v4l2_pix_format pix;
+	unsigned int i;
+	int ret;
+
+	/*
+	 * Get sensor bounds first
+	 */
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_selection,
+			       NULL, &bounds);
+	if (!ret)
+		*r = bounds.r;
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	/*
+	 * If selection is not implemented,
+	 * fallback by enumerating sensor frame sizes
+	 * and take the largest one
+	 */
+	max_width = 0;
+	max_height = 0;
+	max_pixsize = 0;
+	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+		struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
+		unsigned int pixsize = fsize->width * fsize->height;
+
+		if (pixsize > max_pixsize) {
+			max_pixsize = pixsize;
+			max_width = fsize->width;
+			max_height = fsize->height;
+		}
+	}
+	if (max_pixsize > 0) {
+		r->top = 0;
+		r->left = 0;
+		r->width = max_width;
+		r->height = max_height;
+		return 0;
+	}
+
+	/*
+	 * If frame sizes enumeration is not implemented,
+	 * fallback by getting current sensor frame size
+	 */
+	ret = dcmi_get_sensor_format(dcmi, &pix);
+	if (ret)
+		return ret;
+
+	r->top = 0;
+	r->left = 0;
+	r->width = pix.width;
+	r->height = pix.height;
+
+	return 0;
+}
+
+static int dcmi_g_selection(struct file *file, void *fh,
+			    struct v4l2_selection *s)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r = dcmi->sd_bounds;
+		return 0;
+	case V4L2_SEL_TGT_CROP:
+		if (dcmi->do_crop) {
+			s->r = dcmi->crop;
+		} else {
+			s->r.top = 0;
+			s->r.left = 0;
+			s->r.width = dcmi->fmt.fmt.pix.width;
+			s->r.height = dcmi->fmt.fmt.pix.height;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int dcmi_s_selection(struct file *file, void *priv,
+			    struct v4l2_selection *s)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+	struct v4l2_rect r = s->r;
+	struct v4l2_rect max_rect;
+	struct v4l2_pix_format pix;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    s->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* Reset sensor resolution to max resolution */
+	pix.pixelformat = dcmi->fmt.fmt.pix.pixelformat;
+	pix.width = dcmi->sd_bounds.width;
+	pix.height = dcmi->sd_bounds.height;
+	dcmi_set_sensor_format(dcmi, &pix);
+
+	/*
+	 * Make the intersection between
+	 * sensor resolution
+	 * and crop request
+	 */
+	max_rect.top = 0;
+	max_rect.left = 0;
+	max_rect.width = pix.width;
+	max_rect.height = pix.height;
+	v4l2_rect_map_inside(&r, &max_rect);
+	r.top  = clamp_t(s32, r.top, 0, pix.height - r.height);
+	r.left = clamp_t(s32, r.left, 0, pix.width - r.width);
+
+	if (!((r.top == dcmi->sd_bounds.top) &&
+	      (r.left == dcmi->sd_bounds.left) &&
+	      (r.width == dcmi->sd_bounds.width) &&
+	      (r.height == dcmi->sd_bounds.height))) {
+		/* Crop if request is different than sensor resolution */
+		dcmi->do_crop = true;
+		dcmi->crop = r;
+		dev_dbg(dcmi->dev, "s_selection: crop %ux%u@(%u,%u) from %ux%u\n",
+			r.width, r.height, r.left, r.top,
+			pix.width, pix.height);
+	} else {
+		/* Disable crop */
+		dcmi->do_crop = false;
+		dev_dbg(dcmi->dev, "s_selection: crop is disabled\n");
+	}
+
+	s->r = r;
+	return 0;
+}
+
 static int dcmi_querycap(struct file *file, void *priv,
 			 struct v4l2_capability *cap)
 {
@@ -955,6 +1262,8 @@ static int dcmi_release(struct file *file)
 	.vidioc_g_fmt_vid_cap		= dcmi_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap		= dcmi_s_fmt_vid_cap,
 	.vidioc_enum_fmt_vid_cap	= dcmi_enum_fmt_vid_cap,
+	.vidioc_g_selection		= dcmi_g_selection,
+	.vidioc_s_selection		= dcmi_s_selection,
 
 	.vidioc_enum_input		= dcmi_enum_input,
 	.vidioc_g_input			= dcmi_g_input,
@@ -1004,7 +1313,7 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
 	};
 	int ret;
 
-	ret = dcmi_try_fmt(dcmi, &f, NULL);
+	ret = dcmi_try_fmt(dcmi, &f, NULL, NULL);
 	if (ret)
 		return ret;
 	dcmi->sd_format = dcmi->sd_formats[0];
@@ -1075,6 +1384,51 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)
 	return 0;
 }
 
+static int dcmi_framesizes_init(struct stm32_dcmi *dcmi)
+{
+	unsigned int num_fsize = 0;
+	struct v4l2_subdev *subdev = dcmi->entity.subdev;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.code = dcmi->sd_format->mbus_code,
+	};
+	unsigned int ret;
+	unsigned int i;
+
+	/* Allocate discrete framesizes array */
+	while (!v4l2_subdev_call(subdev, pad, enum_frame_size,
+				 NULL, &fse))
+		fse.index++;
+
+	num_fsize = fse.index;
+	if (!num_fsize)
+		return 0;
+
+	dcmi->num_of_sd_framesizes = num_fsize;
+	dcmi->sd_framesizes = devm_kcalloc(dcmi->dev, num_fsize,
+					   sizeof(struct dcmi_framesize),
+					   GFP_KERNEL);
+	if (!dcmi->sd_framesizes) {
+		dev_err(dcmi->dev, "Could not allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Fill array with sensor supported framesizes */
+	dev_dbg(dcmi->dev, "Sensor supports %u frame sizes:\n", num_fsize);
+	for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+		fse.index = i;
+		ret = v4l2_subdev_call(subdev, pad, enum_frame_size,
+				       NULL, &fse);
+		if (ret)
+			return ret;
+		dcmi->sd_framesizes[fse.index].width = fse.max_width;
+		dcmi->sd_framesizes[fse.index].height = fse.max_height;
+		dev_dbg(dcmi->dev, "%ux%u\n", fse.max_width, fse.max_height);
+	}
+
+	return 0;
+}
+
 static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
 {
 	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
@@ -1087,6 +1441,18 @@ static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
 		return ret;
 	}
 
+	ret = dcmi_framesizes_init(dcmi);
+	if (ret) {
+		dev_err(dcmi->dev, "Could not initialize framesizes\n");
+		return ret;
+	}
+
+	ret = dcmi_get_sensor_bounds(dcmi, &dcmi->sd_bounds);
+	if (ret) {
+		dev_err(dcmi->dev, "Could not get sensor bounds\n");
+		return ret;
+	}
+
 	ret = dcmi_set_default_fmt(dcmi);
 	if (ret) {
 		dev_err(dcmi->dev, "Could not set default format\n");
-- 
1.9.1