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