drivers/media/platform/msm/ais/camera/camera.c

   1 /* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
   2  *
   3  * This program is free software; you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License version 2 and
   5  * only version 2 as published by the Free Software Foundation.
   6  *
   7  * This program is distributed in the hope that it will be useful,
   8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10  * GNU General Public License for more details.
  11  */
  12
  13
  14 #include <linux/of.h>
  15 #include <linux/module.h>
  16 #include <linux/workqueue.h>
  17 #include <linux/delay.h>
  18 #include <linux/types.h>
  19 #include <linux/list.h>
  20 #include <linux/ioctl.h>
  21 #include <linux/spinlock.h>
  22 #include <linux/proc_fs.h>
  23 #include <linux/atomic.h>
  24 #include <linux/wait.h>
  25 #include <linux/videodev2.h>
  26 #include <linux/msm_ion.h>
  27 #include <linux/iommu.h>
  28 #include <linux/platform_device.h>
  29 #include <media/v4l2-fh.h>
  30 #include <media/videobuf2-v4l2.h>
  31
  32 #include "camera.h"
  33 #include "msm.h"
  34 #include "msm_vb2.h"
  35
  36 #define fh_to_private(__fh) \
  37         container_of(__fh, struct camera_v4l2_private, fh)
  38
  39 struct camera_v4l2_private {
  40         struct v4l2_fh fh;
  41         unsigned int stream_id;
  42         unsigned int is_vb2_valid; /*0 if no vb2 buffers on stream, else 1*/
  43         struct vb2_queue vb2_q;
  44         bool stream_created;
  45         struct mutex lock;
  46 };
  47
  48 static void camera_pack_event(struct file *filep, int evt_id,
  49         int command, int value, struct v4l2_event *event)
  50 {
  51         struct msm_v4l2_event_data *event_data =
  52                 (struct msm_v4l2_event_data *)&event->u.data[0];
  53         struct msm_video_device *pvdev = video_drvdata(filep);
  54         struct camera_v4l2_private *sp = fh_to_private(filep->private_data);
  55
  56         /* always MSM_CAMERA_V4L2_EVENT_TYPE */
  57         event->type = MSM_CAMERA_V4L2_EVENT_TYPE;
  58         event->id = evt_id;
  59         event_data->command = command;
  60         event_data->session_id = pvdev->vdev->num;
  61         event_data->stream_id = sp->stream_id;
  62         event_data->arg_value = value;
  63 }
  64
  65 static int camera_check_event_status(struct v4l2_event *event)
  66 {
  67         struct msm_v4l2_event_data *event_data =
  68                 (struct msm_v4l2_event_data *)&event->u.data[0];
  69
  70         if (event_data->status > MSM_CAMERA_ERR_EVT_BASE) {
  71                 pr_err("%s : event_data status out of bounds\n",
  72                                 __func__);
  73                 pr_err("%s : Line %d event_data->status 0X%x\n",
  74                                 __func__, __LINE__, event_data->status);
  75
  76                 switch (event_data->status) {
  77                 case MSM_CAMERA_ERR_CMD_FAIL:
  78                 case MSM_CAMERA_ERR_MAPPING:
  79                         return -EFAULT;
  80                 case MSM_CAMERA_ERR_DEVICE_BUSY:
  81                         return -EBUSY;
  82                 default:
  83                         return -EFAULT;
  84                 }
  85         }
  86
  87         return 0;
  88 }
  89
  90 static int camera_v4l2_querycap(struct file *filep, void *fh,
  91         struct v4l2_capability *cap)
  92 {
  93         int rc;
  94         struct v4l2_event event;
  95
  96         if (msm_is_daemon_present() == false)
  97                 return 0;
  98
  99         /* can use cap->driver to make differentiation */
 100         camera_pack_event(filep, MSM_CAMERA_GET_PARM,
 101                 MSM_CAMERA_PRIV_QUERY_CAP, -1, &event);
 102
 103         rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 104         if (rc < 0)
 105                 return rc;
 106
 107         rc = camera_check_event_status(&event);
 108
 109         return rc;
 110 }
 111
 112 static int camera_v4l2_s_crop(struct file *filep, void *fh,
 113         const struct v4l2_crop *crop)
 114 {
 115         int rc = 0;
 116         struct v4l2_event event;
 117
 118         if (msm_is_daemon_present() == false)
 119                 return 0;
 120
 121         if (crop->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 122
 123                 camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 124                         MSM_CAMERA_PRIV_S_CROP, -1, &event);
 125
 126                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 127                 if (rc < 0)
 128                         return rc;
 129
 130                 rc = camera_check_event_status(&event);
 131         }
 132
 133         return rc;
 134 }
 135
 136 static int camera_v4l2_g_crop(struct file *filep, void *fh,
 137         struct v4l2_crop *crop)
 138 {
 139         int rc = 0;
 140         struct v4l2_event event;
 141
 142         if (msm_is_daemon_present() == false)
 143                 return 0;
 144
 145         if (crop->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 146                 camera_pack_event(filep, MSM_CAMERA_GET_PARM,
 147                         MSM_CAMERA_PRIV_G_CROP, -1, &event);
 148
 149                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 150                 if (rc < 0)
 151                         return rc;
 152
 153                 rc = camera_check_event_status(&event);
 154         }
 155
 156         return rc;
 157 }
 158
 159 static int camera_v4l2_queryctrl(struct file *filep, void *fh,
 160         struct v4l2_queryctrl *ctrl)
 161 {
 162         int rc = 0;
 163         struct v4l2_event event;
 164
 165         if (msm_is_daemon_present() == false)
 166                 return 0;
 167
 168         if (ctrl->type == V4L2_CTRL_TYPE_MENU) {
 169
 170                 camera_pack_event(filep, MSM_CAMERA_GET_PARM,
 171                         ctrl->id, -1, &event);
 172
 173                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 174                 if (rc < 0)
 175                         return rc;
 176
 177                 rc = camera_check_event_status(&event);
 178         }
 179
 180         return rc;
 181 }
 182
 183 static int camera_v4l2_g_ctrl(struct file *filep, void *fh,
 184         struct v4l2_control *ctrl)
 185 {
 186         int rc = 0;
 187         struct v4l2_event event;
 188         struct msm_video_device *pvdev = video_drvdata(filep);
 189         unsigned int session_id = pvdev->vdev->num;
 190
 191         if (ctrl->id >= V4L2_CID_PRIVATE_BASE) {
 192                 if (ctrl->id == MSM_CAMERA_PRIV_G_SESSION_ID) {
 193                         ctrl->value = session_id;
 194                 } else {
 195                         camera_pack_event(filep, MSM_CAMERA_GET_PARM,
 196                                         ctrl->id, -1, &event);
 197
 198                         rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 199                         if (rc < 0)
 200                                 return rc;
 201
 202                         rc = camera_check_event_status(&event);
 203                 }
 204         }
 205
 206         return rc;
 207 }
 208
 209 static int camera_v4l2_s_ctrl(struct file *filep, void *fh,
 210         struct v4l2_control *ctrl)
 211 {
 212         int rc = 0;
 213         struct v4l2_event event;
 214         struct msm_v4l2_event_data *event_data;
 215
 216         if (ctrl->id >= V4L2_CID_PRIVATE_BASE) {
 217                 camera_pack_event(filep, MSM_CAMERA_SET_PARM, ctrl->id,
 218                 ctrl->value, &event);
 219
 220                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 221                 if (rc < 0)
 222                         return rc;
 223                 event_data = (struct msm_v4l2_event_data *)event.u.data;
 224                 ctrl->value = event_data->ret_value;
 225                 rc = camera_check_event_status(&event);
 226         }
 227
 228         return rc;
 229 }
 230
 231 static int camera_v4l2_reqbufs(struct file *filep, void *fh,
 232         struct v4l2_requestbuffers *req)
 233 {
 234         int ret;
 235         struct msm_session *session;
 236         struct camera_v4l2_private *sp = fh_to_private(fh);
 237         struct msm_video_device *pvdev = video_drvdata(filep);
 238         unsigned int session_id = pvdev->vdev->num;
 239
 240         session = msm_session_find(session_id);
 241         if (WARN_ON(!session))
 242                 return -EIO;
 243         mutex_lock(&sp->lock);
 244         ret = vb2_reqbufs(&sp->vb2_q, req);
 245         mutex_unlock(&sp->lock);
 246         return ret;
 247 }
 248
 249 static int camera_v4l2_querybuf(struct file *filep, void *fh,
 250         struct v4l2_buffer *pb)
 251 {
 252         return 0;
 253 }
 254
 255 static int camera_v4l2_qbuf(struct file *filep, void *fh,
 256         struct v4l2_buffer *pb)
 257 {
 258         int ret;
 259         struct msm_session *session;
 260         struct camera_v4l2_private *sp = fh_to_private(fh);
 261         struct msm_video_device *pvdev = video_drvdata(filep);
 262         unsigned int session_id = pvdev->vdev->num;
 263
 264         session = msm_session_find(session_id);
 265         if (WARN_ON(!session))
 266                 return -EIO;
 267         mutex_lock(&sp->lock);
 268         ret = vb2_qbuf(&sp->vb2_q, pb);
 269         mutex_unlock(&sp->lock);
 270         return ret;
 271 }
 272
 273 static int camera_v4l2_dqbuf(struct file *filep, void *fh,
 274         struct v4l2_buffer *pb)
 275 {
 276         int ret;
 277         struct msm_session *session;
 278         struct camera_v4l2_private *sp = fh_to_private(fh);
 279         struct msm_video_device *pvdev = video_drvdata(filep);
 280         unsigned int session_id = pvdev->vdev->num;
 281
 282         session = msm_session_find(session_id);
 283         if (WARN_ON(!session))
 284                 return -EIO;
 285         mutex_lock(&sp->lock);
 286         ret = vb2_dqbuf(&sp->vb2_q, pb, filep->f_flags & O_NONBLOCK);
 287         mutex_unlock(&sp->lock);
 288         return ret;
 289 }
 290
 291 static int camera_v4l2_streamon(struct file *filep, void *fh,
 292         enum v4l2_buf_type buf_type)
 293 {
 294         struct v4l2_event event;
 295         int rc;
 296         struct camera_v4l2_private *sp = fh_to_private(fh);
 297
 298         mutex_lock(&sp->lock);
 299         rc = vb2_streamon(&sp->vb2_q, buf_type);
 300         mutex_unlock(&sp->lock);
 301
 302         if (msm_is_daemon_present() == false)
 303                 return 0;
 304
 305         camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 306                 MSM_CAMERA_PRIV_STREAM_ON, -1, &event);
 307
 308         rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 309         if (rc < 0)
 310                 return rc;
 311
 312         rc = camera_check_event_status(&event);
 313         return rc;
 314 }
 315
 316 static int camera_v4l2_streamoff(struct file *filep, void *fh,
 317                 enum v4l2_buf_type buf_type)
 318 {
 319         struct v4l2_event event;
 320         int rc = 0;
 321         struct camera_v4l2_private *sp = fh_to_private(fh);
 322
 323         if (msm_is_daemon_present() != false) {
 324                 camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 325                         MSM_CAMERA_PRIV_STREAM_OFF, -1, &event);
 326
 327                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 328                 if (rc < 0)
 329                         return rc;
 330                 rc = camera_check_event_status(&event);
 331         }
 332         mutex_lock(&sp->lock);
 333         vb2_streamoff(&sp->vb2_q, buf_type);
 334         mutex_unlock(&sp->lock);
 335         return rc;
 336 }
 337
 338 static int camera_v4l2_g_fmt_vid_cap_mplane(struct file *filep, void *fh,
 339         struct v4l2_format *pfmt)
 340 {
 341         int rc = -EINVAL;
 342
 343         if (msm_is_daemon_present() == false)
 344                 return 0;
 345
 346         if (pfmt->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 347                 struct v4l2_event event;
 348
 349                 camera_pack_event(filep, MSM_CAMERA_GET_PARM,
 350                         MSM_CAMERA_PRIV_G_FMT, -1, &event);
 351
 352                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 353                 if (rc < 0)
 354                         return rc;
 355
 356                 rc = camera_check_event_status(&event);
 357         }
 358
 359         return rc;
 360 }
 361
 362 static int camera_v4l2_s_fmt_vid_cap_mplane(struct file *filep, void *fh,
 363         struct v4l2_format *pfmt)
 364 {
 365         int rc = 0;
 366         int i = 0;
 367         struct v4l2_event event;
 368         struct camera_v4l2_private *sp = fh_to_private(fh);
 369         struct msm_v4l2_format_data *user_fmt;
 370
 371         if (pfmt->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 372
 373                 if (WARN_ON(!sp->vb2_q.drv_priv))
 374                         return -ENOMEM;
 375
 376                 memcpy(sp->vb2_q.drv_priv, pfmt->fmt.raw_data,
 377                         sizeof(struct msm_v4l2_format_data));
 378                 user_fmt = (struct msm_v4l2_format_data *)sp->vb2_q.drv_priv;
 379
 380                 pr_debug("%s: num planes :%c\n", __func__,
 381                                         user_fmt->num_planes);
 382                 /* num_planes need to bound checked, otherwise for loop
 383                  * can execute forever
 384                  */
 385                 if (WARN_ON(user_fmt->num_planes > VIDEO_MAX_PLANES))
 386                         return -EINVAL;
 387                 for (i = 0; i < user_fmt->num_planes; i++)
 388                         pr_debug("%s: plane size[%d]\n", __func__,
 389                                         user_fmt->plane_sizes[i]);
 390
 391                 if (msm_is_daemon_present() != false) {
 392                         camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 393                                 MSM_CAMERA_PRIV_S_FMT, -1, &event);
 394
 395                         rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 396                         if (rc < 0)
 397                                 return rc;
 398
 399                         rc = camera_check_event_status(&event);
 400                         if (rc < 0)
 401                                 return rc;
 402                 }
 403                 sp->is_vb2_valid = 1;
 404         }
 405
 406         return rc;
 407 }
 408
 409 static int camera_v4l2_try_fmt_vid_cap_mplane(struct file *filep, void *fh,
 410         struct v4l2_format *pfmt)
 411 {
 412         return 0;
 413 }
 414
 415
 416 static int camera_v4l2_g_parm(struct file *filep, void *fh,
 417         struct v4l2_streamparm *a)
 418 {
 419         /* TODO */
 420         return 0;
 421 }
 422
 423 static int camera_v4l2_s_parm(struct file *filep, void *fh,
 424         struct v4l2_streamparm *parm)
 425 {
 426         int rc = 0;
 427         struct v4l2_event event;
 428         struct msm_v4l2_event_data *event_data =
 429                 (struct msm_v4l2_event_data *)&event.u.data[0];
 430         struct camera_v4l2_private *sp = fh_to_private(fh);
 431
 432         camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 433                 MSM_CAMERA_PRIV_NEW_STREAM, -1, &event);
 434
 435         rc = msm_create_stream(event_data->session_id,
 436                 event_data->stream_id, &sp->vb2_q);
 437         if (rc < 0)
 438                 return rc;
 439
 440         if (msm_is_daemon_present() != false) {
 441                 rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 442                 if (rc < 0)
 443                         goto error;
 444
 445                 rc = camera_check_event_status(&event);
 446                 if (rc < 0)
 447                         goto error;
 448         }
 449         /* use stream_id as stream index */
 450         parm->parm.capture.extendedmode = sp->stream_id;
 451         sp->stream_created = true;
 452
 453         return rc;
 454
 455 error:
 456         msm_delete_stream(event_data->session_id,
 457                 event_data->stream_id);
 458         return rc;
 459 }
 460
 461 static int camera_v4l2_subscribe_event(struct v4l2_fh *fh,
 462         const struct v4l2_event_subscription *sub)
 463 {
 464         int rc = 0;
 465         struct camera_v4l2_private *sp = fh_to_private(fh);
 466
 467         rc = v4l2_event_subscribe(&sp->fh, sub, 5, NULL);
 468
 469         return rc;
 470 }
 471
 472 static int camera_v4l2_unsubscribe_event(struct v4l2_fh *fh,
 473         const struct v4l2_event_subscription *sub)
 474 {
 475         int rc = 0;
 476         struct camera_v4l2_private *sp = fh_to_private(fh);
 477
 478         rc = v4l2_event_unsubscribe(&sp->fh, sub);
 479
 480         return rc;
 481 }
 482
 483 static long camera_v4l2_vidioc_private_ioctl(struct file *filep, void *fh,
 484         bool valid_prio, unsigned int cmd, void *arg)
 485 {
 486         struct camera_v4l2_private *sp = fh_to_private(fh);
 487         struct msm_video_device *pvdev = video_drvdata(filep);
 488         struct msm_camera_private_ioctl_arg *k_ioctl = arg;
 489         long rc = -EINVAL;
 490
 491         if (WARN_ON(!k_ioctl || !pvdev))
 492                 return -EIO;
 493
 494         if (cmd != VIDIOC_MSM_CAMERA_PRIVATE_IOCTL_CMD)
 495                 return -EINVAL;
 496
 497         switch (k_ioctl->id) {
 498         case MSM_CAMERA_PRIV_IOCTL_ID_RETURN_BUF: {
 499                 struct msm_camera_return_buf ptr, *tmp = NULL;
 500
 501                 MSM_CAM_GET_IOCTL_ARG_PTR(&tmp, &k_ioctl->ioctl_ptr,
 502                         sizeof(tmp));
 503                 if (copy_from_user(&ptr, (void __user *)tmp,
 504                         sizeof(struct msm_camera_return_buf))) {
 505                         return -EFAULT;
 506                 }
 507                 rc = msm_vb2_return_buf_by_idx(pvdev->vdev->num, sp->stream_id,
 508                         ptr.index);
 509                 }
 510                 break;
 511         default:
 512                 pr_debug("unimplemented id %d", k_ioctl->id);
 513                 return -EINVAL;
 514         }
 515         return rc;
 516 }
 517
 518 static const struct v4l2_ioctl_ops camera_v4l2_ioctl_ops = {
 519         .vidioc_querycap = camera_v4l2_querycap,
 520         .vidioc_s_crop = camera_v4l2_s_crop,
 521         .vidioc_g_crop = camera_v4l2_g_crop,
 522         .vidioc_queryctrl = camera_v4l2_queryctrl,
 523         .vidioc_g_ctrl = camera_v4l2_g_ctrl,
 524         .vidioc_s_ctrl = camera_v4l2_s_ctrl,
 525         .vidioc_reqbufs = camera_v4l2_reqbufs,
 526         .vidioc_querybuf = camera_v4l2_querybuf,
 527         .vidioc_qbuf = camera_v4l2_qbuf,
 528         .vidioc_dqbuf = camera_v4l2_dqbuf,
 529         .vidioc_streamon =  camera_v4l2_streamon,
 530         .vidioc_streamoff = camera_v4l2_streamoff,
 531         .vidioc_g_fmt_vid_cap_mplane = camera_v4l2_g_fmt_vid_cap_mplane,
 532         .vidioc_s_fmt_vid_cap_mplane = camera_v4l2_s_fmt_vid_cap_mplane,
 533         .vidioc_try_fmt_vid_cap_mplane = camera_v4l2_try_fmt_vid_cap_mplane,
 534
 535         /* Stream type-dependent parameter ioctls */
 536         .vidioc_g_parm = camera_v4l2_g_parm,
 537         .vidioc_s_parm = camera_v4l2_s_parm,
 538
 539         /* event subscribe/unsubscribe */
 540         .vidioc_subscribe_event = camera_v4l2_subscribe_event,
 541         .vidioc_unsubscribe_event = camera_v4l2_unsubscribe_event,
 542         .vidioc_default = camera_v4l2_vidioc_private_ioctl,
 543 };
 544
 545 static int camera_v4l2_fh_open(struct file *filep)
 546 {
 547         struct msm_video_device *pvdev = video_drvdata(filep);
 548         struct camera_v4l2_private *sp;
 549         unsigned int stream_id;
 550
 551         sp = kzalloc(sizeof(*sp), GFP_KERNEL);
 552         if (!sp)
 553                 return -ENOMEM;
 554
 555         filep->private_data = &sp->fh;
 556
 557         /* stream_id = open id */
 558         stream_id = atomic_read(&pvdev->opened);
 559         sp->stream_id = find_first_zero_bit(
 560                 (const unsigned long *)&stream_id, MSM_CAMERA_STREAM_CNT_BITS);
 561         pr_debug("%s: Found stream_id=%d\n", __func__, sp->stream_id);
 562
 563         mutex_init(&sp->lock);
 564
 565         v4l2_fh_init(&sp->fh, pvdev->vdev);
 566         v4l2_fh_add(&sp->fh);
 567
 568         return 0;
 569 }
 570
 571 static int camera_v4l2_fh_release(struct file *filep)
 572 {
 573         struct camera_v4l2_private *sp = fh_to_private(filep->private_data);
 574
 575         if (sp) {
 576                 v4l2_fh_del(&sp->fh);
 577                 v4l2_fh_exit(&sp->fh);
 578         }
 579
 580         mutex_destroy(&sp->lock);
 581         kzfree(sp);
 582         return 0;
 583 }
 584
 585 static int camera_v4l2_vb2_q_init(struct file *filep)
 586 {
 587         struct camera_v4l2_private *sp = fh_to_private(filep->private_data);
 588         struct vb2_queue *q = &sp->vb2_q;
 589
 590         memset(q, 0, sizeof(struct vb2_queue));
 591
 592         /* free up this buffer when stream is done */
 593         q->drv_priv =
 594                 kzalloc(sizeof(struct msm_v4l2_format_data), GFP_KERNEL);
 595         if (!q->drv_priv) {
 596                 pr_err("%s : memory not available\n", __func__);
 597                 return -ENOMEM;
 598         }
 599
 600         q->mem_ops = msm_vb2_get_q_mem_ops();
 601         q->ops = msm_vb2_get_q_ops();
 602
 603         /* default queue type */
 604         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 605         q->io_modes = VB2_USERPTR;
 606         q->buf_struct_size = sizeof(struct msm_vb2_buffer);
 607         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 608         return vb2_queue_init(q);
 609 }
 610
 611 static void camera_v4l2_vb2_q_release(struct file *filep)
 612 {
 613         struct camera_v4l2_private *sp = filep->private_data;
 614
 615         kzfree(sp->vb2_q.drv_priv);
 616         mutex_lock(&sp->lock);
 617         vb2_queue_release(&sp->vb2_q);
 618         mutex_unlock(&sp->lock);
 619 }
 620
 621 static int camera_v4l2_open(struct file *filep)
 622 {
 623         int rc = 0;
 624         struct v4l2_event event;
 625         struct msm_video_device *pvdev = video_drvdata(filep);
 626         unsigned int opn_idx, idx;
 627
 628         if (WARN_ON(!pvdev))
 629                 return -EIO;
 630
 631         rc = camera_v4l2_fh_open(filep);
 632         if (rc < 0) {
 633                 pr_err("%s : camera_v4l2_fh_open failed Line %d rc %d\n",
 634                                 __func__, __LINE__, rc);
 635                 goto fh_open_fail;
 636         }
 637
 638         opn_idx = atomic_read(&pvdev->opened);
 639         idx = opn_idx;
 640         /* every stream has a vb2 queue */
 641         rc = camera_v4l2_vb2_q_init(filep);
 642         if (rc < 0) {
 643                 pr_err("%s : vb2 queue init fails Line %d rc %d\n",
 644                                 __func__, __LINE__, rc);
 645                 goto vb2_q_fail;
 646         }
 647
 648         if (!atomic_read(&pvdev->opened)) {
 649                 pm_stay_awake(&pvdev->vdev->dev);
 650
 651                 /* Disable power collapse latency */
 652                 msm_pm_qos_update_request(CAMERA_DISABLE_PC_LATENCY);
 653
 654                 /* create a new session when first opened */
 655                 rc = msm_create_session(pvdev->vdev->num, pvdev->vdev);
 656                 if (rc < 0) {
 657                         pr_err("%s : session creation failed Line %d rc %d\n",
 658                                         __func__, __LINE__, rc);
 659                         goto session_fail;
 660                 }
 661
 662                 rc = msm_create_command_ack_q(pvdev->vdev->num,
 663                         find_first_zero_bit((const unsigned long *)&opn_idx,
 664                                 MSM_CAMERA_STREAM_CNT_BITS));
 665                 if (rc < 0) {
 666                         pr_err("%s : creation of command_ack queue failed\n",
 667                                         __func__);
 668                         pr_err("%s : Line %d rc %d\n", __func__, __LINE__, rc);
 669                         goto command_ack_q_fail;
 670                 }
 671
 672                 if (msm_is_daemon_present() != false) {
 673                         camera_pack_event(filep, MSM_CAMERA_NEW_SESSION,
 674                                 0, -1, &event);
 675                         rc = msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 676                         if (rc < 0) {
 677                                 pr_err("%s : NEW_SESSION event failed,rc %d\n",
 678                                         __func__, rc);
 679                                 goto post_fail;
 680                         }
 681
 682                         rc = camera_check_event_status(&event);
 683                         if (rc < 0)
 684                                 goto post_fail;
 685                 }
 686                 /* Enable power collapse latency */
 687                 msm_pm_qos_update_request(CAMERA_ENABLE_PC_LATENCY);
 688         } else {
 689                 rc = msm_create_command_ack_q(pvdev->vdev->num,
 690                         find_first_zero_bit((const unsigned long *)&opn_idx,
 691                                 MSM_CAMERA_STREAM_CNT_BITS));
 692                 if (rc < 0) {
 693                         pr_err("%s : creation of command_ack queue failed Line %d rc %d\n",
 694                                         __func__, __LINE__, rc);
 695                         goto stream_fail;
 696                 }
 697         }
 698         idx |= (1 << find_first_zero_bit((const unsigned long *)&opn_idx,
 699                                 MSM_CAMERA_STREAM_CNT_BITS));
 700         atomic_cmpxchg(&pvdev->opened, opn_idx, idx);
 701
 702         return rc;
 703
 704 post_fail:
 705         msm_delete_command_ack_q(pvdev->vdev->num, 0);
 706 command_ack_q_fail:
 707         msm_destroy_session(pvdev->vdev->num);
 708 session_fail:
 709         pm_relax(&pvdev->vdev->dev);
 710 stream_fail:
 711         camera_v4l2_vb2_q_release(filep);
 712 vb2_q_fail:
 713         camera_v4l2_fh_release(filep);
 714 fh_open_fail:
 715         return rc;
 716 }
 717
 718 static unsigned int camera_v4l2_poll(struct file *filep,
 719         struct poll_table_struct *wait)
 720 {
 721         int rc = 0;
 722         struct camera_v4l2_private *sp = fh_to_private(filep->private_data);
 723
 724         if (sp->is_vb2_valid == 1)
 725                 rc = vb2_poll(&sp->vb2_q, filep, wait);
 726
 727         poll_wait(filep, &sp->fh.wait, wait);
 728         if (v4l2_event_pending(&sp->fh))
 729                 rc |= POLLPRI;
 730
 731         return rc;
 732 }
 733
 734 static int camera_v4l2_close(struct file *filep)
 735 {
 736         struct v4l2_event event;
 737         struct msm_video_device *pvdev = video_drvdata(filep);
 738         struct camera_v4l2_private *sp = fh_to_private(filep->private_data);
 739         unsigned int opn_idx, mask;
 740         struct msm_session *session;
 741
 742         if (WARN_ON(!pvdev))
 743                 return -EIO;
 744
 745         session = msm_session_find(pvdev->vdev->num);
 746         if (WARN_ON(!session))
 747                 return -EIO;
 748
 749         mutex_lock(&session->close_lock);
 750         opn_idx = atomic_read(&pvdev->opened);
 751         mask = (1 << sp->stream_id);
 752         opn_idx &= ~mask;
 753         atomic_set(&pvdev->opened, opn_idx);
 754
 755         if (msm_is_daemon_present() != false && sp->stream_created == true) {
 756                 pr_debug("%s: close stream_id=%d\n", __func__, sp->stream_id);
 757                 camera_pack_event(filep, MSM_CAMERA_SET_PARM,
 758                         MSM_CAMERA_PRIV_DEL_STREAM, -1, &event);
 759                 msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 760         }
 761
 762         if (sp->stream_created == true)
 763                 sp->stream_created = false;
 764
 765         if (atomic_read(&pvdev->opened) == 0) {
 766                 if (msm_is_daemon_present() != false) {
 767                         camera_pack_event(filep, MSM_CAMERA_DEL_SESSION,
 768                                 0, -1, &event);
 769                         msm_post_event(&event, MSM_POST_EVT_TIMEOUT);
 770                 }
 771                 msm_delete_command_ack_q(pvdev->vdev->num, 0);
 772                 msm_delete_stream(pvdev->vdev->num, sp->stream_id);
 773                 mutex_unlock(&session->close_lock);
 774                 /* This should take care of both normal close
 775                  * and application crashes
 776                  */
 777                 camera_v4l2_vb2_q_release(filep);
 778                 msm_destroy_session(pvdev->vdev->num);
 779
 780                 pm_relax(&pvdev->vdev->dev);
 781         } else {
 782                 msm_delete_command_ack_q(pvdev->vdev->num,
 783                         sp->stream_id);
 784
 785                 camera_v4l2_vb2_q_release(filep);
 786                 msm_delete_stream(pvdev->vdev->num, sp->stream_id);
 787                 mutex_unlock(&session->close_lock);
 788         }
 789
 790         camera_v4l2_fh_release(filep);
 791
 792         return 0;
 793 }
 794
 795 #ifdef CONFIG_COMPAT
 796 static long camera_handle_internal_compat_ioctl(struct file *file,
 797                 unsigned int cmd, unsigned long arg)
 798 {
 799         long rc = 0;
 800         struct msm_camera_private_ioctl_arg k_ioctl;
 801         void *tmp_compat_ioctl_ptr = NULL;
 802
 803         rc = msm_copy_camera_private_ioctl_args(arg,
 804                 &k_ioctl, &tmp_compat_ioctl_ptr);
 805         if (rc < 0) {
 806                 pr_err("Subdev cmd %d failed\n", cmd);
 807                 return rc;
 808         }
 809         switch (k_ioctl.id) {
 810         case MSM_CAMERA_PRIV_IOCTL_ID_RETURN_BUF: {
 811                 if (k_ioctl.size != sizeof(struct msm_camera_return_buf)) {
 812                         pr_debug("Invalid size for id %d with size %d",
 813                                 k_ioctl.id, k_ioctl.size);
 814                         return -EINVAL;
 815                 }
 816
 817                 if (tmp_compat_ioctl_ptr == NULL) {
 818                         pr_debug("Invalid ptr for id %d", k_ioctl.id);
 819                         return -EINVAL;
 820                 }
 821                 k_ioctl.ioctl_ptr = (__u64)(uintptr_t)tmp_compat_ioctl_ptr;
 822
 823                 rc = camera_v4l2_vidioc_private_ioctl(file, file->private_data,
 824                         0, cmd, (void *)&k_ioctl);
 825                 }
 826                 break;
 827         default:
 828                 pr_debug("unimplemented id %d", k_ioctl.id);
 829                 return -EINVAL;
 830         }
 831         return rc;
 832 }
 833
 834 static long camera_v4l2_compat_ioctl(struct file *file, unsigned int cmd,
 835         unsigned long arg)
 836 {
 837         long ret = 0;
 838
 839         switch (cmd) {
 840         case VIDIOC_MSM_CAMERA_PRIVATE_IOCTL_CMD: {
 841                 ret = camera_handle_internal_compat_ioctl(file, cmd, arg);
 842                 if (ret < 0) {
 843                         pr_debug("Subdev cmd %d fail\n", cmd);
 844                         return ret;
 845                 }
 846                 }
 847                 break;
 848         default:
 849                 ret = -ENOIOCTLCMD;
 850                 break;
 851
 852         }
 853         return ret;
 854 }
 855 #endif
 856 static struct v4l2_file_operations camera_v4l2_fops = {
 857         .owner   = THIS_MODULE,
 858         .open   = camera_v4l2_open,
 859         .poll   = camera_v4l2_poll,
 860         .release = camera_v4l2_close,
 861         .unlocked_ioctl   = video_ioctl2,
 862 #ifdef CONFIG_COMPAT
 863         .compat_ioctl32 = camera_v4l2_compat_ioctl,
 864 #endif
 865 };
 866
 867 int camera_init_v4l2(struct device *dev, unsigned int *session)
 868 {
 869         struct msm_video_device *pvdev;
 870         struct v4l2_device *v4l2_dev;
 871         int rc = 0;
 872
 873         pvdev = kzalloc(sizeof(struct msm_video_device),
 874                 GFP_KERNEL);
 875         if (!pvdev) {
 876                 rc = -ENOMEM;
 877                 goto init_end;
 878         }
 879
 880         pvdev->vdev = video_device_alloc();
 881         if (!pvdev->vdev) {
 882                 rc = -ENOMEM;
 883                 goto video_fail;
 884         }
 885
 886         v4l2_dev = kzalloc(sizeof(struct v4l2_device), GFP_KERNEL);
 887         if (!v4l2_dev) {
 888                 rc = -ENOMEM;
 889                 goto v4l2_fail;
 890         }
 891
 892 #if defined(CONFIG_MEDIA_CONTROLLER)
 893         v4l2_dev->mdev = kzalloc(sizeof(struct media_device),
 894                                                          GFP_KERNEL);
 895         if (!v4l2_dev->mdev) {
 896                 rc = -ENOMEM;
 897                 goto mdev_fail;
 898         }
 899         strlcpy(v4l2_dev->mdev->model, MSM_CAMERA_NAME,
 900                         sizeof(v4l2_dev->mdev->model));
 901
 902         v4l2_dev->mdev->dev = dev;
 903
 904         rc = media_device_register(v4l2_dev->mdev);
 905         if (WARN_ON(rc < 0))
 906                 goto media_fail;
 907
 908         rc = media_entity_init(&pvdev->vdev->entity, 0, NULL, 0);
 909         if (WARN_ON(rc < 0))
 910                 goto entity_fail;
 911         pvdev->vdev->entity.type = MEDIA_ENT_T_DEVNODE_V4L;
 912         pvdev->vdev->entity.group_id = QCAMERA_VNODE_GROUP_ID;
 913 #endif
 914
 915         v4l2_dev->notify = NULL;
 916         pvdev->vdev->v4l2_dev = v4l2_dev;
 917
 918         rc = v4l2_device_register(dev, pvdev->vdev->v4l2_dev);
 919         if (WARN_ON(rc < 0))
 920                 goto register_fail;
 921
 922         strlcpy(pvdev->vdev->name, "msm-sensor", sizeof(pvdev->vdev->name));
 923         pvdev->vdev->release  = video_device_release;
 924         pvdev->vdev->fops     = &camera_v4l2_fops;
 925         pvdev->vdev->ioctl_ops = &camera_v4l2_ioctl_ops;
 926         pvdev->vdev->minor     = -1;
 927         pvdev->vdev->vfl_type  = VFL_TYPE_GRABBER;
 928         rc = video_register_device(pvdev->vdev,
 929                 VFL_TYPE_GRABBER, -1);
 930         if (WARN_ON(rc < 0))
 931                 goto video_register_fail;
 932 #if defined(CONFIG_MEDIA_CONTROLLER)
 933         /* FIXME: How to get rid of this messy? */
 934         pvdev->vdev->entity.name = video_device_node_name(pvdev->vdev);
 935 #endif
 936
 937         *session = pvdev->vdev->num;
 938         atomic_set(&pvdev->opened, 0);
 939         video_set_drvdata(pvdev->vdev, pvdev);
 940         device_init_wakeup(&pvdev->vdev->dev, 1);
 941         goto init_end;
 942
 943 video_register_fail:
 944         v4l2_device_unregister(pvdev->vdev->v4l2_dev);
 945 register_fail:
 946 #if defined(CONFIG_MEDIA_CONTROLLER)
 947         media_entity_cleanup(&pvdev->vdev->entity);
 948 entity_fail:
 949         media_device_unregister(v4l2_dev->mdev);
 950 media_fail:
 951         kzfree(v4l2_dev->mdev);
 952 mdev_fail:
 953 #endif
 954         kzfree(v4l2_dev);
 955 v4l2_fail:
 956         video_device_release(pvdev->vdev);
 957 video_fail:
 958         kzfree(pvdev);
 959 init_end:
 960         return rc;
 961 }