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