drivers/media/video/rk30_camera_pingpong.c

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