drivers/media/video/rk30_camera_oneframe.c

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