drivers/media/platform/s3c-camif/camif-capture.c

   1 /*
   2  * s3c24xx/s3c64xx SoC series Camera Interface (CAMIF) driver
   3  *
   4  * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
   5  * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
   6  *
   7  * Based on drivers/media/platform/s5p-fimc,
   8  * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13 */
  14 #define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
  15
  16 #include <linux/bug.h>
  17 #include <linux/clk.h>
  18 #include <linux/device.h>
  19 #include <linux/errno.h>
  20 #include <linux/i2c.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/io.h>
  23 #include <linux/kernel.h>
  24 #include <linux/list.h>
  25 #include <linux/module.h>
  26 #include <linux/platform_device.h>
  27 #include <linux/pm_runtime.h>
  28 #include <linux/ratelimit.h>
  29 #include <linux/slab.h>
  30 #include <linux/types.h>
  31 #include <linux/videodev2.h>
  32
  33 #include <media/media-device.h>
  34 #include <media/v4l2-ctrls.h>
  35 #include <media/v4l2-event.h>
  36 #include <media/v4l2-ioctl.h>
  37 #include <media/videobuf2-v4l2.h>
  38 #include <media/videobuf2-dma-contig.h>
  39
  40 #include "camif-core.h"
  41 #include "camif-regs.h"
  42
  43 static int debug;
  44 module_param(debug, int, 0644);
  45
  46 /* Locking: called with vp->camif->slock spinlock held */
  47 static void camif_cfg_video_path(struct camif_vp *vp)
  48 {
  49         WARN_ON(s3c_camif_get_scaler_config(vp, &vp->scaler));
  50         camif_hw_set_scaler(vp);
  51         camif_hw_set_flip(vp);
  52         camif_hw_set_target_format(vp);
  53         camif_hw_set_output_dma(vp);
  54 }
  55
  56 static void camif_prepare_dma_offset(struct camif_vp *vp)
  57 {
  58         struct camif_frame *f = &vp->out_frame;
  59
  60         f->dma_offset.initial = f->rect.top * f->f_width + f->rect.left;
  61         f->dma_offset.line = f->f_width - (f->rect.left + f->rect.width);
  62
  63         pr_debug("dma_offset: initial: %d, line: %d\n",
  64                  f->dma_offset.initial, f->dma_offset.line);
  65 }
  66
  67 /* Locking: called with camif->slock spinlock held */
  68 static int s3c_camif_hw_init(struct camif_dev *camif, struct camif_vp *vp)
  69 {
  70         const struct s3c_camif_variant *variant = camif->variant;
  71
  72         if (camif->sensor.sd == NULL || vp->out_fmt == NULL)
  73                 return -EINVAL;
  74
  75         if (variant->ip_revision == S3C244X_CAMIF_IP_REV)
  76                 camif_hw_clear_fifo_overflow(vp);
  77         camif_hw_set_camera_bus(camif);
  78         camif_hw_set_source_format(camif);
  79         camif_hw_set_camera_crop(camif);
  80         camif_hw_set_test_pattern(camif, camif->test_pattern);
  81         if (variant->has_img_effect)
  82                 camif_hw_set_effect(camif, camif->colorfx,
  83                                 camif->colorfx_cb, camif->colorfx_cr);
  84         if (variant->ip_revision == S3C6410_CAMIF_IP_REV)
  85                 camif_hw_set_input_path(vp);
  86         camif_cfg_video_path(vp);
  87         vp->state &= ~ST_VP_CONFIG;
  88
  89         return 0;
  90 }
  91
  92 /*
  93  * Initialize the video path, only up from the scaler stage. The camera
  94  * input interface set up is skipped. This is useful to enable one of the
  95  * video paths when the other is already running.
  96  * Locking: called with camif->slock spinlock held.
  97  */
  98 static int s3c_camif_hw_vp_init(struct camif_dev *camif, struct camif_vp *vp)
  99 {
 100         unsigned int ip_rev = camif->variant->ip_revision;
 101
 102         if (vp->out_fmt == NULL)
 103                 return -EINVAL;
 104
 105         camif_prepare_dma_offset(vp);
 106         if (ip_rev == S3C244X_CAMIF_IP_REV)
 107                 camif_hw_clear_fifo_overflow(vp);
 108         camif_cfg_video_path(vp);
 109         vp->state &= ~ST_VP_CONFIG;
 110         return 0;
 111 }
 112
 113 static int sensor_set_power(struct camif_dev *camif, int on)
 114 {
 115         struct cam_sensor *sensor = &camif->sensor;
 116         int err = 0;
 117
 118         if (camif->sensor.power_count == !on)
 119                 err = v4l2_subdev_call(sensor->sd, core, s_power, on);
 120         if (err == -ENOIOCTLCMD)
 121                 err = 0;
 122         if (!err)
 123                 sensor->power_count += on ? 1 : -1;
 124
 125         pr_debug("on: %d, power_count: %d, err: %d\n",
 126                  on, sensor->power_count, err);
 127
 128         return err;
 129 }
 130
 131 static int sensor_set_streaming(struct camif_dev *camif, int on)
 132 {
 133         struct cam_sensor *sensor = &camif->sensor;
 134         int err = 0;
 135
 136         if (camif->sensor.stream_count == !on)
 137                 err = v4l2_subdev_call(sensor->sd, video, s_stream, on);
 138         if (!err)
 139                 sensor->stream_count += on ? 1 : -1;
 140
 141         pr_debug("on: %d, stream_count: %d, err: %d\n",
 142                  on, sensor->stream_count, err);
 143
 144         return err;
 145 }
 146
 147 /*
 148  * Reinitialize the driver so it is ready to start streaming again.
 149  * Return any buffers to vb2, perform CAMIF software reset and
 150  * turn off streaming at the data pipeline (sensor) if required.
 151  */
 152 static int camif_reinitialize(struct camif_vp *vp)
 153 {
 154         struct camif_dev *camif = vp->camif;
 155         struct camif_buffer *buf;
 156         unsigned long flags;
 157         bool streaming;
 158
 159         spin_lock_irqsave(&camif->slock, flags);
 160         streaming = vp->state & ST_VP_SENSOR_STREAMING;
 161
 162         vp->state &= ~(ST_VP_PENDING | ST_VP_RUNNING | ST_VP_OFF |
 163                        ST_VP_ABORTING | ST_VP_STREAMING |
 164                        ST_VP_SENSOR_STREAMING | ST_VP_LASTIRQ);
 165
 166         /* Release unused buffers */
 167         while (!list_empty(&vp->pending_buf_q)) {
 168                 buf = camif_pending_queue_pop(vp);
 169                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 170         }
 171
 172         while (!list_empty(&vp->active_buf_q)) {
 173                 buf = camif_active_queue_pop(vp);
 174                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 175         }
 176
 177         spin_unlock_irqrestore(&camif->slock, flags);
 178
 179         if (!streaming)
 180                 return 0;
 181
 182         return sensor_set_streaming(camif, 0);
 183 }
 184
 185 static bool s3c_vp_active(struct camif_vp *vp)
 186 {
 187         struct camif_dev *camif = vp->camif;
 188         unsigned long flags;
 189         bool ret;
 190
 191         spin_lock_irqsave(&camif->slock, flags);
 192         ret = (vp->state & ST_VP_RUNNING) || (vp->state & ST_VP_PENDING);
 193         spin_unlock_irqrestore(&camif->slock, flags);
 194
 195         return ret;
 196 }
 197
 198 static bool camif_is_streaming(struct camif_dev *camif)
 199 {
 200         unsigned long flags;
 201         bool status;
 202
 203         spin_lock_irqsave(&camif->slock, flags);
 204         status = camif->stream_count > 0;
 205         spin_unlock_irqrestore(&camif->slock, flags);
 206
 207         return status;
 208 }
 209
 210 static int camif_stop_capture(struct camif_vp *vp)
 211 {
 212         struct camif_dev *camif = vp->camif;
 213         unsigned long flags;
 214         int ret;
 215
 216         if (!s3c_vp_active(vp))
 217                 return 0;
 218
 219         spin_lock_irqsave(&camif->slock, flags);
 220         vp->state &= ~(ST_VP_OFF | ST_VP_LASTIRQ);
 221         vp->state |= ST_VP_ABORTING;
 222         spin_unlock_irqrestore(&camif->slock, flags);
 223
 224         ret = wait_event_timeout(vp->irq_queue,
 225                            !(vp->state & ST_VP_ABORTING),
 226                            msecs_to_jiffies(CAMIF_STOP_TIMEOUT));
 227
 228         spin_lock_irqsave(&camif->slock, flags);
 229
 230         if (ret == 0 && !(vp->state & ST_VP_OFF)) {
 231                 /* Timed out, forcibly stop capture */
 232                 vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
 233                                ST_VP_LASTIRQ);
 234
 235                 camif_hw_disable_capture(vp);
 236                 camif_hw_enable_scaler(vp, false);
 237         }
 238
 239         spin_unlock_irqrestore(&camif->slock, flags);
 240
 241         return camif_reinitialize(vp);
 242 }
 243
 244 static int camif_prepare_addr(struct camif_vp *vp, struct vb2_buffer *vb,
 245                               struct camif_addr *paddr)
 246 {
 247         struct camif_frame *frame = &vp->out_frame;
 248         u32 pix_size;
 249
 250         if (vb == NULL || frame == NULL)
 251                 return -EINVAL;
 252
 253         pix_size = frame->rect.width * frame->rect.height;
 254
 255         pr_debug("colplanes: %d, pix_size: %u\n",
 256                  vp->out_fmt->colplanes, pix_size);
 257
 258         paddr->y = vb2_dma_contig_plane_dma_addr(vb, 0);
 259
 260         switch (vp->out_fmt->colplanes) {
 261         case 1:
 262                 paddr->cb = 0;
 263                 paddr->cr = 0;
 264                 break;
 265         case 2:
 266                 /* decompose Y into Y/Cb */
 267                 paddr->cb = (u32)(paddr->y + pix_size);
 268                 paddr->cr = 0;
 269                 break;
 270         case 3:
 271                 paddr->cb = (u32)(paddr->y + pix_size);
 272                 /* decompose Y into Y/Cb/Cr */
 273                 if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
 274                         paddr->cr = (u32)(paddr->cb + (pix_size >> 1));
 275                 else /* 420 */
 276                         paddr->cr = (u32)(paddr->cb + (pix_size >> 2));
 277
 278                 if (vp->out_fmt->color == IMG_FMT_YCRCB420)
 279                         swap(paddr->cb, paddr->cr);
 280                 break;
 281         default:
 282                 return -EINVAL;
 283         }
 284
 285         pr_debug("DMA address: y: %pad  cb: %pad cr: %pad\n",
 286                  &paddr->y, &paddr->cb, &paddr->cr);
 287
 288         return 0;
 289 }
 290
 291 irqreturn_t s3c_camif_irq_handler(int irq, void *priv)
 292 {
 293         struct camif_vp *vp = priv;
 294         struct camif_dev *camif = vp->camif;
 295         unsigned int ip_rev = camif->variant->ip_revision;
 296         unsigned int status;
 297
 298         spin_lock(&camif->slock);
 299
 300         if (ip_rev == S3C6410_CAMIF_IP_REV)
 301                 camif_hw_clear_pending_irq(vp);
 302
 303         status = camif_hw_get_status(vp);
 304
 305         if (ip_rev == S3C244X_CAMIF_IP_REV && (status & CISTATUS_OVF_MASK)) {
 306                 camif_hw_clear_fifo_overflow(vp);
 307                 goto unlock;
 308         }
 309
 310         if (vp->state & ST_VP_ABORTING) {
 311                 if (vp->state & ST_VP_OFF) {
 312                         /* Last IRQ */
 313                         vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
 314                                        ST_VP_LASTIRQ);
 315                         wake_up(&vp->irq_queue);
 316                         goto unlock;
 317                 } else if (vp->state & ST_VP_LASTIRQ) {
 318                         camif_hw_disable_capture(vp);
 319                         camif_hw_enable_scaler(vp, false);
 320                         camif_hw_set_lastirq(vp, false);
 321                         vp->state |= ST_VP_OFF;
 322                 } else {
 323                         /* Disable capture, enable last IRQ */
 324                         camif_hw_set_lastirq(vp, true);
 325                         vp->state |= ST_VP_LASTIRQ;
 326                 }
 327         }
 328
 329         if (!list_empty(&vp->pending_buf_q) && (vp->state & ST_VP_RUNNING) &&
 330             !list_empty(&vp->active_buf_q)) {
 331                 unsigned int index;
 332                 struct camif_buffer *vbuf;
 333                 /*
 334                  * Get previous DMA write buffer index:
 335                  * 0 => DMA buffer 0, 2;
 336                  * 1 => DMA buffer 1, 3.
 337                  */
 338                 index = (CISTATUS_FRAMECNT(status) + 2) & 1;
 339                 vbuf = camif_active_queue_peek(vp, index);
 340
 341                 if (!WARN_ON(vbuf == NULL)) {
 342                         /* Dequeue a filled buffer */
 343                         v4l2_get_timestamp(&vbuf->vb.timestamp);
 344                         vbuf->vb.sequence = vp->frame_sequence++;
 345                         vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 346
 347                         /* Set up an empty buffer at the DMA engine */
 348                         vbuf = camif_pending_queue_pop(vp);
 349                         vbuf->index = index;
 350                         camif_hw_set_output_addr(vp, &vbuf->paddr, index);
 351                         camif_hw_set_output_addr(vp, &vbuf->paddr, index + 2);
 352
 353                         /* Scheduled in H/W, add to the queue */
 354                         camif_active_queue_add(vp, vbuf);
 355                 }
 356         } else if (!(vp->state & ST_VP_ABORTING) &&
 357                    (vp->state & ST_VP_PENDING))  {
 358                 vp->state |= ST_VP_RUNNING;
 359         }
 360
 361         if (vp->state & ST_VP_CONFIG) {
 362                 camif_prepare_dma_offset(vp);
 363                 camif_hw_set_camera_crop(camif);
 364                 camif_hw_set_scaler(vp);
 365                 camif_hw_set_flip(vp);
 366                 camif_hw_set_test_pattern(camif, camif->test_pattern);
 367                 if (camif->variant->has_img_effect)
 368                         camif_hw_set_effect(camif, camif->colorfx,
 369                                     camif->colorfx_cb, camif->colorfx_cr);
 370                 vp->state &= ~ST_VP_CONFIG;
 371         }
 372 unlock:
 373         spin_unlock(&camif->slock);
 374         return IRQ_HANDLED;
 375 }
 376
 377 static int start_streaming(struct vb2_queue *vq, unsigned int count)
 378 {
 379         struct camif_vp *vp = vb2_get_drv_priv(vq);
 380         struct camif_dev *camif = vp->camif;
 381         unsigned long flags;
 382         int ret;
 383
 384         /*
 385          * We assume the codec capture path is always activated
 386          * first, before the preview path starts streaming.
 387          * This is required to avoid internal FIFO overflow and
 388          * a need for CAMIF software reset.
 389          */
 390         spin_lock_irqsave(&camif->slock, flags);
 391
 392         if (camif->stream_count == 0) {
 393                 camif_hw_reset(camif);
 394                 ret = s3c_camif_hw_init(camif, vp);
 395         } else {
 396                 ret = s3c_camif_hw_vp_init(camif, vp);
 397         }
 398         spin_unlock_irqrestore(&camif->slock, flags);
 399
 400         if (ret < 0) {
 401                 camif_reinitialize(vp);
 402                 return ret;
 403         }
 404
 405         spin_lock_irqsave(&camif->slock, flags);
 406         vp->frame_sequence = 0;
 407         vp->state |= ST_VP_PENDING;
 408
 409         if (!list_empty(&vp->pending_buf_q) &&
 410             (!(vp->state & ST_VP_STREAMING) ||
 411              !(vp->state & ST_VP_SENSOR_STREAMING))) {
 412
 413                 camif_hw_enable_scaler(vp, vp->scaler.enable);
 414                 camif_hw_enable_capture(vp);
 415                 vp->state |= ST_VP_STREAMING;
 416
 417                 if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
 418                         vp->state |= ST_VP_SENSOR_STREAMING;
 419                         spin_unlock_irqrestore(&camif->slock, flags);
 420                         ret = sensor_set_streaming(camif, 1);
 421                         if (ret)
 422                                 v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
 423                         if (debug)
 424                                 camif_hw_dump_regs(camif, __func__);
 425
 426                         return ret;
 427                 }
 428         }
 429
 430         spin_unlock_irqrestore(&camif->slock, flags);
 431         return 0;
 432 }
 433
 434 static void stop_streaming(struct vb2_queue *vq)
 435 {
 436         struct camif_vp *vp = vb2_get_drv_priv(vq);
 437         camif_stop_capture(vp);
 438 }
 439
 440 static int queue_setup(struct vb2_queue *vq, const void *parg,
 441                        unsigned int *num_buffers, unsigned int *num_planes,
 442                        unsigned int sizes[], void *allocators[])
 443 {
 444         const struct v4l2_format *pfmt = parg;
 445         const struct v4l2_pix_format *pix = NULL;
 446         struct camif_vp *vp = vb2_get_drv_priv(vq);
 447         struct camif_dev *camif = vp->camif;
 448         struct camif_frame *frame = &vp->out_frame;
 449         const struct camif_fmt *fmt;
 450         unsigned int size;
 451
 452         if (pfmt) {
 453                 pix = &pfmt->fmt.pix;
 454                 fmt = s3c_camif_find_format(vp, &pix->pixelformat, -1);
 455                 if (fmt == NULL)
 456                         return -EINVAL;
 457                 size = (pix->width * pix->height * fmt->depth) / 8;
 458         } else {
 459                 fmt = vp->out_fmt;
 460                 if (fmt == NULL)
 461                         return -EINVAL;
 462                 size = (frame->f_width * frame->f_height * fmt->depth) / 8;
 463         }
 464
 465         *num_planes = 1;
 466
 467         if (pix)
 468                 sizes[0] = max(size, pix->sizeimage);
 469         else
 470                 sizes[0] = size;
 471         allocators[0] = camif->alloc_ctx;
 472
 473         pr_debug("size: %u\n", sizes[0]);
 474         return 0;
 475 }
 476
 477 static int buffer_prepare(struct vb2_buffer *vb)
 478 {
 479         struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
 480
 481         if (vp->out_fmt == NULL)
 482                 return -EINVAL;
 483
 484         if (vb2_plane_size(vb, 0) < vp->payload) {
 485                 v4l2_err(&vp->vdev, "buffer too small: %lu, required: %u\n",
 486                          vb2_plane_size(vb, 0), vp->payload);
 487                 return -EINVAL;
 488         }
 489         vb2_set_plane_payload(vb, 0, vp->payload);
 490
 491         return 0;
 492 }
 493
 494 static void buffer_queue(struct vb2_buffer *vb)
 495 {
 496         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 497         struct camif_buffer *buf = container_of(vbuf, struct camif_buffer, vb);
 498         struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
 499         struct camif_dev *camif = vp->camif;
 500         unsigned long flags;
 501
 502         spin_lock_irqsave(&camif->slock, flags);
 503         WARN_ON(camif_prepare_addr(vp, &buf->vb.vb2_buf, &buf->paddr));
 504
 505         if (!(vp->state & ST_VP_STREAMING) && vp->active_buffers < 2) {
 506                 /* Schedule an empty buffer in H/W */
 507                 buf->index = vp->buf_index;
 508
 509                 camif_hw_set_output_addr(vp, &buf->paddr, buf->index);
 510                 camif_hw_set_output_addr(vp, &buf->paddr, buf->index + 2);
 511
 512                 camif_active_queue_add(vp, buf);
 513                 vp->buf_index = !vp->buf_index;
 514         } else {
 515                 camif_pending_queue_add(vp, buf);
 516         }
 517
 518         if (vb2_is_streaming(&vp->vb_queue) && !list_empty(&vp->pending_buf_q)
 519                 && !(vp->state & ST_VP_STREAMING)) {
 520
 521                 vp->state |= ST_VP_STREAMING;
 522                 camif_hw_enable_scaler(vp, vp->scaler.enable);
 523                 camif_hw_enable_capture(vp);
 524                 spin_unlock_irqrestore(&camif->slock, flags);
 525
 526                 if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
 527                         if (sensor_set_streaming(camif, 1) == 0)
 528                                 vp->state |= ST_VP_SENSOR_STREAMING;
 529                         else
 530                                 v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
 531
 532                         if (debug)
 533                                 camif_hw_dump_regs(camif, __func__);
 534                 }
 535                 return;
 536         }
 537         spin_unlock_irqrestore(&camif->slock, flags);
 538 }
 539
 540 static const struct vb2_ops s3c_camif_qops = {
 541         .queue_setup     = queue_setup,
 542         .buf_prepare     = buffer_prepare,
 543         .buf_queue       = buffer_queue,
 544         .wait_prepare    = vb2_ops_wait_prepare,
 545         .wait_finish     = vb2_ops_wait_finish,
 546         .start_streaming = start_streaming,
 547         .stop_streaming  = stop_streaming,
 548 };
 549
 550 static int s3c_camif_open(struct file *file)
 551 {
 552         struct camif_vp *vp = video_drvdata(file);
 553         struct camif_dev *camif = vp->camif;
 554         int ret;
 555
 556         pr_debug("[vp%d] state: %#x,  owner: %p, pid: %d\n", vp->id,
 557                  vp->state, vp->owner, task_pid_nr(current));
 558
 559         if (mutex_lock_interruptible(&camif->lock))
 560                 return -ERESTARTSYS;
 561
 562         ret = v4l2_fh_open(file);
 563         if (ret < 0)
 564                 goto unlock;
 565
 566         ret = pm_runtime_get_sync(camif->dev);
 567         if (ret < 0)
 568                 goto err_pm;
 569
 570         ret = sensor_set_power(camif, 1);
 571         if (!ret)
 572                 goto unlock;
 573
 574         pm_runtime_put(camif->dev);
 575 err_pm:
 576         v4l2_fh_release(file);
 577 unlock:
 578         mutex_unlock(&camif->lock);
 579         return ret;
 580 }
 581
 582 static int s3c_camif_close(struct file *file)
 583 {
 584         struct camif_vp *vp = video_drvdata(file);
 585         struct camif_dev *camif = vp->camif;
 586         int ret;
 587
 588         pr_debug("[vp%d] state: %#x, owner: %p, pid: %d\n", vp->id,
 589                  vp->state, vp->owner, task_pid_nr(current));
 590
 591         mutex_lock(&camif->lock);
 592
 593         if (vp->owner == file->private_data) {
 594                 camif_stop_capture(vp);
 595                 vb2_queue_release(&vp->vb_queue);
 596                 vp->owner = NULL;
 597         }
 598
 599         sensor_set_power(camif, 0);
 600
 601         pm_runtime_put(camif->dev);
 602         ret = v4l2_fh_release(file);
 603
 604         mutex_unlock(&camif->lock);
 605         return ret;
 606 }
 607
 608 static unsigned int s3c_camif_poll(struct file *file,
 609                                    struct poll_table_struct *wait)
 610 {
 611         struct camif_vp *vp = video_drvdata(file);
 612         struct camif_dev *camif = vp->camif;
 613         int ret;
 614
 615         mutex_lock(&camif->lock);
 616         if (vp->owner && vp->owner != file->private_data)
 617                 ret = -EBUSY;
 618         else
 619                 ret = vb2_poll(&vp->vb_queue, file, wait);
 620
 621         mutex_unlock(&camif->lock);
 622         return ret;
 623 }
 624
 625 static int s3c_camif_mmap(struct file *file, struct vm_area_struct *vma)
 626 {
 627         struct camif_vp *vp = video_drvdata(file);
 628         int ret;
 629
 630         if (vp->owner && vp->owner != file->private_data)
 631                 ret = -EBUSY;
 632         else
 633                 ret = vb2_mmap(&vp->vb_queue, vma);
 634
 635         return ret;
 636 }
 637
 638 static const struct v4l2_file_operations s3c_camif_fops = {
 639         .owner          = THIS_MODULE,
 640         .open           = s3c_camif_open,
 641         .release        = s3c_camif_close,
 642         .poll           = s3c_camif_poll,
 643         .unlocked_ioctl = video_ioctl2,
 644         .mmap           = s3c_camif_mmap,
 645 };
 646
 647 /*
 648  * Video node IOCTLs
 649  */
 650
 651 static int s3c_camif_vidioc_querycap(struct file *file, void *priv,
 652                                      struct v4l2_capability *cap)
 653 {
 654         struct camif_vp *vp = video_drvdata(file);
 655
 656         strlcpy(cap->driver, S3C_CAMIF_DRIVER_NAME, sizeof(cap->driver));
 657         strlcpy(cap->card, S3C_CAMIF_DRIVER_NAME, sizeof(cap->card));
 658         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s.%d",
 659                  dev_name(vp->camif->dev), vp->id);
 660
 661         cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
 662         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 663
 664         return 0;
 665 }
 666
 667 static int s3c_camif_vidioc_enum_input(struct file *file, void *priv,
 668                                        struct v4l2_input *input)
 669 {
 670         struct camif_vp *vp = video_drvdata(file);
 671         struct v4l2_subdev *sensor = vp->camif->sensor.sd;
 672
 673         if (input->index || sensor == NULL)
 674                 return -EINVAL;
 675
 676         input->type = V4L2_INPUT_TYPE_CAMERA;
 677         strlcpy(input->name, sensor->name, sizeof(input->name));
 678         return 0;
 679 }
 680
 681 static int s3c_camif_vidioc_s_input(struct file *file, void *priv,
 682                                     unsigned int i)
 683 {
 684         return i == 0 ? 0 : -EINVAL;
 685 }
 686
 687 static int s3c_camif_vidioc_g_input(struct file *file, void *priv,
 688                                     unsigned int *i)
 689 {
 690         *i = 0;
 691         return 0;
 692 }
 693
 694 static int s3c_camif_vidioc_enum_fmt(struct file *file, void *priv,
 695                                      struct v4l2_fmtdesc *f)
 696 {
 697         struct camif_vp *vp = video_drvdata(file);
 698         const struct camif_fmt *fmt;
 699
 700         fmt = s3c_camif_find_format(vp, NULL, f->index);
 701         if (!fmt)
 702                 return -EINVAL;
 703
 704         strlcpy(f->description, fmt->name, sizeof(f->description));
 705         f->pixelformat = fmt->fourcc;
 706
 707         pr_debug("fmt(%d): %s\n", f->index, f->description);
 708         return 0;
 709 }
 710
 711 static int s3c_camif_vidioc_g_fmt(struct file *file, void *priv,
 712                                   struct v4l2_format *f)
 713 {
 714         struct camif_vp *vp = video_drvdata(file);
 715         struct v4l2_pix_format *pix = &f->fmt.pix;
 716         struct camif_frame *frame = &vp->out_frame;
 717         const struct camif_fmt *fmt = vp->out_fmt;
 718
 719         pix->bytesperline = frame->f_width * fmt->ybpp;
 720         pix->sizeimage = vp->payload;
 721
 722         pix->pixelformat = fmt->fourcc;
 723         pix->width = frame->f_width;
 724         pix->height = frame->f_height;
 725         pix->field = V4L2_FIELD_NONE;
 726         pix->colorspace = V4L2_COLORSPACE_JPEG;
 727
 728         return 0;
 729 }
 730
 731 static int __camif_video_try_format(struct camif_vp *vp,
 732                                     struct v4l2_pix_format *pix,
 733                                     const struct camif_fmt **ffmt)
 734 {
 735         struct camif_dev *camif = vp->camif;
 736         struct v4l2_rect *crop = &camif->camif_crop;
 737         unsigned int wmin, hmin, sc_hrmax, sc_vrmax;
 738         const struct vp_pix_limits *pix_lim;
 739         const struct camif_fmt *fmt;
 740
 741         fmt = s3c_camif_find_format(vp, &pix->pixelformat, 0);
 742
 743         if (WARN_ON(fmt == NULL))
 744                 return -EINVAL;
 745
 746         if (ffmt)
 747                 *ffmt = fmt;
 748
 749         pix_lim = &camif->variant->vp_pix_limits[vp->id];
 750
 751         pr_debug("fmt: %ux%u, crop: %ux%u, bytesperline: %u\n",
 752                  pix->width, pix->height, crop->width, crop->height,
 753                  pix->bytesperline);
 754         /*
 755          * Calculate minimum width and height according to the configured
 756          * camera input interface crop rectangle and the resizer's capabilities.
 757          */
 758         sc_hrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->width) - 3));
 759         sc_vrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->height) - 1));
 760
 761         wmin = max_t(u32, pix_lim->min_out_width, crop->width / sc_hrmax);
 762         wmin = round_up(wmin, pix_lim->out_width_align);
 763         hmin = max_t(u32, 8, crop->height / sc_vrmax);
 764         hmin = round_up(hmin, 8);
 765
 766         v4l_bound_align_image(&pix->width, wmin, pix_lim->max_sc_out_width,
 767                               ffs(pix_lim->out_width_align) - 1,
 768                               &pix->height, hmin, pix_lim->max_height, 0, 0);
 769
 770         pix->bytesperline = pix->width * fmt->ybpp;
 771         pix->sizeimage = (pix->width * pix->height * fmt->depth) / 8;
 772         pix->pixelformat = fmt->fourcc;
 773         pix->colorspace = V4L2_COLORSPACE_JPEG;
 774         pix->field = V4L2_FIELD_NONE;
 775
 776         pr_debug("%ux%u, wmin: %d, hmin: %d, sc_hrmax: %d, sc_vrmax: %d\n",
 777                  pix->width, pix->height, wmin, hmin, sc_hrmax, sc_vrmax);
 778
 779         return 0;
 780 }
 781
 782 static int s3c_camif_vidioc_try_fmt(struct file *file, void *priv,
 783                                     struct v4l2_format *f)
 784 {
 785         struct camif_vp *vp = video_drvdata(file);
 786         return __camif_video_try_format(vp, &f->fmt.pix, NULL);
 787 }
 788
 789 static int s3c_camif_vidioc_s_fmt(struct file *file, void *priv,
 790                                   struct v4l2_format *f)
 791 {
 792         struct v4l2_pix_format *pix = &f->fmt.pix;
 793         struct camif_vp *vp = video_drvdata(file);
 794         struct camif_frame *out_frame = &vp->out_frame;
 795         const struct camif_fmt *fmt = NULL;
 796         int ret;
 797
 798         pr_debug("[vp%d]\n", vp->id);
 799
 800         if (vb2_is_busy(&vp->vb_queue))
 801                 return -EBUSY;
 802
 803         ret = __camif_video_try_format(vp, &f->fmt.pix, &fmt);
 804         if (ret < 0)
 805                 return ret;
 806
 807         vp->out_fmt = fmt;
 808         vp->payload = pix->sizeimage;
 809         out_frame->f_width = pix->width;
 810         out_frame->f_height = pix->height;
 811
 812         /* Reset composition rectangle */
 813         out_frame->rect.width = pix->width;
 814         out_frame->rect.height = pix->height;
 815         out_frame->rect.left = 0;
 816         out_frame->rect.top = 0;
 817
 818         if (vp->owner == NULL)
 819                 vp->owner = priv;
 820
 821         pr_debug("%ux%u. payload: %u. fmt: %s. %d %d. sizeimage: %d. bpl: %d\n",
 822                 out_frame->f_width, out_frame->f_height, vp->payload, fmt->name,
 823                 pix->width * pix->height * fmt->depth, fmt->depth,
 824                 pix->sizeimage, pix->bytesperline);
 825
 826         return 0;
 827 }
 828
 829 /* Only check pixel formats at the sensor and the camif subdev pads */
 830 static int camif_pipeline_validate(struct camif_dev *camif)
 831 {
 832         struct v4l2_subdev_format src_fmt;
 833         struct media_pad *pad;
 834         int ret;
 835
 836         /* Retrieve format at the sensor subdev source pad */
 837         pad = media_entity_remote_pad(&camif->pads[0]);
 838         if (!pad || media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 839                 return -EPIPE;
 840
 841         src_fmt.pad = pad->index;
 842         src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 843         ret = v4l2_subdev_call(camif->sensor.sd, pad, get_fmt, NULL, &src_fmt);
 844         if (ret < 0 && ret != -ENOIOCTLCMD)
 845                 return -EPIPE;
 846
 847         if (src_fmt.format.width != camif->mbus_fmt.width ||
 848             src_fmt.format.height != camif->mbus_fmt.height ||
 849             src_fmt.format.code != camif->mbus_fmt.code)
 850                 return -EPIPE;
 851
 852         return 0;
 853 }
 854
 855 static int s3c_camif_streamon(struct file *file, void *priv,
 856                               enum v4l2_buf_type type)
 857 {
 858         struct camif_vp *vp = video_drvdata(file);
 859         struct camif_dev *camif = vp->camif;
 860         struct media_entity *sensor = &camif->sensor.sd->entity;
 861         int ret;
 862
 863         pr_debug("[vp%d]\n", vp->id);
 864
 865         if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 866                 return -EINVAL;
 867
 868         if (vp->owner && vp->owner != priv)
 869                 return -EBUSY;
 870
 871         if (s3c_vp_active(vp))
 872                 return 0;
 873
 874         ret = media_entity_pipeline_start(sensor, camif->m_pipeline);
 875         if (ret < 0)
 876                 return ret;
 877
 878         ret = camif_pipeline_validate(camif);
 879         if (ret < 0) {
 880                 media_entity_pipeline_stop(sensor);
 881                 return ret;
 882         }
 883
 884         return vb2_streamon(&vp->vb_queue, type);
 885 }
 886
 887 static int s3c_camif_streamoff(struct file *file, void *priv,
 888                                enum v4l2_buf_type type)
 889 {
 890         struct camif_vp *vp = video_drvdata(file);
 891         struct camif_dev *camif = vp->camif;
 892         int ret;
 893
 894         pr_debug("[vp%d]\n", vp->id);
 895
 896         if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 897                 return -EINVAL;
 898
 899         if (vp->owner && vp->owner != priv)
 900                 return -EBUSY;
 901
 902         ret = vb2_streamoff(&vp->vb_queue, type);
 903         if (ret == 0)
 904                 media_entity_pipeline_stop(&camif->sensor.sd->entity);
 905         return ret;
 906 }
 907
 908 static int s3c_camif_reqbufs(struct file *file, void *priv,
 909                              struct v4l2_requestbuffers *rb)
 910 {
 911         struct camif_vp *vp = video_drvdata(file);
 912         int ret;
 913
 914         pr_debug("[vp%d] rb count: %d, owner: %p, priv: %p\n",
 915                  vp->id, rb->count, vp->owner, priv);
 916
 917         if (vp->owner && vp->owner != priv)
 918                 return -EBUSY;
 919
 920         if (rb->count)
 921                 rb->count = max_t(u32, CAMIF_REQ_BUFS_MIN, rb->count);
 922         else
 923                 vp->owner = NULL;
 924
 925         ret = vb2_reqbufs(&vp->vb_queue, rb);
 926         if (ret < 0)
 927                 return ret;
 928
 929         if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
 930                 rb->count = 0;
 931                 vb2_reqbufs(&vp->vb_queue, rb);
 932                 ret = -ENOMEM;
 933         }
 934
 935         vp->reqbufs_count = rb->count;
 936         if (vp->owner == NULL && rb->count > 0)
 937                 vp->owner = priv;
 938
 939         return ret;
 940 }
 941
 942 static int s3c_camif_querybuf(struct file *file, void *priv,
 943                               struct v4l2_buffer *buf)
 944 {
 945         struct camif_vp *vp = video_drvdata(file);
 946         return vb2_querybuf(&vp->vb_queue, buf);
 947 }
 948
 949 static int s3c_camif_qbuf(struct file *file, void *priv,
 950                           struct v4l2_buffer *buf)
 951 {
 952         struct camif_vp *vp = video_drvdata(file);
 953
 954         pr_debug("[vp%d]\n", vp->id);
 955
 956         if (vp->owner && vp->owner != priv)
 957                 return -EBUSY;
 958
 959         return vb2_qbuf(&vp->vb_queue, buf);
 960 }
 961
 962 static int s3c_camif_dqbuf(struct file *file, void *priv,
 963                            struct v4l2_buffer *buf)
 964 {
 965         struct camif_vp *vp = video_drvdata(file);
 966
 967         pr_debug("[vp%d] sequence: %d\n", vp->id, vp->frame_sequence);
 968
 969         if (vp->owner && vp->owner != priv)
 970                 return -EBUSY;
 971
 972         return vb2_dqbuf(&vp->vb_queue, buf, file->f_flags & O_NONBLOCK);
 973 }
 974
 975 static int s3c_camif_create_bufs(struct file *file, void *priv,
 976                                  struct v4l2_create_buffers *create)
 977 {
 978         struct camif_vp *vp = video_drvdata(file);
 979         int ret;
 980
 981         if (vp->owner && vp->owner != priv)
 982                 return -EBUSY;
 983
 984         create->count = max_t(u32, 1, create->count);
 985         ret = vb2_create_bufs(&vp->vb_queue, create);
 986
 987         if (!ret && vp->owner == NULL)
 988                 vp->owner = priv;
 989
 990         return ret;
 991 }
 992
 993 static int s3c_camif_prepare_buf(struct file *file, void *priv,
 994                                  struct v4l2_buffer *b)
 995 {
 996         struct camif_vp *vp = video_drvdata(file);
 997         return vb2_prepare_buf(&vp->vb_queue, b);
 998 }
 999
1000 static int s3c_camif_g_selection(struct file *file, void *priv,
1001                                  struct v4l2_selection *sel)
1002 {
1003         struct camif_vp *vp = video_drvdata(file);
1004
1005         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1006                 return -EINVAL;
1007
1008         switch (sel->target) {
1009         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1010         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1011                 sel->r.left = 0;
1012                 sel->r.top = 0;
1013                 sel->r.width = vp->out_frame.f_width;
1014                 sel->r.height = vp->out_frame.f_height;
1015                 return 0;
1016
1017         case V4L2_SEL_TGT_COMPOSE:
1018                 sel->r = vp->out_frame.rect;
1019                 return 0;
1020         }
1021
1022         return -EINVAL;
1023 }
1024
1025 static void __camif_try_compose(struct camif_dev *camif, struct camif_vp *vp,
1026                                 struct v4l2_rect *r)
1027 {
1028         /* s3c244x doesn't support composition */
1029         if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV) {
1030                 *r = vp->out_frame.rect;
1031                 return;
1032         }
1033
1034         /* TODO: s3c64xx */
1035 }
1036
1037 static int s3c_camif_s_selection(struct file *file, void *priv,
1038                                  struct v4l2_selection *sel)
1039 {
1040         struct camif_vp *vp = video_drvdata(file);
1041         struct camif_dev *camif = vp->camif;
1042         struct v4l2_rect rect = sel->r;
1043         unsigned long flags;
1044
1045         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1046             sel->target != V4L2_SEL_TGT_COMPOSE)
1047                 return -EINVAL;
1048
1049         __camif_try_compose(camif, vp, &rect);
1050
1051         sel->r = rect;
1052         spin_lock_irqsave(&camif->slock, flags);
1053         vp->out_frame.rect = rect;
1054         vp->state |= ST_VP_CONFIG;
1055         spin_unlock_irqrestore(&camif->slock, flags);
1056
1057         pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%dx%d\n",
1058                 sel->type, sel->target, sel->flags,
1059                 sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1060
1061         return 0;
1062 }
1063
1064 static const struct v4l2_ioctl_ops s3c_camif_ioctl_ops = {
1065         .vidioc_querycap          = s3c_camif_vidioc_querycap,
1066         .vidioc_enum_input        = s3c_camif_vidioc_enum_input,
1067         .vidioc_g_input           = s3c_camif_vidioc_g_input,
1068         .vidioc_s_input           = s3c_camif_vidioc_s_input,
1069         .vidioc_enum_fmt_vid_cap  = s3c_camif_vidioc_enum_fmt,
1070         .vidioc_try_fmt_vid_cap   = s3c_camif_vidioc_try_fmt,
1071         .vidioc_s_fmt_vid_cap     = s3c_camif_vidioc_s_fmt,
1072         .vidioc_g_fmt_vid_cap     = s3c_camif_vidioc_g_fmt,
1073         .vidioc_g_selection       = s3c_camif_g_selection,
1074         .vidioc_s_selection       = s3c_camif_s_selection,
1075         .vidioc_reqbufs           = s3c_camif_reqbufs,
1076         .vidioc_querybuf          = s3c_camif_querybuf,
1077         .vidioc_prepare_buf       = s3c_camif_prepare_buf,
1078         .vidioc_create_bufs       = s3c_camif_create_bufs,
1079         .vidioc_qbuf              = s3c_camif_qbuf,
1080         .vidioc_dqbuf             = s3c_camif_dqbuf,
1081         .vidioc_streamon          = s3c_camif_streamon,
1082         .vidioc_streamoff         = s3c_camif_streamoff,
1083         .vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
1084         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1085         .vidioc_log_status        = v4l2_ctrl_log_status,
1086 };
1087
1088 /*
1089  * Video node controls
1090  */
1091 static int s3c_camif_video_s_ctrl(struct v4l2_ctrl *ctrl)
1092 {
1093         struct camif_vp *vp = ctrl->priv;
1094         struct camif_dev *camif = vp->camif;
1095         unsigned long flags;
1096
1097         pr_debug("[vp%d] ctrl: %s, value: %d\n", vp->id,
1098                  ctrl->name, ctrl->val);
1099
1100         spin_lock_irqsave(&camif->slock, flags);
1101
1102         switch (ctrl->id) {
1103         case V4L2_CID_HFLIP:
1104                 vp->hflip = ctrl->val;
1105                 break;
1106
1107         case V4L2_CID_VFLIP:
1108                 vp->vflip = ctrl->val;
1109                 break;
1110         }
1111
1112         vp->state |= ST_VP_CONFIG;
1113         spin_unlock_irqrestore(&camif->slock, flags);
1114         return 0;
1115 }
1116
1117 /* Codec and preview video node control ops */
1118 static const struct v4l2_ctrl_ops s3c_camif_video_ctrl_ops = {
1119         .s_ctrl = s3c_camif_video_s_ctrl,
1120 };
1121
1122 int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
1123 {
1124         struct camif_vp *vp = &camif->vp[idx];
1125         struct vb2_queue *q = &vp->vb_queue;
1126         struct video_device *vfd = &vp->vdev;
1127         struct v4l2_ctrl *ctrl;
1128         int ret;
1129
1130         memset(vfd, 0, sizeof(*vfd));
1131         snprintf(vfd->name, sizeof(vfd->name), "camif-%s",
1132                  vp->id == 0 ? "codec" : "preview");
1133
1134         vfd->fops = &s3c_camif_fops;
1135         vfd->ioctl_ops = &s3c_camif_ioctl_ops;
1136         vfd->v4l2_dev = &camif->v4l2_dev;
1137         vfd->minor = -1;
1138         vfd->release = video_device_release_empty;
1139         vfd->lock = &camif->lock;
1140         vp->reqbufs_count = 0;
1141
1142         INIT_LIST_HEAD(&vp->pending_buf_q);
1143         INIT_LIST_HEAD(&vp->active_buf_q);
1144
1145         memset(q, 0, sizeof(*q));
1146         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1147         q->io_modes = VB2_MMAP | VB2_USERPTR;
1148         q->ops = &s3c_camif_qops;
1149         q->mem_ops = &vb2_dma_contig_memops;
1150         q->buf_struct_size = sizeof(struct camif_buffer);
1151         q->drv_priv = vp;
1152         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1153         q->lock = &vp->camif->lock;
1154
1155         ret = vb2_queue_init(q);
1156         if (ret)
1157                 goto err_vd_rel;
1158
1159         vp->pad.flags = MEDIA_PAD_FL_SINK;
1160         ret = media_entity_init(&vfd->entity, 1, &vp->pad, 0);
1161         if (ret)
1162                 goto err_vd_rel;
1163
1164         video_set_drvdata(vfd, vp);
1165
1166         v4l2_ctrl_handler_init(&vp->ctrl_handler, 1);
1167         ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1168                                  V4L2_CID_HFLIP, 0, 1, 1, 0);
1169         if (ctrl)
1170                 ctrl->priv = vp;
1171         ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1172                                  V4L2_CID_VFLIP, 0, 1, 1, 0);
1173         if (ctrl)
1174                 ctrl->priv = vp;
1175
1176         ret = vp->ctrl_handler.error;
1177         if (ret < 0)
1178                 goto err_me_cleanup;
1179
1180         vfd->ctrl_handler = &vp->ctrl_handler;
1181
1182         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
1183         if (ret)
1184                 goto err_ctrlh_free;
1185
1186         v4l2_info(&camif->v4l2_dev, "registered %s as /dev/%s\n",
1187                   vfd->name, video_device_node_name(vfd));
1188         return 0;
1189
1190 err_ctrlh_free:
1191         v4l2_ctrl_handler_free(&vp->ctrl_handler);
1192 err_me_cleanup:
1193         media_entity_cleanup(&vfd->entity);
1194 err_vd_rel:
1195         video_device_release(vfd);
1196         return ret;
1197 }
1198
1199 void s3c_camif_unregister_video_node(struct camif_dev *camif, int idx)
1200 {
1201         struct video_device *vfd = &camif->vp[idx].vdev;
1202
1203         if (video_is_registered(vfd)) {
1204                 video_unregister_device(vfd);
1205                 media_entity_cleanup(&vfd->entity);
1206                 v4l2_ctrl_handler_free(vfd->ctrl_handler);
1207         }
1208 }
1209
1210 /* Media bus pixel formats supported at the camif input */
1211 static const u32 camif_mbus_formats[] = {
1212         MEDIA_BUS_FMT_YUYV8_2X8,
1213         MEDIA_BUS_FMT_YVYU8_2X8,
1214         MEDIA_BUS_FMT_UYVY8_2X8,
1215         MEDIA_BUS_FMT_VYUY8_2X8,
1216 };
1217
1218 /*
1219  *  Camera input interface subdev operations
1220  */
1221
1222 static int s3c_camif_subdev_enum_mbus_code(struct v4l2_subdev *sd,
1223                                         struct v4l2_subdev_pad_config *cfg,
1224                                         struct v4l2_subdev_mbus_code_enum *code)
1225 {
1226         if (code->index >= ARRAY_SIZE(camif_mbus_formats))
1227                 return -EINVAL;
1228
1229         code->code = camif_mbus_formats[code->index];
1230         return 0;
1231 }
1232
1233 static int s3c_camif_subdev_get_fmt(struct v4l2_subdev *sd,
1234                                     struct v4l2_subdev_pad_config *cfg,
1235                                     struct v4l2_subdev_format *fmt)
1236 {
1237         struct camif_dev *camif = v4l2_get_subdevdata(sd);
1238         struct v4l2_mbus_framefmt *mf = &fmt->format;
1239
1240         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1241                 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1242                 fmt->format = *mf;
1243                 return 0;
1244         }
1245
1246         mutex_lock(&camif->lock);
1247
1248         switch (fmt->pad) {
1249         case CAMIF_SD_PAD_SINK:
1250                 /* full camera input pixel size */
1251                 *mf = camif->mbus_fmt;
1252                 break;
1253
1254         case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1255                 /* crop rectangle at camera interface input */
1256                 mf->width = camif->camif_crop.width;
1257                 mf->height = camif->camif_crop.height;
1258                 mf->code = camif->mbus_fmt.code;
1259                 break;
1260         }
1261
1262         mutex_unlock(&camif->lock);
1263         mf->field = V4L2_FIELD_NONE;
1264         mf->colorspace = V4L2_COLORSPACE_JPEG;
1265         return 0;
1266 }
1267
1268 static void __camif_subdev_try_format(struct camif_dev *camif,
1269                                 struct v4l2_mbus_framefmt *mf, int pad)
1270 {
1271         const struct s3c_camif_variant *variant = camif->variant;
1272         const struct vp_pix_limits *pix_lim;
1273         unsigned int i;
1274
1275         /* FIXME: constraints against codec or preview path ? */
1276         pix_lim = &variant->vp_pix_limits[VP_CODEC];
1277
1278         for (i = 0; i < ARRAY_SIZE(camif_mbus_formats); i++)
1279                 if (camif_mbus_formats[i] == mf->code)
1280                         break;
1281
1282         if (i == ARRAY_SIZE(camif_mbus_formats))
1283                 mf->code = camif_mbus_formats[0];
1284
1285         if (pad == CAMIF_SD_PAD_SINK) {
1286                 v4l_bound_align_image(&mf->width, 8, CAMIF_MAX_PIX_WIDTH,
1287                                       ffs(pix_lim->out_width_align) - 1,
1288                                       &mf->height, 8, CAMIF_MAX_PIX_HEIGHT, 0,
1289                                       0);
1290         } else {
1291                 struct v4l2_rect *crop = &camif->camif_crop;
1292                 v4l_bound_align_image(&mf->width, 8, crop->width,
1293                                       ffs(pix_lim->out_width_align) - 1,
1294                                       &mf->height, 8, crop->height,
1295                                       0, 0);
1296         }
1297
1298         v4l2_dbg(1, debug, &camif->subdev, "%ux%u\n", mf->width, mf->height);
1299 }
1300
1301 static int s3c_camif_subdev_set_fmt(struct v4l2_subdev *sd,
1302                                     struct v4l2_subdev_pad_config *cfg,
1303                                     struct v4l2_subdev_format *fmt)
1304 {
1305         struct camif_dev *camif = v4l2_get_subdevdata(sd);
1306         struct v4l2_mbus_framefmt *mf = &fmt->format;
1307         struct v4l2_rect *crop = &camif->camif_crop;
1308         int i;
1309
1310         v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %ux%u\n",
1311                  fmt->pad, mf->code, mf->width, mf->height);
1312
1313         mf->field = V4L2_FIELD_NONE;
1314         mf->colorspace = V4L2_COLORSPACE_JPEG;
1315         mutex_lock(&camif->lock);
1316
1317         /*
1318          * No pixel format change at the camera input is allowed
1319          * while streaming.
1320          */
1321         if (vb2_is_busy(&camif->vp[VP_CODEC].vb_queue) ||
1322             vb2_is_busy(&camif->vp[VP_PREVIEW].vb_queue)) {
1323                 mutex_unlock(&camif->lock);
1324                 return -EBUSY;
1325         }
1326
1327         __camif_subdev_try_format(camif, mf, fmt->pad);
1328
1329         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1330                 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1331                 *mf = fmt->format;
1332                 mutex_unlock(&camif->lock);
1333                 return 0;
1334         }
1335
1336         switch (fmt->pad) {
1337         case CAMIF_SD_PAD_SINK:
1338                 camif->mbus_fmt = *mf;
1339                 /* Reset sink crop rectangle. */
1340                 crop->width = mf->width;
1341                 crop->height = mf->height;
1342                 crop->left = 0;
1343                 crop->top = 0;
1344                 /*
1345                  * Reset source format (the camif's crop rectangle)
1346                  * and the video output resolution.
1347                  */
1348                 for (i = 0; i < CAMIF_VP_NUM; i++) {
1349                         struct camif_frame *frame = &camif->vp[i].out_frame;
1350                         frame->rect = *crop;
1351                         frame->f_width = mf->width;
1352                         frame->f_height = mf->height;
1353                 }
1354                 break;
1355
1356         case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1357                 /* Pixel format can be only changed on the sink pad. */
1358                 mf->code = camif->mbus_fmt.code;
1359                 mf->width = crop->width;
1360                 mf->height = crop->height;
1361                 break;
1362         }
1363
1364         mutex_unlock(&camif->lock);
1365         return 0;
1366 }
1367
1368 static int s3c_camif_subdev_get_selection(struct v4l2_subdev *sd,
1369                                           struct v4l2_subdev_pad_config *cfg,
1370                                           struct v4l2_subdev_selection *sel)
1371 {
1372         struct camif_dev *camif = v4l2_get_subdevdata(sd);
1373         struct v4l2_rect *crop = &camif->camif_crop;
1374         struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1375
1376         if ((sel->target != V4L2_SEL_TGT_CROP &&
1377             sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
1378             sel->pad != CAMIF_SD_PAD_SINK)
1379                 return -EINVAL;
1380
1381         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1382                 sel->r = *v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
1383                 return 0;
1384         }
1385
1386         mutex_lock(&camif->lock);
1387
1388         if (sel->target == V4L2_SEL_TGT_CROP) {
1389                 sel->r = *crop;
1390         } else { /* crop bounds */
1391                 sel->r.width = mf->width;
1392                 sel->r.height = mf->height;
1393                 sel->r.left = 0;
1394                 sel->r.top = 0;
1395         }
1396
1397         mutex_unlock(&camif->lock);
1398
1399         v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d) %dx%d, size: %ux%u\n",
1400                  __func__, crop->left, crop->top, crop->width,
1401                  crop->height, mf->width, mf->height);
1402
1403         return 0;
1404 }
1405
1406 static void __camif_try_crop(struct camif_dev *camif, struct v4l2_rect *r)
1407 {
1408         struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1409         const struct camif_pix_limits *pix_lim = &camif->variant->pix_limits;
1410         unsigned int left = 2 * r->left;
1411         unsigned int top = 2 * r->top;
1412
1413         /*
1414          * Following constraints must be met:
1415          *  - r->width + 2 * r->left = mf->width;
1416          *  - r->height + 2 * r->top = mf->height;
1417          *  - crop rectangle size and position must be aligned
1418          *    to 8 or 2 pixels, depending on SoC version.
1419          */
1420         v4l_bound_align_image(&r->width, 0, mf->width,
1421                               ffs(pix_lim->win_hor_offset_align) - 1,
1422                               &r->height, 0, mf->height, 1, 0);
1423
1424         v4l_bound_align_image(&left, 0, mf->width - r->width,
1425                               ffs(pix_lim->win_hor_offset_align),
1426                               &top, 0, mf->height - r->height, 2, 0);
1427
1428         r->left = left / 2;
1429         r->top = top / 2;
1430         r->width = mf->width - left;
1431         r->height = mf->height - top;
1432         /*
1433          * Make sure we either downscale or upscale both the pixel
1434          * width and height. Just return current crop rectangle if
1435          * this scaler constraint is not met.
1436          */
1437         if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV &&
1438             camif_is_streaming(camif)) {
1439                 unsigned int i;
1440
1441                 for (i = 0; i < CAMIF_VP_NUM; i++) {
1442                         struct v4l2_rect *or = &camif->vp[i].out_frame.rect;
1443                         if ((or->width > r->width) == (or->height > r->height))
1444                                 continue;
1445                         *r = camif->camif_crop;
1446                         pr_debug("Width/height scaling direction limitation\n");
1447                         break;
1448                 }
1449         }
1450
1451         v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%dx%d, fmt: %ux%u\n",
1452                  r->left, r->top, r->width, r->height, mf->width, mf->height);
1453 }
1454
1455 static int s3c_camif_subdev_set_selection(struct v4l2_subdev *sd,
1456                                           struct v4l2_subdev_pad_config *cfg,
1457                                           struct v4l2_subdev_selection *sel)
1458 {
1459         struct camif_dev *camif = v4l2_get_subdevdata(sd);
1460         struct v4l2_rect *crop = &camif->camif_crop;
1461         struct camif_scaler scaler;
1462
1463         if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != CAMIF_SD_PAD_SINK)
1464                 return -EINVAL;
1465
1466         mutex_lock(&camif->lock);
1467         __camif_try_crop(camif, &sel->r);
1468
1469         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1470                 *v4l2_subdev_get_try_crop(sd, cfg, sel->pad) = sel->r;
1471         } else {
1472                 unsigned long flags;
1473                 unsigned int i;
1474
1475                 spin_lock_irqsave(&camif->slock, flags);
1476                 *crop = sel->r;
1477
1478                 for (i = 0; i < CAMIF_VP_NUM; i++) {
1479                         struct camif_vp *vp = &camif->vp[i];
1480                         scaler = vp->scaler;
1481                         if (s3c_camif_get_scaler_config(vp, &scaler))
1482                                 continue;
1483                         vp->scaler = scaler;
1484                         vp->state |= ST_VP_CONFIG;
1485                 }
1486
1487                 spin_unlock_irqrestore(&camif->slock, flags);
1488         }
1489         mutex_unlock(&camif->lock);
1490
1491         v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %u, f_h: %u\n",
1492                  __func__, crop->left, crop->top, crop->width, crop->height,
1493                  camif->mbus_fmt.width, camif->mbus_fmt.height);
1494
1495         return 0;
1496 }
1497
1498 static const struct v4l2_subdev_pad_ops s3c_camif_subdev_pad_ops = {
1499         .enum_mbus_code = s3c_camif_subdev_enum_mbus_code,
1500         .get_selection = s3c_camif_subdev_get_selection,
1501         .set_selection = s3c_camif_subdev_set_selection,
1502         .get_fmt = s3c_camif_subdev_get_fmt,
1503         .set_fmt = s3c_camif_subdev_set_fmt,
1504 };
1505
1506 static struct v4l2_subdev_ops s3c_camif_subdev_ops = {
1507         .pad = &s3c_camif_subdev_pad_ops,
1508 };
1509
1510 static int s3c_camif_subdev_s_ctrl(struct v4l2_ctrl *ctrl)
1511 {
1512         struct camif_dev *camif = container_of(ctrl->handler, struct camif_dev,
1513                                                ctrl_handler);
1514         unsigned long flags;
1515
1516         spin_lock_irqsave(&camif->slock, flags);
1517
1518         switch (ctrl->id) {
1519         case V4L2_CID_COLORFX:
1520                 camif->colorfx = camif->ctrl_colorfx->val;
1521                 /* Set Cb, Cr */
1522                 switch (ctrl->val) {
1523                 case V4L2_COLORFX_SEPIA:
1524                         camif->colorfx_cb = 115;
1525                         camif->colorfx_cr = 145;
1526                         break;
1527                 case V4L2_COLORFX_SET_CBCR:
1528                         camif->colorfx_cb = camif->ctrl_colorfx_cbcr->val >> 8;
1529                         camif->colorfx_cr = camif->ctrl_colorfx_cbcr->val & 0xff;
1530                         break;
1531                 default:
1532                         /* for V4L2_COLORFX_BW and others */
1533                         camif->colorfx_cb = 128;
1534                         camif->colorfx_cr = 128;
1535                 }
1536                 break;
1537         case V4L2_CID_TEST_PATTERN:
1538                 camif->test_pattern = camif->ctrl_test_pattern->val;
1539                 break;
1540         default:
1541                 WARN_ON(1);
1542         }
1543
1544         camif->vp[VP_CODEC].state |= ST_VP_CONFIG;
1545         camif->vp[VP_PREVIEW].state |= ST_VP_CONFIG;
1546         spin_unlock_irqrestore(&camif->slock, flags);
1547
1548         return 0;
1549 }
1550
1551 static const struct v4l2_ctrl_ops s3c_camif_subdev_ctrl_ops = {
1552         .s_ctrl = s3c_camif_subdev_s_ctrl,
1553 };
1554
1555 static const char * const s3c_camif_test_pattern_menu[] = {
1556         "Disabled",
1557         "Color bars",
1558         "Horizontal increment",
1559         "Vertical increment",
1560 };
1561
1562 int s3c_camif_create_subdev(struct camif_dev *camif)
1563 {
1564         struct v4l2_ctrl_handler *handler = &camif->ctrl_handler;
1565         struct v4l2_subdev *sd = &camif->subdev;
1566         int ret;
1567
1568         v4l2_subdev_init(sd, &s3c_camif_subdev_ops);
1569         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1570         strlcpy(sd->name, "S3C-CAMIF", sizeof(sd->name));
1571
1572         camif->pads[CAMIF_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1573         camif->pads[CAMIF_SD_PAD_SOURCE_C].flags = MEDIA_PAD_FL_SOURCE;
1574         camif->pads[CAMIF_SD_PAD_SOURCE_P].flags = MEDIA_PAD_FL_SOURCE;
1575
1576         ret = media_entity_init(&sd->entity, CAMIF_SD_PADS_NUM,
1577                                 camif->pads, 0);
1578         if (ret)
1579                 return ret;
1580
1581         v4l2_ctrl_handler_init(handler, 3);
1582         camif->ctrl_test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1583                         &s3c_camif_subdev_ctrl_ops, V4L2_CID_TEST_PATTERN,
1584                         ARRAY_SIZE(s3c_camif_test_pattern_menu) - 1, 0, 0,
1585                         s3c_camif_test_pattern_menu);
1586
1587         if (camif->variant->has_img_effect) {
1588                 camif->ctrl_colorfx = v4l2_ctrl_new_std_menu(handler,
1589                                 &s3c_camif_subdev_ctrl_ops,
1590                                 V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
1591                                 ~0x981f, V4L2_COLORFX_NONE);
1592
1593                 camif->ctrl_colorfx_cbcr = v4l2_ctrl_new_std(handler,
1594                                 &s3c_camif_subdev_ctrl_ops,
1595                                 V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
1596         }
1597
1598         if (handler->error) {
1599                 v4l2_ctrl_handler_free(handler);
1600                 media_entity_cleanup(&sd->entity);
1601                 return handler->error;
1602         }
1603
1604         if (camif->variant->has_img_effect)
1605                 v4l2_ctrl_auto_cluster(2, &camif->ctrl_colorfx,
1606                                V4L2_COLORFX_SET_CBCR, false);
1607
1608         sd->ctrl_handler = handler;
1609         v4l2_set_subdevdata(sd, camif);
1610
1611         return 0;
1612 }
1613
1614 void s3c_camif_unregister_subdev(struct camif_dev *camif)
1615 {
1616         struct v4l2_subdev *sd = &camif->subdev;
1617
1618         /* Return if not registered */
1619         if (v4l2_get_subdevdata(sd) == NULL)
1620                 return;
1621
1622         v4l2_device_unregister_subdev(sd);
1623         media_entity_cleanup(&sd->entity);
1624         v4l2_ctrl_handler_free(&camif->ctrl_handler);
1625         v4l2_set_subdevdata(sd, NULL);
1626 }
1627
1628 int s3c_camif_set_defaults(struct camif_dev *camif)
1629 {
1630         unsigned int ip_rev = camif->variant->ip_revision;
1631         int i;
1632
1633         for (i = 0; i < CAMIF_VP_NUM; i++) {
1634                 struct camif_vp *vp = &camif->vp[i];
1635                 struct camif_frame *f = &vp->out_frame;
1636
1637                 vp->camif = camif;
1638                 vp->id = i;
1639                 vp->offset = camif->variant->vp_offset;
1640
1641                 if (ip_rev == S3C244X_CAMIF_IP_REV)
1642                         vp->fmt_flags = i ? FMT_FL_S3C24XX_PREVIEW :
1643                                         FMT_FL_S3C24XX_CODEC;
1644                 else
1645                         vp->fmt_flags = FMT_FL_S3C64XX;
1646
1647                 vp->out_fmt = s3c_camif_find_format(vp, NULL, 0);
1648                 BUG_ON(vp->out_fmt == NULL);
1649
1650                 memset(f, 0, sizeof(*f));
1651                 f->f_width = CAMIF_DEF_WIDTH;
1652                 f->f_height = CAMIF_DEF_HEIGHT;
1653                 f->rect.width = CAMIF_DEF_WIDTH;
1654                 f->rect.height = CAMIF_DEF_HEIGHT;
1655
1656                 /* Scaler is always enabled */
1657                 vp->scaler.enable = 1;
1658
1659                 vp->payload = (f->f_width * f->f_height *
1660                                vp->out_fmt->depth) / 8;
1661         }
1662
1663         memset(&camif->mbus_fmt, 0, sizeof(camif->mbus_fmt));
1664         camif->mbus_fmt.width = CAMIF_DEF_WIDTH;
1665         camif->mbus_fmt.height = CAMIF_DEF_HEIGHT;
1666         camif->mbus_fmt.code  = camif_mbus_formats[0];
1667
1668         memset(&camif->camif_crop, 0, sizeof(camif->camif_crop));
1669         camif->camif_crop.width = CAMIF_DEF_WIDTH;
1670         camif->camif_crop.height = CAMIF_DEF_HEIGHT;
1671
1672         return 0;
1673 }