1 /* 2 * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com> 3 * 4 * Original author: 5 * Ben Collins <bcollins@ubuntu.com> 6 * 7 * Additional work by: 8 * John Brooks <john.brooks@bluecherry.net> 9 * 10 * This program is free software; you can redistribute it and/or modify 11 * it under the terms of the GNU General Public License as published by 12 * the Free Software Foundation; either version 2 of the License, or 13 * (at your option) any later version. 14 * 15 * This program is distributed in the hope that it will be useful, 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 * GNU General Public License for more details. 19 */ 20 21 #include <linux/kernel.h> 22 #include <linux/module.h> 23 #include <linux/kthread.h> 24 #include <linux/freezer.h> 25 26 #include <media/v4l2-ioctl.h> 27 #include <media/v4l2-common.h> 28 #include <media/v4l2-event.h> 29 #include <media/videobuf2-dma-contig.h> 30 31 #include "solo6x10.h" 32 #include "solo6x10-tw28.h" 33 34 /* Image size is two fields, SOLO_HW_BPL is one horizontal line in hardware */ 35 #define SOLO_HW_BPL 2048 36 #define solo_vlines(__solo) (__solo->video_vsize * 2) 37 #define solo_image_size(__solo) (solo_bytesperline(__solo) * \ 38 solo_vlines(__solo)) 39 #define solo_bytesperline(__solo) (__solo->video_hsize * 2) 40 41 #define MIN_VID_BUFFERS 2 42 43 static inline void erase_on(struct solo_dev *solo_dev) 44 { 45 solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON); 46 solo_dev->erasing = 1; 47 solo_dev->frame_blank = 0; 48 } 49 50 static inline int erase_off(struct solo_dev *solo_dev) 51 { 52 if (!solo_dev->erasing) 53 return 0; 54 55 /* First time around, assert erase off */ 56 if (!solo_dev->frame_blank) 57 solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, 0); 58 /* Keep the erasing flag on for 8 frames minimum */ 59 if (solo_dev->frame_blank++ >= 8) 60 solo_dev->erasing = 0; 61 62 return 1; 63 } 64 65 void solo_video_in_isr(struct solo_dev *solo_dev) 66 { 67 wake_up_interruptible_all(&solo_dev->disp_thread_wait); 68 } 69 70 static void solo_win_setup(struct solo_dev *solo_dev, u8 ch, 71 int sx, int sy, int ex, int ey, int scale) 72 { 73 if (ch >= solo_dev->nr_chans) 74 return; 75 76 /* Here, we just keep window/channel the same */ 77 solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL0(ch), 78 SOLO_VI_WIN_CHANNEL(ch) | 79 SOLO_VI_WIN_SX(sx) | 80 SOLO_VI_WIN_EX(ex) | 81 SOLO_VI_WIN_SCALE(scale)); 82 83 solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL1(ch), 84 SOLO_VI_WIN_SY(sy) | 85 SOLO_VI_WIN_EY(ey)); 86 } 87 88 static int solo_v4l2_ch_ext_4up(struct solo_dev *solo_dev, u8 idx, int on) 89 { 90 u8 ch = idx * 4; 91 92 if (ch >= solo_dev->nr_chans) 93 return -EINVAL; 94 95 if (!on) { 96 u8 i; 97 98 for (i = ch; i < ch + 4; i++) 99 solo_win_setup(solo_dev, i, solo_dev->video_hsize, 100 solo_vlines(solo_dev), 101 solo_dev->video_hsize, 102 solo_vlines(solo_dev), 0); 103 return 0; 104 } 105 106 /* Row 1 */ 107 solo_win_setup(solo_dev, ch, 0, 0, solo_dev->video_hsize / 2, 108 solo_vlines(solo_dev) / 2, 3); 109 solo_win_setup(solo_dev, ch + 1, solo_dev->video_hsize / 2, 0, 110 solo_dev->video_hsize, solo_vlines(solo_dev) / 2, 3); 111 /* Row 2 */ 112 solo_win_setup(solo_dev, ch + 2, 0, solo_vlines(solo_dev) / 2, 113 solo_dev->video_hsize / 2, solo_vlines(solo_dev), 3); 114 solo_win_setup(solo_dev, ch + 3, solo_dev->video_hsize / 2, 115 solo_vlines(solo_dev) / 2, solo_dev->video_hsize, 116 solo_vlines(solo_dev), 3); 117 118 return 0; 119 } 120 121 static int solo_v4l2_ch_ext_16up(struct solo_dev *solo_dev, int on) 122 { 123 int sy, ysize, hsize, i; 124 125 if (!on) { 126 for (i = 0; i < 16; i++) 127 solo_win_setup(solo_dev, i, solo_dev->video_hsize, 128 solo_vlines(solo_dev), 129 solo_dev->video_hsize, 130 solo_vlines(solo_dev), 0); 131 return 0; 132 } 133 134 ysize = solo_vlines(solo_dev) / 4; 135 hsize = solo_dev->video_hsize / 4; 136 137 for (sy = 0, i = 0; i < 4; i++, sy += ysize) { 138 solo_win_setup(solo_dev, i * 4, 0, sy, hsize, 139 sy + ysize, 5); 140 solo_win_setup(solo_dev, (i * 4) + 1, hsize, sy, 141 hsize * 2, sy + ysize, 5); 142 solo_win_setup(solo_dev, (i * 4) + 2, hsize * 2, sy, 143 hsize * 3, sy + ysize, 5); 144 solo_win_setup(solo_dev, (i * 4) + 3, hsize * 3, sy, 145 solo_dev->video_hsize, sy + ysize, 5); 146 } 147 148 return 0; 149 } 150 151 static int solo_v4l2_ch(struct solo_dev *solo_dev, u8 ch, int on) 152 { 153 u8 ext_ch; 154 155 if (ch < solo_dev->nr_chans) { 156 solo_win_setup(solo_dev, ch, on ? 0 : solo_dev->video_hsize, 157 on ? 0 : solo_vlines(solo_dev), 158 solo_dev->video_hsize, solo_vlines(solo_dev), 159 on ? 1 : 0); 160 return 0; 161 } 162 163 if (ch >= solo_dev->nr_chans + solo_dev->nr_ext) 164 return -EINVAL; 165 166 ext_ch = ch - solo_dev->nr_chans; 167 168 /* 4up's first */ 169 if (ext_ch < 4) 170 return solo_v4l2_ch_ext_4up(solo_dev, ext_ch, on); 171 172 /* Remaining case is 16up for 16-port */ 173 return solo_v4l2_ch_ext_16up(solo_dev, on); 174 } 175 176 static int solo_v4l2_set_ch(struct solo_dev *solo_dev, u8 ch) 177 { 178 if (ch >= solo_dev->nr_chans + solo_dev->nr_ext) 179 return -EINVAL; 180 181 erase_on(solo_dev); 182 183 solo_v4l2_ch(solo_dev, solo_dev->cur_disp_ch, 0); 184 solo_v4l2_ch(solo_dev, ch, 1); 185 186 solo_dev->cur_disp_ch = ch; 187 188 return 0; 189 } 190 191 static void solo_fillbuf(struct solo_dev *solo_dev, 192 struct vb2_buffer *vb) 193 { 194 dma_addr_t vbuf; 195 unsigned int fdma_addr; 196 int error = -1; 197 int i; 198 199 vbuf = vb2_dma_contig_plane_dma_addr(vb, 0); 200 if (!vbuf) 201 goto finish_buf; 202 203 if (erase_off(solo_dev)) { 204 void *p = vb2_plane_vaddr(vb, 0); 205 int image_size = solo_image_size(solo_dev); 206 207 for (i = 0; i < image_size; i += 2) { 208 ((u8 *)p)[i] = 0x80; 209 ((u8 *)p)[i + 1] = 0x00; 210 } 211 error = 0; 212 } else { 213 fdma_addr = SOLO_DISP_EXT_ADDR + (solo_dev->old_write * 214 (SOLO_HW_BPL * solo_vlines(solo_dev))); 215 216 error = solo_p2m_dma_t(solo_dev, 0, vbuf, fdma_addr, 217 solo_bytesperline(solo_dev), 218 solo_vlines(solo_dev), SOLO_HW_BPL); 219 } 220 221 finish_buf: 222 if (!error) { 223 vb2_set_plane_payload(vb, 0, 224 solo_vlines(solo_dev) * solo_bytesperline(solo_dev)); 225 vb->v4l2_buf.sequence = solo_dev->sequence++; 226 v4l2_get_timestamp(&vb->v4l2_buf.timestamp); 227 } 228 229 vb2_buffer_done(vb, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); 230 } 231 232 static void solo_thread_try(struct solo_dev *solo_dev) 233 { 234 struct solo_vb2_buf *vb; 235 236 /* Only "break" from this loop if slock is held, otherwise 237 * just return. */ 238 for (;;) { 239 unsigned int cur_write; 240 241 cur_write = SOLO_VI_STATUS0_PAGE( 242 solo_reg_read(solo_dev, SOLO_VI_STATUS0)); 243 if (cur_write == solo_dev->old_write) 244 return; 245 246 spin_lock(&solo_dev->slock); 247 248 if (list_empty(&solo_dev->vidq_active)) 249 break; 250 251 vb = list_first_entry(&solo_dev->vidq_active, struct solo_vb2_buf, 252 list); 253 254 solo_dev->old_write = cur_write; 255 list_del(&vb->list); 256 257 spin_unlock(&solo_dev->slock); 258 259 solo_fillbuf(solo_dev, &vb->vb); 260 } 261 262 assert_spin_locked(&solo_dev->slock); 263 spin_unlock(&solo_dev->slock); 264 } 265 266 static int solo_thread(void *data) 267 { 268 struct solo_dev *solo_dev = data; 269 DECLARE_WAITQUEUE(wait, current); 270 271 set_freezable(); 272 add_wait_queue(&solo_dev->disp_thread_wait, &wait); 273 274 for (;;) { 275 long timeout = schedule_timeout_interruptible(HZ); 276 277 if (timeout == -ERESTARTSYS || kthread_should_stop()) 278 break; 279 solo_thread_try(solo_dev); 280 try_to_freeze(); 281 } 282 283 remove_wait_queue(&solo_dev->disp_thread_wait, &wait); 284 285 return 0; 286 } 287 288 static int solo_start_thread(struct solo_dev *solo_dev) 289 { 290 int ret = 0; 291 292 solo_dev->kthread = kthread_run(solo_thread, solo_dev, SOLO6X10_NAME "_disp"); 293 294 if (IS_ERR(solo_dev->kthread)) { 295 ret = PTR_ERR(solo_dev->kthread); 296 solo_dev->kthread = NULL; 297 return ret; 298 } 299 solo_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN); 300 301 return ret; 302 } 303 304 static void solo_stop_thread(struct solo_dev *solo_dev) 305 { 306 if (!solo_dev->kthread) 307 return; 308 309 solo_irq_off(solo_dev, SOLO_IRQ_VIDEO_IN); 310 kthread_stop(solo_dev->kthread); 311 solo_dev->kthread = NULL; 312 } 313 314 static int solo_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt, 315 unsigned int *num_buffers, unsigned int *num_planes, 316 unsigned int sizes[], void *alloc_ctxs[]) 317 { 318 struct solo_dev *solo_dev = vb2_get_drv_priv(q); 319 320 sizes[0] = solo_image_size(solo_dev); 321 alloc_ctxs[0] = solo_dev->alloc_ctx; 322 *num_planes = 1; 323 324 if (*num_buffers < MIN_VID_BUFFERS) 325 *num_buffers = MIN_VID_BUFFERS; 326 327 return 0; 328 } 329 330 static int solo_start_streaming(struct vb2_queue *q, unsigned int count) 331 { 332 struct solo_dev *solo_dev = vb2_get_drv_priv(q); 333 334 solo_dev->sequence = 0; 335 return solo_start_thread(solo_dev); 336 } 337 338 static void solo_stop_streaming(struct vb2_queue *q) 339 { 340 struct solo_dev *solo_dev = vb2_get_drv_priv(q); 341 342 solo_stop_thread(solo_dev); 343 INIT_LIST_HEAD(&solo_dev->vidq_active); 344 } 345 346 static void solo_buf_queue(struct vb2_buffer *vb) 347 { 348 struct vb2_queue *vq = vb->vb2_queue; 349 struct solo_dev *solo_dev = vb2_get_drv_priv(vq); 350 struct solo_vb2_buf *solo_vb = 351 container_of(vb, struct solo_vb2_buf, vb); 352 353 spin_lock(&solo_dev->slock); 354 list_add_tail(&solo_vb->list, &solo_dev->vidq_active); 355 spin_unlock(&solo_dev->slock); 356 wake_up_interruptible(&solo_dev->disp_thread_wait); 357 } 358 359 static const struct vb2_ops solo_video_qops = { 360 .queue_setup = solo_queue_setup, 361 .buf_queue = solo_buf_queue, 362 .start_streaming = solo_start_streaming, 363 .stop_streaming = solo_stop_streaming, 364 .wait_prepare = vb2_ops_wait_prepare, 365 .wait_finish = vb2_ops_wait_finish, 366 }; 367 368 static int solo_querycap(struct file *file, void *priv, 369 struct v4l2_capability *cap) 370 { 371 struct solo_dev *solo_dev = video_drvdata(file); 372 373 strcpy(cap->driver, SOLO6X10_NAME); 374 strcpy(cap->card, "Softlogic 6x10"); 375 snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s", 376 pci_name(solo_dev->pdev)); 377 cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | 378 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING; 379 cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; 380 return 0; 381 } 382 383 static int solo_enum_ext_input(struct solo_dev *solo_dev, 384 struct v4l2_input *input) 385 { 386 static const char * const dispnames_1[] = { "4UP" }; 387 static const char * const dispnames_2[] = { "4UP-1", "4UP-2" }; 388 static const char * const dispnames_5[] = { 389 "4UP-1", "4UP-2", "4UP-3", "4UP-4", "16UP" 390 }; 391 const char * const *dispnames; 392 393 if (input->index >= (solo_dev->nr_chans + solo_dev->nr_ext)) 394 return -EINVAL; 395 396 if (solo_dev->nr_ext == 5) 397 dispnames = dispnames_5; 398 else if (solo_dev->nr_ext == 2) 399 dispnames = dispnames_2; 400 else 401 dispnames = dispnames_1; 402 403 snprintf(input->name, sizeof(input->name), "Multi %s", 404 dispnames[input->index - solo_dev->nr_chans]); 405 406 return 0; 407 } 408 409 static int solo_enum_input(struct file *file, void *priv, 410 struct v4l2_input *input) 411 { 412 struct solo_dev *solo_dev = video_drvdata(file); 413 414 if (input->index >= solo_dev->nr_chans) { 415 int ret = solo_enum_ext_input(solo_dev, input); 416 417 if (ret < 0) 418 return ret; 419 } else { 420 snprintf(input->name, sizeof(input->name), "Camera %d", 421 input->index + 1); 422 423 /* We can only check this for normal inputs */ 424 if (!tw28_get_video_status(solo_dev, input->index)) 425 input->status = V4L2_IN_ST_NO_SIGNAL; 426 } 427 428 input->type = V4L2_INPUT_TYPE_CAMERA; 429 input->std = solo_dev->vfd->tvnorms; 430 return 0; 431 } 432 433 static int solo_set_input(struct file *file, void *priv, unsigned int index) 434 { 435 struct solo_dev *solo_dev = video_drvdata(file); 436 int ret = solo_v4l2_set_ch(solo_dev, index); 437 438 if (!ret) { 439 while (erase_off(solo_dev)) 440 /* Do nothing */; 441 } 442 443 return ret; 444 } 445 446 static int solo_get_input(struct file *file, void *priv, unsigned int *index) 447 { 448 struct solo_dev *solo_dev = video_drvdata(file); 449 450 *index = solo_dev->cur_disp_ch; 451 452 return 0; 453 } 454 455 static int solo_enum_fmt_cap(struct file *file, void *priv, 456 struct v4l2_fmtdesc *f) 457 { 458 if (f->index) 459 return -EINVAL; 460 461 f->pixelformat = V4L2_PIX_FMT_UYVY; 462 strlcpy(f->description, "UYUV 4:2:2 Packed", sizeof(f->description)); 463 464 return 0; 465 } 466 467 static int solo_try_fmt_cap(struct file *file, void *priv, 468 struct v4l2_format *f) 469 { 470 struct solo_dev *solo_dev = video_drvdata(file); 471 struct v4l2_pix_format *pix = &f->fmt.pix; 472 int image_size = solo_image_size(solo_dev); 473 474 if (pix->pixelformat != V4L2_PIX_FMT_UYVY) 475 return -EINVAL; 476 477 pix->width = solo_dev->video_hsize; 478 pix->height = solo_vlines(solo_dev); 479 pix->sizeimage = image_size; 480 pix->field = V4L2_FIELD_INTERLACED; 481 pix->pixelformat = V4L2_PIX_FMT_UYVY; 482 pix->colorspace = V4L2_COLORSPACE_SMPTE170M; 483 pix->priv = 0; 484 return 0; 485 } 486 487 static int solo_set_fmt_cap(struct file *file, void *priv, 488 struct v4l2_format *f) 489 { 490 struct solo_dev *solo_dev = video_drvdata(file); 491 492 if (vb2_is_busy(&solo_dev->vidq)) 493 return -EBUSY; 494 495 /* For right now, if it doesn't match our running config, 496 * then fail */ 497 return solo_try_fmt_cap(file, priv, f); 498 } 499 500 static int solo_get_fmt_cap(struct file *file, void *priv, 501 struct v4l2_format *f) 502 { 503 struct solo_dev *solo_dev = video_drvdata(file); 504 struct v4l2_pix_format *pix = &f->fmt.pix; 505 506 pix->width = solo_dev->video_hsize; 507 pix->height = solo_vlines(solo_dev); 508 pix->pixelformat = V4L2_PIX_FMT_UYVY; 509 pix->field = V4L2_FIELD_INTERLACED; 510 pix->sizeimage = solo_image_size(solo_dev); 511 pix->colorspace = V4L2_COLORSPACE_SMPTE170M; 512 pix->bytesperline = solo_bytesperline(solo_dev); 513 pix->priv = 0; 514 515 return 0; 516 } 517 518 static int solo_g_std(struct file *file, void *priv, v4l2_std_id *i) 519 { 520 struct solo_dev *solo_dev = video_drvdata(file); 521 522 if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) 523 *i = V4L2_STD_NTSC_M; 524 else 525 *i = V4L2_STD_PAL; 526 return 0; 527 } 528 529 int solo_set_video_type(struct solo_dev *solo_dev, bool is_50hz) 530 { 531 int i; 532 533 /* Make sure all video nodes are idle */ 534 if (vb2_is_busy(&solo_dev->vidq)) 535 return -EBUSY; 536 for (i = 0; i < solo_dev->nr_chans; i++) 537 if (vb2_is_busy(&solo_dev->v4l2_enc[i]->vidq)) 538 return -EBUSY; 539 solo_dev->video_type = is_50hz ? SOLO_VO_FMT_TYPE_PAL : 540 SOLO_VO_FMT_TYPE_NTSC; 541 /* Reconfigure for the new standard */ 542 solo_disp_init(solo_dev); 543 solo_enc_init(solo_dev); 544 solo_tw28_init(solo_dev); 545 for (i = 0; i < solo_dev->nr_chans; i++) 546 solo_update_mode(solo_dev->v4l2_enc[i]); 547 return solo_v4l2_set_ch(solo_dev, solo_dev->cur_disp_ch); 548 } 549 550 static int solo_s_std(struct file *file, void *priv, v4l2_std_id std) 551 { 552 struct solo_dev *solo_dev = video_drvdata(file); 553 554 return solo_set_video_type(solo_dev, std & V4L2_STD_625_50); 555 } 556 557 static int solo_s_ctrl(struct v4l2_ctrl *ctrl) 558 { 559 struct solo_dev *solo_dev = 560 container_of(ctrl->handler, struct solo_dev, disp_hdl); 561 562 switch (ctrl->id) { 563 case V4L2_CID_MOTION_TRACE: 564 if (ctrl->val) { 565 solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 566 SOLO_VI_MOTION_Y_ADD | 567 SOLO_VI_MOTION_Y_VALUE(0x20) | 568 SOLO_VI_MOTION_CB_VALUE(0x10) | 569 SOLO_VI_MOTION_CR_VALUE(0x10)); 570 solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 571 SOLO_VI_MOTION_CR_ADD | 572 SOLO_VI_MOTION_Y_VALUE(0x10) | 573 SOLO_VI_MOTION_CB_VALUE(0x80) | 574 SOLO_VI_MOTION_CR_VALUE(0x10)); 575 } else { 576 solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0); 577 solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0); 578 } 579 return 0; 580 default: 581 break; 582 } 583 return -EINVAL; 584 } 585 586 static const struct v4l2_file_operations solo_v4l2_fops = { 587 .owner = THIS_MODULE, 588 .open = v4l2_fh_open, 589 .release = vb2_fop_release, 590 .read = vb2_fop_read, 591 .poll = vb2_fop_poll, 592 .mmap = vb2_fop_mmap, 593 .unlocked_ioctl = video_ioctl2, 594 }; 595 596 static const struct v4l2_ioctl_ops solo_v4l2_ioctl_ops = { 597 .vidioc_querycap = solo_querycap, 598 .vidioc_s_std = solo_s_std, 599 .vidioc_g_std = solo_g_std, 600 /* Input callbacks */ 601 .vidioc_enum_input = solo_enum_input, 602 .vidioc_s_input = solo_set_input, 603 .vidioc_g_input = solo_get_input, 604 /* Video capture format callbacks */ 605 .vidioc_enum_fmt_vid_cap = solo_enum_fmt_cap, 606 .vidioc_try_fmt_vid_cap = solo_try_fmt_cap, 607 .vidioc_s_fmt_vid_cap = solo_set_fmt_cap, 608 .vidioc_g_fmt_vid_cap = solo_get_fmt_cap, 609 /* Streaming I/O */ 610 .vidioc_reqbufs = vb2_ioctl_reqbufs, 611 .vidioc_querybuf = vb2_ioctl_querybuf, 612 .vidioc_qbuf = vb2_ioctl_qbuf, 613 .vidioc_dqbuf = vb2_ioctl_dqbuf, 614 .vidioc_streamon = vb2_ioctl_streamon, 615 .vidioc_streamoff = vb2_ioctl_streamoff, 616 /* Logging and events */ 617 .vidioc_log_status = v4l2_ctrl_log_status, 618 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 619 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 620 }; 621 622 static struct video_device solo_v4l2_template = { 623 .name = SOLO6X10_NAME, 624 .fops = &solo_v4l2_fops, 625 .ioctl_ops = &solo_v4l2_ioctl_ops, 626 .minor = -1, 627 .release = video_device_release, 628 .tvnorms = V4L2_STD_NTSC_M | V4L2_STD_PAL, 629 }; 630 631 static const struct v4l2_ctrl_ops solo_ctrl_ops = { 632 .s_ctrl = solo_s_ctrl, 633 }; 634 635 static const struct v4l2_ctrl_config solo_motion_trace_ctrl = { 636 .ops = &solo_ctrl_ops, 637 .id = V4L2_CID_MOTION_TRACE, 638 .name = "Motion Detection Trace", 639 .type = V4L2_CTRL_TYPE_BOOLEAN, 640 .max = 1, 641 .step = 1, 642 }; 643 644 int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr) 645 { 646 int ret; 647 int i; 648 649 init_waitqueue_head(&solo_dev->disp_thread_wait); 650 spin_lock_init(&solo_dev->slock); 651 mutex_init(&solo_dev->lock); 652 INIT_LIST_HEAD(&solo_dev->vidq_active); 653 654 solo_dev->vfd = video_device_alloc(); 655 if (!solo_dev->vfd) 656 return -ENOMEM; 657 658 *solo_dev->vfd = solo_v4l2_template; 659 solo_dev->vfd->v4l2_dev = &solo_dev->v4l2_dev; 660 solo_dev->vfd->queue = &solo_dev->vidq; 661 solo_dev->vfd->lock = &solo_dev->lock; 662 v4l2_ctrl_handler_init(&solo_dev->disp_hdl, 1); 663 v4l2_ctrl_new_custom(&solo_dev->disp_hdl, &solo_motion_trace_ctrl, NULL); 664 if (solo_dev->disp_hdl.error) { 665 ret = solo_dev->disp_hdl.error; 666 goto fail; 667 } 668 solo_dev->vfd->ctrl_handler = &solo_dev->disp_hdl; 669 670 video_set_drvdata(solo_dev->vfd, solo_dev); 671 672 solo_dev->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 673 solo_dev->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ; 674 solo_dev->vidq.ops = &solo_video_qops; 675 solo_dev->vidq.mem_ops = &vb2_dma_contig_memops; 676 solo_dev->vidq.drv_priv = solo_dev; 677 solo_dev->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 678 solo_dev->vidq.gfp_flags = __GFP_DMA32; 679 solo_dev->vidq.buf_struct_size = sizeof(struct solo_vb2_buf); 680 solo_dev->vidq.lock = &solo_dev->lock; 681 ret = vb2_queue_init(&solo_dev->vidq); 682 if (ret < 0) 683 goto fail; 684 685 solo_dev->alloc_ctx = vb2_dma_contig_init_ctx(&solo_dev->pdev->dev); 686 if (IS_ERR(solo_dev->alloc_ctx)) { 687 dev_err(&solo_dev->pdev->dev, "Can't allocate buffer context"); 688 return PTR_ERR(solo_dev->alloc_ctx); 689 } 690 691 /* Cycle all the channels and clear */ 692 for (i = 0; i < solo_dev->nr_chans; i++) { 693 solo_v4l2_set_ch(solo_dev, i); 694 while (erase_off(solo_dev)) 695 /* Do nothing */; 696 } 697 698 /* Set the default display channel */ 699 solo_v4l2_set_ch(solo_dev, 0); 700 while (erase_off(solo_dev)) 701 /* Do nothing */; 702 703 ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, nr); 704 if (ret < 0) 705 goto fail; 706 707 snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)", 708 SOLO6X10_NAME, solo_dev->vfd->num); 709 710 dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with " 711 "%d inputs (%d extended)\n", solo_dev->vfd->num, 712 solo_dev->nr_chans, solo_dev->nr_ext); 713 714 return 0; 715 716 fail: 717 video_device_release(solo_dev->vfd); 718 vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx); 719 v4l2_ctrl_handler_free(&solo_dev->disp_hdl); 720 solo_dev->vfd = NULL; 721 return ret; 722 } 723 724 void solo_v4l2_exit(struct solo_dev *solo_dev) 725 { 726 if (solo_dev->vfd == NULL) 727 return; 728 729 video_unregister_device(solo_dev->vfd); 730 vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx); 731 v4l2_ctrl_handler_free(&solo_dev->disp_hdl); 732 solo_dev->vfd = NULL; 733 } 734