drivers/media/video/rk30_camera_pingpong.c

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