drivers/media/video/rk30_camera_oneframe.c

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