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