drivers/media/video/rk30_camera_oneframe.c

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