drivers/media/video/rk30_camera_oneframe.c

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