drivers/staging/dream/camera/msm_camera.c

   1 /*
   2  * Copyright (C) 2008-2009 QUALCOMM Incorporated.
   3  */
   4
   5 /* FIXME: most allocations need not be GFP_ATOMIC */
   6 /* FIXME: management of mutexes */
   7 /* FIXME: msm_pmem_region_lookup return values */
   8 /* FIXME: way too many copy to/from user */
   9 /* FIXME: does region->active mean free */
  10 /* FIXME: check limits on command lenghts passed from userspace */
  11 /* FIXME: __msm_release: which queues should we flush when opencnt != 0 */
  12
  13 #include <linux/kernel.h>
  14 #include <linux/module.h>
  15 #include <linux/slab.h>
  16 #include <linux/init.h>
  17 #include <linux/sched.h>
  18 #include <mach/board.h>
  19
  20 #include <linux/fs.h>
  21 #include <linux/list.h>
  22 #include <linux/uaccess.h>
  23 #include <linux/android_pmem.h>
  24 #include <linux/poll.h>
  25 #include <media/msm_camera.h>
  26 #include <mach/camera.h>
  27
  28 #define MSM_MAX_CAMERA_SENSORS 5
  29
  30 #define ERR_USER_COPY(to) pr_err("%s(%d): copy %s user\n", \
  31                                 __func__, __LINE__, ((to) ? "to" : "from"))
  32 #define ERR_COPY_FROM_USER() ERR_USER_COPY(0)
  33 #define ERR_COPY_TO_USER() ERR_USER_COPY(1)
  34
  35 static struct class *msm_class;
  36 static dev_t msm_devno;
  37 static LIST_HEAD(msm_sensors);
  38
  39 #define __CONTAINS(r, v, l, field) ({                           \
  40         typeof(r) __r = r;                                      \
  41         typeof(v) __v = v;                                      \
  42         typeof(v) __e = __v + l;                                \
  43         int res = __v >= __r->field &&                          \
  44                 __e <= __r->field + __r->len;                   \
  45         res;                                                    \
  46 })
  47
  48 #define CONTAINS(r1, r2, field) ({                              \
  49         typeof(r2) __r2 = r2;                                   \
  50         __CONTAINS(r1, __r2->field, __r2->len, field);          \
  51 })
  52
  53 #define IN_RANGE(r, v, field) ({                                \
  54         typeof(r) __r = r;                                      \
  55         typeof(v) __vv = v;                                     \
  56         int res = ((__vv >= __r->field) &&                      \
  57                 (__vv < (__r->field + __r->len)));              \
  58         res;                                                    \
  59 })
  60
  61 #define OVERLAPS(r1, r2, field) ({                              \
  62         typeof(r1) __r1 = r1;                                   \
  63         typeof(r2) __r2 = r2;                                   \
  64         typeof(__r2->field) __v = __r2->field;                  \
  65         typeof(__v) __e = __v + __r2->len - 1;                  \
  66         int res = (IN_RANGE(__r1, __v, field) ||                \
  67                    IN_RANGE(__r1, __e, field));                 \
  68         res;                                                    \
  69 })
  70
  71 #define MSM_DRAIN_QUEUE_NOSYNC(sync, name) do {                 \
  72         struct msm_queue_cmd *qcmd = NULL;                      \
  73         CDBG("%s: draining queue "#name"\n", __func__);         \
  74         while (!list_empty(&(sync)->name)) {                    \
  75                 qcmd = list_first_entry(&(sync)->name,          \
  76                         struct msm_queue_cmd, list);            \
  77                 list_del_init(&qcmd->list);                     \
  78                 kfree(qcmd);                                    \
  79         };                                                      \
  80 } while (0)
  81
  82 #define MSM_DRAIN_QUEUE(sync, name) do {                        \
  83         unsigned long flags;                                    \
  84         spin_lock_irqsave(&(sync)->name##_lock, flags);         \
  85         MSM_DRAIN_QUEUE_NOSYNC(sync, name);                     \
  86         spin_unlock_irqrestore(&(sync)->name##_lock, flags);    \
  87 } while (0)
  88
  89 static int check_overlap(struct hlist_head *ptype,
  90                         unsigned long paddr,
  91                         unsigned long len)
  92 {
  93         struct msm_pmem_region *region;
  94         struct msm_pmem_region t = { .paddr = paddr, .len = len };
  95         struct hlist_node *node;
  96
  97         hlist_for_each_entry(region, node, ptype, list) {
  98                 if (CONTAINS(region, &t, paddr) ||
  99                                 CONTAINS(&t, region, paddr) ||
 100                                 OVERLAPS(region, &t, paddr)) {
 101                         printk(KERN_ERR
 102                                 " region (PHYS %p len %ld)"
 103                                 " clashes with registered region"
 104                                 " (paddr %p len %ld)\n",
 105                                 (void *)t.paddr, t.len,
 106                                 (void *)region->paddr, region->len);
 107                         return -1;
 108                 }
 109         }
 110
 111         return 0;
 112 }
 113
 114 static int msm_pmem_table_add(struct hlist_head *ptype,
 115         struct msm_pmem_info *info)
 116 {
 117         struct file *file;
 118         unsigned long paddr;
 119         unsigned long vstart;
 120         unsigned long len;
 121         int rc;
 122         struct msm_pmem_region *region;
 123
 124         rc = get_pmem_file(info->fd, &paddr, &vstart, &len, &file);
 125         if (rc < 0) {
 126                 pr_err("msm_pmem_table_add: get_pmem_file fd %d error %d\n",
 127                         info->fd, rc);
 128                 return rc;
 129         }
 130
 131         if (check_overlap(ptype, paddr, len) < 0)
 132                 return -EINVAL;
 133
 134         CDBG("%s: type = %d, paddr = 0x%lx, vaddr = 0x%lx\n",
 135                 __func__,
 136                 info->type, paddr, (unsigned long)info->vaddr);
 137
 138         region = kmalloc(sizeof(*region), GFP_KERNEL);
 139         if (!region)
 140                 return -ENOMEM;
 141
 142         INIT_HLIST_NODE(&region->list);
 143
 144         region->type = info->type;
 145         region->vaddr = info->vaddr;
 146         region->paddr = paddr;
 147         region->len = len;
 148         region->file = file;
 149         region->y_off = info->y_off;
 150         region->cbcr_off = info->cbcr_off;
 151         region->fd = info->fd;
 152         region->active = info->active;
 153
 154         hlist_add_head(&(region->list), ptype);
 155
 156         return 0;
 157 }
 158
 159 /* return of 0 means failure */
 160 static uint8_t msm_pmem_region_lookup(struct hlist_head *ptype,
 161         int pmem_type, struct msm_pmem_region *reg, uint8_t maxcount)
 162 {
 163         struct msm_pmem_region *region;
 164         struct msm_pmem_region *regptr;
 165         struct hlist_node *node, *n;
 166
 167         uint8_t rc = 0;
 168
 169         regptr = reg;
 170
 171         hlist_for_each_entry_safe(region, node, n, ptype, list) {
 172                 if (region->type == pmem_type && region->active) {
 173                         *regptr = *region;
 174                         rc += 1;
 175                         if (rc >= maxcount)
 176                                 break;
 177                         regptr++;
 178                 }
 179         }
 180
 181         return rc;
 182 }
 183
 184 static unsigned long msm_pmem_frame_ptov_lookup(struct msm_sync *sync,
 185                 unsigned long pyaddr,
 186                 unsigned long pcbcraddr,
 187                 uint32_t *yoff, uint32_t *cbcroff, int *fd)
 188 {
 189         struct msm_pmem_region *region;
 190         struct hlist_node *node, *n;
 191
 192         hlist_for_each_entry_safe(region, node, n, &sync->frame, list) {
 193                 if (pyaddr == (region->paddr + region->y_off) &&
 194                                 pcbcraddr == (region->paddr +
 195                                                 region->cbcr_off) &&
 196                                 region->active) {
 197                         /* offset since we could pass vaddr inside
 198                          * a registerd pmem buffer
 199                          */
 200                         *yoff = region->y_off;
 201                         *cbcroff = region->cbcr_off;
 202                         *fd = region->fd;
 203                         region->active = 0;
 204                         return (unsigned long)(region->vaddr);
 205                 }
 206         }
 207
 208         return 0;
 209 }
 210
 211 static unsigned long msm_pmem_stats_ptov_lookup(struct msm_sync *sync,
 212                 unsigned long addr, int *fd)
 213 {
 214         struct msm_pmem_region *region;
 215         struct hlist_node *node, *n;
 216
 217         hlist_for_each_entry_safe(region, node, n, &sync->stats, list) {
 218                 if (addr == region->paddr && region->active) {
 219                         /* offset since we could pass vaddr inside a
 220                          * registered pmem buffer */
 221                         *fd = region->fd;
 222                         region->active = 0;
 223                         return (unsigned long)(region->vaddr);
 224                 }
 225         }
 226
 227         return 0;
 228 }
 229
 230 static unsigned long msm_pmem_frame_vtop_lookup(struct msm_sync *sync,
 231                 unsigned long buffer,
 232                 uint32_t yoff, uint32_t cbcroff, int fd)
 233 {
 234         struct msm_pmem_region *region;
 235         struct hlist_node *node, *n;
 236
 237         hlist_for_each_entry_safe(region,
 238                 node, n, &sync->frame, list) {
 239                 if (((unsigned long)(region->vaddr) == buffer) &&
 240                                 (region->y_off == yoff) &&
 241                                 (region->cbcr_off == cbcroff) &&
 242                                 (region->fd == fd) &&
 243                                 (region->active == 0)) {
 244
 245                         region->active = 1;
 246                         return region->paddr;
 247                 }
 248         }
 249
 250         return 0;
 251 }
 252
 253 static unsigned long msm_pmem_stats_vtop_lookup(
 254                 struct msm_sync *sync,
 255                 unsigned long buffer,
 256                 int fd)
 257 {
 258         struct msm_pmem_region *region;
 259         struct hlist_node *node, *n;
 260
 261         hlist_for_each_entry_safe(region, node, n, &sync->stats, list) {
 262                 if (((unsigned long)(region->vaddr) == buffer) &&
 263                                 (region->fd == fd) && region->active == 0) {
 264                         region->active = 1;
 265                         return region->paddr;
 266                 }
 267         }
 268
 269         return 0;
 270 }
 271
 272 static int __msm_pmem_table_del(struct msm_sync *sync,
 273                 struct msm_pmem_info *pinfo)
 274 {
 275         int rc = 0;
 276         struct msm_pmem_region *region;
 277         struct hlist_node *node, *n;
 278
 279         switch (pinfo->type) {
 280         case MSM_PMEM_OUTPUT1:
 281         case MSM_PMEM_OUTPUT2:
 282         case MSM_PMEM_THUMBAIL:
 283         case MSM_PMEM_MAINIMG:
 284         case MSM_PMEM_RAW_MAINIMG:
 285                 hlist_for_each_entry_safe(region, node, n,
 286                         &sync->frame, list) {
 287
 288                         if (pinfo->type == region->type &&
 289                                         pinfo->vaddr == region->vaddr &&
 290                                         pinfo->fd == region->fd) {
 291                                 hlist_del(node);
 292                                 put_pmem_file(region->file);
 293                                 kfree(region);
 294                         }
 295                 }
 296                 break;
 297
 298         case MSM_PMEM_AEC_AWB:
 299         case MSM_PMEM_AF:
 300                 hlist_for_each_entry_safe(region, node, n,
 301                         &sync->stats, list) {
 302
 303                         if (pinfo->type == region->type &&
 304                                         pinfo->vaddr == region->vaddr &&
 305                                         pinfo->fd == region->fd) {
 306                                 hlist_del(node);
 307                                 put_pmem_file(region->file);
 308                                 kfree(region);
 309                         }
 310                 }
 311                 break;
 312
 313         default:
 314                 rc = -EINVAL;
 315                 break;
 316         }
 317
 318         return rc;
 319 }
 320
 321 static int msm_pmem_table_del(struct msm_sync *sync, void __user *arg)
 322 {
 323         struct msm_pmem_info info;
 324
 325         if (copy_from_user(&info, arg, sizeof(info))) {
 326                 ERR_COPY_FROM_USER();
 327                 return -EFAULT;
 328         }
 329
 330         return __msm_pmem_table_del(sync, &info);
 331 }
 332
 333 static int __msm_get_frame(struct msm_sync *sync,
 334                 struct msm_frame *frame)
 335 {
 336         unsigned long flags;
 337         int rc = 0;
 338
 339         struct msm_queue_cmd *qcmd = NULL;
 340         struct msm_vfe_phy_info *pphy;
 341
 342         spin_lock_irqsave(&sync->prev_frame_q_lock, flags);
 343         if (!list_empty(&sync->prev_frame_q)) {
 344                 qcmd = list_first_entry(&sync->prev_frame_q,
 345                         struct msm_queue_cmd, list);
 346                 list_del_init(&qcmd->list);
 347         }
 348         spin_unlock_irqrestore(&sync->prev_frame_q_lock, flags);
 349
 350         if (!qcmd) {
 351                 pr_err("%s: no preview frame.\n", __func__);
 352                 return -EAGAIN;
 353         }
 354
 355         pphy = (struct msm_vfe_phy_info *)(qcmd->command);
 356
 357         frame->buffer =
 358                 msm_pmem_frame_ptov_lookup(sync,
 359                         pphy->y_phy,
 360                         pphy->cbcr_phy, &(frame->y_off),
 361                         &(frame->cbcr_off), &(frame->fd));
 362         if (!frame->buffer) {
 363                 pr_err("%s: cannot get frame, invalid lookup address "
 364                         "y=%x cbcr=%x offset=%d\n",
 365                         __func__,
 366                         pphy->y_phy,
 367                         pphy->cbcr_phy,
 368                         frame->y_off);
 369                 rc = -EINVAL;
 370         }
 371
 372         CDBG("__msm_get_frame: y=0x%x, cbcr=0x%x, qcmd=0x%x, virt_addr=0x%x\n",
 373                 pphy->y_phy, pphy->cbcr_phy, (int) qcmd, (int) frame->buffer);
 374
 375         kfree(qcmd);
 376         return rc;
 377 }
 378
 379 static int msm_get_frame(struct msm_sync *sync, void __user *arg)
 380 {
 381         int rc = 0;
 382         struct msm_frame frame;
 383
 384         if (copy_from_user(&frame,
 385                                 arg,
 386                                 sizeof(struct msm_frame))) {
 387                 ERR_COPY_FROM_USER();
 388                 return -EFAULT;
 389         }
 390
 391         rc = __msm_get_frame(sync, &frame);
 392         if (rc < 0)
 393                 return rc;
 394
 395         if (sync->croplen) {
 396                 if (frame.croplen > sync->croplen) {
 397                         pr_err("msm_get_frame: invalid frame croplen %d\n",
 398                                 frame.croplen);
 399                         return -EINVAL;
 400                 }
 401
 402                 if (copy_to_user((void *)frame.cropinfo,
 403                                 sync->cropinfo,
 404                                 sync->croplen)) {
 405                         ERR_COPY_TO_USER();
 406                         return -EFAULT;
 407                 }
 408         }
 409
 410         if (copy_to_user((void *)arg,
 411                                 &frame, sizeof(struct msm_frame))) {
 412                 ERR_COPY_TO_USER();
 413                 rc = -EFAULT;
 414         }
 415
 416         CDBG("Got frame!!!\n");
 417
 418         return rc;
 419 }
 420
 421 static int msm_enable_vfe(struct msm_sync *sync, void __user *arg)
 422 {
 423         int rc = -EIO;
 424         struct camera_enable_cmd cfg;
 425
 426         if (copy_from_user(&cfg,
 427                         arg,
 428                         sizeof(struct camera_enable_cmd))) {
 429                 ERR_COPY_FROM_USER();
 430                 return -EFAULT;
 431         }
 432
 433         if (sync->vfefn.vfe_enable)
 434                 rc = sync->vfefn.vfe_enable(&cfg);
 435
 436         CDBG("msm_enable_vfe: returned rc = %d\n", rc);
 437         return rc;
 438 }
 439
 440 static int msm_disable_vfe(struct msm_sync *sync, void __user *arg)
 441 {
 442         int rc = -EIO;
 443         struct camera_enable_cmd cfg;
 444
 445         if (copy_from_user(&cfg,
 446                         arg,
 447                         sizeof(struct camera_enable_cmd))) {
 448                 ERR_COPY_FROM_USER();
 449                 return -EFAULT;
 450         }
 451
 452         if (sync->vfefn.vfe_disable)
 453                 rc = sync->vfefn.vfe_disable(&cfg, NULL);
 454
 455         CDBG("msm_disable_vfe: returned rc = %d\n", rc);
 456         return rc;
 457 }
 458
 459 static struct msm_queue_cmd *__msm_control(struct msm_sync *sync,
 460                 struct msm_control_device_queue *queue,
 461                 struct msm_queue_cmd *qcmd,
 462                 int timeout)
 463 {
 464         unsigned long flags;
 465         int rc;
 466
 467         spin_lock_irqsave(&sync->msg_event_q_lock, flags);
 468         list_add_tail(&qcmd->list, &sync->msg_event_q);
 469         /* wake up config thread */
 470         wake_up(&sync->msg_event_wait);
 471         spin_unlock_irqrestore(&sync->msg_event_q_lock, flags);
 472
 473         if (!queue)
 474                 return NULL;
 475
 476         /* wait for config status */
 477         rc = wait_event_interruptible_timeout(
 478                         queue->ctrl_status_wait,
 479                         !list_empty_careful(&queue->ctrl_status_q),
 480                         timeout);
 481         if (list_empty_careful(&queue->ctrl_status_q)) {
 482                 if (!rc)
 483                         rc = -ETIMEDOUT;
 484                 if (rc < 0) {
 485                         pr_err("msm_control: wait_event error %d\n", rc);
 486 #if 0
 487                         /* This is a bit scary.  If we time out too early, we
 488                          * will free qcmd at the end of this function, and the
 489                          * dsp may do the same when it does respond, so we
 490                          * remove the message from the source queue.
 491                          */
 492                         pr_err("%s: error waiting for ctrl_status_q: %d\n",
 493                                 __func__, rc);
 494                         spin_lock_irqsave(&sync->msg_event_q_lock, flags);
 495                         list_del_init(&qcmd->list);
 496                         spin_unlock_irqrestore(&sync->msg_event_q_lock, flags);
 497 #endif
 498                         return ERR_PTR(rc);
 499                 }
 500         }
 501
 502         /* control command status is ready */
 503         spin_lock_irqsave(&queue->ctrl_status_q_lock, flags);
 504         BUG_ON(list_empty(&queue->ctrl_status_q));
 505         qcmd = list_first_entry(&queue->ctrl_status_q,
 506                         struct msm_queue_cmd, list);
 507         list_del_init(&qcmd->list);
 508         spin_unlock_irqrestore(&queue->ctrl_status_q_lock, flags);
 509
 510         return qcmd;
 511 }
 512
 513 static int msm_control(struct msm_control_device *ctrl_pmsm,
 514                         int block,
 515                         void __user *arg)
 516 {
 517         int rc = 0;
 518
 519         struct msm_sync *sync = ctrl_pmsm->pmsm->sync;
 520         struct msm_ctrl_cmd udata, *ctrlcmd;
 521         struct msm_queue_cmd *qcmd = NULL, *qcmd_temp;
 522
 523         if (copy_from_user(&udata, arg, sizeof(struct msm_ctrl_cmd))) {
 524                 ERR_COPY_FROM_USER();
 525                 rc = -EFAULT;
 526                 goto end;
 527         }
 528
 529         qcmd = kmalloc(sizeof(struct msm_queue_cmd) +
 530                                 sizeof(struct msm_ctrl_cmd) + udata.length,
 531                                 GFP_KERNEL);
 532         if (!qcmd) {
 533                 pr_err("msm_control: cannot allocate buffer\n");
 534                 rc = -ENOMEM;
 535                 goto end;
 536         }
 537
 538         qcmd->type = MSM_CAM_Q_CTRL;
 539         qcmd->command = ctrlcmd = (struct msm_ctrl_cmd *)(qcmd + 1);
 540         *ctrlcmd = udata;
 541         ctrlcmd->value = ctrlcmd + 1;
 542
 543         if (udata.length) {
 544                 if (copy_from_user(ctrlcmd->value,
 545                                 udata.value, udata.length)) {
 546                         ERR_COPY_FROM_USER();
 547                         rc = -EFAULT;
 548                         goto end;
 549                 }
 550         }
 551
 552         if (!block) {
 553                 /* qcmd will be set to NULL */
 554                 qcmd = __msm_control(sync, NULL, qcmd, 0);
 555                 goto end;
 556         }
 557
 558         qcmd_temp = __msm_control(sync,
 559                                   &ctrl_pmsm->ctrl_q,
 560                                   qcmd, MAX_SCHEDULE_TIMEOUT);
 561
 562         if (IS_ERR(qcmd_temp)) {
 563                 rc = PTR_ERR(qcmd_temp);
 564                 goto end;
 565         }
 566         qcmd = qcmd_temp;
 567
 568         if (qcmd->command) {
 569                 void __user *to = udata.value;
 570                 udata = *(struct msm_ctrl_cmd *)qcmd->command;
 571                 if (udata.length > 0) {
 572                         if (copy_to_user(to,
 573                                          udata.value,
 574                                          udata.length)) {
 575                                 ERR_COPY_TO_USER();
 576                                 rc = -EFAULT;
 577                                 goto end;
 578                         }
 579                 }
 580                 udata.value = to;
 581
 582                 if (copy_to_user((void *)arg, &udata,
 583                                 sizeof(struct msm_ctrl_cmd))) {
 584                         ERR_COPY_TO_USER();
 585                         rc = -EFAULT;
 586                         goto end;
 587                 }
 588         }
 589
 590 end:
 591         /* Note: if we get here as a result of an error, we will free the
 592          * qcmd that we kmalloc() in this function.  When we come here as
 593          * a result of a successful completion, we are freeing the qcmd that
 594          * we dequeued from queue->ctrl_status_q.
 595          */
 596         kfree(qcmd);
 597
 598         CDBG("msm_control: end rc = %d\n", rc);
 599         return rc;
 600 }
 601
 602 static int msm_get_stats(struct msm_sync *sync, void __user *arg)
 603 {
 604         unsigned long flags;
 605         int timeout;
 606         int rc = 0;
 607
 608         struct msm_stats_event_ctrl se;
 609
 610         struct msm_queue_cmd *qcmd = NULL;
 611         struct msm_ctrl_cmd  *ctrl = NULL;
 612         struct msm_vfe_resp  *data = NULL;
 613         struct msm_stats_buf stats;
 614
 615         if (copy_from_user(&se, arg,
 616                         sizeof(struct msm_stats_event_ctrl))) {
 617                 ERR_COPY_FROM_USER();
 618                 return -EFAULT;
 619         }
 620
 621         timeout = (int)se.timeout_ms;
 622
 623         CDBG("msm_get_stats timeout %d\n", timeout);
 624         rc = wait_event_interruptible_timeout(
 625                         sync->msg_event_wait,
 626                         !list_empty_careful(&sync->msg_event_q),
 627                         msecs_to_jiffies(timeout));
 628         if (list_empty_careful(&sync->msg_event_q)) {
 629                 if (rc == 0)
 630                         rc = -ETIMEDOUT;
 631                 if (rc < 0) {
 632                         pr_err("msm_get_stats error %d\n", rc);
 633                         return rc;
 634                 }
 635         }
 636         CDBG("msm_get_stats returned from wait: %d\n", rc);
 637
 638         spin_lock_irqsave(&sync->msg_event_q_lock, flags);
 639         BUG_ON(list_empty(&sync->msg_event_q));
 640         qcmd = list_first_entry(&sync->msg_event_q,
 641                         struct msm_queue_cmd, list);
 642         list_del_init(&qcmd->list);
 643         spin_unlock_irqrestore(&sync->msg_event_q_lock, flags);
 644
 645         CDBG("=== received from DSP === %d\n", qcmd->type);
 646
 647         switch (qcmd->type) {
 648         case MSM_CAM_Q_VFE_EVT:
 649         case MSM_CAM_Q_VFE_MSG:
 650                 data = (struct msm_vfe_resp *)(qcmd->command);
 651
 652                 /* adsp event and message */
 653                 se.resptype = MSM_CAM_RESP_STAT_EVT_MSG;
 654
 655                 /* 0 - msg from aDSP, 1 - event from mARM */
 656                 se.stats_event.type   = data->evt_msg.type;
 657                 se.stats_event.msg_id = data->evt_msg.msg_id;
 658                 se.stats_event.len    = data->evt_msg.len;
 659
 660                 CDBG("msm_get_stats, qcmd->type = %d\n", qcmd->type);
 661                 CDBG("length = %d\n", se.stats_event.len);
 662                 CDBG("msg_id = %d\n", se.stats_event.msg_id);
 663
 664                 if ((data->type == VFE_MSG_STATS_AF) ||
 665                                 (data->type == VFE_MSG_STATS_WE)) {
 666
 667                         stats.buffer =
 668                         msm_pmem_stats_ptov_lookup(sync,
 669                                         data->phy.sbuf_phy,
 670                                         &(stats.fd));
 671                         if (!stats.buffer) {
 672                                 pr_err("%s: msm_pmem_stats_ptov_lookup error\n",
 673                                         __func__);
 674                                 rc = -EINVAL;
 675                                 goto failure;
 676                         }
 677
 678                         if (copy_to_user((void *)(se.stats_event.data),
 679                                         &stats,
 680                                         sizeof(struct msm_stats_buf))) {
 681                                 ERR_COPY_TO_USER();
 682                                 rc = -EFAULT;
 683                                 goto failure;
 684                         }
 685                 } else if ((data->evt_msg.len > 0) &&
 686                                 (data->type == VFE_MSG_GENERAL)) {
 687                         if (copy_to_user((void *)(se.stats_event.data),
 688                                         data->evt_msg.data,
 689                                         data->evt_msg.len)) {
 690                                 ERR_COPY_TO_USER();
 691                                 rc = -EFAULT;
 692                         }
 693                 } else if (data->type == VFE_MSG_OUTPUT1 ||
 694                         data->type == VFE_MSG_OUTPUT2) {
 695                         if (copy_to_user((void *)(se.stats_event.data),
 696                                         data->extdata,
 697                                         data->extlen)) {
 698                                 ERR_COPY_TO_USER();
 699                                 rc = -EFAULT;
 700                         }
 701                 } else if (data->type == VFE_MSG_SNAPSHOT && sync->pict_pp) {
 702                         struct msm_postproc buf;
 703                         struct msm_pmem_region region;
 704                         buf.fmnum = msm_pmem_region_lookup(&sync->frame,
 705                                         MSM_PMEM_MAINIMG,
 706                                         &region, 1);
 707                         if (buf.fmnum == 1) {
 708                                 buf.fmain.buffer = (unsigned long)region.vaddr;
 709                                 buf.fmain.y_off  = region.y_off;
 710                                 buf.fmain.cbcr_off = region.cbcr_off;
 711                                 buf.fmain.fd = region.fd;
 712                         } else {
 713                                 buf.fmnum = msm_pmem_region_lookup(&sync->frame,
 714                                                 MSM_PMEM_RAW_MAINIMG,
 715                                                 &region, 1);
 716                                 if (buf.fmnum == 1) {
 717                                         buf.fmain.path = MSM_FRAME_PREV_2;
 718                                         buf.fmain.buffer =
 719                                                 (unsigned long)region.vaddr;
 720                                         buf.fmain.fd = region.fd;
 721                                 } else {
 722                                         pr_err("%s: pmem lookup failed\n",
 723                                                 __func__);
 724                                         rc = -EINVAL;
 725                                 }
 726                         }
 727
 728                         if (copy_to_user((void *)(se.stats_event.data), &buf,
 729                                         sizeof(buf))) {
 730                                 ERR_COPY_TO_USER();
 731                                 rc = -EFAULT;
 732                                 goto failure;
 733                         }
 734                         CDBG("snapshot copy_to_user!\n");
 735                 }
 736                 break;
 737
 738         case MSM_CAM_Q_CTRL:
 739                 /* control command from control thread */
 740                 ctrl = (struct msm_ctrl_cmd *)(qcmd->command);
 741
 742                 CDBG("msm_get_stats, qcmd->type = %d\n", qcmd->type);
 743                 CDBG("length = %d\n", ctrl->length);
 744
 745                 if (ctrl->length > 0) {
 746                         if (copy_to_user((void *)(se.ctrl_cmd.value),
 747                                                 ctrl->value,
 748                                                 ctrl->length)) {
 749                                 ERR_COPY_TO_USER();
 750                                 rc = -EFAULT;
 751                                 goto failure;
 752                         }
 753                 }
 754
 755                 se.resptype = MSM_CAM_RESP_CTRL;
 756
 757                 /* what to control */
 758                 se.ctrl_cmd.type = ctrl->type;
 759                 se.ctrl_cmd.length = ctrl->length;
 760                 se.ctrl_cmd.resp_fd = ctrl->resp_fd;
 761                 break;
 762
 763         case MSM_CAM_Q_V4L2_REQ:
 764                 /* control command from v4l2 client */
 765                 ctrl = (struct msm_ctrl_cmd *)(qcmd->command);
 766
 767                 CDBG("msm_get_stats, qcmd->type = %d\n", qcmd->type);
 768                 CDBG("length = %d\n", ctrl->length);
 769
 770                 if (ctrl->length > 0) {
 771                         if (copy_to_user((void *)(se.ctrl_cmd.value),
 772                                         ctrl->value, ctrl->length)) {
 773                                 ERR_COPY_TO_USER();
 774                                 rc = -EFAULT;
 775                                 goto failure;
 776                         }
 777                 }
 778
 779                 /* 2 tells config thread this is v4l2 request */
 780                 se.resptype = MSM_CAM_RESP_V4L2;
 781
 782                 /* what to control */
 783                 se.ctrl_cmd.type   = ctrl->type;
 784                 se.ctrl_cmd.length = ctrl->length;
 785                 break;
 786
 787         default:
 788                 rc = -EFAULT;
 789                 goto failure;
 790         } /* switch qcmd->type */
 791
 792         if (copy_to_user((void *)arg, &se, sizeof(se))) {
 793                 ERR_COPY_TO_USER();
 794                 rc = -EFAULT;
 795         }
 796
 797 failure:
 798         kfree(qcmd);
 799
 800         CDBG("msm_get_stats: %d\n", rc);
 801         return rc;
 802 }
 803
 804 static int msm_ctrl_cmd_done(struct msm_control_device *ctrl_pmsm,
 805                 void __user *arg)
 806 {
 807         unsigned long flags;
 808         int rc = 0;
 809
 810         struct msm_ctrl_cmd udata, *ctrlcmd;
 811         struct msm_queue_cmd *qcmd = NULL;
 812
 813         if (copy_from_user(&udata, arg, sizeof(struct msm_ctrl_cmd))) {
 814                 ERR_COPY_FROM_USER();
 815                 rc = -EFAULT;
 816                 goto end;
 817         }
 818
 819         qcmd = kmalloc(sizeof(struct msm_queue_cmd) +
 820                         sizeof(struct msm_ctrl_cmd) + udata.length,
 821                         GFP_KERNEL);
 822         if (!qcmd) {
 823                 rc = -ENOMEM;
 824                 goto end;
 825         }
 826
 827         qcmd->command = ctrlcmd = (struct msm_ctrl_cmd *)(qcmd + 1);
 828         *ctrlcmd = udata;
 829         if (udata.length > 0) {
 830                 ctrlcmd->value = ctrlcmd + 1;
 831                 if (copy_from_user(ctrlcmd->value,
 832                                         (void *)udata.value,
 833                                         udata.length)) {
 834                         ERR_COPY_FROM_USER();
 835                         rc = -EFAULT;
 836                         kfree(qcmd);
 837                         goto end;
 838                 }
 839         } else
 840                 ctrlcmd->value = NULL;
 841
 842 end:
 843         CDBG("msm_ctrl_cmd_done: end rc = %d\n", rc);
 844         if (rc == 0) {
 845                 /* wake up control thread */
 846                 spin_lock_irqsave(&ctrl_pmsm->ctrl_q.ctrl_status_q_lock, flags);
 847                 list_add_tail(&qcmd->list, &ctrl_pmsm->ctrl_q.ctrl_status_q);
 848                 wake_up(&ctrl_pmsm->ctrl_q.ctrl_status_wait);
 849                 spin_unlock_irqrestore(&ctrl_pmsm->ctrl_q.ctrl_status_q_lock, flags);
 850         }
 851
 852         return rc;
 853 }
 854
 855 static int msm_config_vfe(struct msm_sync *sync, void __user *arg)
 856 {
 857         struct msm_vfe_cfg_cmd cfgcmd;
 858         struct msm_pmem_region region[8];
 859         struct axidata axi_data;
 860         void *data = NULL;
 861         int rc = -EIO;
 862
 863         memset(&axi_data, 0, sizeof(axi_data));
 864
 865         if (copy_from_user(&cfgcmd, arg, sizeof(cfgcmd))) {
 866                 ERR_COPY_FROM_USER();
 867                 return -EFAULT;
 868         }
 869
 870         switch (cfgcmd.cmd_type) {
 871         case CMD_STATS_ENABLE:
 872                 axi_data.bufnum1 =
 873                         msm_pmem_region_lookup(&sync->stats,
 874                                         MSM_PMEM_AEC_AWB, &region[0],
 875                                         NUM_WB_EXP_STAT_OUTPUT_BUFFERS);
 876                 if (!axi_data.bufnum1) {
 877                         pr_err("%s: pmem region lookup error\n", __func__);
 878                         return -EINVAL;
 879                 }
 880                 axi_data.region = &region[0];
 881                 data = &axi_data;
 882                 break;
 883         case CMD_STATS_AF_ENABLE:
 884                 axi_data.bufnum1 =
 885                         msm_pmem_region_lookup(&sync->stats,
 886                                         MSM_PMEM_AF, &region[0],
 887                                         NUM_AF_STAT_OUTPUT_BUFFERS);
 888                 if (!axi_data.bufnum1) {
 889                         pr_err("%s: pmem region lookup error\n", __func__);
 890                         return -EINVAL;
 891                 }
 892                 axi_data.region = &region[0];
 893                 data = &axi_data;
 894                 break;
 895         case CMD_GENERAL:
 896         case CMD_STATS_DISABLE:
 897                 break;
 898         default:
 899                 pr_err("%s: unknown command type %d\n",
 900                         __func__, cfgcmd.cmd_type);
 901                 return -EINVAL;
 902         }
 903
 904
 905         if (sync->vfefn.vfe_config)
 906                 rc = sync->vfefn.vfe_config(&cfgcmd, data);
 907
 908         return rc;
 909 }
 910
 911 static int msm_frame_axi_cfg(struct msm_sync *sync,
 912                 struct msm_vfe_cfg_cmd *cfgcmd)
 913 {
 914         int rc = -EIO;
 915         struct axidata axi_data;
 916         void *data = &axi_data;
 917         struct msm_pmem_region region[8];
 918         int pmem_type;
 919
 920         memset(&axi_data, 0, sizeof(axi_data));
 921
 922         switch (cfgcmd->cmd_type) {
 923         case CMD_AXI_CFG_OUT1:
 924                 pmem_type = MSM_PMEM_OUTPUT1;
 925                 axi_data.bufnum1 =
 926                         msm_pmem_region_lookup(&sync->frame, pmem_type,
 927                                 &region[0], 8);
 928                 if (!axi_data.bufnum1) {
 929                         pr_err("%s: pmem region lookup error\n", __func__);
 930                         return -EINVAL;
 931                 }
 932                 break;
 933
 934         case CMD_AXI_CFG_OUT2:
 935                 pmem_type = MSM_PMEM_OUTPUT2;
 936                 axi_data.bufnum2 =
 937                         msm_pmem_region_lookup(&sync->frame, pmem_type,
 938                                 &region[0], 8);
 939                 if (!axi_data.bufnum2) {
 940                         pr_err("%s: pmem region lookup error\n", __func__);
 941                         return -EINVAL;
 942                 }
 943                 break;
 944
 945         case CMD_AXI_CFG_SNAP_O1_AND_O2:
 946                 pmem_type = MSM_PMEM_THUMBAIL;
 947                 axi_data.bufnum1 =
 948                         msm_pmem_region_lookup(&sync->frame, pmem_type,
 949                                 &region[0], 8);
 950                 if (!axi_data.bufnum1) {
 951                         pr_err("%s: pmem region lookup error\n", __func__);
 952                         return -EINVAL;
 953                 }
 954
 955                 pmem_type = MSM_PMEM_MAINIMG;
 956                 axi_data.bufnum2 =
 957                         msm_pmem_region_lookup(&sync->frame, pmem_type,
 958                                 &region[axi_data.bufnum1], 8);
 959                 if (!axi_data.bufnum2) {
 960                         pr_err("%s: pmem region lookup error\n", __func__);
 961                         return -EINVAL;
 962                 }
 963                 break;
 964
 965         case CMD_RAW_PICT_AXI_CFG:
 966                 pmem_type = MSM_PMEM_RAW_MAINIMG;
 967                 axi_data.bufnum2 =
 968                         msm_pmem_region_lookup(&sync->frame, pmem_type,
 969                                 &region[0], 8);
 970                 if (!axi_data.bufnum2) {
 971                         pr_err("%s: pmem region lookup error\n", __func__);
 972                         return -EINVAL;
 973                 }
 974                 break;
 975
 976         case CMD_GENERAL:
 977                 data = NULL;
 978                 break;
 979
 980         default:
 981                 pr_err("%s: unknown command type %d\n",
 982                         __func__, cfgcmd->cmd_type);
 983                 return -EINVAL;
 984         }
 985
 986         axi_data.region = &region[0];
 987
 988         /* send the AXI configuration command to driver */
 989         if (sync->vfefn.vfe_config)
 990                 rc = sync->vfefn.vfe_config(cfgcmd, data);
 991
 992         return rc;
 993 }
 994
 995 static int msm_get_sensor_info(struct msm_sync *sync, void __user *arg)
 996 {
 997         int rc = 0;
 998         struct msm_camsensor_info info;
 999         struct msm_camera_sensor_info *sdata;
1000
1001         if (copy_from_user(&info,
1002                         arg,
1003                         sizeof(struct msm_camsensor_info))) {
1004                 ERR_COPY_FROM_USER();
1005                 return -EFAULT;
1006         }
1007
1008         sdata = sync->pdev->dev.platform_data;
1009         CDBG("sensor_name %s\n", sdata->sensor_name);
1010
1011         memcpy(&info.name[0],
1012                 sdata->sensor_name,
1013                 MAX_SENSOR_NAME);
1014         info.flash_enabled = sdata->flash_type != MSM_CAMERA_FLASH_NONE;
1015
1016         /* copy back to user space */
1017         if (copy_to_user((void *)arg,
1018                         &info,
1019                         sizeof(struct msm_camsensor_info))) {
1020                 ERR_COPY_TO_USER();
1021                 rc = -EFAULT;
1022         }
1023
1024         return rc;
1025 }
1026
1027 static int __msm_put_frame_buf(struct msm_sync *sync,
1028                 struct msm_frame *pb)
1029 {
1030         unsigned long pphy;
1031         struct msm_vfe_cfg_cmd cfgcmd;
1032
1033         int rc = -EIO;
1034
1035         pphy = msm_pmem_frame_vtop_lookup(sync,
1036                 pb->buffer,
1037                 pb->y_off, pb->cbcr_off, pb->fd);
1038
1039         if (pphy != 0) {
1040                 CDBG("rel: vaddr = 0x%lx, paddr = 0x%lx\n",
1041                         pb->buffer, pphy);
1042                 cfgcmd.cmd_type = CMD_FRAME_BUF_RELEASE;
1043                 cfgcmd.value    = (void *)pb;
1044                 if (sync->vfefn.vfe_config)
1045                         rc = sync->vfefn.vfe_config(&cfgcmd, &pphy);
1046         } else {
1047                 pr_err("%s: msm_pmem_frame_vtop_lookup failed\n",
1048                         __func__);
1049                 rc = -EINVAL;
1050         }
1051
1052         return rc;
1053 }
1054
1055 static int msm_put_frame_buffer(struct msm_sync *sync, void __user *arg)
1056 {
1057         struct msm_frame buf_t;
1058
1059         if (copy_from_user(&buf_t,
1060                                 arg,
1061                                 sizeof(struct msm_frame))) {
1062                 ERR_COPY_FROM_USER();
1063                 return -EFAULT;
1064         }
1065
1066         return __msm_put_frame_buf(sync, &buf_t);
1067 }
1068
1069 static int __msm_register_pmem(struct msm_sync *sync,
1070                 struct msm_pmem_info *pinfo)
1071 {
1072         int rc = 0;
1073
1074         switch (pinfo->type) {
1075         case MSM_PMEM_OUTPUT1:
1076         case MSM_PMEM_OUTPUT2:
1077         case MSM_PMEM_THUMBAIL:
1078         case MSM_PMEM_MAINIMG:
1079         case MSM_PMEM_RAW_MAINIMG:
1080                 rc = msm_pmem_table_add(&sync->frame, pinfo);
1081                 break;
1082
1083         case MSM_PMEM_AEC_AWB:
1084         case MSM_PMEM_AF:
1085                 rc = msm_pmem_table_add(&sync->stats, pinfo);
1086                 break;
1087
1088         default:
1089                 rc = -EINVAL;
1090                 break;
1091         }
1092
1093         return rc;
1094 }
1095
1096 static int msm_register_pmem(struct msm_sync *sync, void __user *arg)
1097 {
1098         struct msm_pmem_info info;
1099
1100         if (copy_from_user(&info, arg, sizeof(info))) {
1101                 ERR_COPY_FROM_USER();
1102                 return -EFAULT;
1103         }
1104
1105         return __msm_register_pmem(sync, &info);
1106 }
1107
1108 static int msm_stats_axi_cfg(struct msm_sync *sync,
1109                 struct msm_vfe_cfg_cmd *cfgcmd)
1110 {
1111         int rc = -EIO;
1112         struct axidata axi_data;
1113         void *data = &axi_data;
1114
1115         struct msm_pmem_region region[3];
1116         int pmem_type = MSM_PMEM_MAX;
1117
1118         memset(&axi_data, 0, sizeof(axi_data));
1119
1120         switch (cfgcmd->cmd_type) {
1121         case CMD_STATS_AXI_CFG:
1122                 pmem_type = MSM_PMEM_AEC_AWB;
1123                 break;
1124         case CMD_STATS_AF_AXI_CFG:
1125                 pmem_type = MSM_PMEM_AF;
1126                 break;
1127         case CMD_GENERAL:
1128                 data = NULL;
1129                 break;
1130         default:
1131                 pr_err("%s: unknown command type %d\n",
1132                         __func__, cfgcmd->cmd_type);
1133                 return -EINVAL;
1134         }
1135
1136         if (cfgcmd->cmd_type != CMD_GENERAL) {
1137                 axi_data.bufnum1 =
1138                         msm_pmem_region_lookup(&sync->stats, pmem_type,
1139                                 &region[0], NUM_WB_EXP_STAT_OUTPUT_BUFFERS);
1140                 if (!axi_data.bufnum1) {
1141                         pr_err("%s: pmem region lookup error\n", __func__);
1142                         return -EINVAL;
1143                 }
1144                 axi_data.region = &region[0];
1145         }
1146
1147         /* send the AEC/AWB STATS configuration command to driver */
1148         if (sync->vfefn.vfe_config)
1149                 rc = sync->vfefn.vfe_config(cfgcmd, &axi_data);
1150
1151         return rc;
1152 }
1153
1154 static int msm_put_stats_buffer(struct msm_sync *sync, void __user *arg)
1155 {
1156         int rc = -EIO;
1157
1158         struct msm_stats_buf buf;
1159         unsigned long pphy;
1160         struct msm_vfe_cfg_cmd cfgcmd;
1161
1162         if (copy_from_user(&buf, arg,
1163                                 sizeof(struct msm_stats_buf))) {
1164                 ERR_COPY_FROM_USER();
1165                 return -EFAULT;
1166         }
1167
1168         CDBG("msm_put_stats_buffer\n");
1169         pphy = msm_pmem_stats_vtop_lookup(sync, buf.buffer, buf.fd);
1170
1171         if (pphy != 0) {
1172                 if (buf.type == STAT_AEAW)
1173                         cfgcmd.cmd_type = CMD_STATS_BUF_RELEASE;
1174                 else if (buf.type == STAT_AF)
1175                         cfgcmd.cmd_type = CMD_STATS_AF_BUF_RELEASE;
1176                 else {
1177                         pr_err("%s: invalid buf type %d\n",
1178                                 __func__,
1179                                 buf.type);
1180                         rc = -EINVAL;
1181                         goto put_done;
1182                 }
1183
1184                 cfgcmd.value = (void *)&buf;
1185
1186                 if (sync->vfefn.vfe_config) {
1187                         rc = sync->vfefn.vfe_config(&cfgcmd, &pphy);
1188                         if (rc < 0)
1189                                 pr_err("msm_put_stats_buffer: "\
1190                                         "vfe_config err %d\n", rc);
1191                 } else
1192                         pr_err("msm_put_stats_buffer: vfe_config is NULL\n");
1193         } else {
1194                 pr_err("msm_put_stats_buffer: NULL physical address\n");
1195                 rc = -EINVAL;
1196         }
1197
1198 put_done:
1199         return rc;
1200 }
1201
1202 static int msm_axi_config(struct msm_sync *sync, void __user *arg)
1203 {
1204         struct msm_vfe_cfg_cmd cfgcmd;
1205
1206         if (copy_from_user(&cfgcmd, arg, sizeof(cfgcmd))) {
1207                 ERR_COPY_FROM_USER();
1208                 return -EFAULT;
1209         }
1210
1211         switch (cfgcmd.cmd_type) {
1212         case CMD_AXI_CFG_OUT1:
1213         case CMD_AXI_CFG_OUT2:
1214         case CMD_AXI_CFG_SNAP_O1_AND_O2:
1215         case CMD_RAW_PICT_AXI_CFG:
1216                 return msm_frame_axi_cfg(sync, &cfgcmd);
1217
1218         case CMD_STATS_AXI_CFG:
1219         case CMD_STATS_AF_AXI_CFG:
1220                 return msm_stats_axi_cfg(sync, &cfgcmd);
1221
1222         default:
1223                 pr_err("%s: unknown command type %d\n",
1224                         __func__,
1225                         cfgcmd.cmd_type);
1226                 return -EINVAL;
1227         }
1228
1229         return 0;
1230 }
1231
1232 static int __msm_get_pic(struct msm_sync *sync, struct msm_ctrl_cmd *ctrl)
1233 {
1234         unsigned long flags;
1235         int rc = 0;
1236         int tm;
1237
1238         struct msm_queue_cmd *qcmd = NULL;
1239
1240         tm = (int)ctrl->timeout_ms;
1241
1242         rc = wait_event_interruptible_timeout(
1243                         sync->pict_frame_wait,
1244                         !list_empty_careful(&sync->pict_frame_q),
1245                         msecs_to_jiffies(tm));
1246         if (list_empty_careful(&sync->pict_frame_q)) {
1247                 if (rc == 0)
1248                         return -ETIMEDOUT;
1249                 if (rc < 0) {
1250                         pr_err("msm_camera_get_picture, rc = %d\n", rc);
1251                         return rc;
1252                 }
1253         }
1254
1255         spin_lock_irqsave(&sync->pict_frame_q_lock, flags);
1256         BUG_ON(list_empty(&sync->pict_frame_q));
1257         qcmd = list_first_entry(&sync->pict_frame_q,
1258                         struct msm_queue_cmd, list);
1259         list_del_init(&qcmd->list);
1260         spin_unlock_irqrestore(&sync->pict_frame_q_lock, flags);
1261
1262         if (qcmd->command != NULL) {
1263                 struct msm_ctrl_cmd *q =
1264                         (struct msm_ctrl_cmd *)qcmd->command;
1265                 ctrl->type = q->type;
1266                 ctrl->status = q->status;
1267         } else {
1268                 ctrl->type = -1;
1269                 ctrl->status = -1;
1270         }
1271
1272         kfree(qcmd);
1273         return rc;
1274 }
1275
1276 static int msm_get_pic(struct msm_sync *sync, void __user *arg)
1277 {
1278         struct msm_ctrl_cmd ctrlcmd_t;
1279         int rc;
1280
1281         if (copy_from_user(&ctrlcmd_t,
1282                                 arg,
1283                                 sizeof(struct msm_ctrl_cmd))) {
1284                 ERR_COPY_FROM_USER();
1285                 return -EFAULT;
1286         }
1287
1288         rc = __msm_get_pic(sync, &ctrlcmd_t);
1289         if (rc < 0)
1290                 return rc;
1291
1292         if (sync->croplen) {
1293                 if (ctrlcmd_t.length < sync->croplen) {
1294                         pr_err("msm_get_pic: invalid len %d\n",
1295                                 ctrlcmd_t.length);
1296                         return -EINVAL;
1297                 }
1298                 if (copy_to_user(ctrlcmd_t.value,
1299                                 sync->cropinfo,
1300                                 sync->croplen)) {
1301                         ERR_COPY_TO_USER();
1302                         return -EFAULT;
1303                 }
1304         }
1305
1306         if (copy_to_user((void *)arg,
1307                 &ctrlcmd_t,
1308                 sizeof(struct msm_ctrl_cmd))) {
1309                 ERR_COPY_TO_USER();
1310                 return -EFAULT;
1311         }
1312         return 0;
1313 }
1314
1315 static int msm_set_crop(struct msm_sync *sync, void __user *arg)
1316 {
1317         struct crop_info crop;
1318
1319         if (copy_from_user(&crop,
1320                                 arg,
1321                                 sizeof(struct crop_info))) {
1322                 ERR_COPY_FROM_USER();
1323                 return -EFAULT;
1324         }
1325
1326         if (!sync->croplen) {
1327                 sync->cropinfo = kmalloc(crop.len, GFP_KERNEL);
1328                 if (!sync->cropinfo)
1329                         return -ENOMEM;
1330         } else if (sync->croplen < crop.len)
1331                 return -EINVAL;
1332
1333         if (copy_from_user(sync->cropinfo,
1334                                 crop.info,
1335                                 crop.len)) {
1336                 ERR_COPY_FROM_USER();
1337                 kfree(sync->cropinfo);
1338                 return -EFAULT;
1339         }
1340
1341         sync->croplen = crop.len;
1342
1343         return 0;
1344 }
1345
1346 static int msm_pict_pp_done(struct msm_sync *sync, void __user *arg)
1347 {
1348         struct msm_ctrl_cmd udata;
1349         struct msm_ctrl_cmd *ctrlcmd = NULL;
1350         struct msm_queue_cmd *qcmd = NULL;
1351         unsigned long flags;
1352         int rc = 0;
1353
1354         if (!sync->pict_pp)
1355                 return -EINVAL;
1356
1357         if (copy_from_user(&udata, arg, sizeof(struct msm_ctrl_cmd))) {
1358                 ERR_COPY_FROM_USER();
1359                 rc = -EFAULT;
1360                 goto pp_fail;
1361         }
1362
1363         qcmd = kmalloc(sizeof(struct msm_queue_cmd) +
1364                         sizeof(struct msm_ctrl_cmd),
1365                         GFP_KERNEL);
1366         if (!qcmd) {
1367                 rc = -ENOMEM;
1368                 goto pp_fail;
1369         }
1370
1371         qcmd->type = MSM_CAM_Q_VFE_MSG;
1372         qcmd->command = ctrlcmd = (struct msm_ctrl_cmd *)(qcmd + 1);
1373         memset(ctrlcmd, 0, sizeof(struct msm_ctrl_cmd));
1374         ctrlcmd->type = udata.type;
1375         ctrlcmd->status = udata.status;
1376
1377         spin_lock_irqsave(&sync->pict_frame_q_lock, flags);
1378         list_add_tail(&qcmd->list, &sync->pict_frame_q);
1379         spin_unlock_irqrestore(&sync->pict_frame_q_lock, flags);
1380         wake_up(&sync->pict_frame_wait);
1381
1382 pp_fail:
1383         return rc;
1384 }
1385
1386 static long msm_ioctl_common(struct msm_device *pmsm,
1387                 unsigned int cmd,
1388                 void __user *argp)
1389 {
1390         CDBG("msm_ioctl_common\n");
1391         switch (cmd) {
1392         case MSM_CAM_IOCTL_REGISTER_PMEM:
1393                 return msm_register_pmem(pmsm->sync, argp);
1394         case MSM_CAM_IOCTL_UNREGISTER_PMEM:
1395                 return msm_pmem_table_del(pmsm->sync, argp);
1396         default:
1397                 return -EINVAL;
1398         }
1399 }
1400
1401 static long msm_ioctl_config(struct file *filep, unsigned int cmd,
1402         unsigned long arg)
1403 {
1404         int rc = -EINVAL;
1405         void __user *argp = (void __user *)arg;
1406         struct msm_device *pmsm = filep->private_data;
1407
1408         CDBG("msm_ioctl_config cmd = %d\n", _IOC_NR(cmd));
1409
1410         switch (cmd) {
1411         case MSM_CAM_IOCTL_GET_SENSOR_INFO:
1412                 rc = msm_get_sensor_info(pmsm->sync, argp);
1413                 break;
1414
1415         case MSM_CAM_IOCTL_CONFIG_VFE:
1416                 /* Coming from config thread for update */
1417                 rc = msm_config_vfe(pmsm->sync, argp);
1418                 break;
1419
1420         case MSM_CAM_IOCTL_GET_STATS:
1421                 /* Coming from config thread wait
1422                  * for vfe statistics and control requests */
1423                 rc = msm_get_stats(pmsm->sync, argp);
1424                 break;
1425
1426         case MSM_CAM_IOCTL_ENABLE_VFE:
1427                 /* This request comes from control thread:
1428                  * enable either QCAMTASK or VFETASK */
1429                 rc = msm_enable_vfe(pmsm->sync, argp);
1430                 break;
1431
1432         case MSM_CAM_IOCTL_DISABLE_VFE:
1433                 /* This request comes from control thread:
1434                  * disable either QCAMTASK or VFETASK */
1435                 rc = msm_disable_vfe(pmsm->sync, argp);
1436                 break;
1437
1438         case MSM_CAM_IOCTL_VFE_APPS_RESET:
1439                 msm_camio_vfe_blk_reset();
1440                 rc = 0;
1441                 break;
1442
1443         case MSM_CAM_IOCTL_RELEASE_STATS_BUFFER:
1444                 rc = msm_put_stats_buffer(pmsm->sync, argp);
1445                 break;
1446
1447         case MSM_CAM_IOCTL_AXI_CONFIG:
1448                 rc = msm_axi_config(pmsm->sync, argp);
1449                 break;
1450
1451         case MSM_CAM_IOCTL_SET_CROP:
1452                 rc = msm_set_crop(pmsm->sync, argp);
1453                 break;
1454
1455         case MSM_CAM_IOCTL_PICT_PP: {
1456                 uint8_t enable;
1457                 if (copy_from_user(&enable, argp, sizeof(enable))) {
1458                         ERR_COPY_FROM_USER();
1459                         rc = -EFAULT;
1460                 } else {
1461                         pmsm->sync->pict_pp = enable;
1462                         rc = 0;
1463                 }
1464                 break;
1465         }
1466
1467         case MSM_CAM_IOCTL_PICT_PP_DONE:
1468                 rc = msm_pict_pp_done(pmsm->sync, argp);
1469                 break;
1470
1471         case MSM_CAM_IOCTL_SENSOR_IO_CFG:
1472                 rc = pmsm->sync->sctrl.s_config(argp);
1473                 break;
1474
1475         case MSM_CAM_IOCTL_FLASH_LED_CFG: {
1476                 uint32_t led_state;
1477                 if (copy_from_user(&led_state, argp, sizeof(led_state))) {
1478                         ERR_COPY_FROM_USER();
1479                         rc = -EFAULT;
1480                 } else
1481                         rc = msm_camera_flash_set_led_state(led_state);
1482                 break;
1483         }
1484
1485         default:
1486                 rc = msm_ioctl_common(pmsm, cmd, argp);
1487                 break;
1488         }
1489
1490         CDBG("msm_ioctl_config cmd = %d DONE\n", _IOC_NR(cmd));
1491         return rc;
1492 }
1493
1494 static int msm_unblock_poll_frame(struct msm_sync *);
1495
1496 static long msm_ioctl_frame(struct file *filep, unsigned int cmd,
1497         unsigned long arg)
1498 {
1499         int rc = -EINVAL;
1500         void __user *argp = (void __user *)arg;
1501         struct msm_device *pmsm = filep->private_data;
1502
1503
1504         switch (cmd) {
1505         case MSM_CAM_IOCTL_GETFRAME:
1506                 /* Coming from frame thread to get frame
1507                  * after SELECT is done */
1508                 rc = msm_get_frame(pmsm->sync, argp);
1509                 break;
1510         case MSM_CAM_IOCTL_RELEASE_FRAME_BUFFER:
1511                 rc = msm_put_frame_buffer(pmsm->sync, argp);
1512                 break;
1513         case MSM_CAM_IOCTL_UNBLOCK_POLL_FRAME:
1514                 rc = msm_unblock_poll_frame(pmsm->sync);
1515                 break;
1516         default:
1517                 break;
1518         }
1519
1520         return rc;
1521 }
1522
1523
1524 static long msm_ioctl_control(struct file *filep, unsigned int cmd,
1525         unsigned long arg)
1526 {
1527         int rc = -EINVAL;
1528         void __user *argp = (void __user *)arg;
1529         struct msm_control_device *ctrl_pmsm = filep->private_data;
1530         struct msm_device *pmsm = ctrl_pmsm->pmsm;
1531
1532         switch (cmd) {
1533         case MSM_CAM_IOCTL_CTRL_COMMAND:
1534                 /* Coming from control thread, may need to wait for
1535                  * command status */
1536                 rc = msm_control(ctrl_pmsm, 1, argp);
1537                 break;
1538         case MSM_CAM_IOCTL_CTRL_COMMAND_2:
1539                 /* Sends a message, returns immediately */
1540                 rc = msm_control(ctrl_pmsm, 0, argp);
1541                 break;
1542         case MSM_CAM_IOCTL_CTRL_CMD_DONE:
1543                 /* Config thread calls the control thread to notify it
1544                  * of the result of a MSM_CAM_IOCTL_CTRL_COMMAND.
1545                  */
1546                 rc = msm_ctrl_cmd_done(ctrl_pmsm, argp);
1547                 break;
1548         case MSM_CAM_IOCTL_GET_PICTURE:
1549                 rc = msm_get_pic(pmsm->sync, argp);
1550                 break;
1551         default:
1552                 rc = msm_ioctl_common(pmsm, cmd, argp);
1553                 break;
1554         }
1555
1556         return rc;
1557 }
1558
1559 static int __msm_release(struct msm_sync *sync)
1560 {
1561         struct msm_pmem_region *region;
1562         struct hlist_node *hnode;
1563         struct hlist_node *n;
1564
1565         mutex_lock(&sync->lock);
1566         if (sync->opencnt)
1567                 sync->opencnt--;
1568
1569         if (!sync->opencnt) {
1570                 /* need to clean up system resource */
1571                 if (sync->vfefn.vfe_release)
1572                         sync->vfefn.vfe_release(sync->pdev);
1573
1574                 if (sync->cropinfo) {
1575                         kfree(sync->cropinfo);
1576                         sync->cropinfo = NULL;
1577                         sync->croplen = 0;
1578                 }
1579
1580                 hlist_for_each_entry_safe(region, hnode, n,
1581                                 &sync->frame, list) {
1582                         hlist_del(hnode);
1583                         put_pmem_file(region->file);
1584                         kfree(region);
1585                 }
1586
1587                 hlist_for_each_entry_safe(region, hnode, n,
1588                                 &sync->stats, list) {
1589                         hlist_del(hnode);
1590                         put_pmem_file(region->file);
1591                         kfree(region);
1592                 }
1593
1594                 MSM_DRAIN_QUEUE(sync, msg_event_q);
1595                 MSM_DRAIN_QUEUE(sync, prev_frame_q);
1596                 MSM_DRAIN_QUEUE(sync, pict_frame_q);
1597
1598                 sync->sctrl.s_release();
1599
1600                 sync->apps_id = NULL;
1601                 CDBG("msm_release completed!\n");
1602         }
1603         mutex_unlock(&sync->lock);
1604
1605         return 0;
1606 }
1607
1608 static int msm_release_config(struct inode *node, struct file *filep)
1609 {
1610         int rc;
1611         struct msm_device *pmsm = filep->private_data;
1612         printk("msm_camera: RELEASE %s\n", filep->f_path.dentry->d_name.name);
1613         rc = __msm_release(pmsm->sync);
1614         atomic_set(&pmsm->opened, 0);
1615         return rc;
1616 }
1617
1618 static int msm_release_control(struct inode *node, struct file *filep)
1619 {
1620         int rc;
1621         struct msm_control_device *ctrl_pmsm = filep->private_data;
1622         struct msm_device *pmsm = ctrl_pmsm->pmsm;
1623         printk(KERN_INFO "msm_camera: RELEASE %s\n",
1624                                         filep->f_path.dentry->d_name.name);
1625         rc = __msm_release(pmsm->sync);
1626         if (!rc) {
1627                 MSM_DRAIN_QUEUE(&ctrl_pmsm->ctrl_q, ctrl_status_q);
1628                 MSM_DRAIN_QUEUE(pmsm->sync, pict_frame_q);
1629         }
1630         kfree(ctrl_pmsm);
1631         return rc;
1632 }
1633
1634 static int msm_release_frame(struct inode *node, struct file *filep)
1635 {
1636         int rc;
1637         struct msm_device *pmsm = filep->private_data;
1638         printk(KERN_INFO "msm_camera: RELEASE %s\n",
1639                                         filep->f_path.dentry->d_name.name);
1640         rc = __msm_release(pmsm->sync);
1641         if (!rc) {
1642                 MSM_DRAIN_QUEUE(pmsm->sync, prev_frame_q);
1643                 atomic_set(&pmsm->opened, 0);
1644         }
1645         return rc;
1646 }
1647
1648 static int msm_unblock_poll_frame(struct msm_sync *sync)
1649 {
1650         unsigned long flags;
1651         CDBG("msm_unblock_poll_frame\n");
1652         spin_lock_irqsave(&sync->prev_frame_q_lock, flags);
1653         sync->unblock_poll_frame = 1;
1654         wake_up(&sync->prev_frame_wait);
1655         spin_unlock_irqrestore(&sync->prev_frame_q_lock, flags);
1656         return 0;
1657 }
1658
1659 static unsigned int __msm_poll_frame(struct msm_sync *sync,
1660                 struct file *filep,
1661                 struct poll_table_struct *pll_table)
1662 {
1663         int rc = 0;
1664         unsigned long flags;
1665
1666         poll_wait(filep, &sync->prev_frame_wait, pll_table);
1667
1668         spin_lock_irqsave(&sync->prev_frame_q_lock, flags);
1669         if (!list_empty_careful(&sync->prev_frame_q))
1670                 /* frame ready */
1671                 rc = POLLIN | POLLRDNORM;
1672         if (sync->unblock_poll_frame) {
1673                 CDBG("%s: sync->unblock_poll_frame is true\n", __func__);
1674                 rc |= POLLPRI;
1675                 sync->unblock_poll_frame = 0;
1676         }
1677         spin_unlock_irqrestore(&sync->prev_frame_q_lock, flags);
1678
1679         return rc;
1680 }
1681
1682 static unsigned int msm_poll_frame(struct file *filep,
1683         struct poll_table_struct *pll_table)
1684 {
1685         struct msm_device *pmsm = filep->private_data;
1686         return __msm_poll_frame(pmsm->sync, filep, pll_table);
1687 }
1688
1689 /*
1690  * This function executes in interrupt context.
1691  */
1692
1693 static void *msm_vfe_sync_alloc(int size,
1694                         void *syncdata __attribute__((unused)))
1695 {
1696         struct msm_queue_cmd *qcmd =
1697                 kmalloc(sizeof(struct msm_queue_cmd) + size, GFP_ATOMIC);
1698         return qcmd ? qcmd + 1 : NULL;
1699 }
1700
1701 /*
1702  * This function executes in interrupt context.
1703  */
1704
1705 static void msm_vfe_sync(struct msm_vfe_resp *vdata,
1706                 enum msm_queue qtype, void *syncdata)
1707 {
1708         struct msm_queue_cmd *qcmd = NULL;
1709         struct msm_queue_cmd *qcmd_frame = NULL;
1710         struct msm_vfe_phy_info *fphy;
1711
1712         unsigned long flags;
1713         struct msm_sync *sync = (struct msm_sync *)syncdata;
1714         if (!sync) {
1715                 pr_err("msm_camera: no context in dsp callback.\n");
1716                 return;
1717         }
1718
1719         qcmd = ((struct msm_queue_cmd *)vdata) - 1;
1720         qcmd->type = qtype;
1721
1722         if (qtype == MSM_CAM_Q_VFE_MSG) {
1723                 switch (vdata->type) {
1724                 case VFE_MSG_OUTPUT1:
1725                 case VFE_MSG_OUTPUT2:
1726                         qcmd_frame =
1727                                 kmalloc(sizeof(struct msm_queue_cmd) +
1728                                         sizeof(struct msm_vfe_phy_info),
1729                                         GFP_ATOMIC);
1730                         if (!qcmd_frame)
1731                                 goto mem_fail;
1732                         fphy = (struct msm_vfe_phy_info *)(qcmd_frame + 1);
1733                         *fphy = vdata->phy;
1734
1735                         qcmd_frame->type = MSM_CAM_Q_VFE_MSG;
1736                         qcmd_frame->command = fphy;
1737
1738                         CDBG("qcmd_frame= 0x%x phy_y= 0x%x, phy_cbcr= 0x%x\n",
1739                                 (int) qcmd_frame, fphy->y_phy, fphy->cbcr_phy);
1740
1741                         spin_lock_irqsave(&sync->prev_frame_q_lock, flags);
1742                         list_add_tail(&qcmd_frame->list, &sync->prev_frame_q);
1743                         wake_up(&sync->prev_frame_wait);
1744                         spin_unlock_irqrestore(&sync->prev_frame_q_lock, flags);
1745                         CDBG("woke up frame thread\n");
1746                         break;
1747                 case VFE_MSG_SNAPSHOT:
1748                         if (sync->pict_pp)
1749                                 break;
1750
1751                         CDBG("snapshot pp = %d\n", sync->pict_pp);
1752                         qcmd_frame =
1753                                 kmalloc(sizeof(struct msm_queue_cmd),
1754                                         GFP_ATOMIC);
1755                         if (!qcmd_frame)
1756                                 goto mem_fail;
1757                         qcmd_frame->type = MSM_CAM_Q_VFE_MSG;
1758                         qcmd_frame->command = NULL;
1759                                 spin_lock_irqsave(&sync->pict_frame_q_lock,
1760                                 flags);
1761                         list_add_tail(&qcmd_frame->list, &sync->pict_frame_q);
1762                         wake_up(&sync->pict_frame_wait);
1763                         spin_unlock_irqrestore(&sync->pict_frame_q_lock, flags);
1764                         CDBG("woke up picture thread\n");
1765                         break;
1766                 default:
1767                         CDBG("%s: qtype = %d not handled\n",
1768                                 __func__, vdata->type);
1769                         break;
1770                 }
1771         }
1772
1773         qcmd->command = (void *)vdata;
1774         CDBG("vdata->type = %d\n", vdata->type);
1775
1776         spin_lock_irqsave(&sync->msg_event_q_lock, flags);
1777         list_add_tail(&qcmd->list, &sync->msg_event_q);
1778         wake_up(&sync->msg_event_wait);
1779         spin_unlock_irqrestore(&sync->msg_event_q_lock, flags);
1780         CDBG("woke up config thread\n");
1781         return;
1782
1783 mem_fail:
1784         kfree(qcmd);
1785 }
1786
1787 static struct msm_vfe_callback msm_vfe_s = {
1788         .vfe_resp = msm_vfe_sync,
1789         .vfe_alloc = msm_vfe_sync_alloc,
1790 };
1791
1792 static int __msm_open(struct msm_sync *sync, const char *const apps_id)
1793 {
1794         int rc = 0;
1795
1796         mutex_lock(&sync->lock);
1797         if (sync->apps_id && strcmp(sync->apps_id, apps_id)) {
1798                 pr_err("msm_camera(%s): sensor %s is already opened for %s\n",
1799                         apps_id,
1800                         sync->sdata->sensor_name,
1801                         sync->apps_id);
1802                 rc = -EBUSY;
1803                 goto msm_open_done;
1804         }
1805
1806         sync->apps_id = apps_id;
1807
1808         if (!sync->opencnt) {
1809
1810                 msm_camvfe_fn_init(&sync->vfefn, sync);
1811                 if (sync->vfefn.vfe_init) {
1812                         rc = sync->vfefn.vfe_init(&msm_vfe_s,
1813                                 sync->pdev);
1814                         if (rc < 0) {
1815                                 pr_err("vfe_init failed at %d\n", rc);
1816                                 goto msm_open_done;
1817                         }
1818                         rc = sync->sctrl.s_init(sync->sdata);
1819                         if (rc < 0) {
1820                                 pr_err("sensor init failed: %d\n", rc);
1821                                 goto msm_open_done;
1822                         }
1823                 } else {
1824                         pr_err("no sensor init func\n");
1825                         rc = -ENODEV;
1826                         goto msm_open_done;
1827                 }
1828
1829                 if (rc >= 0) {
1830                         INIT_HLIST_HEAD(&sync->frame);
1831                         INIT_HLIST_HEAD(&sync->stats);
1832                         sync->unblock_poll_frame = 0;
1833                 }
1834         }
1835         sync->opencnt++;
1836
1837 msm_open_done:
1838         mutex_unlock(&sync->lock);
1839         return rc;
1840 }
1841
1842 static int msm_open_common(struct inode *inode, struct file *filep,
1843                            int once)
1844 {
1845         int rc;
1846         struct msm_device *pmsm =
1847                 container_of(inode->i_cdev, struct msm_device, cdev);
1848
1849         CDBG("msm_camera: open %s\n", filep->f_path.dentry->d_name.name);
1850
1851         if (atomic_cmpxchg(&pmsm->opened, 0, 1) && once) {
1852                 pr_err("msm_camera: %s is already opened.\n",
1853                         filep->f_path.dentry->d_name.name);
1854                 return -EBUSY;
1855         }
1856
1857         rc = nonseekable_open(inode, filep);
1858         if (rc < 0) {
1859                 pr_err("msm_open: nonseekable_open error %d\n", rc);
1860                 return rc;
1861         }
1862
1863         rc = __msm_open(pmsm->sync, MSM_APPS_ID_PROP);
1864         if (rc < 0)
1865                 return rc;
1866
1867         filep->private_data = pmsm;
1868
1869         CDBG("msm_open() open: rc = %d\n", rc);
1870         return rc;
1871 }
1872
1873 static int msm_open(struct inode *inode, struct file *filep)
1874 {
1875         return msm_open_common(inode, filep, 1);
1876 }
1877
1878 static int msm_open_control(struct inode *inode, struct file *filep)
1879 {
1880         int rc;
1881
1882         struct msm_control_device *ctrl_pmsm =
1883                 kmalloc(sizeof(struct msm_control_device), GFP_KERNEL);
1884         if (!ctrl_pmsm)
1885                 return -ENOMEM;
1886
1887         rc = msm_open_common(inode, filep, 0);
1888         if (rc < 0) {
1889                 kfree(ctrl_pmsm);
1890                 return rc;
1891         }
1892
1893         ctrl_pmsm->pmsm = filep->private_data;
1894         filep->private_data = ctrl_pmsm;
1895         spin_lock_init(&ctrl_pmsm->ctrl_q.ctrl_status_q_lock);
1896         INIT_LIST_HEAD(&ctrl_pmsm->ctrl_q.ctrl_status_q);
1897         init_waitqueue_head(&ctrl_pmsm->ctrl_q.ctrl_status_wait);
1898
1899         CDBG("msm_open() open: rc = %d\n", rc);
1900         return rc;
1901 }
1902
1903 static int __msm_v4l2_control(struct msm_sync *sync,
1904                 struct msm_ctrl_cmd *out)
1905 {
1906         int rc = 0;
1907
1908         struct msm_queue_cmd *qcmd = NULL, *rcmd = NULL;
1909         struct msm_ctrl_cmd *ctrl;
1910         struct msm_control_device_queue FIXME;
1911
1912         /* wake up config thread, 4 is for V4L2 application */
1913         qcmd = kmalloc(sizeof(struct msm_queue_cmd), GFP_KERNEL);
1914         if (!qcmd) {
1915                 pr_err("msm_control: cannot allocate buffer\n");
1916                 rc = -ENOMEM;
1917                 goto end;
1918         }
1919         qcmd->type = MSM_CAM_Q_V4L2_REQ;
1920         qcmd->command = out;
1921
1922         rcmd = __msm_control(sync, &FIXME, qcmd, out->timeout_ms);
1923         if (IS_ERR(rcmd)) {
1924                 rc = PTR_ERR(rcmd);
1925                 goto end;
1926         }
1927
1928         ctrl = (struct msm_ctrl_cmd *)(rcmd->command);
1929         /* FIXME: we should just set out->length = ctrl->length; */
1930         BUG_ON(out->length < ctrl->length);
1931         memcpy(out->value, ctrl->value, ctrl->length);
1932
1933 end:
1934         kfree(rcmd);
1935         CDBG("__msm_v4l2_control: end rc = %d\n", rc);
1936         return rc;
1937 }
1938
1939 static const struct file_operations msm_fops_config = {
1940         .owner = THIS_MODULE,
1941         .open = msm_open,
1942         .unlocked_ioctl = msm_ioctl_config,
1943         .release = msm_release_config,
1944 };
1945
1946 static const struct file_operations msm_fops_control = {
1947         .owner = THIS_MODULE,
1948         .open = msm_open_control,
1949         .unlocked_ioctl = msm_ioctl_control,
1950         .release = msm_release_control,
1951 };
1952
1953 static const struct file_operations msm_fops_frame = {
1954         .owner = THIS_MODULE,
1955         .open = msm_open,
1956         .unlocked_ioctl = msm_ioctl_frame,
1957         .release = msm_release_frame,
1958         .poll = msm_poll_frame,
1959 };
1960
1961 static int msm_setup_cdev(struct msm_device *msm,
1962                         int node,
1963                         dev_t devno,
1964                         const char *suffix,
1965                         const struct file_operations *fops)
1966 {
1967         int rc = -ENODEV;
1968
1969         struct device *device =
1970                 device_create(msm_class, NULL,
1971                         devno, NULL,
1972                         "%s%d", suffix, node);
1973
1974         if (IS_ERR(device)) {
1975                 rc = PTR_ERR(device);
1976                 pr_err("msm_camera: error creating device: %d\n", rc);
1977                 return rc;
1978         }
1979
1980         cdev_init(&msm->cdev, fops);
1981         msm->cdev.owner = THIS_MODULE;
1982
1983         rc = cdev_add(&msm->cdev, devno, 1);
1984         if (rc < 0) {
1985                 pr_err("msm_camera: error adding cdev: %d\n", rc);
1986                 device_destroy(msm_class, devno);
1987                 return rc;
1988         }
1989
1990         return rc;
1991 }
1992
1993 static int msm_tear_down_cdev(struct msm_device *msm, dev_t devno)
1994 {
1995         cdev_del(&msm->cdev);
1996         device_destroy(msm_class, devno);
1997         return 0;
1998 }
1999
2000 int msm_v4l2_register(struct msm_v4l2_driver *drv)
2001 {
2002         /* FIXME: support multiple sensors */
2003         if (list_empty(&msm_sensors))
2004                 return -ENODEV;
2005
2006         drv->sync = list_first_entry(&msm_sensors, struct msm_sync, list);
2007         drv->open      = __msm_open;
2008         drv->release   = __msm_release;
2009         drv->ctrl      = __msm_v4l2_control;
2010         drv->reg_pmem  = __msm_register_pmem;
2011         drv->get_frame = __msm_get_frame;
2012         drv->put_frame = __msm_put_frame_buf;
2013         drv->get_pict  = __msm_get_pic;
2014         drv->drv_poll  = __msm_poll_frame;
2015
2016         return 0;
2017 }
2018 EXPORT_SYMBOL(msm_v4l2_register);
2019
2020 int msm_v4l2_unregister(struct msm_v4l2_driver *drv)
2021 {
2022         drv->sync = NULL;
2023         return 0;
2024 }
2025 EXPORT_SYMBOL(msm_v4l2_unregister);
2026
2027 static int msm_sync_init(struct msm_sync *sync,
2028                 struct platform_device *pdev,
2029                 int (*sensor_probe)(const struct msm_camera_sensor_info *,
2030                                 struct msm_sensor_ctrl *))
2031 {
2032         int rc = 0;
2033         struct msm_sensor_ctrl sctrl;
2034         sync->sdata = pdev->dev.platform_data;
2035
2036         spin_lock_init(&sync->msg_event_q_lock);
2037         INIT_LIST_HEAD(&sync->msg_event_q);
2038         init_waitqueue_head(&sync->msg_event_wait);
2039
2040         spin_lock_init(&sync->prev_frame_q_lock);
2041         INIT_LIST_HEAD(&sync->prev_frame_q);
2042         init_waitqueue_head(&sync->prev_frame_wait);
2043
2044         spin_lock_init(&sync->pict_frame_q_lock);
2045         INIT_LIST_HEAD(&sync->pict_frame_q);
2046         init_waitqueue_head(&sync->pict_frame_wait);
2047
2048         rc = msm_camio_probe_on(pdev);
2049         if (rc < 0)
2050                 return rc;
2051         rc = sensor_probe(sync->sdata, &sctrl);
2052         if (rc >= 0) {
2053                 sync->pdev = pdev;
2054                 sync->sctrl = sctrl;
2055         }
2056         msm_camio_probe_off(pdev);
2057         if (rc < 0) {
2058                 pr_err("msm_camera: failed to initialize %s\n",
2059                         sync->sdata->sensor_name);
2060                 return rc;
2061         }
2062
2063         sync->opencnt = 0;
2064         mutex_init(&sync->lock);
2065         CDBG("initialized %s\n", sync->sdata->sensor_name);
2066         return rc;
2067 }
2068
2069 static int msm_sync_destroy(struct msm_sync *sync)
2070 {
2071         return 0;
2072 }
2073
2074 static int msm_device_init(struct msm_device *pmsm,
2075                 struct msm_sync *sync,
2076                 int node)
2077 {
2078         int dev_num = 3 * node;
2079         int rc = msm_setup_cdev(pmsm, node,
2080                 MKDEV(MAJOR(msm_devno), dev_num),
2081                 "control", &msm_fops_control);
2082         if (rc < 0) {
2083                 pr_err("error creating control node: %d\n", rc);
2084                 return rc;
2085         }
2086
2087         rc = msm_setup_cdev(pmsm + 1, node,
2088                 MKDEV(MAJOR(msm_devno), dev_num + 1),
2089                 "config", &msm_fops_config);
2090         if (rc < 0) {
2091                 pr_err("error creating config node: %d\n", rc);
2092                 msm_tear_down_cdev(pmsm, MKDEV(MAJOR(msm_devno),
2093                                 dev_num));
2094                 return rc;
2095         }
2096
2097         rc = msm_setup_cdev(pmsm + 2, node,
2098                 MKDEV(MAJOR(msm_devno), dev_num + 2),
2099                 "frame", &msm_fops_frame);
2100         if (rc < 0) {
2101                 pr_err("error creating frame node: %d\n", rc);
2102                 msm_tear_down_cdev(pmsm,
2103                         MKDEV(MAJOR(msm_devno), dev_num));
2104                 msm_tear_down_cdev(pmsm + 1,
2105                         MKDEV(MAJOR(msm_devno), dev_num + 1));
2106                 return rc;
2107         }
2108
2109         atomic_set(&pmsm[0].opened, 0);
2110         atomic_set(&pmsm[1].opened, 0);
2111         atomic_set(&pmsm[2].opened, 0);
2112
2113         pmsm[0].sync = sync;
2114         pmsm[1].sync = sync;
2115         pmsm[2].sync = sync;
2116
2117         return rc;
2118 }
2119
2120 int msm_camera_drv_start(struct platform_device *dev,
2121                 int (*sensor_probe)(const struct msm_camera_sensor_info *,
2122                         struct msm_sensor_ctrl *))
2123 {
2124         struct msm_device *pmsm = NULL;
2125         struct msm_sync *sync;
2126         int rc = -ENODEV;
2127         static int camera_node;
2128
2129         if (camera_node >= MSM_MAX_CAMERA_SENSORS) {
2130                 pr_err("msm_camera: too many camera sensors\n");
2131                 return rc;
2132         }
2133
2134         if (!msm_class) {
2135                 /* There are three device nodes per sensor */
2136                 rc = alloc_chrdev_region(&msm_devno, 0,
2137                                 3 * MSM_MAX_CAMERA_SENSORS,
2138                                 "msm_camera");
2139                 if (rc < 0) {
2140                         pr_err("msm_camera: failed to allocate chrdev: %d\n",
2141                                 rc);
2142                         return rc;
2143                 }
2144
2145                 msm_class = class_create(THIS_MODULE, "msm_camera");
2146                 if (IS_ERR(msm_class)) {
2147                         rc = PTR_ERR(msm_class);
2148                         pr_err("msm_camera: create device class failed: %d\n",
2149                                 rc);
2150                         return rc;
2151                 }
2152         }
2153
2154         pmsm = kzalloc(sizeof(struct msm_device) * 3 +
2155                         sizeof(struct msm_sync), GFP_ATOMIC);
2156         if (!pmsm)
2157                 return -ENOMEM;
2158         sync = (struct msm_sync *)(pmsm + 3);
2159
2160         rc = msm_sync_init(sync, dev, sensor_probe);
2161         if (rc < 0) {
2162                 kfree(pmsm);
2163                 return rc;
2164         }
2165
2166         CDBG("setting camera node %d\n", camera_node);
2167         rc = msm_device_init(pmsm, sync, camera_node);
2168         if (rc < 0) {
2169                 msm_sync_destroy(sync);
2170                 kfree(pmsm);
2171                 return rc;
2172         }
2173
2174         camera_node++;
2175         list_add(&sync->list, &msm_sensors);
2176         return rc;
2177 }
2178 EXPORT_SYMBOL(msm_camera_drv_start);