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