drivers/media/video/rk30_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 #ifdef CONFIG_ARCH_RK30
  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 <linux/kthread.h>
  33 #include <mach/iomux.h>
  34 #include <media/v4l2-common.h>
  35 #include <media/v4l2-dev.h>
  36 #include <media/videobuf-dma-contig.h>
  37 #include <media/soc_camera.h>
  38 #include <media/soc_mediabus.h>
  39 #include <mach/io.h>
  40 #include <plat/ipp.h>
  41 #include <mach/rk30_camera.h>
  42 #include <mach/cru.h>
  43 #include <mach/pmu.h>
  44
  45 static int debug ;
  46 module_param(debug, int, S_IRUGO|S_IWUSR);
  47
  48 #define dprintk(level, fmt, arg...) do {                        \
  49         if (debug >= level)                                     \
  50         printk(KERN_WARNING"rk_camera: " fmt , ## arg); } while (0)
  51
  52 #define RKCAMERA_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  53 #define RKCAMERA_DG(format, ...) dprintk(1, format, ## __VA_ARGS__)
  54
  55 // CIF Reg Offset
  56 #define  CIF_CIF_CTRL                       0x00
  57 #define  CIF_CIF_INTEN                      0x04
  58 #define  CIF_CIF_INTSTAT                    0x08
  59 #define  CIF_CIF_FOR                        0x0c
  60 #define  CIF_CIF_LINE_NUM_ADDR              0x10
  61 #define  CIF_CIF_FRM0_ADDR_Y                0x14
  62 #define  CIF_CIF_FRM0_ADDR_UV               0x18
  63 #define  CIF_CIF_FRM1_ADDR_Y                0x1c
  64 #define  CIF_CIF_FRM1_ADDR_UV               0x20
  65 #define  CIF_CIF_VIR_LINE_WIDTH             0x24
  66 #define  CIF_CIF_SET_SIZE                   0x28
  67 #define  CIF_CIF_SCM_ADDR_Y                 0x2c
  68 #define  CIF_CIF_SCM_ADDR_U                 0x30
  69 #define  CIF_CIF_SCM_ADDR_V                 0x34
  70 #define  CIF_CIF_WB_UP_FILTER               0x38
  71 #define  CIF_CIF_WB_LOW_FILTER              0x3c
  72 #define  CIF_CIF_WBC_CNT                    0x40
  73 #define  CIF_CIF_CROP                       0x44
  74 #define  CIF_CIF_SCL_CTRL                   0x48
  75 #define  CIF_CIF_SCL_DST                    0x4c
  76 #define  CIF_CIF_SCL_FCT                    0x50
  77 #define  CIF_CIF_SCL_VALID_NUM              0x54
  78 #define  CIF_CIF_LINE_LOOP_CTR              0x58
  79 #define  CIF_CIF_FRAME_STATUS               0x60
  80 #define  CIF_CIF_CUR_DST                    0x64
  81 #define  CIF_CIF_LAST_LINE                  0x68
  82 #define  CIF_CIF_LAST_PIX                   0x6c
  83
  84 //The key register bit descrition
  85 // CIF_CTRL Reg , ignore SCM,WBC,ISP,
  86 #define  DISABLE_CAPTURE              (0x00<<0)
  87 #define  ENABLE_CAPTURE               (0x01<<0)
  88 #define  MODE_ONEFRAME                (0x00<<1)
  89 #define  MODE_PINGPONG                (0x01<<1)
  90 #define  MODE_LINELOOP                (0x02<<1)
  91 #define  AXI_BURST_16                 (0x0F << 12)
  92
  93 //CIF_CIF_INTEN
  94 #define  FRAME_END_EN                   (0x01<<1)
  95 #define  BUS_ERR_EN                             (0x01<<6)
  96 #define  SCL_ERR_EN                             (0x01<<7)
  97
  98 //CIF_CIF_FOR
  99 #define  VSY_HIGH_ACTIVE                   (0x01<<0)
 100 #define  VSY_LOW_ACTIVE                    (0x00<<0)
 101 #define  HSY_LOW_ACTIVE                                (0x01<<1)
 102 #define  HSY_HIGH_ACTIVE                               (0x00<<1)
 103 #define  INPUT_MODE_YUV                                (0x00<<2)
 104 #define  INPUT_MODE_PAL                                (0x02<<2)
 105 #define  INPUT_MODE_NTSC                               (0x03<<2)
 106 #define  INPUT_MODE_RAW                                (0x04<<2)
 107 #define  INPUT_MODE_JPEG                               (0x05<<2)
 108 #define  INPUT_MODE_MIPI                               (0x06<<2)
 109 #define  YUV_INPUT_ORDER_UYVY(ori)             (ori & (~(0x03<<5)))
 110 #define  YUV_INPUT_ORDER_YVYU(ori)                 ((ori & (~(0x01<<6)))|(0x01<<5))
 111 #define  YUV_INPUT_ORDER_VYUY(ori)                 ((ori & (~(0x01<<5))) | (0x1<<6))
 112 #define  YUV_INPUT_ORDER_YUYV(ori)                 (ori|(0x03<<5))
 113 #define  YUV_INPUT_422                         (0x00<<7)
 114 #define  YUV_INPUT_420                         (0x01<<7)
 115 #define  INPUT_420_ORDER_EVEN                  (0x00<<8)
 116 #define  INPUT_420_ORDER_ODD                   (0x01<<8)
 117 #define  CCIR_INPUT_ORDER_ODD                  (0x00<<9)
 118 #define  CCIR_INPUT_ORDER_EVEN             (0x01<<9)
 119 #define  RAW_DATA_WIDTH_8                  (0x00<<11)
 120 #define  RAW_DATA_WIDTH_10                 (0x01<<11)
 121 #define  RAW_DATA_WIDTH_12                 (0x02<<11)
 122 #define  YUV_OUTPUT_422                    (0x00<<16)
 123 #define  YUV_OUTPUT_420                    (0x01<<16)
 124 #define  OUTPUT_420_ORDER_EVEN             (0x00<<17)
 125 #define  OUTPUT_420_ORDER_ODD              (0x01<<17)
 126 #define  RAWD_DATA_LITTLE_ENDIAN           (0x00<<18)
 127 #define  RAWD_DATA_BIG_ENDIAN              (0x01<<18)
 128 #define  UV_STORAGE_ORDER_UVUV             (0x00<<19)
 129 #define  UV_STORAGE_ORDER_VUVU             (0x01<<19)
 130
 131 //CIF_CIF_SCL_CTRL
 132 #define ENABLE_SCL_DOWN                    (0x01<<0)
 133 #define DISABLE_SCL_DOWN                   (0x00<<0)
 134 #define ENABLE_SCL_UP                      (0x01<<1)
 135 #define DISABLE_SCL_UP                     (0x00<<1)
 136 #define ENABLE_YUV_16BIT_BYPASS            (0x01<<4)
 137 #define DISABLE_YUV_16BIT_BYPASS           (0x00<<4)
 138 #define ENABLE_RAW_16BIT_BYPASS            (0x01<<5)
 139 #define DISABLE_RAW_16BIT_BYPASS           (0x00<<5)
 140 #define ENABLE_32BIT_BYPASS                (0x01<<6)
 141 #define DISABLE_32BIT_BYPASS               (0x00<<6)
 142
 143 //CRU,PIXCLOCK
 144 #define CRU_PCLK_REG30                     0xbc
 145 #define ENANABLE_INVERT_PCLK_CIF0          ((0x1<<24)|(0x1<<8))
 146 #define DISABLE_INVERT_PCLK_CIF0           ((0x1<<24)|(0x0<<8))
 147 #define ENANABLE_INVERT_PCLK_CIF1          ((0x1<<28)|(0x1<<12))
 148 #define DISABLE_INVERT_PCLK_CIF1           ((0x1<<28)|(0x0<<12))
 149
 150 #define CRU_CIF_RST_REG30                  0x128
 151 #define MASK_RST_CIF0                      (0x01 << 30)
 152 #define MASK_RST_CIF1                      (0x01 << 31)
 153 #define RQUEST_RST_CIF0                    (0x01 << 14)
 154 #define RQUEST_RST_CIF1                    (0x01 << 15)
 155
 156 #define MIN(x,y)   ((x<y) ? x: y)
 157 #define MAX(x,y)    ((x>y) ? x: y)
 158 #define RK_SENSOR_24MHZ      24*1000*1000          /* MHz */
 159 #define RK_SENSOR_48MHZ      48
 160
 161 #define write_cif_reg(base,addr, val)  __raw_writel(val, addr+(base))
 162 #define read_cif_reg(base,addr) __raw_readl(addr+(base))
 163 #define mask_cif_reg(addr, msk, val)    write_cif_reg(addr, (val)|((~(msk))&read_cif_reg(addr)))
 164
 165 #define write_cru_reg(addr, val)  __raw_writel(val, addr+RK30_CRU_BASE)
 166 #define read_cru_reg(addr) __raw_readl(addr+RK30_CRU_BASE)
 167 #define mask_cru_reg(addr, msk, val)    write_cru_reg(addr,(val)|((~(msk))&read_cru_reg(addr)))
 168
 169 //when work_with_ipp is not enabled,CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF is not defined.something wrong with it
 170 #ifdef CONFIG_VIDEO_RK29_WORK_IPP//CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 171 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 172 #define CAM_WORKQUEUE_IS_EN()   ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)\
 173                                       || (pcdev->icd_cb.sensor_cb))
 174 #define CAM_IPPWORK_IS_EN()     ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height))
 175 #else
 176 #define CAM_WORKQUEUE_IS_EN()  (true)
 177 #define CAM_IPPWORK_IS_EN()     ((pcdev->zoominfo.a.c.width != pcdev->icd->user_width) || (pcdev->zoominfo.a.c.height != pcdev->icd->user_height))
 178 #endif
 179 #else //CONFIG_VIDEO_RK29_WORK_IPP
 180 #define CAM_WORKQUEUE_IS_EN()    (false)
 181 #define CAM_IPPWORK_IS_EN()      (false)
 182 #endif
 183
 184 #define IS_CIF0()               (pcdev->hostid == RK_CAM_PLATFORM_DEV_ID_0)
 185 //Configure Macro
 186 /*
 187 *                        Driver Version Note
 188 *
 189 *v0.0.x : this driver is 2.6.32 kernel driver;
 190 *v0.1.x : this driver is 3.0.8 kernel driver;
 191 *
 192 *v0.x.1 : this driver first support rk2918;
 193 *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
 194 *                 and V4L2_PIX_FMT_YUV422P;
 195 *v0.x.3 : this driver support VIDIOC_ENUM_FRAMEINTERVALS;
 196 *v0.x.4 : this driver support digital zoom;
 197 *v0.x.5 : this driver support test framerate and query framerate from board file configuration;
 198 *v0.x.6 : this driver improve test framerate method;
 199 *v0.x.7 : digital zoom use the ipp to do scale and crop , otherwise ipp just do the scale. Something wrong with digital zoom if
 200           we do crop with cif and do scale with ipp , we will fix this  next version.
 201 *v0.x.8 : temp version,reinit capture list when setup video buf.
 202 *v0.x.9 : 1. add the case of IPP unsupportted ,just cropped by CIF(not do the scale) this version.
 203           2. flush workqueue when releas buffer
 204 *v0.x.a: 1. reset cif and wake up vb when cif have't receive data in a fixed time(now setted as 2 secs) so that app can
 205              be quitted
 206           2. when the flash is on ,flash off in a fixed time to prevent from flash light too hot.
 207           3. when front and back camera are the same sensor,and one has the flash ,other is not,flash can't work corrrectly ,fix it
 208           4. add  menu configs for convineuent to customize sensor series
 209 *v0.x.b:  specify the version is  NOT sure stable.
 210 *v0.x.c:  1. add cif reset when resolution changed to avoid of collecting data erro
 211           2. irq process is splitted to two step.
 212 *v0.x.e: fix bugs of early suspend when display_pd is closed.
 213 *v0.x.f: fix calculate ipp memory size is enough or not in try_fmt function;
 214 *v0.x.11: fix struct rk_camera_work may be reentrant
 215 */
 216 #define RK_CAM_VERSION_CODE KERNEL_VERSION(0, 2, 0x11)
 217
 218 /* limit to rk29 hardware capabilities */
 219 #define RK_CAM_BUS_PARAM   (SOCAM_MASTER |\
 220                 SOCAM_HSYNC_ACTIVE_HIGH |\
 221                 SOCAM_HSYNC_ACTIVE_LOW |\
 222                 SOCAM_VSYNC_ACTIVE_HIGH |\
 223                 SOCAM_VSYNC_ACTIVE_LOW |\
 224                 SOCAM_PCLK_SAMPLE_RISING |\
 225                 SOCAM_PCLK_SAMPLE_FALLING|\
 226                 SOCAM_DATA_ACTIVE_HIGH |\
 227                 SOCAM_DATA_ACTIVE_LOW|\
 228                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 229                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 230
 231 #define RK_CAM_W_MIN        48
 232 #define RK_CAM_H_MIN        32
 233 #define RK_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 234 #define RK_CAM_H_MAX        2764
 235 #define RK_CAM_FRAME_INVAL_INIT 3
 236 #define RK_CAM_FRAME_INVAL_DC 3          /* ddl@rock-chips.com :  */
 237 #define RK30_CAM_FRAME_MEASURE  5
 238 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 239 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 240
 241 /* buffer for one video frame */
 242 struct rk_camera_buffer
 243 {
 244     /* common v4l buffer stuff -- must be first */
 245     struct videobuf_buffer vb;
 246     enum v4l2_mbus_pixelcode    code;
 247     int                 inwork;
 248 };
 249 enum rk_camera_reg_state
 250 {
 251         Reg_Invalidate,
 252         Reg_Validate
 253 };
 254
 255 struct rk_camera_reg
 256 {
 257         unsigned int cifCtrl;
 258         unsigned int cifCrop;
 259         unsigned int cifFs;
 260         unsigned int cifIntEn;
 261         unsigned int cifFmt;
 262     unsigned int cifVirWidth;
 263     unsigned int cifScale;
 264 //      unsigned int VipCrm;
 265         enum rk_camera_reg_state Inval;
 266 };
 267 struct rk_camera_work
 268 {
 269         struct videobuf_buffer *vb;
 270         struct rk_camera_dev *pcdev;
 271         struct work_struct work;
 272     struct list_head queue;
 273     unsigned int index;
 274 };
 275 struct rk_camera_frmivalenum
 276 {
 277     struct v4l2_frmivalenum fival;
 278     struct rk_camera_frmivalenum *nxt;
 279 };
 280 struct rk_camera_frmivalinfo
 281 {
 282     struct soc_camera_device *icd;
 283     struct rk_camera_frmivalenum *fival_list;
 284 };
 285 struct rk_camera_zoominfo
 286 {
 287     struct semaphore sem;
 288     struct v4l2_crop a;
 289     int vir_width;
 290     int zoom_rate;
 291 };
 292 struct rk_camera_timer{
 293         struct rk_camera_dev *pcdev;
 294         struct hrtimer timer;
 295     bool istarted;
 296         };
 297 struct rk_camera_dev
 298 {
 299         struct soc_camera_host  soc_host;
 300         struct device           *dev;
 301         /* RK2827x is only supposed to handle one camera on its Quick Capture
 302          * interface. If anyone ever builds hardware to enable more than
 303          * one camera, they will have to modify this driver too */
 304         struct soc_camera_device *icd;
 305
 306         //************must modify start************/
 307         struct clk *pd_cif;
 308         struct clk *aclk_cif;
 309     struct clk *hclk_cif;
 310     struct clk *cif_clk_in;
 311     struct clk *cif_clk_out;
 312         //************must modify end************/
 313         void __iomem *base;
 314         int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 315         unsigned int irq;
 316         unsigned int fps;
 317     unsigned int last_fps;
 318     unsigned long frame_interval;
 319         unsigned int pixfmt;
 320         //for ipp
 321         unsigned int vipmem_phybase;
 322         unsigned int vipmem_size;
 323         unsigned int vipmem_bsize;
 324
 325         int host_width; //croped size
 326         int host_height;
 327         int host_left;  //sensor output size ?
 328         int host_top;
 329         int hostid;
 330     int icd_width;
 331     int icd_height;
 332
 333         struct rk29camera_platform_data *pdata;
 334         struct resource         *res;
 335         struct list_head        capture;
 336         struct rk_camera_zoominfo zoominfo;
 337
 338         spinlock_t              lock;
 339
 340         struct videobuf_buffer  *active;
 341         struct rk_camera_reg reginfo_suspend;
 342         struct workqueue_struct *camera_wq;
 343         struct rk_camera_work *camera_work;
 344     struct list_head camera_work_queue;
 345     spinlock_t camera_work_lock;
 346         unsigned int camera_work_count;
 347         struct rk_camera_timer fps_timer;
 348         struct rk_camera_work camera_reinit_work;
 349         int icd_init;
 350         rk29_camera_sensor_cb_s icd_cb;
 351         struct rk_camera_frmivalinfo icd_frmival[2];
 352  //   atomic_t to_process_frames;
 353     bool timer_get_fps;
 354     unsigned int reinit_times;
 355     struct videobuf_queue *video_vq;
 356     bool stop_cif;
 357     struct timeval first_tv;
 358 };
 359
 360 static const struct v4l2_queryctrl rk_camera_controls[] =
 361 {
 362         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
 363     {
 364         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 365         .type           = V4L2_CTRL_TYPE_INTEGER,
 366         .name           = "DigitalZoom Control",
 367         .minimum        = 100,
 368         .maximum        = 300,
 369         .step           = 5,
 370         .default_value = 100,
 371     },
 372     #endif
 373 };
 374
 375 static DEFINE_MUTEX(camera_lock);
 376 static const char *rk_cam_driver_description = "RK_Camera";
 377
 378 static int rk_camera_s_stream(struct soc_camera_device *icd, int enable);
 379 static void rk_camera_capture_process(struct work_struct *work);
 380
 381
 382 /*
 383  *  Videobuf operations
 384  */
 385 static int rk_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 386                                unsigned int *size)
 387 {
 388     struct soc_camera_device *icd = vq->priv_data;
 389         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 390     struct rk_camera_dev *pcdev = ici->priv;
 391         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 392                                                 icd->current_fmt->host_fmt);
 393         int bytes_per_line_host = soc_mbus_bytes_per_line(pcdev->host_width,
 394                                                 icd->current_fmt->host_fmt);
 395     unsigned int i;
 396     struct rk_camera_work *wk;
 397
 398     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 399
 400         if (bytes_per_line_host < 0)
 401                 return bytes_per_line_host;
 402
 403         /* planar capture requires Y, U and V buffers to be page aligned */
 404         *size = PAGE_ALIGN(bytes_per_line*icd->user_height);       /* Y pages UV pages, yuv422*/
 405         pcdev->vipmem_bsize = PAGE_ALIGN(bytes_per_line_host * pcdev->host_height);
 406
 407         if (CAM_WORKQUEUE_IS_EN()) {
 408         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 409         if (CAM_IPPWORK_IS_EN())
 410         #endif
 411         {
 412             BUG_ON(pcdev->vipmem_size<pcdev->vipmem_bsize);
 413                 if (*count > pcdev->vipmem_size/pcdev->vipmem_bsize) {    /* Buffers must be limited, when this resolution is genered by IPP */
 414                         *count = pcdev->vipmem_size/pcdev->vipmem_bsize;
 415                 }
 416         }
 417                 if ((pcdev->camera_work_count != *count) && pcdev->camera_work) {
 418                         kfree(pcdev->camera_work);
 419                         pcdev->camera_work = NULL;
 420                         pcdev->camera_work_count = 0;
 421                 }
 422
 423                 if (pcdev->camera_work == NULL) {
 424                         pcdev->camera_work = wk = kzalloc(sizeof(struct rk_camera_work)*(*count), GFP_KERNEL);
 425                         if (pcdev->camera_work == NULL) {
 426                                 RKCAMERA_TR("\n %s kmalloc fail\n", __FUNCTION__);
 427                                 BUG();
 428                         }
 429             INIT_LIST_HEAD(&pcdev->camera_work_queue);
 430
 431             for (i=0; i<(*count); i++) {
 432                 wk->index = i;
 433                 list_add_tail(&wk->queue, &pcdev->camera_work_queue);
 434                 wk++;
 435             }
 436                         pcdev->camera_work_count = (*count);
 437                 }
 438
 439         }
 440     pcdev->video_vq = vq;
 441     RKCAMERA_DG("%s..%d.. videobuf size:%d, vipmem_buf size:%d, count:%d \n",__FUNCTION__,__LINE__, *size,pcdev->vipmem_size, *count);
 442
 443     return 0;
 444 }
 445 static void rk_videobuf_free(struct videobuf_queue *vq, struct rk_camera_buffer *buf)
 446 {
 447     struct soc_camera_device *icd = vq->priv_data;
 448
 449     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 450             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 451
 452         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 453         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 454                 return;
 455
 456     if (in_interrupt())
 457         BUG();
 458     /*
 459          * This waits until this buffer is out of danger, i.e., until it is no
 460          * longer in STATE_QUEUED or STATE_ACTIVE
 461          */
 462         //videobuf_waiton(vq, &buf->vb, 0, 0);
 463     videobuf_dma_contig_free(vq, &buf->vb);
 464     dev_dbg(&icd->dev, "%s freed\n", __func__);
 465     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 466         return;
 467 }
 468 static int rk_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 469 {
 470     struct soc_camera_device *icd = vq->priv_data;
 471     struct rk_camera_buffer *buf;
 472     int ret;
 473     int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 474                                                 icd->current_fmt->host_fmt);
 475         if (bytes_per_line < 0)
 476                 return bytes_per_line;
 477
 478     buf = container_of(vb, struct rk_camera_buffer, vb);
 479
 480     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 481             vb, vb->baddr, vb->bsize);
 482
 483     /* Added list head initialization on alloc */
 484     WARN_ON(!list_empty(&vb->queue));
 485
 486     BUG_ON(NULL == icd->current_fmt);
 487
 488     if (buf->code    != icd->current_fmt->code ||
 489             vb->width   != icd->user_width ||
 490             vb->height  != icd->user_height ||
 491              vb->field   != field) {
 492         buf->code    = icd->current_fmt->code;
 493         vb->width   = icd->user_width;
 494         vb->height  = icd->user_height;
 495         vb->field   = field;
 496         vb->state   = VIDEOBUF_NEEDS_INIT;
 497     }
 498
 499     vb->size = bytes_per_line*vb->height;          /* ddl@rock-chips.com : fmt->depth is coorect */
 500     if (0 != vb->baddr && vb->bsize < vb->size) {
 501         ret = -EINVAL;
 502         goto out;
 503     }
 504
 505     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 506         ret = videobuf_iolock(vq, vb, NULL);
 507         if (ret) {
 508             goto fail;
 509         }
 510         vb->state = VIDEOBUF_PREPARED;
 511     }
 512
 513     return 0;
 514 fail:
 515     rk_videobuf_free(vq, buf);
 516 out:
 517     return ret;
 518 }
 519
 520 static inline void rk_videobuf_capture(struct videobuf_buffer *vb,struct rk_camera_dev *rk_pcdev)
 521 {
 522         unsigned int y_addr,uv_addr;
 523         struct rk_camera_dev *pcdev = rk_pcdev;
 524
 525     if (vb) {
 526                 if (CAM_WORKQUEUE_IS_EN()) {
 527                         y_addr = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 528                         uv_addr = y_addr + pcdev->host_width*pcdev->host_height;
 529                         if (y_addr > (pcdev->vipmem_phybase + pcdev->vipmem_size - pcdev->vipmem_bsize)) {
 530                                 RKCAMERA_TR("vipmem for IPP is overflow! %dx%d -> %dx%d vb_index:%d\n",pcdev->host_width,pcdev->host_height,
 531                                                   pcdev->icd->user_width,pcdev->icd->user_height, vb->i);
 532                                 BUG();
 533                         }
 534                 } else {
 535                         y_addr = vb->boff;
 536                         uv_addr = y_addr + vb->width * vb->height;
 537                 }
 538         write_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_Y, y_addr);
 539         write_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_UV, uv_addr);
 540         write_cif_reg(pcdev->base,CIF_CIF_FRM1_ADDR_Y, y_addr);
 541         write_cif_reg(pcdev->base,CIF_CIF_FRM1_ADDR_UV, uv_addr);
 542         write_cif_reg(pcdev->base,CIF_CIF_FRAME_STATUS,  0x00000002);//frame1 has been ready to receive data,frame 2 is not used
 543     }
 544 }
 545 /* Locking: Caller holds q->irqlock */
 546 static void rk_videobuf_queue(struct videobuf_queue *vq,
 547                                 struct videobuf_buffer *vb)
 548 {
 549     struct soc_camera_device *icd = vq->priv_data;
 550     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 551     struct rk_camera_dev *pcdev = ici->priv;
 552
 553     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 554             vb, vb->baddr, vb->bsize);
 555
 556     vb->state = VIDEOBUF_QUEUED;
 557         if (list_empty(&pcdev->capture)) {
 558                 list_add_tail(&vb->queue, &pcdev->capture);
 559         } else {
 560                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 561                         list_add_tail(&vb->queue, &pcdev->capture);
 562                 else
 563                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 564         }
 565     if (!pcdev->active) {
 566         pcdev->active = vb;
 567         rk_videobuf_capture(vb,pcdev);
 568     }
 569 }
 570 static int rk_pixfmt2ippfmt(unsigned int pixfmt, int *ippfmt)
 571 {
 572         switch (pixfmt)
 573         {
 574                 case V4L2_PIX_FMT_NV16:
 575                 case V4L2_PIX_FMT_NV61:
 576                 {
 577                         *ippfmt = IPP_Y_CBCR_H2V1;
 578                         break;
 579                 }
 580                 case V4L2_PIX_FMT_NV12:
 581                 case V4L2_PIX_FMT_NV21:
 582                 {
 583                         *ippfmt = IPP_Y_CBCR_H2V2;
 584                         break;
 585                 }
 586                 default:
 587                         goto rk_pixfmt2ippfmt_err;
 588         }
 589
 590         return 0;
 591 rk_pixfmt2ippfmt_err:
 592         return -1;
 593 }
 594 static void rk_camera_capture_process(struct work_struct *work)
 595 {
 596         struct rk_camera_work *camera_work = container_of(work, struct rk_camera_work, work);
 597         struct videobuf_buffer *vb = camera_work->vb;
 598         struct rk_camera_dev *pcdev = camera_work->pcdev;
 599         struct rk29_ipp_req ipp_req;
 600         unsigned long int flags;
 601     int src_y_offset,src_uv_offset,dst_y_offset,dst_uv_offset,src_y_size,dst_y_size;
 602     int scale_times,w,h,vipdata_base;
 603
 604     /*
 605     *ddl@rock-chips.com:
 606     * IPP Dest image resolution is 2047x1088, so scale operation break up some times
 607     */
 608     if ((pcdev->icd->user_width > 0x7f0) || (pcdev->icd->user_height > 0x430)) {
 609         scale_times = MAX((pcdev->icd->user_width/0x7f0),(pcdev->icd->user_height/0x430));
 610         scale_times++;
 611     } else {
 612         scale_times = 1;
 613     }
 614     memset(&ipp_req, 0, sizeof(struct rk29_ipp_req));
 615
 616
 617     down(&pcdev->zoominfo.sem);
 618     ipp_req.timeout = 3000;
 619     ipp_req.flag = IPP_ROT_0;
 620     ipp_req.store_clip_mode =1;
 621     ipp_req.src0.w = pcdev->zoominfo.a.c.width/scale_times;
 622     ipp_req.src0.h = pcdev->zoominfo.a.c.height/scale_times;
 623     ipp_req.src_vir_w = pcdev->zoominfo.vir_width;
 624     rk_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.src0.fmt);
 625     ipp_req.dst0.w = pcdev->icd->user_width/scale_times;
 626     ipp_req.dst0.h = pcdev->icd->user_height/scale_times;
 627     ipp_req.dst_vir_w = pcdev->icd->user_width;
 628     rk_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.dst0.fmt);
 629     vipdata_base = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 630     src_y_size = pcdev->host_width*pcdev->host_height;  //vipmem
 631     dst_y_size = pcdev->icd->user_width*pcdev->icd->user_height;
 632     for (h=0; h<scale_times; h++) {
 633         for (w=0; w<scale_times; w++) {
 634             int ipp_times = 3;
 635             src_y_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width
 636                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 637                     src_uv_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width/2
 638                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 639
 640             dst_y_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times + pcdev->icd->user_width*w/scale_times;
 641             dst_uv_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times/2 + pcdev->icd->user_width*w/scale_times;
 642
 643                 ipp_req.src0.YrgbMst = vipdata_base + src_y_offset;
 644                 ipp_req.src0.CbrMst = vipdata_base + src_y_size + src_uv_offset;
 645                 ipp_req.dst0.YrgbMst = vb->boff + dst_y_offset;
 646                 ipp_req.dst0.CbrMst = vb->boff + dst_y_size + dst_uv_offset;
 647                 while(ipp_times-- > 0) {
 648                 if (ipp_blit_sync(&ipp_req)){
 649                     RKCAMERA_TR("ipp do erro,do again,ipp_times = %d!\n",ipp_times);
 650                  } else {
 651                     break;
 652                  }
 653             }
 654
 655             if (ipp_times <= 0) {
 656                         spin_lock_irqsave(&pcdev->lock, flags);
 657                         vb->state = VIDEOBUF_NEEDS_INIT;
 658                         spin_unlock_irqrestore(&pcdev->lock, flags);
 659                         RKCAMERA_TR("Capture image(vb->i:0x%x) which IPP operated is error:\n",vb->i);
 660                         RKCAMERA_TR("widx:%d hidx:%d ",w,h);
 661                         RKCAMERA_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);
 662                         RKCAMERA_TR("ipp_req.src0.YrgbMst:0x%x ipp_req.src0.CbrMst:0x%x \n", ipp_req.src0.YrgbMst,ipp_req.src0.CbrMst);
 663                         RKCAMERA_TR("ipp_req.src0.w:0x%x ipp_req.src0.h:0x%x \n",ipp_req.src0.w,ipp_req.src0.h);
 664                         RKCAMERA_TR("ipp_req.src0.fmt:0x%x\n",ipp_req.src0.fmt);
 665                         RKCAMERA_TR("ipp_req.dst0.YrgbMst:0x%x ipp_req.dst0.CbrMst:0x%x \n",ipp_req.dst0.YrgbMst,ipp_req.dst0.CbrMst);
 666                         RKCAMERA_TR("ipp_req.dst0.w:0x%x ipp_req.dst0.h:0x%x \n",ipp_req.dst0.w ,ipp_req.dst0.h);
 667                         RKCAMERA_TR("ipp_req.dst0.fmt:0x%x\n",ipp_req.dst0.fmt);
 668                         RKCAMERA_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);
 669                         RKCAMERA_TR("ipp_req.timeout:0x%x ipp_req.flag :0x%x\n",ipp_req.timeout,ipp_req.flag);
 670
 671                         goto do_ipp_err;
 672                 }
 673         }
 674     }
 675
 676     if (pcdev->icd_cb.sensor_cb)
 677         (pcdev->icd_cb.sensor_cb)(vb);
 678 do_ipp_err:
 679     up(&pcdev->zoominfo.sem);
 680     wake_up(&(camera_work->vb->done));
 681     spin_lock_irqsave(&pcdev->camera_work_lock, flags);
 682     list_add_tail(&camera_work->queue, &pcdev->camera_work_queue);
 683     spin_unlock_irqrestore(&pcdev->camera_work_lock, flags);
 684         return;
 685 }
 686 static irqreturn_t rk_camera_irq(int irq, void *data)
 687 {
 688     struct rk_camera_dev *pcdev = data;
 689     struct videobuf_buffer *vb;
 690         struct rk_camera_work *wk;
 691         struct timeval tv;
 692     unsigned long tmp_intstat;
 693     unsigned long tmp_cifctrl;
 694
 695     tmp_intstat = read_cif_reg(pcdev->base,CIF_CIF_INTSTAT);
 696     tmp_cifctrl = read_cif_reg(pcdev->base,CIF_CIF_CTRL);
 697     if(pcdev->stop_cif == true)
 698         {
 699         printk("cif has stopped by app,needn't to deal this irq\n");
 700         write_cif_reg(pcdev->base,CIF_CIF_INTSTAT,0xFFFFFFFF);  /* clear vip interrupte single  */
 701          return IRQ_HANDLED;
 702              }
 703     if ((tmp_intstat & 0x0200) /*&& ((tmp_intstat & 0x1)==0)*/){//bit9 =1 ,bit0 = 0
 704         write_cif_reg(pcdev->base,CIF_CIF_INTSTAT,0x0200);  /* clear vip interrupte single  */
 705         if(tmp_cifctrl & ENABLE_CAPTURE)
 706             write_cif_reg(pcdev->base,CIF_CIF_CTRL, (tmp_cifctrl & ~ENABLE_CAPTURE));
 707          return IRQ_HANDLED;
 708     }
 709
 710     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 711     if (read_cif_reg(pcdev->base,CIF_CIF_FRAME_STATUS) & 0x01) {
 712         write_cif_reg(pcdev->base,CIF_CIF_INTSTAT,0x01);  /* clear vip interrupte single  */
 713         if (!pcdev->fps) {
 714             do_gettimeofday(&pcdev->first_tv);
 715         }
 716                 pcdev->fps++;
 717                 if (!pcdev->active)
 718                         goto RK_CAMERA_IRQ_END;
 719         if (pcdev->frame_inval>0) {
 720             pcdev->frame_inval--;
 721             rk_videobuf_capture(pcdev->active,pcdev);
 722             goto RK_CAMERA_IRQ_END;
 723         } else if (pcdev->frame_inval) {
 724                 RKCAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 725             pcdev->frame_inval = 0;
 726         }
 727         if(pcdev->fps == RK30_CAM_FRAME_MEASURE) {
 728             do_gettimeofday(&tv);
 729             pcdev->frame_interval = ((tv.tv_sec*1000000 + tv.tv_usec) - (pcdev->first_tv.tv_sec*1000000 + pcdev->first_tv.tv_usec))
 730                                     /(RK30_CAM_FRAME_MEASURE-1);
 731         }
 732         vb = pcdev->active;
 733         if(!vb){
 734             printk("no acticve buffer!!!\n");
 735             goto RK_CAMERA_IRQ_END;
 736             }
 737                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 738                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 739                 list_del_init(&vb->queue);
 740                 }
 741         pcdev->active = NULL;
 742         if (!list_empty(&pcdev->capture)) {
 743             pcdev->active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 744                         if (pcdev->active) {
 745                 WARN_ON(pcdev->active->state != VIDEOBUF_QUEUED);
 746                                 rk_videobuf_capture(pcdev->active,pcdev);
 747                         }
 748         }
 749         if (pcdev->active == NULL) {
 750                         RKCAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 751                 }
 752
 753                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 754                 vb->state = VIDEOBUF_DONE;
 755                 do_gettimeofday(&vb->ts);
 756                 vb->field_count++;
 757                 }
 758                 if (CAM_WORKQUEUE_IS_EN()) {
 759             if (!list_empty(&pcdev->camera_work_queue)) {
 760                 wk = list_entry(pcdev->camera_work_queue.next, struct rk_camera_work, queue);
 761                 list_del_init(&wk->queue);
 762                 INIT_WORK(&(wk->work), rk_camera_capture_process);
 763                         wk->vb = vb;
 764                         wk->pcdev = pcdev;
 765                         queue_work(pcdev->camera_wq, &(wk->work));
 766             }
 767                 } else {
 768                         wake_up(&vb->done);
 769                 }
 770
 771     }
 772
 773 RK_CAMERA_IRQ_END:
 774     if((tmp_cifctrl & ENABLE_CAPTURE) == 0)
 775         write_cif_reg(pcdev->base,CIF_CIF_CTRL, (tmp_cifctrl | ENABLE_CAPTURE));
 776     return IRQ_HANDLED;
 777 }
 778
 779
 780 static void rk_videobuf_release(struct videobuf_queue *vq,
 781                                   struct videobuf_buffer *vb)
 782 {
 783     struct rk_camera_buffer *buf = container_of(vb, struct rk_camera_buffer, vb);
 784     struct soc_camera_device *icd = vq->priv_data;
 785     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 786     struct rk_camera_dev *pcdev = ici->priv;
 787 #ifdef DEBUG
 788     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 789             vb, vb->baddr, vb->bsize);
 790
 791     switch (vb->state)
 792     {
 793         case VIDEOBUF_ACTIVE:
 794             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 795             break;
 796         case VIDEOBUF_QUEUED:
 797             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 798             break;
 799         case VIDEOBUF_PREPARED:
 800             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 801             break;
 802         default:
 803             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 804             break;
 805     }
 806 #endif
 807     if (vb->i == 0) {
 808         flush_workqueue(pcdev->camera_wq);
 809     }
 810
 811         if (vb == pcdev->active) {
 812                 RKCAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 813                 interruptible_sleep_on_timeout(&vb->done, msecs_to_jiffies(500));
 814         flush_workqueue(pcdev->camera_wq);
 815                 RKCAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 816         }
 817     rk_videobuf_free(vq, buf);
 818 }
 819
 820 static struct videobuf_queue_ops rk_videobuf_ops =
 821 {
 822     .buf_setup      = rk_videobuf_setup,
 823     .buf_prepare    = rk_videobuf_prepare,
 824     .buf_queue      = rk_videobuf_queue,
 825     .buf_release    = rk_videobuf_release,
 826 };
 827
 828 static void rk_camera_init_videobuf(struct videobuf_queue *q,
 829                                       struct soc_camera_device *icd)
 830 {
 831     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 832     struct rk_camera_dev *pcdev = ici->priv;
 833
 834     /* We must pass NULL as dev pointer, then all pci_* dma operations
 835      * transform to normal dma_* ones. */
 836     videobuf_queue_dma_contig_init(q,
 837                                    &rk_videobuf_ops,
 838                                    ici->v4l2_dev.dev, &pcdev->lock,
 839                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 840                                    V4L2_FIELD_NONE,
 841                                    sizeof(struct rk_camera_buffer),
 842                                    icd,&icd->video_lock);
 843 }
 844 static int rk_camera_activate(struct rk_camera_dev *pcdev, struct soc_camera_device *icd)
 845 {
 846     int err = 0;
 847
 848     if(!pcdev->aclk_cif || !pcdev->hclk_cif || !pcdev->cif_clk_in || !pcdev->cif_clk_out){
 849         RKCAMERA_TR(KERN_ERR "failed to get cif clock source\n");
 850         err = -ENOENT;
 851         goto RK_CAMERA_ACTIVE_ERR;
 852         }
 853
 854         clk_enable(pcdev->pd_cif);
 855         clk_enable(pcdev->aclk_cif);
 856
 857         clk_enable(pcdev->hclk_cif);
 858         clk_enable(pcdev->cif_clk_in);
 859
 860     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 861         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 862         clk_enable(pcdev->cif_clk_out);
 863     clk_set_rate(pcdev->cif_clk_out,RK_SENSOR_24MHZ);
 864
 865         ndelay(10);
 866     //soft reset  the registers
 867     #if 0 //has somthing wrong when suspend and resume now
 868     if(IS_CIF0()){
 869         printk("before set cru register reset cif0 0x%x\n\n",read_cru_reg(CRU_CIF_RST_REG30));
 870                 //dump regs
 871         {
 872                 printk("CIF_CIF_CTRL = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_CTRL));
 873                 printk("CIF_CIF_INTEN = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_INTEN));
 874                 printk("CIF_CIF_INTSTAT = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_INTSTAT));
 875                 printk("CIF_CIF_FOR = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_FOR));
 876                 printk("CIF_CIF_CROP = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_CROP));
 877                 printk("CIF_CIF_SET_SIZE = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_SET_SIZE));
 878                 printk("CIF_CIF_SCL_CTRL = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL));
 879                 printk("CRU_PCLK_REG30 = 0X%x\n",read_cru_reg(CRU_PCLK_REG30));
 880                 printk("CIF_CIF_LAST_LINE = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LAST_LINE));
 881
 882                 printk("CIF_CIF_LAST_PIX = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LAST_PIX));
 883                 printk("CIF_CIF_VIR_LINE_WIDTH = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH));
 884         printk("CIF_CIF_LINE_NUM_ADDR = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LINE_NUM_ADDR));
 885         printk("CIF_CIF_FRM0_ADDR_Y = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_Y));
 886         printk("CIF_CIF_FRM0_ADDR_UV = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_UV));
 887         printk("CIF_CIF_FRAME_STATUS = 0X%x\n\n",read_cif_reg(pcdev->base,CIF_CIF_FRAME_STATUS));
 888         }
 889
 890         mdelay(100);
 891         write_cru_reg(CRU_CIF_RST_REG30,(/*read_cru_reg(CRU_CIF_RST_REG30)|*/MASK_RST_CIF0|RQUEST_RST_CIF0 ));
 892         printk("set cru register reset cif0 0x%x\n",read_cru_reg(CRU_CIF_RST_REG30));
 893         write_cru_reg(CRU_CIF_RST_REG30,(read_cru_reg(CRU_CIF_RST_REG30)&(~RQUEST_RST_CIF0)) | MASK_RST_CIF0);
 894         mdelay(1000);
 895         printk("clean cru register reset cif0 0x%x\n",read_cru_reg(CRU_CIF_RST_REG30));
 896     }else{
 897         write_cru_reg(CRU_CIF_RST_REG30,MASK_RST_CIF1|RQUEST_RST_CIF1 | (read_cru_reg(CRU_CIF_RST_REG30)));
 898         write_cru_reg(CRU_CIF_RST_REG30,(read_cru_reg(CRU_CIF_RST_REG30)&(~RQUEST_RST_CIF1)) | MASK_RST_CIF1);
 899     }
 900     #endif
 901     write_cif_reg(pcdev->base,CIF_CIF_CTRL,AXI_BURST_16|MODE_ONEFRAME|DISABLE_CAPTURE);   /* ddl@rock-chips.com : vip ahb burst 16 */
 902     write_cif_reg(pcdev->base,CIF_CIF_INTEN, 0x01);    //capture complete interrupt enable
 903     RKCAMERA_DG("%s..%d.. CIF_CIF_CTRL = 0x%x\n",__FUNCTION__,__LINE__,read_cif_reg(pcdev->base, CIF_CIF_CTRL));
 904     return 0;
 905 RK_CAMERA_ACTIVE_ERR:
 906     return -ENODEV;
 907 }
 908
 909 static void rk_camera_deactivate(struct rk_camera_dev *pcdev)
 910 {
 911         clk_disable(pcdev->aclk_cif);
 912
 913         clk_disable(pcdev->hclk_cif);
 914         clk_disable(pcdev->cif_clk_in);
 915
 916         clk_disable(pcdev->cif_clk_out);
 917         clk_enable(pcdev->cif_clk_out);
 918     clk_set_rate(pcdev->cif_clk_out,48*1000*1000);
 919         clk_disable(pcdev->cif_clk_out);
 920
 921         clk_disable(pcdev->pd_cif);
 922     return;
 923 }
 924
 925 /* The following two functions absolutely depend on the fact, that
 926  * there can be only one camera on RK28 quick capture interface */
 927 static int rk_camera_add_device(struct soc_camera_device *icd)
 928 {
 929     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 930     struct rk_camera_dev *pcdev = ici->priv;
 931     struct device *control = to_soc_camera_control(icd);
 932     struct v4l2_subdev *sd;
 933     int ret,i,icd_catch;
 934     struct rk_camera_frmivalenum *fival_list,*fival_nxt;
 935
 936     mutex_lock(&camera_lock);
 937
 938     if (pcdev->icd) {
 939         ret = -EBUSY;
 940         goto ebusy;
 941     }
 942
 943     RKCAMERA_DG("RK Camera driver attached to %s\n",dev_name(icd->pdev));
 944
 945         pcdev->frame_inval = RK_CAM_FRAME_INVAL_INIT;
 946     pcdev->active = NULL;
 947     pcdev->icd = NULL;
 948         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 949     pcdev->zoominfo.zoom_rate = 100;
 950     pcdev->fps_timer.istarted = false;
 951
 952         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 953      * if app havn't dequeue all videobuf before close camera device;
 954         */
 955     INIT_LIST_HEAD(&pcdev->capture);
 956
 957     ret = rk_camera_activate(pcdev,icd);
 958     if (ret)
 959         goto ebusy;
 960     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 961     if (control) {
 962         sd = dev_get_drvdata(control);
 963                 v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_IOREQUEST,(void*)pcdev->pdata);
 964         #if 0
 965         ret = v4l2_subdev_call(sd,core, init, 0);
 966         if (ret)
 967             goto ebusy;
 968         #endif
 969         v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_CB_REGISTER,(void*)(&pcdev->icd_cb));
 970     }
 971     pcdev->icd = icd;
 972     pcdev->icd_init = 0;
 973
 974     icd_catch = 0;
 975     for (i=0; i<2; i++) {
 976         if (pcdev->icd_frmival[i].icd == icd)
 977             icd_catch = 1;
 978         if (pcdev->icd_frmival[i].icd == NULL) {
 979             pcdev->icd_frmival[i].icd = icd;
 980             icd_catch = 1;
 981         }
 982     }
 983     if (icd_catch == 0) {
 984         fival_list = pcdev->icd_frmival[0].fival_list;
 985         fival_nxt = fival_list;
 986         while(fival_nxt != NULL) {
 987             fival_nxt = fival_list->nxt;
 988             kfree(fival_list);
 989             fival_list = fival_nxt;
 990         }
 991         pcdev->icd_frmival[0].icd = icd;
 992         pcdev->icd_frmival[0].fival_list = kzalloc(sizeof(struct rk_camera_frmivalenum),GFP_KERNEL);
 993     }
 994 ebusy:
 995     mutex_unlock(&camera_lock);
 996
 997     return ret;
 998 }
 999 static void rk_camera_remove_device(struct soc_camera_device *icd)
1000 {
1001     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1002     struct rk_camera_dev *pcdev = ici->priv;
1003         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1004
1005         mutex_lock(&camera_lock);
1006     BUG_ON(icd != pcdev->icd);
1007
1008     RKCAMERA_DG("RK Camera driver detached from %s\n",dev_name(icd->pdev));
1009
1010         /* ddl@rock-chips.com: Application will call VIDIOC_STREAMOFF before close device, but
1011            stream may be turn on again before close device, if suspend and resume happened. */
1012         if (read_cif_reg(pcdev->base,CIF_CIF_CTRL) & ENABLE_CAPTURE) {
1013                 rk_camera_s_stream(icd,0);
1014         }
1015
1016     //soft reset  the registers
1017     #if 0 //has somthing wrong when suspend and resume now
1018     if(IS_CIF0()){
1019         write_cru_reg(CRU_CIF_RST_REG30,MASK_RST_CIF0|RQUEST_RST_CIF0 | (read_cru_reg(CRU_CIF_RST_REG30)));
1020         write_cru_reg(CRU_CIF_RST_REG30,(read_cru_reg(CRU_CIF_RST_REG30)&(~RQUEST_RST_CIF0)) | MASK_RST_CIF0);
1021     }else{
1022         write_cru_reg(CRU_CIF_RST_REG30,MASK_RST_CIF1|RQUEST_RST_CIF1 | (read_cru_reg(CRU_CIF_RST_REG30)));
1023         write_cru_reg(CRU_CIF_RST_REG30,(read_cru_reg(CRU_CIF_RST_REG30)&(~RQUEST_RST_CIF1)) | MASK_RST_CIF1);
1024     }
1025     #endif
1026     v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
1027     //if stream off is not been executed,timer is running.
1028     if(pcdev->fps_timer.istarted){
1029          hrtimer_cancel(&pcdev->fps_timer.timer);
1030          pcdev->fps_timer.istarted = false;
1031     }
1032     flush_work(&(pcdev->camera_reinit_work.work));
1033         flush_workqueue((pcdev->camera_wq));
1034
1035         if (pcdev->camera_work) {
1036                 kfree(pcdev->camera_work);
1037                 pcdev->camera_work = NULL;
1038                 pcdev->camera_work_count = 0;
1039         INIT_LIST_HEAD(&pcdev->camera_work_queue);
1040         }
1041         rk_camera_deactivate(pcdev);
1042         pcdev->active = NULL;
1043     pcdev->icd = NULL;
1044     pcdev->icd_cb.sensor_cb = NULL;
1045         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
1046         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
1047      * if app havn't dequeue all videobuf before close camera device;
1048         */
1049     INIT_LIST_HEAD(&pcdev->capture);
1050
1051         mutex_unlock(&camera_lock);
1052         RKCAMERA_DG("%s exit\n",__FUNCTION__);
1053
1054         return;
1055 }
1056 static int rk_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
1057 {
1058     unsigned long bus_flags, camera_flags, common_flags;
1059     unsigned int cif_ctrl_val = 0;
1060         const struct soc_mbus_pixelfmt *fmt;
1061         int ret = 0;
1062         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1063         struct rk_camera_dev *pcdev = ici->priv;
1064     RKCAMERA_DG("%s..%d..\n",__FUNCTION__,__LINE__);
1065
1066         fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
1067         if (!fmt)
1068                 return -EINVAL;
1069
1070     bus_flags = RK_CAM_BUS_PARAM;
1071         /* If requested data width is supported by the platform, use it */
1072         switch (fmt->bits_per_sample) {
1073         case 10:
1074                 if (!(bus_flags & SOCAM_DATAWIDTH_10))
1075                         return -EINVAL;
1076                 break;
1077         case 9:
1078                 if (!(bus_flags & SOCAM_DATAWIDTH_9))
1079                         return -EINVAL;
1080                 break;
1081         case 8:
1082                 if (!(bus_flags & SOCAM_DATAWIDTH_8))
1083                         return -EINVAL;
1084                 break;
1085         default:
1086                 return -EINVAL;
1087         }
1088
1089         if (icd->ops->query_bus_param)
1090         camera_flags = icd->ops->query_bus_param(icd);
1091         else
1092                 camera_flags = 0;
1093
1094     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
1095     if (!common_flags) {
1096         ret = -EINVAL;
1097         goto RK_CAMERA_SET_BUS_PARAM_END;
1098     }
1099
1100     ret = icd->ops->set_bus_param(icd, common_flags);
1101     if (ret < 0)
1102         goto RK_CAMERA_SET_BUS_PARAM_END;
1103
1104     cif_ctrl_val = read_cif_reg(pcdev->base,CIF_CIF_FOR);
1105         RKCAMERA_DG("%s..%d..cif_ctrl_val = 0x%x\n",__FUNCTION__,__LINE__,cif_ctrl_val);
1106     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
1107         if(IS_CIF0()) {
1108                 write_cru_reg(CRU_PCLK_REG30, read_cru_reg(CRU_PCLK_REG30) | ENANABLE_INVERT_PCLK_CIF0);
1109             RKCAMERA_DG("enable cif0 pclk invert\n");
1110         } else {
1111                 write_cru_reg(CRU_PCLK_REG30, read_cru_reg(CRU_PCLK_REG30) | ENANABLE_INVERT_PCLK_CIF1);
1112             RKCAMERA_DG("enable cif1 pclk invert\n");
1113         }
1114     } else {
1115                 if(IS_CIF0()){
1116                         write_cru_reg(CRU_PCLK_REG30, (read_cru_reg(CRU_PCLK_REG30) & 0xFFFFEFF ) | DISABLE_INVERT_PCLK_CIF0);
1117             RKCAMERA_DG("diable cif0 pclk invert\n");
1118         } else {
1119                         write_cru_reg(CRU_PCLK_REG30, (read_cru_reg(CRU_PCLK_REG30) & 0xFFFEFFF) | DISABLE_INVERT_PCLK_CIF1);
1120             RKCAMERA_DG("diable cif1 pclk invert\n");
1121         }
1122     }
1123     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
1124         cif_ctrl_val |= HSY_LOW_ACTIVE;
1125     } else {
1126                 cif_ctrl_val &= ~HSY_LOW_ACTIVE;
1127     }
1128     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
1129         cif_ctrl_val |= VSY_HIGH_ACTIVE;
1130     } else {
1131                 cif_ctrl_val &= ~VSY_HIGH_ACTIVE;
1132     }
1133
1134     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
1135     //vip_ctrl_val |= ENABLE_CAPTURE;
1136     write_cif_reg(pcdev->base,CIF_CIF_FOR, cif_ctrl_val);
1137     RKCAMERA_DG("%s..ctrl:0x%x CIF_CIF_FOR=%x  \n",__FUNCTION__,cif_ctrl_val,read_cif_reg(pcdev->base,CIF_CIF_FOR));
1138
1139 RK_CAMERA_SET_BUS_PARAM_END:
1140         if (ret)
1141         RKCAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
1142     return ret;
1143 }
1144
1145 static int rk_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
1146 {
1147     unsigned long bus_flags, camera_flags;
1148     int ret;
1149
1150     bus_flags = RK_CAM_BUS_PARAM;
1151         if (icd->ops->query_bus_param) {
1152         camera_flags = icd->ops->query_bus_param(icd);
1153         } else {
1154                 camera_flags = 0;
1155         }
1156     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
1157
1158     if (ret < 0)
1159         dev_warn(icd->dev.parent,
1160                          "Flags incompatible: camera %lx, host %lx\n",
1161                          camera_flags, bus_flags);
1162     return ret;
1163 }
1164
1165 static const struct soc_mbus_pixelfmt rk_camera_formats[] = {
1166    {
1167                 .fourcc                 = V4L2_PIX_FMT_NV12,
1168                 .name                   = "YUV420 NV12",
1169                 .bits_per_sample        = 8,
1170                 .packing                = SOC_MBUS_PACKING_1_5X8,
1171                 .order                  = SOC_MBUS_ORDER_LE,
1172         },{
1173                 .fourcc                 = V4L2_PIX_FMT_NV16,
1174                 .name                   = "YUV422 NV16",
1175                 .bits_per_sample        = 8,
1176                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1177                 .order                  = SOC_MBUS_ORDER_LE,
1178         },{
1179                 .fourcc                 = V4L2_PIX_FMT_NV21,
1180                 .name                   = "YUV420 NV21",
1181                 .bits_per_sample        = 8,
1182                 .packing                = SOC_MBUS_PACKING_1_5X8,
1183                 .order                  = SOC_MBUS_ORDER_LE,
1184         },{
1185                 .fourcc                 = V4L2_PIX_FMT_NV61,
1186                 .name                   = "YUV422 NV61",
1187                 .bits_per_sample        = 8,
1188                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1189                 .order                  = SOC_MBUS_ORDER_LE,
1190         }
1191 };
1192
1193 static void rk_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, enum v4l2_mbus_pixelcode icd_code, struct v4l2_rect *rect)
1194 {
1195         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1196     struct rk_camera_dev *pcdev = ici->priv;
1197     unsigned int cif_fs = 0,cif_crop = 0;
1198     unsigned int cif_fmt_val = read_cif_reg(pcdev->base,CIF_CIF_FOR) | INPUT_MODE_YUV|YUV_INPUT_422|INPUT_420_ORDER_EVEN|OUTPUT_420_ORDER_EVEN;
1199     switch (host_pixfmt)
1200     {
1201         case V4L2_PIX_FMT_NV16:
1202             cif_fmt_val &= ~YUV_OUTPUT_422;
1203                     cif_fmt_val &= ~UV_STORAGE_ORDER_UVUV;
1204                     pcdev->frame_inval = RK_CAM_FRAME_INVAL_DC;
1205                     pcdev->pixfmt = host_pixfmt;
1206             break;
1207         case V4L2_PIX_FMT_NV61:
1208                 cif_fmt_val &= ~YUV_OUTPUT_422;
1209                 cif_fmt_val |= UV_STORAGE_ORDER_VUVU;
1210                 pcdev->frame_inval = RK_CAM_FRAME_INVAL_DC;
1211                 pcdev->pixfmt = host_pixfmt;
1212                 break;
1213         case V4L2_PIX_FMT_NV12:
1214             cif_fmt_val |= YUV_OUTPUT_420;
1215                 cif_fmt_val &= ~UV_STORAGE_ORDER_UVUV;
1216                         if (pcdev->frame_inval != RK_CAM_FRAME_INVAL_INIT)
1217                                 pcdev->frame_inval = RK_CAM_FRAME_INVAL_INIT;
1218                         pcdev->pixfmt = host_pixfmt;
1219             break;
1220         case V4L2_PIX_FMT_NV21:
1221                 cif_fmt_val |= YUV_OUTPUT_420;
1222                 cif_fmt_val |= UV_STORAGE_ORDER_VUVU;
1223                 if (pcdev->frame_inval != RK_CAM_FRAME_INVAL_INIT)
1224                         pcdev->frame_inval = RK_CAM_FRAME_INVAL_INIT;
1225                 pcdev->pixfmt = host_pixfmt;
1226                 break;
1227             default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
1228                         cif_fmt_val |= YUV_OUTPUT_422;
1229                 break;
1230     }
1231     switch (icd_code)
1232     {
1233         case V4L2_MBUS_FMT_UYVY8_2X8:
1234             cif_fmt_val = YUV_INPUT_ORDER_UYVY(cif_fmt_val);
1235             break;
1236         case V4L2_MBUS_FMT_YUYV8_2X8:
1237             cif_fmt_val = YUV_INPUT_ORDER_YUYV(cif_fmt_val);
1238             break;
1239         case V4L2_MBUS_FMT_YVYU8_2X8:
1240                 cif_fmt_val = YUV_INPUT_ORDER_YVYU(cif_fmt_val);
1241                 break;
1242         case V4L2_MBUS_FMT_VYUY8_2X8:
1243                 cif_fmt_val = YUV_INPUT_ORDER_VYUY(cif_fmt_val);
1244                 break;
1245         default :
1246                         cif_fmt_val = YUV_INPUT_ORDER_YUYV(cif_fmt_val);
1247             break;
1248     }
1249
1250     {
1251        mdelay(100);
1252         if(IS_CIF0()){
1253     //          pmu_set_idle_request(IDLE_REQ_VIO, true);
1254                 cru_set_soft_reset(SOFT_RST_CIF0, true);
1255                 udelay(5);
1256                 cru_set_soft_reset(SOFT_RST_CIF0, false);
1257     //          pmu_set_idle_request(IDLE_REQ_VIO, false);
1258
1259         }else{
1260     //          pmu_set_idle_request(IDLE_REQ_VIO, true);
1261                 cru_set_soft_reset(SOFT_RST_CIF1, true);
1262                 udelay(5);
1263                 cru_set_soft_reset(SOFT_RST_CIF1, false);
1264     //          pmu_set_idle_request(IDLE_REQ_VIO, false);
1265         }
1266     }
1267     write_cif_reg(pcdev->base,CIF_CIF_CTRL,AXI_BURST_16|MODE_ONEFRAME|DISABLE_CAPTURE);   /* ddl@rock-chips.com : vip ahb burst 16 */
1268     write_cif_reg(pcdev->base,CIF_CIF_INTEN, 0x01|0x200);    //capture complete interrupt enable
1269     write_cif_reg(pcdev->base,CIF_CIF_FOR,cif_fmt_val);         /* ddl@rock-chips.com: VIP capture mode and capture format must be set before FS register set */
1270
1271    // read_cif_reg(pcdev->base,CIF_CIF_INTSTAT);                     /* clear vip interrupte single  */
1272     write_cif_reg(pcdev->base,CIF_CIF_INTSTAT,0xFFFFFFFF);
1273     if((read_cif_reg(pcdev->base,CIF_CIF_CTRL) & MODE_PINGPONG)
1274                 ||(read_cif_reg(pcdev->base,CIF_CIF_CTRL) & MODE_LINELOOP)) {
1275             BUG();
1276      } else{ // this is one frame mode
1277                 cif_crop = (rect->left+ (rect->top<<16));
1278                 cif_fs  = ((rect->width ) + (rect->height<<16));
1279          }
1280         RKCAMERA_TR("%s..%d.. \n",__FUNCTION__,__LINE__);
1281
1282         write_cif_reg(pcdev->base,CIF_CIF_CROP, cif_crop);
1283         write_cif_reg(pcdev->base,CIF_CIF_SET_SIZE, cif_fs);
1284         write_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH, rect->width);
1285         write_cif_reg(pcdev->base,CIF_CIF_FRAME_STATUS,  0x00000003);
1286
1287     //MUST bypass scale
1288         write_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL,0x10);
1289     RKCAMERA_DG("%s.. crop:0x%x fs:0x%x cif_fmt_val:0x%x CIF_CIF_FOR:0x%x\n",__FUNCTION__,cif_crop,cif_fs,cif_fmt_val,read_cif_reg(pcdev->base,CIF_CIF_FOR));
1290         return;
1291 }
1292
1293 static int rk_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
1294                                   struct soc_camera_format_xlate *xlate)
1295 {
1296     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1297     struct device *dev = icd->dev.parent;
1298     int formats = 0, ret;
1299         enum v4l2_mbus_pixelcode code;
1300         const struct soc_mbus_pixelfmt *fmt;
1301
1302         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1303         if (ret < 0)
1304                 /* No more formats */
1305                 return 0;
1306
1307         fmt = soc_mbus_get_fmtdesc(code);
1308         if (!fmt) {
1309                 dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
1310                 return 0;
1311         }
1312
1313     ret = rk_camera_try_bus_param(icd, fmt->bits_per_sample);
1314     if (ret < 0)
1315         return 0;
1316
1317     switch (code) {
1318             case V4L2_MBUS_FMT_UYVY8_2X8:
1319             case V4L2_MBUS_FMT_YUYV8_2X8:
1320             case V4L2_MBUS_FMT_YVYU8_2X8:
1321             case V4L2_MBUS_FMT_VYUY8_2X8:
1322             formats++;
1323             if (xlate) {
1324                 xlate->host_fmt = &rk_camera_formats[0];
1325                 xlate->code     = code;
1326                 xlate++;
1327                 dev_dbg(dev, "Providing format %s using code %d\n",
1328                         rk_camera_formats[0].name,code);
1329             }
1330
1331             formats++;
1332             if (xlate) {
1333                 xlate->host_fmt = &rk_camera_formats[1];
1334                 xlate->code     = code;
1335                 xlate++;
1336                 dev_dbg(dev, "Providing format %s using code %d\n",
1337                         rk_camera_formats[1].name,code);
1338             }
1339
1340             formats++;
1341             if (xlate) {
1342                 xlate->host_fmt = &rk_camera_formats[2];
1343                 xlate->code     = code;
1344                 xlate++;
1345                 dev_dbg(dev, "Providing format %s using code %d\n",
1346                         rk_camera_formats[2].name,code);
1347             }
1348
1349             formats++;
1350             if (xlate) {
1351                 xlate->host_fmt = &rk_camera_formats[3];
1352                 xlate->code     = code;
1353                 xlate++;
1354                 dev_dbg(dev, "Providing format %s using code %d\n",
1355                         rk_camera_formats[3].name,code);
1356             }
1357                         break;
1358         default:
1359             break;
1360     }
1361
1362     return formats;
1363 }
1364
1365 static void rk_camera_put_formats(struct soc_camera_device *icd)
1366 {
1367         return;
1368 }
1369
1370 static int rk_camera_set_crop(struct soc_camera_device *icd,
1371                                struct v4l2_crop *a)
1372 {
1373     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1374         struct v4l2_mbus_framefmt mf;
1375         u32 fourcc = icd->current_fmt->host_fmt->fourcc;
1376     int ret;
1377
1378     ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1379     if (ret < 0)
1380         return ret;
1381
1382     if ((mf.width < (a->c.left + a->c.width)) || (mf.height < (a->c.top + a->c.height)))  {
1383
1384         mf.width = a->c.left + a->c.width;
1385         mf.height = a->c.top + a->c.height;
1386
1387         v4l_bound_align_image(&mf.width, RK_CAM_W_MIN, RK_CAM_W_MAX, 1,
1388             &mf.height, RK_CAM_H_MIN, RK_CAM_H_MAX, 0,
1389             fourcc == V4L2_PIX_FMT_NV16 ?4 : 0);
1390
1391         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1392         if (ret < 0)
1393             return ret;
1394     }
1395
1396     rk_camera_setup_format(icd, fourcc, mf.code, &a->c);
1397
1398     icd->user_width = mf.width;
1399     icd->user_height = mf.height;
1400
1401     return 0;
1402 }
1403
1404 static int rk_camera_set_fmt(struct soc_camera_device *icd,
1405                               struct v4l2_format *f)
1406 {
1407     struct device *dev = icd->dev.parent;
1408     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1409     const struct soc_camera_format_xlate *xlate = NULL;
1410         struct soc_camera_host *ici =to_soc_camera_host(icd->dev.parent);
1411     struct rk_camera_dev *pcdev = ici->priv;
1412     struct v4l2_pix_format *pix = &f->fmt.pix;
1413     struct v4l2_mbus_framefmt mf;
1414     struct v4l2_rect rect;
1415     int ret,usr_w,usr_h;
1416     int stream_on = 0;
1417
1418         usr_w = pix->width;
1419         usr_h = pix->height;
1420     RKCAMERA_TR("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1421     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1422     if (!xlate) {
1423         dev_err(dev, "Format %x not found\n", pix->pixelformat);
1424         ret = -EINVAL;
1425         goto RK_CAMERA_SET_FMT_END;
1426     }
1427
1428     /* ddl@rock-chips.com: sensor init code transmit in here after open */
1429     if (pcdev->icd_init == 0) {
1430         v4l2_subdev_call(sd,core, init, 0);
1431         pcdev->icd_init = 1;
1432     }
1433     stream_on = read_cif_reg(pcdev->base,CIF_CIF_CTRL);
1434     if (stream_on & ENABLE_CAPTURE)
1435         write_cif_reg(pcdev->base,CIF_CIF_CTRL, (stream_on & (~ENABLE_CAPTURE)));
1436
1437         mf.width        = pix->width;
1438         mf.height       = pix->height;
1439         mf.field        = pix->field;
1440         mf.colorspace   = pix->colorspace;
1441         mf.code         = xlate->code;
1442         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1443         if (mf.code != xlate->code)
1444                 return -EINVAL;
1445         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1446         if ((mf.width != usr_w) || (mf.height != usr_h)) {
1447           int ratio;
1448         if (unlikely((mf.width <16) || (mf.width > 8190) || (mf.height < 16) || (mf.height > 8190))) {
1449                 RKCAMERA_TR("Senor and IPP both invalid source resolution(%dx%d)\n",mf.width,mf.height);
1450                 ret = -EINVAL;
1451                 goto RK_CAMERA_SET_FMT_END;
1452         }
1453         if (unlikely((usr_w <16)||(usr_h < 16))) {
1454                 RKCAMERA_TR("Senor and IPP both invalid destination resolution(%dx%d)\n",usr_w,usr_h);
1455                 ret = -EINVAL;
1456             goto RK_CAMERA_SET_FMT_END;
1457         }
1458                 //need crop ?
1459                 if((mf.width*10/mf.height) != (usr_w*10/usr_h)){
1460                         ratio = ((mf.width*10/usr_w) >= (mf.height*10/usr_h))?(mf.height*10/usr_h):(mf.width*10/usr_w);
1461                         pcdev->host_width = ratio*usr_w/10;
1462                         pcdev->host_height = ratio*usr_h/10;
1463             //for ipp ,need 4 bit alligned.
1464                 pcdev->host_width &= ~0x03;
1465                 pcdev->host_height &= ~0x03;
1466                         RKCAMERA_DG("ratio = %d ,host:%d*%d\n",ratio,pcdev->host_width,pcdev->host_height);
1467                         }
1468                 else{ // needn't crop ,just scaled by ipp
1469                         pcdev->host_width = mf.width;
1470                         pcdev->host_height = mf.height;
1471                         }
1472         }
1473         else{
1474                 pcdev->host_width = usr_w;
1475                 pcdev->host_height = usr_h;
1476                 }
1477         #else
1478         //according to crop and scale capability to change , here just cropt to user needed
1479         if (unlikely((mf.width <16) || (mf.width > 8190) || (mf.height < 16) || (mf.height > 8190))) {
1480                 RKCAMERA_TR("Senor invalid source resolution(%dx%d)\n",mf.width,mf.height);
1481                 ret = -EINVAL;
1482                 goto RK_CAMERA_SET_FMT_END;
1483         }
1484         if (unlikely((usr_w <16)||(usr_h < 16))) {
1485                 RKCAMERA_TR("Senor  invalid destination resolution(%dx%d)\n",usr_w,usr_h);
1486                 ret = -EINVAL;
1487             goto RK_CAMERA_SET_FMT_END;
1488         }
1489         pcdev->host_width = usr_w;
1490         pcdev->host_height = usr_h;
1491         #endif
1492     icd->sense = NULL;
1493     if (!ret) {
1494         RKCAMERA_DG("%s..%d.. host:%d*%d , sensor output:%d*%d,user demand:%d*%d\n",__FUNCTION__,__LINE__,
1495                 pcdev->host_width,pcdev->host_height,mf.width,mf.height,usr_w,usr_h);
1496         rect.width = pcdev->host_width;
1497         rect.height = pcdev->host_height;
1498         rect.left = ((mf.width-pcdev->host_width )>>1)&(~0x01);
1499         rect.top = ((mf.height-pcdev->host_height)>>1)&(~0x01);
1500         pcdev->host_left = rect.left;
1501         pcdev->host_top = rect.top;
1502
1503         down(&pcdev->zoominfo.sem);
1504         pcdev->zoominfo.a.c.width = pcdev->host_width*100/pcdev->zoominfo.zoom_rate;
1505                 pcdev->zoominfo.a.c.width &= ~0x03;
1506                 pcdev->zoominfo.a.c.height = pcdev->host_height*100/pcdev->zoominfo.zoom_rate;
1507                 pcdev->zoominfo.a.c.height &= ~0x03;
1508         //now digital zoom use ipp to do crop and scale
1509         if(pcdev->zoominfo.zoom_rate != 100){
1510             pcdev->zoominfo.a.c.left = ((pcdev->host_width - pcdev->zoominfo.a.c.width)>>1)&(~0x01);
1511                 pcdev->zoominfo.a.c.top = ((pcdev->host_height - pcdev->zoominfo.a.c.height)>>1)&(~0x01);
1512             }
1513         else{
1514             pcdev->zoominfo.a.c.left = 0;
1515                 pcdev->zoominfo.a.c.top = 0;
1516             }
1517                 pcdev->zoominfo.vir_width = pcdev->host_width;
1518         up(&pcdev->zoominfo.sem);
1519
1520         /* ddl@rock-chips.com: IPP work limit check */
1521         if ((pcdev->zoominfo.a.c.width != usr_w) || (pcdev->zoominfo.a.c.height != usr_h)) {
1522             if (usr_w > 0x7f0) {
1523                 if (((usr_w>>1)&0x3f) && (((usr_w>>1)&0x3f) <= 8)) {
1524                     RKCAMERA_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));
1525                     ret = -EINVAL;
1526                     goto RK_CAMERA_SET_FMT_END;
1527                 }
1528             } else {
1529                 if ((usr_w&0x3f) && ((usr_w&0x3f) <= 8)) {
1530                     RKCAMERA_TR("IPP Destination resolution(%dx%d, %d mod 64=%d is <= 8)",usr_w,usr_h, usr_w, (int)(usr_w&0x3f));
1531                     ret = -EINVAL;
1532                     goto RK_CAMERA_SET_FMT_END;
1533                 }
1534             }
1535         }
1536         RKCAMERA_DG("%s..%s icd width:%d  user width:%d (zoom: %dx%d@(%d,%d)->%dx%d)\n",__FUNCTION__,xlate->host_fmt->name,
1537                                    rect.width, pix->width, pcdev->zoominfo.a.c.width,pcdev->zoominfo.a.c.height, pcdev->zoominfo.a.c.left,pcdev->zoominfo.a.c.top,
1538                                    pix->width, pix->height);
1539         rk_camera_setup_format(icd, pix->pixelformat, mf.code, &rect);
1540
1541                 if (CAM_IPPWORK_IS_EN()) {
1542                         BUG_ON(pcdev->vipmem_phybase == 0);
1543                 }
1544         pix->width = usr_w;
1545         pix->height = usr_h;
1546         pix->field = mf.field;
1547         pix->colorspace = mf.colorspace;
1548         icd->current_fmt = xlate;
1549         pcdev->icd_width = mf.width;
1550         pcdev->icd_height = mf.height;
1551     }
1552
1553 RK_CAMERA_SET_FMT_END:
1554     if (stream_on & ENABLE_CAPTURE)
1555         write_cif_reg(pcdev->base,CIF_CIF_CTRL, (read_cif_reg(pcdev->base,CIF_CIF_CTRL) | ENABLE_CAPTURE));
1556         if (ret)
1557         RKCAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1558     return ret;
1559 }
1560 static bool rk_camera_fmt_capturechk(struct v4l2_format *f)
1561 {
1562     bool ret = false;
1563
1564         if ((f->fmt.pix.width == 1024) && (f->fmt.pix.height == 768)) {
1565                 ret = true;
1566         } else if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 1024)) {
1567                 ret = true;
1568         } else if ((f->fmt.pix.width == 1600) && (f->fmt.pix.height == 1200)) {
1569                 ret = true;
1570         } else if ((f->fmt.pix.width == 2048) && (f->fmt.pix.height == 1536)) {
1571                 ret = true;
1572         } else if ((f->fmt.pix.width == 2592) && (f->fmt.pix.height == 1944)) {
1573                 ret = true;
1574         }
1575
1576         if (ret == true)
1577                 RKCAMERA_DG("%s %dx%d is capture format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1578         return ret;
1579 }
1580 static int rk_camera_try_fmt(struct soc_camera_device *icd,
1581                                    struct v4l2_format *f)
1582 {
1583     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1584         struct rk_camera_dev *pcdev = ici->priv;
1585     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1586     const struct soc_camera_format_xlate *xlate;
1587     struct v4l2_pix_format *pix = &f->fmt.pix;
1588     __u32 pixfmt = pix->pixelformat;
1589     int ret,usr_w,usr_h,i;
1590         bool is_capture = rk_camera_fmt_capturechk(f);
1591         bool vipmem_is_overflow = false;
1592     struct v4l2_mbus_framefmt mf;
1593     int bytes_per_line_host;
1594
1595         usr_w = pix->width;
1596         usr_h = pix->height;
1597         RKCAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1598
1599     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1600     if (!xlate) {
1601         dev_err(icd->dev.parent, "Format (%c%c%c%c) not found\n", pixfmt & 0xFF, (pixfmt >> 8) & 0xFF,
1602                         (pixfmt >> 16) & 0xFF, (pixfmt >> 24) & 0xFF);
1603         ret = -EINVAL;
1604         RKCAMERA_TR("%s(version:%c%c%c) support format:\n",rk_cam_driver_description,(RK_CAM_VERSION_CODE&0xff0000)>>16,
1605             (RK_CAM_VERSION_CODE&0xff00)>>8,(RK_CAM_VERSION_CODE&0xff));
1606         for (i = 0; i < icd->num_user_formats; i++)
1607                     RKCAMERA_TR("(%c%c%c%c)-%s\n",
1608                     icd->user_formats[i].host_fmt->fourcc & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 8) & 0xFF,
1609                         (icd->user_formats[i].host_fmt->fourcc >> 16) & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 24) & 0xFF,
1610                         icd->user_formats[i].host_fmt->name);
1611         goto RK_CAMERA_TRY_FMT_END;
1612     }
1613    /* limit to rk29 hardware capabilities */
1614     v4l_bound_align_image(&pix->width, RK_CAM_W_MIN, RK_CAM_W_MAX, 1,
1615               &pix->height, RK_CAM_H_MIN, RK_CAM_H_MAX, 0,
1616               pixfmt == V4L2_PIX_FMT_NV16 ? 4 : 0);
1617
1618     pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1619                                                     xlate->host_fmt);
1620         if (pix->bytesperline < 0)
1621                 return pix->bytesperline;
1622
1623     /* limit to sensor capabilities */
1624         mf.width        = pix->width;
1625         mf.height       = pix->height;
1626         mf.field        = pix->field;
1627         mf.colorspace   = pix->colorspace;
1628         mf.code         = xlate->code;
1629
1630         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1631         if (ret < 0)
1632                 goto RK_CAMERA_TRY_FMT_END;
1633     RKCAMERA_DG("%s mf.width:%d  mf.height:%d\n",__FUNCTION__,mf.width,mf.height);
1634         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1635         if ((mf.width != usr_w) || (mf.height != usr_h)) {
1636         bytes_per_line_host = soc_mbus_bytes_per_line(mf.width,icd->current_fmt->host_fmt);
1637                 if (is_capture) {
1638                         vipmem_is_overflow = (PAGE_ALIGN(bytes_per_line_host*mf.height) > pcdev->vipmem_size);
1639                 } else {
1640                         /* Assume preview buffer minimum is 4 */
1641                         vipmem_is_overflow = (PAGE_ALIGN(bytes_per_line_host*mf.height)*4 > pcdev->vipmem_size);
1642                 }
1643                 if (vipmem_is_overflow == false) {
1644                         pix->width = usr_w;
1645                         pix->height = usr_h;
1646                 } else {
1647                         RKCAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",mf.width,mf.height,usr_w,usr_h);
1648             pix->width = mf.width;
1649             pix->height = mf.height;
1650                 }
1651
1652         if ((mf.width < usr_w) || (mf.height < usr_h)) {
1653             if (((usr_w>>1) > mf.width) || ((usr_h>>1) > mf.height)) {
1654                 RKCAMERA_TR("The aspect ratio(%dx%d/%dx%d) is bigger than 2 !\n",mf.width,mf.height,usr_w,usr_h);
1655                 pix->width = mf.width;
1656                 pix->height = mf.height;
1657             }
1658         }
1659         }
1660         #else
1661         //need to change according to crop and scale capablicity
1662         if ((mf.width > usr_w) && (mf.height > usr_h)) {
1663                         pix->width = usr_w;
1664                         pix->height = usr_h;
1665             } else if ((mf.width < usr_w) && (mf.height < usr_h)) {
1666                         RKCAMERA_TR("%dx%d can't scale up to %dx%d!\n",mf.width,mf.height,usr_w,usr_h);
1667             pix->width  = mf.width;
1668                 pix->height     = mf.height;
1669         }
1670     #endif
1671     pix->colorspace     = mf.colorspace;
1672
1673     switch (mf.field) {
1674         case V4L2_FIELD_ANY:
1675         case V4L2_FIELD_NONE:
1676                 pix->field      = V4L2_FIELD_NONE;
1677                 break;
1678         default:
1679                 /* TODO: support interlaced at least in pass-through mode */
1680                 dev_err(icd->dev.parent, "Field type %d unsupported.\n",
1681                         mf.field);
1682                 goto RK_CAMERA_TRY_FMT_END;
1683         }
1684
1685 RK_CAMERA_TRY_FMT_END:
1686         if (ret)
1687         RKCAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1688     return ret;
1689 }
1690
1691 static int rk_camera_reqbufs(struct soc_camera_device *icd,
1692                                struct v4l2_requestbuffers *p)
1693 {
1694     int i;
1695
1696     /* This is for locking debugging only. I removed spinlocks and now I
1697      * check whether .prepare is ever called on a linked buffer, or whether
1698      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1699      * it hadn't triggered */
1700     for (i = 0; i < p->count; i++) {
1701         struct rk_camera_buffer *buf = container_of(icd->vb_vidq.bufs[i],
1702                                                            struct rk_camera_buffer, vb);
1703         buf->inwork = 0;
1704         INIT_LIST_HEAD(&buf->vb.queue);
1705     }
1706
1707     return 0;
1708 }
1709
1710 static unsigned int rk_camera_poll(struct file *file, poll_table *pt)
1711 {
1712     struct soc_camera_device *icd = file->private_data;
1713     struct rk_camera_buffer *buf;
1714
1715     buf = list_entry(icd->vb_vidq.stream.next, struct rk_camera_buffer,
1716                     vb.stream);
1717
1718     poll_wait(file, &buf->vb.done, pt);
1719
1720     if (buf->vb.state == VIDEOBUF_DONE ||
1721             buf->vb.state == VIDEOBUF_ERROR)
1722         return POLLIN|POLLRDNORM;
1723
1724     return 0;
1725 }
1726
1727 static int rk_camera_querycap(struct soc_camera_host *ici,
1728                                 struct v4l2_capability *cap)
1729 {
1730     struct rk_camera_dev *pcdev = ici->priv;
1731     char orientation[5];
1732     int i;
1733
1734     strlcpy(cap->card, dev_name(pcdev->icd->pdev), sizeof(cap->card));
1735     memset(orientation,0x00,sizeof(orientation));
1736     for (i=0; i<RK_CAM_NUM;i++) {
1737         if ((pcdev->pdata->info[i].dev_name!=NULL) && (strcmp(dev_name(pcdev->icd->pdev), pcdev->pdata->info[i].dev_name) == 0)) {
1738             sprintf(orientation,"-%d",pcdev->pdata->info[i].orientation);
1739         }
1740     }
1741
1742     if (orientation[0] != '-') {
1743         RKCAMERA_TR("%s: %s is not registered in rk29_camera_platform_data, orientation apply default value",__FUNCTION__,dev_name(pcdev->icd->pdev));
1744         if (strstr(dev_name(pcdev->icd->pdev),"front"))
1745             strcat(cap->card,"-270");
1746         else
1747             strcat(cap->card,"-90");
1748     } else {
1749         strcat(cap->card,orientation);
1750     }
1751     cap->version = RK_CAM_VERSION_CODE;
1752     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1753
1754     return 0;
1755 }
1756 static void rk_camera_store_register(struct rk_camera_dev *pcdev)
1757 {
1758         pcdev->reginfo_suspend.cifCtrl = read_cif_reg(pcdev->base,CIF_CIF_CTRL);
1759         pcdev->reginfo_suspend.cifCrop = read_cif_reg(pcdev->base,CIF_CIF_CROP);
1760         pcdev->reginfo_suspend.cifFs = read_cif_reg(pcdev->base,CIF_CIF_SET_SIZE);
1761         pcdev->reginfo_suspend.cifIntEn = read_cif_reg(pcdev->base,CIF_CIF_INTEN);
1762         pcdev->reginfo_suspend.cifFmt= read_cif_reg(pcdev->base,CIF_CIF_FOR);
1763         pcdev->reginfo_suspend.cifVirWidth = read_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH);
1764         pcdev->reginfo_suspend.cifScale= read_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL);
1765 }
1766 static void rk_camera_restore_register(struct rk_camera_dev *pcdev)
1767 {
1768         write_cif_reg(pcdev->base,CIF_CIF_CTRL, pcdev->reginfo_suspend.cifCtrl&~ENABLE_CAPTURE);
1769         write_cif_reg(pcdev->base,CIF_CIF_INTEN, pcdev->reginfo_suspend.cifIntEn);
1770         write_cif_reg(pcdev->base,CIF_CIF_CROP, pcdev->reginfo_suspend.cifCrop);
1771         write_cif_reg(pcdev->base,CIF_CIF_SET_SIZE, pcdev->reginfo_suspend.cifFs);
1772         write_cif_reg(pcdev->base,CIF_CIF_FOR, pcdev->reginfo_suspend.cifFmt);
1773         write_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH,pcdev->reginfo_suspend.cifVirWidth);
1774         write_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL, pcdev->reginfo_suspend.cifScale);
1775 }
1776 static int rk_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1777 {
1778     struct soc_camera_host *ici =
1779                     to_soc_camera_host(icd->dev.parent);
1780     struct rk_camera_dev *pcdev = ici->priv;
1781         struct v4l2_subdev *sd;
1782     int ret = 0;
1783
1784         mutex_lock(&camera_lock);
1785         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1786                 rk_camera_s_stream(icd, 0);
1787                 sd = soc_camera_to_subdev(icd);
1788                 v4l2_subdev_call(sd, video, s_stream, 0);
1789                 ret = icd->ops->suspend(icd, state);
1790
1791                 pcdev->reginfo_suspend.cifCtrl = read_cif_reg(pcdev->base,CIF_CIF_CTRL);
1792                 pcdev->reginfo_suspend.cifCrop = read_cif_reg(pcdev->base,CIF_CIF_CROP);
1793                 pcdev->reginfo_suspend.cifFs = read_cif_reg(pcdev->base,CIF_CIF_SET_SIZE);
1794                 pcdev->reginfo_suspend.cifIntEn = read_cif_reg(pcdev->base,CIF_CIF_INTEN);
1795                 pcdev->reginfo_suspend.cifFmt= read_cif_reg(pcdev->base,CIF_CIF_FOR);
1796                 pcdev->reginfo_suspend.cifVirWidth = read_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH);
1797                 pcdev->reginfo_suspend.cifScale= read_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL);
1798
1799                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1800                 rk_camera_deactivate(pcdev);
1801
1802                 RKCAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1803         } else {
1804                 RKCAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1805         }
1806         mutex_unlock(&camera_lock);
1807     return ret;
1808 }
1809
1810 static int rk_camera_resume(struct soc_camera_device *icd)
1811 {
1812     struct soc_camera_host *ici =
1813                     to_soc_camera_host(icd->dev.parent);
1814     struct rk_camera_dev *pcdev = ici->priv;
1815         struct v4l2_subdev *sd;
1816     int ret = 0;
1817
1818         mutex_lock(&camera_lock);
1819         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1820                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1821                         rk_camera_activate(pcdev, icd);
1822                         write_cif_reg(pcdev->base,CIF_CIF_CTRL, pcdev->reginfo_suspend.cifCtrl&~ENABLE_CAPTURE);
1823                         write_cif_reg(pcdev->base,CIF_CIF_INTEN, pcdev->reginfo_suspend.cifIntEn);
1824                         write_cif_reg(pcdev->base,CIF_CIF_CROP, pcdev->reginfo_suspend.cifCrop);
1825                         write_cif_reg(pcdev->base,CIF_CIF_SET_SIZE, pcdev->reginfo_suspend.cifFs);
1826                         write_cif_reg(pcdev->base,CIF_CIF_FOR, pcdev->reginfo_suspend.cifFmt);
1827                         write_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH,pcdev->reginfo_suspend.cifVirWidth);
1828                         write_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL, pcdev->reginfo_suspend.cifScale);
1829                         rk_videobuf_capture(pcdev->active,pcdev);
1830                         rk_camera_s_stream(icd, 1);
1831                         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
1832                 } else {
1833                         RKCAMERA_TR("Resume fail, vip register recored is invalidate!!\n");
1834                         goto rk_camera_resume_end;
1835                 }
1836
1837                 ret = icd->ops->resume(icd);
1838                 sd = soc_camera_to_subdev(icd);
1839                 v4l2_subdev_call(sd, video, s_stream, 1);
1840
1841                 RKCAMERA_DG("%s Enter success\n",__FUNCTION__);
1842         } else {
1843                 RKCAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1844         }
1845
1846 rk_camera_resume_end:
1847         mutex_unlock(&camera_lock);
1848     return ret;
1849 }
1850
1851 static void rk_camera_reinit_work(struct work_struct *work)
1852 {
1853     struct v4l2_subdev *sd;
1854         struct rk_camera_work *camera_work = container_of(work, struct rk_camera_work, work);
1855         struct rk_camera_dev *pcdev = camera_work->pcdev;
1856     struct soc_camera_link *tmp_soc_cam_link;
1857     int index = 0;
1858         unsigned long flags = 0;
1859     if(pcdev->icd == NULL)
1860         return;
1861     sd = soc_camera_to_subdev(pcdev->icd);
1862     tmp_soc_cam_link = to_soc_camera_link(pcdev->icd);
1863         //dump regs
1864         {
1865                 RKCAMERA_TR("CIF_CIF_CTRL = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_CTRL));
1866                 RKCAMERA_TR("CIF_CIF_INTEN = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_INTEN));
1867                 RKCAMERA_TR("CIF_CIF_INTSTAT = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_INTSTAT));
1868                 RKCAMERA_TR("CIF_CIF_FOR = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_FOR));
1869                 RKCAMERA_TR("CIF_CIF_CROP = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_CROP));
1870                 RKCAMERA_TR("CIF_CIF_SET_SIZE = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_SET_SIZE));
1871                 RKCAMERA_TR("CIF_CIF_SCL_CTRL = 0x%x\n",read_cif_reg(pcdev->base,CIF_CIF_SCL_CTRL));
1872                 RKCAMERA_TR("CRU_PCLK_REG30 = 0X%x\n",read_cru_reg(CRU_PCLK_REG30));
1873                 RKCAMERA_TR("CIF_CIF_LAST_LINE = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LAST_LINE));
1874
1875                 RKCAMERA_TR("CIF_CIF_LAST_PIX = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LAST_PIX));
1876                 RKCAMERA_TR("CIF_CIF_VIR_LINE_WIDTH = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_VIR_LINE_WIDTH));
1877         RKCAMERA_TR("CIF_CIF_LINE_NUM_ADDR = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_LINE_NUM_ADDR));
1878         RKCAMERA_TR("CIF_CIF_FRM0_ADDR_Y = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_Y));
1879         RKCAMERA_TR("CIF_CIF_FRM0_ADDR_UV = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_FRM0_ADDR_UV));
1880         RKCAMERA_TR("CIF_CIF_FRAME_STATUS = 0X%x\n",read_cif_reg(pcdev->base,CIF_CIF_FRAME_STATUS));
1881         }
1882
1883     pcdev->stop_cif = true;
1884         write_cif_reg(pcdev->base,CIF_CIF_CTRL, (read_cif_reg(pcdev->base,CIF_CIF_CTRL)&(~ENABLE_CAPTURE)));
1885         RKCAMERA_DG("the reinit times = %d\n",pcdev->reinit_times);
1886
1887         spin_lock_irqsave(pcdev->video_vq->irqlock, flags);
1888         for (index = 0; index < VIDEO_MAX_FRAME; index++) {
1889                 if (NULL == pcdev->video_vq->bufs[index])
1890                         continue;
1891
1892                 if (pcdev->video_vq->bufs[index]->state == VIDEOBUF_QUEUED)
1893             {
1894                         list_del_init(&pcdev->video_vq->bufs[index]->queue);
1895                         pcdev->video_vq->bufs[index]->state = VIDEOBUF_NEEDS_INIT;
1896                         wake_up_all(&pcdev->video_vq->bufs[index]->done);
1897                 printk("wake up video buffer index = %d  !!!\n",index);
1898                 }
1899         }
1900         spin_unlock_irqrestore(pcdev->video_vq->irqlock, flags);
1901
1902         RKCAMERA_TR("the %d reinit times ,wake up video buffers!\n ",pcdev->reinit_times);
1903 }
1904 static enum hrtimer_restart rk_camera_fps_func(struct hrtimer *timer)
1905 {
1906     struct rk_camera_frmivalenum *fival_nxt=NULL,*fival_pre=NULL, *fival_rec=NULL;
1907         struct rk_camera_timer *fps_timer = container_of(timer, struct rk_camera_timer, timer);
1908         struct rk_camera_dev *pcdev = fps_timer->pcdev;
1909     int rec_flag,i;
1910    // static unsigned int last_fps = 0;
1911     struct soc_camera_link *tmp_soc_cam_link;
1912     tmp_soc_cam_link = to_soc_camera_link(pcdev->icd);
1913
1914         RKCAMERA_DG("rk_camera_fps_func fps:0x%x\n",pcdev->fps);
1915         if ((pcdev->fps < 1) || (pcdev->last_fps == pcdev->fps)) {
1916                 RKCAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor delay,last fps = %d,pcdev->fps = %d!\n",pcdev->last_fps,pcdev->fps);
1917                 pcdev->camera_reinit_work.pcdev = pcdev;
1918                 //INIT_WORK(&(pcdev->camera_reinit_work.work), rk_camera_reinit_work);
1919         pcdev->reinit_times++;
1920                 queue_work(pcdev->camera_wq,&(pcdev->camera_reinit_work.work));
1921         } else if(!pcdev->timer_get_fps) {
1922             pcdev->timer_get_fps = true;
1923             for (i=0; i<2; i++) {
1924             if (pcdev->icd == pcdev->icd_frmival[i].icd) {
1925                 fival_nxt = pcdev->icd_frmival[i].fival_list;
1926             }
1927         }
1928
1929         rec_flag = 0;
1930         fival_pre = fival_nxt;
1931         while (fival_nxt != NULL) {
1932
1933             RKCAMERA_DG("%s %c%c%c%c %dx%d framerate : %d/%d\n", dev_name(&pcdev->icd->dev),
1934                 fival_nxt->fival.pixel_format & 0xFF, (fival_nxt->fival.pixel_format >> 8) & 0xFF,
1935                             (fival_nxt->fival.pixel_format >> 16) & 0xFF, (fival_nxt->fival.pixel_format >> 24),
1936                             fival_nxt->fival.width, fival_nxt->fival.height, fival_nxt->fival.discrete.denominator,
1937                             fival_nxt->fival.discrete.numerator);
1938
1939             if (((fival_nxt->fival.pixel_format == pcdev->pixfmt)
1940                 && (fival_nxt->fival.height == pcdev->icd->user_height)
1941                 && (fival_nxt->fival.width == pcdev->icd->user_width))
1942                 || (fival_nxt->fival.discrete.denominator == 0)) {
1943
1944                 if (fival_nxt->fival.discrete.denominator == 0) {
1945                     fival_nxt->fival.index = 0;
1946                     fival_nxt->fival.width = pcdev->icd->user_width;
1947                     fival_nxt->fival.height= pcdev->icd->user_height;
1948                     fival_nxt->fival.pixel_format = pcdev->pixfmt;
1949                     fival_nxt->fival.discrete.denominator = pcdev->frame_interval;
1950                     fival_nxt->fival.reserved[1] = (pcdev->icd_width<<16)
1951                                                     |(pcdev->icd_height);
1952                     fival_nxt->fival.discrete.numerator = 1000000;
1953                     fival_nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1954                 }
1955                 rec_flag = 1;
1956                 fival_rec = fival_nxt;
1957             }
1958             fival_pre = fival_nxt;
1959             fival_nxt = fival_nxt->nxt;
1960         }
1961
1962         if ((rec_flag == 0) && fival_pre) {
1963             fival_pre->nxt = kzalloc(sizeof(struct rk_camera_frmivalenum), GFP_ATOMIC);
1964             if (fival_pre->nxt != NULL) {
1965                 fival_pre->nxt->fival.index = fival_pre->fival.index++;
1966                 fival_pre->nxt->fival.width = pcdev->icd->user_width;
1967                 fival_pre->nxt->fival.height= pcdev->icd->user_height;
1968                 fival_pre->nxt->fival.pixel_format = pcdev->pixfmt;
1969
1970                 fival_pre->nxt->fival.discrete.denominator = pcdev->frame_interval;
1971                 fival_pre->nxt->fival.reserved[1] = (pcdev->icd_width<<16)
1972                                                     |(pcdev->icd_height);
1973                 fival_pre->nxt->fival.discrete.numerator = 1000000;
1974                 fival_pre->nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1975                 rec_flag = 1;
1976                 fival_rec = fival_pre->nxt;
1977             }
1978         }
1979         }
1980     pcdev->last_fps = pcdev->fps ;
1981     pcdev->fps_timer.timer.node.expires= ktime_add_us(pcdev->fps_timer.timer.node.expires, ktime_to_us(ktime_set(3, 0)));
1982     pcdev->fps_timer.timer._softexpires= ktime_add_us(pcdev->fps_timer.timer._softexpires, ktime_to_us(ktime_set(3, 0)));
1983     //return HRTIMER_NORESTART;
1984     if(pcdev->reinit_times >=2)
1985         return HRTIMER_NORESTART;
1986     else
1987         return HRTIMER_RESTART;
1988 }
1989 static int rk_camera_s_stream(struct soc_camera_device *icd, int enable)
1990 {
1991         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1992     struct rk_camera_dev *pcdev = ici->priv;
1993     int cif_ctrl_val;
1994         int ret;
1995         unsigned long flags;
1996
1997         WARN_ON(pcdev->icd != icd);
1998
1999         cif_ctrl_val = read_cif_reg(pcdev->base,CIF_CIF_CTRL);
2000         if (enable) {
2001                 pcdev->fps = 0;
2002         pcdev->last_fps = 0;
2003         pcdev->frame_interval = 0;
2004                 hrtimer_cancel(&(pcdev->fps_timer.timer));
2005                 pcdev->fps_timer.pcdev = pcdev;
2006         pcdev->timer_get_fps = false;
2007         pcdev->reinit_times  = 0;
2008         pcdev->stop_cif = false;
2009 //              hrtimer_start(&(pcdev->fps_timer.timer),ktime_set(3, 0),HRTIMER_MODE_REL);
2010                 cif_ctrl_val |= ENABLE_CAPTURE;
2011                 write_cif_reg(pcdev->base,CIF_CIF_CTRL, cif_ctrl_val);
2012                 hrtimer_start(&(pcdev->fps_timer.timer),ktime_set(3, 0),HRTIMER_MODE_REL);
2013         pcdev->fps_timer.istarted = true;
2014         } else {
2015             //cancel timer before stop cif
2016                 ret = hrtimer_cancel(&pcdev->fps_timer.timer);
2017         pcdev->fps_timer.istarted = false;
2018         flush_work(&(pcdev->camera_reinit_work.work));
2019
2020         cif_ctrl_val &= ~ENABLE_CAPTURE;
2021                 spin_lock_irqsave(&pcdev->lock, flags);
2022         write_cif_reg(pcdev->base,CIF_CIF_CTRL, cif_ctrl_val);
2023         pcdev->stop_cif = true;
2024         spin_unlock_irqrestore(&pcdev->lock, flags);
2025                 flush_workqueue((pcdev->camera_wq));
2026                 RKCAMERA_DG("STREAM_OFF cancel timer and flush work:0x%x \n", ret);
2027         }
2028     //must be reinit,or will be somthing wrong in irq process.
2029     if(enable == false){
2030         pcdev->active = NULL;
2031         INIT_LIST_HEAD(&pcdev->capture);
2032         }
2033         RKCAMERA_DG("%s.. enable : 0x%x , CIF_CIF_CTRL = 0x%x\n", __FUNCTION__, enable,read_cif_reg(pcdev->base,CIF_CIF_CTRL));
2034         return 0;
2035 }
2036 int rk_camera_enum_frameintervals(struct soc_camera_device *icd, struct v4l2_frmivalenum *fival)
2037 {
2038     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
2039     struct rk_camera_dev *pcdev = ici->priv;
2040     struct rk_camera_frmivalenum *fival_list = NULL;
2041     struct v4l2_frmivalenum *fival_head = NULL;
2042     int i,ret = 0,index;
2043
2044     index = fival->index & 0x00ffffff;
2045     if ((fival->index & 0xff000000) == 0xff000000) {   /* ddl@rock-chips.com: detect framerate */
2046         for (i=0; i<2; i++) {
2047             if (pcdev->icd_frmival[i].icd == icd) {
2048                 fival_list = pcdev->icd_frmival[i].fival_list;
2049             }
2050         }
2051
2052         if (fival_list != NULL) {
2053             i = 0;
2054             while (fival_list != NULL) {
2055                 if ((fival->pixel_format == fival_list->fival.pixel_format)
2056                     && (fival->height == fival_list->fival.height)
2057                     && (fival->width == fival_list->fival.width)) {
2058                     if (i == index)
2059                         break;
2060                     i++;
2061                 }
2062                 fival_list = fival_list->nxt;
2063             }
2064
2065             if ((i==index) && (fival_list != NULL)) {
2066                 memcpy(fival, &fival_list->fival, sizeof(struct v4l2_frmivalenum));
2067             } else {
2068                 ret = -EINVAL;
2069             }
2070         } else {
2071             RKCAMERA_TR("%s: fival_list is NULL\n",__FUNCTION__);
2072             ret = -EINVAL;
2073         }
2074     }  else {
2075
2076         for (i=0; i<RK_CAM_NUM; i++) {
2077             if (pcdev->pdata->info[i].dev_name && (strcmp(dev_name(pcdev->icd->pdev),pcdev->pdata->info[i].dev_name) == 0)) {
2078                 fival_head = pcdev->pdata->info[i].fival;
2079             }
2080         }
2081
2082         if (fival_head == NULL) {
2083             RKCAMERA_TR("%s: %s is not registered in rk_camera_platform_data!!",__FUNCTION__,dev_name(pcdev->icd->pdev));
2084             ret = -EINVAL;
2085             goto rk_camera_enum_frameintervals_end;
2086         }
2087
2088         i = 0;
2089         while (fival_head->width && fival_head->height) {
2090             if ((fival->pixel_format == fival_head->pixel_format)
2091                 && (fival->height == fival_head->height)
2092                 && (fival->width == fival_head->width)) {
2093                 if (i == index) {
2094                     break;
2095                 }
2096                 i++;
2097             }
2098             fival_head++;
2099         }
2100
2101         if ((i == index) && (fival->height == fival_head->height) && (fival->width == fival_head->width)) {
2102             memcpy(fival, fival_head, sizeof(struct v4l2_frmivalenum));
2103             RKCAMERA_DG("%s %dx%d@%c%c%c%c framerate : %d/%d\n", dev_name(pcdev->icd->pdev),
2104                 fival->width, fival->height,
2105                 fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
2106                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
2107                              fival->discrete.denominator,fival->discrete.numerator);
2108         } else {
2109             if (index == 0)
2110                 RKCAMERA_TR("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(pcdev->icd->pdev),
2111                     fival->width,fival->height,
2112                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
2113                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
2114                             index);
2115             else
2116                 RKCAMERA_DG("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(pcdev->icd->pdev),
2117                     fival->width,fival->height,
2118                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
2119                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
2120                             index);
2121             ret = -EINVAL;
2122         }
2123     }
2124 rk_camera_enum_frameintervals_end:
2125     return ret;
2126 }
2127
2128 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
2129 static int rk_camera_set_digit_zoom(struct soc_camera_device *icd,
2130                                                                 const struct v4l2_queryctrl *qctrl, int zoom_rate)
2131 {
2132         struct v4l2_crop a;
2133         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
2134         struct rk_camera_dev *pcdev = ici->priv;
2135
2136
2137         //change the crop and scale parameters
2138     a.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2139         a.c.width = pcdev->host_width*100/zoom_rate;
2140         a.c.width &= ~0x03;
2141         a.c.height = pcdev->host_height*100/zoom_rate;
2142         a.c.height &= ~0x03;
2143         a.c.left = (pcdev->host_width - a.c.width)>>1;
2144         a.c.top = (pcdev->host_height - a.c.height)>>1;
2145     down(&pcdev->zoominfo.sem);
2146         pcdev->zoominfo.a.c.height = a.c.height;
2147         pcdev->zoominfo.a.c.width = a.c.width;
2148         pcdev->zoominfo.a.c.top = a.c.top;
2149         pcdev->zoominfo.a.c.left = a.c.left;
2150         pcdev->zoominfo.vir_width = pcdev->host_width;
2151     up(&pcdev->zoominfo.sem);
2152         RKCAMERA_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, icd->user_width, icd->user_height );
2153
2154         return 0;
2155 }
2156 #endif
2157 static inline struct v4l2_queryctrl const *rk_camera_soc_camera_find_qctrl(
2158         struct soc_camera_host_ops *ops, int id)
2159 {
2160         int i;
2161
2162         for (i = 0; i < ops->num_controls; i++)
2163                 if (ops->controls[i].id == id)
2164                         return &ops->controls[i];
2165
2166         return NULL;
2167 }
2168
2169
2170 static int rk_camera_set_ctrl(struct soc_camera_device *icd,
2171                                                                 struct v4l2_control *sctrl)
2172 {
2173
2174         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
2175         const struct v4l2_queryctrl *qctrl;
2176     struct rk_camera_dev *pcdev = ici->priv;
2177     int ret = 0;
2178
2179         qctrl = rk_camera_soc_camera_find_qctrl(ici->ops, sctrl->id);
2180         if (!qctrl) {
2181                 ret = -ENOIOCTLCMD;
2182         goto rk_camera_set_ctrl_end;
2183         }
2184
2185         switch (sctrl->id)
2186         {
2187         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
2188                 case V4L2_CID_ZOOM_ABSOLUTE:
2189                 {
2190                         if ((sctrl->value < qctrl->minimum) || (sctrl->value > qctrl->maximum)){
2191                         ret = -EINVAL;
2192                 goto rk_camera_set_ctrl_end;
2193                 }
2194             ret = rk_camera_set_digit_zoom(icd, qctrl, sctrl->value);
2195                         if (ret == 0) {
2196                                 pcdev->zoominfo.zoom_rate = sctrl->value;
2197             } else {
2198                 goto rk_camera_set_ctrl_end;
2199             }
2200                         break;
2201                 }
2202     #endif
2203                 default:
2204                         ret = -ENOIOCTLCMD;
2205                         break;
2206         }
2207 rk_camera_set_ctrl_end:
2208         return ret;
2209 }
2210
2211 static struct soc_camera_host_ops rk_soc_camera_host_ops =
2212 {
2213     .owner              = THIS_MODULE,
2214     .add                = rk_camera_add_device,
2215     .remove             = rk_camera_remove_device,
2216     .suspend    = rk_camera_suspend,
2217     .resume             = rk_camera_resume,
2218     .enum_frameinervals = rk_camera_enum_frameintervals,
2219     .set_crop   = rk_camera_set_crop,
2220     .get_formats        = rk_camera_get_formats,
2221     .put_formats        = rk_camera_put_formats,
2222     .set_fmt    = rk_camera_set_fmt,
2223     .try_fmt    = rk_camera_try_fmt,
2224     .init_videobuf      = rk_camera_init_videobuf,
2225     .reqbufs    = rk_camera_reqbufs,
2226     .poll               = rk_camera_poll,
2227     .querycap   = rk_camera_querycap,
2228     .set_bus_param      = rk_camera_set_bus_param,
2229     .s_stream = rk_camera_s_stream,   /* ddl@rock-chips.com : Add stream control for host */
2230     .set_ctrl = rk_camera_set_ctrl,
2231     .controls = rk_camera_controls,
2232     .num_controls = ARRAY_SIZE(rk_camera_controls)
2233
2234 };
2235 static void rk_camera_cif_iomux(int cif_index)
2236 {
2237     switch(cif_index){
2238         case 0:
2239             rk30_mux_api_set(GPIO1B3_CIF0CLKOUT_NAME, GPIO1B_CIF0_CLKOUT);
2240             break;
2241         case 1:
2242             rk30_mux_api_set(GPIO1C0_CIF1DATA2_RMIICLKOUT_RMIICLKIN_NAME,GPIO1C_CIF1_DATA2);
2243             rk30_mux_api_set(GPIO1C1_CIFDATA3_RMIITXEN_NAME,GPIO1C_CIF_DATA3);
2244             rk30_mux_api_set(GPIO1C2_CIF1DATA4_RMIITXD1_NAME,GPIO1C_CIF1_DATA4);
2245             rk30_mux_api_set(GPIO1C3_CIFDATA5_RMIITXD0_NAME,GPIO1C_CIF_DATA5);
2246             rk30_mux_api_set(GPIO1C4_CIFDATA6_RMIIRXERR_NAME,GPIO1C_CIF_DATA6);
2247             rk30_mux_api_set(GPIO1C5_CIFDATA7_RMIICRSDVALID_NAME,GPIO1C_CIF_DATA7);
2248             rk30_mux_api_set(GPIO1C6_CIFDATA8_RMIIRXD1_NAME,GPIO1C_CIF_DATA8);
2249             rk30_mux_api_set(GPIO1C7_CIFDATA9_RMIIRXD0_NAME,GPIO1C_CIF_DATA9);
2250
2251             rk30_mux_api_set(GPIO1D0_CIF1VSYNC_MIIMD_NAME,GPIO1D_CIF1_VSYNC);
2252             rk30_mux_api_set(GPIO1D1_CIF1HREF_MIIMDCLK_NAME,GPIO1D_CIF1_HREF);
2253             rk30_mux_api_set(GPIO1D2_CIF1CLKIN_NAME,GPIO1D_CIF1_CLKIN);
2254             rk30_mux_api_set(GPIO1D3_CIF1DATA0_NAME,GPIO1D_CIF1_DATA0);
2255             rk30_mux_api_set(GPIO1D4_CIF1DATA1_NAME,GPIO1D_CIF1_DATA1);
2256             rk30_mux_api_set(GPIO1D5_CIF1DATA10_NAME,GPIO1D_CIF1_DATA10);
2257             rk30_mux_api_set(GPIO1D6_CIF1DATA11_NAME,GPIO1D_CIF1_DATA11);
2258             rk30_mux_api_set(GPIO1D7_CIF1CLKOUT_NAME,GPIO1D_CIF1_CLKOUT);
2259             break;
2260         default:
2261             printk("cif index is erro!!!\n");
2262         }
2263
2264
2265 }
2266 static int rk_camera_probe(struct platform_device *pdev)
2267 {
2268     struct rk_camera_dev *pcdev;
2269     struct resource *res;
2270     struct rk_camera_frmivalenum *fival_list,*fival_nxt;
2271     int irq,i;
2272     int err = 0;
2273
2274     RKCAMERA_TR("RK30 Camera driver version: v%d.%d.%d\n",(RK_CAM_VERSION_CODE&0xff0000)>>16,
2275         (RK_CAM_VERSION_CODE&0xff00)>>8,RK_CAM_VERSION_CODE&0xff);
2276
2277     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
2278     irq = platform_get_irq(pdev, 0);
2279     if (!res || irq < 0) {
2280         err = -ENODEV;
2281         goto exit;
2282     }
2283     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
2284     if (!pcdev) {
2285         dev_err(&pdev->dev, "Could not allocate pcdev\n");
2286         err = -ENOMEM;
2287         goto exit_alloc;
2288     }
2289
2290         pcdev->zoominfo.zoom_rate = 100;
2291         pcdev->hostid = pdev->id;
2292     /*config output clk*/ // must modify start
2293     if(IS_CIF0()){
2294         pcdev->pd_cif = clk_get(NULL, "pd_cif0");
2295         pcdev->aclk_cif = clk_get(NULL, "aclk_cif0");
2296         pcdev->hclk_cif = clk_get(NULL, "hclk_cif0");
2297         pcdev->cif_clk_in = clk_get(NULL, "cif0_in");
2298         pcdev->cif_clk_out = clk_get(NULL, "cif0_out");
2299         rk_camera_cif_iomux(0);
2300     } else {
2301         pcdev->pd_cif = clk_get(NULL, "pd_cif1");
2302         pcdev->aclk_cif = clk_get(NULL, "aclk_cif1");
2303         pcdev->hclk_cif = clk_get(NULL, "hclk_cif1");
2304         pcdev->cif_clk_in = clk_get(NULL, "cif1_in");
2305         pcdev->cif_clk_out = clk_get(NULL, "cif1_out");
2306
2307         rk_camera_cif_iomux(1);
2308     }
2309
2310     if(IS_ERR(pcdev->pd_cif) || IS_ERR(pcdev->aclk_cif) || IS_ERR(pcdev->hclk_cif) || IS_ERR(pcdev->cif_clk_in) || IS_ERR(pcdev->cif_clk_out)){
2311         RKCAMERA_TR(KERN_ERR "%s(%d): failed to get cif clock source\n",__FUNCTION__,__LINE__);
2312         err = -ENOENT;
2313         goto exit_reqmem_vip;
2314     }
2315
2316     dev_set_drvdata(&pdev->dev, pcdev);
2317     pcdev->res = res;
2318     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
2319
2320         if (pcdev->pdata && pcdev->pdata->io_init) {
2321         pcdev->pdata->io_init();
2322     }
2323         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
2324         if (pcdev->pdata && IS_CIF0()) {
2325                 pcdev->vipmem_phybase = pcdev->pdata->meminfo.start;
2326                 pcdev->vipmem_size = pcdev->pdata->meminfo.size;
2327                 RKCAMERA_TR("%s(%d): Memory(start:0x%x size:0x%x) for IPP obtain \n",__FUNCTION__,__LINE__, pcdev->pdata->meminfo.start,pcdev->pdata->meminfo.size);
2328         } else if (pcdev->pdata) {
2329                 pcdev->vipmem_phybase = pcdev->pdata->meminfo_cif1.start;
2330                 pcdev->vipmem_size = pcdev->pdata->meminfo_cif1.size;
2331                 RKCAMERA_TR("%s(%d): Memory(start:0x%x size:0x%x) for IPP obtain \n",__FUNCTION__,__LINE__, pcdev->pdata->meminfo.start,pcdev->pdata->meminfo.size);
2332         } else {
2333                 RKCAMERA_TR("\n%s Memory for IPP have not obtain! IPP Function is fail\n",__FUNCTION__);
2334                 pcdev->vipmem_phybase = 0;
2335                 pcdev->vipmem_size = 0;
2336         }
2337         #endif
2338     INIT_LIST_HEAD(&pcdev->capture);
2339     INIT_LIST_HEAD(&pcdev->camera_work_queue);
2340     spin_lock_init(&pcdev->lock);
2341     spin_lock_init(&pcdev->camera_work_lock);
2342     sema_init(&pcdev->zoominfo.sem,1);
2343
2344     /*
2345      * Request the regions.
2346      */
2347     if(res) {
2348         if (!request_mem_region(res->start, res->end - res->start + 1,
2349                                 RK29_CAM_DRV_NAME)) {
2350             err = -EBUSY;
2351             goto exit_reqmem_vip;
2352         }
2353         pcdev->base = ioremap(res->start, res->end - res->start + 1);
2354         if (pcdev->base == NULL) {
2355             dev_err(pcdev->dev, "ioremap() of registers failed\n");
2356             err = -ENXIO;
2357             goto exit_ioremap_vip;
2358         }
2359     }
2360
2361     pcdev->irq = irq;
2362     pcdev->dev = &pdev->dev;
2363
2364     /* config buffer address */
2365     /* request irq */
2366     if(irq > 0){
2367         err = request_irq(pcdev->irq, rk_camera_irq, 0, RK29_CAM_DRV_NAME,
2368                           pcdev);
2369         if (err) {
2370             dev_err(pcdev->dev, "Camera interrupt register failed \n");
2371             goto exit_reqirq;
2372         }
2373         }
2374
2375 #ifdef CONFIG_VIDEO_RK29_WORK_IPP
2376     if(IS_CIF0()) {
2377         pcdev->camera_wq = create_workqueue("rk_cam_wkque_cif0");
2378     } else {
2379         pcdev->camera_wq = create_workqueue("rk_cam_wkque_cif1");
2380     }
2381     if (pcdev->camera_wq == NULL)
2382         goto exit_free_irq;
2383 #endif
2384
2385         pcdev->camera_reinit_work.pcdev = pcdev;
2386         INIT_WORK(&(pcdev->camera_reinit_work.work), rk_camera_reinit_work);
2387
2388     for (i=0; i<2; i++) {
2389         pcdev->icd_frmival[i].icd = NULL;
2390         pcdev->icd_frmival[i].fival_list = kzalloc(sizeof(struct rk_camera_frmivalenum),GFP_KERNEL);
2391
2392     }
2393     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
2394     pcdev->soc_host.ops         = &rk_soc_camera_host_ops;
2395     pcdev->soc_host.priv                = pcdev;
2396     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
2397     pcdev->soc_host.nr          = pdev->id;
2398
2399     err = soc_camera_host_register(&pcdev->soc_host);
2400     if (err)
2401         goto exit_free_irq;
2402         pcdev->fps_timer.pcdev = pcdev;
2403         hrtimer_init(&(pcdev->fps_timer.timer), CLOCK_MONOTONIC, HRTIMER_MODE_REL);
2404         pcdev->fps_timer.timer.function = rk_camera_fps_func;
2405     pcdev->icd_cb.sensor_cb = NULL;
2406
2407     RKCAMERA_DG("%s(%d) Exit  \n",__FUNCTION__,__LINE__);
2408     return 0;
2409
2410 exit_free_irq:
2411
2412     for (i=0; i<2; i++) {
2413         fival_list = pcdev->icd_frmival[i].fival_list;
2414         fival_nxt = fival_list;
2415         while(fival_nxt != NULL) {
2416             fival_nxt = fival_list->nxt;
2417             kfree(fival_list);
2418             fival_list = fival_nxt;
2419         }
2420     }
2421
2422     free_irq(pcdev->irq, pcdev);
2423         if (pcdev->camera_wq) {
2424                 destroy_workqueue(pcdev->camera_wq);
2425                 pcdev->camera_wq = NULL;
2426         }
2427 exit_reqirq:
2428     iounmap(pcdev->base);
2429 exit_ioremap_vip:
2430     release_mem_region(res->start, res->end - res->start + 1);
2431
2432 exit_reqmem_vip:
2433     if(pcdev->aclk_cif)
2434         pcdev->aclk_cif = NULL;
2435     if(pcdev->hclk_cif)
2436         pcdev->hclk_cif = NULL;
2437     if(pcdev->cif_clk_in)
2438         pcdev->cif_clk_in = NULL;
2439     if(pcdev->cif_clk_out)
2440         pcdev->cif_clk_out = NULL;
2441
2442     kfree(pcdev);
2443 exit_alloc:
2444
2445 exit:
2446     return err;
2447 }
2448
2449 static int __devexit rk_camera_remove(struct platform_device *pdev)
2450 {
2451     struct rk_camera_dev *pcdev = platform_get_drvdata(pdev);
2452     struct resource *res;
2453     struct rk_camera_frmivalenum *fival_list,*fival_nxt;
2454     int i;
2455
2456     free_irq(pcdev->irq, pcdev);
2457
2458         if (pcdev->camera_wq) {
2459                 destroy_workqueue(pcdev->camera_wq);
2460                 pcdev->camera_wq = NULL;
2461         }
2462
2463     for (i=0; i<2; i++) {
2464         fival_list = pcdev->icd_frmival[i].fival_list;
2465         fival_nxt = fival_list;
2466         while(fival_nxt != NULL) {
2467             fival_nxt = fival_list->nxt;
2468             kfree(fival_list);
2469             fival_list = fival_nxt;
2470         }
2471     }
2472
2473     soc_camera_host_unregister(&pcdev->soc_host);
2474
2475     res = pcdev->res;
2476     release_mem_region(res->start, res->end - res->start + 1);
2477     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
2478         pcdev->pdata->io_deinit(0);
2479         pcdev->pdata->io_deinit(1);
2480     }
2481
2482     kfree(pcdev);
2483
2484     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
2485
2486     return 0;
2487 }
2488
2489 static struct platform_driver rk_camera_driver =
2490 {
2491     .driver     = {
2492         .name   = RK29_CAM_DRV_NAME,
2493     },
2494     .probe              = rk_camera_probe,
2495     .remove             = __devexit_p(rk_camera_remove),
2496 };
2497
2498 static int rk_camera_init_async(void *unused)
2499 {
2500     RKCAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2501     platform_driver_register(&rk_camera_driver);
2502     return 0;
2503 }
2504
2505 static int __devinit rk_camera_init(void)
2506 {
2507     RKCAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2508     kthread_run(rk_camera_init_async, NULL, "rk_camera_init");
2509     return 0;
2510 }
2511
2512 static void __exit rk_camera_exit(void)
2513 {
2514     platform_driver_unregister(&rk_camera_driver);
2515 }
2516
2517 device_initcall_sync(rk_camera_init);
2518 module_exit(rk_camera_exit);
2519
2520 MODULE_DESCRIPTION("RKSoc Camera Host driver");
2521 MODULE_AUTHOR("ddl <ddl@rock-chips>");
2522 MODULE_LICENSE("GPL");
2523 #endif