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