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