drivers/media/pci/tw686x/tw686x-video.c

   1 /*
   2  * Copyright (C) 2015 VanguardiaSur - www.vanguardiasur.com.ar
   3  *
   4  * Based on original driver by Krzysztof Ha?asa:
   5  * Copyright (C) 2015 Industrial Research Institute for Automation
   6  * and Measurements PIAP
   7  *
   8  * This program is free software; you can redistribute it and/or modify it
   9  * under the terms of version 2 of the GNU General Public License
  10  * as published by the Free Software Foundation.
  11  *
  12  */
  13
  14 #include <linux/init.h>
  15 #include <linux/delay.h>
  16 #include <linux/list.h>
  17 #include <linux/module.h>
  18 #include <linux/kernel.h>
  19 #include <linux/slab.h>
  20 #include <media/v4l2-common.h>
  21 #include <media/v4l2-event.h>
  22 #include <media/videobuf2-vmalloc.h>
  23 #include "tw686x.h"
  24 #include "tw686x-regs.h"
  25
  26 #define TW686X_INPUTS_PER_CH            4
  27 #define TW686X_VIDEO_WIDTH              720
  28 #define TW686X_VIDEO_HEIGHT(id)         ((id & V4L2_STD_525_60) ? 480 : 576)
  29
  30 static const struct tw686x_format formats[] = {
  31         {
  32                 .fourcc = V4L2_PIX_FMT_UYVY,
  33                 .mode = 0,
  34                 .depth = 16,
  35         }, {
  36                 .fourcc = V4L2_PIX_FMT_RGB565,
  37                 .mode = 5,
  38                 .depth = 16,
  39         }, {
  40                 .fourcc = V4L2_PIX_FMT_YUYV,
  41                 .mode = 6,
  42                 .depth = 16,
  43         }
  44 };
  45
  46 static unsigned int tw686x_fields_map(v4l2_std_id std, unsigned int fps)
  47 {
  48         static const unsigned int map[15] = {
  49                 0x00000000, 0x00000001, 0x00004001, 0x00104001, 0x00404041,
  50                 0x01041041, 0x01104411, 0x01111111, 0x04444445, 0x04511445,
  51                 0x05145145, 0x05151515, 0x05515455, 0x05551555, 0x05555555
  52         };
  53
  54         static const unsigned int std_625_50[26] = {
  55                 0, 1, 1, 2,  3,  3,  4,  4,  5,  5,  6,  7,  7,
  56                    8, 8, 9, 10, 10, 11, 11, 12, 13, 13, 14, 14, 0
  57         };
  58
  59         static const unsigned int std_525_60[31] = {
  60                 0, 1, 1, 1, 2,  2,  3,  3,  4,  4,  5,  5,  6,  6, 7, 7,
  61                    8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 13, 13, 14, 0, 0
  62         };
  63
  64         unsigned int i;
  65
  66         if (std & V4L2_STD_525_60) {
  67                 if (fps >= ARRAY_SIZE(std_525_60))
  68                         fps = 30;
  69                 i = std_525_60[fps];
  70         } else {
  71                 if (fps >= ARRAY_SIZE(std_625_50))
  72                         fps = 25;
  73                 i = std_625_50[fps];
  74         }
  75
  76         return map[i];
  77 }
  78
  79 static void tw686x_set_framerate(struct tw686x_video_channel *vc,
  80                                  unsigned int fps)
  81 {
  82         unsigned int map;
  83
  84         if (vc->fps == fps)
  85                 return;
  86
  87         map = tw686x_fields_map(vc->video_standard, fps) << 1;
  88         map |= map << 1;
  89         if (map > 0)
  90                 map |= BIT(31);
  91         reg_write(vc->dev, VIDEO_FIELD_CTRL[vc->ch], map);
  92         vc->fps = fps;
  93 }
  94
  95 static const struct tw686x_format *format_by_fourcc(unsigned int fourcc)
  96 {
  97         unsigned int cnt;
  98
  99         for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++)
 100                 if (formats[cnt].fourcc == fourcc)
 101                         return &formats[cnt];
 102         return NULL;
 103 }
 104
 105 static int tw686x_queue_setup(struct vb2_queue *vq,
 106                               unsigned int *nbuffers, unsigned int *nplanes,
 107                               unsigned int sizes[], void *alloc_ctxs[])
 108 {
 109         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 110         unsigned int szimage =
 111                 (vc->width * vc->height * vc->format->depth) >> 3;
 112
 113         /*
 114          * Let's request at least three buffers: two for the
 115          * DMA engine and one for userspace.
 116          */
 117         if (vq->num_buffers + *nbuffers < 3)
 118                 *nbuffers = 3 - vq->num_buffers;
 119
 120         if (*nplanes) {
 121                 if (*nplanes != 1 || sizes[0] < szimage)
 122                         return -EINVAL;
 123                 return 0;
 124         }
 125
 126         sizes[0] = szimage;
 127         *nplanes = 1;
 128         return 0;
 129 }
 130
 131 static void tw686x_buf_queue(struct vb2_buffer *vb)
 132 {
 133         struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
 134         struct tw686x_dev *dev = vc->dev;
 135         struct pci_dev *pci_dev;
 136         unsigned long flags;
 137         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 138         struct tw686x_v4l2_buf *buf =
 139                 container_of(vbuf, struct tw686x_v4l2_buf, vb);
 140
 141         /* Check device presence */
 142         spin_lock_irqsave(&dev->lock, flags);
 143         pci_dev = dev->pci_dev;
 144         spin_unlock_irqrestore(&dev->lock, flags);
 145         if (!pci_dev) {
 146                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 147                 return;
 148         }
 149
 150         spin_lock_irqsave(&vc->qlock, flags);
 151         list_add_tail(&buf->list, &vc->vidq_queued);
 152         spin_unlock_irqrestore(&vc->qlock, flags);
 153 }
 154
 155 /*
 156  * We can call this even when alloc_dma failed for the given channel
 157  */
 158 static void tw686x_free_dma(struct tw686x_video_channel *vc, unsigned int pb)
 159 {
 160         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
 161         struct tw686x_dev *dev = vc->dev;
 162         struct pci_dev *pci_dev;
 163         unsigned long flags;
 164
 165         /* Check device presence. Shouldn't really happen! */
 166         spin_lock_irqsave(&dev->lock, flags);
 167         pci_dev = dev->pci_dev;
 168         spin_unlock_irqrestore(&dev->lock, flags);
 169         if (!pci_dev) {
 170                 WARN(1, "trying to deallocate on missing device\n");
 171                 return;
 172         }
 173
 174         if (desc->virt) {
 175                 pci_free_consistent(dev->pci_dev, desc->size,
 176                                     desc->virt, desc->phys);
 177                 desc->virt = NULL;
 178         }
 179 }
 180
 181 static int tw686x_alloc_dma(struct tw686x_video_channel *vc, unsigned int pb)
 182 {
 183         struct tw686x_dev *dev = vc->dev;
 184         u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch];
 185         unsigned int len;
 186         void *virt;
 187
 188         WARN(vc->dma_descs[pb].virt,
 189              "Allocating buffer but previous still here\n");
 190
 191         len = (vc->width * vc->height * vc->format->depth) >> 3;
 192         virt = pci_alloc_consistent(dev->pci_dev, len,
 193                                     &vc->dma_descs[pb].phys);
 194         if (!virt) {
 195                 v4l2_err(&dev->v4l2_dev,
 196                          "dma%d: unable to allocate %s-buffer\n",
 197                          vc->ch, pb ? "B" : "P");
 198                 return -ENOMEM;
 199         }
 200         vc->dma_descs[pb].size = len;
 201         vc->dma_descs[pb].virt = virt;
 202         reg_write(dev, reg, vc->dma_descs[pb].phys);
 203
 204         return 0;
 205 }
 206
 207 static void tw686x_buffer_refill(struct tw686x_video_channel *vc,
 208                                  unsigned int pb)
 209 {
 210         struct tw686x_v4l2_buf *buf;
 211
 212         while (!list_empty(&vc->vidq_queued)) {
 213
 214                 buf = list_first_entry(&vc->vidq_queued,
 215                         struct tw686x_v4l2_buf, list);
 216                 list_del(&buf->list);
 217
 218                 vc->curr_bufs[pb] = buf;
 219                 return;
 220         }
 221         vc->curr_bufs[pb] = NULL;
 222 }
 223
 224 static void tw686x_clear_queue(struct tw686x_video_channel *vc,
 225                                enum vb2_buffer_state state)
 226 {
 227         unsigned int pb;
 228
 229         while (!list_empty(&vc->vidq_queued)) {
 230                 struct tw686x_v4l2_buf *buf;
 231
 232                 buf = list_first_entry(&vc->vidq_queued,
 233                         struct tw686x_v4l2_buf, list);
 234                 list_del(&buf->list);
 235                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 236         }
 237
 238         for (pb = 0; pb < 2; pb++) {
 239                 if (vc->curr_bufs[pb])
 240                         vb2_buffer_done(&vc->curr_bufs[pb]->vb.vb2_buf, state);
 241                 vc->curr_bufs[pb] = NULL;
 242         }
 243 }
 244
 245 static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count)
 246 {
 247         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 248         struct tw686x_dev *dev = vc->dev;
 249         struct pci_dev *pci_dev;
 250         unsigned long flags;
 251         int pb, err;
 252
 253         /* Check device presence */
 254         spin_lock_irqsave(&dev->lock, flags);
 255         pci_dev = dev->pci_dev;
 256         spin_unlock_irqrestore(&dev->lock, flags);
 257         if (!pci_dev) {
 258                 err = -ENODEV;
 259                 goto err_clear_queue;
 260         }
 261
 262         spin_lock_irqsave(&vc->qlock, flags);
 263
 264         /* Sanity check */
 265         if (!vc->dma_descs[0].virt || !vc->dma_descs[1].virt) {
 266                 spin_unlock_irqrestore(&vc->qlock, flags);
 267                 v4l2_err(&dev->v4l2_dev,
 268                          "video%d: refusing to start without DMA buffers\n",
 269                          vc->num);
 270                 err = -ENOMEM;
 271                 goto err_clear_queue;
 272         }
 273
 274         for (pb = 0; pb < 2; pb++)
 275                 tw686x_buffer_refill(vc, pb);
 276         spin_unlock_irqrestore(&vc->qlock, flags);
 277
 278         vc->sequence = 0;
 279         vc->pb = 0;
 280
 281         spin_lock_irqsave(&dev->lock, flags);
 282         tw686x_enable_channel(dev, vc->ch);
 283         spin_unlock_irqrestore(&dev->lock, flags);
 284
 285         mod_timer(&dev->dma_delay_timer, jiffies + msecs_to_jiffies(100));
 286
 287         return 0;
 288
 289 err_clear_queue:
 290         spin_lock_irqsave(&vc->qlock, flags);
 291         tw686x_clear_queue(vc, VB2_BUF_STATE_QUEUED);
 292         spin_unlock_irqrestore(&vc->qlock, flags);
 293         return err;
 294 }
 295
 296 static void tw686x_stop_streaming(struct vb2_queue *vq)
 297 {
 298         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 299         struct tw686x_dev *dev = vc->dev;
 300         struct pci_dev *pci_dev;
 301         unsigned long flags;
 302
 303         /* Check device presence */
 304         spin_lock_irqsave(&dev->lock, flags);
 305         pci_dev = dev->pci_dev;
 306         spin_unlock_irqrestore(&dev->lock, flags);
 307         if (pci_dev)
 308                 tw686x_disable_channel(dev, vc->ch);
 309
 310         spin_lock_irqsave(&vc->qlock, flags);
 311         tw686x_clear_queue(vc, VB2_BUF_STATE_ERROR);
 312         spin_unlock_irqrestore(&vc->qlock, flags);
 313 }
 314
 315 static int tw686x_buf_prepare(struct vb2_buffer *vb)
 316 {
 317         struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
 318         unsigned int size =
 319                 (vc->width * vc->height * vc->format->depth) >> 3;
 320
 321         if (vb2_plane_size(vb, 0) < size)
 322                 return -EINVAL;
 323         vb2_set_plane_payload(vb, 0, size);
 324         return 0;
 325 }
 326
 327 static struct vb2_ops tw686x_video_qops = {
 328         .queue_setup            = tw686x_queue_setup,
 329         .buf_queue              = tw686x_buf_queue,
 330         .buf_prepare            = tw686x_buf_prepare,
 331         .start_streaming        = tw686x_start_streaming,
 332         .stop_streaming         = tw686x_stop_streaming,
 333         .wait_prepare           = vb2_ops_wait_prepare,
 334         .wait_finish            = vb2_ops_wait_finish,
 335 };
 336
 337 static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl)
 338 {
 339         struct tw686x_video_channel *vc;
 340         struct tw686x_dev *dev;
 341         unsigned int ch;
 342
 343         vc = container_of(ctrl->handler, struct tw686x_video_channel,
 344                           ctrl_handler);
 345         dev = vc->dev;
 346         ch = vc->ch;
 347
 348         switch (ctrl->id) {
 349         case V4L2_CID_BRIGHTNESS:
 350                 reg_write(dev, BRIGHT[ch], ctrl->val & 0xff);
 351                 return 0;
 352
 353         case V4L2_CID_CONTRAST:
 354                 reg_write(dev, CONTRAST[ch], ctrl->val);
 355                 return 0;
 356
 357         case V4L2_CID_SATURATION:
 358                 reg_write(dev, SAT_U[ch], ctrl->val);
 359                 reg_write(dev, SAT_V[ch], ctrl->val);
 360                 return 0;
 361
 362         case V4L2_CID_HUE:
 363                 reg_write(dev, HUE[ch], ctrl->val & 0xff);
 364                 return 0;
 365         }
 366
 367         return -EINVAL;
 368 }
 369
 370 static const struct v4l2_ctrl_ops ctrl_ops = {
 371         .s_ctrl = tw686x_s_ctrl,
 372 };
 373
 374 static int tw686x_g_fmt_vid_cap(struct file *file, void *priv,
 375                                 struct v4l2_format *f)
 376 {
 377         struct tw686x_video_channel *vc = video_drvdata(file);
 378
 379         f->fmt.pix.width = vc->width;
 380         f->fmt.pix.height = vc->height;
 381         f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 382         f->fmt.pix.pixelformat = vc->format->fourcc;
 383         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 384         f->fmt.pix.bytesperline = (f->fmt.pix.width * vc->format->depth) / 8;
 385         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 386         return 0;
 387 }
 388
 389 static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 390                                   struct v4l2_format *f)
 391 {
 392         struct tw686x_video_channel *vc = video_drvdata(file);
 393         unsigned int video_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
 394         const struct tw686x_format *format;
 395
 396         format = format_by_fourcc(f->fmt.pix.pixelformat);
 397         if (!format) {
 398                 format = &formats[0];
 399                 f->fmt.pix.pixelformat = format->fourcc;
 400         }
 401
 402         if (f->fmt.pix.width <= TW686X_VIDEO_WIDTH / 2)
 403                 f->fmt.pix.width = TW686X_VIDEO_WIDTH / 2;
 404         else
 405                 f->fmt.pix.width = TW686X_VIDEO_WIDTH;
 406
 407         if (f->fmt.pix.height <= video_height / 2)
 408                 f->fmt.pix.height = video_height / 2;
 409         else
 410                 f->fmt.pix.height = video_height;
 411
 412         f->fmt.pix.bytesperline = (f->fmt.pix.width * format->depth) / 8;
 413         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 414         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 415         f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 416
 417         return 0;
 418 }
 419
 420 static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
 421                                 struct v4l2_format *f)
 422 {
 423         struct tw686x_video_channel *vc = video_drvdata(file);
 424         u32 val, width, line_width, height;
 425         unsigned long bitsperframe;
 426         int err, pb;
 427
 428         if (vb2_is_busy(&vc->vidq))
 429                 return -EBUSY;
 430
 431         bitsperframe = vc->width * vc->height * vc->format->depth;
 432         err = tw686x_try_fmt_vid_cap(file, priv, f);
 433         if (err)
 434                 return err;
 435
 436         vc->format = format_by_fourcc(f->fmt.pix.pixelformat);
 437         vc->width = f->fmt.pix.width;
 438         vc->height = f->fmt.pix.height;
 439
 440         /* We need new DMA buffers if the framesize has changed */
 441         if (bitsperframe != vc->width * vc->height * vc->format->depth) {
 442                 for (pb = 0; pb < 2; pb++)
 443                         tw686x_free_dma(vc, pb);
 444
 445                 for (pb = 0; pb < 2; pb++) {
 446                         err = tw686x_alloc_dma(vc, pb);
 447                         if (err) {
 448                                 if (pb > 0)
 449                                         tw686x_free_dma(vc, 0);
 450                                 return err;
 451                         }
 452                 }
 453         }
 454
 455         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 456
 457         if (vc->width <= TW686X_VIDEO_WIDTH / 2)
 458                 val |= BIT(23);
 459         else
 460                 val &= ~BIT(23);
 461
 462         if (vc->height <= TW686X_VIDEO_HEIGHT(vc->video_standard) / 2)
 463                 val |= BIT(24);
 464         else
 465                 val &= ~BIT(24);
 466
 467         val &= ~(0x7 << 20);
 468         val |= vc->format->mode << 20;
 469         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 470
 471         /* Program the DMA frame size */
 472         width = (vc->width * 2) & 0x7ff;
 473         height = vc->height / 2;
 474         line_width = (vc->width * 2) & 0x7ff;
 475         val = (height << 22) | (line_width << 11)  | width;
 476         reg_write(vc->dev, VDMA_WHP[vc->ch], val);
 477         return 0;
 478 }
 479
 480 static int tw686x_querycap(struct file *file, void *priv,
 481                            struct v4l2_capability *cap)
 482 {
 483         struct tw686x_video_channel *vc = video_drvdata(file);
 484         struct tw686x_dev *dev = vc->dev;
 485
 486         strlcpy(cap->driver, "tw686x", sizeof(cap->driver));
 487         strlcpy(cap->card, dev->name, sizeof(cap->card));
 488         snprintf(cap->bus_info, sizeof(cap->bus_info),
 489                  "PCI:%s", pci_name(dev->pci_dev));
 490         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 491                            V4L2_CAP_READWRITE;
 492         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 493         return 0;
 494 }
 495
 496 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 497 {
 498         struct tw686x_video_channel *vc = video_drvdata(file);
 499         struct v4l2_format f;
 500         u32 val, ret;
 501
 502         if (vc->video_standard == id)
 503                 return 0;
 504
 505         if (vb2_is_busy(&vc->vidq))
 506                 return -EBUSY;
 507
 508         if (id & V4L2_STD_NTSC)
 509                 val = 0;
 510         else if (id & V4L2_STD_PAL)
 511                 val = 1;
 512         else if (id & V4L2_STD_SECAM)
 513                 val = 2;
 514         else if (id & V4L2_STD_NTSC_443)
 515                 val = 3;
 516         else if (id & V4L2_STD_PAL_M)
 517                 val = 4;
 518         else if (id & V4L2_STD_PAL_Nc)
 519                 val = 5;
 520         else if (id & V4L2_STD_PAL_60)
 521                 val = 6;
 522         else
 523                 return -EINVAL;
 524
 525         vc->video_standard = id;
 526         reg_write(vc->dev, SDT[vc->ch], val);
 527
 528         val = reg_read(vc->dev, VIDEO_CONTROL1);
 529         if (id & V4L2_STD_525_60)
 530                 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 531         else
 532                 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 533         reg_write(vc->dev, VIDEO_CONTROL1, val);
 534
 535         /*
 536          * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
 537          * calling g_fmt and s_fmt will sanitize the height
 538          * according to the standard.
 539          */
 540         ret = tw686x_g_fmt_vid_cap(file, priv, &f);
 541         if (!ret)
 542                 tw686x_s_fmt_vid_cap(file, priv, &f);
 543         return 0;
 544 }
 545
 546 static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)
 547 {
 548         struct tw686x_video_channel *vc = video_drvdata(file);
 549         struct tw686x_dev *dev = vc->dev;
 550         unsigned int old_std, detected_std = 0;
 551         unsigned long end;
 552
 553         if (vb2_is_streaming(&vc->vidq))
 554                 return -EBUSY;
 555
 556         /* Enable and start standard detection */
 557         old_std = reg_read(dev, SDT[vc->ch]);
 558         reg_write(dev, SDT[vc->ch], 0x7);
 559         reg_write(dev, SDT_EN[vc->ch], 0xff);
 560
 561         end = jiffies + msecs_to_jiffies(500);
 562         while (time_is_after_jiffies(end)) {
 563
 564                 detected_std = reg_read(dev, SDT[vc->ch]);
 565                 if (!(detected_std & BIT(7)))
 566                         break;
 567                 msleep(100);
 568         }
 569         reg_write(dev, SDT[vc->ch], old_std);
 570
 571         /* Exit if still busy */
 572         if (detected_std & BIT(7))
 573                 return 0;
 574
 575         detected_std = (detected_std >> 4) & 0x7;
 576         switch (detected_std) {
 577         case TW686X_STD_NTSC_M:
 578                 *std &= V4L2_STD_NTSC;
 579                 break;
 580         case TW686X_STD_NTSC_443:
 581                 *std &= V4L2_STD_NTSC_443;
 582                 break;
 583         case TW686X_STD_PAL_M:
 584                 *std &= V4L2_STD_PAL_M;
 585                 break;
 586         case TW686X_STD_PAL_60:
 587                 *std &= V4L2_STD_PAL_60;
 588                 break;
 589         case TW686X_STD_PAL:
 590                 *std &= V4L2_STD_PAL;
 591                 break;
 592         case TW686X_STD_PAL_CN:
 593                 *std &= V4L2_STD_PAL_Nc;
 594                 break;
 595         case TW686X_STD_SECAM:
 596                 *std &= V4L2_STD_SECAM;
 597                 break;
 598         default:
 599                 *std = 0;
 600         }
 601         return 0;
 602 }
 603
 604 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 605 {
 606         struct tw686x_video_channel *vc = video_drvdata(file);
 607
 608         *id = vc->video_standard;
 609         return 0;
 610 }
 611
 612 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 613                                    struct v4l2_fmtdesc *f)
 614 {
 615         if (f->index >= ARRAY_SIZE(formats))
 616                 return -EINVAL;
 617         f->pixelformat = formats[f->index].fourcc;
 618         return 0;
 619 }
 620
 621 static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
 622 {
 623         struct tw686x_video_channel *vc = video_drvdata(file);
 624         u32 val;
 625
 626         if (i >= TW686X_INPUTS_PER_CH)
 627                 return -EINVAL;
 628         if (i == vc->input)
 629                 return 0;
 630         /*
 631          * Not sure we are able to support on the fly input change
 632          */
 633         if (vb2_is_busy(&vc->vidq))
 634                 return -EBUSY;
 635
 636         vc->input = i;
 637
 638         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 639         val &= ~(0x3 << 30);
 640         val |= i << 30;
 641         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 642         return 0;
 643 }
 644
 645 static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)
 646 {
 647         struct tw686x_video_channel *vc = video_drvdata(file);
 648
 649         *i = vc->input;
 650         return 0;
 651 }
 652
 653 static int tw686x_enum_input(struct file *file, void *priv,
 654                              struct v4l2_input *i)
 655 {
 656         struct tw686x_video_channel *vc = video_drvdata(file);
 657         unsigned int vidstat;
 658
 659         if (i->index >= TW686X_INPUTS_PER_CH)
 660                 return -EINVAL;
 661
 662         snprintf(i->name, sizeof(i->name), "Composite%d", i->index);
 663         i->type = V4L2_INPUT_TYPE_CAMERA;
 664         i->std = vc->device->tvnorms;
 665         i->capabilities = V4L2_IN_CAP_STD;
 666
 667         vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);
 668         i->status = 0;
 669         if (vidstat & TW686X_VIDSTAT_VDLOSS)
 670                 i->status |= V4L2_IN_ST_NO_SIGNAL;
 671         if (!(vidstat & TW686X_VIDSTAT_HLOCK))
 672                 i->status |= V4L2_IN_ST_NO_H_LOCK;
 673
 674         return 0;
 675 }
 676
 677 static const struct v4l2_file_operations tw686x_video_fops = {
 678         .owner          = THIS_MODULE,
 679         .open           = v4l2_fh_open,
 680         .unlocked_ioctl = video_ioctl2,
 681         .release        = vb2_fop_release,
 682         .poll           = vb2_fop_poll,
 683         .read           = vb2_fop_read,
 684         .mmap           = vb2_fop_mmap,
 685 };
 686
 687 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
 688         .vidioc_querycap                = tw686x_querycap,
 689         .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
 690         .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
 691         .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
 692         .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
 693
 694         .vidioc_querystd                = tw686x_querystd,
 695         .vidioc_g_std                   = tw686x_g_std,
 696         .vidioc_s_std                   = tw686x_s_std,
 697
 698         .vidioc_enum_input              = tw686x_enum_input,
 699         .vidioc_g_input                 = tw686x_g_input,
 700         .vidioc_s_input                 = tw686x_s_input,
 701
 702         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 703         .vidioc_querybuf                = vb2_ioctl_querybuf,
 704         .vidioc_qbuf                    = vb2_ioctl_qbuf,
 705         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 706         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 707         .vidioc_streamon                = vb2_ioctl_streamon,
 708         .vidioc_streamoff               = vb2_ioctl_streamoff,
 709         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 710
 711         .vidioc_log_status              = v4l2_ctrl_log_status,
 712         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
 713         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 714 };
 715
 716 static void tw686x_buffer_copy(struct tw686x_video_channel *vc,
 717                                unsigned int pb, struct vb2_v4l2_buffer *vb)
 718 {
 719         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
 720         struct vb2_buffer *vb2_buf = &vb->vb2_buf;
 721
 722         vb->field = V4L2_FIELD_INTERLACED;
 723         vb->sequence = vc->sequence++;
 724
 725         memcpy(vb2_plane_vaddr(vb2_buf, 0), desc->virt, desc->size);
 726         vb2_buf->timestamp = ktime_get_ns();
 727         vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);
 728 }
 729
 730 void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,
 731                       unsigned int pb_status, unsigned int fifo_status,
 732                       unsigned int *reset_ch)
 733 {
 734         struct tw686x_video_channel *vc;
 735         struct vb2_v4l2_buffer *vb;
 736         unsigned long flags;
 737         unsigned int ch, pb;
 738
 739         for_each_set_bit(ch, &requests, max_channels(dev)) {
 740                 vc = &dev->video_channels[ch];
 741
 742                 /*
 743                  * This can either be a blue frame (with signal-lost bit set)
 744                  * or a good frame (with signal-lost bit clear). If we have just
 745                  * got signal, then this channel needs resetting.
 746                  */
 747                 if (vc->no_signal && !(fifo_status & BIT(ch))) {
 748                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
 749                                     "video%d: signal recovered\n", vc->num);
 750                         vc->no_signal = false;
 751                         *reset_ch |= BIT(ch);
 752                         vc->pb = 0;
 753                         continue;
 754                 }
 755                 vc->no_signal = !!(fifo_status & BIT(ch));
 756
 757                 /* Check FIFO errors only if there's signal */
 758                 if (!vc->no_signal) {
 759                         u32 fifo_ov, fifo_bad;
 760
 761                         fifo_ov = (fifo_status >> 24) & BIT(ch);
 762                         fifo_bad = (fifo_status >> 16) & BIT(ch);
 763                         if (fifo_ov || fifo_bad) {
 764                                 /* Mark this channel for reset */
 765                                 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
 766                                             "video%d: FIFO error\n", vc->num);
 767                                 *reset_ch |= BIT(ch);
 768                                 vc->pb = 0;
 769                                 continue;
 770                         }
 771                 }
 772
 773                 pb = !!(pb_status & BIT(ch));
 774                 if (vc->pb != pb) {
 775                         /* Mark this channel for reset */
 776                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
 777                                     "video%d: unexpected p-b buffer!\n",
 778                                     vc->num);
 779                         *reset_ch |= BIT(ch);
 780                         vc->pb = 0;
 781                         continue;
 782                 }
 783
 784                 /* handle video stream */
 785                 spin_lock_irqsave(&vc->qlock, flags);
 786                 if (vc->curr_bufs[pb]) {
 787                         vb = &vc->curr_bufs[pb]->vb;
 788                         tw686x_buffer_copy(vc, pb, vb);
 789                 }
 790                 vc->pb = !pb;
 791                 tw686x_buffer_refill(vc, pb);
 792                 spin_unlock_irqrestore(&vc->qlock, flags);
 793         }
 794 }
 795
 796 void tw686x_video_free(struct tw686x_dev *dev)
 797 {
 798         unsigned int ch, pb;
 799
 800         for (ch = 0; ch < max_channels(dev); ch++) {
 801                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
 802
 803                 if (vc->device)
 804                         video_unregister_device(vc->device);
 805
 806                 for (pb = 0; pb < 2; pb++)
 807                         tw686x_free_dma(vc, pb);
 808         }
 809 }
 810
 811 int tw686x_video_init(struct tw686x_dev *dev)
 812 {
 813         unsigned int ch, val, pb;
 814         int err;
 815
 816         err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
 817         if (err)
 818                 return err;
 819
 820         for (ch = 0; ch < max_channels(dev); ch++) {
 821                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
 822                 struct video_device *vdev;
 823
 824                 mutex_init(&vc->vb_mutex);
 825                 spin_lock_init(&vc->qlock);
 826                 INIT_LIST_HEAD(&vc->vidq_queued);
 827
 828                 vc->dev = dev;
 829                 vc->ch = ch;
 830
 831                 /* default settings */
 832                 vc->format = &formats[0];
 833                 vc->video_standard = V4L2_STD_NTSC;
 834                 vc->width = TW686X_VIDEO_WIDTH;
 835                 vc->height = TW686X_VIDEO_HEIGHT(vc->video_standard);
 836                 vc->input = 0;
 837
 838                 reg_write(vc->dev, SDT[ch], 0);
 839                 tw686x_set_framerate(vc, 30);
 840
 841                 reg_write(dev, VDELAY_LO[ch], 0x14);
 842                 reg_write(dev, HACTIVE_LO[ch], 0xd0);
 843                 reg_write(dev, VIDEO_SIZE[ch], 0);
 844
 845                 for (pb = 0; pb < 2; pb++) {
 846                         err = tw686x_alloc_dma(vc, pb);
 847                         if (err)
 848                                 goto error;
 849                 }
 850
 851                 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
 852                 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 853                 vc->vidq.drv_priv = vc;
 854                 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);
 855                 vc->vidq.ops = &tw686x_video_qops;
 856                 vc->vidq.mem_ops = &vb2_vmalloc_memops;
 857                 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 858                 vc->vidq.min_buffers_needed = 2;
 859                 vc->vidq.lock = &vc->vb_mutex;
 860                 vc->vidq.gfp_flags = GFP_DMA32;
 861
 862                 err = vb2_queue_init(&vc->vidq);
 863                 if (err) {
 864                         v4l2_err(&dev->v4l2_dev,
 865                                  "dma%d: cannot init vb2 queue\n", ch);
 866                         goto error;
 867                 }
 868
 869                 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);
 870                 if (err) {
 871                         v4l2_err(&dev->v4l2_dev,
 872                                  "dma%d: cannot init ctrl handler\n", ch);
 873                         goto error;
 874                 }
 875                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 876                                   V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
 877                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 878                                   V4L2_CID_CONTRAST, 0, 255, 1, 100);
 879                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 880                                   V4L2_CID_SATURATION, 0, 255, 1, 128);
 881                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 882                                   V4L2_CID_HUE, -128, 127, 1, 0);
 883                 err = vc->ctrl_handler.error;
 884                 if (err)
 885                         goto error;
 886
 887                 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);
 888                 if (err)
 889                         goto error;
 890
 891                 vdev = video_device_alloc();
 892                 if (!vdev) {
 893                         v4l2_err(&dev->v4l2_dev,
 894                                  "dma%d: unable to allocate device\n", ch);
 895                         err = -ENOMEM;
 896                         goto error;
 897                 }
 898
 899                 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
 900                 vdev->fops = &tw686x_video_fops;
 901                 vdev->ioctl_ops = &tw686x_video_ioctl_ops;
 902                 vdev->release = video_device_release;
 903                 vdev->v4l2_dev = &dev->v4l2_dev;
 904                 vdev->queue = &vc->vidq;
 905                 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;
 906                 vdev->minor = -1;
 907                 vdev->lock = &vc->vb_mutex;
 908                 vdev->ctrl_handler = &vc->ctrl_handler;
 909                 vc->device = vdev;
 910                 video_set_drvdata(vdev, vc);
 911
 912                 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 913                 if (err < 0)
 914                         goto error;
 915                 vc->num = vdev->num;
 916         }
 917
 918         /* Set DMA frame mode on all channels. Only supported mode for now. */
 919         val = TW686X_DEF_PHASE_REF;
 920         for (ch = 0; ch < max_channels(dev); ch++)
 921                 val |= TW686X_FRAME_MODE << (16 + ch * 2);
 922         reg_write(dev, PHASE_REF, val);
 923
 924         reg_write(dev, MISC2[0], 0xe7);
 925         reg_write(dev, VCTRL1[0], 0xcc);
 926         reg_write(dev, LOOP[0], 0xa5);
 927         if (max_channels(dev) > 4) {
 928                 reg_write(dev, VCTRL1[1], 0xcc);
 929                 reg_write(dev, LOOP[1], 0xa5);
 930                 reg_write(dev, MISC2[1], 0xe7);
 931         }
 932         return 0;
 933
 934 error:
 935         tw686x_video_free(dev);
 936         return err;
 937 }