1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * Copyright (C) 2015 VanguardiaSur - www.vanguardiasur.com.ar 4 * 5 * Based on original driver by Krzysztof Ha?asa: 6 * Copyright (C) 2015 Industrial Research Institute for Automation 7 * and Measurements PIAP 8 */ 9 10 #include <linux/init.h> 11 #include <linux/delay.h> 12 #include <linux/list.h> 13 #include <linux/module.h> 14 #include <linux/kernel.h> 15 #include <linux/slab.h> 16 #include <media/v4l2-common.h> 17 #include <media/v4l2-event.h> 18 #include <media/videobuf2-dma-contig.h> 19 #include <media/videobuf2-dma-sg.h> 20 #include <media/videobuf2-vmalloc.h> 21 #include "tw686x.h" 22 #include "tw686x-regs.h" 23 24 #define TW686X_INPUTS_PER_CH 4 25 #define TW686X_VIDEO_WIDTH 720 26 #define TW686X_VIDEO_HEIGHT(id) ((id & V4L2_STD_525_60) ? 480 : 576) 27 #define TW686X_MAX_FPS(id) ((id & V4L2_STD_525_60) ? 30 : 25) 28 29 #define TW686X_MAX_SG_ENTRY_SIZE 4096 30 #define TW686X_MAX_SG_DESC_COUNT 256 /* PAL 720x576 needs 203 4-KB pages */ 31 #define TW686X_SG_TABLE_SIZE (TW686X_MAX_SG_DESC_COUNT * sizeof(struct tw686x_sg_desc)) 32 33 static const struct tw686x_format formats[] = { 34 { 35 .fourcc = V4L2_PIX_FMT_UYVY, 36 .mode = 0, 37 .depth = 16, 38 }, { 39 .fourcc = V4L2_PIX_FMT_RGB565, 40 .mode = 5, 41 .depth = 16, 42 }, { 43 .fourcc = V4L2_PIX_FMT_YUYV, 44 .mode = 6, 45 .depth = 16, 46 } 47 }; 48 49 static void tw686x_buf_done(struct tw686x_video_channel *vc, 50 unsigned int pb) 51 { 52 struct tw686x_dma_desc *desc = &vc->dma_descs[pb]; 53 struct tw686x_dev *dev = vc->dev; 54 struct vb2_v4l2_buffer *vb; 55 struct vb2_buffer *vb2_buf; 56 57 if (vc->curr_bufs[pb]) { 58 vb = &vc->curr_bufs[pb]->vb; 59 60 vb->field = dev->dma_ops->field; 61 vb->sequence = vc->sequence++; 62 vb2_buf = &vb->vb2_buf; 63 64 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY) 65 memcpy(vb2_plane_vaddr(vb2_buf, 0), desc->virt, 66 desc->size); 67 vb2_buf->timestamp = ktime_get_ns(); 68 vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE); 69 } 70 71 vc->pb = !pb; 72 } 73 74 /* 75 * We can call this even when alloc_dma failed for the given channel 76 */ 77 static void tw686x_memcpy_dma_free(struct tw686x_video_channel *vc, 78 unsigned int pb) 79 { 80 struct tw686x_dma_desc *desc = &vc->dma_descs[pb]; 81 struct tw686x_dev *dev = vc->dev; 82 struct pci_dev *pci_dev; 83 unsigned long flags; 84 85 /* Check device presence. Shouldn't really happen! */ 86 spin_lock_irqsave(&dev->lock, flags); 87 pci_dev = dev->pci_dev; 88 spin_unlock_irqrestore(&dev->lock, flags); 89 if (!pci_dev) { 90 WARN(1, "trying to deallocate on missing device\n"); 91 return; 92 } 93 94 if (desc->virt) { 95 pci_free_consistent(dev->pci_dev, desc->size, 96 desc->virt, desc->phys); 97 desc->virt = NULL; 98 } 99 } 100 101 static int tw686x_memcpy_dma_alloc(struct tw686x_video_channel *vc, 102 unsigned int pb) 103 { 104 struct tw686x_dev *dev = vc->dev; 105 u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch]; 106 unsigned int len; 107 void *virt; 108 109 WARN(vc->dma_descs[pb].virt, 110 "Allocating buffer but previous still here\n"); 111 112 len = (vc->width * vc->height * vc->format->depth) >> 3; 113 virt = pci_alloc_consistent(dev->pci_dev, len, 114 &vc->dma_descs[pb].phys); 115 if (!virt) { 116 v4l2_err(&dev->v4l2_dev, 117 "dma%d: unable to allocate %s-buffer\n", 118 vc->ch, pb ? "B" : "P"); 119 return -ENOMEM; 120 } 121 vc->dma_descs[pb].size = len; 122 vc->dma_descs[pb].virt = virt; 123 reg_write(dev, reg, vc->dma_descs[pb].phys); 124 125 return 0; 126 } 127 128 static void tw686x_memcpy_buf_refill(struct tw686x_video_channel *vc, 129 unsigned int pb) 130 { 131 struct tw686x_v4l2_buf *buf; 132 133 while (!list_empty(&vc->vidq_queued)) { 134 135 buf = list_first_entry(&vc->vidq_queued, 136 struct tw686x_v4l2_buf, list); 137 list_del(&buf->list); 138 139 vc->curr_bufs[pb] = buf; 140 return; 141 } 142 vc->curr_bufs[pb] = NULL; 143 } 144 145 static const struct tw686x_dma_ops memcpy_dma_ops = { 146 .alloc = tw686x_memcpy_dma_alloc, 147 .free = tw686x_memcpy_dma_free, 148 .buf_refill = tw686x_memcpy_buf_refill, 149 .mem_ops = &vb2_vmalloc_memops, 150 .hw_dma_mode = TW686X_FRAME_MODE, 151 .field = V4L2_FIELD_INTERLACED, 152 }; 153 154 static void tw686x_contig_buf_refill(struct tw686x_video_channel *vc, 155 unsigned int pb) 156 { 157 struct tw686x_v4l2_buf *buf; 158 159 while (!list_empty(&vc->vidq_queued)) { 160 u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch]; 161 dma_addr_t phys; 162 163 buf = list_first_entry(&vc->vidq_queued, 164 struct tw686x_v4l2_buf, list); 165 list_del(&buf->list); 166 167 phys = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0); 168 reg_write(vc->dev, reg, phys); 169 170 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE; 171 vc->curr_bufs[pb] = buf; 172 return; 173 } 174 vc->curr_bufs[pb] = NULL; 175 } 176 177 static const struct tw686x_dma_ops contig_dma_ops = { 178 .buf_refill = tw686x_contig_buf_refill, 179 .mem_ops = &vb2_dma_contig_memops, 180 .hw_dma_mode = TW686X_FRAME_MODE, 181 .field = V4L2_FIELD_INTERLACED, 182 }; 183 184 static int tw686x_sg_desc_fill(struct tw686x_sg_desc *descs, 185 struct tw686x_v4l2_buf *buf, 186 unsigned int buf_len) 187 { 188 struct sg_table *vbuf = vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0); 189 unsigned int len, entry_len; 190 struct scatterlist *sg; 191 int i, count; 192 193 /* Clear the scatter-gather table */ 194 memset(descs, 0, TW686X_SG_TABLE_SIZE); 195 196 count = 0; 197 for_each_sg(vbuf->sgl, sg, vbuf->nents, i) { 198 dma_addr_t phys = sg_dma_address(sg); 199 len = sg_dma_len(sg); 200 201 while (len && buf_len) { 202 203 if (count == TW686X_MAX_SG_DESC_COUNT) 204 return -ENOMEM; 205 206 entry_len = min_t(unsigned int, len, 207 TW686X_MAX_SG_ENTRY_SIZE); 208 entry_len = min_t(unsigned int, entry_len, buf_len); 209 descs[count].phys = cpu_to_le32(phys); 210 descs[count++].flags_length = 211 cpu_to_le32(BIT(30) | entry_len); 212 phys += entry_len; 213 len -= entry_len; 214 buf_len -= entry_len; 215 } 216 217 if (!buf_len) 218 return 0; 219 } 220 221 return -ENOMEM; 222 } 223 224 static void tw686x_sg_buf_refill(struct tw686x_video_channel *vc, 225 unsigned int pb) 226 { 227 struct tw686x_dev *dev = vc->dev; 228 struct tw686x_v4l2_buf *buf; 229 230 while (!list_empty(&vc->vidq_queued)) { 231 unsigned int buf_len; 232 233 buf = list_first_entry(&vc->vidq_queued, 234 struct tw686x_v4l2_buf, list); 235 list_del(&buf->list); 236 237 buf_len = (vc->width * vc->height * vc->format->depth) >> 3; 238 if (tw686x_sg_desc_fill(vc->sg_descs[pb], buf, buf_len)) { 239 v4l2_err(&dev->v4l2_dev, 240 "dma%d: unable to fill %s-buffer\n", 241 vc->ch, pb ? "B" : "P"); 242 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 243 continue; 244 } 245 246 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE; 247 vc->curr_bufs[pb] = buf; 248 return; 249 } 250 251 vc->curr_bufs[pb] = NULL; 252 } 253 254 static void tw686x_sg_dma_free(struct tw686x_video_channel *vc, 255 unsigned int pb) 256 { 257 struct tw686x_dma_desc *desc = &vc->dma_descs[pb]; 258 struct tw686x_dev *dev = vc->dev; 259 260 if (desc->size) { 261 pci_free_consistent(dev->pci_dev, desc->size, 262 desc->virt, desc->phys); 263 desc->virt = NULL; 264 } 265 266 vc->sg_descs[pb] = NULL; 267 } 268 269 static int tw686x_sg_dma_alloc(struct tw686x_video_channel *vc, 270 unsigned int pb) 271 { 272 struct tw686x_dma_desc *desc = &vc->dma_descs[pb]; 273 struct tw686x_dev *dev = vc->dev; 274 u32 reg = pb ? DMA_PAGE_TABLE1_ADDR[vc->ch] : 275 DMA_PAGE_TABLE0_ADDR[vc->ch]; 276 void *virt; 277 278 if (desc->size) { 279 280 virt = pci_alloc_consistent(dev->pci_dev, desc->size, 281 &desc->phys); 282 if (!virt) { 283 v4l2_err(&dev->v4l2_dev, 284 "dma%d: unable to allocate %s-buffer\n", 285 vc->ch, pb ? "B" : "P"); 286 return -ENOMEM; 287 } 288 desc->virt = virt; 289 reg_write(dev, reg, desc->phys); 290 } else { 291 virt = dev->video_channels[0].dma_descs[pb].virt + 292 vc->ch * TW686X_SG_TABLE_SIZE; 293 } 294 295 vc->sg_descs[pb] = virt; 296 return 0; 297 } 298 299 static int tw686x_sg_setup(struct tw686x_dev *dev) 300 { 301 unsigned int sg_table_size, pb, ch, channels; 302 303 if (is_second_gen(dev)) { 304 /* 305 * TW6865/TW6869: each channel needs a pair of 306 * P-B descriptor tables. 307 */ 308 channels = max_channels(dev); 309 sg_table_size = TW686X_SG_TABLE_SIZE; 310 } else { 311 /* 312 * TW6864/TW6868: we need to allocate a pair of 313 * P-B descriptor tables, common for all channels. 314 * Each table will be bigger than 4 KB. 315 */ 316 channels = 1; 317 sg_table_size = max_channels(dev) * TW686X_SG_TABLE_SIZE; 318 } 319 320 for (ch = 0; ch < channels; ch++) { 321 struct tw686x_video_channel *vc = &dev->video_channels[ch]; 322 323 for (pb = 0; pb < 2; pb++) 324 vc->dma_descs[pb].size = sg_table_size; 325 } 326 327 return 0; 328 } 329 330 static const struct tw686x_dma_ops sg_dma_ops = { 331 .setup = tw686x_sg_setup, 332 .alloc = tw686x_sg_dma_alloc, 333 .free = tw686x_sg_dma_free, 334 .buf_refill = tw686x_sg_buf_refill, 335 .mem_ops = &vb2_dma_sg_memops, 336 .hw_dma_mode = TW686X_SG_MODE, 337 .field = V4L2_FIELD_SEQ_TB, 338 }; 339 340 static const unsigned int fps_map[15] = { 341 /* 342 * bit 31 enables selecting the field control register 343 * bits 0-29 are a bitmask with fields that will be output. 344 * For NTSC (and PAL-M, PAL-60), all 30 bits are used. 345 * For other PAL standards, only the first 25 bits are used. 346 */ 347 0x00000000, /* output all fields */ 348 0x80000006, /* 2 fps (60Hz), 2 fps (50Hz) */ 349 0x80018006, /* 4 fps (60Hz), 4 fps (50Hz) */ 350 0x80618006, /* 6 fps (60Hz), 6 fps (50Hz) */ 351 0x81818186, /* 8 fps (60Hz), 8 fps (50Hz) */ 352 0x86186186, /* 10 fps (60Hz), 8 fps (50Hz) */ 353 0x86619866, /* 12 fps (60Hz), 10 fps (50Hz) */ 354 0x86666666, /* 14 fps (60Hz), 12 fps (50Hz) */ 355 0x9999999e, /* 16 fps (60Hz), 14 fps (50Hz) */ 356 0x99e6799e, /* 18 fps (60Hz), 16 fps (50Hz) */ 357 0x9e79e79e, /* 20 fps (60Hz), 16 fps (50Hz) */ 358 0x9e7e7e7e, /* 22 fps (60Hz), 18 fps (50Hz) */ 359 0x9fe7f9fe, /* 24 fps (60Hz), 20 fps (50Hz) */ 360 0x9ffe7ffe, /* 26 fps (60Hz), 22 fps (50Hz) */ 361 0x9ffffffe, /* 28 fps (60Hz), 24 fps (50Hz) */ 362 }; 363 364 static unsigned int tw686x_real_fps(unsigned int index, unsigned int max_fps) 365 { 366 unsigned long mask; 367 368 if (!index || index >= ARRAY_SIZE(fps_map)) 369 return max_fps; 370 371 mask = GENMASK(max_fps - 1, 0); 372 return hweight_long(fps_map[index] & mask); 373 } 374 375 static unsigned int tw686x_fps_idx(unsigned int fps, unsigned int max_fps) 376 { 377 unsigned int idx, real_fps; 378 int delta; 379 380 /* First guess */ 381 idx = (12 + 15 * fps) / max_fps; 382 383 /* Minimal possible framerate is 2 frames per second */ 384 if (!idx) 385 return 1; 386 387 /* Check if the difference is bigger than abs(1) and adjust */ 388 real_fps = tw686x_real_fps(idx, max_fps); 389 delta = real_fps - fps; 390 if (delta < -1) 391 idx++; 392 else if (delta > 1) 393 idx--; 394 395 /* Max framerate */ 396 if (idx >= 15) 397 return 0; 398 399 return idx; 400 } 401 402 static void tw686x_set_framerate(struct tw686x_video_channel *vc, 403 unsigned int fps) 404 { 405 unsigned int i; 406 407 i = tw686x_fps_idx(fps, TW686X_MAX_FPS(vc->video_standard)); 408 reg_write(vc->dev, VIDEO_FIELD_CTRL[vc->ch], fps_map[i]); 409 vc->fps = tw686x_real_fps(i, TW686X_MAX_FPS(vc->video_standard)); 410 } 411 412 static const struct tw686x_format *format_by_fourcc(unsigned int fourcc) 413 { 414 unsigned int cnt; 415 416 for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++) 417 if (formats[cnt].fourcc == fourcc) 418 return &formats[cnt]; 419 return NULL; 420 } 421 422 static int tw686x_queue_setup(struct vb2_queue *vq, 423 unsigned int *nbuffers, unsigned int *nplanes, 424 unsigned int sizes[], struct device *alloc_devs[]) 425 { 426 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq); 427 unsigned int szimage = 428 (vc->width * vc->height * vc->format->depth) >> 3; 429 430 /* 431 * Let's request at least three buffers: two for the 432 * DMA engine and one for userspace. 433 */ 434 if (vq->num_buffers + *nbuffers < 3) 435 *nbuffers = 3 - vq->num_buffers; 436 437 if (*nplanes) { 438 if (*nplanes != 1 || sizes[0] < szimage) 439 return -EINVAL; 440 return 0; 441 } 442 443 sizes[0] = szimage; 444 *nplanes = 1; 445 return 0; 446 } 447 448 static void tw686x_buf_queue(struct vb2_buffer *vb) 449 { 450 struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue); 451 struct tw686x_dev *dev = vc->dev; 452 struct pci_dev *pci_dev; 453 unsigned long flags; 454 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 455 struct tw686x_v4l2_buf *buf = 456 container_of(vbuf, struct tw686x_v4l2_buf, vb); 457 458 /* Check device presence */ 459 spin_lock_irqsave(&dev->lock, flags); 460 pci_dev = dev->pci_dev; 461 spin_unlock_irqrestore(&dev->lock, flags); 462 if (!pci_dev) { 463 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 464 return; 465 } 466 467 spin_lock_irqsave(&vc->qlock, flags); 468 list_add_tail(&buf->list, &vc->vidq_queued); 469 spin_unlock_irqrestore(&vc->qlock, flags); 470 } 471 472 static void tw686x_clear_queue(struct tw686x_video_channel *vc, 473 enum vb2_buffer_state state) 474 { 475 unsigned int pb; 476 477 while (!list_empty(&vc->vidq_queued)) { 478 struct tw686x_v4l2_buf *buf; 479 480 buf = list_first_entry(&vc->vidq_queued, 481 struct tw686x_v4l2_buf, list); 482 list_del(&buf->list); 483 vb2_buffer_done(&buf->vb.vb2_buf, state); 484 } 485 486 for (pb = 0; pb < 2; pb++) { 487 if (vc->curr_bufs[pb]) 488 vb2_buffer_done(&vc->curr_bufs[pb]->vb.vb2_buf, state); 489 vc->curr_bufs[pb] = NULL; 490 } 491 } 492 493 static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count) 494 { 495 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq); 496 struct tw686x_dev *dev = vc->dev; 497 struct pci_dev *pci_dev; 498 unsigned long flags; 499 int pb, err; 500 501 /* Check device presence */ 502 spin_lock_irqsave(&dev->lock, flags); 503 pci_dev = dev->pci_dev; 504 spin_unlock_irqrestore(&dev->lock, flags); 505 if (!pci_dev) { 506 err = -ENODEV; 507 goto err_clear_queue; 508 } 509 510 spin_lock_irqsave(&vc->qlock, flags); 511 512 /* Sanity check */ 513 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY && 514 (!vc->dma_descs[0].virt || !vc->dma_descs[1].virt)) { 515 spin_unlock_irqrestore(&vc->qlock, flags); 516 v4l2_err(&dev->v4l2_dev, 517 "video%d: refusing to start without DMA buffers\n", 518 vc->num); 519 err = -ENOMEM; 520 goto err_clear_queue; 521 } 522 523 for (pb = 0; pb < 2; pb++) 524 dev->dma_ops->buf_refill(vc, pb); 525 spin_unlock_irqrestore(&vc->qlock, flags); 526 527 vc->sequence = 0; 528 vc->pb = 0; 529 530 spin_lock_irqsave(&dev->lock, flags); 531 tw686x_enable_channel(dev, vc->ch); 532 spin_unlock_irqrestore(&dev->lock, flags); 533 534 mod_timer(&dev->dma_delay_timer, jiffies + msecs_to_jiffies(100)); 535 536 return 0; 537 538 err_clear_queue: 539 spin_lock_irqsave(&vc->qlock, flags); 540 tw686x_clear_queue(vc, VB2_BUF_STATE_QUEUED); 541 spin_unlock_irqrestore(&vc->qlock, flags); 542 return err; 543 } 544 545 static void tw686x_stop_streaming(struct vb2_queue *vq) 546 { 547 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq); 548 struct tw686x_dev *dev = vc->dev; 549 struct pci_dev *pci_dev; 550 unsigned long flags; 551 552 /* Check device presence */ 553 spin_lock_irqsave(&dev->lock, flags); 554 pci_dev = dev->pci_dev; 555 spin_unlock_irqrestore(&dev->lock, flags); 556 if (pci_dev) 557 tw686x_disable_channel(dev, vc->ch); 558 559 spin_lock_irqsave(&vc->qlock, flags); 560 tw686x_clear_queue(vc, VB2_BUF_STATE_ERROR); 561 spin_unlock_irqrestore(&vc->qlock, flags); 562 } 563 564 static int tw686x_buf_prepare(struct vb2_buffer *vb) 565 { 566 struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue); 567 unsigned int size = 568 (vc->width * vc->height * vc->format->depth) >> 3; 569 570 if (vb2_plane_size(vb, 0) < size) 571 return -EINVAL; 572 vb2_set_plane_payload(vb, 0, size); 573 return 0; 574 } 575 576 static const struct vb2_ops tw686x_video_qops = { 577 .queue_setup = tw686x_queue_setup, 578 .buf_queue = tw686x_buf_queue, 579 .buf_prepare = tw686x_buf_prepare, 580 .start_streaming = tw686x_start_streaming, 581 .stop_streaming = tw686x_stop_streaming, 582 .wait_prepare = vb2_ops_wait_prepare, 583 .wait_finish = vb2_ops_wait_finish, 584 }; 585 586 static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl) 587 { 588 struct tw686x_video_channel *vc; 589 struct tw686x_dev *dev; 590 unsigned int ch; 591 592 vc = container_of(ctrl->handler, struct tw686x_video_channel, 593 ctrl_handler); 594 dev = vc->dev; 595 ch = vc->ch; 596 597 switch (ctrl->id) { 598 case V4L2_CID_BRIGHTNESS: 599 reg_write(dev, BRIGHT[ch], ctrl->val & 0xff); 600 return 0; 601 602 case V4L2_CID_CONTRAST: 603 reg_write(dev, CONTRAST[ch], ctrl->val); 604 return 0; 605 606 case V4L2_CID_SATURATION: 607 reg_write(dev, SAT_U[ch], ctrl->val); 608 reg_write(dev, SAT_V[ch], ctrl->val); 609 return 0; 610 611 case V4L2_CID_HUE: 612 reg_write(dev, HUE[ch], ctrl->val & 0xff); 613 return 0; 614 } 615 616 return -EINVAL; 617 } 618 619 static const struct v4l2_ctrl_ops ctrl_ops = { 620 .s_ctrl = tw686x_s_ctrl, 621 }; 622 623 static int tw686x_g_fmt_vid_cap(struct file *file, void *priv, 624 struct v4l2_format *f) 625 { 626 struct tw686x_video_channel *vc = video_drvdata(file); 627 struct tw686x_dev *dev = vc->dev; 628 629 f->fmt.pix.width = vc->width; 630 f->fmt.pix.height = vc->height; 631 f->fmt.pix.field = dev->dma_ops->field; 632 f->fmt.pix.pixelformat = vc->format->fourcc; 633 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; 634 f->fmt.pix.bytesperline = (f->fmt.pix.width * vc->format->depth) / 8; 635 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; 636 return 0; 637 } 638 639 static int tw686x_try_fmt_vid_cap(struct file *file, void *priv, 640 struct v4l2_format *f) 641 { 642 struct tw686x_video_channel *vc = video_drvdata(file); 643 struct tw686x_dev *dev = vc->dev; 644 unsigned int video_height = TW686X_VIDEO_HEIGHT(vc->video_standard); 645 const struct tw686x_format *format; 646 647 format = format_by_fourcc(f->fmt.pix.pixelformat); 648 if (!format) { 649 format = &formats[0]; 650 f->fmt.pix.pixelformat = format->fourcc; 651 } 652 653 if (f->fmt.pix.width <= TW686X_VIDEO_WIDTH / 2) 654 f->fmt.pix.width = TW686X_VIDEO_WIDTH / 2; 655 else 656 f->fmt.pix.width = TW686X_VIDEO_WIDTH; 657 658 if (f->fmt.pix.height <= video_height / 2) 659 f->fmt.pix.height = video_height / 2; 660 else 661 f->fmt.pix.height = video_height; 662 663 f->fmt.pix.bytesperline = (f->fmt.pix.width * format->depth) / 8; 664 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; 665 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; 666 f->fmt.pix.field = dev->dma_ops->field; 667 668 return 0; 669 } 670 671 static int tw686x_set_format(struct tw686x_video_channel *vc, 672 unsigned int pixelformat, unsigned int width, 673 unsigned int height, bool realloc) 674 { 675 struct tw686x_dev *dev = vc->dev; 676 u32 val, dma_width, dma_height, dma_line_width; 677 int err, pb; 678 679 vc->format = format_by_fourcc(pixelformat); 680 vc->width = width; 681 vc->height = height; 682 683 /* We need new DMA buffers if the framesize has changed */ 684 if (dev->dma_ops->alloc && realloc) { 685 for (pb = 0; pb < 2; pb++) 686 dev->dma_ops->free(vc, pb); 687 688 for (pb = 0; pb < 2; pb++) { 689 err = dev->dma_ops->alloc(vc, pb); 690 if (err) { 691 if (pb > 0) 692 dev->dma_ops->free(vc, 0); 693 return err; 694 } 695 } 696 } 697 698 val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]); 699 700 if (vc->width <= TW686X_VIDEO_WIDTH / 2) 701 val |= BIT(23); 702 else 703 val &= ~BIT(23); 704 705 if (vc->height <= TW686X_VIDEO_HEIGHT(vc->video_standard) / 2) 706 val |= BIT(24); 707 else 708 val &= ~BIT(24); 709 710 val &= ~0x7ffff; 711 712 /* Program the DMA scatter-gather */ 713 if (dev->dma_mode == TW686X_DMA_MODE_SG) { 714 u32 start_idx, end_idx; 715 716 start_idx = is_second_gen(dev) ? 717 0 : vc->ch * TW686X_MAX_SG_DESC_COUNT; 718 end_idx = start_idx + TW686X_MAX_SG_DESC_COUNT - 1; 719 720 val |= (end_idx << 10) | start_idx; 721 } 722 723 val &= ~(0x7 << 20); 724 val |= vc->format->mode << 20; 725 reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val); 726 727 /* Program the DMA frame size */ 728 dma_width = (vc->width * 2) & 0x7ff; 729 dma_height = vc->height / 2; 730 dma_line_width = (vc->width * 2) & 0x7ff; 731 val = (dma_height << 22) | (dma_line_width << 11) | dma_width; 732 reg_write(vc->dev, VDMA_WHP[vc->ch], val); 733 return 0; 734 } 735 736 static int tw686x_s_fmt_vid_cap(struct file *file, void *priv, 737 struct v4l2_format *f) 738 { 739 struct tw686x_video_channel *vc = video_drvdata(file); 740 unsigned long area; 741 bool realloc; 742 int err; 743 744 if (vb2_is_busy(&vc->vidq)) 745 return -EBUSY; 746 747 area = vc->width * vc->height; 748 err = tw686x_try_fmt_vid_cap(file, priv, f); 749 if (err) 750 return err; 751 752 realloc = area != (f->fmt.pix.width * f->fmt.pix.height); 753 return tw686x_set_format(vc, f->fmt.pix.pixelformat, 754 f->fmt.pix.width, f->fmt.pix.height, 755 realloc); 756 } 757 758 static int tw686x_querycap(struct file *file, void *priv, 759 struct v4l2_capability *cap) 760 { 761 struct tw686x_video_channel *vc = video_drvdata(file); 762 struct tw686x_dev *dev = vc->dev; 763 764 strscpy(cap->driver, "tw686x", sizeof(cap->driver)); 765 strscpy(cap->card, dev->name, sizeof(cap->card)); 766 snprintf(cap->bus_info, sizeof(cap->bus_info), 767 "PCI:%s", pci_name(dev->pci_dev)); 768 cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | 769 V4L2_CAP_READWRITE; 770 cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; 771 return 0; 772 } 773 774 static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id) 775 { 776 u32 val; 777 778 if (id & V4L2_STD_NTSC) 779 val = 0; 780 else if (id & V4L2_STD_PAL) 781 val = 1; 782 else if (id & V4L2_STD_SECAM) 783 val = 2; 784 else if (id & V4L2_STD_NTSC_443) 785 val = 3; 786 else if (id & V4L2_STD_PAL_M) 787 val = 4; 788 else if (id & V4L2_STD_PAL_Nc) 789 val = 5; 790 else if (id & V4L2_STD_PAL_60) 791 val = 6; 792 else 793 return -EINVAL; 794 795 vc->video_standard = id; 796 reg_write(vc->dev, SDT[vc->ch], val); 797 798 val = reg_read(vc->dev, VIDEO_CONTROL1); 799 if (id & V4L2_STD_525_60) 800 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch)); 801 else 802 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch)); 803 reg_write(vc->dev, VIDEO_CONTROL1, val); 804 805 return 0; 806 } 807 808 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id) 809 { 810 struct tw686x_video_channel *vc = video_drvdata(file); 811 struct v4l2_format f; 812 int ret; 813 814 if (vc->video_standard == id) 815 return 0; 816 817 if (vb2_is_busy(&vc->vidq)) 818 return -EBUSY; 819 820 ret = tw686x_set_standard(vc, id); 821 if (ret) 822 return ret; 823 /* 824 * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change, 825 * calling g_fmt and s_fmt will sanitize the height 826 * according to the standard. 827 */ 828 tw686x_g_fmt_vid_cap(file, priv, &f); 829 tw686x_s_fmt_vid_cap(file, priv, &f); 830 831 /* 832 * Frame decimation depends on the chosen standard, 833 * so reset it to the current value. 834 */ 835 tw686x_set_framerate(vc, vc->fps); 836 return 0; 837 } 838 839 static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std) 840 { 841 struct tw686x_video_channel *vc = video_drvdata(file); 842 struct tw686x_dev *dev = vc->dev; 843 unsigned int old_std, detected_std = 0; 844 unsigned long end; 845 846 if (vb2_is_streaming(&vc->vidq)) 847 return -EBUSY; 848 849 /* Enable and start standard detection */ 850 old_std = reg_read(dev, SDT[vc->ch]); 851 reg_write(dev, SDT[vc->ch], 0x7); 852 reg_write(dev, SDT_EN[vc->ch], 0xff); 853 854 end = jiffies + msecs_to_jiffies(500); 855 while (time_is_after_jiffies(end)) { 856 857 detected_std = reg_read(dev, SDT[vc->ch]); 858 if (!(detected_std & BIT(7))) 859 break; 860 msleep(100); 861 } 862 reg_write(dev, SDT[vc->ch], old_std); 863 864 /* Exit if still busy */ 865 if (detected_std & BIT(7)) 866 return 0; 867 868 detected_std = (detected_std >> 4) & 0x7; 869 switch (detected_std) { 870 case TW686X_STD_NTSC_M: 871 *std &= V4L2_STD_NTSC; 872 break; 873 case TW686X_STD_NTSC_443: 874 *std &= V4L2_STD_NTSC_443; 875 break; 876 case TW686X_STD_PAL_M: 877 *std &= V4L2_STD_PAL_M; 878 break; 879 case TW686X_STD_PAL_60: 880 *std &= V4L2_STD_PAL_60; 881 break; 882 case TW686X_STD_PAL: 883 *std &= V4L2_STD_PAL; 884 break; 885 case TW686X_STD_PAL_CN: 886 *std &= V4L2_STD_PAL_Nc; 887 break; 888 case TW686X_STD_SECAM: 889 *std &= V4L2_STD_SECAM; 890 break; 891 default: 892 *std = 0; 893 } 894 return 0; 895 } 896 897 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id) 898 { 899 struct tw686x_video_channel *vc = video_drvdata(file); 900 901 *id = vc->video_standard; 902 return 0; 903 } 904 905 static int tw686x_enum_framesizes(struct file *file, void *priv, 906 struct v4l2_frmsizeenum *fsize) 907 { 908 struct tw686x_video_channel *vc = video_drvdata(file); 909 910 if (fsize->index) 911 return -EINVAL; 912 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 913 fsize->stepwise.max_width = TW686X_VIDEO_WIDTH; 914 fsize->stepwise.min_width = fsize->stepwise.max_width / 2; 915 fsize->stepwise.step_width = fsize->stepwise.min_width; 916 fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard); 917 fsize->stepwise.min_height = fsize->stepwise.max_height / 2; 918 fsize->stepwise.step_height = fsize->stepwise.min_height; 919 return 0; 920 } 921 922 static int tw686x_enum_frameintervals(struct file *file, void *priv, 923 struct v4l2_frmivalenum *ival) 924 { 925 struct tw686x_video_channel *vc = video_drvdata(file); 926 int max_fps = TW686X_MAX_FPS(vc->video_standard); 927 int max_rates = DIV_ROUND_UP(max_fps, 2); 928 929 if (ival->index >= max_rates) 930 return -EINVAL; 931 932 ival->type = V4L2_FRMIVAL_TYPE_DISCRETE; 933 ival->discrete.numerator = 1; 934 if (ival->index < (max_rates - 1)) 935 ival->discrete.denominator = (ival->index + 1) * 2; 936 else 937 ival->discrete.denominator = max_fps; 938 return 0; 939 } 940 941 static int tw686x_g_parm(struct file *file, void *priv, 942 struct v4l2_streamparm *sp) 943 { 944 struct tw686x_video_channel *vc = video_drvdata(file); 945 struct v4l2_captureparm *cp = &sp->parm.capture; 946 947 if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) 948 return -EINVAL; 949 sp->parm.capture.readbuffers = 3; 950 951 cp->capability = V4L2_CAP_TIMEPERFRAME; 952 cp->timeperframe.numerator = 1; 953 cp->timeperframe.denominator = vc->fps; 954 return 0; 955 } 956 957 static int tw686x_s_parm(struct file *file, void *priv, 958 struct v4l2_streamparm *sp) 959 { 960 struct tw686x_video_channel *vc = video_drvdata(file); 961 struct v4l2_captureparm *cp = &sp->parm.capture; 962 unsigned int denominator = cp->timeperframe.denominator; 963 unsigned int numerator = cp->timeperframe.numerator; 964 unsigned int fps; 965 966 if (vb2_is_busy(&vc->vidq)) 967 return -EBUSY; 968 969 fps = (!numerator || !denominator) ? 0 : denominator / numerator; 970 if (vc->fps != fps) 971 tw686x_set_framerate(vc, fps); 972 return tw686x_g_parm(file, priv, sp); 973 } 974 975 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv, 976 struct v4l2_fmtdesc *f) 977 { 978 if (f->index >= ARRAY_SIZE(formats)) 979 return -EINVAL; 980 f->pixelformat = formats[f->index].fourcc; 981 return 0; 982 } 983 984 static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i) 985 { 986 u32 val; 987 988 vc->input = i; 989 990 val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]); 991 val &= ~(0x3 << 30); 992 val |= i << 30; 993 reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val); 994 } 995 996 static int tw686x_s_input(struct file *file, void *priv, unsigned int i) 997 { 998 struct tw686x_video_channel *vc = video_drvdata(file); 999 1000 if (i >= TW686X_INPUTS_PER_CH) 1001 return -EINVAL; 1002 if (i == vc->input) 1003 return 0; 1004 /* 1005 * Not sure we are able to support on the fly input change 1006 */ 1007 if (vb2_is_busy(&vc->vidq)) 1008 return -EBUSY; 1009 1010 tw686x_set_input(vc, i); 1011 return 0; 1012 } 1013 1014 static int tw686x_g_input(struct file *file, void *priv, unsigned int *i) 1015 { 1016 struct tw686x_video_channel *vc = video_drvdata(file); 1017 1018 *i = vc->input; 1019 return 0; 1020 } 1021 1022 static int tw686x_enum_input(struct file *file, void *priv, 1023 struct v4l2_input *i) 1024 { 1025 struct tw686x_video_channel *vc = video_drvdata(file); 1026 unsigned int vidstat; 1027 1028 if (i->index >= TW686X_INPUTS_PER_CH) 1029 return -EINVAL; 1030 1031 snprintf(i->name, sizeof(i->name), "Composite%d", i->index); 1032 i->type = V4L2_INPUT_TYPE_CAMERA; 1033 i->std = vc->device->tvnorms; 1034 i->capabilities = V4L2_IN_CAP_STD; 1035 1036 vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]); 1037 i->status = 0; 1038 if (vidstat & TW686X_VIDSTAT_VDLOSS) 1039 i->status |= V4L2_IN_ST_NO_SIGNAL; 1040 if (!(vidstat & TW686X_VIDSTAT_HLOCK)) 1041 i->status |= V4L2_IN_ST_NO_H_LOCK; 1042 1043 return 0; 1044 } 1045 1046 static const struct v4l2_file_operations tw686x_video_fops = { 1047 .owner = THIS_MODULE, 1048 .open = v4l2_fh_open, 1049 .unlocked_ioctl = video_ioctl2, 1050 .release = vb2_fop_release, 1051 .poll = vb2_fop_poll, 1052 .read = vb2_fop_read, 1053 .mmap = vb2_fop_mmap, 1054 }; 1055 1056 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = { 1057 .vidioc_querycap = tw686x_querycap, 1058 .vidioc_g_fmt_vid_cap = tw686x_g_fmt_vid_cap, 1059 .vidioc_s_fmt_vid_cap = tw686x_s_fmt_vid_cap, 1060 .vidioc_enum_fmt_vid_cap = tw686x_enum_fmt_vid_cap, 1061 .vidioc_try_fmt_vid_cap = tw686x_try_fmt_vid_cap, 1062 1063 .vidioc_querystd = tw686x_querystd, 1064 .vidioc_g_std = tw686x_g_std, 1065 .vidioc_s_std = tw686x_s_std, 1066 1067 .vidioc_g_parm = tw686x_g_parm, 1068 .vidioc_s_parm = tw686x_s_parm, 1069 .vidioc_enum_framesizes = tw686x_enum_framesizes, 1070 .vidioc_enum_frameintervals = tw686x_enum_frameintervals, 1071 1072 .vidioc_enum_input = tw686x_enum_input, 1073 .vidioc_g_input = tw686x_g_input, 1074 .vidioc_s_input = tw686x_s_input, 1075 1076 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1077 .vidioc_querybuf = vb2_ioctl_querybuf, 1078 .vidioc_qbuf = vb2_ioctl_qbuf, 1079 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1080 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1081 .vidioc_streamon = vb2_ioctl_streamon, 1082 .vidioc_streamoff = vb2_ioctl_streamoff, 1083 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1084 1085 .vidioc_log_status = v4l2_ctrl_log_status, 1086 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 1087 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 1088 }; 1089 1090 void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests, 1091 unsigned int pb_status, unsigned int fifo_status, 1092 unsigned int *reset_ch) 1093 { 1094 struct tw686x_video_channel *vc; 1095 unsigned long flags; 1096 unsigned int ch, pb; 1097 1098 for_each_set_bit(ch, &requests, max_channels(dev)) { 1099 vc = &dev->video_channels[ch]; 1100 1101 /* 1102 * This can either be a blue frame (with signal-lost bit set) 1103 * or a good frame (with signal-lost bit clear). If we have just 1104 * got signal, then this channel needs resetting. 1105 */ 1106 if (vc->no_signal && !(fifo_status & BIT(ch))) { 1107 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev, 1108 "video%d: signal recovered\n", vc->num); 1109 vc->no_signal = false; 1110 *reset_ch |= BIT(ch); 1111 vc->pb = 0; 1112 continue; 1113 } 1114 vc->no_signal = !!(fifo_status & BIT(ch)); 1115 1116 /* Check FIFO errors only if there's signal */ 1117 if (!vc->no_signal) { 1118 u32 fifo_ov, fifo_bad; 1119 1120 fifo_ov = (fifo_status >> 24) & BIT(ch); 1121 fifo_bad = (fifo_status >> 16) & BIT(ch); 1122 if (fifo_ov || fifo_bad) { 1123 /* Mark this channel for reset */ 1124 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev, 1125 "video%d: FIFO error\n", vc->num); 1126 *reset_ch |= BIT(ch); 1127 vc->pb = 0; 1128 continue; 1129 } 1130 } 1131 1132 pb = !!(pb_status & BIT(ch)); 1133 if (vc->pb != pb) { 1134 /* Mark this channel for reset */ 1135 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev, 1136 "video%d: unexpected p-b buffer!\n", 1137 vc->num); 1138 *reset_ch |= BIT(ch); 1139 vc->pb = 0; 1140 continue; 1141 } 1142 1143 spin_lock_irqsave(&vc->qlock, flags); 1144 tw686x_buf_done(vc, pb); 1145 dev->dma_ops->buf_refill(vc, pb); 1146 spin_unlock_irqrestore(&vc->qlock, flags); 1147 } 1148 } 1149 1150 void tw686x_video_free(struct tw686x_dev *dev) 1151 { 1152 unsigned int ch, pb; 1153 1154 for (ch = 0; ch < max_channels(dev); ch++) { 1155 struct tw686x_video_channel *vc = &dev->video_channels[ch]; 1156 1157 video_unregister_device(vc->device); 1158 1159 if (dev->dma_ops->free) 1160 for (pb = 0; pb < 2; pb++) 1161 dev->dma_ops->free(vc, pb); 1162 } 1163 } 1164 1165 int tw686x_video_init(struct tw686x_dev *dev) 1166 { 1167 unsigned int ch, val; 1168 int err; 1169 1170 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY) 1171 dev->dma_ops = &memcpy_dma_ops; 1172 else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG) 1173 dev->dma_ops = &contig_dma_ops; 1174 else if (dev->dma_mode == TW686X_DMA_MODE_SG) 1175 dev->dma_ops = &sg_dma_ops; 1176 else 1177 return -EINVAL; 1178 1179 err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev); 1180 if (err) 1181 return err; 1182 1183 if (dev->dma_ops->setup) { 1184 err = dev->dma_ops->setup(dev); 1185 if (err) 1186 return err; 1187 } 1188 1189 /* Initialize vc->dev and vc->ch for the error path */ 1190 for (ch = 0; ch < max_channels(dev); ch++) { 1191 struct tw686x_video_channel *vc = &dev->video_channels[ch]; 1192 1193 vc->dev = dev; 1194 vc->ch = ch; 1195 } 1196 1197 for (ch = 0; ch < max_channels(dev); ch++) { 1198 struct tw686x_video_channel *vc = &dev->video_channels[ch]; 1199 struct video_device *vdev; 1200 1201 mutex_init(&vc->vb_mutex); 1202 spin_lock_init(&vc->qlock); 1203 INIT_LIST_HEAD(&vc->vidq_queued); 1204 1205 /* default settings */ 1206 err = tw686x_set_standard(vc, V4L2_STD_NTSC); 1207 if (err) 1208 goto error; 1209 1210 err = tw686x_set_format(vc, formats[0].fourcc, 1211 TW686X_VIDEO_WIDTH, 1212 TW686X_VIDEO_HEIGHT(vc->video_standard), 1213 true); 1214 if (err) 1215 goto error; 1216 1217 tw686x_set_input(vc, 0); 1218 tw686x_set_framerate(vc, 30); 1219 reg_write(dev, VDELAY_LO[ch], 0x14); 1220 reg_write(dev, HACTIVE_LO[ch], 0xd0); 1221 reg_write(dev, VIDEO_SIZE[ch], 0); 1222 1223 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF; 1224 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 1225 vc->vidq.drv_priv = vc; 1226 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf); 1227 vc->vidq.ops = &tw686x_video_qops; 1228 vc->vidq.mem_ops = dev->dma_ops->mem_ops; 1229 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1230 vc->vidq.min_buffers_needed = 2; 1231 vc->vidq.lock = &vc->vb_mutex; 1232 vc->vidq.gfp_flags = dev->dma_mode != TW686X_DMA_MODE_MEMCPY ? 1233 GFP_DMA32 : 0; 1234 vc->vidq.dev = &dev->pci_dev->dev; 1235 1236 err = vb2_queue_init(&vc->vidq); 1237 if (err) { 1238 v4l2_err(&dev->v4l2_dev, 1239 "dma%d: cannot init vb2 queue\n", ch); 1240 goto error; 1241 } 1242 1243 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4); 1244 if (err) { 1245 v4l2_err(&dev->v4l2_dev, 1246 "dma%d: cannot init ctrl handler\n", ch); 1247 goto error; 1248 } 1249 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops, 1250 V4L2_CID_BRIGHTNESS, -128, 127, 1, 0); 1251 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops, 1252 V4L2_CID_CONTRAST, 0, 255, 1, 100); 1253 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops, 1254 V4L2_CID_SATURATION, 0, 255, 1, 128); 1255 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops, 1256 V4L2_CID_HUE, -128, 127, 1, 0); 1257 err = vc->ctrl_handler.error; 1258 if (err) 1259 goto error; 1260 1261 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler); 1262 if (err) 1263 goto error; 1264 1265 vdev = video_device_alloc(); 1266 if (!vdev) { 1267 v4l2_err(&dev->v4l2_dev, 1268 "dma%d: unable to allocate device\n", ch); 1269 err = -ENOMEM; 1270 goto error; 1271 } 1272 1273 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name); 1274 vdev->fops = &tw686x_video_fops; 1275 vdev->ioctl_ops = &tw686x_video_ioctl_ops; 1276 vdev->release = video_device_release; 1277 vdev->v4l2_dev = &dev->v4l2_dev; 1278 vdev->queue = &vc->vidq; 1279 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50; 1280 vdev->minor = -1; 1281 vdev->lock = &vc->vb_mutex; 1282 vdev->ctrl_handler = &vc->ctrl_handler; 1283 vc->device = vdev; 1284 video_set_drvdata(vdev, vc); 1285 1286 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1); 1287 if (err < 0) 1288 goto error; 1289 vc->num = vdev->num; 1290 } 1291 1292 val = TW686X_DEF_PHASE_REF; 1293 for (ch = 0; ch < max_channels(dev); ch++) 1294 val |= dev->dma_ops->hw_dma_mode << (16 + ch * 2); 1295 reg_write(dev, PHASE_REF, val); 1296 1297 reg_write(dev, MISC2[0], 0xe7); 1298 reg_write(dev, VCTRL1[0], 0xcc); 1299 reg_write(dev, LOOP[0], 0xa5); 1300 if (max_channels(dev) > 4) { 1301 reg_write(dev, VCTRL1[1], 0xcc); 1302 reg_write(dev, LOOP[1], 0xa5); 1303 reg_write(dev, MISC2[1], 0xe7); 1304 } 1305 return 0; 1306 1307 error: 1308 tw686x_video_free(dev); 1309 return err; 1310 } 1311