linux/drivers/media/platform/s3c-camif/camif-capture.c
<<
>>
Prefs
   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
  40static int debug;
  41module_param(debug, int, 0644);
  42
  43/* Locking: called with vp->camif->slock spinlock held */
  44static 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
  53static 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 */
  65static 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 */
  95static 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
 110static 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
 128static 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 */
 149static 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
 182static 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
 195static 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
 207static 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
 241static 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
 288irqreturn_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        }
 369unlock:
 370        spin_unlock(&camif->slock);
 371        return IRQ_HANDLED;
 372}
 373
 374static 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
 431static 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
 437static 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
 461static 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
 478static 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
 524static 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
 534static 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_get_sync(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);
 559err_pm:
 560        v4l2_fh_release(file);
 561unlock:
 562        mutex_unlock(&camif->lock);
 563        return ret;
 564}
 565
 566static 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
 592static __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
 609static 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
 622static 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
 635static 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
 647static 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
 661static int s3c_camif_vidioc_s_input(struct file *file, void *priv,
 662                                    unsigned int i)
 663{
 664        return i == 0 ? 0 : -EINVAL;
 665}
 666
 667static int s3c_camif_vidioc_g_input(struct file *file, void *priv,
 668                                    unsigned int *i)
 669{
 670        *i = 0;
 671        return 0;
 672}
 673
 674static 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
 688static 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
 708static 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
 759static 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
 766static 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 */
 807static int camif_pipeline_validate(struct camif_dev *camif)
 808{
 809        struct v4l2_subdev_format src_fmt;
 810        struct media_pad *pad;
 811        int ret;
 812
 813        /* Retrieve format at the sensor subdev source pad */
 814        pad = media_entity_remote_pad(&camif->pads[0]);
 815        if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 816                return -EPIPE;
 817
 818        src_fmt.pad = pad->index;
 819        src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 820        ret = v4l2_subdev_call(camif->sensor.sd, pad, get_fmt, NULL, &src_fmt);
 821        if (ret < 0 && ret != -ENOIOCTLCMD)
 822                return -EPIPE;
 823
 824        if (src_fmt.format.width != camif->mbus_fmt.width ||
 825            src_fmt.format.height != camif->mbus_fmt.height ||
 826            src_fmt.format.code != camif->mbus_fmt.code)
 827                return -EPIPE;
 828
 829        return 0;
 830}
 831
 832static int s3c_camif_streamon(struct file *file, void *priv,
 833                              enum v4l2_buf_type type)
 834{
 835        struct camif_vp *vp = video_drvdata(file);
 836        struct camif_dev *camif = vp->camif;
 837        struct media_entity *sensor = &camif->sensor.sd->entity;
 838        int ret;
 839
 840        pr_debug("[vp%d]\n", vp->id);
 841
 842        if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 843                return -EINVAL;
 844
 845        if (vp->owner && vp->owner != priv)
 846                return -EBUSY;
 847
 848        if (s3c_vp_active(vp))
 849                return 0;
 850
 851        ret = media_pipeline_start(sensor, camif->m_pipeline);
 852        if (ret < 0)
 853                return ret;
 854
 855        ret = camif_pipeline_validate(camif);
 856        if (ret < 0) {
 857                media_pipeline_stop(sensor);
 858                return ret;
 859        }
 860
 861        return vb2_streamon(&vp->vb_queue, type);
 862}
 863
 864static int s3c_camif_streamoff(struct file *file, void *priv,
 865                               enum v4l2_buf_type type)
 866{
 867        struct camif_vp *vp = video_drvdata(file);
 868        struct camif_dev *camif = vp->camif;
 869        int ret;
 870
 871        pr_debug("[vp%d]\n", vp->id);
 872
 873        if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 874                return -EINVAL;
 875
 876        if (vp->owner && vp->owner != priv)
 877                return -EBUSY;
 878
 879        ret = vb2_streamoff(&vp->vb_queue, type);
 880        if (ret == 0)
 881                media_pipeline_stop(&camif->sensor.sd->entity);
 882        return ret;
 883}
 884
 885static int s3c_camif_reqbufs(struct file *file, void *priv,
 886                             struct v4l2_requestbuffers *rb)
 887{
 888        struct camif_vp *vp = video_drvdata(file);
 889        int ret;
 890
 891        pr_debug("[vp%d] rb count: %d, owner: %p, priv: %p\n",
 892                 vp->id, rb->count, vp->owner, priv);
 893
 894        if (vp->owner && vp->owner != priv)
 895                return -EBUSY;
 896
 897        if (rb->count)
 898                rb->count = max_t(u32, CAMIF_REQ_BUFS_MIN, rb->count);
 899        else
 900                vp->owner = NULL;
 901
 902        ret = vb2_reqbufs(&vp->vb_queue, rb);
 903        if (ret < 0)
 904                return ret;
 905
 906        if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
 907                rb->count = 0;
 908                vb2_reqbufs(&vp->vb_queue, rb);
 909                ret = -ENOMEM;
 910        }
 911
 912        vp->reqbufs_count = rb->count;
 913        if (vp->owner == NULL && rb->count > 0)
 914                vp->owner = priv;
 915
 916        return ret;
 917}
 918
 919static int s3c_camif_querybuf(struct file *file, void *priv,
 920                              struct v4l2_buffer *buf)
 921{
 922        struct camif_vp *vp = video_drvdata(file);
 923        return vb2_querybuf(&vp->vb_queue, buf);
 924}
 925
 926static int s3c_camif_qbuf(struct file *file, void *priv,
 927                          struct v4l2_buffer *buf)
 928{
 929        struct camif_vp *vp = video_drvdata(file);
 930
 931        pr_debug("[vp%d]\n", vp->id);
 932
 933        if (vp->owner && vp->owner != priv)
 934                return -EBUSY;
 935
 936        return vb2_qbuf(&vp->vb_queue, vp->vdev.v4l2_dev->mdev, buf);
 937}
 938
 939static int s3c_camif_dqbuf(struct file *file, void *priv,
 940                           struct v4l2_buffer *buf)
 941{
 942        struct camif_vp *vp = video_drvdata(file);
 943
 944        pr_debug("[vp%d] sequence: %d\n", vp->id, vp->frame_sequence);
 945
 946        if (vp->owner && vp->owner != priv)
 947                return -EBUSY;
 948
 949        return vb2_dqbuf(&vp->vb_queue, buf, file->f_flags & O_NONBLOCK);
 950}
 951
 952static int s3c_camif_create_bufs(struct file *file, void *priv,
 953                                 struct v4l2_create_buffers *create)
 954{
 955        struct camif_vp *vp = video_drvdata(file);
 956        int ret;
 957
 958        if (vp->owner && vp->owner != priv)
 959                return -EBUSY;
 960
 961        create->count = max_t(u32, 1, create->count);
 962        ret = vb2_create_bufs(&vp->vb_queue, create);
 963
 964        if (!ret && vp->owner == NULL)
 965                vp->owner = priv;
 966
 967        return ret;
 968}
 969
 970static int s3c_camif_prepare_buf(struct file *file, void *priv,
 971                                 struct v4l2_buffer *b)
 972{
 973        struct camif_vp *vp = video_drvdata(file);
 974        return vb2_prepare_buf(&vp->vb_queue, vp->vdev.v4l2_dev->mdev, b);
 975}
 976
 977static int s3c_camif_g_selection(struct file *file, void *priv,
 978                                 struct v4l2_selection *sel)
 979{
 980        struct camif_vp *vp = video_drvdata(file);
 981
 982        if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 983                return -EINVAL;
 984
 985        switch (sel->target) {
 986        case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 987        case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 988                sel->r.left = 0;
 989                sel->r.top = 0;
 990                sel->r.width = vp->out_frame.f_width;
 991                sel->r.height = vp->out_frame.f_height;
 992                return 0;
 993
 994        case V4L2_SEL_TGT_COMPOSE:
 995                sel->r = vp->out_frame.rect;
 996                return 0;
 997        }
 998
 999        return -EINVAL;
1000}
1001
1002static void __camif_try_compose(struct camif_dev *camif, struct camif_vp *vp,
1003                                struct v4l2_rect *r)
1004{
1005        /* s3c244x doesn't support composition */
1006        if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV) {
1007                *r = vp->out_frame.rect;
1008                return;
1009        }
1010
1011        /* TODO: s3c64xx */
1012}
1013
1014static int s3c_camif_s_selection(struct file *file, void *priv,
1015                                 struct v4l2_selection *sel)
1016{
1017        struct camif_vp *vp = video_drvdata(file);
1018        struct camif_dev *camif = vp->camif;
1019        struct v4l2_rect rect = sel->r;
1020        unsigned long flags;
1021
1022        if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1023            sel->target != V4L2_SEL_TGT_COMPOSE)
1024                return -EINVAL;
1025
1026        __camif_try_compose(camif, vp, &rect);
1027
1028        sel->r = rect;
1029        spin_lock_irqsave(&camif->slock, flags);
1030        vp->out_frame.rect = rect;
1031        vp->state |= ST_VP_CONFIG;
1032        spin_unlock_irqrestore(&camif->slock, flags);
1033
1034        pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%dx%d\n",
1035                sel->type, sel->target, sel->flags,
1036                sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1037
1038        return 0;
1039}
1040
1041static const struct v4l2_ioctl_ops s3c_camif_ioctl_ops = {
1042        .vidioc_querycap          = s3c_camif_vidioc_querycap,
1043        .vidioc_enum_input        = s3c_camif_vidioc_enum_input,
1044        .vidioc_g_input           = s3c_camif_vidioc_g_input,
1045        .vidioc_s_input           = s3c_camif_vidioc_s_input,
1046        .vidioc_enum_fmt_vid_cap  = s3c_camif_vidioc_enum_fmt,
1047        .vidioc_try_fmt_vid_cap   = s3c_camif_vidioc_try_fmt,
1048        .vidioc_s_fmt_vid_cap     = s3c_camif_vidioc_s_fmt,
1049        .vidioc_g_fmt_vid_cap     = s3c_camif_vidioc_g_fmt,
1050        .vidioc_g_selection       = s3c_camif_g_selection,
1051        .vidioc_s_selection       = s3c_camif_s_selection,
1052        .vidioc_reqbufs           = s3c_camif_reqbufs,
1053        .vidioc_querybuf          = s3c_camif_querybuf,
1054        .vidioc_prepare_buf       = s3c_camif_prepare_buf,
1055        .vidioc_create_bufs       = s3c_camif_create_bufs,
1056        .vidioc_qbuf              = s3c_camif_qbuf,
1057        .vidioc_dqbuf             = s3c_camif_dqbuf,
1058        .vidioc_streamon          = s3c_camif_streamon,
1059        .vidioc_streamoff         = s3c_camif_streamoff,
1060        .vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
1061        .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1062        .vidioc_log_status        = v4l2_ctrl_log_status,
1063};
1064
1065/*
1066 * Video node controls
1067 */
1068static int s3c_camif_video_s_ctrl(struct v4l2_ctrl *ctrl)
1069{
1070        struct camif_vp *vp = ctrl->priv;
1071        struct camif_dev *camif = vp->camif;
1072        unsigned long flags;
1073
1074        pr_debug("[vp%d] ctrl: %s, value: %d\n", vp->id,
1075                 ctrl->name, ctrl->val);
1076
1077        spin_lock_irqsave(&camif->slock, flags);
1078
1079        switch (ctrl->id) {
1080        case V4L2_CID_HFLIP:
1081                vp->hflip = ctrl->val;
1082                break;
1083
1084        case V4L2_CID_VFLIP:
1085                vp->vflip = ctrl->val;
1086                break;
1087        }
1088
1089        vp->state |= ST_VP_CONFIG;
1090        spin_unlock_irqrestore(&camif->slock, flags);
1091        return 0;
1092}
1093
1094/* Codec and preview video node control ops */
1095static const struct v4l2_ctrl_ops s3c_camif_video_ctrl_ops = {
1096        .s_ctrl = s3c_camif_video_s_ctrl,
1097};
1098
1099int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
1100{
1101        struct camif_vp *vp = &camif->vp[idx];
1102        struct vb2_queue *q = &vp->vb_queue;
1103        struct video_device *vfd = &vp->vdev;
1104        struct v4l2_ctrl *ctrl;
1105        int ret;
1106
1107        memset(vfd, 0, sizeof(*vfd));
1108        snprintf(vfd->name, sizeof(vfd->name), "camif-%s",
1109                 vp->id == 0 ? "codec" : "preview");
1110
1111        vfd->fops = &s3c_camif_fops;
1112        vfd->ioctl_ops = &s3c_camif_ioctl_ops;
1113        vfd->v4l2_dev = &camif->v4l2_dev;
1114        vfd->minor = -1;
1115        vfd->release = video_device_release_empty;
1116        vfd->lock = &camif->lock;
1117        vp->reqbufs_count = 0;
1118
1119        INIT_LIST_HEAD(&vp->pending_buf_q);
1120        INIT_LIST_HEAD(&vp->active_buf_q);
1121
1122        memset(q, 0, sizeof(*q));
1123        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1124        q->io_modes = VB2_MMAP | VB2_USERPTR;
1125        q->ops = &s3c_camif_qops;
1126        q->mem_ops = &vb2_dma_contig_memops;
1127        q->buf_struct_size = sizeof(struct camif_buffer);
1128        q->drv_priv = vp;
1129        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1130        q->lock = &vp->camif->lock;
1131        q->dev = camif->v4l2_dev.dev;
1132
1133        ret = vb2_queue_init(q);
1134        if (ret)
1135                goto err_vd_rel;
1136
1137        vp->pad.flags = MEDIA_PAD_FL_SINK;
1138        ret = media_entity_pads_init(&vfd->entity, 1, &vp->pad);
1139        if (ret)
1140                goto err_vd_rel;
1141
1142        video_set_drvdata(vfd, vp);
1143
1144        v4l2_ctrl_handler_init(&vp->ctrl_handler, 1);
1145        ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1146                                 V4L2_CID_HFLIP, 0, 1, 1, 0);
1147        if (ctrl)
1148                ctrl->priv = vp;
1149        ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
1150                                 V4L2_CID_VFLIP, 0, 1, 1, 0);
1151        if (ctrl)
1152                ctrl->priv = vp;
1153
1154        ret = vp->ctrl_handler.error;
1155        if (ret < 0)
1156                goto err_me_cleanup;
1157
1158        vfd->ctrl_handler = &vp->ctrl_handler;
1159        vfd->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
1160
1161        ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
1162        if (ret)
1163                goto err_ctrlh_free;
1164
1165        v4l2_info(&camif->v4l2_dev, "registered %s as /dev/%s\n",
1166                  vfd->name, video_device_node_name(vfd));
1167        return 0;
1168
1169err_ctrlh_free:
1170        v4l2_ctrl_handler_free(&vp->ctrl_handler);
1171err_me_cleanup:
1172        media_entity_cleanup(&vfd->entity);
1173err_vd_rel:
1174        video_device_release(vfd);
1175        return ret;
1176}
1177
1178void s3c_camif_unregister_video_node(struct camif_dev *camif, int idx)
1179{
1180        struct video_device *vfd = &camif->vp[idx].vdev;
1181
1182        if (video_is_registered(vfd)) {
1183                video_unregister_device(vfd);
1184                media_entity_cleanup(&vfd->entity);
1185                v4l2_ctrl_handler_free(vfd->ctrl_handler);
1186        }
1187}
1188
1189/* Media bus pixel formats supported at the camif input */
1190static const u32 camif_mbus_formats[] = {
1191        MEDIA_BUS_FMT_YUYV8_2X8,
1192        MEDIA_BUS_FMT_YVYU8_2X8,
1193        MEDIA_BUS_FMT_UYVY8_2X8,
1194        MEDIA_BUS_FMT_VYUY8_2X8,
1195};
1196
1197/*
1198 *  Camera input interface subdev operations
1199 */
1200
1201static int s3c_camif_subdev_enum_mbus_code(struct v4l2_subdev *sd,
1202                                        struct v4l2_subdev_pad_config *cfg,
1203                                        struct v4l2_subdev_mbus_code_enum *code)
1204{
1205        if (code->index >= ARRAY_SIZE(camif_mbus_formats))
1206                return -EINVAL;
1207
1208        code->code = camif_mbus_formats[code->index];
1209        return 0;
1210}
1211
1212static int s3c_camif_subdev_get_fmt(struct v4l2_subdev *sd,
1213                                    struct v4l2_subdev_pad_config *cfg,
1214                                    struct v4l2_subdev_format *fmt)
1215{
1216        struct camif_dev *camif = v4l2_get_subdevdata(sd);
1217        struct v4l2_mbus_framefmt *mf = &fmt->format;
1218
1219        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1220                mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1221                fmt->format = *mf;
1222                return 0;
1223        }
1224
1225        mutex_lock(&camif->lock);
1226
1227        switch (fmt->pad) {
1228        case CAMIF_SD_PAD_SINK:
1229                /* full camera input pixel size */
1230                *mf = camif->mbus_fmt;
1231                break;
1232
1233        case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1234                /* crop rectangle at camera interface input */
1235                mf->width = camif->camif_crop.width;
1236                mf->height = camif->camif_crop.height;
1237                mf->code = camif->mbus_fmt.code;
1238                break;
1239        }
1240
1241        mutex_unlock(&camif->lock);
1242        mf->field = V4L2_FIELD_NONE;
1243        mf->colorspace = V4L2_COLORSPACE_JPEG;
1244        return 0;
1245}
1246
1247static void __camif_subdev_try_format(struct camif_dev *camif,
1248                                struct v4l2_mbus_framefmt *mf, int pad)
1249{
1250        const struct s3c_camif_variant *variant = camif->variant;
1251        const struct vp_pix_limits *pix_lim;
1252        unsigned int i;
1253
1254        /* FIXME: constraints against codec or preview path ? */
1255        pix_lim = &variant->vp_pix_limits[VP_CODEC];
1256
1257        for (i = 0; i < ARRAY_SIZE(camif_mbus_formats); i++)
1258                if (camif_mbus_formats[i] == mf->code)
1259                        break;
1260
1261        if (i == ARRAY_SIZE(camif_mbus_formats))
1262                mf->code = camif_mbus_formats[0];
1263
1264        if (pad == CAMIF_SD_PAD_SINK) {
1265                v4l_bound_align_image(&mf->width, 8, CAMIF_MAX_PIX_WIDTH,
1266                                      ffs(pix_lim->out_width_align) - 1,
1267                                      &mf->height, 8, CAMIF_MAX_PIX_HEIGHT, 0,
1268                                      0);
1269        } else {
1270                struct v4l2_rect *crop = &camif->camif_crop;
1271                v4l_bound_align_image(&mf->width, 8, crop->width,
1272                                      ffs(pix_lim->out_width_align) - 1,
1273                                      &mf->height, 8, crop->height,
1274                                      0, 0);
1275        }
1276
1277        v4l2_dbg(1, debug, &camif->subdev, "%ux%u\n", mf->width, mf->height);
1278}
1279
1280static int s3c_camif_subdev_set_fmt(struct v4l2_subdev *sd,
1281                                    struct v4l2_subdev_pad_config *cfg,
1282                                    struct v4l2_subdev_format *fmt)
1283{
1284        struct camif_dev *camif = v4l2_get_subdevdata(sd);
1285        struct v4l2_mbus_framefmt *mf = &fmt->format;
1286        struct v4l2_rect *crop = &camif->camif_crop;
1287        int i;
1288
1289        v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %ux%u\n",
1290                 fmt->pad, mf->code, mf->width, mf->height);
1291
1292        mf->field = V4L2_FIELD_NONE;
1293        mf->colorspace = V4L2_COLORSPACE_JPEG;
1294        mutex_lock(&camif->lock);
1295
1296        /*
1297         * No pixel format change at the camera input is allowed
1298         * while streaming.
1299         */
1300        if (vb2_is_busy(&camif->vp[VP_CODEC].vb_queue) ||
1301            vb2_is_busy(&camif->vp[VP_PREVIEW].vb_queue)) {
1302                mutex_unlock(&camif->lock);
1303                return -EBUSY;
1304        }
1305
1306        __camif_subdev_try_format(camif, mf, fmt->pad);
1307
1308        if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1309                mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1310                *mf = fmt->format;
1311                mutex_unlock(&camif->lock);
1312                return 0;
1313        }
1314
1315        switch (fmt->pad) {
1316        case CAMIF_SD_PAD_SINK:
1317                camif->mbus_fmt = *mf;
1318                /* Reset sink crop rectangle. */
1319                crop->width = mf->width;
1320                crop->height = mf->height;
1321                crop->left = 0;
1322                crop->top = 0;
1323                /*
1324                 * Reset source format (the camif's crop rectangle)
1325                 * and the video output resolution.
1326                 */
1327                for (i = 0; i < CAMIF_VP_NUM; i++) {
1328                        struct camif_frame *frame = &camif->vp[i].out_frame;
1329                        frame->rect = *crop;
1330                        frame->f_width = mf->width;
1331                        frame->f_height = mf->height;
1332                }
1333                break;
1334
1335        case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
1336                /* Pixel format can be only changed on the sink pad. */
1337                mf->code = camif->mbus_fmt.code;
1338                mf->width = crop->width;
1339                mf->height = crop->height;
1340                break;
1341        }
1342
1343        mutex_unlock(&camif->lock);
1344        return 0;
1345}
1346
1347static int s3c_camif_subdev_get_selection(struct v4l2_subdev *sd,
1348                                          struct v4l2_subdev_pad_config *cfg,
1349                                          struct v4l2_subdev_selection *sel)
1350{
1351        struct camif_dev *camif = v4l2_get_subdevdata(sd);
1352        struct v4l2_rect *crop = &camif->camif_crop;
1353        struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1354
1355        if ((sel->target != V4L2_SEL_TGT_CROP &&
1356            sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
1357            sel->pad != CAMIF_SD_PAD_SINK)
1358                return -EINVAL;
1359
1360        if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1361                sel->r = *v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
1362                return 0;
1363        }
1364
1365        mutex_lock(&camif->lock);
1366
1367        if (sel->target == V4L2_SEL_TGT_CROP) {
1368                sel->r = *crop;
1369        } else { /* crop bounds */
1370                sel->r.width = mf->width;
1371                sel->r.height = mf->height;
1372                sel->r.left = 0;
1373                sel->r.top = 0;
1374        }
1375
1376        mutex_unlock(&camif->lock);
1377
1378        v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d) %dx%d, size: %ux%u\n",
1379                 __func__, crop->left, crop->top, crop->width,
1380                 crop->height, mf->width, mf->height);
1381
1382        return 0;
1383}
1384
1385static void __camif_try_crop(struct camif_dev *camif, struct v4l2_rect *r)
1386{
1387        struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
1388        const struct camif_pix_limits *pix_lim = &camif->variant->pix_limits;
1389        unsigned int left = 2 * r->left;
1390        unsigned int top = 2 * r->top;
1391
1392        /*
1393         * Following constraints must be met:
1394         *  - r->width + 2 * r->left = mf->width;
1395         *  - r->height + 2 * r->top = mf->height;
1396         *  - crop rectangle size and position must be aligned
1397         *    to 8 or 2 pixels, depending on SoC version.
1398         */
1399        v4l_bound_align_image(&r->width, 0, mf->width,
1400                              ffs(pix_lim->win_hor_offset_align) - 1,
1401                              &r->height, 0, mf->height, 1, 0);
1402
1403        v4l_bound_align_image(&left, 0, mf->width - r->width,
1404                              ffs(pix_lim->win_hor_offset_align),
1405                              &top, 0, mf->height - r->height, 2, 0);
1406
1407        r->left = left / 2;
1408        r->top = top / 2;
1409        r->width = mf->width - left;
1410        r->height = mf->height - top;
1411        /*
1412         * Make sure we either downscale or upscale both the pixel
1413         * width and height. Just return current crop rectangle if
1414         * this scaler constraint is not met.
1415         */
1416        if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV &&
1417            camif_is_streaming(camif)) {
1418                unsigned int i;
1419
1420                for (i = 0; i < CAMIF_VP_NUM; i++) {
1421                        struct v4l2_rect *or = &camif->vp[i].out_frame.rect;
1422                        if ((or->width > r->width) == (or->height > r->height))
1423                                continue;
1424                        *r = camif->camif_crop;
1425                        pr_debug("Width/height scaling direction limitation\n");
1426                        break;
1427                }
1428        }
1429
1430        v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%dx%d, fmt: %ux%u\n",
1431                 r->left, r->top, r->width, r->height, mf->width, mf->height);
1432}
1433
1434static int s3c_camif_subdev_set_selection(struct v4l2_subdev *sd,
1435                                          struct v4l2_subdev_pad_config *cfg,
1436                                          struct v4l2_subdev_selection *sel)
1437{
1438        struct camif_dev *camif = v4l2_get_subdevdata(sd);
1439        struct v4l2_rect *crop = &camif->camif_crop;
1440        struct camif_scaler scaler;
1441
1442        if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != CAMIF_SD_PAD_SINK)
1443                return -EINVAL;
1444
1445        mutex_lock(&camif->lock);
1446        __camif_try_crop(camif, &sel->r);
1447
1448        if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1449                *v4l2_subdev_get_try_crop(sd, cfg, sel->pad) = sel->r;
1450        } else {
1451                unsigned long flags;
1452                unsigned int i;
1453
1454                spin_lock_irqsave(&camif->slock, flags);
1455                *crop = sel->r;
1456
1457                for (i = 0; i < CAMIF_VP_NUM; i++) {
1458                        struct camif_vp *vp = &camif->vp[i];
1459                        scaler = vp->scaler;
1460                        if (s3c_camif_get_scaler_config(vp, &scaler))
1461                                continue;
1462                        vp->scaler = scaler;
1463                        vp->state |= ST_VP_CONFIG;
1464                }
1465
1466                spin_unlock_irqrestore(&camif->slock, flags);
1467        }
1468        mutex_unlock(&camif->lock);
1469
1470        v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %u, f_h: %u\n",
1471                 __func__, crop->left, crop->top, crop->width, crop->height,
1472                 camif->mbus_fmt.width, camif->mbus_fmt.height);
1473
1474        return 0;
1475}
1476
1477static const struct v4l2_subdev_pad_ops s3c_camif_subdev_pad_ops = {
1478        .enum_mbus_code = s3c_camif_subdev_enum_mbus_code,
1479        .get_selection = s3c_camif_subdev_get_selection,
1480        .set_selection = s3c_camif_subdev_set_selection,
1481        .get_fmt = s3c_camif_subdev_get_fmt,
1482        .set_fmt = s3c_camif_subdev_set_fmt,
1483};
1484
1485static const struct v4l2_subdev_ops s3c_camif_subdev_ops = {
1486        .pad = &s3c_camif_subdev_pad_ops,
1487};
1488
1489static int s3c_camif_subdev_s_ctrl(struct v4l2_ctrl *ctrl)
1490{
1491        struct camif_dev *camif = container_of(ctrl->handler, struct camif_dev,
1492                                               ctrl_handler);
1493        unsigned long flags;
1494
1495        spin_lock_irqsave(&camif->slock, flags);
1496
1497        switch (ctrl->id) {
1498        case V4L2_CID_COLORFX:
1499                camif->colorfx = camif->ctrl_colorfx->val;
1500                /* Set Cb, Cr */
1501                switch (ctrl->val) {
1502                case V4L2_COLORFX_SEPIA:
1503                        camif->colorfx_cb = 115;
1504                        camif->colorfx_cr = 145;
1505                        break;
1506                case V4L2_COLORFX_SET_CBCR:
1507                        camif->colorfx_cb = camif->ctrl_colorfx_cbcr->val >> 8;
1508                        camif->colorfx_cr = camif->ctrl_colorfx_cbcr->val & 0xff;
1509                        break;
1510                default:
1511                        /* for V4L2_COLORFX_BW and others */
1512                        camif->colorfx_cb = 128;
1513                        camif->colorfx_cr = 128;
1514                }
1515                break;
1516        case V4L2_CID_TEST_PATTERN:
1517                camif->test_pattern = camif->ctrl_test_pattern->val;
1518                break;
1519        default:
1520                WARN_ON(1);
1521        }
1522
1523        camif->vp[VP_CODEC].state |= ST_VP_CONFIG;
1524        camif->vp[VP_PREVIEW].state |= ST_VP_CONFIG;
1525        spin_unlock_irqrestore(&camif->slock, flags);
1526
1527        return 0;
1528}
1529
1530static const struct v4l2_ctrl_ops s3c_camif_subdev_ctrl_ops = {
1531        .s_ctrl = s3c_camif_subdev_s_ctrl,
1532};
1533
1534static const char * const s3c_camif_test_pattern_menu[] = {
1535        "Disabled",
1536        "Color bars",
1537        "Horizontal increment",
1538        "Vertical increment",
1539};
1540
1541int s3c_camif_create_subdev(struct camif_dev *camif)
1542{
1543        struct v4l2_ctrl_handler *handler = &camif->ctrl_handler;
1544        struct v4l2_subdev *sd = &camif->subdev;
1545        int ret;
1546
1547        v4l2_subdev_init(sd, &s3c_camif_subdev_ops);
1548        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1549        strscpy(sd->name, "S3C-CAMIF", sizeof(sd->name));
1550
1551        camif->pads[CAMIF_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1552        camif->pads[CAMIF_SD_PAD_SOURCE_C].flags = MEDIA_PAD_FL_SOURCE;
1553        camif->pads[CAMIF_SD_PAD_SOURCE_P].flags = MEDIA_PAD_FL_SOURCE;
1554
1555        ret = media_entity_pads_init(&sd->entity, CAMIF_SD_PADS_NUM,
1556                                camif->pads);
1557        if (ret)
1558                return ret;
1559
1560        v4l2_ctrl_handler_init(handler, 3);
1561        camif->ctrl_test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1562                        &s3c_camif_subdev_ctrl_ops, V4L2_CID_TEST_PATTERN,
1563                        ARRAY_SIZE(s3c_camif_test_pattern_menu) - 1, 0, 0,
1564                        s3c_camif_test_pattern_menu);
1565
1566        if (camif->variant->has_img_effect) {
1567                camif->ctrl_colorfx = v4l2_ctrl_new_std_menu(handler,
1568                                &s3c_camif_subdev_ctrl_ops,
1569                                V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
1570                                ~0x981f, V4L2_COLORFX_NONE);
1571
1572                camif->ctrl_colorfx_cbcr = v4l2_ctrl_new_std(handler,
1573                                &s3c_camif_subdev_ctrl_ops,
1574                                V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
1575        }
1576
1577        if (handler->error) {
1578                v4l2_ctrl_handler_free(handler);
1579                media_entity_cleanup(&sd->entity);
1580                return handler->error;
1581        }
1582
1583        if (camif->variant->has_img_effect)
1584                v4l2_ctrl_auto_cluster(2, &camif->ctrl_colorfx,
1585                               V4L2_COLORFX_SET_CBCR, false);
1586
1587        sd->ctrl_handler = handler;
1588        v4l2_set_subdevdata(sd, camif);
1589
1590        return 0;
1591}
1592
1593void s3c_camif_unregister_subdev(struct camif_dev *camif)
1594{
1595        struct v4l2_subdev *sd = &camif->subdev;
1596
1597        /* Return if not registered */
1598        if (v4l2_get_subdevdata(sd) == NULL)
1599                return;
1600
1601        v4l2_device_unregister_subdev(sd);
1602        media_entity_cleanup(&sd->entity);
1603        v4l2_ctrl_handler_free(&camif->ctrl_handler);
1604        v4l2_set_subdevdata(sd, NULL);
1605}
1606
1607int s3c_camif_set_defaults(struct camif_dev *camif)
1608{
1609        unsigned int ip_rev = camif->variant->ip_revision;
1610        int i;
1611
1612        for (i = 0; i < CAMIF_VP_NUM; i++) {
1613                struct camif_vp *vp = &camif->vp[i];
1614                struct camif_frame *f = &vp->out_frame;
1615
1616                vp->camif = camif;
1617                vp->id = i;
1618                vp->offset = camif->variant->vp_offset;
1619
1620                if (ip_rev == S3C244X_CAMIF_IP_REV)
1621                        vp->fmt_flags = i ? FMT_FL_S3C24XX_PREVIEW :
1622                                        FMT_FL_S3C24XX_CODEC;
1623                else
1624                        vp->fmt_flags = FMT_FL_S3C64XX;
1625
1626                vp->out_fmt = s3c_camif_find_format(vp, NULL, 0);
1627                BUG_ON(vp->out_fmt == NULL);
1628
1629                memset(f, 0, sizeof(*f));
1630                f->f_width = CAMIF_DEF_WIDTH;
1631                f->f_height = CAMIF_DEF_HEIGHT;
1632                f->rect.width = CAMIF_DEF_WIDTH;
1633                f->rect.height = CAMIF_DEF_HEIGHT;
1634
1635                /* Scaler is always enabled */
1636                vp->scaler.enable = 1;
1637
1638                vp->payload = (f->f_width * f->f_height *
1639                               vp->out_fmt->depth) / 8;
1640        }
1641
1642        memset(&camif->mbus_fmt, 0, sizeof(camif->mbus_fmt));
1643        camif->mbus_fmt.width = CAMIF_DEF_WIDTH;
1644        camif->mbus_fmt.height = CAMIF_DEF_HEIGHT;
1645        camif->mbus_fmt.code  = camif_mbus_formats[0];
1646
1647        memset(&camif->camif_crop, 0, sizeof(camif->camif_crop));
1648        camif->camif_crop.width = CAMIF_DEF_WIDTH;
1649        camif->camif_crop.height = CAMIF_DEF_HEIGHT;
1650
1651        return 0;
1652}
1653