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