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