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