drivers/media/video/rk29_camera_oneframe.c

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