drivers/media/video/rk29_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 #ifdef CONFIG_ARCH_RK29
  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 <mach/rk29_camera.h>
  33 #include <mach/rk29_iomap.h>
  34 #include <mach/iomux.h>
  35 #include <media/v4l2-common.h>
  36 #include <media/v4l2-dev.h>
  37 #include <media/videobuf-dma-contig.h>
  38 #include <media/soc_camera.h>
  39 #include <media/soc_mediabus.h>
  40 #include <mach/rk29-ipp.h>
  41
  42
  43 static int debug;
  44 module_param(debug, int, S_IRUGO|S_IWUSR|S_IWGRP);
  45
  46 #define dprintk(level, fmt, arg...) do {                        \
  47         if (debug >= level)                                     \
  48         printk(KERN_WARNING"rk29xx_camera: " fmt , ## arg); } while (0)
  49
  50 #define RK29CAMERA_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  51 #define RK29CAMERA_DG(format, ...) dprintk(1, format, ## __VA_ARGS__)
  52
  53 // VIP Reg Offset
  54 #define RK29_VIP_AHBR_CTRL                0x00
  55 #define RK29_VIP_INT_MASK                 0x04
  56 #define RK29_VIP_INT_STS                  0x08
  57 #define RK29_VIP_STS                      0x0c
  58 #define RK29_VIP_CTRL                     0x10
  59 #define RK29_VIP_CAPTURE_F1SA_Y           0x14
  60 #define RK29_VIP_CAPTURE_F1SA_UV          0x18
  61 #define RK29_VIP_CAPTURE_F1SA_Cr          0x1c
  62 #define RK29_VIP_CAPTURE_F2SA_Y           0x20
  63 #define RK29_VIP_CAPTURE_F2SA_UV          0x24
  64 #define RK29_VIP_CAPTURE_F2SA_Cr          0x28
  65 #define RK29_VIP_FB_SR                    0x2c
  66 #define RK29_VIP_FS                       0x30
  67 #define RK29_VIP_VIPRESERVED              0x34
  68 #define RK29_VIP_CROP                     0x38
  69 #define RK29_VIP_CRM                      0x3c
  70 #define RK29_VIP_RESET                    0x40
  71 #define RK29_VIP_L_SFT                    0x44
  72
  73 //The key register bit descrition
  74 // VIP_CTRL Reg
  75 #define  DISABLE_CAPTURE              (0x00<<0)
  76 #define  ENABLE_CAPTURE               (0x01<<0)
  77 #define  HSY_HIGH_ACTIVE              (0x00<<1)
  78 #define  HSY_LOW_ACTIVE               (0x01<<1)
  79 #define  VIP_CCIR656                  (0x00<<2)
  80 #define  VIP_SENSOR                   (0x01<<2)
  81 #define  SENSOR_UYVY                  (0x00<<3)
  82 #define  SENSOR_YUYV                  (0x01<<3)
  83 #define  VIP_YUV                      (0x00<<4)
  84 #define  VIP_RAW                      (0x01<<4)
  85 #define  CON_OR_PIN                   (0x00<<5)
  86 #define  ONEFRAME                     (0x01<<5)
  87 #define  VIPREGYUV420                 (0x00<<6)
  88 #define  VIPREGYUV422                 (0x01<<6)
  89 #define  FIELD0_START                 (0x00<<7)
  90 #define  FIELD1_START                 (0x01<<7)
  91 #define  CONTINUOUS                   (0x00<<8)
  92 #define  PING_PONG                    (0x01<<8)
  93 #define  POSITIVE_EDGE                (0x00<<9)
  94 #define  NEGATIVE_EDGE                (0x01<<9)
  95 #define  VIPREGNTSC                   (0x00<<10)
  96 #define  VIPREGPAL                    (0x01<<10)
  97 #define  VIP_DATA_LITTLEEND           (0x00<<11)
  98 #define  VIP_DATA_BIGEND              (0x01<<11)
  99 #define  VSY_LOW_ACTIVE               (0x00<<12)
 100 #define  VSY_HIGH_ACTIVE              (0x01<<12)
 101 #define  VIP_RAWINPUT_BYPASS          (0x00<<13)
 102 #define  VIP_RAWINPUT_POSITIVE_EDGE   (0x01<<13)
 103 #define  VIP_RAWINPUT_NEGATIVE_EDGE   (0x02<<13)
 104
 105 // GRF_SOC_CON0 Reg
 106 #define  GRF_SOC_CON0_Reg             0xbc
 107 #define  VIP_AXIMASTER                (0x00<<0)
 108 #define  VIP_AHBMASTER                (0x01<<2)
 109
 110 // GRF_OS_REG0
 111 #define  GRF_OS_REG0                  0xd0
 112 #define  VIP_ACLK_DIV_HCLK_1          (0x00<<0)
 113 #define  VIP_ACLK_DIV_HCLK_2          (0x01<<0)
 114
 115
 116 #define MIN(x,y)   ((x<y) ? x: y)
 117 #define MAX(x,y)    ((x>y) ? x: y)
 118 #define RK29_SENSOR_24MHZ      24           /* MHz */
 119 #define RK29_SENSOR_48MHZ      48
 120
 121 #define write_vip_reg(addr, val)  __raw_writel(val, addr+(rk29_camdev_info_ptr->base))
 122 #define read_vip_reg(addr) __raw_readl(addr+(rk29_camdev_info_ptr->base))
 123 #define mask_vip_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 124
 125 #define write_grf_reg(addr, val)  __raw_writel(val, addr+RK29_GRF_BASE)
 126 #define read_grf_reg(addr) __raw_readl(addr+RK29_GRF_BASE)
 127 #define mask_grf_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 128
 129 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 130 #define CAM_WORKQUEUE_IS_EN()   ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)\
 131                                   || (pcdev->icd_cb.sensor_cb))
 132 #define CAM_IPPWORK_IS_EN()     ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height))
 133 #else
 134 #define CAM_WORKQUEUE_IS_EN()   (true)
 135 #define CAM_IPPWORK_IS_EN()     ((pcdev->zoominfo.a.c.width != pcdev->icd->user_width) || (pcdev->zoominfo.a.c.height != pcdev->icd->user_height))
 136 #endif
 137 //Configure Macro
 138 /*
 139 *            Driver Version Note
 140 *
 141 *v0.0.x : this driver is 2.6.32 kernel driver;
 142 *v0.1.x : this driver is 3.0.8 kernel driver;
 143 *
 144 *v0.x.1 : this driver first support rk2918;
 145 *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
 146 *         and V4L2_PIX_FMT_YUV422P;
 147 *v0.x.3 : this driver support VIDIOC_ENUM_FRAMEINTERVALS;
 148 *v0.x.4 : this driver support digital zoom;
 149 *v0.x.5 : this driver support test framerate and query framerate from board file configuration;
 150 *v0.x.6 : this driver improve test framerate method;
 151 *v0.x.7 : this driver product resolution by IPP crop and scale, which user request but sensor can't support;
 152 *         note: this version is only provide yifang client, which is not official version;
 153 *v0.x.8 : this driver and rk29_camera.c support upto 3 back-sensors and upto 3 front-sensors;
 154 *v0.x.9 : camera io code is compatible for rk29xx and rk30xx
 155 *v0.x.a : fix error when calculate crop left-top point coordinate;
 156 *         note: this version provided as patch camera_patch_v1.1
 157 *v0.x.b : fix sensor autofocus thread may in running when reinit sensor if sensor haven't stream on in first init;
 158 *v0.x.c : fix work queue havn't been finished after close device;
 159 *v0.x.d : fix error when calculate crop left-top point coordinate;
 160 */
 161 #define RK29_CAM_VERSION_CODE KERNEL_VERSION(0, 1, 0xd)
 162
 163 /* limit to rk29 hardware capabilities */
 164 #define RK29_CAM_BUS_PARAM   (SOCAM_MASTER |\
 165                 SOCAM_HSYNC_ACTIVE_HIGH |\
 166                 SOCAM_HSYNC_ACTIVE_LOW |\
 167                 SOCAM_VSYNC_ACTIVE_HIGH |\
 168                 SOCAM_VSYNC_ACTIVE_LOW |\
 169                 SOCAM_PCLK_SAMPLE_RISING |\
 170                 SOCAM_PCLK_SAMPLE_FALLING|\
 171                 SOCAM_DATA_ACTIVE_HIGH |\
 172                 SOCAM_DATA_ACTIVE_LOW|\
 173                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 174                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 175
 176 #define RK29_CAM_W_MIN        48
 177 #define RK29_CAM_H_MIN        32
 178 #define RK29_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 179 #define RK29_CAM_H_MAX        2764
 180 #define RK29_CAM_FRAME_INVAL_INIT 3
 181 #define RK29_CAM_FRAME_INVAL_DC 3          /* ddl@rock-chips.com :  */
 182 #define RK29_CAM_FRAME_MEASURE  5
 183
 184 #define RK29_CAM_AXI   0
 185 #define RK29_CAM_AHB   1
 186 #define CONFIG_RK29_CAM_WORK_BUS    RK29_CAM_AXI
 187
 188 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 189 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 190
 191 /* buffer for one video frame */
 192 struct rk29_buffer
 193 {
 194     /* common v4l buffer stuff -- must be first */
 195     struct videobuf_buffer vb;
 196     enum v4l2_mbus_pixelcode    code;
 197     int                 inwork;
 198 };
 199 enum rk29_camera_reg_state
 200 {
 201         Reg_Invalidate,
 202         Reg_Validate
 203 };
 204
 205 struct rk29_camera_reg
 206 {
 207         unsigned int VipCtrl;
 208         unsigned int VipCrop;
 209         unsigned int VipFs;
 210         unsigned int VipIntMsk;
 211         unsigned int VipCrm;
 212         enum rk29_camera_reg_state Inval;
 213 };
 214 struct rk29_camera_work
 215 {
 216         struct videobuf_buffer *vb;
 217         struct rk29_camera_dev *pcdev;
 218         struct work_struct work;
 219 };
 220 struct rk29_camera_frmivalenum
 221 {
 222     struct v4l2_frmivalenum fival;
 223     struct rk29_camera_frmivalenum *nxt;
 224 };
 225 struct rk29_camera_frmivalinfo
 226 {
 227     struct soc_camera_device *icd;
 228     struct rk29_camera_frmivalenum *fival_list;
 229 };
 230 struct rk29_camera_zoominfo
 231 {
 232     struct semaphore sem;
 233     struct v4l2_crop a;
 234     int zoom_rate;
 235 };
 236 struct rk29_camera_dev
 237 {
 238     struct soc_camera_host      soc_host;
 239     struct device               *dev;
 240     /* RK2827x is only supposed to handle one camera on its Quick Capture
 241      * interface. If anyone ever builds hardware to enable more than
 242      * one camera, they will have to modify this driver too */
 243     struct soc_camera_device *icd;
 244
 245         struct clk *aclk_ddr_lcdc;
 246         struct clk *aclk_disp_matrix;
 247
 248         struct clk *hclk_cpu_display;
 249         struct clk *vip_slave;
 250
 251         struct clk *vip_out;
 252         struct clk *vip_input;
 253         struct clk *vip_bus;
 254
 255         struct clk *hclk_disp_matrix;
 256         struct clk *vip_matrix;
 257
 258         struct clk *pd_display;
 259
 260     void __iomem *base;
 261         void __iomem *grf_base;
 262     int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 263     unsigned int irq;
 264         unsigned int fps;
 265     unsigned long frame_interval;
 266         unsigned int pixfmt;
 267         unsigned int vipmem_phybase;
 268         unsigned int vipmem_size;
 269         unsigned int vipmem_bsize;
 270         int host_width;
 271         int host_height;
 272     int icd_width;
 273     int icd_height;
 274
 275     struct rk29camera_platform_data *pdata;
 276     struct resource             *res;
 277
 278     struct list_head    capture;
 279     struct rk29_camera_zoominfo zoominfo;
 280
 281     spinlock_t          lock;
 282
 283     struct videobuf_buffer      *active;
 284         struct rk29_camera_reg reginfo_suspend;
 285         struct workqueue_struct *camera_wq;
 286         struct rk29_camera_work *camera_work;
 287         unsigned int camera_work_count;
 288         struct hrtimer fps_timer;
 289         struct work_struct camera_reinit_work;
 290     int icd_init;
 291     rk29_camera_sensor_cb_s icd_cb;
 292     struct rk29_camera_frmivalinfo icd_frmival[2];
 293 };
 294
 295 static const struct v4l2_queryctrl rk29_camera_controls[] =
 296 {
 297         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
 298     {
 299         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 300         .type           = V4L2_CTRL_TYPE_INTEGER,
 301         .name           = "DigitalZoom Control",
 302         .minimum        = 100,
 303         .maximum        = 300,
 304         .step           = 5,
 305         .default_value = 100,
 306     },
 307     #endif
 308 };
 309
 310 static DEFINE_MUTEX(camera_lock);
 311 static const char *rk29_cam_driver_description = "RK29_Camera";
 312 static struct rk29_camera_dev *rk29_camdev_info_ptr;
 313
 314 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable);
 315
 316
 317 /*
 318  *  Videobuf operations
 319  */
 320 static int rk29_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 321                                unsigned int *size)
 322 {
 323     struct soc_camera_device *icd = vq->priv_data;
 324         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 325     struct rk29_camera_dev *pcdev = ici->priv;
 326     int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 327                                                 icd->current_fmt->host_fmt);
 328     int bytes_per_line_host = soc_mbus_bytes_per_line(pcdev->host_width,
 329                                                 icd->current_fmt->host_fmt);
 330
 331     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 332
 333         if (bytes_per_line < 0)
 334                 return bytes_per_line;
 335
 336         /* planar capture requires Y, U and V buffers to be page aligned */
 337         *size = PAGE_ALIGN(bytes_per_line*icd->user_height);       /* Y pages UV pages, yuv422*/
 338         pcdev->vipmem_bsize = PAGE_ALIGN(bytes_per_line_host * pcdev->host_height);
 339
 340
 341         if (CAM_WORKQUEUE_IS_EN()) {
 342         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_OFF
 343         if (CAM_IPPWORK_IS_EN())
 344         #endif
 345         {
 346             BUG_ON(pcdev->vipmem_size<pcdev->vipmem_bsize);
 347                 if (*count > pcdev->vipmem_size/pcdev->vipmem_bsize) {    /* Buffers must be limited, when this resolution is genered by IPP */
 348                         *count = pcdev->vipmem_size/pcdev->vipmem_bsize;
 349                 }
 350         }
 351                 if ((pcdev->camera_work_count != *count) && pcdev->camera_work) {
 352                         kfree(pcdev->camera_work);
 353                         pcdev->camera_work = NULL;
 354                         pcdev->camera_work_count = 0;
 355                 }
 356
 357                 if (pcdev->camera_work == NULL) {
 358                         pcdev->camera_work = kmalloc(sizeof(struct rk29_camera_work)*(*count), GFP_KERNEL);
 359                         if (pcdev->camera_work == NULL) {
 360                                 RK29CAMERA_TR("\n %s kmalloc fail\n", __FUNCTION__);
 361                                 BUG();
 362                         }
 363                         pcdev->camera_work_count = *count;
 364                 }
 365         }
 366
 367     RK29CAMERA_DG("%s..%d.. videobuf size:%d, vipmem_buf size:%d, count:%d \n",__FUNCTION__,__LINE__, *size,pcdev->vipmem_size, *count);
 368
 369     return 0;
 370 }
 371 static void rk29_videobuf_free(struct videobuf_queue *vq, struct rk29_buffer *buf)
 372 {
 373     struct soc_camera_device *icd = vq->priv_data;
 374
 375     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 376             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 377
 378         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 379         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 380                 return;
 381
 382     if (in_interrupt())
 383         BUG();
 384     /*
 385          * This waits until this buffer is out of danger, i.e., until it is no
 386          * longer in STATE_QUEUED or STATE_ACTIVE
 387          */
 388         //videobuf_waiton(vq, &buf->vb, 0, 0);
 389     videobuf_dma_contig_free(vq, &buf->vb);
 390     dev_dbg(&icd->dev, "%s freed\n", __func__);
 391     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 392         return;
 393 }
 394 static int rk29_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 395 {
 396     struct soc_camera_device *icd = vq->priv_data;
 397     struct rk29_buffer *buf;
 398     int ret;
 399     int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 400                                                 icd->current_fmt->host_fmt);
 401         if (bytes_per_line < 0)
 402                 return bytes_per_line;
 403
 404     buf = container_of(vb, struct rk29_buffer, vb);
 405
 406     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 407             vb, vb->baddr, vb->bsize);
 408
 409     //RK29CAMERA_TR("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 410
 411     /* Added list head initialization on alloc */
 412     WARN_ON(!list_empty(&vb->queue));
 413
 414     /* This can be useful if you want to see if we actually fill
 415      * the buffer with something */
 416     //memset((void *)vb->baddr, 0xaa, vb->bsize);
 417
 418     BUG_ON(NULL == icd->current_fmt);
 419
 420     if (buf->code    != icd->current_fmt->code ||
 421             vb->width   != icd->user_width ||
 422             vb->height  != icd->user_height ||
 423              vb->field   != field) {
 424         buf->code    = icd->current_fmt->code;
 425         vb->width   = icd->user_width;
 426         vb->height  = icd->user_height;
 427         vb->field   = field;
 428         vb->state   = VIDEOBUF_NEEDS_INIT;
 429     }
 430
 431     vb->size = bytes_per_line*vb->height;          /* ddl@rock-chips.com : fmt->depth is coorect */
 432     if (0 != vb->baddr && vb->bsize < vb->size) {
 433         ret = -EINVAL;
 434         goto out;
 435     }
 436
 437     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 438         ret = videobuf_iolock(vq, vb, NULL);
 439         if (ret) {
 440             goto fail;
 441         }
 442         vb->state = VIDEOBUF_PREPARED;
 443     }
 444     //RK29CAMERA_TR("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 445     return 0;
 446 fail:
 447     rk29_videobuf_free(vq, buf);
 448 out:
 449     return ret;
 450 }
 451
 452 static inline void rk29_videobuf_capture(struct videobuf_buffer *vb)
 453 {
 454         unsigned int y_addr,uv_addr;
 455         struct rk29_camera_dev *pcdev = rk29_camdev_info_ptr;
 456
 457     if (vb) {
 458                 if (CAM_WORKQUEUE_IS_EN()) {
 459                         y_addr = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 460                         uv_addr = y_addr + pcdev->host_width*pcdev->host_height;
 461
 462                         if (y_addr > (pcdev->vipmem_phybase + pcdev->vipmem_size - pcdev->vipmem_bsize)) {
 463                                 RK29CAMERA_TR("vipmem for IPP is overflow! %dx%d -> %dx%d vb_index:%d\n",pcdev->host_width,pcdev->host_height,
 464                                                   pcdev->icd->user_width,pcdev->icd->user_height, vb->i);
 465                                 BUG();
 466                         }
 467                 } else {
 468                         y_addr = vb->boff;
 469                         uv_addr = y_addr + vb->width * vb->height;
 470                 }
 471         write_vip_reg(RK29_VIP_CAPTURE_F1SA_Y, y_addr);
 472         write_vip_reg(RK29_VIP_CAPTURE_F1SA_UV, uv_addr);
 473         write_vip_reg(RK29_VIP_CAPTURE_F2SA_Y, y_addr);
 474         write_vip_reg(RK29_VIP_CAPTURE_F2SA_UV, uv_addr);
 475         write_vip_reg(RK29_VIP_FB_SR,  0x00000002);//frame1 has been ready to receive data,frame 2 is not used
 476     }
 477 }
 478 /* Locking: Caller holds q->irqlock */
 479 static void rk29_videobuf_queue(struct videobuf_queue *vq,
 480                                 struct videobuf_buffer *vb)
 481 {
 482     struct soc_camera_device *icd = vq->priv_data;
 483     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 484     struct rk29_camera_dev *pcdev = ici->priv;
 485
 486     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 487             vb, vb->baddr, vb->bsize);
 488
 489     vb->state = VIDEOBUF_QUEUED;
 490
 491         if (list_empty(&pcdev->capture)) {
 492                 list_add_tail(&vb->queue, &pcdev->capture);
 493         } else {
 494                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 495                         list_add_tail(&vb->queue, &pcdev->capture);
 496                 else
 497                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 498         }
 499     if (!pcdev->active) {
 500         pcdev->active = vb;
 501         rk29_videobuf_capture(vb);
 502     }
 503 }
 504 static int rk29_pixfmt2ippfmt(unsigned int pixfmt, int *ippfmt)
 505 {
 506         switch (pixfmt)
 507         {
 508                 case V4L2_PIX_FMT_NV16:
 509         case V4L2_PIX_FMT_YUV422P:
 510                 {
 511                         *ippfmt = IPP_Y_CBCR_H2V1;
 512                         break;
 513                 }
 514                 case V4L2_PIX_FMT_NV12:
 515         case V4L2_PIX_FMT_YUV420:
 516                 {
 517                         *ippfmt = IPP_Y_CBCR_H2V2;
 518                         break;
 519                 }
 520                 default:
 521                         goto rk29_pixfmt2ippfmt_err;
 522         }
 523
 524         return 0;
 525 rk29_pixfmt2ippfmt_err:
 526         return -1;
 527 }
 528 static void rk29_camera_capture_process(struct work_struct *work)
 529 {
 530         struct rk29_camera_work *camera_work = container_of(work, struct rk29_camera_work, work);
 531         struct videobuf_buffer *vb = camera_work->vb;
 532         struct rk29_camera_dev *pcdev = camera_work->pcdev;
 533         struct rk29_ipp_req ipp_req;
 534         unsigned long int flags;
 535     int src_y_offset,src_uv_offset,dst_y_offset,dst_uv_offset,src_y_size,dst_y_size;
 536     int scale_times,w,h,vipdata_base;
 537
 538     /*
 539     *ddl@rock-chips.com:
 540     * IPP Dest image resolution is 2047x1088, so scale operation break up some times
 541     */
 542     if ((pcdev->icd->user_width > 0x7f0) || (pcdev->icd->user_height > 0x430)) {
 543         scale_times = MAX((pcdev->icd->user_width/0x7f0),(pcdev->icd->user_height/0x430));
 544         scale_times++;
 545     } else {
 546         scale_times = 1;
 547     }
 548
 549     memset(&ipp_req, 0, sizeof(struct rk29_ipp_req));
 550
 551     down(&pcdev->zoominfo.sem);
 552
 553     ipp_req.timeout = 100;
 554     ipp_req.flag = IPP_ROT_0;
 555     ipp_req.src0.w = pcdev->zoominfo.a.c.width/scale_times;
 556     ipp_req.src0.h = pcdev->zoominfo.a.c.height/scale_times;
 557     ipp_req.src_vir_w = pcdev->host_width;
 558     rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.src0.fmt);
 559     ipp_req.dst0.w = pcdev->icd->user_width/scale_times;
 560     ipp_req.dst0.h = pcdev->icd->user_height/scale_times;
 561     ipp_req.dst_vir_w = pcdev->icd->user_width;
 562     rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.dst0.fmt);
 563
 564     vipdata_base = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 565     src_y_size = pcdev->host_width*pcdev->host_height;
 566     dst_y_size = pcdev->icd->user_width*pcdev->icd->user_height;
 567
 568     for (h=0; h<scale_times; h++) {
 569         for (w=0; w<scale_times; w++) {
 570
 571             src_y_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width
 572                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 573                     src_uv_offset = (pcdev->zoominfo.a.c.top + h*pcdev->zoominfo.a.c.height/scale_times)* pcdev->host_width/2
 574                         + pcdev->zoominfo.a.c.left + w*pcdev->zoominfo.a.c.width/scale_times;
 575
 576             dst_y_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times + pcdev->icd->user_width*w/scale_times;
 577             dst_uv_offset = pcdev->icd->user_width*pcdev->icd->user_height*h/scale_times/2 + pcdev->icd->user_width*w/scale_times;
 578
 579                 ipp_req.src0.YrgbMst = vipdata_base + src_y_offset;
 580                 ipp_req.src0.CbrMst = vipdata_base + src_y_size + src_uv_offset;
 581                 ipp_req.dst0.YrgbMst = vb->boff + dst_y_offset;
 582                 ipp_req.dst0.CbrMst = vb->boff + dst_y_size + dst_uv_offset;
 583
 584                 if (ipp_blit_sync(&ipp_req)) {
 585                         spin_lock_irqsave(&pcdev->lock, flags);
 586                         vb->state = VIDEOBUF_ERROR;
 587                         spin_unlock_irqrestore(&pcdev->lock, flags);
 588
 589                 RK29CAMERA_TR("Capture image(vb->i:0x%x) which IPP operated is error:\n",vb->i);
 590                 RK29CAMERA_TR("widx:%d hidx:%d ",w,h);
 591                 RK29CAMERA_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);
 592                 RK29CAMERA_TR("ipp_req.src0.YrgbMst:0x%x ipp_req.src0.CbrMst:0x%x \n", ipp_req.src0.YrgbMst,ipp_req.src0.CbrMst);
 593                 RK29CAMERA_TR("ipp_req.src0.w:0x%x ipp_req.src0.h:0x%x \n",ipp_req.src0.w,ipp_req.src0.h);
 594                 RK29CAMERA_TR("ipp_req.src0.fmt:0x%x\n",ipp_req.src0.fmt);
 595                 RK29CAMERA_TR("ipp_req.dst0.YrgbMst:0x%x ipp_req.dst0.CbrMst:0x%x \n",ipp_req.dst0.YrgbMst,ipp_req.dst0.CbrMst);
 596                 RK29CAMERA_TR("ipp_req.dst0.w:0x%x ipp_req.dst0.h:0x%x \n",ipp_req.dst0.w ,ipp_req.dst0.h);
 597                 RK29CAMERA_TR("ipp_req.dst0.fmt:0x%x\n",ipp_req.dst0.fmt);
 598                 RK29CAMERA_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);
 599                 RK29CAMERA_TR("ipp_req.timeout:0x%x ipp_req.flag :0x%x\n",ipp_req.timeout,ipp_req.flag);
 600
 601                         goto do_ipp_err;
 602                 }
 603         }
 604     }
 605
 606     if (pcdev->icd_cb.sensor_cb)
 607         (pcdev->icd_cb.sensor_cb)(vb);
 608
 609 do_ipp_err:
 610     up(&pcdev->zoominfo.sem);
 611     wake_up(&(camera_work->vb->done));
 612         return;
 613 }
 614 static irqreturn_t rk29_camera_irq(int irq, void *data)
 615 {
 616     struct rk29_camera_dev *pcdev = data;
 617     struct videobuf_buffer *vb;
 618         struct rk29_camera_work *wk;
 619     static struct timeval first_tv;
 620     struct timeval tv;
 621
 622     read_vip_reg(RK29_VIP_INT_STS);    /* clear vip interrupte single  */
 623     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 624     if (read_vip_reg(RK29_VIP_FB_SR) & 0x01) {
 625         if (!pcdev->fps) {
 626             do_gettimeofday(&first_tv);
 627         }
 628                 pcdev->fps++;
 629                 if (!pcdev->active)
 630                         goto RK29_CAMERA_IRQ_END;
 631
 632         if (pcdev->frame_inval>0) {
 633             pcdev->frame_inval--;
 634             rk29_videobuf_capture(pcdev->active);
 635             goto RK29_CAMERA_IRQ_END;
 636         } else if (pcdev->frame_inval) {
 637                 RK29CAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 638             pcdev->frame_inval = 0;
 639         }
 640
 641         if(pcdev->fps == RK29_CAM_FRAME_MEASURE) {
 642             do_gettimeofday(&tv);
 643             pcdev->frame_interval = ((tv.tv_sec*1000000 + tv.tv_usec) - (first_tv.tv_sec*1000000 + first_tv.tv_usec))
 644                                     /(RK29_CAM_FRAME_MEASURE-1);
 645         }
 646
 647         vb = pcdev->active;
 648                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 649                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 650                 list_del_init(&vb->queue);
 651                 }
 652
 653         pcdev->active = NULL;
 654         if (!list_empty(&pcdev->capture)) {
 655             pcdev->active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 656                         if (pcdev->active) {
 657                                 rk29_videobuf_capture(pcdev->active);
 658                         }
 659         }
 660
 661         if (pcdev->active == NULL) {
 662                         RK29CAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 663         }
 664
 665                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 666                 vb->state = VIDEOBUF_DONE;
 667                 do_gettimeofday(&vb->ts);
 668                 vb->field_count++;
 669                 }
 670
 671                 if (CAM_WORKQUEUE_IS_EN()) {
 672                         wk = pcdev->camera_work + vb->i;
 673                         INIT_WORK(&(wk->work), rk29_camera_capture_process);
 674                         wk->vb = vb;
 675                         wk->pcdev = pcdev;
 676                         queue_work(pcdev->camera_wq, &(wk->work));
 677                 } else {
 678                         wake_up(&vb->done);
 679                 }
 680     }
 681
 682 RK29_CAMERA_IRQ_END:
 683     return IRQ_HANDLED;
 684 }
 685
 686
 687 static void rk29_videobuf_release(struct videobuf_queue *vq,
 688                                   struct videobuf_buffer *vb)
 689 {
 690     struct rk29_buffer *buf = container_of(vb, struct rk29_buffer, vb);
 691     struct soc_camera_device *icd = vq->priv_data;
 692     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 693     struct rk29_camera_dev *pcdev = ici->priv;
 694 #ifdef DEBUG
 695     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 696             vb, vb->baddr, vb->bsize);
 697
 698     switch (vb->state)
 699     {
 700         case VIDEOBUF_ACTIVE:
 701             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 702             break;
 703         case VIDEOBUF_QUEUED:
 704             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 705             break;
 706         case VIDEOBUF_PREPARED:
 707             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 708             break;
 709         default:
 710             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 711             break;
 712     }
 713 #endif
 714     if(vb->i == 0){
 715         flush_workqueue(pcdev->camera_wq);
 716     }
 717
 718         if (vb == pcdev->active) {
 719                 RK29CAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 720                 interruptible_sleep_on_timeout(&vb->done, msecs_to_jiffies(500));
 721         flush_workqueue(pcdev->camera_wq);
 722                 RK29CAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 723         }
 724     rk29_videobuf_free(vq, buf);
 725 }
 726
 727 static struct videobuf_queue_ops rk29_videobuf_ops =
 728 {
 729     .buf_setup      = rk29_videobuf_setup,
 730     .buf_prepare    = rk29_videobuf_prepare,
 731     .buf_queue      = rk29_videobuf_queue,
 732     .buf_release    = rk29_videobuf_release,
 733 };
 734
 735 static void rk29_camera_init_videobuf(struct videobuf_queue *q,
 736                                       struct soc_camera_device *icd)
 737 {
 738     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 739     struct rk29_camera_dev *pcdev = ici->priv;
 740
 741     /* We must pass NULL as dev pointer, then all pci_* dma operations
 742      * transform to normal dma_* ones. */
 743     videobuf_queue_dma_contig_init(q,
 744                                    &rk29_videobuf_ops,
 745                                    ici->v4l2_dev.dev, &pcdev->lock,
 746                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 747                                    V4L2_FIELD_NONE,
 748                                    sizeof(struct rk29_buffer),
 749                                    icd,&icd->video_lock);
 750 }
 751 static int rk29_camera_activate(struct rk29_camera_dev *pcdev, struct soc_camera_device *icd)
 752 {
 753     unsigned long sensor_bus_flags = SOCAM_MCLK_24MHZ;
 754     struct clk *parent;
 755
 756     RK29CAMERA_DG("%s..%d.. \n",__FUNCTION__,__LINE__);
 757     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
 758                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus || !pcdev->pd_display ||
 759                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) || IS_ERR(pcdev->pd_display) ||
 760                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
 761
 762         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
 763         goto RK29_CAMERA_ACTIVE_ERR;
 764     }
 765
 766         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
 767                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
 768
 769         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
 770         goto RK29_CAMERA_ACTIVE_ERR;
 771     }
 772
 773         clk_enable(pcdev->pd_display);
 774
 775         clk_enable(pcdev->aclk_ddr_lcdc);
 776         clk_enable(pcdev->aclk_disp_matrix);
 777
 778         clk_enable(pcdev->hclk_cpu_display);
 779         clk_enable(pcdev->vip_slave);
 780
 781 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 782         clk_enable(pcdev->hclk_disp_matrix);
 783         clk_enable(pcdev->vip_matrix);
 784 #endif
 785
 786         clk_enable(pcdev->vip_input);
 787         clk_enable(pcdev->vip_bus);
 788
 789     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 790         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 791
 792     if (sensor_bus_flags & SOCAM_MCLK_48MHZ) {
 793         parent = clk_get(NULL, "clk48m");
 794         if (!parent || IS_ERR(parent))
 795              goto RK29_CAMERA_ACTIVE_ERR;
 796     } else if (sensor_bus_flags & SOCAM_MCLK_27MHZ) {
 797         parent = clk_get(NULL, "extclk");
 798         if (!parent || IS_ERR(parent))
 799              goto RK29_CAMERA_ACTIVE_ERR;
 800     } else {
 801         parent = clk_get(NULL, "xin24m");
 802         if (!parent || IS_ERR(parent))
 803              goto RK29_CAMERA_ACTIVE_ERR;
 804     }
 805
 806     clk_set_parent(pcdev->vip_out, parent);
 807
 808     clk_enable(pcdev->vip_out);
 809     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_VIP_CLKOUT);
 810     ndelay(10);
 811
 812 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 813         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)|VIP_AHBMASTER);  //VIP Config to AHB
 814         write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)&(~VIP_ACLK_DIV_HCLK_2));   //aclk:hclk = 1:1
 815 #else
 816         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)&(~VIP_AHBMASTER));  //VIP Config to AXI
 817     write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)|VIP_ACLK_DIV_HCLK_2);   //aclk:hclk = 2:1
 818 #endif
 819         ndelay(10);
 820
 821     write_vip_reg(RK29_VIP_RESET, 0x76543210);  /* ddl@rock-chips.com : vip software reset */
 822     udelay(10);
 823
 824     write_vip_reg(RK29_VIP_AHBR_CTRL, 0x07);   /* ddl@rock-chips.com : vip ahb burst 16 */
 825     write_vip_reg(RK29_VIP_INT_MASK, 0x01);    //capture complete interrupt enable
 826     write_vip_reg(RK29_VIP_CRM,  0x00000000);  //Y/CB/CR color modification
 827
 828     return 0;
 829 RK29_CAMERA_ACTIVE_ERR:
 830     return -ENODEV;
 831 }
 832
 833 static void rk29_camera_deactivate(struct rk29_camera_dev *pcdev)
 834 {
 835     //pcdev->active = NULL;
 836
 837     write_vip_reg(RK29_VIP_CTRL, 0);
 838     read_vip_reg(RK29_VIP_INT_STS);             //clear vip interrupte single
 839
 840     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_GPIO1B4);
 841     clk_disable(pcdev->vip_out);
 842
 843         clk_disable(pcdev->vip_input);
 844         clk_disable(pcdev->vip_bus);
 845
 846 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 847         clk_disable(pcdev->hclk_disp_matrix);
 848         clk_disable(pcdev->vip_matrix);
 849 #endif
 850
 851         clk_disable(pcdev->hclk_cpu_display);
 852         clk_disable(pcdev->vip_slave);
 853
 854         clk_disable(pcdev->aclk_ddr_lcdc);
 855         clk_disable(pcdev->aclk_disp_matrix);
 856
 857         clk_disable(pcdev->pd_display);
 858     return;
 859 }
 860
 861 /* The following two functions absolutely depend on the fact, that
 862  * there can be only one camera on RK28 quick capture interface */
 863 static int rk29_camera_add_device(struct soc_camera_device *icd)
 864 {
 865     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 866     struct rk29_camera_dev *pcdev = ici->priv;
 867     struct device *control = to_soc_camera_control(icd);
 868     struct v4l2_subdev *sd;
 869     int ret,i,icd_catch;
 870     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
 871
 872     mutex_lock(&camera_lock);
 873
 874     if (pcdev->icd) {
 875         ret = -EBUSY;
 876         goto ebusy;
 877     }
 878
 879     RK29CAMERA_DG("RK29 Camera driver attached to %s\n",dev_name(icd->pdev));
 880
 881         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 882     pcdev->active = NULL;
 883     pcdev->icd = NULL;
 884         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 885     pcdev->zoominfo.zoom_rate = 100;
 886
 887         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 888      * if app havn't dequeue all videobuf before close camera device;
 889         */
 890     INIT_LIST_HEAD(&pcdev->capture);
 891
 892     ret = rk29_camera_activate(pcdev,icd);
 893     if (ret)
 894         goto ebusy;
 895
 896     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 897     if (control) {
 898         sd = dev_get_drvdata(control);
 899                 v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_IOREQUEST,(void*)pcdev->pdata);
 900         #if 0
 901         ret = v4l2_subdev_call(sd,core, init, 0);
 902         if (ret)
 903             goto ebusy;
 904         #endif
 905         v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_CB_REGISTER,(void*)(&pcdev->icd_cb));
 906     }
 907
 908     pcdev->icd = icd;
 909     pcdev->icd_init = 0;
 910
 911     icd_catch = 0;
 912     for (i=0; i<2; i++) {
 913         if (pcdev->icd_frmival[i].icd == icd)
 914             icd_catch = 1;
 915         if (pcdev->icd_frmival[i].icd == NULL) {
 916             pcdev->icd_frmival[i].icd = icd;
 917             icd_catch = 1;
 918         }
 919     }
 920
 921     if (icd_catch == 0) {
 922         fival_list = pcdev->icd_frmival[0].fival_list;
 923         fival_nxt = fival_list;
 924         while(fival_nxt != NULL) {
 925             fival_nxt = fival_list->nxt;
 926             kfree(fival_list);
 927             fival_list = fival_nxt;
 928         }
 929         pcdev->icd_frmival[0].icd = icd;
 930         pcdev->icd_frmival[0].fival_list = kzalloc(sizeof(struct rk29_camera_frmivalenum),GFP_KERNEL);
 931     }
 932 ebusy:
 933     mutex_unlock(&camera_lock);
 934
 935     return ret;
 936 }
 937 static void rk29_camera_remove_device(struct soc_camera_device *icd)
 938 {
 939     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 940     struct rk29_camera_dev *pcdev = ici->priv;
 941         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 942
 943         mutex_lock(&camera_lock);
 944     BUG_ON(icd != pcdev->icd);
 945
 946     RK29CAMERA_DG("RK29 Camera driver detached from %s\n",dev_name(icd->pdev));
 947
 948         /* ddl@rock-chips.com: Application will call VIDIOC_STREAMOFF before close device, but
 949            stream may be turn on again before close device, if suspend and resume happened. */
 950         if (read_vip_reg(RK29_VIP_CTRL) & ENABLE_CAPTURE) {
 951                 rk29_camera_s_stream(icd,0);
 952         }
 953
 954     v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
 955         rk29_camera_deactivate(pcdev);
 956
 957         if (pcdev->camera_work) {
 958                 kfree(pcdev->camera_work);
 959                 pcdev->camera_work = NULL;
 960                 pcdev->camera_work_count = 0;
 961         }
 962
 963         pcdev->active = NULL;
 964     pcdev->icd = NULL;
 965     pcdev->icd_cb.sensor_cb = NULL;
 966         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 967         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 968      * if app havn't dequeue all videobuf before close camera device;
 969         */
 970     INIT_LIST_HEAD(&pcdev->capture);
 971
 972         mutex_unlock(&camera_lock);
 973         RK29CAMERA_DG("%s exit\n",__FUNCTION__);
 974
 975         return;
 976 }
 977 static int rk29_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 978 {
 979     unsigned long bus_flags, camera_flags, common_flags;
 980     unsigned int vip_ctrl_val = 0;
 981         const struct soc_mbus_pixelfmt *fmt;
 982         int ret = 0;
 983
 984     RK29CAMERA_DG("%s..%d..\n",__FUNCTION__,__LINE__);
 985
 986         fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
 987         if (!fmt)
 988                 return -EINVAL;
 989
 990     bus_flags = RK29_CAM_BUS_PARAM;
 991         /* If requested data width is supported by the platform, use it */
 992         switch (fmt->bits_per_sample) {
 993         case 10:
 994                 if (!(bus_flags & SOCAM_DATAWIDTH_10))
 995                         return -EINVAL;
 996                 break;
 997         case 9:
 998                 if (!(bus_flags & SOCAM_DATAWIDTH_9))
 999                         return -EINVAL;
1000                 break;
1001         case 8:
1002                 if (!(bus_flags & SOCAM_DATAWIDTH_8))
1003                         return -EINVAL;
1004                 break;
1005         default:
1006                 return -EINVAL;
1007         }
1008
1009         if (icd->ops->query_bus_param)
1010         camera_flags = icd->ops->query_bus_param(icd);
1011         else
1012                 camera_flags = 0;
1013
1014     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
1015     if (!common_flags) {
1016         ret = -EINVAL;
1017         goto RK29_CAMERA_SET_BUS_PARAM_END;
1018     }
1019
1020     ret = icd->ops->set_bus_param(icd, common_flags);
1021     if (ret < 0)
1022         goto RK29_CAMERA_SET_BUS_PARAM_END;
1023
1024     vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1025     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
1026         vip_ctrl_val |= NEGATIVE_EDGE;
1027     } else {
1028                 vip_ctrl_val &= ~NEGATIVE_EDGE;
1029     }
1030     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
1031         vip_ctrl_val |= HSY_LOW_ACTIVE;
1032     } else {
1033                 vip_ctrl_val &= ~HSY_LOW_ACTIVE;
1034     }
1035     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
1036         vip_ctrl_val |= VSY_HIGH_ACTIVE;
1037     } else {
1038                 vip_ctrl_val &= ~VSY_HIGH_ACTIVE;
1039     }
1040
1041     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
1042     //vip_ctrl_val |= ENABLE_CAPTURE;
1043
1044     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1045     RK29CAMERA_DG("%s..ctrl:0x%x CtrReg=%x AXI_AHB:0x%x aclk_hclk:0x%x \n",__FUNCTION__,vip_ctrl_val,read_vip_reg(RK29_VIP_CTRL),
1046                 read_grf_reg(GRF_SOC_CON0_Reg)&VIP_AHBMASTER, read_grf_reg(GRF_OS_REG0)&VIP_ACLK_DIV_HCLK_2);
1047
1048 RK29_CAMERA_SET_BUS_PARAM_END:
1049         if (ret)
1050         RK29CAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
1051     return ret;
1052 }
1053
1054 static int rk29_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
1055 {
1056     unsigned long bus_flags, camera_flags;
1057     int ret;
1058
1059     bus_flags = RK29_CAM_BUS_PARAM;
1060         if (icd->ops->query_bus_param) {
1061         camera_flags = icd->ops->query_bus_param(icd);
1062         } else {
1063                 camera_flags = 0;
1064         }
1065     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
1066
1067     if (ret < 0)
1068         dev_warn(icd->dev.parent,
1069                          "Flags incompatible: camera %lx, host %lx\n",
1070                          camera_flags, bus_flags);
1071     return ret;
1072 }
1073
1074 static const struct soc_mbus_pixelfmt rk29_camera_formats[] = {
1075    {
1076                 .fourcc                 = V4L2_PIX_FMT_NV12,
1077                 .name                   = "YUV420 NV12",
1078                 .bits_per_sample        = 8,
1079                 .packing                = SOC_MBUS_PACKING_1_5X8,
1080                 .order                  = SOC_MBUS_ORDER_LE,
1081         },{
1082                 .fourcc                 = V4L2_PIX_FMT_NV16,
1083                 .name                   = "YUV422 NV16",
1084                 .bits_per_sample        = 8,
1085                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1086                 .order                  = SOC_MBUS_ORDER_LE,
1087         },{
1088                 .fourcc                 = V4L2_PIX_FMT_YUV420,
1089                 .name                   = "NV12(v0.0.1)",
1090                 .bits_per_sample        = 8,
1091                 .packing                = SOC_MBUS_PACKING_1_5X8,
1092                 .order                  = SOC_MBUS_ORDER_LE,
1093         },{
1094                 .fourcc                 = V4L2_PIX_FMT_YUV422P,
1095                 .name                   = "NV16(v0.0.1)",
1096                 .bits_per_sample        = 8,
1097                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1098                 .order                  = SOC_MBUS_ORDER_LE,
1099         }
1100 };
1101
1102 static void rk29_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, enum v4l2_mbus_pixelcode icd_code, struct v4l2_rect *rect)
1103 {
1104         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1105     struct rk29_camera_dev *pcdev = ici->priv;
1106     unsigned int vip_fs = 0,vip_crop = 0;
1107     unsigned int vip_ctrl_val = VIP_SENSOR|ONEFRAME|DISABLE_CAPTURE;
1108
1109     switch (host_pixfmt)
1110     {
1111         case V4L2_PIX_FMT_NV16:
1112         case V4L2_PIX_FMT_YUV422P:  /* ddl@rock-chips.com: V4L2_PIX_FMT_YUV422P is V4L2_PIX_FMT_NV16 actually in 0.0.1 driver */
1113             vip_ctrl_val |= VIPREGYUV422;
1114                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
1115                         pcdev->pixfmt = host_pixfmt;
1116             break;
1117         case V4L2_PIX_FMT_NV12:
1118         case V4L2_PIX_FMT_YUV420:   /* ddl@rock-chips.com: V4L2_PIX_FMT_YUV420 is V4L2_PIX_FMT_NV12 actually in 0.0.1 driver */
1119             vip_ctrl_val |= VIPREGYUV420;
1120                         if (pcdev->frame_inval != RK29_CAM_FRAME_INVAL_INIT)
1121                                 pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
1122                         pcdev->pixfmt = host_pixfmt;
1123             break;
1124         default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
1125             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & VIPREGYUV422);
1126             break;
1127     }
1128
1129     switch (icd_code)
1130     {
1131         case V4L2_MBUS_FMT_UYVY8_2X8:
1132             vip_ctrl_val |= SENSOR_UYVY;
1133             break;
1134         case V4L2_MBUS_FMT_YUYV8_2X8:
1135             vip_ctrl_val |= SENSOR_YUYV;
1136             break;
1137         default :
1138             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & SENSOR_YUYV);
1139             break;
1140     }
1141
1142     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);         /* ddl@rock-chips.com: VIP capture mode and capture format must be set before FS register set */
1143
1144     read_vip_reg(RK29_VIP_INT_STS);                     /* clear vip interrupte single  */
1145
1146     if (vip_ctrl_val & ONEFRAME)  {
1147         vip_crop = ((rect->left<<16) + rect->top);
1148         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
1149     } else if (vip_ctrl_val & PING_PONG) {
1150         BUG();
1151     }
1152
1153     write_vip_reg(RK29_VIP_CROP, vip_crop);
1154     write_vip_reg(RK29_VIP_FS, vip_fs);
1155
1156     write_vip_reg(RK29_VIP_FB_SR,  0x00000003);
1157
1158         pcdev->host_width = rect->width;
1159         pcdev->host_height = rect->height;
1160
1161     RK29CAMERA_DG("%s.. crop:0x%x fs:0x%x ctrl:0x%x CtrlReg:0x%x\n",__FUNCTION__,vip_crop,vip_fs,vip_ctrl_val,read_vip_reg(RK29_VIP_CTRL));
1162         return;
1163 }
1164
1165 static int rk29_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
1166                                   struct soc_camera_format_xlate *xlate)
1167 {
1168     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1169     struct device *dev = icd->dev.parent;
1170     int formats = 0, ret;
1171         enum v4l2_mbus_pixelcode code;
1172         const struct soc_mbus_pixelfmt *fmt;
1173
1174         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1175         if (ret < 0)
1176                 /* No more formats */
1177                 return 0;
1178
1179         fmt = soc_mbus_get_fmtdesc(code);
1180         if (!fmt) {
1181                 dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
1182                 return 0;
1183         }
1184
1185     ret = rk29_camera_try_bus_param(icd, fmt->bits_per_sample);
1186     if (ret < 0)
1187         return 0;
1188
1189     switch (code) {
1190         case V4L2_MBUS_FMT_UYVY8_2X8:
1191         case V4L2_MBUS_FMT_YUYV8_2X8:
1192             formats++;
1193             if (xlate) {
1194                 xlate->host_fmt = &rk29_camera_formats[0];
1195                 xlate->code     = code;
1196                 xlate++;
1197                 dev_dbg(dev, "Providing format %s using code %d\n",
1198                         rk29_camera_formats[0].name,code);
1199             }
1200
1201             formats++;
1202             if (xlate) {
1203                 xlate->host_fmt = &rk29_camera_formats[1];
1204                 xlate->code     = code;
1205                 xlate++;
1206                 dev_dbg(dev, "Providing format %s using code %d\n",
1207                         rk29_camera_formats[1].name,code);
1208             }
1209
1210             formats++;
1211             if (xlate) {
1212                 xlate->host_fmt = &rk29_camera_formats[2];
1213                 xlate->code     = code;
1214                 xlate++;
1215                 dev_dbg(dev, "Providing format %s using code %d\n",
1216                         rk29_camera_formats[2].name,code);
1217             }
1218
1219             formats++;
1220             if (xlate) {
1221                 xlate->host_fmt = &rk29_camera_formats[3];
1222                 xlate->code     = code;
1223                 xlate++;
1224                 dev_dbg(dev, "Providing format %s using code %d\n",
1225                         rk29_camera_formats[3].name,code);;
1226             }
1227                         break;
1228         default:
1229             break;
1230     }
1231
1232     return formats;
1233 }
1234
1235 static void rk29_camera_put_formats(struct soc_camera_device *icd)
1236 {
1237         return;
1238 }
1239
1240 static int rk29_camera_set_crop(struct soc_camera_device *icd,
1241                                struct v4l2_crop *a)
1242 {
1243     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1244         struct v4l2_mbus_framefmt mf;
1245         u32 fourcc = icd->current_fmt->host_fmt->fourcc;
1246     int ret;
1247
1248     ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1249     if (ret < 0)
1250         return ret;
1251
1252     if ((mf.width < (a->c.left + a->c.width)) || (mf.height < (a->c.top + a->c.height)))  {
1253
1254         mf.width = a->c.left + a->c.width;
1255         mf.height = a->c.top + a->c.height;
1256
1257         v4l_bound_align_image(&mf.width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1258             &mf.height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1259             fourcc == V4L2_PIX_FMT_NV16 ?4 : 0);
1260
1261         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1262         if (ret < 0)
1263             return ret;
1264     }
1265
1266     rk29_camera_setup_format(icd, fourcc, mf.code, &a->c);
1267
1268     icd->user_width = mf.width;
1269     icd->user_height = mf.height;
1270
1271     return 0;
1272 }
1273
1274 static int rk29_camera_set_fmt(struct soc_camera_device *icd,
1275                               struct v4l2_format *f)
1276 {
1277     struct device *dev = icd->dev.parent;
1278     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1279     const struct soc_camera_format_xlate *xlate = NULL;
1280         struct soc_camera_host *ici =to_soc_camera_host(icd->dev.parent);
1281     struct rk29_camera_dev *pcdev = ici->priv;
1282     struct v4l2_pix_format *pix = &f->fmt.pix;
1283     struct v4l2_mbus_framefmt mf;
1284     struct v4l2_rect rect;
1285     int ret,usr_w,usr_h,icd_width,icd_height;
1286     int stream_on = 0;
1287
1288         usr_w = pix->width;
1289         usr_h = pix->height;
1290     RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1291
1292     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1293     if (!xlate) {
1294         dev_err(dev, "Format %x not found\n", pix->pixelformat);
1295         ret = -EINVAL;
1296         goto RK29_CAMERA_SET_FMT_END;
1297     }
1298
1299     /* ddl@rock-chips.com: sensor init code transmit in here after open */
1300     if (pcdev->icd_init == 0) {
1301         v4l2_subdev_call(sd,core, init, 0);
1302         pcdev->icd_init = 1;
1303     }
1304
1305     stream_on = read_vip_reg(RK29_VIP_CTRL);
1306     if (stream_on & ENABLE_CAPTURE)
1307         write_vip_reg(RK29_VIP_CTRL, (stream_on & (~ENABLE_CAPTURE)));
1308
1309         mf.width        = pix->width;
1310         mf.height       = pix->height;
1311         mf.field        = pix->field;
1312         mf.colorspace   = pix->colorspace;
1313         mf.code         = xlate->code;
1314
1315         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1316
1317         if (mf.code != xlate->code)
1318                 return -EINVAL;
1319
1320     icd_width = mf.width;
1321     icd_height = mf.height;
1322         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1323         if ((mf.width != usr_w) || (mf.height != usr_h)) {
1324         if (unlikely((mf.width <16) || (mf.width > 8190) || (mf.height < 16) || (mf.height > 8190))) {
1325                 RK29CAMERA_TR("Senor and IPP both invalid source resolution(%dx%d)\n",mf.width,mf.height);
1326                 ret = -EINVAL;
1327                 goto RK29_CAMERA_SET_FMT_END;
1328         }
1329         if (unlikely((usr_w <16)||(usr_h < 16))) {
1330                 RK29CAMERA_TR("Senor and IPP both invalid destination resolution(%dx%d)\n",usr_w,usr_h);
1331                 ret = -EINVAL;
1332             goto RK29_CAMERA_SET_FMT_END;
1333         }
1334         }
1335         #endif
1336     icd->sense = NULL;
1337
1338     if (!ret) {
1339
1340         if (mf.width*usr_h == mf.height*usr_w) {
1341             rect.width = mf.width;
1342             rect.height = mf.height;
1343         } else {
1344             int ratio;
1345             if (usr_w > usr_h) {
1346                 if (mf.width > usr_w) {
1347                     ratio = MIN((mf.width*10/usr_w),(mf.height*10/usr_h))-1;
1348                     rect.width = usr_w*ratio/10;
1349                     rect.height = usr_h*ratio/10;
1350                 } else {
1351                     ratio = MAX((usr_w*10/mf.width),(usr_h*10/mf.height))+1;
1352                     rect.width = usr_w*10/ratio;
1353                     rect.height = usr_h*10/ratio;
1354                 }
1355             } else {
1356                 if (mf.height > usr_h) {
1357                     ratio = MIN((mf.width*10/usr_w),(mf.height*10/usr_h))-1;
1358                     rect.width = usr_w*ratio/10;
1359                     rect.height = usr_h*ratio/10;
1360                 } else {
1361                     ratio = MAX((usr_w*10/mf.width),(usr_h*10/mf.height))+1;
1362                     rect.width = usr_w*10/ratio;
1363                     rect.height = usr_h*10/ratio;
1364                 }
1365             }
1366         }
1367
1368         rect.left = (mf.width - rect.width)/2;
1369         rect.top = (mf.height - rect.height)/2;
1370
1371         down(&pcdev->zoominfo.sem);
1372         pcdev->zoominfo.a.c.width = rect.width*100/pcdev->zoominfo.zoom_rate;
1373                 pcdev->zoominfo.a.c.width &= ~0x03;
1374                 pcdev->zoominfo.a.c.height = rect.height*100/pcdev->zoominfo.zoom_rate;
1375                 pcdev->zoominfo.a.c.height &= ~0x03;
1376                 pcdev->zoominfo.a.c.left = ((rect.width - pcdev->zoominfo.a.c.width)/2 + rect.left)&(~0x01);
1377                 pcdev->zoominfo.a.c.top = ((rect.height - pcdev->zoominfo.a.c.height)/2 + rect.top)&(~0x01);
1378         up(&pcdev->zoominfo.sem);
1379
1380         /* ddl@rock-chips.com: IPP work limit check */
1381         if ((pcdev->zoominfo.a.c.width != usr_w) || (pcdev->zoominfo.a.c.height != usr_h)) {
1382             if (usr_w > 0x7f0) {
1383                 if (((usr_w>>1)&0x3f) && (((usr_w>>1)&0x3f) <= 8)) {
1384                     RK29CAMERA_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));
1385                     ret = -EINVAL;
1386                     goto RK29_CAMERA_SET_FMT_END;
1387                 }
1388             } else {
1389                 if ((usr_w&0x3f) && ((usr_w&0x3f) <= 8)) {
1390                     RK29CAMERA_TR("IPP Destination resolution(%dx%d, %d mod 64=%d is <= 8)",usr_w,usr_h, usr_w, (int)(usr_w&0x3f));
1391                     ret = -EINVAL;
1392                     goto RK29_CAMERA_SET_FMT_END;
1393                 }
1394             }
1395         }
1396
1397         /* ddl@rock-chips.com: Crop is doing by IPP, not by VIP in rk2918 */
1398         rect.left = 0;
1399         rect.top = 0;
1400         rect.width = mf.width;
1401         rect.height = mf.height;
1402
1403         RK29CAMERA_DG("%s..%s Sensor output:%dx%d  VIP output:%dx%d (zoom: %dx%d@(%d,%d)->%dx%d)\n",__FUNCTION__,xlate->host_fmt->name,
1404                                    mf.width, mf.height,rect.width,rect.height, pcdev->zoominfo.a.c.width,pcdev->zoominfo.a.c.height, pcdev->zoominfo.a.c.left,pcdev->zoominfo.a.c.top,
1405                                    pix->width, pix->height);
1406
1407
1408         rk29_camera_setup_format(icd, pix->pixelformat, mf.code, &rect);
1409
1410                 if (CAM_IPPWORK_IS_EN()) {
1411                         BUG_ON(pcdev->vipmem_phybase == 0);
1412                 }
1413         pcdev->icd_width = icd_width;
1414         pcdev->icd_height = icd_height;
1415
1416         pix->width = usr_w;
1417         pix->height = usr_h;
1418         pix->field = mf.field;
1419         pix->colorspace = mf.colorspace;
1420         icd->current_fmt = xlate;
1421     }
1422
1423 RK29_CAMERA_SET_FMT_END:
1424     if (stream_on & ENABLE_CAPTURE)
1425         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL) | ENABLE_CAPTURE));
1426         if (ret)
1427         RK29CAMERA_TR("\n%s: Driver isn't support %dx%d resolution which user request!\n",__FUNCTION__,usr_w,usr_h);
1428     return ret;
1429 }
1430 static bool rk29_camera_fmt_capturechk(struct v4l2_format *f)
1431 {
1432     bool ret = false;
1433
1434         if ((f->fmt.pix.width == 1024) && (f->fmt.pix.height == 768)) {
1435                 ret = true;
1436         } else if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 1024)) {
1437                 ret = true;
1438         } else if ((f->fmt.pix.width == 1600) && (f->fmt.pix.height == 1200)) {
1439                 ret = true;
1440         } else if ((f->fmt.pix.width == 2048) && (f->fmt.pix.height == 1536)) {
1441                 ret = true;
1442         } else if ((f->fmt.pix.width == 2592) && (f->fmt.pix.height == 1944)) {
1443                 ret = true;
1444         }
1445
1446         if (ret == true)
1447                 RK29CAMERA_DG("%s %dx%d is capture format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1448         return ret;
1449 }
1450 static int rk29_camera_try_fmt(struct soc_camera_device *icd,
1451                                    struct v4l2_format *f)
1452 {
1453     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1454         struct rk29_camera_dev *pcdev = ici->priv;
1455     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1456     const struct soc_camera_format_xlate *xlate;
1457     struct v4l2_pix_format *pix = &f->fmt.pix;
1458     __u32 pixfmt = pix->pixelformat;
1459     int ret,usr_w,usr_h,i;
1460         bool is_capture = rk29_camera_fmt_capturechk(f);
1461         bool vipmem_is_overflow = false;
1462     struct v4l2_mbus_framefmt mf;
1463     int bytes_per_line_host;
1464
1465         usr_w = pix->width;
1466         usr_h = pix->height;
1467         RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1468
1469     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1470     if (!xlate) {
1471         dev_err(icd->dev.parent, "Format (%c%c%c%c) not found\n", pixfmt & 0xFF, (pixfmt >> 8) & 0xFF,
1472                         (pixfmt >> 16) & 0xFF, (pixfmt >> 24) & 0xFF);
1473         ret = -EINVAL;
1474         RK29CAMERA_TR("%s(version:%c%c%c) support format:\n",rk29_cam_driver_description,(RK29_CAM_VERSION_CODE&0xff0000)>>16,
1475             (RK29_CAM_VERSION_CODE&0xff00)>>8,(RK29_CAM_VERSION_CODE&0xff));
1476         for (i = 0; i < icd->num_user_formats; i++)
1477                     RK29CAMERA_TR("(%c%c%c%c)-%s\n",
1478                     icd->user_formats[i].host_fmt->fourcc & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 8) & 0xFF,
1479                         (icd->user_formats[i].host_fmt->fourcc >> 16) & 0xFF, (icd->user_formats[i].host_fmt->fourcc >> 24) & 0xFF,
1480                         icd->user_formats[i].host_fmt->name);
1481         goto RK29_CAMERA_TRY_FMT_END;
1482     }
1483    /* limit to rk29 hardware capabilities */
1484     v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1485               &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1486               pixfmt == V4L2_PIX_FMT_NV16 ? 4 : 0);
1487
1488     pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1489                                                     xlate->host_fmt);
1490         if (pix->bytesperline < 0)
1491                 return pix->bytesperline;
1492
1493     /* limit to sensor capabilities */
1494         mf.width        = pix->width;
1495         mf.height       = pix->height;
1496         mf.field        = pix->field;
1497         mf.colorspace   = pix->colorspace;
1498         mf.code         = xlate->code;
1499
1500         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1501         if (ret < 0)
1502                 goto RK29_CAMERA_TRY_FMT_END;
1503     RK29CAMERA_DG("%s mf.width:%d  mf.height:%d\n",__FUNCTION__,mf.width,mf.height);
1504         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1505         if ((mf.width != usr_w) || (mf.height != usr_h)) {
1506         bytes_per_line_host = soc_mbus_bytes_per_line(mf.width,icd->current_fmt->host_fmt);
1507                 if (is_capture) {
1508                         vipmem_is_overflow = (PAGE_ALIGN(bytes_per_line_host*mf.height) > pcdev->vipmem_size);
1509                 } else {
1510                         /* Assume preview buffer minimum is 4 */
1511                         vipmem_is_overflow = (PAGE_ALIGN(bytes_per_line_host*mf.height)*4 > pcdev->vipmem_size);
1512                 }
1513                 if (vipmem_is_overflow == false) {
1514                         pix->width = usr_w;
1515                         pix->height = usr_h;
1516                 } else {
1517                         RK29CAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",mf.width,mf.height,usr_w,usr_h);
1518             pix->width = mf.width;
1519             pix->height = mf.height;
1520                 }
1521
1522         if ((mf.width < usr_w) || (mf.height < usr_h)) {
1523             if (((usr_w>>1) > mf.width) || ((usr_h>>1) > mf.height)) {
1524                 RK29CAMERA_TR("The aspect ratio(%dx%d/%dx%d) is bigger than 2 !\n",mf.width,mf.height,usr_w,usr_h);
1525                 pix->width = mf.width;
1526                 pix->height = mf.height;
1527             }
1528         }
1529         }
1530         #else
1531     pix->width  = mf.width;
1532         pix->height     = mf.height;
1533     #endif
1534     pix->colorspace     = mf.colorspace;
1535
1536     switch (mf.field) {
1537         case V4L2_FIELD_ANY:
1538         case V4L2_FIELD_NONE:
1539                 pix->field      = V4L2_FIELD_NONE;
1540                 break;
1541         default:
1542                 /* TODO: support interlaced at least in pass-through mode */
1543                 dev_err(icd->dev.parent, "Field type %d unsupported.\n",
1544                         mf.field);
1545                 goto RK29_CAMERA_TRY_FMT_END;
1546         }
1547
1548 RK29_CAMERA_TRY_FMT_END:
1549         if (ret)
1550         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1551     return ret;
1552 }
1553
1554 static int rk29_camera_reqbufs(struct soc_camera_device *icd,
1555                                struct v4l2_requestbuffers *p)
1556 {
1557     int i;
1558
1559     /* This is for locking debugging only. I removed spinlocks and now I
1560      * check whether .prepare is ever called on a linked buffer, or whether
1561      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1562      * it hadn't triggered */
1563     for (i = 0; i < p->count; i++) {
1564         struct rk29_buffer *buf = container_of(icd->vb_vidq.bufs[i],
1565                                                            struct rk29_buffer, vb);
1566         buf->inwork = 0;
1567         INIT_LIST_HEAD(&buf->vb.queue);
1568     }
1569
1570     return 0;
1571 }
1572
1573 static unsigned int rk29_camera_poll(struct file *file, poll_table *pt)
1574 {
1575     struct soc_camera_device *icd = file->private_data;
1576     struct rk29_buffer *buf;
1577
1578     buf = list_entry(icd->vb_vidq.stream.next, struct rk29_buffer,
1579                      vb.stream);
1580
1581     poll_wait(file, &buf->vb.done, pt);
1582
1583     if (buf->vb.state == VIDEOBUF_DONE ||
1584             buf->vb.state == VIDEOBUF_ERROR)
1585         return POLLIN|POLLRDNORM;
1586
1587     return 0;
1588 }
1589
1590 static int rk29_camera_querycap(struct soc_camera_host *ici,
1591                                 struct v4l2_capability *cap)
1592 {
1593     struct rk29_camera_dev *pcdev = ici->priv;
1594     char orientation[5];
1595     int i;
1596
1597     strlcpy(cap->card, dev_name(pcdev->icd->pdev), sizeof(cap->card));
1598
1599     memset(orientation,0x00,sizeof(orientation));
1600     for (i=0; i<RK29_CAM_SUPPORT_NUMS;i++) {
1601         if ((pcdev->pdata->info[i].dev_name!=NULL) && (strcmp(dev_name(pcdev->icd->pdev), pcdev->pdata->info[i].dev_name) == 0)) {
1602             sprintf(orientation,"-%d",pcdev->pdata->info[i].orientation);
1603         }
1604     }
1605
1606     if (orientation[0] != '-') {
1607         RK29CAMERA_TR("%s: %s is not registered in rk29_camera_platform_data, orientation apply default value",__FUNCTION__,dev_name(pcdev->icd->pdev));
1608         if (strstr(dev_name(pcdev->icd->pdev),"front"))
1609             strcat(cap->card,"-270");
1610         else
1611             strcat(cap->card,"-90");
1612     } else {
1613         strcat(cap->card,orientation);
1614     }
1615
1616     cap->version = RK29_CAM_VERSION_CODE;
1617     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1618
1619     return 0;
1620 }
1621
1622 static int rk29_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1623 {
1624     struct soc_camera_host *ici =
1625                     to_soc_camera_host(icd->dev.parent);
1626     struct rk29_camera_dev *pcdev = ici->priv;
1627         struct v4l2_subdev *sd;
1628     int ret = 0,tmp;
1629
1630         mutex_lock(&camera_lock);
1631         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1632                 rk29_camera_s_stream(icd, 0);
1633                 sd = soc_camera_to_subdev(icd);
1634                 v4l2_subdev_call(sd, video, s_stream, 0);
1635                 ret = icd->ops->suspend(icd, state);
1636
1637                 pcdev->reginfo_suspend.VipCtrl = read_vip_reg(RK29_VIP_CTRL);
1638                 pcdev->reginfo_suspend.VipCrop = read_vip_reg(RK29_VIP_CROP);
1639                 pcdev->reginfo_suspend.VipFs = read_vip_reg(RK29_VIP_FS);
1640                 pcdev->reginfo_suspend.VipIntMsk = read_vip_reg(RK29_VIP_INT_MASK);
1641                 pcdev->reginfo_suspend.VipCrm = read_vip_reg(RK29_VIP_CRM);
1642
1643                 tmp = pcdev->reginfo_suspend.VipFs>>16;         /* ddl@rock-chips.com */
1644                 tmp += pcdev->reginfo_suspend.VipCrop>>16;
1645                 pcdev->reginfo_suspend.VipFs = (pcdev->reginfo_suspend.VipFs & 0xffff) | (tmp<<16);
1646
1647                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1648                 rk29_camera_deactivate(pcdev);
1649
1650                 RK29CAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1651         } else {
1652                 RK29CAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1653         }
1654         mutex_unlock(&camera_lock);
1655     return ret;
1656 }
1657
1658 static int rk29_camera_resume(struct soc_camera_device *icd)
1659 {
1660     struct soc_camera_host *ici =
1661                     to_soc_camera_host(icd->dev.parent);
1662     struct rk29_camera_dev *pcdev = ici->priv;
1663         struct v4l2_subdev *sd;
1664     int ret = 0;
1665
1666         mutex_lock(&camera_lock);
1667         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1668                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1669                         rk29_camera_activate(pcdev, icd);
1670                         write_vip_reg(RK29_VIP_INT_MASK, pcdev->reginfo_suspend.VipIntMsk);
1671                         write_vip_reg(RK29_VIP_CRM, pcdev->reginfo_suspend.VipCrm);
1672                         write_vip_reg(RK29_VIP_CTRL, pcdev->reginfo_suspend.VipCtrl&~ENABLE_CAPTURE);
1673                         write_vip_reg(RK29_VIP_CROP, pcdev->reginfo_suspend.VipCrop);
1674                         write_vip_reg(RK29_VIP_FS, pcdev->reginfo_suspend.VipFs);
1675
1676                         rk29_videobuf_capture(pcdev->active);
1677                         rk29_camera_s_stream(icd, 1);
1678                         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
1679                 } else {
1680                         RK29CAMERA_TR("Resume fail, vip register recored is invalidate!!\n");
1681                         goto rk29_camera_resume_end;
1682                 }
1683
1684                 ret = icd->ops->resume(icd);
1685                 sd = soc_camera_to_subdev(icd);
1686                 v4l2_subdev_call(sd, video, s_stream, 1);
1687
1688                 RK29CAMERA_DG("%s Enter success\n",__FUNCTION__);
1689         } else {
1690                 RK29CAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1691         }
1692
1693 rk29_camera_resume_end:
1694         mutex_unlock(&camera_lock);
1695     return ret;
1696 }
1697
1698 static void rk29_camera_reinit_work(struct work_struct *work)
1699 {
1700         struct device *control;
1701     struct v4l2_subdev *sd;
1702         struct v4l2_mbus_framefmt mf;
1703         const struct soc_camera_format_xlate *xlate;
1704         int ret;
1705
1706         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)&(~ENABLE_CAPTURE)));
1707
1708         control = to_soc_camera_control(rk29_camdev_info_ptr->icd);
1709         sd = dev_get_drvdata(control);
1710     v4l2_subdev_call(sd, video, s_stream, 0);  /* ddl@rock-chips.com: Avoid sensor autofocus thread is running */
1711         ret = v4l2_subdev_call(sd,core, init, 1);
1712
1713         mf.width = rk29_camdev_info_ptr->icd->user_width;
1714         mf.height = rk29_camdev_info_ptr->icd->user_height;
1715         xlate = soc_camera_xlate_by_fourcc(rk29_camdev_info_ptr->icd, rk29_camdev_info_ptr->icd->current_fmt->host_fmt->fourcc);
1716         mf.code = xlate->code;
1717
1718         ret |= v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1719     v4l2_subdev_call(sd, video, s_stream, 1);
1720         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)|ENABLE_CAPTURE));
1721
1722         RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor now! ret:0x%x\n",ret);
1723 }
1724 static enum hrtimer_restart rk29_camera_fps_func(struct hrtimer *timer)
1725 {
1726     struct rk29_camera_frmivalenum *fival_nxt=NULL,*fival_pre=NULL, *fival_rec=NULL;
1727     int rec_flag,i;
1728
1729         RK29CAMERA_DG("rk29_camera_fps_func fps:0x%x\n",rk29_camdev_info_ptr->fps);
1730         if (rk29_camdev_info_ptr->fps < 2) {
1731                 RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor delay!\n");
1732                 INIT_WORK(&rk29_camdev_info_ptr->camera_reinit_work, rk29_camera_reinit_work);
1733                 queue_work(rk29_camdev_info_ptr->camera_wq,&(rk29_camdev_info_ptr->camera_reinit_work));
1734         } else {
1735             for (i=0; i<2; i++) {
1736             if (rk29_camdev_info_ptr->icd == rk29_camdev_info_ptr->icd_frmival[i].icd) {
1737                 fival_nxt = rk29_camdev_info_ptr->icd_frmival[i].fival_list;
1738             }
1739         }
1740
1741         rec_flag = 0;
1742         fival_pre = fival_nxt;
1743         while (fival_nxt != NULL) {
1744
1745             RK29CAMERA_DG("%s %c%c%c%c %dx%d framerate : %d/%d\n", dev_name(&rk29_camdev_info_ptr->icd->dev),
1746                 fival_nxt->fival.pixel_format & 0xFF, (fival_nxt->fival.pixel_format >> 8) & 0xFF,
1747                             (fival_nxt->fival.pixel_format >> 16) & 0xFF, (fival_nxt->fival.pixel_format >> 24),
1748                             fival_nxt->fival.width, fival_nxt->fival.height, fival_nxt->fival.discrete.denominator,
1749                             fival_nxt->fival.discrete.numerator);
1750
1751             if (((fival_nxt->fival.pixel_format == rk29_camdev_info_ptr->pixfmt)
1752                 && (fival_nxt->fival.height == rk29_camdev_info_ptr->icd->user_height)
1753                 && (fival_nxt->fival.width == rk29_camdev_info_ptr->icd->user_width))
1754                 || (fival_nxt->fival.discrete.denominator == 0)) {
1755
1756                 fival_nxt->fival.index = 0;
1757                 fival_nxt->fival.width = rk29_camdev_info_ptr->icd->user_width;
1758                 fival_nxt->fival.height= rk29_camdev_info_ptr->icd->user_height;
1759                 fival_nxt->fival.pixel_format = rk29_camdev_info_ptr->pixfmt;
1760                 fival_nxt->fival.discrete.denominator = rk29_camdev_info_ptr->frame_interval;
1761                 fival_nxt->fival.reserved[1] = (rk29_camdev_info_ptr->icd_width<<16)
1762                                                     |(rk29_camdev_info_ptr->icd_height);
1763                 fival_nxt->fival.discrete.numerator = 1000000;
1764                 fival_nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1765
1766                 rec_flag = 1;
1767                 fival_rec = fival_nxt;
1768             }
1769             fival_pre = fival_nxt;
1770             fival_nxt = fival_nxt->nxt;
1771         }
1772
1773         if ((rec_flag == 0) && fival_pre) {
1774             fival_pre->nxt = kzalloc(sizeof(struct rk29_camera_frmivalenum), GFP_ATOMIC);
1775             if (fival_pre->nxt != NULL) {
1776                 fival_pre->nxt->fival.index = fival_pre->fival.index++;
1777                 fival_pre->nxt->fival.width = rk29_camdev_info_ptr->icd->user_width;
1778                 fival_pre->nxt->fival.height= rk29_camdev_info_ptr->icd->user_height;
1779                 fival_pre->nxt->fival.pixel_format = rk29_camdev_info_ptr->pixfmt;
1780
1781                 fival_pre->nxt->fival.discrete.denominator = rk29_camdev_info_ptr->frame_interval;
1782
1783                 fival_pre->nxt->fival.reserved[1] = (rk29_camdev_info_ptr->icd_width<<16)
1784                                                     |(rk29_camdev_info_ptr->icd_height);
1785
1786                 fival_pre->nxt->fival.discrete.numerator = 1000000;
1787                 fival_pre->nxt->fival.type = V4L2_FRMIVAL_TYPE_DISCRETE;
1788                 rec_flag = 1;
1789                 fival_rec = fival_pre->nxt;
1790             }
1791         }
1792         }
1793
1794         return HRTIMER_NORESTART;
1795 }
1796 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable)
1797 {
1798         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1799     struct rk29_camera_dev *pcdev = ici->priv;
1800     int vip_ctrl_val;
1801         int ret;
1802
1803         WARN_ON(pcdev->icd != icd);
1804     pcdev->frame_interval = 0;
1805         vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1806         if (enable) {
1807                 pcdev->fps = 0;
1808                 hrtimer_cancel(&pcdev->fps_timer);
1809                 hrtimer_start(&pcdev->fps_timer,ktime_set(1, 0),HRTIMER_MODE_REL);
1810                 vip_ctrl_val |= ENABLE_CAPTURE;
1811         } else {
1812         vip_ctrl_val &= ~ENABLE_CAPTURE;
1813                 ret = hrtimer_cancel(&pcdev->fps_timer);
1814                 ret |= flush_work(&rk29_camdev_info_ptr->camera_reinit_work);
1815                 RK29CAMERA_DG("STREAM_OFF cancel timer and flush work:0x%x \n", ret);
1816         }
1817         write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1818
1819         RK29CAMERA_DG("%s.. enable : 0x%x \n", __FUNCTION__, enable);
1820         return 0;
1821 }
1822 int rk29_camera_enum_frameintervals(struct soc_camera_device *icd, struct v4l2_frmivalenum *fival)
1823 {
1824     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1825     struct rk29_camera_dev *pcdev = ici->priv;
1826     struct rk29_camera_frmivalenum *fival_list = NULL;
1827     struct v4l2_frmivalenum *fival_head=NULL;
1828     int i,ret = 0,index;
1829
1830     index = fival->index & 0x00ffffff;
1831     if ((fival->index & 0xff000000) == 0xff000000) {   /* ddl@rock-chips.com: detect framerate */
1832         for (i=0; i<2; i++) {
1833             if (pcdev->icd_frmival[i].icd == icd) {
1834                 fival_list = pcdev->icd_frmival[i].fival_list;
1835             }
1836         }
1837
1838         if (fival_list != NULL) {
1839             i = 0;
1840             while (fival_list != NULL) {
1841                 if ((fival->pixel_format == fival_list->fival.pixel_format)
1842                     && (fival->height == fival_list->fival.height)
1843                     && (fival->width == fival_list->fival.width)) {
1844                     if (i == index)
1845                         break;
1846                     i++;
1847                 }
1848                 fival_list = fival_list->nxt;
1849             }
1850
1851             if ((i==index) && (fival_list != NULL)) {
1852                 memcpy(fival, &fival_list->fival, sizeof(struct v4l2_frmivalenum));
1853             } else {
1854                 ret = -EINVAL;
1855             }
1856         } else {
1857             RK29CAMERA_TR("%s: fival_list is NULL\n",__FUNCTION__);
1858             ret = -EINVAL;
1859         }
1860     } else {
1861
1862         for (i=0; i<RK29_CAM_SUPPORT_NUMS; i++) {
1863             if (pcdev->pdata->info[i].dev_name && (strcmp(dev_name(pcdev->icd->pdev),pcdev->pdata->info[i].dev_name) == 0)) {
1864                 fival_head = pcdev->pdata->info[i].fival;
1865             }
1866         }
1867
1868         if (fival_head == NULL) {
1869             RK29CAMERA_TR("%s: %s is not registered in rk29_camera_platform_data!!",__FUNCTION__,dev_name(pcdev->icd->pdev));
1870             ret = -EINVAL;
1871             goto rk29_camera_enum_frameintervals_end;
1872         }
1873
1874         i = 0;
1875         while (fival_head->width && fival_head->height) {
1876             if ((fival->pixel_format == fival_head->pixel_format)
1877                 && (fival->height == fival_head->height)
1878                 && (fival->width == fival_head->width)) {
1879                 if (i == index) {
1880                     break;
1881                 }
1882                 i++;
1883             }
1884             fival_head++;
1885         }
1886
1887         if ((i == index) && (fival->height == fival_head->height) && (fival->width == fival_head->width)) {
1888             memcpy(fival, fival_head, sizeof(struct v4l2_frmivalenum));
1889             RK29CAMERA_DG("%s %dx%d@%c%c%c%c framerate : %d/%d\n", dev_name(rk29_camdev_info_ptr->icd->pdev),
1890                 fival->width, fival->height,
1891                 fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1892                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1893                              fival->discrete.denominator,fival->discrete.numerator);
1894         } else {
1895             if (index == 0)
1896                 RK29CAMERA_TR("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(rk29_camdev_info_ptr->icd->pdev),
1897                     fival->width,fival->height,
1898                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1899                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1900                             index);
1901             else
1902                 RK29CAMERA_DG("%s have not catch %d%d@%c%c%c%c index(%d) framerate\n",dev_name(rk29_camdev_info_ptr->icd->pdev),
1903                     fival->width,fival->height,
1904                     fival->pixel_format & 0xFF, (fival->pixel_format >> 8) & 0xFF,
1905                             (fival->pixel_format >> 16) & 0xFF, (fival->pixel_format >> 24),
1906                             index);
1907             ret = -EINVAL;
1908         }
1909     }
1910 rk29_camera_enum_frameintervals_end:
1911     return ret;
1912 }
1913
1914 #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
1915 static int rk29_camera_set_digit_zoom(struct soc_camera_device *icd,
1916                                                                 const struct v4l2_queryctrl *qctrl, int zoom_rate)
1917 {
1918         struct v4l2_crop a;
1919         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1920         struct rk29_camera_dev *pcdev = ici->priv;
1921
1922 /* ddl@rock-chips.com : The largest resolution is 2047x1088, so larger resolution must be operated some times
1923    (Assume operate times is 4),but resolution which ipp can operate ,it is width and height must be even. */
1924         a.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1925         a.c.width = pcdev->host_width*100/zoom_rate;
1926         a.c.width &= ~0x03;
1927         a.c.height = pcdev->host_height*100/zoom_rate;
1928         a.c.height &= ~0x03;
1929
1930         a.c.left = ((pcdev->host_width - a.c.width)>>1)&(~0x01);
1931         a.c.top = ((pcdev->host_height - a.c.height)>>1)&(~0x01);
1932
1933     down(&pcdev->zoominfo.sem);
1934         pcdev->zoominfo.a.c.height = a.c.height;
1935         pcdev->zoominfo.a.c.width = a.c.width;
1936         pcdev->zoominfo.a.c.top = a.c.top;
1937         pcdev->zoominfo.a.c.left = a.c.left;
1938     up(&pcdev->zoominfo.sem);
1939
1940         RK29CAMERA_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, pcdev->host_width, pcdev->host_height );
1941
1942         return 0;
1943 }
1944 #endif
1945 static inline struct v4l2_queryctrl const *rk29_camera_soc_camera_find_qctrl(
1946         struct soc_camera_host_ops *ops, int id)
1947 {
1948         int i;
1949
1950         for (i = 0; i < ops->num_controls; i++)
1951                 if (ops->controls[i].id == id)
1952                         return &ops->controls[i];
1953
1954         return NULL;
1955 }
1956
1957
1958 static int rk29_camera_set_ctrl(struct soc_camera_device *icd,
1959                                                                 struct v4l2_control *sctrl)
1960 {
1961
1962         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1963         const struct v4l2_queryctrl *qctrl;
1964     struct rk29_camera_dev *pcdev = ici->priv;
1965     int ret = 0;
1966
1967         qctrl = rk29_camera_soc_camera_find_qctrl(ici->ops, sctrl->id);
1968         if (!qctrl) {
1969                 ret = -ENOIOCTLCMD;
1970         goto rk29_camera_set_ctrl_end;
1971         }
1972
1973         switch (sctrl->id)
1974         {
1975         #ifdef CONFIG_VIDEO_RK29_DIGITALZOOM_IPP_ON
1976                 case V4L2_CID_ZOOM_ABSOLUTE:
1977                 {
1978                         if ((sctrl->value < qctrl->minimum) || (sctrl->value > qctrl->maximum)){
1979                         ret = -EINVAL;
1980                 goto rk29_camera_set_ctrl_end;
1981                 }
1982             ret = rk29_camera_set_digit_zoom(icd, qctrl, sctrl->value);
1983                         if (ret == 0) {
1984                                 pcdev->zoominfo.zoom_rate = sctrl->value;
1985             } else {
1986                 goto rk29_camera_set_ctrl_end;
1987             }
1988                         break;
1989                 }
1990     #endif
1991                 default:
1992                         ret = -ENOIOCTLCMD;
1993                         break;
1994         }
1995 rk29_camera_set_ctrl_end:
1996         return ret;
1997 }
1998
1999 static struct soc_camera_host_ops rk29_soc_camera_host_ops =
2000 {
2001     .owner              = THIS_MODULE,
2002     .add                = rk29_camera_add_device,
2003     .remove             = rk29_camera_remove_device,
2004     .suspend    = rk29_camera_suspend,
2005     .resume             = rk29_camera_resume,
2006     .enum_frameinervals = rk29_camera_enum_frameintervals,
2007     .set_crop   = rk29_camera_set_crop,
2008     .get_formats        = rk29_camera_get_formats,
2009     .put_formats        = rk29_camera_put_formats,
2010     .set_fmt    = rk29_camera_set_fmt,
2011     .try_fmt    = rk29_camera_try_fmt,
2012     .init_videobuf      = rk29_camera_init_videobuf,
2013     .reqbufs    = rk29_camera_reqbufs,
2014     .poll               = rk29_camera_poll,
2015     .querycap   = rk29_camera_querycap,
2016     .set_bus_param      = rk29_camera_set_bus_param,
2017     .s_stream = rk29_camera_s_stream,   /* ddl@rock-chips.com : Add stream control for host */
2018     .set_ctrl = rk29_camera_set_ctrl,
2019     .controls = rk29_camera_controls,
2020     .num_controls = ARRAY_SIZE(rk29_camera_controls)
2021
2022 };
2023 static int rk29_camera_probe(struct platform_device *pdev)
2024 {
2025     struct rk29_camera_dev *pcdev;
2026     struct resource *res;
2027     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
2028     int irq,i;
2029     int err = 0;
2030
2031     RK29CAMERA_TR("RK29 Camera driver version: v%d.%d.%d\n",(RK29_CAM_VERSION_CODE&0xff0000)>>16,
2032         (RK29_CAM_VERSION_CODE&0xff00)>>8,RK29_CAM_VERSION_CODE&0xff);
2033
2034     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
2035     irq = platform_get_irq(pdev, 0);
2036     if (!res || irq < 0) {
2037         err = -ENODEV;
2038         goto exit;
2039     }
2040
2041     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
2042     if (!pcdev) {
2043         dev_err(&pdev->dev, "Could not allocate pcdev\n");
2044         err = -ENOMEM;
2045         goto exit_alloc;
2046     }
2047     rk29_camdev_info_ptr = pcdev;
2048
2049     /*config output clk*/
2050         pcdev->aclk_ddr_lcdc = clk_get(&pdev->dev, "aclk_ddr_lcdc");
2051         pcdev->aclk_disp_matrix = clk_get(&pdev->dev, "aclk_disp_matrix");
2052
2053         pcdev->hclk_cpu_display = clk_get(&pdev->dev, "hclk_cpu_display");
2054         pcdev->vip_slave = clk_get(&pdev->dev, "vip_slave");
2055         pcdev->vip_out = clk_get(&pdev->dev,"vip_out");
2056         pcdev->vip_input = clk_get(&pdev->dev,"vip_input");
2057         pcdev->vip_bus = clk_get(&pdev->dev, "vip_bus");
2058
2059         pcdev->hclk_disp_matrix = clk_get(&pdev->dev,"hclk_disp_matrix");
2060         pcdev->vip_matrix = clk_get(&pdev->dev,"vip_matrix");
2061
2062         pcdev->pd_display = clk_get(&pdev->dev,"pd_display");
2063
2064         pcdev->zoominfo.zoom_rate = 100;
2065
2066     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
2067                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus || !pcdev->pd_display ||
2068                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) || IS_ERR(pcdev->pd_display) ||
2069                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
2070
2071         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
2072         err = -ENOENT;
2073         goto exit_reqmem;
2074     }
2075
2076         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
2077                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
2078
2079         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
2080         err = -ENOENT;
2081         goto exit_reqmem;
2082     }
2083
2084     dev_set_drvdata(&pdev->dev, pcdev);
2085     pcdev->res = res;
2086
2087     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
2088     if (pcdev->pdata && pcdev->pdata->io_init) {
2089         pcdev->pdata->io_init();
2090     }
2091         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
2092         if (pcdev->pdata && (strcmp(pcdev->pdata->meminfo.name,"camera_ipp_mem")==0)) {
2093                 pcdev->vipmem_phybase = pcdev->pdata->meminfo.start;
2094                 pcdev->vipmem_size = pcdev->pdata->meminfo.size;
2095                 RK29CAMERA_DG("\n%s Memory(start:0x%x size:0x%x) for IPP obtain \n",__FUNCTION__, pcdev->pdata->meminfo.start,pcdev->pdata->meminfo.size);
2096         } else {
2097                 RK29CAMERA_TR("\n%s Memory for IPP have not obtain! IPP Function is fail\n",__FUNCTION__);
2098                 pcdev->vipmem_phybase = 0;
2099                 pcdev->vipmem_size = 0;
2100         }
2101         #endif
2102     INIT_LIST_HEAD(&pcdev->capture);
2103     spin_lock_init(&pcdev->lock);
2104     sema_init(&pcdev->zoominfo.sem,1);
2105
2106     /*
2107      * Request the regions.
2108      */
2109     if (!request_mem_region(res->start, res->end - res->start + 1,
2110                             RK29_CAM_DRV_NAME)) {
2111         err = -EBUSY;
2112         goto exit_reqmem;
2113     }
2114
2115     pcdev->base = ioremap(res->start, res->end - res->start + 1);
2116     if (pcdev->base == NULL) {
2117         dev_err(pcdev->dev, "ioremap() of registers failed\n");
2118         err = -ENXIO;
2119         goto exit_ioremap;
2120     }
2121
2122     pcdev->irq = irq;
2123     pcdev->dev = &pdev->dev;
2124
2125     /* config buffer address */
2126     /* request irq */
2127     err = request_irq(pcdev->irq, rk29_camera_irq, 0, RK29_CAM_DRV_NAME,
2128                       pcdev);
2129     if (err) {
2130         dev_err(pcdev->dev, "Camera interrupt register failed \n");
2131         goto exit_reqirq;
2132     }
2133
2134         pcdev->camera_wq = create_workqueue("rk_camera_workqueue");
2135         if (pcdev->camera_wq == NULL)
2136                 goto exit_free_irq;
2137         INIT_WORK(&pcdev->camera_reinit_work, rk29_camera_reinit_work);
2138
2139     for (i=0; i<2; i++) {
2140         pcdev->icd_frmival[i].icd = NULL;
2141         pcdev->icd_frmival[i].fival_list = kzalloc(sizeof(struct rk29_camera_frmivalenum),GFP_KERNEL);
2142
2143     }
2144
2145     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
2146     pcdev->soc_host.ops         = &rk29_soc_camera_host_ops;
2147     pcdev->soc_host.priv                = pcdev;
2148     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
2149     pcdev->soc_host.nr          = pdev->id;
2150
2151     err = soc_camera_host_register(&pcdev->soc_host);
2152     if (err)
2153         goto exit_free_irq;
2154
2155         hrtimer_init(&pcdev->fps_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
2156         pcdev->fps_timer.function = rk29_camera_fps_func;
2157     pcdev->icd_cb.sensor_cb = NULL;
2158
2159     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2160     return 0;
2161
2162 exit_free_irq:
2163
2164     for (i=0; i<2; i++) {
2165         fival_list = pcdev->icd_frmival[i].fival_list;
2166         fival_nxt = fival_list;
2167         while(fival_nxt != NULL) {
2168             fival_nxt = fival_list->nxt;
2169             kfree(fival_list);
2170             fival_list = fival_nxt;
2171         }
2172     }
2173
2174     free_irq(pcdev->irq, pcdev);
2175         if (pcdev->camera_wq) {
2176                 destroy_workqueue(pcdev->camera_wq);
2177                 pcdev->camera_wq = NULL;
2178         }
2179 exit_reqirq:
2180     iounmap(pcdev->base);
2181 exit_ioremap:
2182     release_mem_region(res->start, res->end - res->start + 1);
2183 exit_reqmem:
2184     if (pcdev->aclk_ddr_lcdc) {
2185                 clk_put(pcdev->aclk_ddr_lcdc);
2186                 pcdev->aclk_ddr_lcdc = NULL;
2187     }
2188         if (pcdev->aclk_disp_matrix) {
2189                 clk_put(pcdev->aclk_disp_matrix);
2190                 pcdev->aclk_disp_matrix = NULL;
2191     }
2192         if (pcdev->hclk_cpu_display) {
2193                 clk_put(pcdev->hclk_cpu_display);
2194                 pcdev->hclk_cpu_display = NULL;
2195     }
2196         if (pcdev->vip_slave) {
2197                 clk_put(pcdev->vip_slave);
2198                 pcdev->vip_slave = NULL;
2199     }
2200         if (pcdev->vip_out) {
2201                 clk_put(pcdev->vip_out);
2202                 pcdev->vip_out = NULL;
2203     }
2204         if (pcdev->vip_input) {
2205                 clk_put(pcdev->vip_input);
2206                 pcdev->vip_input = NULL;
2207     }
2208         if (pcdev->vip_bus) {
2209                 clk_put(pcdev->vip_bus);
2210                 pcdev->vip_bus = NULL;
2211     }
2212     if (pcdev->hclk_disp_matrix) {
2213                 clk_put(pcdev->hclk_disp_matrix);
2214                 pcdev->hclk_disp_matrix = NULL;
2215     }
2216         if (pcdev->vip_matrix) {
2217                 clk_put(pcdev->vip_matrix);
2218                 pcdev->vip_matrix = NULL;
2219     }
2220     kfree(pcdev);
2221 exit_alloc:
2222     rk29_camdev_info_ptr = NULL;
2223 exit:
2224     return err;
2225 }
2226
2227 static int __devexit rk29_camera_remove(struct platform_device *pdev)
2228 {
2229     struct rk29_camera_dev *pcdev = platform_get_drvdata(pdev);
2230     struct resource *res;
2231     struct rk29_camera_frmivalenum *fival_list,*fival_nxt;
2232     int i;
2233
2234     free_irq(pcdev->irq, pcdev);
2235
2236         if (pcdev->camera_wq) {
2237                 destroy_workqueue(pcdev->camera_wq);
2238                 pcdev->camera_wq = NULL;
2239         }
2240
2241     for (i=0; i<2; i++) {
2242         fival_list = pcdev->icd_frmival[i].fival_list;
2243         fival_nxt = fival_list;
2244         while(fival_nxt != NULL) {
2245             fival_nxt = fival_list->nxt;
2246             kfree(fival_list);
2247             fival_list = fival_nxt;
2248         }
2249     }
2250
2251     soc_camera_host_unregister(&pcdev->soc_host);
2252
2253     res = pcdev->res;
2254     release_mem_region(res->start, res->end - res->start + 1);
2255
2256     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
2257         pcdev->pdata->io_deinit(0);
2258                 pcdev->pdata->io_deinit(1);
2259     }
2260
2261     kfree(pcdev);
2262     rk29_camdev_info_ptr = NULL;
2263     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
2264
2265     return 0;
2266 }
2267
2268 static struct platform_driver rk29_camera_driver =
2269 {
2270     .driver     = {
2271         .name   = RK29_CAM_DRV_NAME,
2272     },
2273     .probe              = rk29_camera_probe,
2274     .remove             = __devexit_p(rk29_camera_remove),
2275 };
2276
2277
2278 static int __devinit rk29_camera_init(void)
2279 {
2280     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
2281     return platform_driver_register(&rk29_camera_driver);
2282 }
2283
2284 static void __exit rk29_camera_exit(void)
2285 {
2286     platform_driver_unregister(&rk29_camera_driver);
2287 }
2288
2289 device_initcall_sync(rk29_camera_init);
2290 module_exit(rk29_camera_exit);
2291
2292 MODULE_DESCRIPTION("RK29 Soc Camera Host driver");
2293 MODULE_AUTHOR("ddl <ddl@rock-chips>");
2294 MODULE_LICENSE("GPL");
2295 #endif