1 /* 2 * vpif-display - VPIF display driver 3 * Display driver for TI DaVinci VPIF 4 * 5 * Copyright (C) 2009 Texas Instruments Incorporated - https://www.ti.com/ 6 * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@gmail.com> 7 * 8 * This program is free software; you can redistribute it and/or 9 * modify it under the terms of the GNU General Public License as 10 * published by the Free Software Foundation version 2. 11 * 12 * This program is distributed .as is. WITHOUT ANY WARRANTY of any 13 * kind, whether express or implied; without even the implied warranty 14 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 * GNU General Public License for more details. 16 */ 17 18 #include <linux/interrupt.h> 19 #include <linux/module.h> 20 #include <linux/platform_device.h> 21 #include <linux/slab.h> 22 23 #include <media/v4l2-ioctl.h> 24 25 #include "vpif.h" 26 #include "vpif_display.h" 27 28 MODULE_DESCRIPTION("TI DaVinci VPIF Display driver"); 29 MODULE_LICENSE("GPL"); 30 MODULE_VERSION(VPIF_DISPLAY_VERSION); 31 32 #define VPIF_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50) 33 34 #define vpif_err(fmt, arg...) v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg) 35 #define vpif_dbg(level, debug, fmt, arg...) \ 36 v4l2_dbg(level, debug, &vpif_obj.v4l2_dev, fmt, ## arg) 37 38 static int debug = 1; 39 40 module_param(debug, int, 0644); 41 42 MODULE_PARM_DESC(debug, "Debug level 0-1"); 43 44 #define VPIF_DRIVER_NAME "vpif_display" 45 MODULE_ALIAS("platform:" VPIF_DRIVER_NAME); 46 47 /* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */ 48 static int ycmux_mode; 49 50 static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} }; 51 52 static struct vpif_device vpif_obj = { {NULL} }; 53 static struct device *vpif_dev; 54 static void vpif_calculate_offsets(struct channel_obj *ch); 55 static void vpif_config_addr(struct channel_obj *ch, int muxmode); 56 57 static inline 58 struct vpif_disp_buffer *to_vpif_buffer(struct vb2_v4l2_buffer *vb) 59 { 60 return container_of(vb, struct vpif_disp_buffer, vb); 61 } 62 63 /** 64 * vpif_buffer_prepare : callback function for buffer prepare 65 * @vb: ptr to vb2_buffer 66 * 67 * This is the callback function for buffer prepare when vb2_qbuf() 68 * function is called. The buffer is prepared and user space virtual address 69 * or user address is converted into physical address 70 */ 71 static int vpif_buffer_prepare(struct vb2_buffer *vb) 72 { 73 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 74 struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue); 75 struct common_obj *common; 76 77 common = &ch->common[VPIF_VIDEO_INDEX]; 78 79 vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage); 80 if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) 81 return -EINVAL; 82 83 vbuf->field = common->fmt.fmt.pix.field; 84 85 if (vb->vb2_queue->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT) { 86 unsigned long addr = vb2_dma_contig_plane_dma_addr(vb, 0); 87 88 if (!ISALIGNED(addr + common->ytop_off) || 89 !ISALIGNED(addr + common->ybtm_off) || 90 !ISALIGNED(addr + common->ctop_off) || 91 !ISALIGNED(addr + common->cbtm_off)) { 92 vpif_err("buffer offset not aligned to 8 bytes\n"); 93 return -EINVAL; 94 } 95 } 96 97 return 0; 98 } 99 100 /** 101 * vpif_buffer_queue_setup : Callback function for buffer setup. 102 * @vq: vb2_queue ptr 103 * @nbuffers: ptr to number of buffers requested by application 104 * @nplanes: contains number of distinct video planes needed to hold a frame 105 * @sizes: contains the size (in bytes) of each plane. 106 * @alloc_devs: ptr to allocation context 107 * 108 * This callback function is called when reqbuf() is called to adjust 109 * the buffer count and buffer size 110 */ 111 static int vpif_buffer_queue_setup(struct vb2_queue *vq, 112 unsigned int *nbuffers, unsigned int *nplanes, 113 unsigned int sizes[], struct device *alloc_devs[]) 114 { 115 struct channel_obj *ch = vb2_get_drv_priv(vq); 116 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 117 unsigned size = common->fmt.fmt.pix.sizeimage; 118 119 if (*nplanes) { 120 if (sizes[0] < size) 121 return -EINVAL; 122 size = sizes[0]; 123 } 124 125 if (vq->num_buffers + *nbuffers < 3) 126 *nbuffers = 3 - vq->num_buffers; 127 128 *nplanes = 1; 129 sizes[0] = size; 130 131 /* Calculate the offset for Y and C data in the buffer */ 132 vpif_calculate_offsets(ch); 133 134 return 0; 135 } 136 137 /** 138 * vpif_buffer_queue : Callback function to add buffer to DMA queue 139 * @vb: ptr to vb2_buffer 140 * 141 * This callback function queues the buffer to DMA engine 142 */ 143 static void vpif_buffer_queue(struct vb2_buffer *vb) 144 { 145 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 146 struct vpif_disp_buffer *buf = to_vpif_buffer(vbuf); 147 struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue); 148 struct common_obj *common; 149 unsigned long flags; 150 151 common = &ch->common[VPIF_VIDEO_INDEX]; 152 153 /* add the buffer to the DMA queue */ 154 spin_lock_irqsave(&common->irqlock, flags); 155 list_add_tail(&buf->list, &common->dma_queue); 156 spin_unlock_irqrestore(&common->irqlock, flags); 157 } 158 159 /** 160 * vpif_start_streaming : Starts the DMA engine for streaming 161 * @vq: ptr to vb2_buffer 162 * @count: number of buffers 163 */ 164 static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count) 165 { 166 struct vpif_display_config *vpif_config_data = 167 vpif_dev->platform_data; 168 struct channel_obj *ch = vb2_get_drv_priv(vq); 169 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 170 struct vpif_params *vpif = &ch->vpifparams; 171 struct vpif_disp_buffer *buf, *tmp; 172 unsigned long addr, flags; 173 int ret; 174 175 spin_lock_irqsave(&common->irqlock, flags); 176 177 /* Initialize field_id */ 178 ch->field_id = 0; 179 180 /* clock settings */ 181 if (vpif_config_data->set_clock) { 182 ret = vpif_config_data->set_clock(ch->vpifparams.std_info. 183 ycmux_mode, ch->vpifparams.std_info.hd_sd); 184 if (ret < 0) { 185 vpif_err("can't set clock\n"); 186 goto err; 187 } 188 } 189 190 /* set the parameters and addresses */ 191 ret = vpif_set_video_params(vpif, ch->channel_id + 2); 192 if (ret < 0) 193 goto err; 194 195 ycmux_mode = ret; 196 vpif_config_addr(ch, ret); 197 /* Get the next frame from the buffer queue */ 198 common->next_frm = common->cur_frm = 199 list_entry(common->dma_queue.next, 200 struct vpif_disp_buffer, list); 201 202 list_del(&common->cur_frm->list); 203 spin_unlock_irqrestore(&common->irqlock, flags); 204 205 addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb.vb2_buf, 0); 206 common->set_addr((addr + common->ytop_off), 207 (addr + common->ybtm_off), 208 (addr + common->ctop_off), 209 (addr + common->cbtm_off)); 210 211 /* 212 * Set interrupt for both the fields in VPIF 213 * Register enable channel in VPIF register 214 */ 215 channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1; 216 if (VPIF_CHANNEL2_VIDEO == ch->channel_id) { 217 channel2_intr_assert(); 218 channel2_intr_enable(1); 219 enable_channel2(1); 220 if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en) 221 channel2_clipping_enable(1); 222 } 223 224 if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) { 225 channel3_intr_assert(); 226 channel3_intr_enable(1); 227 enable_channel3(1); 228 if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en) 229 channel3_clipping_enable(1); 230 } 231 232 return 0; 233 234 err: 235 list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) { 236 list_del(&buf->list); 237 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED); 238 } 239 spin_unlock_irqrestore(&common->irqlock, flags); 240 241 return ret; 242 } 243 244 /** 245 * vpif_stop_streaming : Stop the DMA engine 246 * @vq: ptr to vb2_queue 247 * 248 * This callback stops the DMA engine and any remaining buffers 249 * in the DMA queue are released. 250 */ 251 static void vpif_stop_streaming(struct vb2_queue *vq) 252 { 253 struct channel_obj *ch = vb2_get_drv_priv(vq); 254 struct common_obj *common; 255 unsigned long flags; 256 257 common = &ch->common[VPIF_VIDEO_INDEX]; 258 259 /* Disable channel */ 260 if (VPIF_CHANNEL2_VIDEO == ch->channel_id) { 261 enable_channel2(0); 262 channel2_intr_enable(0); 263 } 264 if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) { 265 enable_channel3(0); 266 channel3_intr_enable(0); 267 } 268 269 /* release all active buffers */ 270 spin_lock_irqsave(&common->irqlock, flags); 271 if (common->cur_frm == common->next_frm) { 272 vb2_buffer_done(&common->cur_frm->vb.vb2_buf, 273 VB2_BUF_STATE_ERROR); 274 } else { 275 if (common->cur_frm) 276 vb2_buffer_done(&common->cur_frm->vb.vb2_buf, 277 VB2_BUF_STATE_ERROR); 278 if (common->next_frm) 279 vb2_buffer_done(&common->next_frm->vb.vb2_buf, 280 VB2_BUF_STATE_ERROR); 281 } 282 283 while (!list_empty(&common->dma_queue)) { 284 common->next_frm = list_entry(common->dma_queue.next, 285 struct vpif_disp_buffer, list); 286 list_del(&common->next_frm->list); 287 vb2_buffer_done(&common->next_frm->vb.vb2_buf, 288 VB2_BUF_STATE_ERROR); 289 } 290 spin_unlock_irqrestore(&common->irqlock, flags); 291 } 292 293 static const struct vb2_ops video_qops = { 294 .queue_setup = vpif_buffer_queue_setup, 295 .wait_prepare = vb2_ops_wait_prepare, 296 .wait_finish = vb2_ops_wait_finish, 297 .buf_prepare = vpif_buffer_prepare, 298 .start_streaming = vpif_start_streaming, 299 .stop_streaming = vpif_stop_streaming, 300 .buf_queue = vpif_buffer_queue, 301 }; 302 303 static void process_progressive_mode(struct common_obj *common) 304 { 305 unsigned long addr; 306 307 spin_lock(&common->irqlock); 308 /* Get the next buffer from buffer queue */ 309 common->next_frm = list_entry(common->dma_queue.next, 310 struct vpif_disp_buffer, list); 311 /* Remove that buffer from the buffer queue */ 312 list_del(&common->next_frm->list); 313 spin_unlock(&common->irqlock); 314 315 /* Set top and bottom field addrs in VPIF registers */ 316 addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb.vb2_buf, 0); 317 common->set_addr(addr + common->ytop_off, 318 addr + common->ybtm_off, 319 addr + common->ctop_off, 320 addr + common->cbtm_off); 321 } 322 323 static void process_interlaced_mode(int fid, struct common_obj *common) 324 { 325 /* device field id and local field id are in sync */ 326 /* If this is even field */ 327 if (0 == fid) { 328 if (common->cur_frm == common->next_frm) 329 return; 330 331 /* one frame is displayed If next frame is 332 * available, release cur_frm and move on */ 333 /* Copy frame display time */ 334 common->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns(); 335 /* Change status of the cur_frm */ 336 vb2_buffer_done(&common->cur_frm->vb.vb2_buf, 337 VB2_BUF_STATE_DONE); 338 /* Make cur_frm pointing to next_frm */ 339 common->cur_frm = common->next_frm; 340 341 } else if (1 == fid) { /* odd field */ 342 spin_lock(&common->irqlock); 343 if (list_empty(&common->dma_queue) 344 || (common->cur_frm != common->next_frm)) { 345 spin_unlock(&common->irqlock); 346 return; 347 } 348 spin_unlock(&common->irqlock); 349 /* one field is displayed configure the next 350 * frame if it is available else hold on current 351 * frame */ 352 /* Get next from the buffer queue */ 353 process_progressive_mode(common); 354 } 355 } 356 357 /* 358 * vpif_channel_isr: It changes status of the displayed buffer, takes next 359 * buffer from the queue and sets its address in VPIF registers 360 */ 361 static irqreturn_t vpif_channel_isr(int irq, void *dev_id) 362 { 363 struct vpif_device *dev = &vpif_obj; 364 struct channel_obj *ch; 365 struct common_obj *common; 366 int fid = -1, i; 367 int channel_id; 368 369 channel_id = *(int *)(dev_id); 370 if (!vpif_intr_status(channel_id + 2)) 371 return IRQ_NONE; 372 373 ch = dev->dev[channel_id]; 374 for (i = 0; i < VPIF_NUMOBJECTS; i++) { 375 common = &ch->common[i]; 376 /* If streaming is started in this channel */ 377 378 if (1 == ch->vpifparams.std_info.frm_fmt) { 379 spin_lock(&common->irqlock); 380 if (list_empty(&common->dma_queue)) { 381 spin_unlock(&common->irqlock); 382 continue; 383 } 384 spin_unlock(&common->irqlock); 385 386 /* Progressive mode */ 387 if (!channel_first_int[i][channel_id]) { 388 /* Mark status of the cur_frm to 389 * done and unlock semaphore on it */ 390 common->cur_frm->vb.vb2_buf.timestamp = 391 ktime_get_ns(); 392 vb2_buffer_done(&common->cur_frm->vb.vb2_buf, 393 VB2_BUF_STATE_DONE); 394 /* Make cur_frm pointing to next_frm */ 395 common->cur_frm = common->next_frm; 396 } 397 398 channel_first_int[i][channel_id] = 0; 399 process_progressive_mode(common); 400 } else { 401 /* Interlaced mode */ 402 /* If it is first interrupt, ignore it */ 403 404 if (channel_first_int[i][channel_id]) { 405 channel_first_int[i][channel_id] = 0; 406 continue; 407 } 408 409 if (0 == i) { 410 ch->field_id ^= 1; 411 /* Get field id from VPIF registers */ 412 fid = vpif_channel_getfid(ch->channel_id + 2); 413 /* If fid does not match with stored field id */ 414 if (fid != ch->field_id) { 415 /* Make them in sync */ 416 if (0 == fid) 417 ch->field_id = fid; 418 419 return IRQ_HANDLED; 420 } 421 } 422 process_interlaced_mode(fid, common); 423 } 424 } 425 426 return IRQ_HANDLED; 427 } 428 429 static int vpif_update_std_info(struct channel_obj *ch) 430 { 431 struct video_obj *vid_ch = &ch->video; 432 struct vpif_params *vpifparams = &ch->vpifparams; 433 struct vpif_channel_config_params *std_info = &vpifparams->std_info; 434 const struct vpif_channel_config_params *config; 435 436 int i; 437 438 for (i = 0; i < vpif_ch_params_count; i++) { 439 config = &vpif_ch_params[i]; 440 if (config->hd_sd == 0) { 441 vpif_dbg(2, debug, "SD format\n"); 442 if (config->stdid & vid_ch->stdid) { 443 memcpy(std_info, config, sizeof(*config)); 444 break; 445 } 446 } 447 } 448 449 if (i == vpif_ch_params_count) { 450 vpif_dbg(1, debug, "Format not found\n"); 451 return -EINVAL; 452 } 453 454 return 0; 455 } 456 457 static int vpif_update_resolution(struct channel_obj *ch) 458 { 459 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 460 struct video_obj *vid_ch = &ch->video; 461 struct vpif_params *vpifparams = &ch->vpifparams; 462 struct vpif_channel_config_params *std_info = &vpifparams->std_info; 463 464 if (!vid_ch->stdid && !vid_ch->dv_timings.bt.height) 465 return -EINVAL; 466 467 if (vid_ch->stdid) { 468 if (vpif_update_std_info(ch)) 469 return -EINVAL; 470 } 471 472 common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P; 473 common->fmt.fmt.pix.width = std_info->width; 474 common->fmt.fmt.pix.height = std_info->height; 475 vpif_dbg(1, debug, "Pixel details: Width = %d,Height = %d\n", 476 common->fmt.fmt.pix.width, common->fmt.fmt.pix.height); 477 478 /* Set height and width paramateres */ 479 common->height = std_info->height; 480 common->width = std_info->width; 481 common->fmt.fmt.pix.sizeimage = common->height * common->width * 2; 482 483 if (vid_ch->stdid) 484 common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; 485 else 486 common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709; 487 488 if (ch->vpifparams.std_info.frm_fmt) 489 common->fmt.fmt.pix.field = V4L2_FIELD_NONE; 490 else 491 common->fmt.fmt.pix.field = V4L2_FIELD_INTERLACED; 492 493 return 0; 494 } 495 496 /* 497 * vpif_calculate_offsets: This function calculates buffers offset for Y and C 498 * in the top and bottom field 499 */ 500 static void vpif_calculate_offsets(struct channel_obj *ch) 501 { 502 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 503 struct vpif_params *vpifparams = &ch->vpifparams; 504 enum v4l2_field field = common->fmt.fmt.pix.field; 505 struct video_obj *vid_ch = &ch->video; 506 unsigned int hpitch, sizeimage; 507 508 if (V4L2_FIELD_ANY == common->fmt.fmt.pix.field) { 509 if (ch->vpifparams.std_info.frm_fmt) 510 vid_ch->buf_field = V4L2_FIELD_NONE; 511 else 512 vid_ch->buf_field = V4L2_FIELD_INTERLACED; 513 } else { 514 vid_ch->buf_field = common->fmt.fmt.pix.field; 515 } 516 517 sizeimage = common->fmt.fmt.pix.sizeimage; 518 519 hpitch = common->fmt.fmt.pix.bytesperline; 520 if ((V4L2_FIELD_NONE == vid_ch->buf_field) || 521 (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) { 522 common->ytop_off = 0; 523 common->ybtm_off = hpitch; 524 common->ctop_off = sizeimage / 2; 525 common->cbtm_off = sizeimage / 2 + hpitch; 526 } else if (V4L2_FIELD_SEQ_TB == vid_ch->buf_field) { 527 common->ytop_off = 0; 528 common->ybtm_off = sizeimage / 4; 529 common->ctop_off = sizeimage / 2; 530 common->cbtm_off = common->ctop_off + sizeimage / 4; 531 } else if (V4L2_FIELD_SEQ_BT == vid_ch->buf_field) { 532 common->ybtm_off = 0; 533 common->ytop_off = sizeimage / 4; 534 common->cbtm_off = sizeimage / 2; 535 common->ctop_off = common->cbtm_off + sizeimage / 4; 536 } 537 538 if ((V4L2_FIELD_NONE == vid_ch->buf_field) || 539 (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) { 540 vpifparams->video_params.storage_mode = 1; 541 } else { 542 vpifparams->video_params.storage_mode = 0; 543 } 544 545 if (ch->vpifparams.std_info.frm_fmt == 1) { 546 vpifparams->video_params.hpitch = 547 common->fmt.fmt.pix.bytesperline; 548 } else { 549 if ((field == V4L2_FIELD_ANY) || 550 (field == V4L2_FIELD_INTERLACED)) 551 vpifparams->video_params.hpitch = 552 common->fmt.fmt.pix.bytesperline * 2; 553 else 554 vpifparams->video_params.hpitch = 555 common->fmt.fmt.pix.bytesperline; 556 } 557 558 ch->vpifparams.video_params.stdid = ch->vpifparams.std_info.stdid; 559 } 560 561 static void vpif_config_addr(struct channel_obj *ch, int muxmode) 562 { 563 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 564 565 if (VPIF_CHANNEL3_VIDEO == ch->channel_id) { 566 common->set_addr = ch3_set_videobuf_addr; 567 } else { 568 if (2 == muxmode) 569 common->set_addr = ch2_set_videobuf_addr_yc_nmux; 570 else 571 common->set_addr = ch2_set_videobuf_addr; 572 } 573 } 574 575 /* functions implementing ioctls */ 576 /** 577 * vpif_querycap() - QUERYCAP handler 578 * @file: file ptr 579 * @priv: file handle 580 * @cap: ptr to v4l2_capability structure 581 */ 582 static int vpif_querycap(struct file *file, void *priv, 583 struct v4l2_capability *cap) 584 { 585 struct vpif_display_config *config = vpif_dev->platform_data; 586 587 strscpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver)); 588 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s", 589 dev_name(vpif_dev)); 590 strscpy(cap->card, config->card_name, sizeof(cap->card)); 591 592 return 0; 593 } 594 595 static int vpif_enum_fmt_vid_out(struct file *file, void *priv, 596 struct v4l2_fmtdesc *fmt) 597 { 598 if (fmt->index != 0) 599 return -EINVAL; 600 601 /* Fill in the information about format */ 602 fmt->pixelformat = V4L2_PIX_FMT_YUV422P; 603 return 0; 604 } 605 606 static int vpif_g_fmt_vid_out(struct file *file, void *priv, 607 struct v4l2_format *fmt) 608 { 609 struct video_device *vdev = video_devdata(file); 610 struct channel_obj *ch = video_get_drvdata(vdev); 611 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 612 613 /* Check the validity of the buffer type */ 614 if (common->fmt.type != fmt->type) 615 return -EINVAL; 616 617 if (vpif_update_resolution(ch)) 618 return -EINVAL; 619 *fmt = common->fmt; 620 return 0; 621 } 622 623 static int vpif_try_fmt_vid_out(struct file *file, void *priv, 624 struct v4l2_format *fmt) 625 { 626 struct video_device *vdev = video_devdata(file); 627 struct channel_obj *ch = video_get_drvdata(vdev); 628 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 629 struct v4l2_pix_format *pixfmt = &fmt->fmt.pix; 630 631 /* 632 * to suppress v4l-compliance warnings silently correct 633 * the pixelformat 634 */ 635 if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P) 636 pixfmt->pixelformat = common->fmt.fmt.pix.pixelformat; 637 638 if (vpif_update_resolution(ch)) 639 return -EINVAL; 640 641 pixfmt->colorspace = common->fmt.fmt.pix.colorspace; 642 pixfmt->field = common->fmt.fmt.pix.field; 643 pixfmt->bytesperline = common->fmt.fmt.pix.width; 644 pixfmt->width = common->fmt.fmt.pix.width; 645 pixfmt->height = common->fmt.fmt.pix.height; 646 pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2; 647 648 return 0; 649 } 650 651 static int vpif_s_fmt_vid_out(struct file *file, void *priv, 652 struct v4l2_format *fmt) 653 { 654 struct video_device *vdev = video_devdata(file); 655 struct channel_obj *ch = video_get_drvdata(vdev); 656 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 657 struct v4l2_pix_format *pixfmt = &fmt->fmt.pix; 658 int ret; 659 660 if (vb2_is_busy(&common->buffer_queue)) 661 return -EBUSY; 662 663 ret = vpif_try_fmt_vid_out(file, priv, fmt); 664 if (ret) 665 return ret; 666 667 /* store the pix format in the channel object */ 668 common->fmt.fmt.pix = *pixfmt; 669 670 /* store the format in the channel object */ 671 common->fmt = *fmt; 672 return 0; 673 } 674 675 static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id) 676 { 677 struct vpif_display_config *config = vpif_dev->platform_data; 678 struct video_device *vdev = video_devdata(file); 679 struct channel_obj *ch = video_get_drvdata(vdev); 680 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 681 struct vpif_display_chan_config *chan_cfg; 682 struct v4l2_output output; 683 int ret; 684 685 if (!config->chan_config[ch->channel_id].outputs) 686 return -ENODATA; 687 688 chan_cfg = &config->chan_config[ch->channel_id]; 689 output = chan_cfg->outputs[ch->output_idx].output; 690 if (output.capabilities != V4L2_OUT_CAP_STD) 691 return -ENODATA; 692 693 if (vb2_is_busy(&common->buffer_queue)) 694 return -EBUSY; 695 696 697 if (!(std_id & VPIF_V4L2_STD)) 698 return -EINVAL; 699 700 /* Call encoder subdevice function to set the standard */ 701 ch->video.stdid = std_id; 702 memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings)); 703 /* Get the information about the standard */ 704 if (vpif_update_resolution(ch)) 705 return -EINVAL; 706 707 common->fmt.fmt.pix.bytesperline = common->fmt.fmt.pix.width; 708 709 ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video, 710 s_std_output, std_id); 711 if (ret < 0) { 712 vpif_err("Failed to set output standard\n"); 713 return ret; 714 } 715 716 ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video, 717 s_std, std_id); 718 if (ret < 0) 719 vpif_err("Failed to set standard for sub devices\n"); 720 return ret; 721 } 722 723 static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std) 724 { 725 struct vpif_display_config *config = vpif_dev->platform_data; 726 struct video_device *vdev = video_devdata(file); 727 struct channel_obj *ch = video_get_drvdata(vdev); 728 struct vpif_display_chan_config *chan_cfg; 729 struct v4l2_output output; 730 731 if (!config->chan_config[ch->channel_id].outputs) 732 return -ENODATA; 733 734 chan_cfg = &config->chan_config[ch->channel_id]; 735 output = chan_cfg->outputs[ch->output_idx].output; 736 if (output.capabilities != V4L2_OUT_CAP_STD) 737 return -ENODATA; 738 739 *std = ch->video.stdid; 740 return 0; 741 } 742 743 static int vpif_enum_output(struct file *file, void *fh, 744 struct v4l2_output *output) 745 { 746 747 struct vpif_display_config *config = vpif_dev->platform_data; 748 struct video_device *vdev = video_devdata(file); 749 struct channel_obj *ch = video_get_drvdata(vdev); 750 struct vpif_display_chan_config *chan_cfg; 751 752 chan_cfg = &config->chan_config[ch->channel_id]; 753 if (output->index >= chan_cfg->output_count) { 754 vpif_dbg(1, debug, "Invalid output index\n"); 755 return -EINVAL; 756 } 757 758 *output = chan_cfg->outputs[output->index].output; 759 return 0; 760 } 761 762 /** 763 * vpif_output_to_subdev() - Maps output to sub device 764 * @vpif_cfg: global config ptr 765 * @chan_cfg: channel config ptr 766 * @index: Given output index from application 767 * 768 * lookup the sub device information for a given output index. 769 * we report all the output to application. output table also 770 * has sub device name for the each output 771 */ 772 static int 773 vpif_output_to_subdev(struct vpif_display_config *vpif_cfg, 774 struct vpif_display_chan_config *chan_cfg, int index) 775 { 776 struct vpif_subdev_info *subdev_info; 777 const char *subdev_name; 778 int i; 779 780 vpif_dbg(2, debug, "vpif_output_to_subdev\n"); 781 782 if (!chan_cfg->outputs) 783 return -1; 784 785 subdev_name = chan_cfg->outputs[index].subdev_name; 786 if (!subdev_name) 787 return -1; 788 789 /* loop through the sub device list to get the sub device info */ 790 for (i = 0; i < vpif_cfg->subdev_count; i++) { 791 subdev_info = &vpif_cfg->subdevinfo[i]; 792 if (!strcmp(subdev_info->name, subdev_name)) 793 return i; 794 } 795 return -1; 796 } 797 798 /** 799 * vpif_set_output() - Select an output 800 * @vpif_cfg: global config ptr 801 * @ch: channel 802 * @index: Given output index from application 803 * 804 * Select the given output. 805 */ 806 static int vpif_set_output(struct vpif_display_config *vpif_cfg, 807 struct channel_obj *ch, int index) 808 { 809 struct vpif_display_chan_config *chan_cfg = 810 &vpif_cfg->chan_config[ch->channel_id]; 811 struct v4l2_subdev *sd = NULL; 812 u32 input = 0, output = 0; 813 int sd_index; 814 int ret; 815 816 sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index); 817 if (sd_index >= 0) 818 sd = vpif_obj.sd[sd_index]; 819 820 if (sd) { 821 input = chan_cfg->outputs[index].input_route; 822 output = chan_cfg->outputs[index].output_route; 823 ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0); 824 if (ret < 0 && ret != -ENOIOCTLCMD) { 825 vpif_err("Failed to set output\n"); 826 return ret; 827 } 828 829 } 830 ch->output_idx = index; 831 ch->sd = sd; 832 if (chan_cfg->outputs) 833 /* update tvnorms from the sub device output info */ 834 ch->video_dev.tvnorms = chan_cfg->outputs[index].output.std; 835 return 0; 836 } 837 838 static int vpif_s_output(struct file *file, void *priv, unsigned int i) 839 { 840 struct vpif_display_config *config = vpif_dev->platform_data; 841 struct video_device *vdev = video_devdata(file); 842 struct channel_obj *ch = video_get_drvdata(vdev); 843 struct vpif_display_chan_config *chan_cfg; 844 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 845 846 if (vb2_is_busy(&common->buffer_queue)) 847 return -EBUSY; 848 849 chan_cfg = &config->chan_config[ch->channel_id]; 850 851 if (i >= chan_cfg->output_count) 852 return -EINVAL; 853 854 return vpif_set_output(config, ch, i); 855 } 856 857 static int vpif_g_output(struct file *file, void *priv, unsigned int *i) 858 { 859 struct video_device *vdev = video_devdata(file); 860 struct channel_obj *ch = video_get_drvdata(vdev); 861 862 *i = ch->output_idx; 863 864 return 0; 865 } 866 867 /** 868 * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler 869 * @file: file ptr 870 * @priv: file handle 871 * @timings: input timings 872 */ 873 static int 874 vpif_enum_dv_timings(struct file *file, void *priv, 875 struct v4l2_enum_dv_timings *timings) 876 { 877 struct vpif_display_config *config = vpif_dev->platform_data; 878 struct video_device *vdev = video_devdata(file); 879 struct channel_obj *ch = video_get_drvdata(vdev); 880 struct vpif_display_chan_config *chan_cfg; 881 struct v4l2_output output; 882 int ret; 883 884 if (!config->chan_config[ch->channel_id].outputs) 885 return -ENODATA; 886 887 chan_cfg = &config->chan_config[ch->channel_id]; 888 output = chan_cfg->outputs[ch->output_idx].output; 889 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 890 return -ENODATA; 891 892 timings->pad = 0; 893 894 ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings); 895 if (ret == -ENOIOCTLCMD || ret == -ENODEV) 896 return -EINVAL; 897 return ret; 898 } 899 900 /** 901 * vpif_s_dv_timings() - S_DV_TIMINGS handler 902 * @file: file ptr 903 * @priv: file handle 904 * @timings: digital video timings 905 */ 906 static int vpif_s_dv_timings(struct file *file, void *priv, 907 struct v4l2_dv_timings *timings) 908 { 909 struct vpif_display_config *config = vpif_dev->platform_data; 910 struct video_device *vdev = video_devdata(file); 911 struct channel_obj *ch = video_get_drvdata(vdev); 912 struct vpif_params *vpifparams = &ch->vpifparams; 913 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 914 struct vpif_channel_config_params *std_info = &vpifparams->std_info; 915 struct video_obj *vid_ch = &ch->video; 916 struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt; 917 struct vpif_display_chan_config *chan_cfg; 918 struct v4l2_output output; 919 int ret; 920 921 if (!config->chan_config[ch->channel_id].outputs) 922 return -ENODATA; 923 924 chan_cfg = &config->chan_config[ch->channel_id]; 925 output = chan_cfg->outputs[ch->output_idx].output; 926 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 927 return -ENODATA; 928 929 if (vb2_is_busy(&common->buffer_queue)) 930 return -EBUSY; 931 932 if (timings->type != V4L2_DV_BT_656_1120) { 933 vpif_dbg(2, debug, "Timing type not defined\n"); 934 return -EINVAL; 935 } 936 937 /* Configure subdevice timings, if any */ 938 ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings); 939 if (ret == -ENOIOCTLCMD || ret == -ENODEV) 940 ret = 0; 941 if (ret < 0) { 942 vpif_dbg(2, debug, "Error setting custom DV timings\n"); 943 return ret; 944 } 945 946 if (!(timings->bt.width && timings->bt.height && 947 (timings->bt.hbackporch || 948 timings->bt.hfrontporch || 949 timings->bt.hsync) && 950 timings->bt.vfrontporch && 951 (timings->bt.vbackporch || 952 timings->bt.vsync))) { 953 vpif_dbg(2, debug, "Timings for width, height, horizontal back porch, horizontal sync, horizontal front porch, vertical back porch, vertical sync and vertical back porch must be defined\n"); 954 return -EINVAL; 955 } 956 957 vid_ch->dv_timings = *timings; 958 959 /* Configure video port timings */ 960 961 std_info->eav2sav = V4L2_DV_BT_BLANKING_WIDTH(bt) - 8; 962 std_info->sav2eav = bt->width; 963 964 std_info->l1 = 1; 965 std_info->l3 = bt->vsync + bt->vbackporch + 1; 966 967 std_info->vsize = V4L2_DV_BT_FRAME_HEIGHT(bt); 968 if (bt->interlaced) { 969 if (bt->il_vbackporch || bt->il_vfrontporch || bt->il_vsync) { 970 std_info->l5 = std_info->vsize/2 - 971 (bt->vfrontporch - 1); 972 std_info->l7 = std_info->vsize/2 + 1; 973 std_info->l9 = std_info->l7 + bt->il_vsync + 974 bt->il_vbackporch + 1; 975 std_info->l11 = std_info->vsize - 976 (bt->il_vfrontporch - 1); 977 } else { 978 vpif_dbg(2, debug, "Required timing values for interlaced BT format missing\n"); 979 return -EINVAL; 980 } 981 } else { 982 std_info->l5 = std_info->vsize - (bt->vfrontporch - 1); 983 } 984 strscpy(std_info->name, "Custom timings BT656/1120", 985 sizeof(std_info->name)); 986 std_info->width = bt->width; 987 std_info->height = bt->height; 988 std_info->frm_fmt = bt->interlaced ? 0 : 1; 989 std_info->ycmux_mode = 0; 990 std_info->capture_format = 0; 991 std_info->vbi_supported = 0; 992 std_info->hd_sd = 1; 993 std_info->stdid = 0; 994 vid_ch->stdid = 0; 995 996 return 0; 997 } 998 999 /** 1000 * vpif_g_dv_timings() - G_DV_TIMINGS handler 1001 * @file: file ptr 1002 * @priv: file handle 1003 * @timings: digital video timings 1004 */ 1005 static int vpif_g_dv_timings(struct file *file, void *priv, 1006 struct v4l2_dv_timings *timings) 1007 { 1008 struct vpif_display_config *config = vpif_dev->platform_data; 1009 struct video_device *vdev = video_devdata(file); 1010 struct channel_obj *ch = video_get_drvdata(vdev); 1011 struct vpif_display_chan_config *chan_cfg; 1012 struct video_obj *vid_ch = &ch->video; 1013 struct v4l2_output output; 1014 1015 if (!config->chan_config[ch->channel_id].outputs) 1016 goto error; 1017 1018 chan_cfg = &config->chan_config[ch->channel_id]; 1019 output = chan_cfg->outputs[ch->output_idx].output; 1020 1021 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 1022 goto error; 1023 1024 *timings = vid_ch->dv_timings; 1025 1026 return 0; 1027 error: 1028 return -ENODATA; 1029 } 1030 1031 /* 1032 * vpif_log_status() - Status information 1033 * @file: file ptr 1034 * @priv: file handle 1035 * 1036 * Returns zero. 1037 */ 1038 static int vpif_log_status(struct file *filep, void *priv) 1039 { 1040 /* status for sub devices */ 1041 v4l2_device_call_all(&vpif_obj.v4l2_dev, 0, core, log_status); 1042 1043 return 0; 1044 } 1045 1046 /* vpif display ioctl operations */ 1047 static const struct v4l2_ioctl_ops vpif_ioctl_ops = { 1048 .vidioc_querycap = vpif_querycap, 1049 .vidioc_enum_fmt_vid_out = vpif_enum_fmt_vid_out, 1050 .vidioc_g_fmt_vid_out = vpif_g_fmt_vid_out, 1051 .vidioc_s_fmt_vid_out = vpif_s_fmt_vid_out, 1052 .vidioc_try_fmt_vid_out = vpif_try_fmt_vid_out, 1053 1054 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1055 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1056 .vidioc_querybuf = vb2_ioctl_querybuf, 1057 .vidioc_qbuf = vb2_ioctl_qbuf, 1058 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1059 .vidioc_expbuf = vb2_ioctl_expbuf, 1060 .vidioc_streamon = vb2_ioctl_streamon, 1061 .vidioc_streamoff = vb2_ioctl_streamoff, 1062 1063 .vidioc_s_std = vpif_s_std, 1064 .vidioc_g_std = vpif_g_std, 1065 1066 .vidioc_enum_output = vpif_enum_output, 1067 .vidioc_s_output = vpif_s_output, 1068 .vidioc_g_output = vpif_g_output, 1069 1070 .vidioc_enum_dv_timings = vpif_enum_dv_timings, 1071 .vidioc_s_dv_timings = vpif_s_dv_timings, 1072 .vidioc_g_dv_timings = vpif_g_dv_timings, 1073 1074 .vidioc_log_status = vpif_log_status, 1075 }; 1076 1077 static const struct v4l2_file_operations vpif_fops = { 1078 .owner = THIS_MODULE, 1079 .open = v4l2_fh_open, 1080 .release = vb2_fop_release, 1081 .unlocked_ioctl = video_ioctl2, 1082 .mmap = vb2_fop_mmap, 1083 .poll = vb2_fop_poll 1084 }; 1085 1086 /*Configure the channels, buffer sizei, request irq */ 1087 static int initialize_vpif(void) 1088 { 1089 int free_channel_objects_index; 1090 int err, i, j; 1091 1092 /* Allocate memory for six channel objects */ 1093 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1094 vpif_obj.dev[i] = 1095 kzalloc(sizeof(struct channel_obj), GFP_KERNEL); 1096 /* If memory allocation fails, return error */ 1097 if (!vpif_obj.dev[i]) { 1098 free_channel_objects_index = i; 1099 err = -ENOMEM; 1100 goto vpif_init_free_channel_objects; 1101 } 1102 } 1103 1104 return 0; 1105 1106 vpif_init_free_channel_objects: 1107 for (j = 0; j < free_channel_objects_index; j++) 1108 kfree(vpif_obj.dev[j]); 1109 return err; 1110 } 1111 1112 static void free_vpif_objs(void) 1113 { 1114 int i; 1115 1116 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) 1117 kfree(vpif_obj.dev[i]); 1118 } 1119 1120 static int vpif_probe_complete(void) 1121 { 1122 struct common_obj *common; 1123 struct video_device *vdev; 1124 struct channel_obj *ch; 1125 struct vb2_queue *q; 1126 int j, err, k; 1127 1128 for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) { 1129 ch = vpif_obj.dev[j]; 1130 /* Initialize field of the channel objects */ 1131 for (k = 0; k < VPIF_NUMOBJECTS; k++) { 1132 common = &ch->common[k]; 1133 spin_lock_init(&common->irqlock); 1134 mutex_init(&common->lock); 1135 common->set_addr = NULL; 1136 common->ytop_off = 0; 1137 common->ybtm_off = 0; 1138 common->ctop_off = 0; 1139 common->cbtm_off = 0; 1140 common->cur_frm = NULL; 1141 common->next_frm = NULL; 1142 memset(&common->fmt, 0, sizeof(common->fmt)); 1143 } 1144 ch->initialized = 0; 1145 if (vpif_obj.config->subdev_count) 1146 ch->sd = vpif_obj.sd[0]; 1147 ch->channel_id = j; 1148 1149 memset(&ch->vpifparams, 0, sizeof(ch->vpifparams)); 1150 1151 ch->common[VPIF_VIDEO_INDEX].fmt.type = 1152 V4L2_BUF_TYPE_VIDEO_OUTPUT; 1153 1154 /* select output 0 */ 1155 err = vpif_set_output(vpif_obj.config, ch, 0); 1156 if (err) 1157 goto probe_out; 1158 1159 /* set initial format */ 1160 ch->video.stdid = V4L2_STD_525_60; 1161 memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings)); 1162 vpif_update_resolution(ch); 1163 1164 /* Initialize vb2 queue */ 1165 q = &common->buffer_queue; 1166 q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; 1167 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1168 q->drv_priv = ch; 1169 q->ops = &video_qops; 1170 q->mem_ops = &vb2_dma_contig_memops; 1171 q->buf_struct_size = sizeof(struct vpif_disp_buffer); 1172 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1173 q->min_buffers_needed = 1; 1174 q->lock = &common->lock; 1175 q->dev = vpif_dev; 1176 err = vb2_queue_init(q); 1177 if (err) { 1178 vpif_err("vpif_display: vb2_queue_init() failed\n"); 1179 goto probe_out; 1180 } 1181 1182 INIT_LIST_HEAD(&common->dma_queue); 1183 1184 /* register video device */ 1185 vpif_dbg(1, debug, "channel=%p,channel->video_dev=%p\n", 1186 ch, &ch->video_dev); 1187 1188 /* Initialize the video_device structure */ 1189 vdev = &ch->video_dev; 1190 strscpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name)); 1191 vdev->release = video_device_release_empty; 1192 vdev->fops = &vpif_fops; 1193 vdev->ioctl_ops = &vpif_ioctl_ops; 1194 vdev->v4l2_dev = &vpif_obj.v4l2_dev; 1195 vdev->vfl_dir = VFL_DIR_TX; 1196 vdev->queue = q; 1197 vdev->lock = &common->lock; 1198 vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING; 1199 video_set_drvdata(&ch->video_dev, ch); 1200 err = video_register_device(vdev, VFL_TYPE_VIDEO, 1201 (j ? 3 : 2)); 1202 if (err < 0) 1203 goto probe_out; 1204 } 1205 1206 return 0; 1207 1208 probe_out: 1209 for (k = 0; k < j; k++) { 1210 ch = vpif_obj.dev[k]; 1211 video_unregister_device(&ch->video_dev); 1212 } 1213 return err; 1214 } 1215 1216 /* 1217 * vpif_probe: This function creates device entries by register itself to the 1218 * V4L2 driver and initializes fields of each channel objects 1219 */ 1220 static __init int vpif_probe(struct platform_device *pdev) 1221 { 1222 struct vpif_subdev_info *subdevdata; 1223 struct i2c_adapter *i2c_adap; 1224 int subdev_count; 1225 int res_idx = 0; 1226 int i, err; 1227 1228 if (!pdev->dev.platform_data) { 1229 dev_warn(&pdev->dev, "Missing platform data. Giving up.\n"); 1230 return -EINVAL; 1231 } 1232 1233 vpif_dev = &pdev->dev; 1234 err = initialize_vpif(); 1235 1236 if (err) { 1237 v4l2_err(vpif_dev->driver, "Error initializing vpif\n"); 1238 return err; 1239 } 1240 1241 err = v4l2_device_register(vpif_dev, &vpif_obj.v4l2_dev); 1242 if (err) { 1243 v4l2_err(vpif_dev->driver, "Error registering v4l2 device\n"); 1244 goto vpif_free; 1245 } 1246 1247 do { 1248 int irq; 1249 1250 err = platform_get_irq_optional(pdev, res_idx); 1251 if (err < 0 && err != -ENXIO) 1252 goto vpif_unregister; 1253 if (err > 0) 1254 irq = err; 1255 else 1256 break; 1257 1258 err = devm_request_irq(&pdev->dev, irq, vpif_channel_isr, 1259 IRQF_SHARED, VPIF_DRIVER_NAME, 1260 (void *)(&vpif_obj.dev[res_idx]->channel_id)); 1261 if (err) { 1262 vpif_err("VPIF IRQ request failed\n"); 1263 goto vpif_unregister; 1264 } 1265 } while (++res_idx); 1266 1267 vpif_obj.config = pdev->dev.platform_data; 1268 subdev_count = vpif_obj.config->subdev_count; 1269 subdevdata = vpif_obj.config->subdevinfo; 1270 vpif_obj.sd = kcalloc(subdev_count, sizeof(*vpif_obj.sd), GFP_KERNEL); 1271 if (!vpif_obj.sd) { 1272 err = -ENOMEM; 1273 goto vpif_unregister; 1274 } 1275 1276 i2c_adap = i2c_get_adapter(vpif_obj.config->i2c_adapter_id); 1277 for (i = 0; i < subdev_count; i++) { 1278 vpif_obj.sd[i] = 1279 v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev, 1280 i2c_adap, 1281 &subdevdata[i].board_info, 1282 NULL); 1283 if (!vpif_obj.sd[i]) { 1284 vpif_err("Error registering v4l2 subdevice\n"); 1285 err = -ENODEV; 1286 goto probe_subdev_out; 1287 } 1288 1289 if (vpif_obj.sd[i]) 1290 vpif_obj.sd[i]->grp_id = 1 << i; 1291 } 1292 err = vpif_probe_complete(); 1293 if (err) 1294 goto probe_subdev_out; 1295 1296 return 0; 1297 1298 probe_subdev_out: 1299 kfree(vpif_obj.sd); 1300 vpif_unregister: 1301 v4l2_device_unregister(&vpif_obj.v4l2_dev); 1302 vpif_free: 1303 free_vpif_objs(); 1304 1305 return err; 1306 } 1307 1308 /* 1309 * vpif_remove: It un-register channels from V4L2 driver 1310 */ 1311 static int vpif_remove(struct platform_device *device) 1312 { 1313 struct channel_obj *ch; 1314 int i; 1315 1316 v4l2_device_unregister(&vpif_obj.v4l2_dev); 1317 1318 kfree(vpif_obj.sd); 1319 /* un-register device */ 1320 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1321 /* Get the pointer to the channel object */ 1322 ch = vpif_obj.dev[i]; 1323 /* Unregister video device */ 1324 video_unregister_device(&ch->video_dev); 1325 } 1326 free_vpif_objs(); 1327 1328 return 0; 1329 } 1330 1331 #ifdef CONFIG_PM_SLEEP 1332 static int vpif_suspend(struct device *dev) 1333 { 1334 struct common_obj *common; 1335 struct channel_obj *ch; 1336 int i; 1337 1338 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1339 /* Get the pointer to the channel object */ 1340 ch = vpif_obj.dev[i]; 1341 common = &ch->common[VPIF_VIDEO_INDEX]; 1342 1343 if (!vb2_start_streaming_called(&common->buffer_queue)) 1344 continue; 1345 1346 mutex_lock(&common->lock); 1347 /* Disable channel */ 1348 if (ch->channel_id == VPIF_CHANNEL2_VIDEO) { 1349 enable_channel2(0); 1350 channel2_intr_enable(0); 1351 } 1352 if (ch->channel_id == VPIF_CHANNEL3_VIDEO || 1353 ycmux_mode == 2) { 1354 enable_channel3(0); 1355 channel3_intr_enable(0); 1356 } 1357 mutex_unlock(&common->lock); 1358 } 1359 1360 return 0; 1361 } 1362 1363 static int vpif_resume(struct device *dev) 1364 { 1365 1366 struct common_obj *common; 1367 struct channel_obj *ch; 1368 int i; 1369 1370 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1371 /* Get the pointer to the channel object */ 1372 ch = vpif_obj.dev[i]; 1373 common = &ch->common[VPIF_VIDEO_INDEX]; 1374 1375 if (!vb2_start_streaming_called(&common->buffer_queue)) 1376 continue; 1377 1378 mutex_lock(&common->lock); 1379 /* Enable channel */ 1380 if (ch->channel_id == VPIF_CHANNEL2_VIDEO) { 1381 enable_channel2(1); 1382 channel2_intr_enable(1); 1383 } 1384 if (ch->channel_id == VPIF_CHANNEL3_VIDEO || 1385 ycmux_mode == 2) { 1386 enable_channel3(1); 1387 channel3_intr_enable(1); 1388 } 1389 mutex_unlock(&common->lock); 1390 } 1391 1392 return 0; 1393 } 1394 1395 #endif 1396 1397 static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume); 1398 1399 static __refdata struct platform_driver vpif_driver = { 1400 .driver = { 1401 .name = VPIF_DRIVER_NAME, 1402 .pm = &vpif_pm_ops, 1403 }, 1404 .probe = vpif_probe, 1405 .remove = vpif_remove, 1406 }; 1407 1408 module_platform_driver(vpif_driver); 1409