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