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