drivers/media/video/rk29_camera_oneframe.c

   1 /*
   2  * V4L2 Driver for RK28 camera host
   3  *
   4  * Copyright (C) 2006, Sascha Hauer, Pengutronix
   5  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  */
  12
  13 #include <linux/init.h>
  14 #include <linux/module.h>
  15 #include <linux/io.h>
  16 #include <linux/delay.h>
  17 #include <linux/slab.h>
  18 #include <linux/dma-mapping.h>
  19 #include <linux/errno.h>
  20 #include <linux/fs.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/kernel.h>
  23 #include <linux/mm.h>
  24 #include <linux/moduleparam.h>
  25 #include <linux/time.h>
  26 #include <linux/clk.h>
  27 #include <linux/version.h>
  28 #include <linux/device.h>
  29 #include <linux/platform_device.h>
  30 #include <linux/mutex.h>
  31 #include <linux/videodev2.h>
  32
  33 #include <mach/rk29_camera.h>
  34 #include <mach/rk29_iomap.h>
  35 #include <mach/iomux.h>
  36
  37 #include <media/v4l2-common.h>
  38 #include <media/v4l2-dev.h>
  39 #include <media/videobuf-dma-contig.h>
  40 #include <media/soc_camera.h>
  41
  42 // VIP Reg Offset
  43 #define RK29_VIP_AHBR_CTRL                0x00
  44 #define RK29_VIP_INT_MASK                 0x04
  45 #define RK29_VIP_INT_STS                  0x08
  46 #define RK29_VIP_STS                      0x0c
  47 #define RK29_VIP_CTRL                     0x10
  48 #define RK29_VIP_CAPTURE_F1SA_Y           0x14
  49 #define RK29_VIP_CAPTURE_F1SA_UV          0x18
  50 #define RK29_VIP_CAPTURE_F1SA_Cr          0x1c
  51 #define RK29_VIP_CAPTURE_F2SA_Y           0x20
  52 #define RK29_VIP_CAPTURE_F2SA_UV          0x24
  53 #define RK29_VIP_CAPTURE_F2SA_Cr          0x28
  54 #define RK29_VIP_FB_SR                    0x2c
  55 #define RK29_VIP_FS                       0x30
  56 #define RK29_VIP_VIPRESERVED              0x34
  57 #define RK29_VIP_CROP                     0x38
  58 #define RK29_VIP_CRM                      0x3c
  59 #define RK29_VIP_RESET                    0x40
  60 #define RK29_VIP_L_SFT                    0x44
  61
  62 //The key register bit descrition
  63 // VIP_CTRL Reg
  64 #define  DISABLE_CAPTURE              (0x00<<0)
  65 #define  ENABLE_CAPTURE               (0x01<<0)
  66 #define  HSY_HIGH_ACTIVE              (0x00<<1)
  67 #define  HSY_LOW_ACTIVE               (0x01<<1)
  68 #define  VIP_CCIR656                  (0x00<<2)
  69 #define  VIP_SENSOR                   (0x01<<2)
  70 #define  SENSOR_UYVY                  (0x00<<3)
  71 #define  SENSOR_YUYV                  (0x01<<3)
  72 #define  VIP_YUV                      (0x00<<4)
  73 #define  VIP_RAW                      (0x01<<4)
  74 #define  CON_OR_PIN                   (0x00<<5)
  75 #define  ONEFRAME                     (0x01<<5)
  76 #define  VIPREGYUV420                 (0x00<<6)
  77 #define  VIPREGYUV422                 (0x01<<6)
  78 #define  FIELD0_START                 (0x00<<7)
  79 #define  FIELD1_START                 (0x01<<7)
  80 #define  CONTINUOUS                   (0x00<<8)
  81 #define  PING_PONG                    (0x01<<8)
  82 #define  POSITIVE_EDGE                (0x00<<9)
  83 #define  NEGATIVE_EDGE                (0x01<<9)
  84 #define  VIPREGNTSC                   (0x00<<10)
  85 #define  VIPREGPAL                    (0x01<<10)
  86 #define  VIP_DATA_LITTLEEND           (0x00<<11)
  87 #define  VIP_DATA_BIGEND              (0x01<<11)
  88 #define  VSY_LOW_ACTIVE               (0x00<<12)
  89 #define  VSY_HIGH_ACTIVE              (0x01<<12)
  90 #define  VIP_RAWINPUT_BYPASS          (0x00<<13)
  91 #define  VIP_RAWINPUT_POSITIVE_EDGE   (0x01<<13)
  92 #define  VIP_RAWINPUT_NEGATIVE_EDGE   (0x02<<13)
  93
  94 // GRF_SOC_CON0 Reg
  95 #define  GRF_SOC_CON0_Reg             0xbc
  96 #define  VIP_AXIMASTER                (0x00<<0)
  97 #define  VIP_AHBMASTER                (0x01<<2)
  98
  99 // GRF_OS_REG0
 100 #define  GRF_OS_REG0                  0xd0
 101 #define  VIP_ACLK_DIV_HCLK_1          (0x00<<0)
 102 #define  VIP_ACLK_DIV_HCLK_2          (0x01<<0)
 103
 104
 105 #define MIN(x,y)   ((x<y) ? x: y)
 106 #define MAX(x,y)    ((x>y) ? x: y)
 107 #define RK29_SENSOR_24MHZ      24           /* MHz */
 108 #define RK29_SENSOR_48MHZ      48
 109
 110 #define write_vip_reg(addr, val)  __raw_writel(val, addr+(rk29_camdev_info_ptr->base))
 111 #define read_vip_reg(addr) __raw_readl(addr+(rk29_camdev_info_ptr->base))
 112 #define mask_vip_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 113
 114 #define write_grf_reg(addr, val)  __raw_writel(val, addr+RK29_GRF_BASE)
 115 #define read_grf_reg(addr) __raw_readl(addr+RK29_GRF_BASE)
 116 #define mask_grf_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 117
 118 //Configure Macro
 119 #define CONFIG_RK29CAMERA_TR      1
 120 #define CONFIG_RK29CAMERA_DEBUG   1
 121 #if (CONFIG_RK29CAMERA_TR)
 122         #define RK29CAMERA_TR(format, ...)      printk(format, ## __VA_ARGS__)
 123         #if (CONFIG_RK29CAMERA_DEBUG)
 124         #define RK29CAMERA_DG(format, ...)      printk(format, ## __VA_ARGS__)
 125         #else
 126         #define RK29CAMERA_DG(format, ...)
 127         #endif
 128 #else
 129         #define RK29CAMERA_TR(format, ...)
 130         #define RK29CAMERA_DG(format, ...)
 131 #endif
 132
 133 #define RK29_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 1)
 134
 135 /* limit to rk29 hardware capabilities */
 136 #define RK29_CAM_BUS_PARAM   (SOCAM_MASTER |\
 137                 SOCAM_HSYNC_ACTIVE_HIGH |\
 138                 SOCAM_HSYNC_ACTIVE_LOW |\
 139                 SOCAM_VSYNC_ACTIVE_HIGH |\
 140                 SOCAM_VSYNC_ACTIVE_LOW |\
 141                 SOCAM_PCLK_SAMPLE_RISING |\
 142                 SOCAM_PCLK_SAMPLE_FALLING|\
 143                 SOCAM_DATA_ACTIVE_HIGH |\
 144                 SOCAM_DATA_ACTIVE_LOW|\
 145                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 146                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 147
 148 #define RK29_CAM_W_MIN        48
 149 #define RK29_CAM_H_MIN        32
 150 #define RK29_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 151 #define RK29_CAM_H_MAX        2764
 152 #define RK29_CAM_FRAME_INVAL_INIT 3
 153 #define RK29_CAM_FRAME_INVAL_DC 1          /* ddl@rock-chips.com :  */
 154
 155
 156
 157 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 158 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 159 extern void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 160             struct videobuf_queue_ops *ops,
 161             struct device *dev,
 162             spinlock_t *irqlock,
 163             enum v4l2_buf_type type,
 164             enum v4l2_field field,
 165             unsigned int msize,
 166             void *priv);
 167
 168 /* buffer for one video frame */
 169 struct rk29_buffer
 170 {
 171     /* common v4l buffer stuff -- must be first */
 172     struct videobuf_buffer vb;
 173     const struct soc_camera_data_format        *fmt;
 174     int                 inwork;
 175 };
 176 enum rk29_camera_reg_state
 177 {
 178         Reg_Invalidate,
 179         Reg_Validate
 180 };
 181
 182 struct rk29_camera_reg
 183 {
 184         unsigned int VipCtrl;
 185         unsigned int VipCrop;
 186         unsigned int VipFs;
 187         unsigned int VipIntMsk;
 188         unsigned int VipCrm;
 189         enum rk29_camera_reg_state Inval;
 190 };
 191 struct rk29_camera_dev
 192 {
 193     struct soc_camera_host      soc_host;
 194     struct device               *dev;
 195     /* RK2827x is only supposed to handle one camera on its Quick Capture
 196      * interface. If anyone ever builds hardware to enable more than
 197      * one camera, they will have to modify this driver too */
 198     struct soc_camera_device *icd;
 199     struct clk *clk;
 200     void __iomem *base;
 201         void __iomem *grf_base;
 202     int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 203     unsigned int irq;
 204
 205     struct rk29camera_platform_data *pdata;
 206     struct resource             *res;
 207
 208     struct list_head    capture;
 209
 210     spinlock_t          lock;
 211
 212     struct videobuf_buffer      *active;
 213         struct videobuf_queue *vb_vidq_ptr;
 214         struct rk29_camera_reg reginfo_suspend;
 215 };
 216 static DEFINE_MUTEX(camera_lock);
 217 static const char *rk29_cam_driver_description = "RK29_Camera";
 218 static struct rk29_camera_dev *rk29_camdev_info_ptr;
 219
 220 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable);
 221
 222
 223 /*
 224  *  Videobuf operations
 225  */
 226 static int rk29_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 227                                unsigned int *size)
 228 {
 229     struct soc_camera_device *icd = vq->priv_data;
 230     int bytes_per_pixel = (icd->current_fmt->depth + 7) >> 3;
 231
 232     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 233
 234     /* planar capture requires Y, U and V buffers to be page aligned */
 235     *size = PAGE_ALIGN( icd->user_width * icd->user_height * bytes_per_pixel);                               /* Y pages UV pages, yuv422*/
 236
 237     RK29CAMERA_DG("\n%s..%d.. size = %d\n",__FUNCTION__,__LINE__, *size);
 238
 239     return 0;
 240 }
 241 static void rk29_videobuf_free(struct videobuf_queue *vq, struct rk29_buffer *buf)
 242 {
 243     struct soc_camera_device *icd = vq->priv_data;
 244
 245     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 246             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 247
 248         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 249         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 250                 return;
 251
 252     if (in_interrupt())
 253         BUG();
 254
 255     videobuf_dma_contig_free(vq, &buf->vb);
 256     dev_dbg(&icd->dev, "%s freed\n", __func__);
 257     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 258         return;
 259 }
 260 static int rk29_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 261 {
 262     struct soc_camera_device *icd = vq->priv_data;
 263     struct rk29_buffer *buf;
 264     int ret;
 265
 266     buf = container_of(vb, struct rk29_buffer, vb);
 267
 268     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 269             vb, vb->baddr, vb->bsize);
 270
 271     //RK29CAMERA_TR("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 272
 273     /* Added list head initialization on alloc */
 274     WARN_ON(!list_empty(&vb->queue));
 275
 276     /* This can be useful if you want to see if we actually fill
 277      * the buffer with something */
 278     //memset((void *)vb->baddr, 0xaa, vb->bsize);
 279
 280     BUG_ON(NULL == icd->current_fmt);
 281
 282     if (buf->fmt    != icd->current_fmt ||
 283             vb->width   != icd->user_width ||
 284             vb->height  != icd->user_height ||
 285              vb->field   != field) {
 286         buf->fmt    = icd->current_fmt;
 287         vb->width   = icd->user_width;
 288         vb->height  = icd->user_height;
 289         vb->field   = field;
 290         vb->state   = VIDEOBUF_NEEDS_INIT;
 291     }
 292
 293     vb->size = vb->width * vb->height * ((buf->fmt->depth + 7) >> 3) ;          /* ddl@rock-chips.com : fmt->depth is coorect */
 294     if (0 != vb->baddr && vb->bsize < vb->size) {
 295         ret = -EINVAL;
 296         goto out;
 297     }
 298
 299     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 300         ret = videobuf_iolock(vq, vb, NULL);
 301         if (ret) {
 302             goto fail;
 303         }
 304         vb->state = VIDEOBUF_PREPARED;
 305     }
 306     //RK29CAMERA_TR("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 307     return 0;
 308 fail:
 309     rk29_videobuf_free(vq, buf);
 310 out:
 311     return ret;
 312 }
 313
 314 static inline void rk29_videobuf_capture(struct videobuf_buffer *vb)
 315 {
 316     unsigned int size;
 317
 318     if (vb) {
 319         size = vb->width * vb->height; /* Y pages UV pages, yuv422*/
 320         write_vip_reg(RK29_VIP_CAPTURE_F1SA_Y, vb->boff);
 321         write_vip_reg(RK29_VIP_CAPTURE_F1SA_UV, vb->boff + size);
 322         write_vip_reg(RK29_VIP_CAPTURE_F2SA_Y, vb->boff);
 323         write_vip_reg(RK29_VIP_CAPTURE_F2SA_UV, vb->boff + size);
 324         write_vip_reg(RK29_VIP_FB_SR,  0x00000002);//frame1 has been ready to receive data,frame 2 is not used
 325     }
 326 }
 327 /* Locking: Caller holds q->irqlock */
 328 static void rk29_videobuf_queue(struct videobuf_queue *vq,
 329                                 struct videobuf_buffer *vb)
 330 {
 331     struct soc_camera_device *icd = vq->priv_data;
 332     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 333     struct rk29_camera_dev *pcdev = ici->priv;
 334
 335     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 336             vb, vb->baddr, vb->bsize);
 337
 338     vb->state = VIDEOBUF_QUEUED;
 339
 340         if (!list_empty(&pcdev->capture)) {
 341                 list_add_tail(&vb->queue, &pcdev->capture);
 342         } else {
 343                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 344                         list_add_tail(&vb->queue, &pcdev->capture);
 345                 else
 346                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 347         }
 348
 349     if (!pcdev->active) {
 350         pcdev->active = vb;
 351         rk29_videobuf_capture(vb);
 352     }
 353 }
 354 static irqreturn_t rk29_camera_irq(int irq, void *data)
 355 {
 356     struct rk29_camera_dev *pcdev = data;
 357     struct videobuf_buffer *vb;
 358
 359     read_vip_reg(RK29_VIP_INT_STS);    /* clear vip interrupte single  */
 360
 361     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 362     if (read_vip_reg(RK29_VIP_FB_SR) & 0x01) {
 363
 364                 if (!pcdev->active)
 365                         goto RK29_CAMERA_IRQ_END;
 366
 367         if (pcdev->frame_inval>0) {
 368             pcdev->frame_inval--;
 369             rk29_videobuf_capture(pcdev->active);
 370             goto RK29_CAMERA_IRQ_END;
 371         } else if (pcdev->frame_inval) {
 372                 RK29CAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 373             pcdev->frame_inval = 0;
 374         }
 375
 376         vb = pcdev->active;
 377                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 378                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 379                 list_del_init(&vb->queue);
 380                 }
 381
 382         pcdev->active = NULL;
 383         if (!list_empty(&pcdev->capture)) {
 384             pcdev->active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 385                         if (pcdev->active) {
 386                                 rk29_videobuf_capture(pcdev->active);
 387                         }
 388         }
 389
 390         if (pcdev->active == NULL) {
 391                         RK29CAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 392         }
 393
 394                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 395                 vb->state = VIDEOBUF_DONE;
 396                 do_gettimeofday(&vb->ts);
 397                 vb->field_count++;
 398                 }
 399
 400         wake_up(&vb->done);
 401     }
 402
 403 RK29_CAMERA_IRQ_END:
 404     return IRQ_HANDLED;
 405 }
 406
 407
 408 static void rk29_videobuf_release(struct videobuf_queue *vq,
 409                                   struct videobuf_buffer *vb)
 410 {
 411     struct rk29_buffer *buf = container_of(vb, struct rk29_buffer, vb);
 412     struct soc_camera_device *icd = vq->priv_data;
 413     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 414     struct rk29_camera_dev *pcdev = ici->priv;
 415
 416 #ifdef DEBUG
 417     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 418             vb, vb->baddr, vb->bsize);
 419
 420     switch (vb->state)
 421     {
 422         case VIDEOBUF_ACTIVE:
 423             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 424             break;
 425         case VIDEOBUF_QUEUED:
 426             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 427             break;
 428         case VIDEOBUF_PREPARED:
 429             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 430             break;
 431         default:
 432             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 433             break;
 434     }
 435 #endif
 436         if (vb == pcdev->active) {
 437                 RK29CAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 438                 interruptible_sleep_on_timeout(&vb->done, 100);
 439                 RK29CAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 440         }
 441     rk29_videobuf_free(vq, buf);
 442 }
 443
 444 static struct videobuf_queue_ops rk29_videobuf_ops =
 445 {
 446     .buf_setup      = rk29_videobuf_setup,
 447     .buf_prepare    = rk29_videobuf_prepare,
 448     .buf_queue      = rk29_videobuf_queue,
 449     .buf_release    = rk29_videobuf_release,
 450 };
 451
 452 static void rk29_camera_init_videobuf(struct videobuf_queue *q,
 453                                       struct soc_camera_device *icd)
 454 {
 455     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 456     struct rk29_camera_dev *pcdev = ici->priv;
 457
 458     /* We must pass NULL as dev pointer, then all pci_* dma operations
 459      * transform to normal dma_* ones. */
 460     videobuf_queue_dma_contig_init(q,
 461                                    &rk29_videobuf_ops,
 462                                    ici->v4l2_dev.dev, &pcdev->lock,
 463                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 464                                    V4L2_FIELD_NONE,
 465                                    sizeof(struct rk29_buffer),
 466                                    icd);
 467         pcdev->vb_vidq_ptr = q;         /* ddl@rock-chips.com */
 468 }
 469 static int rk29_camera_activate(struct rk29_camera_dev *pcdev, struct soc_camera_device *icd)
 470 {
 471     unsigned long sensor_bus_flags = SOCAM_MCLK_24MHZ;
 472     struct clk *parent;
 473
 474     RK29CAMERA_DG("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 475     if (!pcdev->clk || IS_ERR(pcdev->clk))
 476         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk source\n");
 477
 478     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 479         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 480
 481     if (sensor_bus_flags & SOCAM_MCLK_48MHZ) {
 482         parent = clk_get(NULL, "clk48m");
 483         if (!parent || IS_ERR(parent))
 484              goto RK29_CAMERA_ACTIVE_ERR;
 485     } else if (sensor_bus_flags & SOCAM_MCLK_27MHZ) {
 486         parent = clk_get(NULL, "extclk");
 487         if (!parent || IS_ERR(parent))
 488              goto RK29_CAMERA_ACTIVE_ERR;
 489     } else {
 490         parent = clk_get(NULL, "xin24m");
 491         if (!parent || IS_ERR(parent))
 492              goto RK29_CAMERA_ACTIVE_ERR;
 493     }
 494
 495     clk_set_parent(pcdev->clk, parent);
 496
 497     clk_enable(pcdev->clk);
 498     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_VIP_CLKOUT);
 499     ndelay(10);
 500
 501         //write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)|VIP_AHBMASTER);  //VIP Config to AHB
 502         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)&(~VIP_AHBMASTER));  //VIP Config to AXI
 503         //write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)&(~VIP_ACLK_DIV_HCLK_2));   //aclk:hclk = 1:1
 504     write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)|VIP_ACLK_DIV_HCLK_2);   //aclk:hclk = 2:1
 505
 506     write_vip_reg(RK29_VIP_RESET, 0x76543210);  /* ddl@rock-chips.com : vip software reset */
 507     udelay(10);
 508
 509     write_vip_reg(RK29_VIP_AHBR_CTRL, 0x07);   /* ddl@rock-chips.com : vip ahb burst 16 */
 510     write_vip_reg(RK29_VIP_INT_MASK, 0x01);    //capture complete interrupt enable
 511     write_vip_reg(RK29_VIP_CRM,  0x00000000);  //Y/CB/CR color modification
 512
 513     return 0;
 514 RK29_CAMERA_ACTIVE_ERR:
 515     return -ENODEV;
 516 }
 517
 518 static void rk29_camera_deactivate(struct rk29_camera_dev *pcdev)
 519 {
 520     pcdev->active = NULL;
 521
 522     write_vip_reg(RK29_VIP_CTRL, 0);
 523     read_vip_reg(RK29_VIP_INT_STS);             //clear vip interrupte single
 524
 525     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_GPIO1B4);
 526     clk_disable(pcdev->clk);
 527
 528     return;
 529 }
 530
 531 /* The following two functions absolutely depend on the fact, that
 532  * there can be only one camera on RK28 quick capture interface */
 533 static int rk29_camera_add_device(struct soc_camera_device *icd)
 534 {
 535     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 536     struct rk29_camera_dev *pcdev = ici->priv;
 537     struct device *control = to_soc_camera_control(icd);
 538     struct v4l2_subdev *sd;
 539     int ret;
 540
 541     mutex_lock(&camera_lock);
 542
 543     if (pcdev->icd) {
 544         ret = -EBUSY;
 545         goto ebusy;
 546     }
 547
 548     dev_info(&icd->dev, "RK29 Camera driver attached to camera %d\n",
 549              icd->devnum);
 550
 551         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 552     pcdev->active = NULL;
 553     pcdev->icd = NULL;
 554         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 555         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 556      * if app havn't dequeue all videobuf before close camera device;
 557         */
 558     INIT_LIST_HEAD(&pcdev->capture);
 559
 560     ret = rk29_camera_activate(pcdev,icd);
 561     if (ret)
 562         goto ebusy;
 563
 564     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 565     if (control) {
 566         sd = dev_get_drvdata(control);
 567         ret = v4l2_subdev_call(sd,core, init, 0);
 568         if (ret)
 569             goto ebusy;
 570     }
 571
 572     pcdev->icd = icd;
 573
 574 ebusy:
 575     mutex_unlock(&camera_lock);
 576
 577     return ret;
 578 }
 579 static void rk29_camera_remove_device(struct soc_camera_device *icd)
 580 {
 581     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 582     struct rk29_camera_dev *pcdev = ici->priv;
 583         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 584
 585     BUG_ON(icd != pcdev->icd);
 586
 587     dev_info(&icd->dev, "RK29 Camera driver detached from camera %d\n",
 588              icd->devnum);
 589
 590     v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
 591         rk29_camera_deactivate(pcdev);
 592
 593         /* ddl@rock-chips.com: Call videobuf_mmap_free here for free the struct video_buffer which malloc in videobuf_alloc */
 594         if (pcdev->vb_vidq_ptr) {
 595                 videobuf_mmap_free(pcdev->vb_vidq_ptr);
 596                 pcdev->vb_vidq_ptr = NULL;
 597         }
 598
 599         pcdev->active = NULL;
 600     pcdev->icd = NULL;
 601         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 602         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 603      * if app havn't dequeue all videobuf before close camera device;
 604         */
 605     INIT_LIST_HEAD(&pcdev->capture);
 606
 607         return;
 608 }
 609
 610 static int rk29_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 611 {
 612     unsigned long bus_flags, camera_flags, common_flags;
 613     unsigned int vip_ctrl_val = 0;
 614     int ret = 0;
 615
 616     RK29CAMERA_DG("\n%s..%d..\n",__FUNCTION__,__LINE__);
 617
 618     bus_flags = RK29_CAM_BUS_PARAM;
 619         if (icd->ops->query_bus_param)
 620         camera_flags = icd->ops->query_bus_param(icd);
 621         else
 622                 camera_flags = 0;
 623
 624     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
 625     if (!common_flags) {
 626         ret = -EINVAL;
 627         goto RK29_CAMERA_SET_BUS_PARAM_END;
 628     }
 629
 630     ret = icd->ops->set_bus_param(icd, common_flags);
 631     if (ret < 0)
 632         goto RK29_CAMERA_SET_BUS_PARAM_END;
 633
 634         if (common_flags & SOCAM_DATAWIDTH_8) {
 635         icd->buswidth = 8;
 636         } else if (common_flags & SOCAM_DATAWIDTH_10) {
 637             icd->buswidth = 10;
 638         }
 639
 640     vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
 641     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
 642         vip_ctrl_val |= NEGATIVE_EDGE;
 643     } else {
 644                 vip_ctrl_val &= ~NEGATIVE_EDGE;
 645     }
 646     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
 647         vip_ctrl_val |= HSY_LOW_ACTIVE;
 648     } else {
 649                 vip_ctrl_val &= ~HSY_LOW_ACTIVE;
 650     }
 651     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
 652         vip_ctrl_val |= VSY_HIGH_ACTIVE;
 653     } else {
 654                 vip_ctrl_val &= ~VSY_HIGH_ACTIVE;
 655     }
 656
 657     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
 658     //vip_ctrl_val |= ENABLE_CAPTURE;
 659
 660     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
 661     RK29CAMERA_DG("\n%s..ctrl:0x%x CtrReg=%x AXI_AHB:0x%x aclk_hclk:0x%x \n",__FUNCTION__,vip_ctrl_val,read_vip_reg(RK29_VIP_CTRL),
 662                 read_grf_reg(GRF_SOC_CON0_Reg)&VIP_AHBMASTER, read_grf_reg(GRF_OS_REG0)&VIP_ACLK_DIV_HCLK_2);
 663
 664 RK29_CAMERA_SET_BUS_PARAM_END:
 665         if (ret)
 666         RK29CAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
 667     return ret;
 668 }
 669
 670 static int rk29_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 671 {
 672     unsigned long bus_flags, camera_flags;
 673     int ret;
 674
 675     bus_flags = RK29_CAM_BUS_PARAM;
 676         if (icd->ops->query_bus_param) {
 677         camera_flags = icd->ops->query_bus_param(icd);
 678         } else {
 679                 camera_flags = 0;
 680         }
 681     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
 682
 683     if (ret < 0)
 684         dev_warn(icd->dev.parent,
 685                          "Flags incompatible: camera %lx, host %lx\n",
 686                          camera_flags, bus_flags);
 687     return ret;
 688 }
 689 static const struct soc_camera_data_format rk29_camera_formats[] = {
 690         {
 691                 .name           = "Planar YUV420 12 bit",
 692                 .depth          = 12,
 693                 .fourcc         = V4L2_PIX_FMT_YUV420,
 694                 .colorspace     = V4L2_COLORSPACE_JPEG,
 695         },{
 696                 .name           = "Planar YUV422 16 bit",
 697                 .depth          = 16,
 698                 .fourcc         = V4L2_PIX_FMT_YUV422P,
 699                 .colorspace     = V4L2_COLORSPACE_JPEG,
 700         },{
 701                 .name           = "Raw Bayer RGB 10 bit",
 702                 .depth          = 16,
 703                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
 704                 .colorspace     = V4L2_COLORSPACE_SRGB,
 705         }
 706 };
 707
 708 static void rk29_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, __u32 cam_pixfmt, struct v4l2_rect *rect)
 709 {
 710         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 711     struct rk29_camera_dev *pcdev = ici->priv;
 712     unsigned int vip_fs = 0,vip_crop = 0;
 713     unsigned int vip_ctrl_val = VIP_SENSOR|ONEFRAME|DISABLE_CAPTURE;
 714
 715     switch (host_pixfmt)
 716     {
 717         case V4L2_PIX_FMT_YUV422P:
 718             vip_ctrl_val |= VIPREGYUV422;
 719                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 720             break;
 721         case V4L2_PIX_FMT_YUV420:
 722             vip_ctrl_val |= VIPREGYUV420;
 723                         if (pcdev->frame_inval != RK29_CAM_FRAME_INVAL_INIT)
 724                                 pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 725             break;
 726                 case V4L2_PIX_FMT_SGRBG10:
 727                         vip_ctrl_val |= (VIP_RAW | VIP_SENSOR | VIP_DATA_LITTLEEND);
 728                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 729                         break;
 730         default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
 731             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & VIPREGYUV422);
 732             break;
 733     }
 734
 735     switch (cam_pixfmt)
 736     {
 737         case V4L2_PIX_FMT_UYVY:
 738             vip_ctrl_val |= SENSOR_UYVY;
 739             break;
 740         case V4L2_PIX_FMT_YUYV:
 741             vip_ctrl_val |= SENSOR_YUYV;
 742             break;
 743         default :
 744             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & SENSOR_YUYV);
 745             break;
 746     }
 747
 748     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);         /* ddl@rock-chips.com: VIP capture mode and capture format must be set before FS register set */
 749
 750     read_vip_reg(RK29_VIP_INT_STS);                     /* clear vip interrupte single  */
 751
 752     if (vip_ctrl_val & ONEFRAME)  {
 753         vip_crop = ((rect->left<<16) + rect->top);
 754         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
 755     } else if (vip_ctrl_val & PING_PONG) {
 756         if (rect->left ||rect->top ) {
 757                         RK29CAMERA_DG("\n %s..PingPang not support Crop \n",__FUNCTION__);
 758                         BUG();
 759         }
 760                 vip_crop = 0;
 761         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
 762     }
 763
 764     write_vip_reg(RK29_VIP_CROP, vip_crop);
 765     write_vip_reg(RK29_VIP_FS, vip_fs);
 766
 767     write_vip_reg(RK29_VIP_FB_SR,  0x00000003);
 768
 769     RK29CAMERA_DG("\n%s.. crop:0x%x fs:0x%x ctrl:0x%x CtrlReg:0x%x\n",__FUNCTION__,vip_crop,vip_fs,vip_ctrl_val,read_vip_reg(RK29_VIP_CTRL));
 770         return;
 771 }
 772
 773 static int rk29_camera_get_formats(struct soc_camera_device *icd, int idx,
 774                                   struct soc_camera_format_xlate *xlate)
 775 {
 776     struct device *dev = icd->dev.parent;
 777     int formats = 0, buswidth, ret;
 778
 779     buswidth = 8;
 780
 781     ret = rk29_camera_try_bus_param(icd, buswidth);
 782     if (ret < 0)
 783         return 0;
 784
 785     switch (icd->formats[idx].fourcc) {
 786         case V4L2_PIX_FMT_UYVY:
 787         case V4L2_PIX_FMT_YUYV:
 788             formats++;
 789             if (xlate) {
 790                 xlate->host_fmt = &rk29_camera_formats[0];
 791                 xlate->cam_fmt = icd->formats + idx;
 792                 xlate->buswidth = buswidth;
 793                 xlate++;
 794                 dev_dbg(dev, "Providing format %s using %s\n",
 795                         rk29_camera_formats[0].name,
 796                         icd->formats[idx].name);
 797             }
 798
 799             formats++;
 800             if (xlate) {
 801                 xlate->host_fmt = &rk29_camera_formats[1];
 802                 xlate->cam_fmt = icd->formats + idx;
 803                 xlate->buswidth = buswidth;
 804                 xlate++;
 805                 dev_dbg(dev, "Providing format %s using %s\n",
 806                         rk29_camera_formats[1].name,
 807                         icd->formats[idx].name);
 808             }
 809                         break;
 810                 case V4L2_PIX_FMT_SGRBG10:
 811                         formats++;
 812             if (xlate) {
 813                 xlate->host_fmt = &rk29_camera_formats[2];
 814                 xlate->cam_fmt = icd->formats + idx;
 815                 xlate->buswidth = 10;
 816                 xlate++;
 817                 dev_dbg(dev, "Providing format %s using %s\n",
 818                         rk29_camera_formats[2].name,
 819                         icd->formats[idx].name);
 820             }
 821                         break;
 822         default:
 823             break;
 824     }
 825
 826     return formats;
 827 }
 828
 829 static void rk29_camera_put_formats(struct soc_camera_device *icd)
 830 {
 831         return;
 832 }
 833
 834 static int rk29_camera_set_crop(struct soc_camera_device *icd,
 835                                struct v4l2_crop *a)
 836 {
 837     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 838     struct v4l2_format f;
 839     struct v4l2_pix_format *pix = &f.fmt.pix;
 840     int ret;
 841
 842     f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 843
 844     ret = v4l2_subdev_call(sd, video, g_fmt, &f);
 845     if (ret < 0)
 846         return ret;
 847
 848     if ((pix->width < (a->c.left + a->c.width)) || (pix->height < (a->c.top + a->c.height)))  {
 849
 850         pix->width = a->c.left + a->c.width;
 851         pix->height = a->c.top + a->c.height;
 852
 853         v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
 854             &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
 855             icd->current_fmt->fourcc == V4L2_PIX_FMT_YUV422P ?4 : 0);
 856
 857         ret = v4l2_subdev_call(sd, video, s_fmt, &f);
 858         if (ret < 0)
 859             return ret;
 860     }
 861
 862     rk29_camera_setup_format(icd, icd->current_fmt->fourcc, pix->pixelformat, &a->c);
 863
 864     icd->user_width = pix->width;
 865     icd->user_height = pix->height;
 866
 867     return 0;
 868 }
 869
 870 static int rk29_camera_set_fmt(struct soc_camera_device *icd,
 871                               struct v4l2_format *f)
 872 {
 873     struct device *dev = icd->dev.parent;
 874     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 875     const struct soc_camera_data_format *cam_fmt = NULL;
 876     const struct soc_camera_format_xlate *xlate = NULL;
 877     struct v4l2_pix_format *pix = &f->fmt.pix;
 878     struct v4l2_format cam_f = *f;
 879     struct v4l2_rect rect;
 880     int ret;
 881
 882     RK29CAMERA_DG("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 883
 884     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 885     if (!xlate) {
 886         dev_err(dev, "Format %x not found\n", pix->pixelformat);
 887         ret = -EINVAL;
 888         goto RK29_CAMERA_SET_FMT_END;
 889     }
 890
 891     cam_fmt = xlate->cam_fmt;
 892
 893     cam_f.fmt.pix.pixelformat = cam_fmt->fourcc;
 894     ret = v4l2_subdev_call(sd, video, s_fmt, &cam_f);
 895     cam_f.fmt.pix.pixelformat = pix->pixelformat;
 896     *pix = cam_f.fmt.pix;
 897
 898     icd->sense = NULL;
 899
 900     if (!ret) {
 901         rect.left = 0;
 902         rect.top = 0;
 903         rect.width = pix->width;
 904         rect.height = pix->height;
 905
 906         RK29CAMERA_DG("\n%s..%s..%s \n",__FUNCTION__,xlate->host_fmt->name, cam_fmt->name);
 907         rk29_camera_setup_format(icd, pix->pixelformat, cam_fmt->fourcc, &rect);
 908
 909         icd->buswidth = xlate->buswidth;
 910         icd->current_fmt = xlate->host_fmt;
 911     }
 912
 913 RK29_CAMERA_SET_FMT_END:
 914         if (ret)
 915         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
 916     return ret;
 917 }
 918
 919 static int rk29_camera_try_fmt(struct soc_camera_device *icd,
 920                                    struct v4l2_format *f)
 921 {
 922     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 923     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 924     const struct soc_camera_format_xlate *xlate;
 925     struct v4l2_pix_format *pix = &f->fmt.pix;
 926     __u32 pixfmt = pix->pixelformat;
 927     enum v4l2_field field;
 928     int ret;
 929
 930     RK29CAMERA_DG("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 931
 932     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 933     if (!xlate) {
 934         dev_err(ici->v4l2_dev.dev, "Format %x not found\n", pixfmt);
 935         ret = -EINVAL;
 936         goto RK29_CAMERA_TRY_FMT_END;
 937     }
 938    /* limit to rk29 hardware capabilities */
 939     v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
 940               &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
 941               pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 942
 943     pix->bytesperline = pix->width * DIV_ROUND_UP(xlate->host_fmt->depth, 8);
 944     pix->sizeimage = pix->height * pix->bytesperline;
 945
 946     /* camera has to see its format, but the user the original one */
 947     pix->pixelformat = xlate->cam_fmt->fourcc;
 948     /* limit to sensor capabilities */
 949     ret = v4l2_subdev_call(sd, video, try_fmt, f);
 950     pix->pixelformat = pixfmt;
 951
 952     field = pix->field;
 953
 954     if (field == V4L2_FIELD_ANY) {
 955         pix->field = V4L2_FIELD_NONE;
 956     } else if (field != V4L2_FIELD_NONE) {
 957         dev_err(icd->dev.parent, "Field type %d unsupported.\n", field);
 958         ret = -EINVAL;
 959         goto RK29_CAMERA_TRY_FMT_END;
 960     }
 961
 962 RK29_CAMERA_TRY_FMT_END:
 963         if (ret)
 964         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
 965     return ret;
 966 }
 967
 968 static int rk29_camera_reqbufs(struct soc_camera_file *icf,
 969                                struct v4l2_requestbuffers *p)
 970 {
 971     int i;
 972
 973     /* This is for locking debugging only. I removed spinlocks and now I
 974      * check whether .prepare is ever called on a linked buffer, or whether
 975      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
 976      * it hadn't triggered */
 977     for (i = 0; i < p->count; i++) {
 978         struct rk29_buffer *buf = container_of(icf->vb_vidq.bufs[i],
 979                                                            struct rk29_buffer, vb);
 980         buf->inwork = 0;
 981         INIT_LIST_HEAD(&buf->vb.queue);
 982     }
 983
 984     return 0;
 985 }
 986
 987 static unsigned int rk29_camera_poll(struct file *file, poll_table *pt)
 988 {
 989     struct soc_camera_file *icf = file->private_data;
 990     struct rk29_buffer *buf;
 991
 992     buf = list_entry(icf->vb_vidq.stream.next, struct rk29_buffer,
 993                      vb.stream);
 994
 995     poll_wait(file, &buf->vb.done, pt);
 996
 997     if (buf->vb.state == VIDEOBUF_DONE ||
 998             buf->vb.state == VIDEOBUF_ERROR)
 999         return POLLIN|POLLRDNORM;
1000
1001     return 0;
1002 }
1003
1004 static int rk29_camera_querycap(struct soc_camera_host *ici,
1005                                 struct v4l2_capability *cap)
1006 {
1007     /* cap->name is set by the firendly caller:-> */
1008     strlcpy(cap->card, rk29_cam_driver_description, sizeof(cap->card));
1009     cap->version = RK29_CAM_VERSION_CODE;
1010     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1011
1012     return 0;
1013 }
1014
1015 static int rk29_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1016 {
1017     struct soc_camera_host *ici =
1018                     to_soc_camera_host(icd->dev.parent);
1019     struct rk29_camera_dev *pcdev = ici->priv;
1020     int ret = 0,tmp;
1021
1022         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1023                 rk29_camera_s_stream(icd, 0);
1024                 ret = icd->ops->suspend(icd, state);
1025
1026                 pcdev->reginfo_suspend.VipCtrl = read_vip_reg(RK29_VIP_CTRL);
1027                 pcdev->reginfo_suspend.VipCrop = read_vip_reg(RK29_VIP_CROP);
1028                 pcdev->reginfo_suspend.VipFs = read_vip_reg(RK29_VIP_FS);
1029                 pcdev->reginfo_suspend.VipIntMsk = read_vip_reg(RK29_VIP_INT_MASK);
1030                 pcdev->reginfo_suspend.VipCrm = read_vip_reg(RK29_VIP_CRM);
1031
1032                 tmp = pcdev->reginfo_suspend.VipFs>>16;         /* ddl@rock-chips.com */
1033                 tmp += pcdev->reginfo_suspend.VipCrop>>16;
1034                 pcdev->reginfo_suspend.VipFs = (pcdev->reginfo_suspend.VipFs & 0xffff) | (tmp<<16);
1035
1036                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1037                 rk29_camera_deactivate(pcdev);
1038
1039                 RK29CAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1040         } else {
1041                 RK29CAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1042         }
1043
1044     return ret;
1045 }
1046
1047 static int rk29_camera_resume(struct soc_camera_device *icd)
1048 {
1049     struct soc_camera_host *ici =
1050                     to_soc_camera_host(icd->dev.parent);
1051     struct rk29_camera_dev *pcdev = ici->priv;
1052     int ret = 0;
1053
1054         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1055                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1056                         rk29_camera_activate(pcdev, icd);
1057                         write_vip_reg(RK29_VIP_INT_MASK, pcdev->reginfo_suspend.VipIntMsk);
1058                         write_vip_reg(RK29_VIP_CRM, pcdev->reginfo_suspend.VipCrm);
1059                         write_vip_reg(RK29_VIP_CTRL, pcdev->reginfo_suspend.VipCtrl&~ENABLE_CAPTURE);
1060                         write_vip_reg(RK29_VIP_CROP, pcdev->reginfo_suspend.VipCrop);
1061                         write_vip_reg(RK29_VIP_FS, pcdev->reginfo_suspend.VipFs);
1062
1063                         rk29_videobuf_capture(pcdev->active);
1064                         rk29_camera_s_stream(icd, 1);
1065                 }
1066                 ret = icd->ops->resume(icd);
1067
1068                 RK29CAMERA_DG("%s Enter success\n",__FUNCTION__);
1069         } else {
1070                 RK29CAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1071         }
1072
1073     return ret;
1074 }
1075
1076 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable)
1077 {
1078         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1079     struct rk29_camera_dev *pcdev = ici->priv;
1080     int vip_ctrl_val;
1081
1082         WARN_ON(pcdev->icd != icd);
1083
1084         vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1085         if (enable) {
1086                 vip_ctrl_val |= ENABLE_CAPTURE;
1087         } else {
1088         vip_ctrl_val &= ~ENABLE_CAPTURE;
1089         }
1090         write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1091
1092         RK29CAMERA_DG("%s.. enable : %d\n", __FUNCTION__, enable);
1093         return 0;
1094 }
1095
1096 static struct soc_camera_host_ops rk29_soc_camera_host_ops =
1097 {
1098     .owner              = THIS_MODULE,
1099     .add                = rk29_camera_add_device,
1100     .remove             = rk29_camera_remove_device,
1101     .suspend    = rk29_camera_suspend,
1102     .resume             = rk29_camera_resume,
1103     .set_crop   = rk29_camera_set_crop,
1104     .get_formats        = rk29_camera_get_formats,
1105     .put_formats        = rk29_camera_put_formats,
1106     .set_fmt    = rk29_camera_set_fmt,
1107     .try_fmt    = rk29_camera_try_fmt,
1108     .init_videobuf      = rk29_camera_init_videobuf,
1109     .reqbufs    = rk29_camera_reqbufs,
1110     .poll               = rk29_camera_poll,
1111     .querycap   = rk29_camera_querycap,
1112     .set_bus_param      = rk29_camera_set_bus_param,
1113     .s_stream = rk29_camera_s_stream   /* ddl@rock-chips.com : Add stream control for host */
1114 };
1115 static int rk29_camera_probe(struct platform_device *pdev)
1116 {
1117     struct rk29_camera_dev *pcdev;
1118     struct resource *res;
1119     int irq;
1120     int err = 0;
1121
1122     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1123     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1124     irq = platform_get_irq(pdev, 0);
1125     if (!res || irq < 0) {
1126         err = -ENODEV;
1127         goto exit;
1128     }
1129
1130     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1131     if (!pcdev) {
1132         dev_err(&pdev->dev, "Could not allocate pcdev\n");
1133         err = -ENOMEM;
1134         goto exit_alloc;
1135     }
1136     rk29_camdev_info_ptr = pcdev;
1137
1138     /*config output clk*/
1139     pcdev->clk = clk_get(&pdev->dev, "vip");
1140     if (!pcdev->clk || IS_ERR(pcdev->clk))  {
1141         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk source\n");
1142         err = -ENOENT;
1143         goto exit_eclkget;
1144     }
1145
1146     dev_set_drvdata(&pdev->dev, pcdev);
1147     pcdev->res = res;
1148
1149     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
1150     if (pcdev->pdata && pcdev->pdata->io_init) {
1151         pcdev->pdata->io_init();
1152     }
1153
1154     INIT_LIST_HEAD(&pcdev->capture);
1155     spin_lock_init(&pcdev->lock);
1156
1157     /*
1158      * Request the regions.
1159      */
1160     if (!request_mem_region(res->start, res->end - res->start + 1,
1161                             RK29_CAM_DRV_NAME)) {
1162         err = -EBUSY;
1163         goto exit_reqmem;
1164     }
1165
1166     pcdev->base = ioremap(res->start, res->end - res->start + 1);
1167     if (pcdev->base == NULL) {
1168         dev_err(pcdev->dev, "ioremap() of registers failed\n");
1169         err = -ENXIO;
1170         goto exit_ioremap;
1171     }
1172
1173     pcdev->irq = irq;
1174     pcdev->dev = &pdev->dev;
1175
1176     /* config buffer address */
1177     /* request irq */
1178     err = request_irq(pcdev->irq, rk29_camera_irq, 0, RK29_CAM_DRV_NAME,
1179                       pcdev);
1180     if (err) {
1181         dev_err(pcdev->dev, "Camera interrupt register failed \n");
1182         goto exit_reqirq;
1183     }
1184
1185     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
1186     pcdev->soc_host.ops         = &rk29_soc_camera_host_ops;
1187     pcdev->soc_host.priv                = pcdev;
1188     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
1189     pcdev->soc_host.nr          = pdev->id;
1190
1191     err = soc_camera_host_register(&pcdev->soc_host);
1192     if (err)
1193         goto exit_free_irq;
1194
1195     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1196     return 0;
1197
1198 exit_free_irq:
1199     free_irq(pcdev->irq, pcdev);
1200 exit_reqirq:
1201     iounmap(pcdev->base);
1202 exit_ioremap:
1203     release_mem_region(res->start, res->end - res->start + 1);
1204 exit_reqmem:
1205     clk_put(pcdev->clk);
1206 exit_eclkget:
1207     kfree(pcdev);
1208 exit_alloc:
1209     rk29_camdev_info_ptr = NULL;
1210 exit:
1211     return err;
1212 }
1213
1214 static int __devexit rk29_camera_remove(struct platform_device *pdev)
1215 {
1216     struct rk29_camera_dev *pcdev = platform_get_drvdata(pdev);
1217     struct resource *res;
1218
1219     free_irq(pcdev->irq, pcdev);
1220
1221     soc_camera_host_unregister(&pcdev->soc_host);
1222
1223     res = pcdev->res;
1224     release_mem_region(res->start, res->end - res->start + 1);
1225
1226     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
1227         pcdev->pdata->io_deinit();
1228     }
1229
1230     kfree(pcdev);
1231     rk29_camdev_info_ptr = NULL;
1232     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
1233
1234     return 0;
1235 }
1236
1237 static struct platform_driver rk29_camera_driver =
1238 {
1239     .driver     = {
1240         .name   = RK29_CAM_DRV_NAME,
1241     },
1242     .probe              = rk29_camera_probe,
1243     .remove             = __devexit_p(rk29_camera_remove),
1244 };
1245
1246
1247 static int __devinit rk29_camera_init(void)
1248 {
1249     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1250     return platform_driver_register(&rk29_camera_driver);
1251 }
1252
1253 static void __exit rk29_camera_exit(void)
1254 {
1255     platform_driver_unregister(&rk29_camera_driver);
1256 }
1257
1258 device_initcall_sync(rk29_camera_init);
1259 module_exit(rk29_camera_exit);
1260
1261 MODULE_DESCRIPTION("RK29 Soc Camera Host driver");
1262 MODULE_AUTHOR("ddl <ddl@rock-chips>");
1263 MODULE_LICENSE("GPL");