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