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