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