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