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_video_buf_addr; 567 } else { 568 if (2 == muxmode) 569 common->set_addr = ch2_set_video_buf_addr_yc_nmux; 570 else 571 common->set_addr = ch2_set_video_buf_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 strscpy(cap->card, config->card_name, sizeof(cap->card)); 589 590 return 0; 591 } 592 593 static int vpif_enum_fmt_vid_out(struct file *file, void *priv, 594 struct v4l2_fmtdesc *fmt) 595 { 596 if (fmt->index != 0) 597 return -EINVAL; 598 599 /* Fill in the information about format */ 600 fmt->pixelformat = V4L2_PIX_FMT_YUV422P; 601 return 0; 602 } 603 604 static int vpif_g_fmt_vid_out(struct file *file, void *priv, 605 struct v4l2_format *fmt) 606 { 607 struct video_device *vdev = video_devdata(file); 608 struct channel_obj *ch = video_get_drvdata(vdev); 609 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 610 611 /* Check the validity of the buffer type */ 612 if (common->fmt.type != fmt->type) 613 return -EINVAL; 614 615 if (vpif_update_resolution(ch)) 616 return -EINVAL; 617 *fmt = common->fmt; 618 return 0; 619 } 620 621 static int vpif_try_fmt_vid_out(struct file *file, void *priv, 622 struct v4l2_format *fmt) 623 { 624 struct video_device *vdev = video_devdata(file); 625 struct channel_obj *ch = video_get_drvdata(vdev); 626 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 627 struct v4l2_pix_format *pixfmt = &fmt->fmt.pix; 628 629 /* 630 * to suppress v4l-compliance warnings silently correct 631 * the pixelformat 632 */ 633 if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P) 634 pixfmt->pixelformat = common->fmt.fmt.pix.pixelformat; 635 636 if (vpif_update_resolution(ch)) 637 return -EINVAL; 638 639 pixfmt->colorspace = common->fmt.fmt.pix.colorspace; 640 pixfmt->field = common->fmt.fmt.pix.field; 641 pixfmt->bytesperline = common->fmt.fmt.pix.width; 642 pixfmt->width = common->fmt.fmt.pix.width; 643 pixfmt->height = common->fmt.fmt.pix.height; 644 pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2; 645 646 return 0; 647 } 648 649 static int vpif_s_fmt_vid_out(struct file *file, void *priv, 650 struct v4l2_format *fmt) 651 { 652 struct video_device *vdev = video_devdata(file); 653 struct channel_obj *ch = video_get_drvdata(vdev); 654 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 655 struct v4l2_pix_format *pixfmt = &fmt->fmt.pix; 656 int ret; 657 658 if (vb2_is_busy(&common->buffer_queue)) 659 return -EBUSY; 660 661 ret = vpif_try_fmt_vid_out(file, priv, fmt); 662 if (ret) 663 return ret; 664 665 /* store the pix format in the channel object */ 666 common->fmt.fmt.pix = *pixfmt; 667 668 /* store the format in the channel object */ 669 common->fmt = *fmt; 670 return 0; 671 } 672 673 static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id) 674 { 675 struct vpif_display_config *config = vpif_dev->platform_data; 676 struct video_device *vdev = video_devdata(file); 677 struct channel_obj *ch = video_get_drvdata(vdev); 678 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 679 struct vpif_display_chan_config *chan_cfg; 680 struct v4l2_output output; 681 int ret; 682 683 if (!config->chan_config[ch->channel_id].outputs) 684 return -ENODATA; 685 686 chan_cfg = &config->chan_config[ch->channel_id]; 687 output = chan_cfg->outputs[ch->output_idx].output; 688 if (output.capabilities != V4L2_OUT_CAP_STD) 689 return -ENODATA; 690 691 if (vb2_is_busy(&common->buffer_queue)) 692 return -EBUSY; 693 694 695 if (!(std_id & VPIF_V4L2_STD)) 696 return -EINVAL; 697 698 /* Call encoder subdevice function to set the standard */ 699 ch->video.stdid = std_id; 700 memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings)); 701 /* Get the information about the standard */ 702 if (vpif_update_resolution(ch)) 703 return -EINVAL; 704 705 common->fmt.fmt.pix.bytesperline = common->fmt.fmt.pix.width; 706 707 ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video, 708 s_std_output, std_id); 709 if (ret < 0) { 710 vpif_err("Failed to set output standard\n"); 711 return ret; 712 } 713 714 ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video, 715 s_std, std_id); 716 if (ret < 0) 717 vpif_err("Failed to set standard for sub devices\n"); 718 return ret; 719 } 720 721 static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std) 722 { 723 struct vpif_display_config *config = vpif_dev->platform_data; 724 struct video_device *vdev = video_devdata(file); 725 struct channel_obj *ch = video_get_drvdata(vdev); 726 struct vpif_display_chan_config *chan_cfg; 727 struct v4l2_output output; 728 729 if (!config->chan_config[ch->channel_id].outputs) 730 return -ENODATA; 731 732 chan_cfg = &config->chan_config[ch->channel_id]; 733 output = chan_cfg->outputs[ch->output_idx].output; 734 if (output.capabilities != V4L2_OUT_CAP_STD) 735 return -ENODATA; 736 737 *std = ch->video.stdid; 738 return 0; 739 } 740 741 static int vpif_enum_output(struct file *file, void *fh, 742 struct v4l2_output *output) 743 { 744 745 struct vpif_display_config *config = vpif_dev->platform_data; 746 struct video_device *vdev = video_devdata(file); 747 struct channel_obj *ch = video_get_drvdata(vdev); 748 struct vpif_display_chan_config *chan_cfg; 749 750 chan_cfg = &config->chan_config[ch->channel_id]; 751 if (output->index >= chan_cfg->output_count) { 752 vpif_dbg(1, debug, "Invalid output index\n"); 753 return -EINVAL; 754 } 755 756 *output = chan_cfg->outputs[output->index].output; 757 return 0; 758 } 759 760 /** 761 * vpif_output_to_subdev() - Maps output to sub device 762 * @vpif_cfg: global config ptr 763 * @chan_cfg: channel config ptr 764 * @index: Given output index from application 765 * 766 * lookup the sub device information for a given output index. 767 * we report all the output to application. output table also 768 * has sub device name for the each output 769 */ 770 static int 771 vpif_output_to_subdev(struct vpif_display_config *vpif_cfg, 772 struct vpif_display_chan_config *chan_cfg, int index) 773 { 774 struct vpif_subdev_info *subdev_info; 775 const char *subdev_name; 776 int i; 777 778 vpif_dbg(2, debug, "vpif_output_to_subdev\n"); 779 780 if (!chan_cfg->outputs) 781 return -1; 782 783 subdev_name = chan_cfg->outputs[index].subdev_name; 784 if (!subdev_name) 785 return -1; 786 787 /* loop through the sub device list to get the sub device info */ 788 for (i = 0; i < vpif_cfg->subdev_count; i++) { 789 subdev_info = &vpif_cfg->subdevinfo[i]; 790 if (!strcmp(subdev_info->name, subdev_name)) 791 return i; 792 } 793 return -1; 794 } 795 796 /** 797 * vpif_set_output() - Select an output 798 * @vpif_cfg: global config ptr 799 * @ch: channel 800 * @index: Given output index from application 801 * 802 * Select the given output. 803 */ 804 static int vpif_set_output(struct vpif_display_config *vpif_cfg, 805 struct channel_obj *ch, int index) 806 { 807 struct vpif_display_chan_config *chan_cfg = 808 &vpif_cfg->chan_config[ch->channel_id]; 809 struct v4l2_subdev *sd = NULL; 810 u32 input = 0, output = 0; 811 int sd_index; 812 int ret; 813 814 sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index); 815 if (sd_index >= 0) 816 sd = vpif_obj.sd[sd_index]; 817 818 if (sd) { 819 input = chan_cfg->outputs[index].input_route; 820 output = chan_cfg->outputs[index].output_route; 821 ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0); 822 if (ret < 0 && ret != -ENOIOCTLCMD) { 823 vpif_err("Failed to set output\n"); 824 return ret; 825 } 826 827 } 828 ch->output_idx = index; 829 ch->sd = sd; 830 if (chan_cfg->outputs) 831 /* update tvnorms from the sub device output info */ 832 ch->video_dev.tvnorms = chan_cfg->outputs[index].output.std; 833 return 0; 834 } 835 836 static int vpif_s_output(struct file *file, void *priv, unsigned int i) 837 { 838 struct vpif_display_config *config = vpif_dev->platform_data; 839 struct video_device *vdev = video_devdata(file); 840 struct channel_obj *ch = video_get_drvdata(vdev); 841 struct vpif_display_chan_config *chan_cfg; 842 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 843 844 if (vb2_is_busy(&common->buffer_queue)) 845 return -EBUSY; 846 847 chan_cfg = &config->chan_config[ch->channel_id]; 848 849 if (i >= chan_cfg->output_count) 850 return -EINVAL; 851 852 return vpif_set_output(config, ch, i); 853 } 854 855 static int vpif_g_output(struct file *file, void *priv, unsigned int *i) 856 { 857 struct video_device *vdev = video_devdata(file); 858 struct channel_obj *ch = video_get_drvdata(vdev); 859 860 *i = ch->output_idx; 861 862 return 0; 863 } 864 865 /** 866 * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler 867 * @file: file ptr 868 * @priv: file handle 869 * @timings: input timings 870 */ 871 static int 872 vpif_enum_dv_timings(struct file *file, void *priv, 873 struct v4l2_enum_dv_timings *timings) 874 { 875 struct vpif_display_config *config = vpif_dev->platform_data; 876 struct video_device *vdev = video_devdata(file); 877 struct channel_obj *ch = video_get_drvdata(vdev); 878 struct vpif_display_chan_config *chan_cfg; 879 struct v4l2_output output; 880 int ret; 881 882 if (!config->chan_config[ch->channel_id].outputs) 883 return -ENODATA; 884 885 chan_cfg = &config->chan_config[ch->channel_id]; 886 output = chan_cfg->outputs[ch->output_idx].output; 887 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 888 return -ENODATA; 889 890 timings->pad = 0; 891 892 ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings); 893 if (ret == -ENOIOCTLCMD || ret == -ENODEV) 894 return -EINVAL; 895 return ret; 896 } 897 898 /** 899 * vpif_s_dv_timings() - S_DV_TIMINGS handler 900 * @file: file ptr 901 * @priv: file handle 902 * @timings: digital video timings 903 */ 904 static int vpif_s_dv_timings(struct file *file, void *priv, 905 struct v4l2_dv_timings *timings) 906 { 907 struct vpif_display_config *config = vpif_dev->platform_data; 908 struct video_device *vdev = video_devdata(file); 909 struct channel_obj *ch = video_get_drvdata(vdev); 910 struct vpif_params *vpifparams = &ch->vpifparams; 911 struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX]; 912 struct vpif_channel_config_params *std_info = &vpifparams->std_info; 913 struct video_obj *vid_ch = &ch->video; 914 struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt; 915 struct vpif_display_chan_config *chan_cfg; 916 struct v4l2_output output; 917 int ret; 918 919 if (!config->chan_config[ch->channel_id].outputs) 920 return -ENODATA; 921 922 chan_cfg = &config->chan_config[ch->channel_id]; 923 output = chan_cfg->outputs[ch->output_idx].output; 924 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 925 return -ENODATA; 926 927 if (vb2_is_busy(&common->buffer_queue)) 928 return -EBUSY; 929 930 if (timings->type != V4L2_DV_BT_656_1120) { 931 vpif_dbg(2, debug, "Timing type not defined\n"); 932 return -EINVAL; 933 } 934 935 /* Configure subdevice timings, if any */ 936 ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings); 937 if (ret == -ENOIOCTLCMD || ret == -ENODEV) 938 ret = 0; 939 if (ret < 0) { 940 vpif_dbg(2, debug, "Error setting custom DV timings\n"); 941 return ret; 942 } 943 944 if (!(timings->bt.width && timings->bt.height && 945 (timings->bt.hbackporch || 946 timings->bt.hfrontporch || 947 timings->bt.hsync) && 948 timings->bt.vfrontporch && 949 (timings->bt.vbackporch || 950 timings->bt.vsync))) { 951 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"); 952 return -EINVAL; 953 } 954 955 vid_ch->dv_timings = *timings; 956 957 /* Configure video port timings */ 958 959 std_info->eav2sav = V4L2_DV_BT_BLANKING_WIDTH(bt) - 8; 960 std_info->sav2eav = bt->width; 961 962 std_info->l1 = 1; 963 std_info->l3 = bt->vsync + bt->vbackporch + 1; 964 965 std_info->vsize = V4L2_DV_BT_FRAME_HEIGHT(bt); 966 if (bt->interlaced) { 967 if (bt->il_vbackporch || bt->il_vfrontporch || bt->il_vsync) { 968 std_info->l5 = std_info->vsize/2 - 969 (bt->vfrontporch - 1); 970 std_info->l7 = std_info->vsize/2 + 1; 971 std_info->l9 = std_info->l7 + bt->il_vsync + 972 bt->il_vbackporch + 1; 973 std_info->l11 = std_info->vsize - 974 (bt->il_vfrontporch - 1); 975 } else { 976 vpif_dbg(2, debug, "Required timing values for interlaced BT format missing\n"); 977 return -EINVAL; 978 } 979 } else { 980 std_info->l5 = std_info->vsize - (bt->vfrontporch - 1); 981 } 982 strscpy(std_info->name, "Custom timings BT656/1120", 983 sizeof(std_info->name)); 984 std_info->width = bt->width; 985 std_info->height = bt->height; 986 std_info->frm_fmt = bt->interlaced ? 0 : 1; 987 std_info->ycmux_mode = 0; 988 std_info->capture_format = 0; 989 std_info->vbi_supported = 0; 990 std_info->hd_sd = 1; 991 std_info->stdid = 0; 992 vid_ch->stdid = 0; 993 994 return 0; 995 } 996 997 /** 998 * vpif_g_dv_timings() - G_DV_TIMINGS handler 999 * @file: file ptr 1000 * @priv: file handle 1001 * @timings: digital video timings 1002 */ 1003 static int vpif_g_dv_timings(struct file *file, void *priv, 1004 struct v4l2_dv_timings *timings) 1005 { 1006 struct vpif_display_config *config = vpif_dev->platform_data; 1007 struct video_device *vdev = video_devdata(file); 1008 struct channel_obj *ch = video_get_drvdata(vdev); 1009 struct vpif_display_chan_config *chan_cfg; 1010 struct video_obj *vid_ch = &ch->video; 1011 struct v4l2_output output; 1012 1013 if (!config->chan_config[ch->channel_id].outputs) 1014 goto error; 1015 1016 chan_cfg = &config->chan_config[ch->channel_id]; 1017 output = chan_cfg->outputs[ch->output_idx].output; 1018 1019 if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS) 1020 goto error; 1021 1022 *timings = vid_ch->dv_timings; 1023 1024 return 0; 1025 error: 1026 return -ENODATA; 1027 } 1028 1029 /* 1030 * vpif_log_status() - Status information 1031 * @file: file ptr 1032 * @priv: file handle 1033 * 1034 * Returns zero. 1035 */ 1036 static int vpif_log_status(struct file *filep, void *priv) 1037 { 1038 /* status for sub devices */ 1039 v4l2_device_call_all(&vpif_obj.v4l2_dev, 0, core, log_status); 1040 1041 return 0; 1042 } 1043 1044 /* vpif display ioctl operations */ 1045 static const struct v4l2_ioctl_ops vpif_ioctl_ops = { 1046 .vidioc_querycap = vpif_querycap, 1047 .vidioc_enum_fmt_vid_out = vpif_enum_fmt_vid_out, 1048 .vidioc_g_fmt_vid_out = vpif_g_fmt_vid_out, 1049 .vidioc_s_fmt_vid_out = vpif_s_fmt_vid_out, 1050 .vidioc_try_fmt_vid_out = vpif_try_fmt_vid_out, 1051 1052 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1053 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1054 .vidioc_querybuf = vb2_ioctl_querybuf, 1055 .vidioc_qbuf = vb2_ioctl_qbuf, 1056 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1057 .vidioc_expbuf = vb2_ioctl_expbuf, 1058 .vidioc_streamon = vb2_ioctl_streamon, 1059 .vidioc_streamoff = vb2_ioctl_streamoff, 1060 1061 .vidioc_s_std = vpif_s_std, 1062 .vidioc_g_std = vpif_g_std, 1063 1064 .vidioc_enum_output = vpif_enum_output, 1065 .vidioc_s_output = vpif_s_output, 1066 .vidioc_g_output = vpif_g_output, 1067 1068 .vidioc_enum_dv_timings = vpif_enum_dv_timings, 1069 .vidioc_s_dv_timings = vpif_s_dv_timings, 1070 .vidioc_g_dv_timings = vpif_g_dv_timings, 1071 1072 .vidioc_log_status = vpif_log_status, 1073 }; 1074 1075 static const struct v4l2_file_operations vpif_fops = { 1076 .owner = THIS_MODULE, 1077 .open = v4l2_fh_open, 1078 .release = vb2_fop_release, 1079 .unlocked_ioctl = video_ioctl2, 1080 .mmap = vb2_fop_mmap, 1081 .poll = vb2_fop_poll 1082 }; 1083 1084 /*Configure the channels, buffer sizei, request irq */ 1085 static int initialize_vpif(void) 1086 { 1087 int free_channel_objects_index; 1088 int err, i, j; 1089 1090 /* Allocate memory for six channel objects */ 1091 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1092 vpif_obj.dev[i] = 1093 kzalloc(sizeof(struct channel_obj), GFP_KERNEL); 1094 /* If memory allocation fails, return error */ 1095 if (!vpif_obj.dev[i]) { 1096 free_channel_objects_index = i; 1097 err = -ENOMEM; 1098 goto vpif_init_free_channel_objects; 1099 } 1100 } 1101 1102 return 0; 1103 1104 vpif_init_free_channel_objects: 1105 for (j = 0; j < free_channel_objects_index; j++) 1106 kfree(vpif_obj.dev[j]); 1107 return err; 1108 } 1109 1110 static void free_vpif_objs(void) 1111 { 1112 int i; 1113 1114 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) 1115 kfree(vpif_obj.dev[i]); 1116 } 1117 1118 static int vpif_probe_complete(void) 1119 { 1120 struct common_obj *common; 1121 struct video_device *vdev; 1122 struct channel_obj *ch; 1123 struct vb2_queue *q; 1124 int j, err, k; 1125 1126 for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) { 1127 ch = vpif_obj.dev[j]; 1128 /* Initialize field of the channel objects */ 1129 for (k = 0; k < VPIF_NUMOBJECTS; k++) { 1130 common = &ch->common[k]; 1131 spin_lock_init(&common->irqlock); 1132 mutex_init(&common->lock); 1133 common->set_addr = NULL; 1134 common->ytop_off = 0; 1135 common->ybtm_off = 0; 1136 common->ctop_off = 0; 1137 common->cbtm_off = 0; 1138 common->cur_frm = NULL; 1139 common->next_frm = NULL; 1140 memset(&common->fmt, 0, sizeof(common->fmt)); 1141 } 1142 ch->initialized = 0; 1143 if (vpif_obj.config->subdev_count) 1144 ch->sd = vpif_obj.sd[0]; 1145 ch->channel_id = j; 1146 1147 memset(&ch->vpifparams, 0, sizeof(ch->vpifparams)); 1148 1149 ch->common[VPIF_VIDEO_INDEX].fmt.type = 1150 V4L2_BUF_TYPE_VIDEO_OUTPUT; 1151 1152 /* select output 0 */ 1153 err = vpif_set_output(vpif_obj.config, ch, 0); 1154 if (err) 1155 goto probe_out; 1156 1157 /* set initial format */ 1158 ch->video.stdid = V4L2_STD_525_60; 1159 memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings)); 1160 vpif_update_resolution(ch); 1161 1162 /* Initialize vb2 queue */ 1163 q = &common->buffer_queue; 1164 q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; 1165 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1166 q->drv_priv = ch; 1167 q->ops = &video_qops; 1168 q->mem_ops = &vb2_dma_contig_memops; 1169 q->buf_struct_size = sizeof(struct vpif_disp_buffer); 1170 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1171 q->min_buffers_needed = 1; 1172 q->lock = &common->lock; 1173 q->dev = vpif_dev; 1174 err = vb2_queue_init(q); 1175 if (err) { 1176 vpif_err("vpif_display: vb2_queue_init() failed\n"); 1177 goto probe_out; 1178 } 1179 1180 INIT_LIST_HEAD(&common->dma_queue); 1181 1182 /* register video device */ 1183 vpif_dbg(1, debug, "channel=%p,channel->video_dev=%p\n", 1184 ch, &ch->video_dev); 1185 1186 /* Initialize the video_device structure */ 1187 vdev = &ch->video_dev; 1188 strscpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name)); 1189 vdev->release = video_device_release_empty; 1190 vdev->fops = &vpif_fops; 1191 vdev->ioctl_ops = &vpif_ioctl_ops; 1192 vdev->v4l2_dev = &vpif_obj.v4l2_dev; 1193 vdev->vfl_dir = VFL_DIR_TX; 1194 vdev->queue = q; 1195 vdev->lock = &common->lock; 1196 vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING; 1197 video_set_drvdata(&ch->video_dev, ch); 1198 err = video_register_device(vdev, VFL_TYPE_VIDEO, 1199 (j ? 3 : 2)); 1200 if (err < 0) 1201 goto probe_out; 1202 } 1203 1204 return 0; 1205 1206 probe_out: 1207 for (k = 0; k < j; k++) { 1208 ch = vpif_obj.dev[k]; 1209 video_unregister_device(&ch->video_dev); 1210 } 1211 return err; 1212 } 1213 1214 /* 1215 * vpif_probe: This function creates device entries by register itself to the 1216 * V4L2 driver and initializes fields of each channel objects 1217 */ 1218 static __init int vpif_probe(struct platform_device *pdev) 1219 { 1220 struct vpif_subdev_info *subdevdata; 1221 struct i2c_adapter *i2c_adap; 1222 int subdev_count; 1223 int res_idx = 0; 1224 int i, err; 1225 1226 if (!pdev->dev.platform_data) { 1227 dev_warn(&pdev->dev, "Missing platform data. Giving up.\n"); 1228 return -EINVAL; 1229 } 1230 1231 vpif_dev = &pdev->dev; 1232 err = initialize_vpif(); 1233 1234 if (err) { 1235 v4l2_err(vpif_dev->driver, "Error initializing vpif\n"); 1236 return err; 1237 } 1238 1239 err = v4l2_device_register(vpif_dev, &vpif_obj.v4l2_dev); 1240 if (err) { 1241 v4l2_err(vpif_dev->driver, "Error registering v4l2 device\n"); 1242 goto vpif_free; 1243 } 1244 1245 do { 1246 int irq; 1247 1248 err = platform_get_irq_optional(pdev, res_idx); 1249 if (err < 0 && err != -ENXIO) 1250 goto vpif_unregister; 1251 if (err > 0) 1252 irq = err; 1253 else 1254 break; 1255 1256 err = devm_request_irq(&pdev->dev, irq, vpif_channel_isr, 1257 IRQF_SHARED, VPIF_DRIVER_NAME, 1258 (void *)(&vpif_obj.dev[res_idx]->channel_id)); 1259 if (err) { 1260 vpif_err("VPIF IRQ request failed\n"); 1261 goto vpif_unregister; 1262 } 1263 } while (++res_idx); 1264 1265 vpif_obj.config = pdev->dev.platform_data; 1266 subdev_count = vpif_obj.config->subdev_count; 1267 subdevdata = vpif_obj.config->subdevinfo; 1268 vpif_obj.sd = kcalloc(subdev_count, sizeof(*vpif_obj.sd), GFP_KERNEL); 1269 if (!vpif_obj.sd) { 1270 err = -ENOMEM; 1271 goto vpif_unregister; 1272 } 1273 1274 i2c_adap = i2c_get_adapter(vpif_obj.config->i2c_adapter_id); 1275 for (i = 0; i < subdev_count; i++) { 1276 vpif_obj.sd[i] = 1277 v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev, 1278 i2c_adap, 1279 &subdevdata[i].board_info, 1280 NULL); 1281 if (!vpif_obj.sd[i]) { 1282 vpif_err("Error registering v4l2 subdevice\n"); 1283 err = -ENODEV; 1284 goto probe_subdev_out; 1285 } 1286 1287 vpif_obj.sd[i]->grp_id = 1 << i; 1288 } 1289 err = vpif_probe_complete(); 1290 if (err) 1291 goto probe_subdev_out; 1292 1293 return 0; 1294 1295 probe_subdev_out: 1296 kfree(vpif_obj.sd); 1297 vpif_unregister: 1298 v4l2_device_unregister(&vpif_obj.v4l2_dev); 1299 vpif_free: 1300 free_vpif_objs(); 1301 1302 return err; 1303 } 1304 1305 /* 1306 * vpif_remove: It un-register channels from V4L2 driver 1307 */ 1308 static void vpif_remove(struct platform_device *device) 1309 { 1310 struct channel_obj *ch; 1311 int i; 1312 1313 v4l2_device_unregister(&vpif_obj.v4l2_dev); 1314 1315 kfree(vpif_obj.sd); 1316 /* un-register device */ 1317 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1318 /* Get the pointer to the channel object */ 1319 ch = vpif_obj.dev[i]; 1320 /* Unregister video device */ 1321 video_unregister_device(&ch->video_dev); 1322 } 1323 free_vpif_objs(); 1324 } 1325 1326 #ifdef CONFIG_PM_SLEEP 1327 static int vpif_suspend(struct device *dev) 1328 { 1329 struct common_obj *common; 1330 struct channel_obj *ch; 1331 int i; 1332 1333 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1334 /* Get the pointer to the channel object */ 1335 ch = vpif_obj.dev[i]; 1336 common = &ch->common[VPIF_VIDEO_INDEX]; 1337 1338 if (!vb2_start_streaming_called(&common->buffer_queue)) 1339 continue; 1340 1341 mutex_lock(&common->lock); 1342 /* Disable channel */ 1343 if (ch->channel_id == VPIF_CHANNEL2_VIDEO) { 1344 enable_channel2(0); 1345 channel2_intr_enable(0); 1346 } 1347 if (ch->channel_id == VPIF_CHANNEL3_VIDEO || 1348 ycmux_mode == 2) { 1349 enable_channel3(0); 1350 channel3_intr_enable(0); 1351 } 1352 mutex_unlock(&common->lock); 1353 } 1354 1355 return 0; 1356 } 1357 1358 static int vpif_resume(struct device *dev) 1359 { 1360 1361 struct common_obj *common; 1362 struct channel_obj *ch; 1363 int i; 1364 1365 for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) { 1366 /* Get the pointer to the channel object */ 1367 ch = vpif_obj.dev[i]; 1368 common = &ch->common[VPIF_VIDEO_INDEX]; 1369 1370 if (!vb2_start_streaming_called(&common->buffer_queue)) 1371 continue; 1372 1373 mutex_lock(&common->lock); 1374 /* Enable channel */ 1375 if (ch->channel_id == VPIF_CHANNEL2_VIDEO) { 1376 enable_channel2(1); 1377 channel2_intr_enable(1); 1378 } 1379 if (ch->channel_id == VPIF_CHANNEL3_VIDEO || 1380 ycmux_mode == 2) { 1381 enable_channel3(1); 1382 channel3_intr_enable(1); 1383 } 1384 mutex_unlock(&common->lock); 1385 } 1386 1387 return 0; 1388 } 1389 1390 #endif 1391 1392 static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume); 1393 1394 static __refdata struct platform_driver vpif_driver = { 1395 .driver = { 1396 .name = VPIF_DRIVER_NAME, 1397 .pm = &vpif_pm_ops, 1398 }, 1399 .probe = vpif_probe, 1400 .remove_new = vpif_remove, 1401 }; 1402 1403 module_platform_driver(vpif_driver); 1404