1 files changed, 46 insertions, 14 deletions

diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index 502e2a40964c..c7680eb83664 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -400,6 +400,35 @@ static int mx3_videobuf_init(struct vb2_buffer *vb)
         return 0;
 }
 
+static int mx3_stop_streaming(struct vb2_queue *q)
+{
+        struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+        struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+        struct mx3_camera_dev *mx3_cam = ici->priv;
+        struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
+        struct dma_chan *chan;
+        struct mx3_camera_buffer *buf, *tmp;
+        unsigned long flags;
+
+        if (ichan) {
+                chan = &ichan->dma_chan;
+                chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+        }
+
+        spin_lock_irqsave(&mx3_cam->lock, flags);
+
+        mx3_cam->active = NULL;
+
+        list_for_each_entry_safe(buf, tmp, &mx3_cam->capture, queue) {
+                buf->state = CSI_BUF_NEEDS_INIT;
+                list_del_init(&buf->queue);
+        }
+
+        spin_unlock_irqrestore(&mx3_cam->lock, flags);
+
+        return 0;
+}
+
 static struct vb2_ops mx3_videobuf_ops = {
         .queue_setup    = mx3_videobuf_setup,
         .buf_prepare    = mx3_videobuf_prepare,
@@ -408,6 +437,7 @@ static struct vb2_ops mx3_videobuf_ops = {
         .buf_init       = mx3_videobuf_init,
         .wait_prepare   = soc_camera_unlock,
         .wait_finish    = soc_camera_lock,
+        .stop_streaming = mx3_stop_streaming,
 };
 
 static int mx3_camera_init_videobuf(struct vb2_queue *q,
@@ -658,8 +688,8 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 
         fmt = soc_mbus_get_fmtdesc(code);
         if (!fmt) {
-                dev_err(icd->dev.parent,
-                        "Invalid format code #%u: %d\n", idx, code);
+                dev_warn(icd->dev.parent,
+                         "Unsupported format code #%u: %d\n", idx, code);
                 return 0;
         }
 
@@ -712,13 +742,9 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 
 static void configure_geometry(struct mx3_camera_dev *mx3_cam,
                                unsigned int width, unsigned int height,
-                               enum v4l2_mbus_pixelcode code)
+                               const struct soc_mbus_pixelfmt *fmt)
 {
         u32 ctrl, width_field, height_field;
-        const struct soc_mbus_pixelfmt *fmt;
-
-        fmt = soc_mbus_get_fmtdesc(code);
-        BUG_ON(!fmt);
 
         if (fourcc_to_ipu_pix(fmt->fourcc) == IPU_PIX_FMT_GENERIC) {
                 /*
@@ -726,8 +752,10 @@ static void configure_geometry(struct mx3_camera_dev *mx3_cam,
                  * the width parameter count the number of samples to
                  * capture to complete the whole image width.
                  */
-                width *= soc_mbus_samples_per_pixel(fmt);
-                BUG_ON(width < 0);
+                unsigned int num, den;
+                int ret = soc_mbus_samples_per_pixel(fmt, &num, &den);
+                BUG_ON(ret < 0);
+                width = width * num / den;
         }
 
         /* Setup frame size - this cannot be changed on-the-fly... */
@@ -774,8 +802,8 @@ static int acquire_dma_channel(struct mx3_camera_dev *mx3_cam)
  */
 static inline void stride_align(__u32 *width)
 {
-        if (((*width + 7) &  ~7) < 4096)
-                *width = (*width + 7) &  ~7;
+        if (ALIGN(*width, 8) < 4096)
+                *width = ALIGN(*width, 8);
         else
                 *width = *width &  ~7;
 }
@@ -801,11 +829,14 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
         if (ret < 0)
                 return ret;
 
-        /* The capture device might have changed its output  */
+        /* The capture device might have changed its output sizes */
         ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
         if (ret < 0)
                 return ret;
 
+        if (mf.code != icd->current_fmt->code)
+                return -EINVAL;
+
         if (mf.width & 7) {
                 /* Ouch! We can only handle 8-byte aligned width... */
                 stride_align(&mf.width);
@@ -815,7 +846,8 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
         }
 
         if (mf.width != icd->user_width || mf.height != icd->user_height)
-                configure_geometry(mx3_cam, mf.width, mf.height, mf.code);
+                configure_geometry(mx3_cam, mf.width, mf.height,
+                                   icd->current_fmt->host_fmt);
 
         dev_dbg(icd->dev.parent, "Sensor cropped %dx%d\n",
                 mf.width, mf.height);
@@ -853,7 +885,7 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
          * mxc_v4l2_s_fmt()
          */
 
-        configure_geometry(mx3_cam, pix->width, pix->height, xlate->code);
+        configure_geometry(mx3_cam, pix->width, pix->height, xlate->host_fmt);
 
         mf.width        = pix->width;
         mf.height       = pix->height;