drivers/staging/media/tw686x-kh/tw686x-kh-video.c

   1 /*
   2  * Copyright (C) 2015 Industrial Research Institute for Automation
   3  * and Measurements PIAP
   4  *
   5  * Written by Krzysztof Ha?asa.
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of version 2 of the GNU General Public License
   9  * as published by the Free Software Foundation.
  10  */
  11
  12 #include <linux/init.h>
  13 #include <linux/list.h>
  14 #include <linux/module.h>
  15 #include <linux/kernel.h>
  16 #include <linux/slab.h>
  17 #include <media/v4l2-common.h>
  18 #include <media/v4l2-event.h>
  19 #include "tw686x-kh.h"
  20 #include "tw686x-kh-regs.h"
  21
  22 #define MAX_SG_ENTRY_SIZE (/* 8192 - 128 */ 4096)
  23 #define MAX_SG_DESC_COUNT 256 /* PAL 704x576 needs up to 198 4-KB pages */
  24
  25 static const struct tw686x_format formats[] = {
  26         {
  27                 .name = "4:2:2 packed, UYVY", /* aka Y422 */
  28                 .fourcc = V4L2_PIX_FMT_UYVY,
  29                 .mode = 0,
  30                 .depth = 16,
  31         }, {
  32 #if 0
  33                 .name = "4:2:0 packed, YUV",
  34                 .mode = 1,      /* non-standard */
  35                 .depth = 12,
  36         }, {
  37                 .name = "4:1:1 packed, YUV",
  38                 .mode = 2,      /* non-standard */
  39                 .depth = 12,
  40         }, {
  41 #endif
  42                 .name = "4:1:1 packed, YUV",
  43                 .fourcc = V4L2_PIX_FMT_Y41P,
  44                 .mode = 3,
  45                 .depth = 12,
  46         }, {
  47                 .name = "15 bpp RGB",
  48                 .fourcc = V4L2_PIX_FMT_RGB555,
  49                 .mode = 4,
  50                 .depth = 16,
  51         }, {
  52                 .name = "16 bpp RGB",
  53                 .fourcc = V4L2_PIX_FMT_RGB565,
  54                 .mode = 5,
  55                 .depth = 16,
  56         }, {
  57                 .name = "4:2:2 packed, YUYV",
  58                 .fourcc = V4L2_PIX_FMT_YUYV,
  59                 .mode = 6,
  60                 .depth = 16,
  61         }
  62         /* mode 7 is "reserved" */
  63 };
  64
  65 static const v4l2_std_id video_standards[7] = {
  66         V4L2_STD_NTSC,
  67         V4L2_STD_PAL,
  68         V4L2_STD_SECAM,
  69         V4L2_STD_NTSC_443,
  70         V4L2_STD_PAL_M,
  71         V4L2_STD_PAL_N,
  72         V4L2_STD_PAL_60,
  73 };
  74
  75 static const struct tw686x_format *format_by_fourcc(unsigned int fourcc)
  76 {
  77         unsigned int cnt;
  78
  79         for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++)
  80                 if (formats[cnt].fourcc == fourcc)
  81                         return &formats[cnt];
  82         return NULL;
  83 }
  84
  85 static void tw686x_get_format(struct tw686x_video_channel *vc,
  86                               struct v4l2_format *f)
  87 {
  88         const struct tw686x_format *format;
  89         unsigned int width, height, height_div = 1;
  90
  91         format = format_by_fourcc(f->fmt.pix.pixelformat);
  92         if (!format) {
  93                 format = &formats[0];
  94                 f->fmt.pix.pixelformat = format->fourcc;
  95         }
  96
  97         width = 704;
  98         if (f->fmt.pix.width < width * 3 / 4 /* halfway */)
  99                 width /= 2;
 100
 101         height = (vc->video_standard & V4L2_STD_625_50) ? 576 : 480;
 102         if (f->fmt.pix.height < height * 3 / 4 /* halfway */)
 103                 height_div = 2;
 104
 105         switch (f->fmt.pix.field) {
 106         case V4L2_FIELD_TOP:
 107         case V4L2_FIELD_BOTTOM:
 108                 height_div = 2;
 109                 break;
 110         case V4L2_FIELD_SEQ_BT:
 111                 if (height_div > 1)
 112                         f->fmt.pix.field = V4L2_FIELD_BOTTOM;
 113                 break;
 114         default:
 115                 if (height_div > 1)
 116                         f->fmt.pix.field = V4L2_FIELD_TOP;
 117                 else
 118                         f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
 119         }
 120         height /= height_div;
 121
 122         f->fmt.pix.width = width;
 123         f->fmt.pix.height = height;
 124         f->fmt.pix.bytesperline = f->fmt.pix.width * format->depth / 8;
 125         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 126         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 127 }
 128
 129 /* video queue operations */
 130
 131 static int tw686x_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
 132                               unsigned int *nplanes, unsigned int sizes[],
 133                               void *alloc_ctxs[])
 134 {
 135         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 136         unsigned int size = vc->width * vc->height * vc->format->depth / 8;
 137
 138         alloc_ctxs[0] = vc->alloc_ctx;
 139         if (*nbuffers < 2)
 140                 *nbuffers = 2;
 141
 142         if (*nplanes)
 143                 return sizes[0] < size ? -EINVAL : 0;
 144
 145         sizes[0] = size;
 146         *nplanes = 1;           /* packed formats only */
 147         return 0;
 148 }
 149
 150 static void tw686x_buf_queue(struct vb2_buffer *vb)
 151 {
 152         struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
 153         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 154         struct tw686x_vb2_buf *buf;
 155
 156         buf = container_of(vbuf, struct tw686x_vb2_buf, vb);
 157
 158         spin_lock(&vc->qlock);
 159         list_add_tail(&buf->list, &vc->vidq_queued);
 160         spin_unlock(&vc->qlock);
 161 }
 162
 163 static void setup_descs(struct tw686x_video_channel *vc, unsigned int n)
 164 {
 165 loop:
 166         while (!list_empty(&vc->vidq_queued)) {
 167                 struct vdma_desc *descs = vc->sg_descs[n];
 168                 struct tw686x_vb2_buf *buf;
 169                 struct sg_table *vbuf;
 170                 struct scatterlist *sg;
 171                 unsigned int buf_len, count = 0;
 172                 int i;
 173
 174                 buf = list_first_entry(&vc->vidq_queued, struct tw686x_vb2_buf,
 175                                        list);
 176                 list_del(&buf->list);
 177
 178                 buf_len = vc->width * vc->height * vc->format->depth / 8;
 179                 if (vb2_plane_size(&buf->vb.vb2_buf, 0) < buf_len) {
 180                         pr_err("Video buffer size too small\n");
 181                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 182                         goto loop; /* try another */
 183                 }
 184
 185                 vbuf = vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0);
 186                 for_each_sg(vbuf->sgl, sg, vbuf->nents, i) {
 187                         dma_addr_t phys = sg_dma_address(sg);
 188                         unsigned int len = sg_dma_len(sg);
 189
 190                         while (len && buf_len) {
 191                                 unsigned int entry_len = min_t(unsigned int, len,
 192                                                            MAX_SG_ENTRY_SIZE);
 193                                 entry_len = min(entry_len, buf_len);
 194                                 if (count == MAX_SG_DESC_COUNT) {
 195                                         pr_err("Video buffer size too fragmented\n");
 196                                         vb2_buffer_done(&buf->vb.vb2_buf,
 197                                                         VB2_BUF_STATE_ERROR);
 198                                         goto loop;
 199                                 }
 200                                 descs[count].phys = cpu_to_le32(phys);
 201                                 descs[count++].flags_length =
 202                                         cpu_to_le32(0x40000000 /* available */ |
 203                                                     entry_len);
 204                                 phys += entry_len;
 205                                 len -= entry_len;
 206                                 buf_len -= entry_len;
 207                         }
 208                         if (!buf_len)
 209                                 break;
 210                 }
 211
 212                 /* clear the remaining entries */
 213                 while (count < MAX_SG_DESC_COUNT) {
 214                         descs[count].phys = 0;
 215                         descs[count++].flags_length = 0; /* unavailable */
 216                 }
 217
 218                 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
 219                 vc->curr_bufs[n] = buf;
 220                 return;
 221         }
 222         vc->curr_bufs[n] = NULL;
 223 }
 224
 225 /* On TW6864 and TW6868, all channels share the pair of video DMA SG tables,
 226    with 10-bit start_idx and end_idx determining start and end of frame buffer
 227    for particular channel.
 228    TW6868 with all its 8 channels would be problematic (only 127 SG entries per
 229    channel) but we support only 4 channels on this chip anyway (the first
 230    4 channels are driven with internal video decoder, the other 4 would require
 231    an external TW286x part).
 232
 233    On TW6865 and TW6869, each channel has its own DMA SG table, with indexes
 234    starting with 0. Both chips have complete sets of internal video decoders
 235    (respectively 4 or 8-channel).
 236
 237    All chips have separate SG tables for two video frames. */
 238
 239 static void setup_dma_cfg(struct tw686x_video_channel *vc)
 240 {
 241         unsigned int field_width = 704;
 242         unsigned int field_height = (vc->video_standard & V4L2_STD_625_50) ?
 243                 288 : 240;
 244         unsigned int start_idx = is_second_gen(vc->dev) ? 0 :
 245                 vc->ch * MAX_SG_DESC_COUNT;
 246         unsigned int end_idx = start_idx + MAX_SG_DESC_COUNT - 1;
 247         u32 dma_cfg = (0 << 30) /* input selection */ |
 248                 (1 << 29) /* field2 dropped (if any) */ |
 249                 ((vc->height < 300) << 28) /* field dropping */ |
 250                 (1 << 27) /* master */ |
 251                 (0 << 25) /* master channel (for slave only) */ |
 252                 (0 << 24) /* (no) vertical (line) decimation */ |
 253                 ((vc->width < 400) << 23) /* horizontal decimation */ |
 254                 (vc->format->mode << 20) /* output video format */ |
 255                 (end_idx << 10) /* DMA end index */ |
 256                 start_idx /* DMA start index */;
 257         u32 reg;
 258
 259         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], dma_cfg);
 260         reg_write(vc->dev, VIDEO_SIZE[vc->ch], (1 << 31) | (field_height << 16)
 261                   | field_width);
 262         reg = reg_read(vc->dev, VIDEO_CONTROL1);
 263         if (vc->video_standard & V4L2_STD_625_50)
 264                 reg |= 1 << (vc->ch + 13);
 265         else
 266                 reg &= ~(1 << (vc->ch + 13));
 267         reg_write(vc->dev, VIDEO_CONTROL1, reg);
 268 }
 269
 270 static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count)
 271 {
 272         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 273         struct tw686x_dev *dev = vc->dev;
 274         u32 dma_ch_mask;
 275         unsigned int n;
 276
 277         setup_dma_cfg(vc);
 278
 279         /* queue video buffers if available */
 280         spin_lock(&vc->qlock);
 281         for (n = 0; n < 2; n++)
 282                 setup_descs(vc, n);
 283         spin_unlock(&vc->qlock);
 284
 285         dev->video_active |= 1 << vc->ch;
 286         vc->seq = 0;
 287         dma_ch_mask = reg_read(dev, DMA_CHANNEL_ENABLE) | (1 << vc->ch);
 288         reg_write(dev, DMA_CHANNEL_ENABLE, dma_ch_mask);
 289         reg_write(dev, DMA_CMD, (1 << 31) | dma_ch_mask);
 290         return 0;
 291 }
 292
 293 static void tw686x_stop_streaming(struct vb2_queue *vq)
 294 {
 295         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 296         struct tw686x_dev *dev = vc->dev;
 297         u32 dma_ch_mask = reg_read(dev, DMA_CHANNEL_ENABLE);
 298         u32 dma_cmd = reg_read(dev, DMA_CMD);
 299         unsigned int n;
 300
 301         dma_ch_mask &= ~(1 << vc->ch);
 302         reg_write(dev, DMA_CHANNEL_ENABLE, dma_ch_mask);
 303
 304         dev->video_active &= ~(1 << vc->ch);
 305
 306         dma_cmd &= ~(1 << vc->ch);
 307         reg_write(dev, DMA_CMD, dma_cmd);
 308
 309         if (!dev->video_active) {
 310                 reg_write(dev, DMA_CMD, 0);
 311                 reg_write(dev, DMA_CHANNEL_ENABLE, 0);
 312         }
 313
 314         spin_lock(&vc->qlock);
 315         while (!list_empty(&vc->vidq_queued)) {
 316                 struct tw686x_vb2_buf *buf;
 317
 318                 buf = list_entry(vc->vidq_queued.next, struct tw686x_vb2_buf,
 319                                  list);
 320                 list_del(&buf->list);
 321                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 322         }
 323
 324         for (n = 0; n < 2; n++)
 325                 if (vc->curr_bufs[n])
 326                         vb2_buffer_done(&vc->curr_bufs[n]->vb.vb2_buf,
 327                                         VB2_BUF_STATE_ERROR);
 328
 329         spin_unlock(&vc->qlock);
 330 }
 331
 332 static struct vb2_ops tw686x_video_qops = {
 333         .queue_setup            = tw686x_queue_setup,
 334         .buf_queue              = tw686x_buf_queue,
 335         .start_streaming        = tw686x_start_streaming,
 336         .stop_streaming         = tw686x_stop_streaming,
 337         .wait_prepare           = vb2_ops_wait_prepare,
 338         .wait_finish            = vb2_ops_wait_finish,
 339 };
 340
 341 static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl)
 342 {
 343         struct tw686x_video_channel *vc;
 344         struct tw686x_dev *dev;
 345         unsigned int ch;
 346
 347         vc = container_of(ctrl->handler, struct tw686x_video_channel,
 348                           ctrl_handler);
 349         dev = vc->dev;
 350         ch = vc->ch;
 351
 352         switch (ctrl->id) {
 353         case V4L2_CID_BRIGHTNESS:
 354                 reg_write(dev, BRIGHT[ch], ctrl->val & 0xFF);
 355                 return 0;
 356
 357         case V4L2_CID_CONTRAST:
 358                 reg_write(dev, CONTRAST[ch], ctrl->val);
 359                 return 0;
 360
 361         case V4L2_CID_SATURATION:
 362                 reg_write(dev, SAT_U[ch], ctrl->val);
 363                 reg_write(dev, SAT_V[ch], ctrl->val);
 364                 return 0;
 365
 366         case V4L2_CID_HUE:
 367                 reg_write(dev, HUE[ch], ctrl->val & 0xFF);
 368                 return 0;
 369         }
 370
 371         return -EINVAL;
 372 }
 373
 374 static const struct v4l2_ctrl_ops ctrl_ops = {
 375         .s_ctrl = tw686x_s_ctrl,
 376 };
 377
 378 static int tw686x_g_fmt_vid_cap(struct file *file, void *priv,
 379                                 struct v4l2_format *f)
 380 {
 381         struct tw686x_video_channel *vc = video_drvdata(file);
 382
 383         f->fmt.pix.width = vc->width;
 384         f->fmt.pix.height = vc->height;
 385         f->fmt.pix.field = vc->field;
 386         f->fmt.pix.pixelformat = vc->format->fourcc;
 387         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 388         f->fmt.pix.bytesperline = f->fmt.pix.width * vc->format->depth / 8;
 389         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 390         return 0;
 391 }
 392
 393 static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 394                                   struct v4l2_format *f)
 395 {
 396         tw686x_get_format(video_drvdata(file), f);
 397         return 0;
 398 }
 399
 400 static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
 401                                 struct v4l2_format *f)
 402 {
 403         struct tw686x_video_channel *vc = video_drvdata(file);
 404
 405         tw686x_get_format(vc, f);
 406         vc->format = format_by_fourcc(f->fmt.pix.pixelformat);
 407         vc->field = f->fmt.pix.field;
 408         vc->width = f->fmt.pix.width;
 409         vc->height = f->fmt.pix.height;
 410         return 0;
 411 }
 412
 413 static int tw686x_querycap(struct file *file, void *priv,
 414                            struct v4l2_capability *cap)
 415 {
 416         struct tw686x_video_channel *vc = video_drvdata(file);
 417         struct tw686x_dev *dev = vc->dev;
 418
 419         strcpy(cap->driver, "tw686x-kh");
 420         strcpy(cap->card, dev->name);
 421         sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci_dev));
 422         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 423         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 424         return 0;
 425 }
 426
 427 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 428 {
 429         struct tw686x_video_channel *vc = video_drvdata(file);
 430         unsigned int cnt;
 431         u32 sdt = 0; /* default */
 432
 433         for (cnt = 0; cnt < ARRAY_SIZE(video_standards); cnt++)
 434                 if (id & video_standards[cnt]) {
 435                         sdt = cnt;
 436                         break;
 437                 }
 438
 439         reg_write(vc->dev, SDT[vc->ch], sdt);
 440         vc->video_standard = video_standards[sdt];
 441         return 0;
 442 }
 443
 444 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 445 {
 446         struct tw686x_video_channel *vc = video_drvdata(file);
 447
 448         *id = vc->video_standard;
 449         return 0;
 450 }
 451
 452 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 453                                    struct v4l2_fmtdesc *f)
 454 {
 455         if (f->index >= ARRAY_SIZE(formats))
 456                 return -EINVAL;
 457
 458         strlcpy(f->description, formats[f->index].name, sizeof(f->description));
 459         f->pixelformat = formats[f->index].fourcc;
 460         return 0;
 461 }
 462
 463 static int tw686x_g_parm(struct file *file, void *priv,
 464                          struct v4l2_streamparm *sp)
 465 {
 466         struct tw686x_video_channel *vc = video_drvdata(file);
 467
 468         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 469                 return -EINVAL;
 470         memset(&sp->parm.capture, 0, sizeof(sp->parm.capture));
 471         sp->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 472         v4l2_video_std_frame_period(vc->video_standard,
 473                                     &sp->parm.capture.timeperframe);
 474
 475         return 0;
 476 }
 477
 478 static int tw686x_enum_input(struct file *file, void *priv,
 479                              struct v4l2_input *inp)
 480 {
 481         /* the chip has internal multiplexer, support can be added
 482            if the actual hw uses it */
 483         if (inp->index)
 484                 return -EINVAL;
 485
 486         snprintf(inp->name, sizeof(inp->name), "Composite");
 487         inp->type = V4L2_INPUT_TYPE_CAMERA;
 488         inp->std = V4L2_STD_ALL;
 489         inp->capabilities = V4L2_IN_CAP_STD;
 490         return 0;
 491 }
 492
 493 static int tw686x_g_input(struct file *file, void *priv, unsigned int *v)
 494 {
 495         *v = 0;
 496         return 0;
 497 }
 498
 499 static int tw686x_s_input(struct file *file, void *priv, unsigned int v)
 500 {
 501         if (v)
 502                 return -EINVAL;
 503         return 0;
 504 }
 505
 506 const struct v4l2_file_operations tw686x_video_fops = {
 507         .owner          = THIS_MODULE,
 508         .open           = v4l2_fh_open,
 509         .unlocked_ioctl = video_ioctl2,
 510         .release        = vb2_fop_release,
 511         .poll           = vb2_fop_poll,
 512         .read           = vb2_fop_read,
 513         .mmap           = vb2_fop_mmap,
 514 };
 515
 516 const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
 517         .vidioc_querycap                = tw686x_querycap,
 518         .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
 519         .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
 520         .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
 521         .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
 522         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 523         .vidioc_querybuf                = vb2_ioctl_querybuf,
 524         .vidioc_qbuf                    = vb2_ioctl_qbuf,
 525         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 526         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 527         .vidioc_streamon                = vb2_ioctl_streamon,
 528         .vidioc_streamoff               = vb2_ioctl_streamoff,
 529         .vidioc_g_std                   = tw686x_g_std,
 530         .vidioc_s_std                   = tw686x_s_std,
 531         .vidioc_g_parm                  = tw686x_g_parm,
 532         .vidioc_enum_input              = tw686x_enum_input,
 533         .vidioc_g_input                 = tw686x_g_input,
 534         .vidioc_s_input                 = tw686x_s_input,
 535         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
 536         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 537 };
 538
 539 static int video_thread(void *arg)
 540 {
 541         struct tw686x_dev *dev = arg;
 542         DECLARE_WAITQUEUE(wait, current);
 543
 544         set_freezable();
 545         add_wait_queue(&dev->video_thread_wait, &wait);
 546
 547         while (1) {
 548                 long timeout = schedule_timeout_interruptible(HZ);
 549                 unsigned int ch;
 550
 551                 if (timeout == -ERESTARTSYS || kthread_should_stop())
 552                         break;
 553
 554                 for (ch = 0; ch < max_channels(dev); ch++) {
 555                         struct tw686x_video_channel *vc;
 556                         unsigned long flags;
 557                         u32 request, n, stat = VB2_BUF_STATE_DONE;
 558
 559                         vc = &dev->video_channels[ch];
 560                         if (!(dev->video_active & (1 << ch)))
 561                                 continue;
 562
 563                         spin_lock_irq(&dev->irq_lock);
 564                         request = dev->dma_requests & (0x01000001 << ch);
 565                         if (request)
 566                                 dev->dma_requests &= ~request;
 567                         spin_unlock_irq(&dev->irq_lock);
 568
 569                         if (!request)
 570                                 continue;
 571
 572                         request >>= ch;
 573
 574                         /* handle channel events */
 575                         if ((request & 0x01000000) |
 576                             (reg_read(dev, VIDEO_FIFO_STATUS) & (0x01010001 << ch)) |
 577                             (reg_read(dev, VIDEO_PARSER_STATUS) & (0x00000101 << ch))) {
 578                                 /* DMA Errors - reset channel */
 579                                 u32 reg;
 580
 581                                 spin_lock_irqsave(&dev->irq_lock, flags);
 582                                 reg = reg_read(dev, DMA_CMD);
 583                                 /* Reset DMA channel */
 584                                 reg_write(dev, DMA_CMD, reg & ~(1 << ch));
 585                                 reg_write(dev, DMA_CMD, reg);
 586                                 spin_unlock_irqrestore(&dev->irq_lock, flags);
 587                                 stat = VB2_BUF_STATE_ERROR;
 588                         }
 589
 590                         /* handle video stream */
 591                         mutex_lock(&vc->vb_mutex);
 592                         spin_lock(&vc->qlock);
 593                         n = !!(reg_read(dev, PB_STATUS) & (1 << ch));
 594                         if (vc->curr_bufs[n]) {
 595                                 struct vb2_v4l2_buffer *vb;
 596
 597                                 vb = &vc->curr_bufs[n]->vb;
 598                                 vb->vb2_buf.timestamp = ktime_get_ns();
 599                                 vb->field = vc->field;
 600                                 if (V4L2_FIELD_HAS_BOTH(vc->field))
 601                                         vb->sequence = vc->seq++;
 602                                 else
 603                                         vb->sequence = (vc->seq++) / 2;
 604                                 vb2_set_plane_payload(&vb->vb2_buf, 0,
 605                                       vc->width * vc->height * vc->format->depth / 8);
 606                                 vb2_buffer_done(&vb->vb2_buf, stat);
 607                         }
 608                         setup_descs(vc, n);
 609                         spin_unlock(&vc->qlock);
 610                         mutex_unlock(&vc->vb_mutex);
 611                 }
 612                 try_to_freeze();
 613         }
 614
 615         remove_wait_queue(&dev->video_thread_wait, &wait);
 616         return 0;
 617 }
 618
 619 int tw686x_video_irq(struct tw686x_dev *dev)
 620 {
 621         unsigned long flags, handled = 0;
 622         u32 requests;
 623
 624         spin_lock_irqsave(&dev->irq_lock, flags);
 625         requests = dev->dma_requests;
 626         spin_unlock_irqrestore(&dev->irq_lock, flags);
 627
 628         if (requests & dev->video_active) {
 629                 wake_up_interruptible_all(&dev->video_thread_wait);
 630                 handled = 1;
 631         }
 632         return handled;
 633 }
 634
 635 void tw686x_video_free(struct tw686x_dev *dev)
 636 {
 637         unsigned int ch, n;
 638
 639         if (dev->video_thread)
 640                 kthread_stop(dev->video_thread);
 641
 642         for (ch = 0; ch < max_channels(dev); ch++) {
 643                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
 644
 645                 v4l2_ctrl_handler_free(&vc->ctrl_handler);
 646                 if (vc->device)
 647                         video_unregister_device(vc->device);
 648                 vb2_dma_sg_cleanup_ctx(vc->alloc_ctx);
 649                 for (n = 0; n < 2; n++) {
 650                         struct dma_desc *descs = &vc->sg_tables[n];
 651
 652                         if (descs->virt)
 653                                 pci_free_consistent(dev->pci_dev, descs->size,
 654                                                     descs->virt, descs->phys);
 655                 }
 656         }
 657
 658         v4l2_device_unregister(&dev->v4l2_dev);
 659 }
 660
 661 #define SG_TABLE_SIZE (MAX_SG_DESC_COUNT * sizeof(struct vdma_desc))
 662
 663 int tw686x_video_init(struct tw686x_dev *dev)
 664 {
 665         unsigned int ch, n;
 666         int err;
 667
 668         init_waitqueue_head(&dev->video_thread_wait);
 669
 670         err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
 671         if (err)
 672                 return err;
 673
 674         reg_write(dev, VIDEO_CONTROL1, 0); /* NTSC, disable scaler */
 675         reg_write(dev, PHASE_REF, 0x00001518); /* Scatter-gather DMA mode */
 676
 677         /* setup required SG table sizes */
 678         for (n = 0; n < 2; n++)
 679                 if (is_second_gen(dev)) {
 680                         /* TW 6865, TW6869 - each channel needs a pair of
 681                            descriptor tables */
 682                         for (ch = 0; ch < max_channels(dev); ch++)
 683                                 dev->video_channels[ch].sg_tables[n].size =
 684                                         SG_TABLE_SIZE;
 685
 686                 } else
 687                         /* TW 6864, TW6868 - we need to allocate a pair of
 688                            descriptor tables, common for all channels.
 689                            Each table will be bigger than 4 KB. */
 690                         dev->video_channels[0].sg_tables[n].size =
 691                                 max_channels(dev) * SG_TABLE_SIZE;
 692
 693         /* allocate SG tables and initialize video channels */
 694         for (ch = 0; ch < max_channels(dev); ch++) {
 695                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
 696                 struct video_device *vdev;
 697
 698                 mutex_init(&vc->vb_mutex);
 699                 spin_lock_init(&vc->qlock);
 700                 INIT_LIST_HEAD(&vc->vidq_queued);
 701
 702                 vc->dev = dev;
 703                 vc->ch = ch;
 704
 705                 /* default settings: NTSC */
 706                 vc->format = &formats[0];
 707                 vc->video_standard = V4L2_STD_NTSC;
 708                 reg_write(vc->dev, SDT[vc->ch], 0);
 709                 vc->field = V4L2_FIELD_SEQ_BT;
 710                 vc->width = 704;
 711                 vc->height = 480;
 712
 713                 for (n = 0; n < 2; n++) {
 714                         void *cpu;
 715
 716                         if (vc->sg_tables[n].size) {
 717                                 unsigned int reg = n ? DMA_PAGE_TABLE1_ADDR[ch] :
 718                                         DMA_PAGE_TABLE0_ADDR[ch];
 719
 720                                 cpu = pci_alloc_consistent(dev->pci_dev,
 721                                                            vc->sg_tables[n].size,
 722                                                            &vc->sg_tables[n].phys);
 723                                 if (!cpu) {
 724                                         pr_err("Error allocating video DMA scatter-gather tables\n");
 725                                         err = -ENOMEM;
 726                                         goto error;
 727                                 }
 728                                 vc->sg_tables[n].virt = cpu;
 729                                 reg_write(dev, reg, vc->sg_tables[n].phys);
 730                         } else
 731                                 cpu = dev->video_channels[0].sg_tables[n].virt +
 732                                         ch * SG_TABLE_SIZE;
 733
 734                         vc->sg_descs[n] = cpu;
 735                 }
 736
 737                 reg_write(dev, VCTRL1[0], 0x24);
 738                 reg_write(dev, LOOP[0], 0xA5);
 739                 if (max_channels(dev) > 4) {
 740                         reg_write(dev, VCTRL1[1], 0x24);
 741                         reg_write(dev, LOOP[1], 0xA5);
 742                 }
 743                 reg_write(dev, VIDEO_FIELD_CTRL[ch], 0);
 744                 reg_write(dev, VDELAY_LO[ch], 0x14);
 745
 746                 vdev = video_device_alloc();
 747                 if (!vdev) {
 748                         pr_warn("Unable to allocate video device\n");
 749                         err = -ENOMEM;
 750                         goto error;
 751                 }
 752
 753                 vc->alloc_ctx = vb2_dma_sg_init_ctx(&dev->pci_dev->dev);
 754                 if (IS_ERR(vc->alloc_ctx)) {
 755                         pr_warn("Unable to initialize DMA scatter-gather context\n");
 756                         err = PTR_ERR(vc->alloc_ctx);
 757                         goto error;
 758                 }
 759
 760                 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 761                 vc->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 762                 vc->vidq.drv_priv = vc;
 763                 vc->vidq.buf_struct_size = sizeof(struct tw686x_vb2_buf);
 764                 vc->vidq.ops = &tw686x_video_qops;
 765                 vc->vidq.mem_ops = &vb2_dma_sg_memops;
 766                 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 767                 vc->vidq.min_buffers_needed = 2;
 768                 vc->vidq.lock = &vc->vb_mutex;
 769
 770                 err = vb2_queue_init(&vc->vidq);
 771                 if (err)
 772                         goto error;
 773
 774                 strcpy(vdev->name, "TW686x-video");
 775                 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
 776                 vdev->fops = &tw686x_video_fops;
 777                 vdev->ioctl_ops = &tw686x_video_ioctl_ops;
 778                 vdev->release = video_device_release;
 779                 vdev->v4l2_dev = &dev->v4l2_dev;
 780                 vdev->queue = &vc->vidq;
 781                 vdev->tvnorms = V4L2_STD_ALL;
 782                 vdev->minor = -1;
 783                 vdev->lock = &vc->vb_mutex;
 784
 785                 dev->video_channels[ch].device = vdev;
 786                 video_set_drvdata(vdev, vc);
 787                 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 788                 if (err < 0)
 789                         goto error;
 790
 791                 v4l2_ctrl_handler_init(&vc->ctrl_handler,
 792                                        4 /* number of controls */);
 793                 vdev->ctrl_handler = &vc->ctrl_handler;
 794                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 795                                   V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
 796                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 797                                   V4L2_CID_CONTRAST, 0, 255, 1, 64);
 798                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
 799                                   V4L2_CID_SATURATION, 0, 255, 1, 128);
 800                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops, V4L2_CID_HUE,
 801                                   -124, 127, 1, 0);
 802                 err = vc->ctrl_handler.error;
 803                 if (err)
 804                         goto error;
 805
 806                 v4l2_ctrl_handler_setup(&vc->ctrl_handler);
 807         }
 808
 809         dev->video_thread = kthread_run(video_thread, dev, "tw686x_video");
 810         if (IS_ERR(dev->video_thread)) {
 811                 err = PTR_ERR(dev->video_thread);
 812                 goto error;
 813         }
 814
 815         return 0;
 816
 817 error:
 818         tw686x_video_free(dev);
 819         return err;
 820 }