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 } 938