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