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

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