drivers/media/video/rk30_camera_oneframe.c

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