drivers/media/platform/s5p-fimc/fimc-lite.c

   1 /*
   2  * Samsung EXYNOS FIMC-LITE (camera host interface) driver
   3 *
   4  * Copyright (C) 2012 Samsung Electronics Co., Ltd.
   5  * Sylwester Nawrocki <s.nawrocki@samsung.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  */
  11 #define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
  12
  13 #include <linux/bug.h>
  14 #include <linux/device.h>
  15 #include <linux/errno.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/kernel.h>
  18 #include <linux/list.h>
  19 #include <linux/module.h>
  20 #include <linux/types.h>
  21 #include <linux/platform_device.h>
  22 #include <linux/pm_runtime.h>
  23 #include <linux/slab.h>
  24 #include <linux/videodev2.h>
  25
  26 #include <media/v4l2-device.h>
  27 #include <media/v4l2-ioctl.h>
  28 #include <media/v4l2-mem2mem.h>
  29 #include <media/videobuf2-core.h>
  30 #include <media/videobuf2-dma-contig.h>
  31 #include <media/s5p_fimc.h>
  32
  33 #include "fimc-mdevice.h"
  34 #include "fimc-core.h"
  35 #include "fimc-lite.h"
  36 #include "fimc-lite-reg.h"
  37
  38 static int debug;
  39 module_param(debug, int, 0644);
  40
  41 static const struct fimc_fmt fimc_lite_formats[] = {
  42         {
  43                 .name           = "YUV 4:2:2 packed, YCbYCr",
  44                 .fourcc         = V4L2_PIX_FMT_YUYV,
  45                 .depth          = { 16 },
  46                 .color          = FIMC_FMT_YCBYCR422,
  47                 .memplanes      = 1,
  48                 .mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
  49         }, {
  50                 .name           = "YUV 4:2:2 packed, CbYCrY",
  51                 .fourcc         = V4L2_PIX_FMT_UYVY,
  52                 .depth          = { 16 },
  53                 .color          = FIMC_FMT_CBYCRY422,
  54                 .memplanes      = 1,
  55                 .mbus_code      = V4L2_MBUS_FMT_UYVY8_2X8,
  56         }, {
  57                 .name           = "YUV 4:2:2 packed, CrYCbY",
  58                 .fourcc         = V4L2_PIX_FMT_VYUY,
  59                 .depth          = { 16 },
  60                 .color          = FIMC_FMT_CRYCBY422,
  61                 .memplanes      = 1,
  62                 .mbus_code      = V4L2_MBUS_FMT_VYUY8_2X8,
  63         }, {
  64                 .name           = "YUV 4:2:2 packed, YCrYCb",
  65                 .fourcc         = V4L2_PIX_FMT_YVYU,
  66                 .depth          = { 16 },
  67                 .color          = FIMC_FMT_YCRYCB422,
  68                 .memplanes      = 1,
  69                 .mbus_code      = V4L2_MBUS_FMT_YVYU8_2X8,
  70         }, {
  71                 .name           = "RAW8 (GRBG)",
  72                 .fourcc         = V4L2_PIX_FMT_SGRBG8,
  73                 .depth          = { 8 },
  74                 .color          = FIMC_FMT_RAW8,
  75                 .memplanes      = 1,
  76                 .mbus_code      = V4L2_MBUS_FMT_SGRBG8_1X8,
  77         }, {
  78                 .name           = "RAW10 (GRBG)",
  79                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
  80                 .depth          = { 10 },
  81                 .color          = FIMC_FMT_RAW10,
  82                 .memplanes      = 1,
  83                 .mbus_code      = V4L2_MBUS_FMT_SGRBG10_1X10,
  84         }, {
  85                 .name           = "RAW12 (GRBG)",
  86                 .fourcc         = V4L2_PIX_FMT_SGRBG12,
  87                 .depth          = { 12 },
  88                 .color          = FIMC_FMT_RAW12,
  89                 .memplanes      = 1,
  90                 .mbus_code      = V4L2_MBUS_FMT_SGRBG12_1X12,
  91         },
  92 };
  93
  94 /**
  95  * fimc_lite_find_format - lookup fimc color format by fourcc or media bus code
  96  * @pixelformat: fourcc to match, ignored if null
  97  * @mbus_code: media bus code to match, ignored if null
  98  * @index: index to the fimc_lite_formats array, ignored if negative
  99  */
 100 static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
 101                                         const u32 *mbus_code, int index)
 102 {
 103         const struct fimc_fmt *fmt, *def_fmt = NULL;
 104         unsigned int i;
 105         int id = 0;
 106
 107         if (index >= (int)ARRAY_SIZE(fimc_lite_formats))
 108                 return NULL;
 109
 110         for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
 111                 fmt = &fimc_lite_formats[i];
 112                 if (pixelformat && fmt->fourcc == *pixelformat)
 113                         return fmt;
 114                 if (mbus_code && fmt->mbus_code == *mbus_code)
 115                         return fmt;
 116                 if (index == id)
 117                         def_fmt = fmt;
 118                 id++;
 119         }
 120         return def_fmt;
 121 }
 122
 123 static int fimc_lite_hw_init(struct fimc_lite *fimc, bool isp_output)
 124 {
 125         struct fimc_pipeline *pipeline = &fimc->pipeline;
 126         struct v4l2_subdev *sensor;
 127         struct fimc_sensor_info *si;
 128         unsigned long flags;
 129
 130         sensor = isp_output ? fimc->sensor : pipeline->subdevs[IDX_SENSOR];
 131
 132         if (sensor == NULL)
 133                 return -ENXIO;
 134
 135         if (fimc->fmt == NULL)
 136                 return -EINVAL;
 137
 138         /* Get sensor configuration data from the sensor subdev */
 139         si = v4l2_get_subdev_hostdata(sensor);
 140         spin_lock_irqsave(&fimc->slock, flags);
 141
 142         flite_hw_set_camera_bus(fimc, &si->pdata);
 143         flite_hw_set_source_format(fimc, &fimc->inp_frame);
 144         flite_hw_set_window_offset(fimc, &fimc->inp_frame);
 145         flite_hw_set_output_dma(fimc, &fimc->out_frame, !isp_output);
 146         flite_hw_set_interrupt_mask(fimc);
 147         flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
 148
 149         if (debug > 0)
 150                 flite_hw_dump_regs(fimc, __func__);
 151
 152         spin_unlock_irqrestore(&fimc->slock, flags);
 153         return 0;
 154 }
 155
 156 /*
 157  * Reinitialize the driver so it is ready to start the streaming again.
 158  * Set fimc->state to indicate stream off and the hardware shut down state.
 159  * If not suspending (@suspend is false), return any buffers to videobuf2.
 160  * Otherwise put any owned buffers onto the pending buffers queue, so they
 161  * can be re-spun when the device is being resumed. Also perform FIMC
 162  * software reset and disable streaming on the whole pipeline if required.
 163  */
 164 static int fimc_lite_reinit(struct fimc_lite *fimc, bool suspend)
 165 {
 166         struct flite_buffer *buf;
 167         unsigned long flags;
 168         bool streaming;
 169
 170         spin_lock_irqsave(&fimc->slock, flags);
 171         streaming = fimc->state & (1 << ST_SENSOR_STREAM);
 172
 173         fimc->state &= ~(1 << ST_FLITE_RUN | 1 << ST_FLITE_OFF |
 174                          1 << ST_FLITE_STREAM | 1 << ST_SENSOR_STREAM);
 175         if (suspend)
 176                 fimc->state |= (1 << ST_FLITE_SUSPENDED);
 177         else
 178                 fimc->state &= ~(1 << ST_FLITE_PENDING |
 179                                  1 << ST_FLITE_SUSPENDED);
 180
 181         /* Release unused buffers */
 182         while (!suspend && !list_empty(&fimc->pending_buf_q)) {
 183                 buf = fimc_lite_pending_queue_pop(fimc);
 184                 vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 185         }
 186         /* If suspending put unused buffers onto pending queue */
 187         while (!list_empty(&fimc->active_buf_q)) {
 188                 buf = fimc_lite_active_queue_pop(fimc);
 189                 if (suspend)
 190                         fimc_lite_pending_queue_add(fimc, buf);
 191                 else
 192                         vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 193         }
 194
 195         spin_unlock_irqrestore(&fimc->slock, flags);
 196
 197         flite_hw_reset(fimc);
 198
 199         if (!streaming)
 200                 return 0;
 201
 202         return fimc_pipeline_call(fimc, set_stream, &fimc->pipeline, 0);
 203 }
 204
 205 static int fimc_lite_stop_capture(struct fimc_lite *fimc, bool suspend)
 206 {
 207         unsigned long flags;
 208
 209         if (!fimc_lite_active(fimc))
 210                 return 0;
 211
 212         spin_lock_irqsave(&fimc->slock, flags);
 213         set_bit(ST_FLITE_OFF, &fimc->state);
 214         flite_hw_capture_stop(fimc);
 215         spin_unlock_irqrestore(&fimc->slock, flags);
 216
 217         wait_event_timeout(fimc->irq_queue,
 218                            !test_bit(ST_FLITE_OFF, &fimc->state),
 219                            (2*HZ/10)); /* 200 ms */
 220
 221         return fimc_lite_reinit(fimc, suspend);
 222 }
 223
 224 /* Must be called  with fimc.slock spinlock held. */
 225 static void fimc_lite_config_update(struct fimc_lite *fimc)
 226 {
 227         flite_hw_set_window_offset(fimc, &fimc->inp_frame);
 228         flite_hw_set_dma_window(fimc, &fimc->out_frame);
 229         flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
 230         clear_bit(ST_FLITE_CONFIG, &fimc->state);
 231 }
 232
 233 static irqreturn_t flite_irq_handler(int irq, void *priv)
 234 {
 235         struct fimc_lite *fimc = priv;
 236         struct flite_buffer *vbuf;
 237         unsigned long flags;
 238         struct timeval *tv;
 239         struct timespec ts;
 240         u32 intsrc;
 241
 242         spin_lock_irqsave(&fimc->slock, flags);
 243
 244         intsrc = flite_hw_get_interrupt_source(fimc);
 245         flite_hw_clear_pending_irq(fimc);
 246
 247         if (test_and_clear_bit(ST_FLITE_OFF, &fimc->state)) {
 248                 wake_up(&fimc->irq_queue);
 249                 goto done;
 250         }
 251
 252         if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW) {
 253                 clear_bit(ST_FLITE_RUN, &fimc->state);
 254                 fimc->events.data_overflow++;
 255         }
 256
 257         if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND) {
 258                 flite_hw_clear_last_capture_end(fimc);
 259                 clear_bit(ST_FLITE_STREAM, &fimc->state);
 260                 wake_up(&fimc->irq_queue);
 261         }
 262
 263         if (atomic_read(&fimc->out_path) != FIMC_IO_DMA)
 264                 goto done;
 265
 266         if ((intsrc & FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART) &&
 267             test_bit(ST_FLITE_RUN, &fimc->state) &&
 268             !list_empty(&fimc->active_buf_q) &&
 269             !list_empty(&fimc->pending_buf_q)) {
 270                 vbuf = fimc_lite_active_queue_pop(fimc);
 271                 ktime_get_ts(&ts);
 272                 tv = &vbuf->vb.v4l2_buf.timestamp;
 273                 tv->tv_sec = ts.tv_sec;
 274                 tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
 275                 vbuf->vb.v4l2_buf.sequence = fimc->frame_count++;
 276                 vb2_buffer_done(&vbuf->vb, VB2_BUF_STATE_DONE);
 277
 278                 vbuf = fimc_lite_pending_queue_pop(fimc);
 279                 flite_hw_set_output_addr(fimc, vbuf->paddr);
 280                 fimc_lite_active_queue_add(fimc, vbuf);
 281         }
 282
 283         if (test_bit(ST_FLITE_CONFIG, &fimc->state))
 284                 fimc_lite_config_update(fimc);
 285
 286         if (list_empty(&fimc->pending_buf_q)) {
 287                 flite_hw_capture_stop(fimc);
 288                 clear_bit(ST_FLITE_STREAM, &fimc->state);
 289         }
 290 done:
 291         set_bit(ST_FLITE_RUN, &fimc->state);
 292         spin_unlock_irqrestore(&fimc->slock, flags);
 293         return IRQ_HANDLED;
 294 }
 295
 296 static int start_streaming(struct vb2_queue *q, unsigned int count)
 297 {
 298         struct fimc_lite *fimc = q->drv_priv;
 299         int ret;
 300
 301         fimc->frame_count = 0;
 302
 303         ret = fimc_lite_hw_init(fimc, false);
 304         if (ret) {
 305                 fimc_lite_reinit(fimc, false);
 306                 return ret;
 307         }
 308
 309         set_bit(ST_FLITE_PENDING, &fimc->state);
 310
 311         if (!list_empty(&fimc->active_buf_q) &&
 312             !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
 313                 flite_hw_capture_start(fimc);
 314
 315                 if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
 316                         fimc_pipeline_call(fimc, set_stream,
 317                                            &fimc->pipeline, 1);
 318         }
 319         if (debug > 0)
 320                 flite_hw_dump_regs(fimc, __func__);
 321
 322         return 0;
 323 }
 324
 325 static int stop_streaming(struct vb2_queue *q)
 326 {
 327         struct fimc_lite *fimc = q->drv_priv;
 328
 329         if (!fimc_lite_active(fimc))
 330                 return -EINVAL;
 331
 332         return fimc_lite_stop_capture(fimc, false);
 333 }
 334
 335 static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 336                        unsigned int *num_buffers, unsigned int *num_planes,
 337                        unsigned int sizes[], void *allocators[])
 338 {
 339         const struct v4l2_pix_format_mplane *pixm = NULL;
 340         struct fimc_lite *fimc = vq->drv_priv;
 341         struct flite_frame *frame = &fimc->out_frame;
 342         const struct fimc_fmt *fmt = fimc->fmt;
 343         unsigned long wh;
 344         int i;
 345
 346         if (pfmt) {
 347                 pixm = &pfmt->fmt.pix_mp;
 348                 fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, -1);
 349                 wh = pixm->width * pixm->height;
 350         } else {
 351                 wh = frame->f_width * frame->f_height;
 352         }
 353
 354         if (fmt == NULL)
 355                 return -EINVAL;
 356
 357         *num_planes = fmt->memplanes;
 358
 359         for (i = 0; i < fmt->memplanes; i++) {
 360                 unsigned int size = (wh * fmt->depth[i]) / 8;
 361                 if (pixm)
 362                         sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
 363                 else
 364                         sizes[i] = size;
 365                 allocators[i] = fimc->alloc_ctx;
 366         }
 367
 368         return 0;
 369 }
 370
 371 static int buffer_prepare(struct vb2_buffer *vb)
 372 {
 373         struct vb2_queue *vq = vb->vb2_queue;
 374         struct fimc_lite *fimc = vq->drv_priv;
 375         int i;
 376
 377         if (fimc->fmt == NULL)
 378                 return -EINVAL;
 379
 380         for (i = 0; i < fimc->fmt->memplanes; i++) {
 381                 unsigned long size = fimc->payload[i];
 382
 383                 if (vb2_plane_size(vb, i) < size) {
 384                         v4l2_err(&fimc->vfd,
 385                                  "User buffer too small (%ld < %ld)\n",
 386                                  vb2_plane_size(vb, i), size);
 387                         return -EINVAL;
 388                 }
 389                 vb2_set_plane_payload(vb, i, size);
 390         }
 391
 392         return 0;
 393 }
 394
 395 static void buffer_queue(struct vb2_buffer *vb)
 396 {
 397         struct flite_buffer *buf
 398                 = container_of(vb, struct flite_buffer, vb);
 399         struct fimc_lite *fimc = vb2_get_drv_priv(vb->vb2_queue);
 400         unsigned long flags;
 401
 402         spin_lock_irqsave(&fimc->slock, flags);
 403         buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
 404
 405         if (!test_bit(ST_FLITE_SUSPENDED, &fimc->state) &&
 406             !test_bit(ST_FLITE_STREAM, &fimc->state) &&
 407             list_empty(&fimc->active_buf_q)) {
 408                 flite_hw_set_output_addr(fimc, buf->paddr);
 409                 fimc_lite_active_queue_add(fimc, buf);
 410         } else {
 411                 fimc_lite_pending_queue_add(fimc, buf);
 412         }
 413
 414         if (vb2_is_streaming(&fimc->vb_queue) &&
 415             !list_empty(&fimc->pending_buf_q) &&
 416             !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
 417                 flite_hw_capture_start(fimc);
 418                 spin_unlock_irqrestore(&fimc->slock, flags);
 419
 420                 if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
 421                         fimc_pipeline_call(fimc, set_stream,
 422                                            &fimc->pipeline, 1);
 423                 return;
 424         }
 425         spin_unlock_irqrestore(&fimc->slock, flags);
 426 }
 427
 428 static void fimc_lock(struct vb2_queue *vq)
 429 {
 430         struct fimc_lite *fimc = vb2_get_drv_priv(vq);
 431         mutex_lock(&fimc->lock);
 432 }
 433
 434 static void fimc_unlock(struct vb2_queue *vq)
 435 {
 436         struct fimc_lite *fimc = vb2_get_drv_priv(vq);
 437         mutex_unlock(&fimc->lock);
 438 }
 439
 440 static const struct vb2_ops fimc_lite_qops = {
 441         .queue_setup     = queue_setup,
 442         .buf_prepare     = buffer_prepare,
 443         .buf_queue       = buffer_queue,
 444         .wait_prepare    = fimc_unlock,
 445         .wait_finish     = fimc_lock,
 446         .start_streaming = start_streaming,
 447         .stop_streaming  = stop_streaming,
 448 };
 449
 450 static void fimc_lite_clear_event_counters(struct fimc_lite *fimc)
 451 {
 452         unsigned long flags;
 453
 454         spin_lock_irqsave(&fimc->slock, flags);
 455         memset(&fimc->events, 0, sizeof(fimc->events));
 456         spin_unlock_irqrestore(&fimc->slock, flags);
 457 }
 458
 459 static int fimc_lite_open(struct file *file)
 460 {
 461         struct fimc_lite *fimc = video_drvdata(file);
 462         struct media_entity *me = &fimc->vfd.entity;
 463         int ret;
 464
 465         mutex_lock(&me->parent->graph_mutex);
 466
 467         mutex_lock(&fimc->lock);
 468         if (atomic_read(&fimc->out_path) != FIMC_IO_DMA) {
 469                 ret = -EBUSY;
 470                 goto done;
 471         }
 472
 473         set_bit(ST_FLITE_IN_USE, &fimc->state);
 474         ret = pm_runtime_get_sync(&fimc->pdev->dev);
 475         if (ret < 0)
 476                 goto done;
 477
 478         ret = v4l2_fh_open(file);
 479         if (ret < 0)
 480                 goto done;
 481
 482         if (++fimc->ref_count == 1 &&
 483             atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
 484                 ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
 485                                          &fimc->vfd.entity, true);
 486                 if (ret < 0) {
 487                         pm_runtime_put_sync(&fimc->pdev->dev);
 488                         fimc->ref_count--;
 489                         v4l2_fh_release(file);
 490                         clear_bit(ST_FLITE_IN_USE, &fimc->state);
 491                 }
 492
 493                 fimc_lite_clear_event_counters(fimc);
 494         }
 495 done:
 496         mutex_unlock(&fimc->lock);
 497         mutex_unlock(&me->parent->graph_mutex);
 498         return ret;
 499 }
 500
 501 static int fimc_lite_close(struct file *file)
 502 {
 503         struct fimc_lite *fimc = video_drvdata(file);
 504         int ret;
 505
 506         mutex_lock(&fimc->lock);
 507
 508         if (--fimc->ref_count == 0 &&
 509             atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
 510                 clear_bit(ST_FLITE_IN_USE, &fimc->state);
 511                 fimc_lite_stop_capture(fimc, false);
 512                 fimc_pipeline_call(fimc, close, &fimc->pipeline);
 513                 clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
 514         }
 515
 516         pm_runtime_put(&fimc->pdev->dev);
 517
 518         if (fimc->ref_count == 0)
 519                 vb2_queue_release(&fimc->vb_queue);
 520
 521         ret = v4l2_fh_release(file);
 522
 523         mutex_unlock(&fimc->lock);
 524         return ret;
 525 }
 526
 527 static unsigned int fimc_lite_poll(struct file *file,
 528                                    struct poll_table_struct *wait)
 529 {
 530         struct fimc_lite *fimc = video_drvdata(file);
 531         int ret;
 532
 533         if (mutex_lock_interruptible(&fimc->lock))
 534                 return POLL_ERR;
 535
 536         ret = vb2_poll(&fimc->vb_queue, file, wait);
 537         mutex_unlock(&fimc->lock);
 538
 539         return ret;
 540 }
 541
 542 static int fimc_lite_mmap(struct file *file, struct vm_area_struct *vma)
 543 {
 544         struct fimc_lite *fimc = video_drvdata(file);
 545         int ret;
 546
 547         if (mutex_lock_interruptible(&fimc->lock))
 548                 return -ERESTARTSYS;
 549
 550         ret = vb2_mmap(&fimc->vb_queue, vma);
 551         mutex_unlock(&fimc->lock);
 552
 553         return ret;
 554 }
 555
 556 static const struct v4l2_file_operations fimc_lite_fops = {
 557         .owner          = THIS_MODULE,
 558         .open           = fimc_lite_open,
 559         .release        = fimc_lite_close,
 560         .poll           = fimc_lite_poll,
 561         .unlocked_ioctl = video_ioctl2,
 562         .mmap           = fimc_lite_mmap,
 563 };
 564
 565 /*
 566  * Format and crop negotiation helpers
 567  */
 568
 569 static const struct fimc_fmt *fimc_lite_try_format(struct fimc_lite *fimc,
 570                                         u32 *width, u32 *height,
 571                                         u32 *code, u32 *fourcc, int pad)
 572 {
 573         struct flite_variant *variant = fimc->variant;
 574         const struct fimc_fmt *fmt;
 575
 576         fmt = fimc_lite_find_format(fourcc, code, 0);
 577         if (WARN_ON(!fmt))
 578                 return NULL;
 579
 580         if (code)
 581                 *code = fmt->mbus_code;
 582         if (fourcc)
 583                 *fourcc = fmt->fourcc;
 584
 585         if (pad == FLITE_SD_PAD_SINK) {
 586                 v4l_bound_align_image(width, 8, variant->max_width,
 587                                       ffs(variant->out_width_align) - 1,
 588                                       height, 0, variant->max_height, 0, 0);
 589         } else {
 590                 v4l_bound_align_image(width, 8, fimc->inp_frame.rect.width,
 591                                       ffs(variant->out_width_align) - 1,
 592                                       height, 0, fimc->inp_frame.rect.height,
 593                                       0, 0);
 594         }
 595
 596         v4l2_dbg(1, debug, &fimc->subdev, "code: 0x%x, %dx%d\n",
 597                  code ? *code : 0, *width, *height);
 598
 599         return fmt;
 600 }
 601
 602 static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
 603 {
 604         struct flite_frame *frame = &fimc->inp_frame;
 605
 606         v4l_bound_align_image(&r->width, 0, frame->f_width, 0,
 607                               &r->height, 0, frame->f_height, 0, 0);
 608
 609         /* Adjust left/top if cropping rectangle got out of bounds */
 610         r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
 611         r->left = round_down(r->left, fimc->variant->win_hor_offs_align);
 612         r->top  = clamp_t(u32, r->top, 0, frame->f_height - r->height);
 613
 614         v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, sink fmt: %dx%d\n",
 615                  r->left, r->top, r->width, r->height,
 616                  frame->f_width, frame->f_height);
 617 }
 618
 619 static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
 620 {
 621         struct flite_frame *frame = &fimc->out_frame;
 622         struct v4l2_rect *crop_rect = &fimc->inp_frame.rect;
 623
 624         /* Scaling is not supported so we enforce compose rectangle size
 625            same as size of the sink crop rectangle. */
 626         r->width = crop_rect->width;
 627         r->height = crop_rect->height;
 628
 629         /* Adjust left/top if the composing rectangle got out of bounds */
 630         r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
 631         r->left = round_down(r->left, fimc->variant->out_hor_offs_align);
 632         r->top  = clamp_t(u32, r->top, 0, fimc->out_frame.f_height - r->height);
 633
 634         v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, source fmt: %dx%d\n",
 635                  r->left, r->top, r->width, r->height,
 636                  frame->f_width, frame->f_height);
 637 }
 638
 639 /*
 640  * Video node ioctl operations
 641  */
 642 static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
 643                                         struct v4l2_capability *cap)
 644 {
 645         strlcpy(cap->driver, FIMC_LITE_DRV_NAME, sizeof(cap->driver));
 646         cap->bus_info[0] = 0;
 647         cap->card[0] = 0;
 648         cap->capabilities = V4L2_CAP_STREAMING;
 649         return 0;
 650 }
 651
 652 static int fimc_lite_enum_fmt_mplane(struct file *file, void *priv,
 653                                      struct v4l2_fmtdesc *f)
 654 {
 655         const struct fimc_fmt *fmt;
 656
 657         if (f->index >= ARRAY_SIZE(fimc_lite_formats))
 658                 return -EINVAL;
 659
 660         fmt = &fimc_lite_formats[f->index];
 661         strlcpy(f->description, fmt->name, sizeof(f->description));
 662         f->pixelformat = fmt->fourcc;
 663
 664         return 0;
 665 }
 666
 667 static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
 668                                   struct v4l2_format *f)
 669 {
 670         struct fimc_lite *fimc = video_drvdata(file);
 671         struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
 672         struct v4l2_plane_pix_format *plane_fmt = &pixm->plane_fmt[0];
 673         struct flite_frame *frame = &fimc->out_frame;
 674         const struct fimc_fmt *fmt = fimc->fmt;
 675
 676         plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
 677         plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
 678
 679         pixm->num_planes = fmt->memplanes;
 680         pixm->pixelformat = fmt->fourcc;
 681         pixm->width = frame->f_width;
 682         pixm->height = frame->f_height;
 683         pixm->field = V4L2_FIELD_NONE;
 684         pixm->colorspace = V4L2_COLORSPACE_JPEG;
 685         return 0;
 686 }
 687
 688 static int fimc_lite_try_fmt(struct fimc_lite *fimc,
 689                              struct v4l2_pix_format_mplane *pixm,
 690                              const struct fimc_fmt **ffmt)
 691 {
 692         struct flite_variant *variant = fimc->variant;
 693         u32 bpl = pixm->plane_fmt[0].bytesperline;
 694         const struct fimc_fmt *fmt;
 695
 696         fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0);
 697         if (WARN_ON(fmt == NULL))
 698                 return -EINVAL;
 699         if (ffmt)
 700                 *ffmt = fmt;
 701         v4l_bound_align_image(&pixm->width, 8, variant->max_width,
 702                               ffs(variant->out_width_align) - 1,
 703                               &pixm->height, 0, variant->max_height, 0, 0);
 704
 705         if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
 706                 pixm->plane_fmt[0].bytesperline = (pixm->width *
 707                                                    fmt->depth[0]) / 8;
 708
 709         if (pixm->plane_fmt[0].sizeimage == 0)
 710                 pixm->plane_fmt[0].sizeimage = (pixm->width * pixm->height *
 711                                                 fmt->depth[0]) / 8;
 712         pixm->num_planes = fmt->memplanes;
 713         pixm->pixelformat = fmt->fourcc;
 714         pixm->colorspace = V4L2_COLORSPACE_JPEG;
 715         pixm->field = V4L2_FIELD_NONE;
 716         return 0;
 717 }
 718
 719 static int fimc_lite_try_fmt_mplane(struct file *file, void *fh,
 720                                     struct v4l2_format *f)
 721 {
 722         struct fimc_lite *fimc = video_drvdata(file);
 723
 724         return fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, NULL);
 725 }
 726
 727 static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
 728                                   struct v4l2_format *f)
 729 {
 730         struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
 731         struct fimc_lite *fimc = video_drvdata(file);
 732         struct flite_frame *frame = &fimc->out_frame;
 733         const struct fimc_fmt *fmt = NULL;
 734         int ret;
 735
 736         if (vb2_is_busy(&fimc->vb_queue))
 737                 return -EBUSY;
 738
 739         ret = fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, &fmt);
 740         if (ret < 0)
 741                 return ret;
 742
 743         fimc->fmt = fmt;
 744         fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
 745                                pixm->plane_fmt[0].sizeimage);
 746         frame->f_width = pixm->width;
 747         frame->f_height = pixm->height;
 748
 749         return 0;
 750 }
 751
 752 static int fimc_pipeline_validate(struct fimc_lite *fimc)
 753 {
 754         struct v4l2_subdev *sd = &fimc->subdev;
 755         struct v4l2_subdev_format sink_fmt, src_fmt;
 756         struct media_pad *pad;
 757         int ret;
 758
 759         while (1) {
 760                 /* Retrieve format at the sink pad */
 761                 pad = &sd->entity.pads[0];
 762                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 763                         break;
 764                 /* Don't call FIMC subdev operation to avoid nested locking */
 765                 if (sd == &fimc->subdev) {
 766                         struct flite_frame *ff = &fimc->out_frame;
 767                         sink_fmt.format.width = ff->f_width;
 768                         sink_fmt.format.height = ff->f_height;
 769                         sink_fmt.format.code = fimc->fmt->mbus_code;
 770                 } else {
 771                         sink_fmt.pad = pad->index;
 772                         sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 773                         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL,
 774                                                &sink_fmt);
 775                         if (ret < 0 && ret != -ENOIOCTLCMD)
 776                                 return -EPIPE;
 777                 }
 778                 /* Retrieve format at the source pad */
 779                 pad = media_entity_remote_source(pad);
 780                 if (pad == NULL ||
 781                     media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 782                         break;
 783
 784                 sd = media_entity_to_v4l2_subdev(pad->entity);
 785                 src_fmt.pad = pad->index;
 786                 src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 787                 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
 788                 if (ret < 0 && ret != -ENOIOCTLCMD)
 789                         return -EPIPE;
 790
 791                 if (src_fmt.format.width != sink_fmt.format.width ||
 792                     src_fmt.format.height != sink_fmt.format.height ||
 793                     src_fmt.format.code != sink_fmt.format.code)
 794                         return -EPIPE;
 795         }
 796         return 0;
 797 }
 798
 799 static int fimc_lite_streamon(struct file *file, void *priv,
 800                               enum v4l2_buf_type type)
 801 {
 802         struct fimc_lite *fimc = video_drvdata(file);
 803         struct v4l2_subdev *sensor = fimc->pipeline.subdevs[IDX_SENSOR];
 804         struct fimc_pipeline *p = &fimc->pipeline;
 805         int ret;
 806
 807         if (fimc_lite_active(fimc))
 808                 return -EBUSY;
 809
 810         ret = media_entity_pipeline_start(&sensor->entity, p->m_pipeline);
 811         if (ret < 0)
 812                 return ret;
 813
 814         ret = fimc_pipeline_validate(fimc);
 815         if (ret) {
 816                 media_entity_pipeline_stop(&sensor->entity);
 817                 return ret;
 818         }
 819
 820         return vb2_streamon(&fimc->vb_queue, type);
 821 }
 822
 823 static int fimc_lite_streamoff(struct file *file, void *priv,
 824                                enum v4l2_buf_type type)
 825 {
 826         struct fimc_lite *fimc = video_drvdata(file);
 827         struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 828         int ret;
 829
 830         ret = vb2_streamoff(&fimc->vb_queue, type);
 831         if (ret == 0)
 832                 media_entity_pipeline_stop(&sd->entity);
 833         return ret;
 834 }
 835
 836 static int fimc_lite_reqbufs(struct file *file, void *priv,
 837                              struct v4l2_requestbuffers *reqbufs)
 838 {
 839         struct fimc_lite *fimc = video_drvdata(file);
 840         int ret;
 841
 842         reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
 843         ret = vb2_reqbufs(&fimc->vb_queue, reqbufs);
 844         if (!ret)
 845                 fimc->reqbufs_count = reqbufs->count;
 846
 847         return ret;
 848 }
 849
 850 static int fimc_lite_querybuf(struct file *file, void *priv,
 851                               struct v4l2_buffer *buf)
 852 {
 853         struct fimc_lite *fimc = video_drvdata(file);
 854
 855         return vb2_querybuf(&fimc->vb_queue, buf);
 856 }
 857
 858 static int fimc_lite_qbuf(struct file *file, void *priv,
 859                           struct v4l2_buffer *buf)
 860 {
 861         struct fimc_lite *fimc = video_drvdata(file);
 862
 863         return vb2_qbuf(&fimc->vb_queue, buf);
 864 }
 865
 866 static int fimc_lite_dqbuf(struct file *file, void *priv,
 867                            struct v4l2_buffer *buf)
 868 {
 869         struct fimc_lite *fimc = video_drvdata(file);
 870
 871         return vb2_dqbuf(&fimc->vb_queue, buf, file->f_flags & O_NONBLOCK);
 872 }
 873
 874 static int fimc_lite_create_bufs(struct file *file, void *priv,
 875                                  struct v4l2_create_buffers *create)
 876 {
 877         struct fimc_lite *fimc = video_drvdata(file);
 878
 879         return vb2_create_bufs(&fimc->vb_queue, create);
 880 }
 881
 882 static int fimc_lite_prepare_buf(struct file *file, void *priv,
 883                                  struct v4l2_buffer *b)
 884 {
 885         struct fimc_lite *fimc = video_drvdata(file);
 886
 887         return vb2_prepare_buf(&fimc->vb_queue, b);
 888 }
 889
 890 /* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
 891 static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
 892 {
 893         if (a->left < b->left || a->top < b->top)
 894                 return 0;
 895         if (a->left + a->width > b->left + b->width)
 896                 return 0;
 897         if (a->top + a->height > b->top + b->height)
 898                 return 0;
 899
 900         return 1;
 901 }
 902
 903 static int fimc_lite_g_selection(struct file *file, void *fh,
 904                                  struct v4l2_selection *sel)
 905 {
 906         struct fimc_lite *fimc = video_drvdata(file);
 907         struct flite_frame *f = &fimc->out_frame;
 908
 909         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 910                 return -EINVAL;
 911
 912         switch (sel->target) {
 913         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 914         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 915                 sel->r.left = 0;
 916                 sel->r.top = 0;
 917                 sel->r.width = f->f_width;
 918                 sel->r.height = f->f_height;
 919                 return 0;
 920
 921         case V4L2_SEL_TGT_COMPOSE:
 922                 sel->r = f->rect;
 923                 return 0;
 924         }
 925
 926         return -EINVAL;
 927 }
 928
 929 static int fimc_lite_s_selection(struct file *file, void *fh,
 930                                  struct v4l2_selection *sel)
 931 {
 932         struct fimc_lite *fimc = video_drvdata(file);
 933         struct flite_frame *f = &fimc->out_frame;
 934         struct v4l2_rect rect = sel->r;
 935         unsigned long flags;
 936
 937         if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
 938             sel->target != V4L2_SEL_TGT_COMPOSE)
 939                 return -EINVAL;
 940
 941         fimc_lite_try_compose(fimc, &rect);
 942
 943         if ((sel->flags & V4L2_SEL_FLAG_LE) &&
 944             !enclosed_rectangle(&rect, &sel->r))
 945                 return -ERANGE;
 946
 947         if ((sel->flags & V4L2_SEL_FLAG_GE) &&
 948             !enclosed_rectangle(&sel->r, &rect))
 949                 return -ERANGE;
 950
 951         sel->r = rect;
 952         spin_lock_irqsave(&fimc->slock, flags);
 953         f->rect = rect;
 954         set_bit(ST_FLITE_CONFIG, &fimc->state);
 955         spin_unlock_irqrestore(&fimc->slock, flags);
 956
 957         return 0;
 958 }
 959
 960 static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
 961         .vidioc_querycap                = fimc_vidioc_querycap_capture,
 962         .vidioc_enum_fmt_vid_cap_mplane = fimc_lite_enum_fmt_mplane,
 963         .vidioc_try_fmt_vid_cap_mplane  = fimc_lite_try_fmt_mplane,
 964         .vidioc_s_fmt_vid_cap_mplane    = fimc_lite_s_fmt_mplane,
 965         .vidioc_g_fmt_vid_cap_mplane    = fimc_lite_g_fmt_mplane,
 966         .vidioc_g_selection             = fimc_lite_g_selection,
 967         .vidioc_s_selection             = fimc_lite_s_selection,
 968         .vidioc_reqbufs                 = fimc_lite_reqbufs,
 969         .vidioc_querybuf                = fimc_lite_querybuf,
 970         .vidioc_prepare_buf             = fimc_lite_prepare_buf,
 971         .vidioc_create_bufs             = fimc_lite_create_bufs,
 972         .vidioc_qbuf                    = fimc_lite_qbuf,
 973         .vidioc_dqbuf                   = fimc_lite_dqbuf,
 974         .vidioc_streamon                = fimc_lite_streamon,
 975         .vidioc_streamoff               = fimc_lite_streamoff,
 976 };
 977
 978 /* Called with the media graph mutex held */
 979 static struct v4l2_subdev *__find_remote_sensor(struct media_entity *me)
 980 {
 981         struct media_pad *pad = &me->pads[0];
 982         struct v4l2_subdev *sd;
 983
 984         while (pad->flags & MEDIA_PAD_FL_SINK) {
 985                 /* source pad */
 986                 pad = media_entity_remote_source(pad);
 987                 if (pad == NULL ||
 988                     media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 989                         break;
 990
 991                 sd = media_entity_to_v4l2_subdev(pad->entity);
 992
 993                 if (sd->grp_id == GRP_ID_FIMC_IS_SENSOR)
 994                         return sd;
 995                 /* sink pad */
 996                 pad = &sd->entity.pads[0];
 997         }
 998         return NULL;
 999 }
1000
1001 /* Capture subdev media entity operations */
1002 static int fimc_lite_link_setup(struct media_entity *entity,
1003                                 const struct media_pad *local,
1004                                 const struct media_pad *remote, u32 flags)
1005 {
1006         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
1007         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1008         unsigned int remote_ent_type = media_entity_type(remote->entity);
1009         int ret = 0;
1010
1011         if (WARN_ON(fimc == NULL))
1012                 return 0;
1013
1014         v4l2_dbg(1, debug, sd, "%s: %s --> %s, flags: 0x%x. source_id: 0x%x\n",
1015                  __func__, remote->entity->name, local->entity->name,
1016                  flags, fimc->source_subdev_grp_id);
1017
1018         mutex_lock(&fimc->lock);
1019
1020         switch (local->index) {
1021         case FLITE_SD_PAD_SINK:
1022                 if (remote_ent_type != MEDIA_ENT_T_V4L2_SUBDEV) {
1023                         ret = -EINVAL;
1024                         break;
1025                 }
1026                 if (flags & MEDIA_LNK_FL_ENABLED) {
1027                         if (fimc->source_subdev_grp_id == 0)
1028                                 fimc->source_subdev_grp_id = sd->grp_id;
1029                         else
1030                                 ret = -EBUSY;
1031                 } else {
1032                         fimc->source_subdev_grp_id = 0;
1033                         fimc->sensor = NULL;
1034                 }
1035                 break;
1036
1037         case FLITE_SD_PAD_SOURCE_DMA:
1038                 if (!(flags & MEDIA_LNK_FL_ENABLED))
1039                         atomic_set(&fimc->out_path, FIMC_IO_NONE);
1040                 else if (remote_ent_type == MEDIA_ENT_T_DEVNODE)
1041                         atomic_set(&fimc->out_path, FIMC_IO_DMA);
1042                 else
1043                         ret = -EINVAL;
1044                 break;
1045
1046         case FLITE_SD_PAD_SOURCE_ISP:
1047                 if (!(flags & MEDIA_LNK_FL_ENABLED))
1048                         atomic_set(&fimc->out_path, FIMC_IO_NONE);
1049                 else if (remote_ent_type == MEDIA_ENT_T_V4L2_SUBDEV)
1050                         atomic_set(&fimc->out_path, FIMC_IO_ISP);
1051                 else
1052                         ret = -EINVAL;
1053                 break;
1054
1055         default:
1056                 v4l2_err(sd, "Invalid pad index\n");
1057                 ret = -EINVAL;
1058         }
1059         mb();
1060
1061         mutex_unlock(&fimc->lock);
1062         return ret;
1063 }
1064
1065 static const struct media_entity_operations fimc_lite_subdev_media_ops = {
1066         .link_setup = fimc_lite_link_setup,
1067 };
1068
1069 static int fimc_lite_subdev_enum_mbus_code(struct v4l2_subdev *sd,
1070                                            struct v4l2_subdev_fh *fh,
1071                                            struct v4l2_subdev_mbus_code_enum *code)
1072 {
1073         const struct fimc_fmt *fmt;
1074
1075         fmt = fimc_lite_find_format(NULL, NULL, code->index);
1076         if (!fmt)
1077                 return -EINVAL;
1078         code->code = fmt->mbus_code;
1079         return 0;
1080 }
1081
1082 static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
1083                                     struct v4l2_subdev_fh *fh,
1084                                     struct v4l2_subdev_format *fmt)
1085 {
1086         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1087         struct v4l2_mbus_framefmt *mf = &fmt->format;
1088         struct flite_frame *f = &fimc->out_frame;
1089
1090         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1091                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1092                 fmt->format = *mf;
1093                 return 0;
1094         }
1095         mf->colorspace = V4L2_COLORSPACE_JPEG;
1096
1097         mutex_lock(&fimc->lock);
1098         mf->code = fimc->fmt->mbus_code;
1099
1100         if (fmt->pad == FLITE_SD_PAD_SINK) {
1101                 /* full camera input frame size */
1102                 mf->width = f->f_width;
1103                 mf->height = f->f_height;
1104         } else {
1105                 /* crop size */
1106                 mf->width = f->rect.width;
1107                 mf->height = f->rect.height;
1108         }
1109         mutex_unlock(&fimc->lock);
1110         return 0;
1111 }
1112
1113 static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
1114                                     struct v4l2_subdev_fh *fh,
1115                                     struct v4l2_subdev_format *fmt)
1116 {
1117         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1118         struct v4l2_mbus_framefmt *mf = &fmt->format;
1119         struct flite_frame *sink = &fimc->inp_frame;
1120         struct flite_frame *source = &fimc->out_frame;
1121         const struct fimc_fmt *ffmt;
1122
1123         v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %dx%d\n",
1124                  fmt->pad, mf->code, mf->width, mf->height);
1125
1126         mf->colorspace = V4L2_COLORSPACE_JPEG;
1127         mutex_lock(&fimc->lock);
1128
1129         if ((atomic_read(&fimc->out_path) == FIMC_IO_ISP &&
1130             sd->entity.stream_count > 0) ||
1131             (atomic_read(&fimc->out_path) == FIMC_IO_DMA &&
1132             vb2_is_busy(&fimc->vb_queue))) {
1133                 mutex_unlock(&fimc->lock);
1134                 return -EBUSY;
1135         }
1136
1137         ffmt = fimc_lite_try_format(fimc, &mf->width, &mf->height,
1138                                     &mf->code, NULL, fmt->pad);
1139
1140         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1141                 mf = v4l2_subdev_get_try_format(fh, fmt->pad);
1142                 *mf = fmt->format;
1143                 mutex_unlock(&fimc->lock);
1144                 return 0;
1145         }
1146
1147         if (fmt->pad == FLITE_SD_PAD_SINK) {
1148                 sink->f_width = mf->width;
1149                 sink->f_height = mf->height;
1150                 fimc->fmt = ffmt;
1151                 /* Set sink crop rectangle */
1152                 sink->rect.width = mf->width;
1153                 sink->rect.height = mf->height;
1154                 sink->rect.left = 0;
1155                 sink->rect.top = 0;
1156                 /* Reset source format and crop rectangle */
1157                 source->rect = sink->rect;
1158                 source->f_width = mf->width;
1159                 source->f_height = mf->height;
1160         } else {
1161                 /* Allow changing format only on sink pad */
1162                 mf->code = fimc->fmt->mbus_code;
1163                 mf->width = sink->rect.width;
1164                 mf->height = sink->rect.height;
1165         }
1166
1167         mutex_unlock(&fimc->lock);
1168         return 0;
1169 }
1170
1171 static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
1172                                           struct v4l2_subdev_fh *fh,
1173                                           struct v4l2_subdev_selection *sel)
1174 {
1175         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1176         struct flite_frame *f = &fimc->inp_frame;
1177
1178         if ((sel->target != V4L2_SEL_TGT_CROP &&
1179              sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
1180              sel->pad != FLITE_SD_PAD_SINK)
1181                 return -EINVAL;
1182
1183         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1184                 sel->r = *v4l2_subdev_get_try_crop(fh, sel->pad);
1185                 return 0;
1186         }
1187
1188         mutex_lock(&fimc->lock);
1189         if (sel->target == V4L2_SEL_TGT_CROP) {
1190                 sel->r = f->rect;
1191         } else {
1192                 sel->r.left = 0;
1193                 sel->r.top = 0;
1194                 sel->r.width = f->f_width;
1195                 sel->r.height = f->f_height;
1196         }
1197         mutex_unlock(&fimc->lock);
1198
1199         v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
1200                  __func__, f->rect.left, f->rect.top, f->rect.width,
1201                  f->rect.height, f->f_width, f->f_height);
1202
1203         return 0;
1204 }
1205
1206 static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
1207                                           struct v4l2_subdev_fh *fh,
1208                                           struct v4l2_subdev_selection *sel)
1209 {
1210         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1211         struct flite_frame *f = &fimc->inp_frame;
1212         int ret = 0;
1213
1214         if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != FLITE_SD_PAD_SINK)
1215                 return -EINVAL;
1216
1217         mutex_lock(&fimc->lock);
1218         fimc_lite_try_crop(fimc, &sel->r);
1219
1220         if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1221                 *v4l2_subdev_get_try_crop(fh, sel->pad) = sel->r;
1222         } else {
1223                 unsigned long flags;
1224                 spin_lock_irqsave(&fimc->slock, flags);
1225                 f->rect = sel->r;
1226                 /* Same crop rectangle on the source pad */
1227                 fimc->out_frame.rect = sel->r;
1228                 set_bit(ST_FLITE_CONFIG, &fimc->state);
1229                 spin_unlock_irqrestore(&fimc->slock, flags);
1230         }
1231         mutex_unlock(&fimc->lock);
1232
1233         v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
1234                  __func__, f->rect.left, f->rect.top, f->rect.width,
1235                  f->rect.height, f->f_width, f->f_height);
1236
1237         return ret;
1238 }
1239
1240 static int fimc_lite_subdev_s_stream(struct v4l2_subdev *sd, int on)
1241 {
1242         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1243         unsigned long flags;
1244         int ret;
1245
1246         /*
1247          * Find sensor subdev linked to FIMC-LITE directly or through
1248          * MIPI-CSIS. This is required for configuration where FIMC-LITE
1249          * is used as a subdev only and feeds data internally to FIMC-IS.
1250          * The pipeline links are protected through entity.stream_count
1251          * so there is no need to take the media graph mutex here.
1252          */
1253         fimc->sensor = __find_remote_sensor(&sd->entity);
1254
1255         if (atomic_read(&fimc->out_path) != FIMC_IO_ISP)
1256                 return -ENOIOCTLCMD;
1257
1258         mutex_lock(&fimc->lock);
1259         if (on) {
1260                 flite_hw_reset(fimc);
1261                 ret = fimc_lite_hw_init(fimc, true);
1262                 if (!ret) {
1263                         spin_lock_irqsave(&fimc->slock, flags);
1264                         flite_hw_capture_start(fimc);
1265                         spin_unlock_irqrestore(&fimc->slock, flags);
1266                 }
1267         } else {
1268                 set_bit(ST_FLITE_OFF, &fimc->state);
1269
1270                 spin_lock_irqsave(&fimc->slock, flags);
1271                 flite_hw_capture_stop(fimc);
1272                 spin_unlock_irqrestore(&fimc->slock, flags);
1273
1274                 ret = wait_event_timeout(fimc->irq_queue,
1275                                 !test_bit(ST_FLITE_OFF, &fimc->state),
1276                                 msecs_to_jiffies(200));
1277                 if (ret == 0)
1278                         v4l2_err(sd, "s_stream(0) timeout\n");
1279                 clear_bit(ST_FLITE_RUN, &fimc->state);
1280         }
1281
1282         mutex_unlock(&fimc->lock);
1283         return ret;
1284 }
1285
1286 static int fimc_lite_log_status(struct v4l2_subdev *sd)
1287 {
1288         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1289
1290         flite_hw_dump_regs(fimc, __func__);
1291         return 0;
1292 }
1293
1294 static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
1295 {
1296         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1297         struct vb2_queue *q = &fimc->vb_queue;
1298         struct video_device *vfd = &fimc->vfd;
1299         int ret;
1300
1301         memset(vfd, 0, sizeof(*vfd));
1302
1303         fimc->fmt = &fimc_lite_formats[0];
1304         atomic_set(&fimc->out_path, FIMC_IO_DMA);
1305
1306         snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
1307                  fimc->index);
1308
1309         vfd->fops = &fimc_lite_fops;
1310         vfd->ioctl_ops = &fimc_lite_ioctl_ops;
1311         vfd->v4l2_dev = sd->v4l2_dev;
1312         vfd->minor = -1;
1313         vfd->release = video_device_release_empty;
1314         vfd->lock = &fimc->lock;
1315         fimc->ref_count = 0;
1316         fimc->reqbufs_count = 0;
1317
1318         INIT_LIST_HEAD(&fimc->pending_buf_q);
1319         INIT_LIST_HEAD(&fimc->active_buf_q);
1320
1321         memset(q, 0, sizeof(*q));
1322         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1323         q->io_modes = VB2_MMAP | VB2_USERPTR;
1324         q->ops = &fimc_lite_qops;
1325         q->mem_ops = &vb2_dma_contig_memops;
1326         q->buf_struct_size = sizeof(struct flite_buffer);
1327         q->drv_priv = fimc;
1328
1329         ret = vb2_queue_init(q);
1330         if (ret < 0)
1331                 return ret;
1332
1333         fimc->vd_pad.flags = MEDIA_PAD_FL_SINK;
1334         ret = media_entity_init(&vfd->entity, 1, &fimc->vd_pad, 0);
1335         if (ret < 0)
1336                 return ret;
1337
1338         video_set_drvdata(vfd, fimc);
1339         fimc->pipeline_ops = v4l2_get_subdev_hostdata(sd);
1340
1341         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
1342         if (ret < 0) {
1343                 media_entity_cleanup(&vfd->entity);
1344                 fimc->pipeline_ops = NULL;
1345                 return ret;
1346         }
1347
1348         v4l2_info(sd->v4l2_dev, "Registered %s as /dev/%s\n",
1349                   vfd->name, video_device_node_name(vfd));
1350         return 0;
1351 }
1352
1353 static void fimc_lite_subdev_unregistered(struct v4l2_subdev *sd)
1354 {
1355         struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
1356
1357         if (fimc == NULL)
1358                 return;
1359
1360         if (video_is_registered(&fimc->vfd)) {
1361                 video_unregister_device(&fimc->vfd);
1362                 media_entity_cleanup(&fimc->vfd.entity);
1363                 fimc->pipeline_ops = NULL;
1364         }
1365 }
1366
1367 static const struct v4l2_subdev_internal_ops fimc_lite_subdev_internal_ops = {
1368         .registered = fimc_lite_subdev_registered,
1369         .unregistered = fimc_lite_subdev_unregistered,
1370 };
1371
1372 static const struct v4l2_subdev_pad_ops fimc_lite_subdev_pad_ops = {
1373         .enum_mbus_code = fimc_lite_subdev_enum_mbus_code,
1374         .get_selection = fimc_lite_subdev_get_selection,
1375         .set_selection = fimc_lite_subdev_set_selection,
1376         .get_fmt = fimc_lite_subdev_get_fmt,
1377         .set_fmt = fimc_lite_subdev_set_fmt,
1378 };
1379
1380 static const struct v4l2_subdev_video_ops fimc_lite_subdev_video_ops = {
1381         .s_stream = fimc_lite_subdev_s_stream,
1382 };
1383
1384 static const struct v4l2_subdev_core_ops fimc_lite_core_ops = {
1385         .log_status = fimc_lite_log_status,
1386 };
1387
1388 static struct v4l2_subdev_ops fimc_lite_subdev_ops = {
1389         .core = &fimc_lite_core_ops,
1390         .video = &fimc_lite_subdev_video_ops,
1391         .pad = &fimc_lite_subdev_pad_ops,
1392 };
1393
1394 static int fimc_lite_s_ctrl(struct v4l2_ctrl *ctrl)
1395 {
1396         struct fimc_lite *fimc = container_of(ctrl->handler, struct fimc_lite,
1397                                               ctrl_handler);
1398         set_bit(ST_FLITE_CONFIG, &fimc->state);
1399         return 0;
1400 }
1401
1402 static const struct v4l2_ctrl_ops fimc_lite_ctrl_ops = {
1403         .s_ctrl = fimc_lite_s_ctrl,
1404 };
1405
1406 static const struct v4l2_ctrl_config fimc_lite_ctrl = {
1407         .ops    = &fimc_lite_ctrl_ops,
1408         .id     = V4L2_CTRL_CLASS_USER | 0x1001,
1409         .type   = V4L2_CTRL_TYPE_BOOLEAN,
1410         .name   = "Test Pattern 640x480",
1411 };
1412
1413 static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
1414 {
1415         struct v4l2_ctrl_handler *handler = &fimc->ctrl_handler;
1416         struct v4l2_subdev *sd = &fimc->subdev;
1417         int ret;
1418
1419         v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
1420         sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
1421         snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
1422
1423         fimc->subdev_pads[FLITE_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1424         fimc->subdev_pads[FLITE_SD_PAD_SOURCE_DMA].flags = MEDIA_PAD_FL_SOURCE;
1425         fimc->subdev_pads[FLITE_SD_PAD_SOURCE_ISP].flags = MEDIA_PAD_FL_SOURCE;
1426         ret = media_entity_init(&sd->entity, FLITE_SD_PADS_NUM,
1427                                 fimc->subdev_pads, 0);
1428         if (ret)
1429                 return ret;
1430
1431         v4l2_ctrl_handler_init(handler, 1);
1432         fimc->test_pattern = v4l2_ctrl_new_custom(handler, &fimc_lite_ctrl,
1433                                                   NULL);
1434         if (handler->error) {
1435                 media_entity_cleanup(&sd->entity);
1436                 return handler->error;
1437         }
1438
1439         sd->ctrl_handler = handler;
1440         sd->internal_ops = &fimc_lite_subdev_internal_ops;
1441         sd->entity.ops = &fimc_lite_subdev_media_ops;
1442         v4l2_set_subdevdata(sd, fimc);
1443
1444         return 0;
1445 }
1446
1447 static void fimc_lite_unregister_capture_subdev(struct fimc_lite *fimc)
1448 {
1449         struct v4l2_subdev *sd = &fimc->subdev;
1450
1451         v4l2_device_unregister_subdev(sd);
1452         media_entity_cleanup(&sd->entity);
1453         v4l2_ctrl_handler_free(&fimc->ctrl_handler);
1454         v4l2_set_subdevdata(sd, NULL);
1455 }
1456
1457 static void fimc_lite_clk_put(struct fimc_lite *fimc)
1458 {
1459         if (IS_ERR_OR_NULL(fimc->clock))
1460                 return;
1461
1462         clk_unprepare(fimc->clock);
1463         clk_put(fimc->clock);
1464         fimc->clock = NULL;
1465 }
1466
1467 static int fimc_lite_clk_get(struct fimc_lite *fimc)
1468 {
1469         int ret;
1470
1471         fimc->clock = clk_get(&fimc->pdev->dev, FLITE_CLK_NAME);
1472         if (IS_ERR(fimc->clock))
1473                 return PTR_ERR(fimc->clock);
1474
1475         ret = clk_prepare(fimc->clock);
1476         if (ret < 0) {
1477                 clk_put(fimc->clock);
1478                 fimc->clock = NULL;
1479         }
1480         return ret;
1481 }
1482
1483 static int fimc_lite_probe(struct platform_device *pdev)
1484 {
1485         struct flite_drvdata *drv_data = fimc_lite_get_drvdata(pdev);
1486         struct fimc_lite *fimc;
1487         struct resource *res;
1488         int ret;
1489
1490         fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
1491         if (!fimc)
1492                 return -ENOMEM;
1493
1494         fimc->index = pdev->id;
1495         fimc->variant = drv_data->variant[fimc->index];
1496         fimc->pdev = pdev;
1497
1498         init_waitqueue_head(&fimc->irq_queue);
1499         spin_lock_init(&fimc->slock);
1500         mutex_init(&fimc->lock);
1501
1502         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1503         fimc->regs = devm_ioremap_resource(&pdev->dev, res);
1504         if (IS_ERR(fimc->regs))
1505                 return PTR_ERR(fimc->regs);
1506
1507         res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
1508         if (res == NULL) {
1509                 dev_err(&pdev->dev, "Failed to get IRQ resource\n");
1510                 return -ENXIO;
1511         }
1512
1513         ret = fimc_lite_clk_get(fimc);
1514         if (ret)
1515                 return ret;
1516
1517         ret = devm_request_irq(&pdev->dev, res->start, flite_irq_handler,
1518                                0, dev_name(&pdev->dev), fimc);
1519         if (ret) {
1520                 dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
1521                 goto err_clk;
1522         }
1523
1524         /* The video node will be created within the subdev's registered() op */
1525         ret = fimc_lite_create_capture_subdev(fimc);
1526         if (ret)
1527                 goto err_clk;
1528
1529         platform_set_drvdata(pdev, fimc);
1530         pm_runtime_enable(&pdev->dev);
1531         ret = pm_runtime_get_sync(&pdev->dev);
1532         if (ret < 0)
1533                 goto err_sd;
1534
1535         fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
1536         if (IS_ERR(fimc->alloc_ctx)) {
1537                 ret = PTR_ERR(fimc->alloc_ctx);
1538                 goto err_pm;
1539         }
1540         pm_runtime_put(&pdev->dev);
1541
1542         dev_dbg(&pdev->dev, "FIMC-LITE.%d registered successfully\n",
1543                 fimc->index);
1544         return 0;
1545 err_pm:
1546         pm_runtime_put(&pdev->dev);
1547 err_sd:
1548         fimc_lite_unregister_capture_subdev(fimc);
1549 err_clk:
1550         fimc_lite_clk_put(fimc);
1551         return ret;
1552 }
1553
1554 static int fimc_lite_runtime_resume(struct device *dev)
1555 {
1556         struct fimc_lite *fimc = dev_get_drvdata(dev);
1557
1558         clk_enable(fimc->clock);
1559         return 0;
1560 }
1561
1562 static int fimc_lite_runtime_suspend(struct device *dev)
1563 {
1564         struct fimc_lite *fimc = dev_get_drvdata(dev);
1565
1566         clk_disable(fimc->clock);
1567         return 0;
1568 }
1569
1570 #ifdef CONFIG_PM_SLEEP
1571 static int fimc_lite_resume(struct device *dev)
1572 {
1573         struct fimc_lite *fimc = dev_get_drvdata(dev);
1574         struct flite_buffer *buf;
1575         unsigned long flags;
1576         int i;
1577
1578         spin_lock_irqsave(&fimc->slock, flags);
1579         if (!test_and_clear_bit(ST_LPM, &fimc->state) ||
1580             !test_bit(ST_FLITE_IN_USE, &fimc->state)) {
1581                 spin_unlock_irqrestore(&fimc->slock, flags);
1582                 return 0;
1583         }
1584         flite_hw_reset(fimc);
1585         spin_unlock_irqrestore(&fimc->slock, flags);
1586
1587         if (!test_and_clear_bit(ST_FLITE_SUSPENDED, &fimc->state))
1588                 return 0;
1589
1590         INIT_LIST_HEAD(&fimc->active_buf_q);
1591         fimc_pipeline_call(fimc, open, &fimc->pipeline,
1592                            &fimc->vfd.entity, false);
1593         fimc_lite_hw_init(fimc, atomic_read(&fimc->out_path) == FIMC_IO_ISP);
1594         clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
1595
1596         for (i = 0; i < fimc->reqbufs_count; i++) {
1597                 if (list_empty(&fimc->pending_buf_q))
1598                         break;
1599                 buf = fimc_lite_pending_queue_pop(fimc);
1600                 buffer_queue(&buf->vb);
1601         }
1602         return 0;
1603 }
1604
1605 static int fimc_lite_suspend(struct device *dev)
1606 {
1607         struct fimc_lite *fimc = dev_get_drvdata(dev);
1608         bool suspend = test_bit(ST_FLITE_IN_USE, &fimc->state);
1609         int ret;
1610
1611         if (test_and_set_bit(ST_LPM, &fimc->state))
1612                 return 0;
1613
1614         ret = fimc_lite_stop_capture(fimc, suspend);
1615         if (ret < 0 || !fimc_lite_active(fimc))
1616                 return ret;
1617
1618         return fimc_pipeline_call(fimc, close, &fimc->pipeline);
1619 }
1620 #endif /* CONFIG_PM_SLEEP */
1621
1622 static int fimc_lite_remove(struct platform_device *pdev)
1623 {
1624         struct fimc_lite *fimc = platform_get_drvdata(pdev);
1625         struct device *dev = &pdev->dev;
1626
1627         pm_runtime_disable(dev);
1628         pm_runtime_set_suspended(dev);
1629         fimc_lite_unregister_capture_subdev(fimc);
1630         vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
1631         fimc_lite_clk_put(fimc);
1632
1633         dev_info(dev, "Driver unloaded\n");
1634         return 0;
1635 }
1636
1637 static struct flite_variant fimc_lite0_variant_exynos4 = {
1638         .max_width              = 8192,
1639         .max_height             = 8192,
1640         .out_width_align        = 8,
1641         .win_hor_offs_align     = 2,
1642         .out_hor_offs_align     = 8,
1643 };
1644
1645 /* EXYNOS4212, EXYNOS4412 */
1646 static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
1647         .variant = {
1648                 [0] = &fimc_lite0_variant_exynos4,
1649                 [1] = &fimc_lite0_variant_exynos4,
1650         },
1651 };
1652
1653 static struct platform_device_id fimc_lite_driver_ids[] = {
1654         {
1655                 .name           = "exynos-fimc-lite",
1656                 .driver_data    = (unsigned long)&fimc_lite_drvdata_exynos4,
1657         },
1658         { /* sentinel */ },
1659 };
1660 MODULE_DEVICE_TABLE(platform, fimc_lite_driver_ids);
1661
1662 static const struct dev_pm_ops fimc_lite_pm_ops = {
1663         SET_SYSTEM_SLEEP_PM_OPS(fimc_lite_suspend, fimc_lite_resume)
1664         SET_RUNTIME_PM_OPS(fimc_lite_runtime_suspend, fimc_lite_runtime_resume,
1665                            NULL)
1666 };
1667
1668 static struct platform_driver fimc_lite_driver = {
1669         .probe          = fimc_lite_probe,
1670         .remove         = fimc_lite_remove,
1671         .id_table       = fimc_lite_driver_ids,
1672         .driver = {
1673                 .name           = FIMC_LITE_DRV_NAME,
1674                 .owner          = THIS_MODULE,
1675                 .pm             = &fimc_lite_pm_ops,
1676         }
1677 };
1678 module_platform_driver(fimc_lite_driver);
1679 MODULE_LICENSE("GPL");
1680 MODULE_ALIAS("platform:" FIMC_LITE_DRV_NAME);