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 #include <mach/rk29_camera.h>
  33 #include <mach/rk29_iomap.h>
  34 #include <mach/iomux.h>
  35 #include <media/v4l2-common.h>
  36 #include <media/v4l2-dev.h>
  37 #include <media/videobuf-dma-contig.h>
  38 #include <media/soc_camera.h>
  39 #include <media/soc_mediabus.h>
  40 #include <mach/rk29-ipp.h>
  41
  42
  43 static int debug;
  44 module_param(debug, int, S_IRUGO|S_IWUSR);
  45
  46 #define dprintk(level, fmt, arg...) do {                        \
  47         if (debug >= level)                                     \
  48         printk(KERN_WARNING"rk29xx_camera: " fmt , ## arg); } while (0)
  49
  50 #define RK29CAMERA_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  51 #define RK29CAMERA_DG(format, ...) dprintk(1, format, ## __VA_ARGS__)
  52
  53 // VIP Reg Offset
  54 #define RK29_VIP_AHBR_CTRL                0x00
  55 #define RK29_VIP_INT_MASK                 0x04
  56 #define RK29_VIP_INT_STS                  0x08
  57 #define RK29_VIP_STS                      0x0c
  58 #define RK29_VIP_CTRL                     0x10
  59 #define RK29_VIP_CAPTURE_F1SA_Y           0x14
  60 #define RK29_VIP_CAPTURE_F1SA_UV          0x18
  61 #define RK29_VIP_CAPTURE_F1SA_Cr          0x1c
  62 #define RK29_VIP_CAPTURE_F2SA_Y           0x20
  63 #define RK29_VIP_CAPTURE_F2SA_UV          0x24
  64 #define RK29_VIP_CAPTURE_F2SA_Cr          0x28
  65 #define RK29_VIP_FB_SR                    0x2c
  66 #define RK29_VIP_FS                       0x30
  67 #define RK29_VIP_VIPRESERVED              0x34
  68 #define RK29_VIP_CROP                     0x38
  69 #define RK29_VIP_CRM                      0x3c
  70 #define RK29_VIP_RESET                    0x40
  71 #define RK29_VIP_L_SFT                    0x44
  72
  73 //The key register bit descrition
  74 // VIP_CTRL Reg
  75 #define  DISABLE_CAPTURE              (0x00<<0)
  76 #define  ENABLE_CAPTURE               (0x01<<0)
  77 #define  HSY_HIGH_ACTIVE              (0x00<<1)
  78 #define  HSY_LOW_ACTIVE               (0x01<<1)
  79 #define  VIP_CCIR656                  (0x00<<2)
  80 #define  VIP_SENSOR                   (0x01<<2)
  81 #define  SENSOR_UYVY                  (0x00<<3)
  82 #define  SENSOR_YUYV                  (0x01<<3)
  83 #define  VIP_YUV                      (0x00<<4)
  84 #define  VIP_RAW                      (0x01<<4)
  85 #define  CON_OR_PIN                   (0x00<<5)
  86 #define  ONEFRAME                     (0x01<<5)
  87 #define  VIPREGYUV420                 (0x00<<6)
  88 #define  VIPREGYUV422                 (0x01<<6)
  89 #define  FIELD0_START                 (0x00<<7)
  90 #define  FIELD1_START                 (0x01<<7)
  91 #define  CONTINUOUS                   (0x00<<8)
  92 #define  PING_PONG                    (0x01<<8)
  93 #define  POSITIVE_EDGE                (0x00<<9)
  94 #define  NEGATIVE_EDGE                (0x01<<9)
  95 #define  VIPREGNTSC                   (0x00<<10)
  96 #define  VIPREGPAL                    (0x01<<10)
  97 #define  VIP_DATA_LITTLEEND           (0x00<<11)
  98 #define  VIP_DATA_BIGEND              (0x01<<11)
  99 #define  VSY_LOW_ACTIVE               (0x00<<12)
 100 #define  VSY_HIGH_ACTIVE              (0x01<<12)
 101 #define  VIP_RAWINPUT_BYPASS          (0x00<<13)
 102 #define  VIP_RAWINPUT_POSITIVE_EDGE   (0x01<<13)
 103 #define  VIP_RAWINPUT_NEGATIVE_EDGE   (0x02<<13)
 104
 105 // GRF_SOC_CON0 Reg
 106 #define  GRF_SOC_CON0_Reg             0xbc
 107 #define  VIP_AXIMASTER                (0x00<<0)
 108 #define  VIP_AHBMASTER                (0x01<<2)
 109
 110 // GRF_OS_REG0
 111 #define  GRF_OS_REG0                  0xd0
 112 #define  VIP_ACLK_DIV_HCLK_1          (0x00<<0)
 113 #define  VIP_ACLK_DIV_HCLK_2          (0x01<<0)
 114
 115
 116 #define MIN(x,y)   ((x<y) ? x: y)
 117 #define MAX(x,y)    ((x>y) ? x: y)
 118 #define RK29_SENSOR_24MHZ      24           /* MHz */
 119 #define RK29_SENSOR_48MHZ      48
 120
 121 #define write_vip_reg(addr, val)  __raw_writel(val, addr+(rk29_camdev_info_ptr->base))
 122 #define read_vip_reg(addr) __raw_readl(addr+(rk29_camdev_info_ptr->base))
 123 #define mask_vip_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 124
 125 #define write_grf_reg(addr, val)  __raw_writel(val, addr+RK29_GRF_BASE)
 126 #define read_grf_reg(addr) __raw_readl(addr+RK29_GRF_BASE)
 127 #define mask_grf_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 128
 129 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 130 #define CAM_WORKQUEUE_IS_EN()   ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)\
 131                                   || (pcdev->icd_cb.sensor_cb))
 132 #define CAM_IPPWORK_IS_EN()     ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height))
 133 #else
 134 #define CAM_WORKQUEUE_IS_EN()   (true)
 135 #define CAM_IPPWORK_IS_EN()     ((pcdev->zoominfo.a.c.width != pcdev->icd->user_width) || (pcdev->zoominfo.a.c.height != pcdev->icd->user_height))
 136 #endif
 137 //Configure Macro
 138 /*
 139 *            Driver Version Note
 140 *
 141 *v0.0.x : this driver is 2.6.32 kernel driver;
 142 *v0.1.x : this driver is 3.0.8 kernel driver;
 143 *
 144 *v0.x.1 : this driver first support rk2918;
 145 *v0.x.2 : fix this driver support v4l2 format is V4L2_PIX_FMT_NV12 and V4L2_PIX_FMT_NV16,is not V4L2_PIX_FMT_YUV420
 146 *         and V4L2_PIX_FMT_YUV422P;
 147 *v0.x.3 : this driver support VIDIOC_ENUM_FRAMEINTERVALS;
 148 *v0.x.4 : this driver support digital zoom;
 149 *v0.x.5 : this driver support test framerate and query framerate from board file configuration;
 150 *v0.x.6 : this driver improve test framerate method;
 151 */
 152 #define RK29_CAM_VERSION_CODE KERNEL_VERSION(0, 1, 6)
 153
 154 /* limit to rk29 hardware capabilities */
 155 #define RK29_CAM_BUS_PARAM   (SOCAM_MASTER |\
 156                 SOCAM_HSYNC_ACTIVE_HIGH |\
 157                 SOCAM_HSYNC_ACTIVE_LOW |\
 158                 SOCAM_VSYNC_ACTIVE_HIGH |\
 159                 SOCAM_VSYNC_ACTIVE_LOW |\
 160                 SOCAM_PCLK_SAMPLE_RISING |\
 161                 SOCAM_PCLK_SAMPLE_FALLING|\
 162                 SOCAM_DATA_ACTIVE_HIGH |\
 163                 SOCAM_DATA_ACTIVE_LOW|\
 164                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 165                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 166
 167 #define RK29_CAM_W_MIN        48
 168 #define RK29_CAM_H_MIN        32
 169 #define RK29_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 170 #define RK29_CAM_H_MAX        2764
 171 #define RK29_CAM_FRAME_INVAL_INIT 3
 172 #define RK29_CAM_FRAME_INVAL_DC 3          /* ddl@rock-chips.com :  */
 173 #define RK29_CAM_FRAME_MEASURE  5
 174
 175 #define RK29_CAM_AXI   0
 176 #define RK29_CAM_AHB   1
 177 #define CONFIG_RK29_CAM_WORK_BUS    RK29_CAM_AXI
 178
 179 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 180 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 181
 182 /* buffer for one video frame */
 183 struct rk29_buffer
 184 {
 185     /* common v4l buffer stuff -- must be first */
 186     struct videobuf_buffer vb;
 187     enum v4l2_mbus_pixelcode    code;
 188     int                 inwork;
 189 };
 190 enum rk29_camera_reg_state
 191 {
 192         Reg_Invalidate,
 193         Reg_Validate
 194 };
 195
 196 struct rk29_camera_reg
 197 {
 198         unsigned int VipCtrl;
 199         unsigned int VipCrop;
 200         unsigned int VipFs;
 201         unsigned int VipIntMsk;
 202         unsigned int VipCrm;
 203         enum rk29_camera_reg_state Inval;
 204 };
 205 struct rk29_camera_work
 206 {
 207         struct videobuf_buffer *vb;
 208         struct rk29_camera_dev *pcdev;
 209         struct work_struct work;
 210 };
 211 struct rk29_camera_frmivalenum
 212 {
 213     struct v4l2_frmivalenum fival;
 214     struct rk29_camera_frmivalenum *nxt;
 215 };
 216 struct rk29_camera_frmivalinfo
 217 {
 218     struct soc_camera_device *icd;
 219     struct rk29_camera_frmivalenum *fival_list;
 220 };
 221 struct rk29_camera_zoominfo
 222 {
 223     struct semaphore sem;
 224     struct v4l2_crop a;
 225     int zoom_rate;
 226 };
 227 struct rk29_camera_dev
 228 {
 229     struct soc_camera_host      soc_host;
 230     struct device               *dev;
 231     /* RK2827x is only supposed to handle one camera on its Quick Capture
 232      * interface. If anyone ever builds hardware to enable more than
 233      * one camera, they will have to modify this driver too */
 234     struct soc_camera_device *icd;
 235
 236         struct clk *aclk_ddr_lcdc;
 237         struct clk *aclk_disp_matrix;
 238
 239         struct clk *hclk_cpu_display;
 240         struct clk *vip_slave;
 241
 242         struct clk *vip_out;
 243         struct clk *vip_input;
 244         struct clk *vip_bus;
 245
 246         struct clk *hclk_disp_matrix;
 247         struct clk *vip_matrix;
 248
 249         struct clk *pd_display;
 250
 251     void __iomem *base;
 252         void __iomem *grf_base;
 253     int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 254     unsigned int irq;
 255         unsigned int fps;
 256     unsigned long frame_interval;
 257         unsigned int pixfmt;
 258         unsigned int vipmem_phybase;
 259         unsigned int vipmem_size;
 260         unsigned int vipmem_bsize;
 261         int host_width;
 262         int host_height;
 263     int icd_width;
 264     int icd_height;
 265
 266     struct rk29camera_platform_data *pdata;
 267     struct resource             *res;
 268
 269     struct list_head    capture;
 270     struct rk29_camera_zoominfo zoominfo;
 271
 272     spinlock_t          lock;
 273
 274     struct videobuf_buffer      *active;
 275         struct rk29_camera_reg reginfo_suspend;
 276         struct workqueue_struct *camera_wq;
 277         struct rk29_camera_work *camera_work;
 278         unsigned int camera_work_count;
 279         struct hrtimer fps_timer;
 280         struct work_struct camera_reinit_work;
 281     int icd_init;
 282     rk29_camera_sensor_cb_s icd_cb;
 283     struct rk29_camera_frmivalinfo icd_frmival[2];
 284 };
 285
 286 static const struct v4l2_queryctrl rk29_camera_controls[] =
 287 {
 288         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
 289     {
 290         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 291         .type           = V4L2_CTRL_TYPE_INTEGER,
 292         .name           = "DigitalZoom Control",
 293         .minimum        = 100,
 294         .maximum        = 300,
 295         .step           = 5,
 296         .default_value = 100,
 297     },
 298     #endif
 299 };
 300
 301 static DEFINE_MUTEX(camera_lock);
 302 static const char *rk29_cam_driver_description = "RK29_Camera";
 303 static struct rk29_camera_dev *rk29_camdev_info_ptr;
 304
 305 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable);
 306
 307
 308 /*
 309  *  Videobuf operations
 310  */
 311 static int rk29_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 312                                unsigned int *size)
 313 {
 314     struct soc_camera_device *icd = vq->priv_data;
 315         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 316     struct rk29_camera_dev *pcdev = ici->priv;
 317     int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 318                                                 icd->current_fmt->host_fmt);
 319
 320     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 321
 322         if (bytes_per_line < 0)
 323                 return bytes_per_line;
 324
 325         /* planar capture requires Y, U and V buffers to be page aligned */
 326         *size = PAGE_ALIGN(bytes_per_line*icd->user_height);       /* Y pages UV pages, yuv422*/
 327         pcdev->vipmem_bsize = PAGE_ALIGN(bytes_per_line * pcdev->host_height);
 328
 329
 330         if (CAM_WORKQUEUE_IS_EN()) {
 331         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 332         if (CAM_IPPWORK_IS_EN())
 333         #endif
 334         {
 335             BUG_ON(pcdev->vipmem_size<pcdev->vipmem_bsize);
 336                 if (*count > pcdev->vipmem_size/pcdev->vipmem_bsize) {    /* Buffers must be limited, when this resolution is genered by IPP */
 337                         *count = pcdev->vipmem_size/pcdev->vipmem_bsize;
 338                 }
 339         }
 340                 if ((pcdev->camera_work_count != *count) && pcdev->camera_work) {
 341                         kfree(pcdev->camera_work);
 342                         pcdev->camera_work = NULL;
 343                         pcdev->camera_work_count = 0;
 344                 }
 345
 346                 if (pcdev->camera_work == NULL) {
 347                         pcdev->camera_work = kmalloc(sizeof(struct rk29_camera_work)*(*count), GFP_KERNEL);
 348                         if (pcdev->camera_work == NULL) {
 349                                 RK29CAMERA_TR("\n %s kmalloc fail\n", __FUNCTION__);
 350                                 BUG();
 351                         }
 352                         pcdev->camera_work_count = *count;
 353                 }
 354         }
 355
 356     RK29CAMERA_DG("%s..%d.. videobuf size:%d, vipmem_buf size:%d, count:%d \n",__FUNCTION__,__LINE__, *size,pcdev->vipmem_size, *count);
 357
 358     return 0;
 359 }
 360 static void rk29_videobuf_free(struct videobuf_queue *vq, struct rk29_buffer *buf)
 361 {
 362     struct soc_camera_device *icd = vq->priv_data;
 363
 364     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 365             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 366
 367         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 368         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 369                 return;
 370
 371     if (in_interrupt())
 372         BUG();
 373     /*
 374          * This waits until this buffer is out of danger, i.e., until it is no
 375          * longer in STATE_QUEUED or STATE_ACTIVE
 376          */
 377         //videobuf_waiton(vq, &buf->vb, 0, 0);
 378     videobuf_dma_contig_free(vq, &buf->vb);
 379     dev_dbg(&icd->dev, "%s freed\n", __func__);
 380     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 381         return;
 382 }
 383 static int rk29_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 384 {
 385     struct soc_camera_device *icd = vq->priv_data;
 386     struct rk29_buffer *buf;
 387     int ret;
 388     int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 389                                                 icd->current_fmt->host_fmt);
 390         if (bytes_per_line < 0)
 391                 return bytes_per_line;
 392
 393     buf = container_of(vb, struct rk29_buffer, vb);
 394
 395     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 396             vb, vb->baddr, vb->bsize);
 397
 398     //RK29CAMERA_TR("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 399
 400     /* Added list head initialization on alloc */
 401     WARN_ON(!list_empty(&vb->queue));
 402
 403     /* This can be useful if you want to see if we actually fill
 404      * the buffer with something */
 405     //memset((void *)vb->baddr, 0xaa, vb->bsize);
 406
 407     BUG_ON(NULL == icd->current_fmt);
 408
 409     if (buf->code    != icd->current_fmt->code ||
 410             vb->width   != icd->user_width ||
 411             vb->height  != icd->user_height ||
 412              vb->field   != field) {
 413         buf->code    = icd->current_fmt->code;
 414         vb->width   = icd->user_width;
 415         vb->height  = icd->user_height;
 416         vb->field   = field;
 417         vb->state   = VIDEOBUF_NEEDS_INIT;
 418     }
 419
 420     vb->size = bytes_per_line*vb->height;          /* ddl@rock-chips.com : fmt->depth is coorect */
 421     if (0 != vb->baddr && vb->bsize < vb->size) {
 422         ret = -EINVAL;
 423         goto out;
 424     }
 425
 426     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 427         ret = videobuf_iolock(vq, vb, NULL);
 428         if (ret) {
 429             goto fail;
 430         }
 431         vb->state = VIDEOBUF_PREPARED;
 432     }
 433     //RK29CAMERA_TR("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 434     return 0;
 435 fail:
 436     rk29_videobuf_free(vq, buf);
 437 out:
 438     return ret;
 439 }
 440
 441 static inline void rk29_videobuf_capture(struct videobuf_buffer *vb)
 442 {
 443         unsigned int y_addr,uv_addr;
 444         struct rk29_camera_dev *pcdev = rk29_camdev_info_ptr;
 445
 446     if (vb) {
 447                 if (CAM_WORKQUEUE_IS_EN()) {
 448                         y_addr = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 449                         uv_addr = y_addr + pcdev->host_width*pcdev->host_height;
 450
 451                         if (y_addr > (pcdev->vipmem_phybase + pcdev->vipmem_size - pcdev->vipmem_bsize)) {
 452                                 RK29CAMERA_TR("vipmem for IPP is overflow! %dx%d -> %dx%d vb_index:%d\n",pcdev->host_width,pcdev->host_height,
 453                                                   pcdev->icd->user_width,pcdev->icd->user_height, vb->i);
 454                                 BUG();
 455                         }
 456                 } else {
 457                         y_addr = vb->boff;
 458                         uv_addr = y_addr + vb->width * vb->height;
 459                 }
 460         write_vip_reg(RK29_VIP_CAPTURE_F1SA_Y, y_addr);
 461         write_vip_reg(RK29_VIP_CAPTURE_F1SA_UV, uv_addr);
 462         write_vip_reg(RK29_VIP_CAPTURE_F2SA_Y, y_addr);
 463         write_vip_reg(RK29_VIP_CAPTURE_F2SA_UV, uv_addr);
 464         write_vip_reg(RK29_VIP_FB_SR,  0x00000002);//frame1 has been ready to receive data,frame 2 is not used
 465     }
 466 }
 467 /* Locking: Caller holds q->irqlock */
 468 static void rk29_videobuf_queue(struct videobuf_queue *vq,
 469                                 struct videobuf_buffer *vb)
 470 {
 471     struct soc_camera_device *icd = vq->priv_data;
 472     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 473     struct rk29_camera_dev *pcdev = ici->priv;
 474
 475     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 476             vb, vb->baddr, vb->bsize);
 477
 478     vb->state = VIDEOBUF_QUEUED;
 479
 480         if (list_empty(&pcdev->capture)) {
 481                 list_add_tail(&vb->queue, &pcdev->capture);
 482         } else {
 483                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 484                         list_add_tail(&vb->queue, &pcdev->capture);
 485                 else
 486                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 487         }
 488     if (!pcdev->active) {
 489         pcdev->active = vb;
 490         rk29_videobuf_capture(vb);
 491     }
 492 }
 493 static int rk29_pixfmt2ippfmt(unsigned int pixfmt, int *ippfmt)
 494 {
 495         switch (pixfmt)
 496         {
 497                 case V4L2_PIX_FMT_NV16:
 498         case V4L2_PIX_FMT_YUV422P:
 499                 {
 500                         *ippfmt = IPP_Y_CBCR_H2V1;
 501                         break;
 502                 }
 503                 case V4L2_PIX_FMT_NV12:
 504         case V4L2_PIX_FMT_YUV420:
 505                 {
 506                         *ippfmt = IPP_Y_CBCR_H2V2;
 507                         break;
 508                 }
 509                 default:
 510                         goto rk29_pixfmt2ippfmt_err;
 511         }
 512
 513         return 0;
 514 rk29_pixfmt2ippfmt_err:
 515         return -1;
 516 }
 517 static void rk29_camera_capture_process(struct work_struct *work)
 518 {
 519         struct rk29_camera_work *camera_work = container_of(work, struct rk29_camera_work, work);
 520         struct videobuf_buffer *vb = camera_work->vb;
 521         struct rk29_camera_dev *pcdev = camera_work->pcdev;
 522         struct rk29_ipp_req ipp_req;
 523         unsigned long int flags;
 524     int src_y_offset,src_uv_offset,dst_y_offset,dst_uv_offset,src_y_size,dst_y_size;
 525     int scale_times,w,h,vipdata_base;
 526
 527     /*
 528     *ddl@rock-chips.com:
 529     * IPP Dest image resolution is 2047x1088, so scale operation break up some times
 530     */
 531     if ((pcdev->icd->user_width > 0x7f0) || (pcdev->icd->user_height > 0x430)) {
 532         scale_times = MAX((pcdev->icd->user_width/0x7f0),(pcdev->icd->user_height/0x430));
 533         scale_times++;
 534     } else {
 535         scale_times = 1;
 536     }
 537
 538     memset(&ipp_req, 0, sizeof(struct rk29_ipp_req));
 539
 540     down(&pcdev->zoominfo.sem);
 541
 542     ipp_req.timeout = 100;
 543     ipp_req.flag = IPP_ROT_0;
 544     ipp_req.src0.w = pcdev->zoominfo.a.c.width/scale_times;
 545     ipp_req.src0.h = pcdev->zoominfo.a.c.height/scale_times;
 546     ipp_req.src_vir_w = pcdev->host_width;
 547     rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.src0.fmt);
 548     ipp_req.dst0.w = pcdev->icd->user_width/scale_times;
 549     ipp_req.dst0.h = pcdev->icd->user_height/scale_times;
 550     ipp_req.dst_vir_w = pcdev->icd->user_width;
 551     rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.dst0.fmt);
 552
 553     vipdata_base = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 554     src_y_size = pcdev->host_width*pcdev->host_height;
 555     dst_y_size = pcdev->icd->user_width*pcdev->icd->user_height;
 556
 557     for (h=0; h<scale_times; h++) {
 558         for (w=0; w<scale_times; w++) {
 559
 560             src_y_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width
 561                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 562                     src_uv_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width/2
 563                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 564
 565             dst_y_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times + pcdev->icd->user_width*w/scale_times;
 566             dst_uv_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times/2 + pcdev->icd->user_width*w/scale_times;
 567
 568                 ipp_req.src0.YrgbMst = vipdata_base + src_y_offset;
 569                 ipp_req.src0.CbrMst = vipdata_base + src_y_size + src_uv_offset;
 570                 ipp_req.dst0.YrgbMst = vb->boff + dst_y_offset;
 571                 ipp_req.dst0.CbrMst = vb->boff + dst_y_size + dst_uv_offset;
 572
 573                 if (ipp_blit_sync(&ipp_req)) {
 574                         spin_lock_irqsave(&pcdev->lock, flags);
 575                         vb->state = VIDEOBUF_ERROR;
 576                         spin_unlock_irqrestore(&pcdev->lock, flags);
 577
 578                 RK29CAMERA_TR("Capture image(vb->i:0x%x) which IPP operated is error:\n",vb->i);
 579                 RK29CAMERA_TR("widx:%d hidx:%d ",w,h);
 580                 RK29CAMERA_TR("%dx%d@(%d,%d)->%dx%d\n",pcdev->zoominfo.a.c.width,pcdev->zoominfo.a.c.height,pcdev->zoominfo.a.c.left,pcdev->zoominfo.a.c.top,pcdev->icd->user_width,pcdev->icd->user_height);
 581                 RK29CAMERA_TR("ipp_req.src0.YrgbMst:0x%x ipp_req.src0.CbrMst:0x%x \n", ipp_req.src0.YrgbMst,ipp_req.src0.CbrMst);
 582                 RK29CAMERA_TR("ipp_req.src0.w:0x%x ipp_req.src0.h:0x%x \n",ipp_req.src0.w,ipp_req.src0.h);
 583                 RK29CAMERA_TR("ipp_req.src0.fmt:0x%x\n",ipp_req.src0.fmt);
 584                 RK29CAMERA_TR("ipp_req.dst0.YrgbMst:0x%x ipp_req.dst0.CbrMst:0x%x \n",ipp_req.dst0.YrgbMst,ipp_req.dst0.CbrMst);
 585                 RK29CAMERA_TR("ipp_req.dst0.w:0x%x ipp_req.dst0.h:0x%x \n",ipp_req.dst0.w ,ipp_req.dst0.h);
 586                 RK29CAMERA_TR("ipp_req.dst0.fmt:0x%x\n",ipp_req.dst0.fmt);
 587                 RK29CAMERA_TR("ipp_req.src_vir_w:0x%x ipp_req.dst_vir_w :0x%x\n",ipp_req.src_vir_w ,ipp_req.dst_vir_w);
 588                 RK29CAMERA_TR("ipp_req.timeout:0x%x ipp_req.flag :0x%x\n",ipp_req.timeout,ipp_req.flag);
 589
 590                         goto do_ipp_err;
 591                 }
 592         }
 593     }
 594
 595     if (pcdev->icd_cb.sensor_cb)
 596         (pcdev->icd_cb.sensor_cb)(vb);
 597
 598 do_ipp_err:
 599     up(&pcdev->zoominfo.sem);
 600     wake_up(&(camera_work->vb->done));
 601         return;
 602 }
 603 static irqreturn_t rk29_camera_irq(int irq, void *data)
 604 {
 605     struct rk29_camera_dev *pcdev = data;
 606     struct videobuf_buffer *vb;
 607         struct rk29_camera_work *wk;
 608     static struct timeval first_tv;
 609     struct timeval tv;
 610
 611     read_vip_reg(RK29_VIP_INT_STS);    /* clear vip interrupte single  */
 612     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 613     if (read_vip_reg(RK29_VIP_FB_SR) & 0x01) {
 614         if (!pcdev->fps) {
 615             do_gettimeofday(&first_tv);
 616         }
 617                 pcdev->fps++;
 618                 if (!pcdev->active)
 619                         goto RK29_CAMERA_IRQ_END;
 620
 621         if (pcdev->frame_inval>0) {
 622             pcdev->frame_inval--;
 623             rk29_videobuf_capture(pcdev->active);
 624             goto RK29_CAMERA_IRQ_END;
 625         } else if (pcdev->frame_inval) {
 626                 RK29CAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 627             pcdev->frame_inval = 0;
 628         }
 629
 630         if(pcdev->fps == RK29_CAM_FRAME_MEASURE) {
 631             do_gettimeofday(&tv);
 632             pcdev->frame_interval = ((tv.tv_sec*1000000 + tv.tv_usec) - (first_tv.tv_sec*1000000 + first_tv.tv_usec))
 633                                     /(RK29_CAM_FRAME_MEASURE-1);
 634         }
 635
 636         vb = pcdev->active;
 637                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 638                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 639                 list_del_init(&vb->queue);
 640                 }
 641
 642         pcdev->active = NULL;
 643         if (!list_empty(&pcdev->capture)) {
 644             pcdev->active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 645                         if (pcdev->active) {
 646                                 rk29_videobuf_capture(pcdev->active);
 647                         }
 648         }
 649
 650         if (pcdev->active == NULL) {
 651                         RK29CAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 652         }
 653
 654                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 655                 vb->state = VIDEOBUF_DONE;
 656                 do_gettimeofday(&vb->ts);
 657                 vb->field_count++;
 658                 }
 659
 660                 if (CAM_WORKQUEUE_IS_EN()) {
 661                         wk = pcdev->camera_work + vb->i;
 662                         INIT_WORK(&(wk->work), rk29_camera_capture_process);
 663                         wk->vb = vb;
 664                         wk->pcdev = pcdev;
 665                         queue_work(pcdev->camera_wq, &(wk->work));
 666                 } else {
 667                         wake_up(&vb->done);
 668                 }
 669     }
 670
 671 RK29_CAMERA_IRQ_END:
 672     return IRQ_HANDLED;
 673 }
 674
 675
 676 static void rk29_videobuf_release(struct videobuf_queue *vq,
 677                                   struct videobuf_buffer *vb)
 678 {
 679     struct rk29_buffer *buf = container_of(vb, struct rk29_buffer, vb);
 680     struct soc_camera_device *icd = vq->priv_data;
 681     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 682     struct rk29_camera_dev *pcdev = ici->priv;
 683 #ifdef DEBUG
 684     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 685             vb, vb->baddr, vb->bsize);
 686
 687     switch (vb->state)
 688     {
 689         case VIDEOBUF_ACTIVE:
 690             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 691             break;
 692         case VIDEOBUF_QUEUED:
 693             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 694             break;
 695         case VIDEOBUF_PREPARED:
 696             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 697             break;
 698         default:
 699             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 700             break;
 701     }
 702 #endif
 703         if (vb == pcdev->active) {
 704                 RK29CAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 705                 interruptible_sleep_on_timeout(&vb->done, 100);
 706                 RK29CAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 707         }
 708     rk29_videobuf_free(vq, buf);
 709 }
 710
 711 static struct videobuf_queue_ops rk29_videobuf_ops =
 712 {
 713     .buf_setup      = rk29_videobuf_setup,
 714     .buf_prepare    = rk29_videobuf_prepare,
 715     .buf_queue      = rk29_videobuf_queue,
 716     .buf_release    = rk29_videobuf_release,
 717 };
 718
 719 static void rk29_camera_init_videobuf(struct videobuf_queue *q,
 720                                       struct soc_camera_device *icd)
 721 {
 722     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 723     struct rk29_camera_dev *pcdev = ici->priv;
 724
 725     /* We must pass NULL as dev pointer, then all pci_* dma operations
 726      * transform to normal dma_* ones. */
 727     videobuf_queue_dma_contig_init(q,
 728                                    &rk29_videobuf_ops,
 729                                    ici->v4l2_dev.dev, &pcdev->lock,
 730                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 731                                    V4L2_FIELD_NONE,
 732                                    sizeof(struct rk29_buffer),
 733                                    icd,&icd->video_lock);
 734 }
 735 static int rk29_camera_activate(struct rk29_camera_dev *pcdev, struct soc_camera_device *icd)
 736 {
 737     unsigned long sensor_bus_flags = SOCAM_MCLK_24MHZ;
 738     struct clk *parent;
 739
 740     RK29CAMERA_DG("%s..%d.. \n",__FUNCTION__,__LINE__);
 741     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
 742                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus || !pcdev->pd_display ||
 743                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) || IS_ERR(pcdev->pd_display) ||
 744                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
 745
 746         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
 747         goto RK29_CAMERA_ACTIVE_ERR;
 748     }
 749
 750         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
 751                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
 752
 753         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
 754         goto RK29_CAMERA_ACTIVE_ERR;
 755     }
 756
 757         clk_enable(pcdev->pd_display);
 758
 759         clk_enable(pcdev->aclk_ddr_lcdc);
 760         clk_enable(pcdev->aclk_disp_matrix);
 761
 762         clk_enable(pcdev->hclk_cpu_display);
 763         clk_enable(pcdev->vip_slave);
 764
 765 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 766         clk_enable(pcdev->hclk_disp_matrix);
 767         clk_enable(pcdev->vip_matrix);
 768 #endif
 769
 770         clk_enable(pcdev->vip_input);
 771         clk_enable(pcdev->vip_bus);
 772
 773     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 774         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 775
 776     if (sensor_bus_flags & SOCAM_MCLK_48MHZ) {
 777         parent = clk_get(NULL, "clk48m");
 778         if (!parent || IS_ERR(parent))
 779              goto RK29_CAMERA_ACTIVE_ERR;
 780     } else if (sensor_bus_flags & SOCAM_MCLK_27MHZ) {
 781         parent = clk_get(NULL, "extclk");
 782         if (!parent || IS_ERR(parent))
 783              goto RK29_CAMERA_ACTIVE_ERR;
 784     } else {
 785         parent = clk_get(NULL, "xin24m");
 786         if (!parent || IS_ERR(parent))
 787              goto RK29_CAMERA_ACTIVE_ERR;
 788     }
 789
 790     clk_set_parent(pcdev->vip_out, parent);
 791
 792     clk_enable(pcdev->vip_out);
 793     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_VIP_CLKOUT);
 794     ndelay(10);
 795
 796 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 797         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)|VIP_AHBMASTER);  //VIP Config to AHB
 798         write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)&(~VIP_ACLK_DIV_HCLK_2));   //aclk:hclk = 1:1
 799 #else
 800         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)&(~VIP_AHBMASTER));  //VIP Config to AXI
 801     write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)|VIP_ACLK_DIV_HCLK_2);   //aclk:hclk = 2:1
 802 #endif
 803         ndelay(10);
 804
 805     write_vip_reg(RK29_VIP_RESET, 0x76543210);  /* ddl@rock-chips.com : vip software reset */
 806     udelay(10);
 807
 808     write_vip_reg(RK29_VIP_AHBR_CTRL, 0x07);   /* ddl@rock-chips.com : vip ahb burst 16 */
 809     write_vip_reg(RK29_VIP_INT_MASK, 0x01);    //capture complete interrupt enable
 810     write_vip_reg(RK29_VIP_CRM,  0x00000000);  //Y/CB/CR color modification
 811
 812     return 0;
 813 RK29_CAMERA_ACTIVE_ERR:
 814     return -ENODEV;
 815 }
 816
 817 static void rk29_camera_deactivate(struct rk29_camera_dev *pcdev)
 818 {
 819     //pcdev->active = NULL;
 820
 821     write_vip_reg(RK29_VIP_CTRL, 0);
 822     read_vip_reg(RK29_VIP_INT_STS);             //clear vip interrupte single
 823
 824     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_GPIO1B4);
 825     clk_disable(pcdev->vip_out);
 826
 827         clk_disable(pcdev->vip_input);
 828         clk_disable(pcdev->vip_bus);
 829
 830 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 831         clk_disable(pcdev->hclk_disp_matrix);
 832         clk_disable(pcdev->vip_matrix);
 833 #endif
 834
 835         clk_disable(pcdev->hclk_cpu_display);
 836         clk_disable(pcdev->vip_slave);
 837
 838         clk_disable(pcdev->aclk_ddr_lcdc);
 839         clk_disable(pcdev->aclk_disp_matrix);
 840
 841         clk_disable(pcdev->pd_display);
 842     return;
 843 }
 844
 845 /* The following two functions absolutely depend on the fact, that
 846  * there can be only one camera on RK28 quick capture interface */
 847 static int rk29_camera_add_device(struct soc_camera_device *icd)
 848 {
 849     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 850     struct rk29_camera_dev *pcdev = ici->priv;
 851     struct device *control = to_soc_camera_control(icd);
 852     struct v4l2_subdev *sd;
 853     int ret,i,icd_catch;
 854     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
 855
 856     mutex_lock(&camera_lock);
 857
 858     if (pcdev->icd) {
 859         ret = -EBUSY;
 860         goto ebusy;
 861     }
 862
 863     dev_info(&icd->dev, "RK29 Camera driver attached to camera%d(%s)\n",
 864              icd->devnum,dev_name(icd->pdev));
 865
 866         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 867     pcdev->active = NULL;
 868     pcdev->icd = NULL;
 869         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 870     pcdev->zoominfo.zoom_rate = 100;
 871
 872         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 873      * if app havn't dequeue all videobuf before close camera device;
 874         */
 875     INIT_LIST_HEAD(&pcdev->capture);
 876
 877     ret = rk29_camera_activate(pcdev,icd);
 878     if (ret)
 879         goto ebusy;
 880
 881     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 882     if (control) {
 883         sd = dev_get_drvdata(control);
 884                 v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_IOREQUEST,(void*)pcdev->pdata);
 885         #if 0
 886         ret = v4l2_subdev_call(sd,core, init, 0);
 887         if (ret)
 888             goto ebusy;
 889         #endif
 890         v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_CB_REGISTER,(void*)(&pcdev->icd_cb));
 891     }
 892
 893     pcdev->icd = icd;
 894     pcdev->icd_init = 0;
 895
 896     icd_catch = 0;
 897     for (i=0; i<2; i++) {
 898         if (pcdev->icd_frmival[i].icd == icd)
 899             icd_catch = 1;
 900         if (pcdev->icd_frmival[i].icd == NULL) {
 901             pcdev->icd_frmival[i].icd = icd;
 902             icd_catch = 1;
 903         }
 904     }
 905
 906     if (icd_catch == 0) {
 907         fival_list = pcdev->icd_frmival[0].fival_list;
 908         fival_nxt = fival_list;
 909         while(fival_nxt != NULL) {
 910             fival_nxt = fival_list->nxt;
 911             kfree(fival_list);
 912             fival_list = fival_nxt;
 913         }
 914         pcdev->icd_frmival[0].icd = icd;
 915         pcdev->icd_frmival[0].fival_list = kzalloc(sizeof(struct rk29_camera_frmivalenum),GFP_KERNEL);
 916     }
 917 ebusy:
 918     mutex_unlock(&camera_lock);
 919
 920     return ret;
 921 }
 922 static void rk29_camera_remove_device(struct soc_camera_device *icd)
 923 {
 924     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 925     struct rk29_camera_dev *pcdev = ici->priv;
 926         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 927
 928         mutex_lock(&camera_lock);
 929     BUG_ON(icd != pcdev->icd);
 930
 931     dev_info(&icd->dev, "RK29 Camera driver detached from camera%d(%s)\n",
 932              icd->devnum,dev_name(icd->pdev));
 933
 934         /* ddl@rock-chips.com: Application will call VIDIOC_STREAMOFF before close device, but
 935            stream may be turn on again before close device, if suspend and resume happened. */
 936         if (read_vip_reg(RK29_VIP_CTRL) & ENABLE_CAPTURE) {
 937                 rk29_camera_s_stream(icd,0);
 938         }
 939
 940     v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
 941         rk29_camera_deactivate(pcdev);
 942
 943         if (pcdev->camera_work) {
 944                 kfree(pcdev->camera_work);
 945                 pcdev->camera_work = NULL;
 946                 pcdev->camera_work_count = 0;
 947         }
 948
 949         pcdev->active = NULL;
 950     pcdev->icd = NULL;
 951     pcdev->icd_cb.sensor_cb = NULL;
 952         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 953         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 954      * if app havn't dequeue all videobuf before close camera device;
 955         */
 956     INIT_LIST_HEAD(&pcdev->capture);
 957
 958         mutex_unlock(&camera_lock);
 959         RK29CAMERA_DG("%s exit\n",__FUNCTION__);
 960
 961         return;
 962 }
 963 static int rk29_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 964 {
 965     unsigned long bus_flags, camera_flags, common_flags;
 966     unsigned int vip_ctrl_val = 0;
 967         const struct soc_mbus_pixelfmt *fmt;
 968         int ret = 0;
 969
 970     RK29CAMERA_DG("%s..%d..\n",__FUNCTION__,__LINE__);
 971
 972         fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
 973         if (!fmt)
 974                 return -EINVAL;
 975
 976     bus_flags = RK29_CAM_BUS_PARAM;
 977         /* If requested data width is supported by the platform, use it */
 978         switch (fmt->bits_per_sample) {
 979         case 10:
 980                 if (!(bus_flags & SOCAM_DATAWIDTH_10))
 981                         return -EINVAL;
 982                 break;
 983         case 9:
 984                 if (!(bus_flags & SOCAM_DATAWIDTH_9))
 985                         return -EINVAL;
 986                 break;
 987         case 8:
 988                 if (!(bus_flags & SOCAM_DATAWIDTH_8))
 989                         return -EINVAL;
 990                 break;
 991         default:
 992                 return -EINVAL;
 993         }
 994
 995         if (icd->ops->query_bus_param)
 996         camera_flags = icd->ops->query_bus_param(icd);
 997         else
 998                 camera_flags = 0;
 999
1000     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
1001     if (!common_flags) {
1002         ret = -EINVAL;
1003         goto RK29_CAMERA_SET_BUS_PARAM_END;
1004     }
1005
1006     ret = icd->ops->set_bus_param(icd, common_flags);
1007     if (ret < 0)
1008         goto RK29_CAMERA_SET_BUS_PARAM_END;
1009
1010     vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1011     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
1012         vip_ctrl_val |= NEGATIVE_EDGE;
1013     } else {
1014                 vip_ctrl_val &= ~NEGATIVE_EDGE;
1015     }
1016     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
1017         vip_ctrl_val |= HSY_LOW_ACTIVE;
1018     } else {
1019                 vip_ctrl_val &= ~HSY_LOW_ACTIVE;
1020     }
1021     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
1022         vip_ctrl_val |= VSY_HIGH_ACTIVE;
1023     } else {
1024                 vip_ctrl_val &= ~VSY_HIGH_ACTIVE;
1025     }
1026
1027     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
1028     //vip_ctrl_val |= ENABLE_CAPTURE;
1029
1030     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1031     RK29CAMERA_DG("%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),
1032                 read_grf_reg(GRF_SOC_CON0_Reg)&VIP_AHBMASTER, read_grf_reg(GRF_OS_REG0)&VIP_ACLK_DIV_HCLK_2);
1033
1034 RK29_CAMERA_SET_BUS_PARAM_END:
1035         if (ret)
1036         RK29CAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
1037     return ret;
1038 }
1039
1040 static int rk29_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
1041 {
1042     unsigned long bus_flags, camera_flags;
1043     int ret;
1044
1045     bus_flags = RK29_CAM_BUS_PARAM;
1046         if (icd->ops->query_bus_param) {
1047         camera_flags = icd->ops->query_bus_param(icd);
1048         } else {
1049                 camera_flags = 0;
1050         }
1051     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
1052
1053     if (ret < 0)
1054         dev_warn(icd->dev.parent,
1055                          "Flags incompatible: camera %lx, host %lx\n",
1056                          camera_flags, bus_flags);
1057     return ret;
1058 }
1059
1060 static const struct soc_mbus_pixelfmt rk29_camera_formats[] = {
1061    {
1062                 .fourcc                 = V4L2_PIX_FMT_NV12,
1063                 .name                   = "YUV420 NV12",
1064                 .bits_per_sample        = 8,
1065                 .packing                = SOC_MBUS_PACKING_1_5X8,
1066                 .order                  = SOC_MBUS_ORDER_LE,
1067         },{
1068                 .fourcc                 = V4L2_PIX_FMT_NV16,
1069                 .name                   = "YUV422 NV16",
1070                 .bits_per_sample        = 8,
1071                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1072                 .order                  = SOC_MBUS_ORDER_LE,
1073         },{
1074                 .fourcc                 = V4L2_PIX_FMT_YUV420,
1075                 .name                   = "NV12(v0.0.1)",
1076                 .bits_per_sample        = 8,
1077                 .packing                = SOC_MBUS_PACKING_1_5X8,
1078                 .order                  = SOC_MBUS_ORDER_LE,
1079         },{
1080                 .fourcc                 = V4L2_PIX_FMT_YUV422P,
1081                 .name                   = "NV16(v0.0.1)",
1082                 .bits_per_sample        = 8,
1083                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1084                 .order                  = SOC_MBUS_ORDER_LE,
1085         }
1086 };
1087
1088 static void rk29_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, enum v4l2_mbus_pixelcode icd_code, struct v4l2_rect *rect)
1089 {
1090         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1091     struct rk29_camera_dev *pcdev = ici->priv;
1092     unsigned int vip_fs = 0,vip_crop = 0;
1093     unsigned int vip_ctrl_val = VIP_SENSOR|ONEFRAME|DISABLE_CAPTURE;
1094
1095     switch (host_pixfmt)
1096     {
1097         case V4L2_PIX_FMT_NV16:
1098         case V4L2_PIX_FMT_YUV422P:  /* ddl@rock-chips.com: V4L2_PIX_FMT_YUV422P is V4L2_PIX_FMT_NV16 actually in 0.0.1 driver */
1099             vip_ctrl_val |= VIPREGYUV422;
1100                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
1101                         pcdev->pixfmt = host_pixfmt;
1102             break;
1103         case V4L2_PIX_FMT_NV12:
1104         case V4L2_PIX_FMT_YUV420:   /* ddl@rock-chips.com: V4L2_PIX_FMT_YUV420 is V4L2_PIX_FMT_NV12 actually in 0.0.1 driver */
1105             vip_ctrl_val |= VIPREGYUV420;
1106                         if (pcdev->frame_inval != RK29_CAM_FRAME_INVAL_INIT)
1107                                 pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
1108                         pcdev->pixfmt = host_pixfmt;
1109             break;
1110         default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
1111             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & VIPREGYUV422);
1112             break;
1113     }
1114
1115     switch (icd_code)
1116     {
1117         case V4L2_MBUS_FMT_UYVY8_2X8:
1118             vip_ctrl_val |= SENSOR_UYVY;
1119             break;
1120         case V4L2_MBUS_FMT_YUYV8_2X8:
1121             vip_ctrl_val |= SENSOR_YUYV;
1122             break;
1123         default :
1124             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & SENSOR_YUYV);
1125             break;
1126     }
1127
1128     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 */
1129
1130     read_vip_reg(RK29_VIP_INT_STS);                     /* clear vip interrupte single  */
1131
1132     if (vip_ctrl_val & ONEFRAME)  {
1133         vip_crop = ((rect->left<<16) + rect->top);
1134         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
1135     } else if (vip_ctrl_val & PING_PONG) {
1136         BUG();
1137     }
1138
1139     write_vip_reg(RK29_VIP_CROP, vip_crop);
1140     write_vip_reg(RK29_VIP_FS, vip_fs);
1141
1142     write_vip_reg(RK29_VIP_FB_SR,  0x00000003);
1143
1144         pcdev->host_width = rect->width;
1145         pcdev->host_height = rect->height;
1146
1147     RK29CAMERA_DG("%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));
1148         return;
1149 }
1150
1151 static int rk29_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
1152                                   struct soc_camera_format_xlate *xlate)
1153 {
1154     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1155     struct device *dev = icd->dev.parent;
1156     int formats = 0, ret;
1157         enum v4l2_mbus_pixelcode code;
1158         const struct soc_mbus_pixelfmt *fmt;
1159
1160         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1161         if (ret < 0)
1162                 /* No more formats */
1163                 return 0;
1164
1165         fmt = soc_mbus_get_fmtdesc(code);
1166         if (!fmt) {
1167                 dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
1168                 return 0;
1169         }
1170
1171     ret = rk29_camera_try_bus_param(icd, fmt->bits_per_sample);
1172     if (ret < 0)
1173         return 0;
1174
1175     switch (code) {
1176         case V4L2_MBUS_FMT_UYVY8_2X8:
1177         case V4L2_MBUS_FMT_YUYV8_2X8:
1178             formats++;
1179             if (xlate) {
1180                 xlate->host_fmt = &rk29_camera_formats[0];
1181                 xlate->code     = code;
1182                 xlate++;
1183                 dev_dbg(dev, "Providing format %s using code %d\n",
1184                         rk29_camera_formats[0].name,code);
1185             }
1186
1187             formats++;
1188             if (xlate) {
1189                 xlate->host_fmt = &rk29_camera_formats[1];
1190                 xlate->code     = code;
1191                 xlate++;
1192                 dev_dbg(dev, "Providing format %s using code %d\n",
1193                         rk29_camera_formats[1].name,code);
1194             }
1195
1196             formats++;
1197             if (xlate) {
1198                 xlate->host_fmt = &rk29_camera_formats[2];
1199                 xlate->code     = code;
1200                 xlate++;
1201                 dev_dbg(dev, "Providing format %s using code %d\n",
1202                         rk29_camera_formats[2].name,code);
1203             }
1204
1205             formats++;
1206             if (xlate) {
1207                 xlate->host_fmt = &rk29_camera_formats[3];
1208                 xlate->code     = code;
1209                 xlate++;
1210                 dev_dbg(dev, "Providing format %s using code %d\n",
1211                         rk29_camera_formats[3].name,code);;
1212             }
1213                         break;
1214         default:
1215             break;
1216     }
1217
1218     return formats;
1219 }
1220
1221 static void rk29_camera_put_formats(struct soc_camera_device *icd)
1222 {
1223         return;
1224 }
1225
1226 static int rk29_camera_set_crop(struct soc_camera_device *icd,
1227                                struct v4l2_crop *a)
1228 {
1229     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1230         struct v4l2_mbus_framefmt mf;
1231         u32 fourcc = icd->current_fmt->host_fmt->fourcc;
1232     int ret;
1233
1234     ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1235     if (ret < 0)
1236         return ret;
1237
1238     if ((mf.width < (a->c.left + a->c.width)) || (mf.height < (a->c.top + a->c.height)))  {
1239
1240         mf.width = a->c.left + a->c.width;
1241         mf.height = a->c.top + a->c.height;
1242
1243         v4l_bound_align_image(&mf.width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1244             &mf.height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1245             fourcc == V4L2_PIX_FMT_NV16 ?4 : 0);
1246
1247         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1248         if (ret < 0)
1249             return ret;
1250     }
1251
1252     rk29_camera_setup_format(icd, fourcc, mf.code, &a->c);
1253
1254     icd->user_width = mf.width;
1255     icd->user_height = mf.height;
1256
1257     return 0;
1258 }
1259
1260 static int rk29_camera_set_fmt(struct soc_camera_device *icd,
1261                               struct v4l2_format *f)
1262 {
1263     struct device *dev = icd->dev.parent;
1264     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1265     const struct soc_camera_format_xlate *xlate = NULL;
1266         struct soc_camera_host *ici =to_soc_camera_host(icd->dev.parent);
1267     struct rk29_camera_dev *pcdev = ici->priv;
1268     struct v4l2_pix_format *pix = &f->fmt.pix;
1269     struct v4l2_mbus_framefmt mf;
1270     struct v4l2_rect rect;
1271     int ret,usr_w,usr_h,icd_width,icd_height;
1272     int stream_on = 0;
1273
1274         usr_w = pix->width;
1275         usr_h = pix->height;
1276     RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1277
1278     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1279     if (!xlate) {
1280         dev_err(dev, "Format %x not found\n", pix->pixelformat);
1281         ret = -EINVAL;
1282         goto RK29_CAMERA_SET_FMT_END;
1283     }
1284
1285     /* ddl@rock-chips.com: sensor init code transmit in here after open */
1286     if (pcdev->icd_init == 0) {
1287         v4l2_subdev_call(sd,core, init, 0);
1288         pcdev->icd_init = 1;
1289     }
1290
1291     stream_on = read_vip_reg(RK29_VIP_CTRL);
1292     if (stream_on & ENABLE_CAPTURE)
1293         write_vip_reg(RK29_VIP_CTRL, (stream_on & (~ENABLE_CAPTURE)));
1294
1295         mf.width        = pix->width;
1296         mf.height       = pix->height;
1297         mf.field        = pix->field;
1298         mf.colorspace   = pix->colorspace;
1299         mf.code         = xlate->code;
1300
1301         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1302
1303         if (mf.code != xlate->code)
1304                 return -EINVAL;
1305
1306     icd_width = mf.width;
1307     icd_height = mf.height;
1308         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1309         if ((mf.width != usr_w) || (mf.height != usr_h)) {
1310         if (unlikely((mf.width <16) || (mf.width > 8190) || (mf.height < 16) || (mf.height > 8190))) {
1311                 RK29CAMERA_TR("Senor and IPP both invalid source resolution(%dx%d)\n",mf.width,mf.height);
1312                 ret = -EINVAL;
1313                 goto RK29_CAMERA_SET_FMT_END;
1314         }
1315         if (unlikely((usr_w <16)||(usr_h < 16))) {
1316                 RK29CAMERA_TR("Senor and IPP both invalid destination resolution(%dx%d)\n",usr_w,usr_h);
1317                 ret = -EINVAL;
1318             goto RK29_CAMERA_SET_FMT_END;
1319         }
1320                 mf.width = usr_w;
1321                 mf.height = usr_h;
1322         }
1323         #endif
1324     icd->sense = NULL;
1325
1326     if (!ret) {
1327         rect.left = 0;
1328         rect.top = 0;
1329         rect.width = mf.width;
1330         rect.height = mf.height;
1331
1332         down(&pcdev->zoominfo.sem);
1333         pcdev->zoominfo.a.c.width = rect.width*100/pcdev->zoominfo.zoom_rate;
1334                 pcdev->zoominfo.a.c.width &= ~0x03;
1335                 pcdev->zoominfo.a.c.height = rect.height*100/pcdev->zoominfo.zoom_rate;
1336                 pcdev->zoominfo.a.c.height &= ~0x03;
1337                 pcdev->zoominfo.a.c.left = ((rect.width - pcdev->zoominfo.a.c.width)>>1)&(~0x01);
1338                 pcdev->zoominfo.a.c.top = ((rect.height - pcdev->zoominfo.a.c.height)>>1)&(~0x01);
1339         up(&pcdev->zoominfo.sem);
1340
1341         /* ddl@rock-chips.com: IPP work limit check */
1342         if ((pcdev->zoominfo.a.c.width != usr_w) || (pcdev->zoominfo.a.c.height != usr_h)) {
1343             if (usr_w > 0x7f0) {
1344                 if (((usr_w>>1)&0x3f) && (((usr_w>>1)&0x3f) <= 8)) {
1345                     RK29CAMERA_TR("IPP Destination resolution(%dx%d, ((%d div 1) mod 64)=%d is <= 8)",usr_w,usr_h, usr_w, (int)((usr_w>>1)&0x3f));
1346                     ret = -EINVAL;
1347                     goto RK29_CAMERA_SET_FMT_END;
1348                 }
1349             } else {
1350                 if ((usr_w&0x3f) && ((usr_w&0x3f) <= 8)) {
1351                     RK29CAMERA_TR("IPP Destination resolution(%dx%d, %d mod 64=%d is <= 8)",usr_w,usr_h, usr_w, (int)(usr_w&0x3f));
1352                     ret = -EINVAL;
1353                     goto RK29_CAMERA_SET_FMT_END;
1354                 }
1355             }
1356         }
1357
1358         RK29CAMERA_DG("%s..%s icd width:%d  host width:%d (zoom: %dx%d@(%d,%d)->%dx%d)\n",__FUNCTION__,xlate->host_fmt->name,
1359                                    rect.width, pix->width, pcdev->zoominfo.a.c.width,pcdev->zoominfo.a.c.height, pcdev->zoominfo.a.c.left,pcdev->zoominfo.a.c.top,
1360                                    pix->width, pix->height);
1361         rk29_camera_setup_format(icd, pix->pixelformat, mf.code, &rect);
1362
1363                 if (CAM_IPPWORK_IS_EN()) {
1364                         BUG_ON(pcdev->vipmem_phybase == 0);
1365                 }
1366         pcdev->icd_width = icd_width;
1367         pcdev->icd_height = icd_height;
1368
1369         pix->width = mf.width;
1370         pix->height = mf.height;
1371         pix->field = mf.field;
1372         pix->colorspace = mf.colorspace;
1373         icd->current_fmt = xlate;
1374     }
1375
1376 RK29_CAMERA_SET_FMT_END:
1377     if (stream_on & ENABLE_CAPTURE)
1378         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL) | ENABLE_CAPTURE));
1379         if (ret)
1380         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1381     return ret;
1382 }
1383 static bool rk29_camera_fmt_capturechk(struct v4l2_format *f)
1384 {
1385     bool ret = false;
1386
1387         if ((f->fmt.pix.width == 1024) && (f->fmt.pix.height == 768)) {
1388                 ret = true;
1389         } else if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 1024)) {
1390                 ret = true;
1391         } else if ((f->fmt.pix.width == 1600) && (f->fmt.pix.height == 1200)) {
1392                 ret = true;
1393         } else if ((f->fmt.pix.width == 2048) && (f->fmt.pix.height == 1536)) {
1394                 ret = true;
1395         } else if ((f->fmt.pix.width == 2592) && (f->fmt.pix.height == 1944)) {
1396                 ret = true;
1397         }
1398
1399         if (ret == true)
1400                 RK29CAMERA_DG("%s %dx%d is capture format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1401         return ret;
1402 }
1403 static int rk29_camera_try_fmt(struct soc_camera_device *icd,
1404                                    struct v4l2_format *f)
1405 {
1406     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1407         struct rk29_camera_dev *pcdev = ici->priv;
1408     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1409     const struct soc_camera_format_xlate *xlate;
1410     struct v4l2_pix_format *pix = &f->fmt.pix;
1411     __u32 pixfmt = pix->pixelformat;
1412     int ret,usr_w,usr_h,i;
1413         bool is_capture = rk29_camera_fmt_capturechk(f);
1414         bool vipmem_is_overflow = false;
1415     struct v4l2_mbus_framefmt mf;
1416
1417         usr_w = pix->width;
1418         usr_h = pix->height;
1419         RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1420
1421     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1422     if (!xlate) {
1423         dev_err(icd->dev.parent, "Format (%c%c%c%c) not found\n", pixfmt & 0xFF, (pixfmt >> 8) & 0xFF,
1424                         (pixfmt >> 16) & 0xFF, (pixfmt >> 24) & 0xFF);
1425         ret = -EINVAL;
1426         RK29CAMERA_TR("%s(version:%c%c%c) support format:\n",rk29_cam_driver_description,(RK29_CAM_VERSION_CODE&0xff0000)>>16,
1427             (RK29_CAM_VERSION_CODE&0xff00)>>8,(RK29_CAM_VERSION_CODE&0xff));
1428         for (i = 0; i < icd->num_user_formats; i++)
1429                     RK29CAMERA_TR("(%c%c%c%c)-%s\n",
1430                     icd->user_formats[i].host_fmt->fourcc & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 8) & 0xFF,
1431                         (icd->user_formats[i].host_fmt->fourcc >> 16) & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 24) & 0xFF,
1432                         icd->user_formats[i].host_fmt->name);
1433         goto RK29_CAMERA_TRY_FMT_END;
1434     }
1435    /* limit to rk29 hardware capabilities */
1436     v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1437               &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1438               pixfmt == V4L2_PIX_FMT_NV16 ? 4 : 0);
1439
1440     pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1441                                                     xlate->host_fmt);
1442         if (pix->bytesperline < 0)
1443                 return pix->bytesperline;
1444
1445     /* limit to sensor capabilities */
1446         mf.width        = pix->width;
1447         mf.height       = pix->height;
1448         mf.field        = pix->field;
1449         mf.colorspace   = pix->colorspace;
1450         mf.code         = xlate->code;
1451
1452         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1453         if (ret < 0)
1454                 goto RK29_CAMERA_TRY_FMT_END;
1455     RK29CAMERA_DG("%s mf.width:%d  mf.height:%d\n",__FUNCTION__,mf.width,mf.height);
1456         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1457         if ((mf.width > usr_w) && (mf.height > usr_h)) {
1458                 if (is_capture) {
1459                         vipmem_is_overflow = (PAGE_ALIGN(pix->bytesperline*pix->height) > pcdev->vipmem_size);
1460                 } else {
1461                         /* Assume preview buffer minimum is 4 */
1462                         vipmem_is_overflow = (PAGE_ALIGN(pix->bytesperline*pix->height)*4 > pcdev->vipmem_size);
1463                 }
1464                 if (vipmem_is_overflow == false) {
1465                         pix->width = usr_w;
1466                         pix->height = usr_h;
1467                 } else {
1468                         RK29CAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",mf.width,mf.height,usr_w,usr_h);
1469             pix->width = mf.width;
1470             pix->height = mf.height;
1471                 }
1472         } else if ((mf.width < usr_w) && (mf.height < usr_h)) {
1473                 if (((usr_w>>1) < mf.width) && ((usr_h>>1) < mf.height)) {
1474                         if (is_capture) {
1475                                 vipmem_is_overflow = (PAGE_ALIGN(pix->bytesperline*pix->height) > pcdev->vipmem_size);
1476                         } else {
1477                                 vipmem_is_overflow = (PAGE_ALIGN(pix->bytesperline*pix->height)*4 > pcdev->vipmem_size);
1478                         }
1479                         if (vipmem_is_overflow == false) {
1480                                 pix->width = usr_w;
1481                                 pix->height = usr_h;
1482                         } else {
1483                                 RK29CAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",mf.width,mf.height,usr_w,usr_h);
1484                 pix->width = mf.width;
1485                 pix->height = mf.height;
1486                         }
1487                 } else {
1488                         RK29CAMERA_TR("The aspect ratio(%dx%d/%dx%d) is bigger than 2 !\n",mf.width,mf.height,usr_w,usr_h);
1489             pix->width = mf.width;
1490             pix->height = mf.height;
1491                 }
1492         }
1493         #else
1494     pix->width  = mf.width;
1495         pix->height     = mf.height;
1496     #endif
1497     pix->colorspace     = mf.colorspace;
1498
1499     switch (mf.field) {
1500         case V4L2_FIELD_ANY:
1501         case V4L2_FIELD_NONE:
1502                 pix->field      = V4L2_FIELD_NONE;
1503                 break;
1504         default:
1505                 /* TODO: support interlaced at least in pass-through mode */
1506                 dev_err(icd->dev.parent, "Field type %d unsupported.\n",
1507                         mf.field);
1508                 goto RK29_CAMERA_TRY_FMT_END;
1509         }
1510
1511 RK29_CAMERA_TRY_FMT_END:
1512         if (ret)
1513         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1514     return ret;
1515 }
1516
1517 static int rk29_camera_reqbufs(struct soc_camera_device *icd,
1518                                struct v4l2_requestbuffers *p)
1519 {
1520     int i;
1521
1522     /* This is for locking debugging only. I removed spinlocks and now I
1523      * check whether .prepare is ever called on a linked buffer, or whether
1524      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1525      * it hadn't triggered */
1526     for (i = 0; i < p->count; i++) {
1527         struct rk29_buffer *buf = container_of(icd->vb_vidq.bufs[i],
1528                                                            struct rk29_buffer, vb);
1529         buf->inwork = 0;
1530         INIT_LIST_HEAD(&buf->vb.queue);
1531     }
1532
1533     return 0;
1534 }
1535
1536 static unsigned int rk29_camera_poll(struct file *file, poll_table *pt)
1537 {
1538     struct soc_camera_device *icd = file->private_data;
1539     struct rk29_buffer *buf;
1540
1541     buf = list_entry(icd->vb_vidq.stream.next, struct rk29_buffer,
1542                      vb.stream);
1543
1544     poll_wait(file, &buf->vb.done, pt);
1545
1546     if (buf->vb.state == VIDEOBUF_DONE ||
1547             buf->vb.state == VIDEOBUF_ERROR)
1548         return POLLIN|POLLRDNORM;
1549
1550     return 0;
1551 }
1552
1553 static int rk29_camera_querycap(struct soc_camera_host *ici,
1554                                 struct v4l2_capability *cap)
1555 {
1556     struct rk29_camera_dev *pcdev = ici->priv;
1557     char orientation[5];
1558
1559     strlcpy(cap->card, dev_name(pcdev->icd->pdev), sizeof(cap->card));
1560     if (strcmp(dev_name(pcdev->icd->pdev), pcdev->pdata->info[0].dev_name) == 0) {
1561         sprintf(orientation,"-%d",pcdev->pdata->info[0].orientation);
1562     } else {
1563         sprintf(orientation,"-%d",pcdev->pdata->info[1].orientation);
1564     }
1565     strcat(cap->card,orientation);
1566     cap->version = RK29_CAM_VERSION_CODE;
1567     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1568
1569     return 0;
1570 }
1571
1572 static int rk29_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1573 {
1574     struct soc_camera_host *ici =
1575                     to_soc_camera_host(icd->dev.parent);
1576     struct rk29_camera_dev *pcdev = ici->priv;
1577         struct v4l2_subdev *sd;
1578     int ret = 0,tmp;
1579
1580         mutex_lock(&camera_lock);
1581         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1582                 rk29_camera_s_stream(icd, 0);
1583                 sd = soc_camera_to_subdev(icd);
1584                 v4l2_subdev_call(sd, video, s_stream, 0);
1585                 ret = icd->ops->suspend(icd, state);
1586
1587                 pcdev->reginfo_suspend.VipCtrl = read_vip_reg(RK29_VIP_CTRL);
1588                 pcdev->reginfo_suspend.VipCrop = read_vip_reg(RK29_VIP_CROP);
1589                 pcdev->reginfo_suspend.VipFs = read_vip_reg(RK29_VIP_FS);
1590                 pcdev->reginfo_suspend.VipIntMsk = read_vip_reg(RK29_VIP_INT_MASK);
1591                 pcdev->reginfo_suspend.VipCrm = read_vip_reg(RK29_VIP_CRM);
1592
1593                 tmp = pcdev->reginfo_suspend.VipFs>>16;         /* ddl@rock-chips.com */
1594                 tmp += pcdev->reginfo_suspend.VipCrop>>16;
1595                 pcdev->reginfo_suspend.VipFs = (pcdev->reginfo_suspend.VipFs & 0xffff) | (tmp<<16);
1596
1597                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1598                 rk29_camera_deactivate(pcdev);
1599
1600                 RK29CAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1601         } else {
1602                 RK29CAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1603         }
1604         mutex_unlock(&camera_lock);
1605     return ret;
1606 }
1607
1608 static int rk29_camera_resume(struct soc_camera_device *icd)
1609 {
1610     struct soc_camera_host *ici =
1611                     to_soc_camera_host(icd->dev.parent);
1612     struct rk29_camera_dev *pcdev = ici->priv;
1613         struct v4l2_subdev *sd;
1614     int ret = 0;
1615
1616         mutex_lock(&camera_lock);
1617         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1618                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1619                         rk29_camera_activate(pcdev, icd);
1620                         write_vip_reg(RK29_VIP_INT_MASK, pcdev->reginfo_suspend.VipIntMsk);
1621                         write_vip_reg(RK29_VIP_CRM, pcdev->reginfo_suspend.VipCrm);
1622                         write_vip_reg(RK29_VIP_CTRL, pcdev->reginfo_suspend.VipCtrl&~ENABLE_CAPTURE);
1623                         write_vip_reg(RK29_VIP_CROP, pcdev->reginfo_suspend.VipCrop);
1624                         write_vip_reg(RK29_VIP_FS, pcdev->reginfo_suspend.VipFs);
1625
1626                         rk29_videobuf_capture(pcdev->active);
1627                         rk29_camera_s_stream(icd, 1);
1628                         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
1629                 } else {
1630                         RK29CAMERA_TR("Resume fail, vip register recored is invalidate!!\n");
1631                         goto rk29_camera_resume_end;
1632                 }
1633
1634                 ret = icd->ops->resume(icd);
1635                 sd = soc_camera_to_subdev(icd);
1636                 v4l2_subdev_call(sd, video, s_stream, 1);
1637
1638                 RK29CAMERA_DG("%s Enter success\n",__FUNCTION__);
1639         } else {
1640                 RK29CAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1641         }
1642
1643 rk29_camera_resume_end:
1644         mutex_unlock(&camera_lock);
1645     return ret;
1646 }
1647
1648 static void rk29_camera_reinit_work(struct work_struct *work)
1649 {
1650         struct device *control;
1651     struct v4l2_subdev *sd;
1652         struct v4l2_mbus_framefmt mf;
1653         const struct soc_camera_format_xlate *xlate;
1654         int ret;
1655
1656         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)&(~ENABLE_CAPTURE)));
1657
1658         control = to_soc_camera_control(rk29_camdev_info_ptr->icd);
1659         sd = dev_get_drvdata(control);
1660         ret = v4l2_subdev_call(sd,core, init, 1);
1661
1662         mf.width = rk29_camdev_info_ptr->icd->user_width;
1663         mf.height = rk29_camdev_info_ptr->icd->user_height;
1664         xlate = soc_camera_xlate_by_fourcc(rk29_camdev_info_ptr->icd, rk29_camdev_info_ptr->icd->current_fmt->host_fmt->fourcc);
1665         mf.code = xlate->code;
1666
1667         ret |= v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1668
1669         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)|ENABLE_CAPTURE));
1670
1671         RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor now! ret:0x%x\n",ret);
1672 }
1673 static enum hrtimer_restart rk29_camera_fps_func(struct hrtimer *timer)
1674 {
1675     struct rk29_camera_frmivalenum *fival_nxt=NULL,*fival_pre=NULL, *fival_rec=NULL;
1676     int rec_flag,i;
1677
1678         RK29CAMERA_DG("rk29_camera_fps_func fps:0x%x\n",rk29_camdev_info_ptr->fps);
1679         if (rk29_camdev_info_ptr->fps < 2) {
1680                 RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor delay!\n");
1681                 INIT_WORK(&rk29_camdev_info_ptr->camera_reinit_work, rk29_camera_reinit_work);
1682                 queue_work(rk29_camdev_info_ptr->camera_wq,&(rk29_camdev_info_ptr->camera_reinit_work));
1683         } else {
1684             for (i=0; i<2; i++) {
1685             if (rk29_camdev_info_ptr->icd == rk29_camdev_info_ptr->icd_frmival[i].icd) {
1686                 fival_nxt = rk29_camdev_info_ptr->icd_frmival[i].fival_list;
1687             }
1688         }
1689
1690         rec_flag = 0;
1691         fival_pre = fival_nxt;
1692         while (fival_nxt != NULL) {
1693
1694             RK29CAMERA_DG("%s %c%c%c%c %dx%d framerate : %d/%d\n", dev_name(&rk29_camdev_info_ptr->icd->dev),
1695                 fival_nxt->fival.pixel_format & 0xFF, (fival_nxt->fival.pixel_format >> 8) & 0xFF,
1696                             (fival_nxt->fival.pixel_format >> 16) & 0xFF, (fival_nxt->fival.pixel_format >> 24),
1697                             fival_nxt->fival.width, fival_nxt->fival.height, fival_nxt->fival.discrete.denominator,
1698                             fival_nxt->fival.discrete.numerator);
1699
1700             if (((fival_nxt->fival.pixel_format == rk29_camdev_info_ptr->pixfmt)
1701                 && (fival_nxt->fival.height == rk29_camdev_info_ptr->icd->user_height)
1702                 && (fival_nxt->fival.width == rk29_camdev_info_ptr->icd->user_width))
1703                 || (fival_nxt->fival.discrete.denominator == 0)) {
1704
1705                 fival_nxt->fival.index = 0;
1706                 fival_nxt->fival.width = rk29_camdev_info_ptr->icd->user_width;
1707                 fival_nxt->fival.height= rk29_camdev_info_ptr->icd->user_height;
1708                 fival_nxt->fival.pixel_format = rk29_camdev_info_ptr->pixfmt;
1709                 fival_nxt->fival.discrete.denominator = rk29_camdev_info_ptr->frame_interval;
1710                 fival_nxt->fival.reserved[1] = (rk29_camdev_info_ptr->icd_width<<16)
1711                                                     |(rk29_camdev_info_ptr->icd_height);
1712                 fival_nxt->fival.discrete.numerator = 1000000;
1713                 fival_nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1714
1715                 rec_flag = 1;
1716                 fival_rec = fival_nxt;
1717             }
1718             fival_pre = fival_nxt;
1719             fival_nxt = fival_nxt->nxt;
1720         }
1721
1722         if ((rec_flag == 0) && fival_pre) {
1723             fival_pre->nxt = kzalloc(sizeof(struct rk29_camera_frmivalenum), GFP_ATOMIC);
1724             if (fival_pre->nxt != NULL) {
1725                 fival_pre->nxt->fival.index = fival_pre->fival.index++;
1726                 fival_pre->nxt->fival.width = rk29_camdev_info_ptr->icd->user_width;
1727                 fival_pre->nxt->fival.height= rk29_camdev_info_ptr->icd->user_height;
1728                 fival_pre->nxt->fival.pixel_format = rk29_camdev_info_ptr->pixfmt;
1729
1730                 fival_pre->nxt->fival.discrete.denominator = rk29_camdev_info_ptr->frame_interval;
1731
1732                 fival_pre->nxt->fival.reserved[1] = (rk29_camdev_info_ptr->icd_width<<16)
1733                                                     |(rk29_camdev_info_ptr->icd_height);
1734
1735                 fival_pre->nxt->fival.discrete.numerator = 1000000;
1736                 fival_pre->nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1737                 rec_flag = 1;
1738                 fival_rec = fival_pre->nxt;
1739             }
1740         }
1741         }
1742
1743         return HRTIMER_NORESTART;
1744 }
1745 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable)
1746 {
1747         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1748     struct rk29_camera_dev *pcdev = ici->priv;
1749     int vip_ctrl_val;
1750         int ret;
1751
1752         WARN_ON(pcdev->icd != icd);
1753     pcdev->frame_interval = 0;
1754         vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1755         if (enable) {
1756                 pcdev->fps = 0;
1757                 hrtimer_cancel(&pcdev->fps_timer);
1758                 hrtimer_start(&pcdev->fps_timer,ktime_set(1, 0),HRTIMER_MODE_REL);
1759                 vip_ctrl_val |= ENABLE_CAPTURE;
1760         } else {
1761         vip_ctrl_val &= ~ENABLE_CAPTURE;
1762                 ret = hrtimer_cancel(&pcdev->fps_timer);
1763                 ret |= flush_work(&rk29_camdev_info_ptr->camera_reinit_work);
1764                 RK29CAMERA_DG("STREAM_OFF cancel timer and flush work:0x%x \n", ret);
1765         }
1766         write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1767
1768         RK29CAMERA_DG("%s.. enable : 0x%x \n", __FUNCTION__, enable);
1769         return 0;
1770 }
1771 int rk29_camera_enum_frameintervals(struct soc_camera_device *icd, struct v4l2_frmivalenum *fival)
1772 {
1773     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1774     struct rk29_camera_dev *pcdev = ici->priv;
1775     struct rk29_camera_frmivalenum *fival_list = NULL;
1776     struct v4l2_frmivalenum *fival_head;
1777     int i,ret = 0,index;
1778
1779     index = fival->index & 0x00ffffff;
1780     if ((fival->index & 0xff000000) == 0xff000000) {   /* ddl@rock-chips.com: detect framerate */
1781         for (i=0; i<2; i++) {
1782             if (pcdev->icd_frmival[i].icd == icd) {
1783                 fival_list = pcdev->icd_frmival[i].fival_list;
1784             }
1785         }
1786
1787         if (fival_list != NULL) {
1788             i = 0;
1789             while (fival_list != NULL) {
1790                 if ((fival->pixel_format == fival_list->fival.pixel_format)
1791                     && (fival->height == fival_list->fival.height)
1792                     && (fival->width == fival_list->fival.width)) {
1793                     if (i == index)
1794                         break;
1795                     i++;
1796                 }
1797                 fival_list = fival_list->nxt;
1798             }
1799
1800             if ((i==index) && (fival_list != NULL)) {
1801                 memcpy(fival, &fival_list->fival, sizeof(struct v4l2_frmivalenum));
1802             } else {
1803                 ret = -EINVAL;
1804             }
1805         } else {
1806             RK29CAMERA_TR("%s: fival_list is NULL\n",__FUNCTION__);
1807             ret = -EINVAL;
1808         }
1809     } else {
1810         if (strcmp(dev_name(pcdev->icd->pdev),pcdev->pdata->info[0].dev_name) == 0) {
1811             fival_head = pcdev->pdata->info[0].fival;
1812         } else {
1813             fival_head = pcdev->pdata->info[1].fival;
1814         }
1815         i = 0;
1816         while (fival_head->width && fival_head->height) {
1817             if ((fival->pixel_format == fival_head->pixel_format)
1818                 && (fival->height == fival_head->height)
1819                 && (fival->width == fival_head->width)) {
1820                 if (i == index) {
1821                     break;
1822                 }
1823                 i++;
1824             }
1825             fival_head++;
1826         }
1827
1828         if ((i == index) && (fival->height == fival_head->height) && (fival->width == fival_head->width)) {
1829             memcpy(fival, fival_head, sizeof(struct v4l2_frmivalenum));
1830             RK29CAMERA_DG("%s %dx%d@%c%c%c%c framerate : %d/%d\n", dev_name(&rk29_camdev_info_ptr->icd->dev),
1831                 fival->width, fival->height,
1832                 fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1833                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1834                              fival->discrete.denominator,fival->discrete.numerator);
1835         } else {
1836             if (index == 0)
1837                 RK29CAMERA_TR("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(&rk29_camdev_info_ptr->icd->dev),
1838                     fival->width,fival->height,
1839                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1840                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1841                             index);
1842             else
1843                 RK29CAMERA_DG("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(&rk29_camdev_info_ptr->icd->dev),
1844                     fival->width,fival->height,
1845                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1846                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1847                             index);
1848             ret = -EINVAL;
1849         }
1850     }
1851
1852     return ret;
1853 }
1854
1855 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
1856 static int rk29_camera_set_digit_zoom(struct soc_camera_device *icd,
1857                                                                 const struct v4l2_queryctrl *qctrl, int zoom_rate)
1858 {
1859         struct v4l2_crop a;
1860         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1861         struct rk29_camera_dev *pcdev = ici->priv;
1862
1863 /* ddl@rock-chips.com : The largest resolution is 2047x1088, so larger resolution must be operated some times
1864    (Assume operate times is 4),but resolution which ipp can operate ,it is width and height must be even. */
1865         a.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1866         a.c.width = pcdev->host_width*100/zoom_rate;
1867         a.c.width &= ~0x03;
1868         a.c.height = pcdev->host_height*100/zoom_rate;
1869         a.c.height &= ~0x03;
1870
1871         a.c.left = ((pcdev->host_width - a.c.width)>>1)&(~0x01);
1872         a.c.top = ((pcdev->host_height - a.c.height)>>1)&(~0x01);
1873
1874     down(&pcdev->zoominfo.sem);
1875         pcdev->zoominfo.a.c.height = a.c.height;
1876         pcdev->zoominfo.a.c.width = a.c.width;
1877         pcdev->zoominfo.a.c.top = a.c.top;
1878         pcdev->zoominfo.a.c.left = a.c.left;
1879     up(&pcdev->zoominfo.sem);
1880
1881         RK29CAMERA_DG("%s..zoom_rate:%d (%dx%d at (%d,%d)-> %dx%d)\n",__FUNCTION__, zoom_rate,a.c.width, a.c.height, a.c.left, a.c.top, pcdev->host_width, pcdev->host_height );
1882
1883         return 0;
1884 }
1885 #endif
1886 static inline struct v4l2_queryctrl const *rk29_camera_soc_camera_find_qctrl(
1887         struct soc_camera_host_ops *ops, int id)
1888 {
1889         int i;
1890
1891         for (i = 0; i < ops->num_controls; i++)
1892                 if (ops->controls[i].id == id)
1893                         return &ops->controls[i];
1894
1895         return NULL;
1896 }
1897
1898
1899 static int rk29_camera_set_ctrl(struct soc_camera_device *icd,
1900                                                                 struct v4l2_control *sctrl)
1901 {
1902
1903         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1904         const struct v4l2_queryctrl *qctrl;
1905     struct rk29_camera_dev *pcdev = ici->priv;
1906     int ret = 0;
1907
1908         qctrl = rk29_camera_soc_camera_find_qctrl(ici->ops, sctrl->id);
1909         if (!qctrl) {
1910                 ret = -ENOIOCTLCMD;
1911         goto rk29_camera_set_ctrl_end;
1912         }
1913
1914         switch (sctrl->id)
1915         {
1916         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
1917                 case V4L2_CID_ZOOM_ABSOLUTE:
1918                 {
1919                         if ((sctrl->value < qctrl->minimum) || (sctrl->value > qctrl->maximum)){
1920                         ret = -EINVAL;
1921                 goto rk29_camera_set_ctrl_end;
1922                 }
1923             ret = rk29_camera_set_digit_zoom(icd, qctrl, sctrl->value);
1924                         if (ret == 0) {
1925                                 pcdev->zoominfo.zoom_rate = sctrl->value;
1926             } else {
1927                 goto rk29_camera_set_ctrl_end;
1928             }
1929                         break;
1930                 }
1931     #endif
1932                 default:
1933                         ret = -ENOIOCTLCMD;
1934                         break;
1935         }
1936 rk29_camera_set_ctrl_end:
1937         return ret;
1938 }
1939
1940 static struct soc_camera_host_ops rk29_soc_camera_host_ops =
1941 {
1942     .owner              = THIS_MODULE,
1943     .add                = rk29_camera_add_device,
1944     .remove             = rk29_camera_remove_device,
1945     .suspend    = rk29_camera_suspend,
1946     .resume             = rk29_camera_resume,
1947     .enum_frameinervals = rk29_camera_enum_frameintervals,
1948     .set_crop   = rk29_camera_set_crop,
1949     .get_formats        = rk29_camera_get_formats,
1950     .put_formats        = rk29_camera_put_formats,
1951     .set_fmt    = rk29_camera_set_fmt,
1952     .try_fmt    = rk29_camera_try_fmt,
1953     .init_videobuf      = rk29_camera_init_videobuf,
1954     .reqbufs    = rk29_camera_reqbufs,
1955     .poll               = rk29_camera_poll,
1956     .querycap   = rk29_camera_querycap,
1957     .set_bus_param      = rk29_camera_set_bus_param,
1958     .s_stream = rk29_camera_s_stream,   /* ddl@rock-chips.com : Add stream control for host */
1959     .set_ctrl = rk29_camera_set_ctrl,
1960     .controls = rk29_camera_controls,
1961     .num_controls = ARRAY_SIZE(rk29_camera_controls)
1962
1963 };
1964 static int rk29_camera_probe(struct platform_device *pdev)
1965 {
1966     struct rk29_camera_dev *pcdev;
1967     struct resource *res;
1968     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
1969     int irq,i;
1970     int err = 0;
1971
1972     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1973     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1974     irq = platform_get_irq(pdev, 0);
1975     if (!res || irq < 0) {
1976         err = -ENODEV;
1977         goto exit;
1978     }
1979
1980     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1981     if (!pcdev) {
1982         dev_err(&pdev->dev, "Could not allocate pcdev\n");
1983         err = -ENOMEM;
1984         goto exit_alloc;
1985     }
1986     rk29_camdev_info_ptr = pcdev;
1987
1988     /*config output clk*/
1989         pcdev->aclk_ddr_lcdc = clk_get(&pdev->dev, "aclk_ddr_lcdc");
1990         pcdev->aclk_disp_matrix = clk_get(&pdev->dev, "aclk_disp_matrix");
1991
1992         pcdev->hclk_cpu_display = clk_get(&pdev->dev, "hclk_cpu_display");
1993         pcdev->vip_slave = clk_get(&pdev->dev, "vip_slave");
1994         pcdev->vip_out = clk_get(&pdev->dev,"vip_out");
1995         pcdev->vip_input = clk_get(&pdev->dev,"vip_input");
1996         pcdev->vip_bus = clk_get(&pdev->dev, "vip_bus");
1997
1998         pcdev->hclk_disp_matrix = clk_get(&pdev->dev,"hclk_disp_matrix");
1999         pcdev->vip_matrix = clk_get(&pdev->dev,"vip_matrix");
2000
2001         pcdev->pd_display = clk_get(&pdev->dev,"pd_display");
2002
2003         pcdev->zoominfo.zoom_rate = 100;
2004
2005     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
2006                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus || !pcdev->pd_display ||
2007                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) || IS_ERR(pcdev->pd_display) ||
2008                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
2009
2010         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
2011         err = -ENOENT;
2012         goto exit_reqmem;
2013     }
2014
2015         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
2016                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
2017
2018         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
2019         err = -ENOENT;
2020         goto exit_reqmem;
2021     }
2022
2023     dev_set_drvdata(&pdev->dev, pcdev);
2024     pcdev->res = res;
2025
2026     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
2027     if (pcdev->pdata && pcdev->pdata->io_init) {
2028         pcdev->pdata->io_init();
2029     }
2030         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
2031         if (pcdev->pdata && (strcmp(pcdev->pdata->meminfo.name,"camera_ipp_mem")==0)) {
2032                 pcdev->vipmem_phybase = pcdev->pdata->meminfo.start;
2033                 pcdev->vipmem_size = pcdev->pdata->meminfo.size;
2034                 RK29CAMERA_DG("\n%s Memory(start:0x%x size:0x%x) for IPP obtain \n",__FUNCTION__, pcdev->pdata->meminfo.start,pcdev->pdata->meminfo.size);
2035         } else {
2036                 RK29CAMERA_TR("\n%s Memory for IPP have not obtain! IPP Function is fail\n",__FUNCTION__);
2037                 pcdev->vipmem_phybase = 0;
2038                 pcdev->vipmem_size = 0;
2039         }
2040         #endif
2041     INIT_LIST_HEAD(&pcdev->capture);
2042     spin_lock_init(&pcdev->lock);
2043     sema_init(&pcdev->zoominfo.sem,1);
2044
2045     /*
2046      * Request the regions.
2047      */
2048     if (!request_mem_region(res->start, res->end - res->start + 1,
2049                             RK29_CAM_DRV_NAME)) {
2050         err = -EBUSY;
2051         goto exit_reqmem;
2052     }
2053
2054     pcdev->base = ioremap(res->start, res->end - res->start + 1);
2055     if (pcdev->base == NULL) {
2056         dev_err(pcdev->dev, "ioremap() of registers failed\n");
2057         err = -ENXIO;
2058         goto exit_ioremap;
2059     }
2060
2061     pcdev->irq = irq;
2062     pcdev->dev = &pdev->dev;
2063
2064     /* config buffer address */
2065     /* request irq */
2066     err = request_irq(pcdev->irq, rk29_camera_irq, 0, RK29_CAM_DRV_NAME,
2067                       pcdev);
2068     if (err) {
2069         dev_err(pcdev->dev, "Camera interrupt register failed \n");
2070         goto exit_reqirq;
2071     }
2072
2073         pcdev->camera_wq = create_workqueue("rk_camera_workqueue");
2074         if (pcdev->camera_wq == NULL)
2075                 goto exit_free_irq;
2076         INIT_WORK(&pcdev->camera_reinit_work, rk29_camera_reinit_work);
2077
2078     for (i=0; i<2; i++) {
2079         pcdev->icd_frmival[i].icd = NULL;
2080         pcdev->icd_frmival[i].fival_list = kzalloc(sizeof(struct rk29_camera_frmivalenum),GFP_KERNEL);
2081
2082     }
2083
2084     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
2085     pcdev->soc_host.ops         = &rk29_soc_camera_host_ops;
2086     pcdev->soc_host.priv                = pcdev;
2087     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
2088     pcdev->soc_host.nr          = pdev->id;
2089
2090     err = soc_camera_host_register(&pcdev->soc_host);
2091     if (err)
2092         goto exit_free_irq;
2093
2094         hrtimer_init(&pcdev->fps_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
2095         pcdev->fps_timer.function = rk29_camera_fps_func;
2096     pcdev->icd_cb.sensor_cb = NULL;
2097
2098     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2099     return 0;
2100
2101 exit_free_irq:
2102
2103     for (i=0; i<2; i++) {
2104         fival_list = pcdev->icd_frmival[i].fival_list;
2105         fival_nxt = fival_list;
2106         while(fival_nxt != NULL) {
2107             fival_nxt = fival_list->nxt;
2108             kfree(fival_list);
2109             fival_list = fival_nxt;
2110         }
2111     }
2112
2113     free_irq(pcdev->irq, pcdev);
2114         if (pcdev->camera_wq) {
2115                 destroy_workqueue(pcdev->camera_wq);
2116                 pcdev->camera_wq = NULL;
2117         }
2118 exit_reqirq:
2119     iounmap(pcdev->base);
2120 exit_ioremap:
2121     release_mem_region(res->start, res->end - res->start + 1);
2122 exit_reqmem:
2123     if (pcdev->aclk_ddr_lcdc) {
2124                 clk_put(pcdev->aclk_ddr_lcdc);
2125                 pcdev->aclk_ddr_lcdc = NULL;
2126     }
2127         if (pcdev->aclk_disp_matrix) {
2128                 clk_put(pcdev->aclk_disp_matrix);
2129                 pcdev->aclk_disp_matrix = NULL;
2130     }
2131         if (pcdev->hclk_cpu_display) {
2132                 clk_put(pcdev->hclk_cpu_display);
2133                 pcdev->hclk_cpu_display = NULL;
2134     }
2135         if (pcdev->vip_slave) {
2136                 clk_put(pcdev->vip_slave);
2137                 pcdev->vip_slave = NULL;
2138     }
2139         if (pcdev->vip_out) {
2140                 clk_put(pcdev->vip_out);
2141                 pcdev->vip_out = NULL;
2142     }
2143         if (pcdev->vip_input) {
2144                 clk_put(pcdev->vip_input);
2145                 pcdev->vip_input = NULL;
2146     }
2147         if (pcdev->vip_bus) {
2148                 clk_put(pcdev->vip_bus);
2149                 pcdev->vip_bus = NULL;
2150     }
2151     if (pcdev->hclk_disp_matrix) {
2152                 clk_put(pcdev->hclk_disp_matrix);
2153                 pcdev->hclk_disp_matrix = NULL;
2154     }
2155         if (pcdev->vip_matrix) {
2156                 clk_put(pcdev->vip_matrix);
2157                 pcdev->vip_matrix = NULL;
2158     }
2159     kfree(pcdev);
2160 exit_alloc:
2161     rk29_camdev_info_ptr = NULL;
2162 exit:
2163     return err;
2164 }
2165
2166 static int __devexit rk29_camera_remove(struct platform_device *pdev)
2167 {
2168     struct rk29_camera_dev *pcdev = platform_get_drvdata(pdev);
2169     struct resource *res;
2170     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
2171     int i;
2172
2173     free_irq(pcdev->irq, pcdev);
2174
2175         if (pcdev->camera_wq) {
2176                 destroy_workqueue(pcdev->camera_wq);
2177                 pcdev->camera_wq = NULL;
2178         }
2179
2180     for (i=0; i<2; i++) {
2181         fival_list = pcdev->icd_frmival[i].fival_list;
2182         fival_nxt = fival_list;
2183         while(fival_nxt != NULL) {
2184             fival_nxt = fival_list->nxt;
2185             kfree(fival_list);
2186             fival_list = fival_nxt;
2187         }
2188     }
2189
2190     soc_camera_host_unregister(&pcdev->soc_host);
2191
2192     res = pcdev->res;
2193     release_mem_region(res->start, res->end - res->start + 1);
2194
2195     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
2196         pcdev->pdata->io_deinit(0);
2197                 pcdev->pdata->io_deinit(1);
2198     }
2199
2200     kfree(pcdev);
2201     rk29_camdev_info_ptr = NULL;
2202     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
2203
2204     return 0;
2205 }
2206
2207 static struct platform_driver rk29_camera_driver =
2208 {
2209     .driver     = {
2210         .name   = RK29_CAM_DRV_NAME,
2211     },
2212     .probe              = rk29_camera_probe,
2213     .remove             = __devexit_p(rk29_camera_remove),
2214 };
2215
2216
2217 static int __devinit rk29_camera_init(void)
2218 {
2219     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2220     return platform_driver_register(&rk29_camera_driver);
2221 }
2222
2223 static void __exit rk29_camera_exit(void)
2224 {
2225     platform_driver_unregister(&rk29_camera_driver);
2226 }
2227
2228 device_initcall_sync(rk29_camera_init);
2229 module_exit(rk29_camera_exit);
2230
2231 MODULE_DESCRIPTION("RK29 Soc Camera Host driver");
2232 MODULE_AUTHOR("ddl <ddl@rock-chips>");
2233 MODULE_LICENSE("GPL");