drivers/media/video/rk29_camera_oneframe.c

   1 /*
   2  * V4L2 Driver for RK28 camera host
   3  *
   4  * Copyright (C) 2006, Sascha Hauer, Pengutronix
   5  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  */
  12
  13 #include <linux/init.h>
  14 #include <linux/module.h>
  15 #include <linux/io.h>
  16 #include <linux/delay.h>
  17 #include <linux/slab.h>
  18 #include <linux/dma-mapping.h>
  19 #include <linux/errno.h>
  20 #include <linux/fs.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/kernel.h>
  23 #include <linux/mm.h>
  24 #include <linux/moduleparam.h>
  25 #include <linux/time.h>
  26 #include <linux/clk.h>
  27 #include <linux/version.h>
  28 #include <linux/device.h>
  29 #include <linux/platform_device.h>
  30 #include <linux/mutex.h>
  31 #include <linux/videodev2.h>
  32 #include <mach/rk29_camera.h>
  33 #include <mach/rk29_iomap.h>
  34 #include <mach/iomux.h>
  35 #include <media/v4l2-common.h>
  36 #include <media/v4l2-dev.h>
  37 #include <media/videobuf-dma-contig.h>
  38 #include <media/soc_camera.h>
  39 #include <mach/rk29-ipp.h>
  40
  41
  42 static int debug;
  43 module_param(debug, int, S_IRUGO|S_IWUSR);
  44
  45 #define dprintk(level, fmt, arg...) do {                        \
  46         if (debug >= level)                                     \
  47         printk(KERN_WARNING"rk29xx_camera: " fmt , ## arg); } while (0)
  48
  49 #define RK29CAMERA_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  50 #define RK29CAMERA_DG(format, ...) dprintk(0, format, ## __VA_ARGS__)
  51
  52 // VIP Reg Offset
  53 #define RK29_VIP_AHBR_CTRL                0x00
  54 #define RK29_VIP_INT_MASK                 0x04
  55 #define RK29_VIP_INT_STS                  0x08
  56 #define RK29_VIP_STS                      0x0c
  57 #define RK29_VIP_CTRL                     0x10
  58 #define RK29_VIP_CAPTURE_F1SA_Y           0x14
  59 #define RK29_VIP_CAPTURE_F1SA_UV          0x18
  60 #define RK29_VIP_CAPTURE_F1SA_Cr          0x1c
  61 #define RK29_VIP_CAPTURE_F2SA_Y           0x20
  62 #define RK29_VIP_CAPTURE_F2SA_UV          0x24
  63 #define RK29_VIP_CAPTURE_F2SA_Cr          0x28
  64 #define RK29_VIP_FB_SR                    0x2c
  65 #define RK29_VIP_FS                       0x30
  66 #define RK29_VIP_VIPRESERVED              0x34
  67 #define RK29_VIP_CROP                     0x38
  68 #define RK29_VIP_CRM                      0x3c
  69 #define RK29_VIP_RESET                    0x40
  70 #define RK29_VIP_L_SFT                    0x44
  71
  72 //The key register bit descrition
  73 // VIP_CTRL Reg
  74 #define  DISABLE_CAPTURE              (0x00<<0)
  75 #define  ENABLE_CAPTURE               (0x01<<0)
  76 #define  HSY_HIGH_ACTIVE              (0x00<<1)
  77 #define  HSY_LOW_ACTIVE               (0x01<<1)
  78 #define  VIP_CCIR656                  (0x00<<2)
  79 #define  VIP_SENSOR                   (0x01<<2)
  80 #define  SENSOR_UYVY                  (0x00<<3)
  81 #define  SENSOR_YUYV                  (0x01<<3)
  82 #define  VIP_YUV                      (0x00<<4)
  83 #define  VIP_RAW                      (0x01<<4)
  84 #define  CON_OR_PIN                   (0x00<<5)
  85 #define  ONEFRAME                     (0x01<<5)
  86 #define  VIPREGYUV420                 (0x00<<6)
  87 #define  VIPREGYUV422                 (0x01<<6)
  88 #define  FIELD0_START                 (0x00<<7)
  89 #define  FIELD1_START                 (0x01<<7)
  90 #define  CONTINUOUS                   (0x00<<8)
  91 #define  PING_PONG                    (0x01<<8)
  92 #define  POSITIVE_EDGE                (0x00<<9)
  93 #define  NEGATIVE_EDGE                (0x01<<9)
  94 #define  VIPREGNTSC                   (0x00<<10)
  95 #define  VIPREGPAL                    (0x01<<10)
  96 #define  VIP_DATA_LITTLEEND           (0x00<<11)
  97 #define  VIP_DATA_BIGEND              (0x01<<11)
  98 #define  VSY_LOW_ACTIVE               (0x00<<12)
  99 #define  VSY_HIGH_ACTIVE              (0x01<<12)
 100 #define  VIP_RAWINPUT_BYPASS          (0x00<<13)
 101 #define  VIP_RAWINPUT_POSITIVE_EDGE   (0x01<<13)
 102 #define  VIP_RAWINPUT_NEGATIVE_EDGE   (0x02<<13)
 103
 104 // GRF_SOC_CON0 Reg
 105 #define  GRF_SOC_CON0_Reg             0xbc
 106 #define  VIP_AXIMASTER                (0x00<<0)
 107 #define  VIP_AHBMASTER                (0x01<<2)
 108
 109 // GRF_OS_REG0
 110 #define  GRF_OS_REG0                  0xd0
 111 #define  VIP_ACLK_DIV_HCLK_1          (0x00<<0)
 112 #define  VIP_ACLK_DIV_HCLK_2          (0x01<<0)
 113
 114
 115 #define MIN(x,y)   ((x<y) ? x: y)
 116 #define MAX(x,y)    ((x>y) ? x: y)
 117 #define RK29_SENSOR_24MHZ      24           /* MHz */
 118 #define RK29_SENSOR_48MHZ      48
 119
 120 #define write_vip_reg(addr, val)  __raw_writel(val, addr+(rk29_camdev_info_ptr->base))
 121 #define read_vip_reg(addr) __raw_readl(addr+(rk29_camdev_info_ptr->base))
 122 #define mask_vip_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 123
 124 #define write_grf_reg(addr, val)  __raw_writel(val, addr+RK29_GRF_BASE)
 125 #define read_grf_reg(addr) __raw_readl(addr+RK29_GRF_BASE)
 126 #define mask_grf_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 127
 128 //Configure Macro
 129 #define RK29_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 1)
 130
 131 /* limit to rk29 hardware capabilities */
 132 #define RK29_CAM_BUS_PARAM   (SOCAM_MASTER |\
 133                 SOCAM_HSYNC_ACTIVE_HIGH |\
 134                 SOCAM_HSYNC_ACTIVE_LOW |\
 135                 SOCAM_VSYNC_ACTIVE_HIGH |\
 136                 SOCAM_VSYNC_ACTIVE_LOW |\
 137                 SOCAM_PCLK_SAMPLE_RISING |\
 138                 SOCAM_PCLK_SAMPLE_FALLING|\
 139                 SOCAM_DATA_ACTIVE_HIGH |\
 140                 SOCAM_DATA_ACTIVE_LOW|\
 141                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 142                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 143
 144 #define RK29_CAM_W_MIN        48
 145 #define RK29_CAM_H_MIN        32
 146 #define RK29_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 147 #define RK29_CAM_H_MAX        2764
 148 #define RK29_CAM_FRAME_INVAL_INIT 3
 149 #define RK29_CAM_FRAME_INVAL_DC 3          /* ddl@rock-chips.com :  */
 150
 151 #define RK29_CAM_AXI   0
 152 #define RK29_CAM_AHB   1
 153 #define CONFIG_RK29_CAM_WORK_BUS    RK29_CAM_AXI
 154
 155 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 156 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 157 extern void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 158             struct videobuf_queue_ops *ops,
 159             struct device *dev,
 160             spinlock_t *irqlock,
 161             enum v4l2_buf_type type,
 162             enum v4l2_field field,
 163             unsigned int msize,
 164             void *priv);
 165
 166 /* buffer for one video frame */
 167 struct rk29_buffer
 168 {
 169     /* common v4l buffer stuff -- must be first */
 170     struct videobuf_buffer vb;
 171     const struct soc_camera_data_format        *fmt;
 172     int                 inwork;
 173 };
 174 enum rk29_camera_reg_state
 175 {
 176         Reg_Invalidate,
 177         Reg_Validate
 178 };
 179
 180 struct rk29_camera_reg
 181 {
 182         unsigned int VipCtrl;
 183         unsigned int VipCrop;
 184         unsigned int VipFs;
 185         unsigned int VipIntMsk;
 186         unsigned int VipCrm;
 187         enum rk29_camera_reg_state Inval;
 188 };
 189 struct rk29_camera_work
 190 {
 191         struct videobuf_buffer *vb;
 192         struct rk29_camera_dev *pcdev;
 193         struct work_struct work;
 194 };
 195 struct rk29_camera_dev
 196 {
 197     struct soc_camera_host      soc_host;
 198     struct device               *dev;
 199     /* RK2827x is only supposed to handle one camera on its Quick Capture
 200      * interface. If anyone ever builds hardware to enable more than
 201      * one camera, they will have to modify this driver too */
 202     struct soc_camera_device *icd;
 203
 204         struct clk *aclk_ddr_lcdc;
 205         struct clk *aclk_disp_matrix;
 206
 207         struct clk *hclk_cpu_display;
 208         struct clk *vip_slave;
 209
 210         struct clk *vip_out;
 211         struct clk *vip_input;
 212         struct clk *vip_bus;
 213
 214         struct clk *hclk_disp_matrix;
 215         struct clk *vip_matrix;
 216
 217     void __iomem *base;
 218         void __iomem *grf_base;
 219     int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 220     unsigned int irq;
 221         unsigned int fps;
 222         unsigned int pixfmt;
 223         unsigned int vipmem_phybase;
 224         unsigned int vipmem_size;
 225         unsigned int vipmem_bsize;
 226         int host_width;
 227         int host_height;
 228
 229     struct rk29camera_platform_data *pdata;
 230     struct resource             *res;
 231
 232     struct list_head    capture;
 233
 234     spinlock_t          lock;
 235
 236     struct videobuf_buffer      *active;
 237         struct videobuf_queue *vb_vidq_ptr;
 238         struct rk29_camera_reg reginfo_suspend;
 239         struct workqueue_struct *camera_wq;
 240         struct rk29_camera_work *camera_work;
 241         unsigned int camera_work_count;
 242         struct hrtimer fps_timer;
 243         struct work_struct camera_reinit_work;
 244 };
 245 static DEFINE_MUTEX(camera_lock);
 246 static const char *rk29_cam_driver_description = "RK29_Camera";
 247 static struct rk29_camera_dev *rk29_camdev_info_ptr;
 248
 249 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable);
 250
 251
 252 /*
 253  *  Videobuf operations
 254  */
 255 static int rk29_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 256                                unsigned int *size)
 257 {
 258     struct soc_camera_device *icd = vq->priv_data;
 259         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 260     struct rk29_camera_dev *pcdev = ici->priv;
 261     int bytes_per_pixel = (icd->current_fmt->depth + 7) >> 3;
 262
 263     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 264
 265         /* planar capture requires Y, U and V buffers to be page aligned */
 266         #if 0
 267     *size = PAGE_ALIGN(icd->user_width* icd->user_height * bytes_per_pixel);                               /* Y pages UV pages, yuv422*/
 268         pcdev->vipmem_bsize = PAGE_ALIGN(pcdev->host_width * pcdev->host_height * bytes_per_pixel);
 269         #else
 270         *size = PAGE_ALIGN((icd->user_width* icd->user_height * icd->current_fmt->depth + 7)>>3);                               /* Y pages UV pages, yuv422*/
 271         pcdev->vipmem_bsize = PAGE_ALIGN((pcdev->host_width * pcdev->host_height * icd->current_fmt->depth + 7)>>3);
 272         #endif
 273
 274         if ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)) {
 275
 276                 if (*count > pcdev->vipmem_size/pcdev->vipmem_bsize) {    /* Buffers must be limited, when this resolution is genered by IPP */
 277                         *count = pcdev->vipmem_size/pcdev->vipmem_bsize;
 278                 }
 279
 280                 if ((pcdev->camera_work_count != *count) && pcdev->camera_work) {
 281                         kfree(pcdev->camera_work);
 282                         pcdev->camera_work = NULL;
 283                         pcdev->camera_work_count = 0;
 284                 }
 285
 286                 if (pcdev->camera_work == NULL) {
 287                         pcdev->camera_work = kmalloc(sizeof(struct rk29_camera_work)*(*count), GFP_KERNEL);
 288                         if (pcdev->camera_work == NULL) {
 289                                 RK29CAMERA_TR("\n %s kmalloc fail\n", __FUNCTION__);
 290                                 BUG();
 291                         }
 292                         pcdev->camera_work_count = *count;
 293                 }
 294         }
 295
 296     RK29CAMERA_DG("%s..%d.. videobuf size:%d, vipmem_buf size:%d \n",__FUNCTION__,__LINE__, *size,pcdev->vipmem_bsize);
 297
 298     return 0;
 299 }
 300 static void rk29_videobuf_free(struct videobuf_queue *vq, struct rk29_buffer *buf)
 301 {
 302     struct soc_camera_device *icd = vq->priv_data;
 303
 304     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 305             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 306
 307         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 308         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 309                 return;
 310
 311     if (in_interrupt())
 312         BUG();
 313
 314     videobuf_dma_contig_free(vq, &buf->vb);
 315     dev_dbg(&icd->dev, "%s freed\n", __func__);
 316     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 317         return;
 318 }
 319 static int rk29_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 320 {
 321     struct soc_camera_device *icd = vq->priv_data;
 322     struct rk29_buffer *buf;
 323     int ret;
 324
 325     buf = container_of(vb, struct rk29_buffer, vb);
 326
 327     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 328             vb, vb->baddr, vb->bsize);
 329
 330     //RK29CAMERA_TR("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 331
 332     /* Added list head initialization on alloc */
 333     WARN_ON(!list_empty(&vb->queue));
 334
 335     /* This can be useful if you want to see if we actually fill
 336      * the buffer with something */
 337     //memset((void *)vb->baddr, 0xaa, vb->bsize);
 338
 339     BUG_ON(NULL == icd->current_fmt);
 340
 341     if (buf->fmt    != icd->current_fmt ||
 342             vb->width   != icd->user_width ||
 343             vb->height  != icd->user_height ||
 344              vb->field   != field) {
 345         buf->fmt    = icd->current_fmt;
 346         vb->width   = icd->user_width;
 347         vb->height  = icd->user_height;
 348         vb->field   = field;
 349         vb->state   = VIDEOBUF_NEEDS_INIT;
 350     }
 351
 352     vb->size = (((vb->width * vb->height *buf->fmt->depth) + 7) >> 3) ;          /* ddl@rock-chips.com : fmt->depth is coorect */
 353     if (0 != vb->baddr && vb->bsize < vb->size) {
 354         ret = -EINVAL;
 355         goto out;
 356     }
 357
 358     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 359         ret = videobuf_iolock(vq, vb, NULL);
 360         if (ret) {
 361             goto fail;
 362         }
 363         vb->state = VIDEOBUF_PREPARED;
 364     }
 365     //RK29CAMERA_TR("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 366     return 0;
 367 fail:
 368     rk29_videobuf_free(vq, buf);
 369 out:
 370     return ret;
 371 }
 372
 373 static inline void rk29_videobuf_capture(struct videobuf_buffer *vb)
 374 {
 375         unsigned int y_addr,uv_addr;
 376         struct rk29_camera_dev *pcdev = rk29_camdev_info_ptr;
 377
 378     if (vb) {
 379                 if ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)) {
 380                         y_addr = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 381                         uv_addr = y_addr + pcdev->host_width*pcdev->host_height;
 382
 383                         if (y_addr > (pcdev->vipmem_phybase + pcdev->vipmem_size - pcdev->vipmem_bsize)) {
 384                                 RK29CAMERA_TR("vipmem for IPP is overflow! %dx%d -> %dx%d vb_index:%d\n",pcdev->host_width,pcdev->host_height,
 385                                                   pcdev->icd->user_width,pcdev->icd->user_height, vb->i);
 386                                 BUG();
 387                         }
 388                 } else {
 389                         y_addr = vb->boff;
 390                         uv_addr = y_addr + vb->width * vb->height;
 391                 }
 392         write_vip_reg(RK29_VIP_CAPTURE_F1SA_Y, y_addr);
 393         write_vip_reg(RK29_VIP_CAPTURE_F1SA_UV, uv_addr);
 394         write_vip_reg(RK29_VIP_CAPTURE_F2SA_Y, y_addr);
 395         write_vip_reg(RK29_VIP_CAPTURE_F2SA_UV, uv_addr);
 396         write_vip_reg(RK29_VIP_FB_SR,  0x00000002);//frame1 has been ready to receive data,frame 2 is not used
 397     }
 398 }
 399 /* Locking: Caller holds q->irqlock */
 400 static void rk29_videobuf_queue(struct videobuf_queue *vq,
 401                                 struct videobuf_buffer *vb)
 402 {
 403     struct soc_camera_device *icd = vq->priv_data;
 404     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 405     struct rk29_camera_dev *pcdev = ici->priv;
 406
 407     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 408             vb, vb->baddr, vb->bsize);
 409
 410     vb->state = VIDEOBUF_QUEUED;
 411
 412         if (list_empty(&pcdev->capture)) {
 413                 list_add_tail(&vb->queue, &pcdev->capture);
 414         } else {
 415                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 416                         list_add_tail(&vb->queue, &pcdev->capture);
 417                 else
 418                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 419         }
 420
 421     if (!pcdev->active) {
 422         pcdev->active = vb;
 423         rk29_videobuf_capture(vb);
 424     }
 425 }
 426 static int rk29_pixfmt2ippfmt(unsigned int pixfmt, int *ippfmt)
 427 {
 428         switch (pixfmt)
 429         {
 430                 case V4L2_PIX_FMT_YUV422P:
 431                 {
 432                         *ippfmt = IPP_Y_CBCR_H2V1;
 433                         break;
 434                 }
 435                 case V4L2_PIX_FMT_YUV420:
 436                 {
 437                         *ippfmt = IPP_Y_CBCR_H2V2;
 438                         break;
 439                 }
 440                 default:
 441                         goto rk29_pixfmt2ippfmt_err;
 442         }
 443
 444         return 0;
 445 rk29_pixfmt2ippfmt_err:
 446         return -1;
 447 }
 448 static void rk29_camera_capture_process(struct work_struct *work)
 449 {
 450         struct rk29_camera_work *camera_work = container_of(work, struct rk29_camera_work, work);
 451         struct videobuf_buffer *vb = camera_work->vb;
 452         struct rk29_camera_dev *pcdev = camera_work->pcdev;
 453         struct rk29_ipp_req ipp_req;
 454         unsigned long int flags;
 455
 456         ipp_req.src0.YrgbMst = pcdev->vipmem_phybase + vb->i*pcdev->vipmem_bsize;
 457         ipp_req.src0.CbrMst= ipp_req.src0.YrgbMst + pcdev->host_width*pcdev->host_height;
 458         ipp_req.src0.w = pcdev->host_width;
 459         ipp_req.src0.h = pcdev->host_height;
 460         rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.src0.fmt);
 461
 462         ipp_req.dst0.YrgbMst = vb->boff;
 463         ipp_req.dst0.CbrMst= vb->boff+vb->width * vb->height;
 464         ipp_req.dst0.w = pcdev->icd->user_width;
 465         ipp_req.dst0.h = pcdev->icd->user_height;
 466         rk29_pixfmt2ippfmt(pcdev->pixfmt, &ipp_req.dst0.fmt);
 467
 468         ipp_req.src_vir_w = ipp_req.src0.w;
 469         ipp_req.dst_vir_w = ipp_req.dst0.w;
 470         ipp_req.timeout = 100;
 471         ipp_req.flag = IPP_ROT_0;
 472
 473         if (ipp_do_blit(&ipp_req)) {
 474                 spin_lock_irqsave(&pcdev->lock, flags);
 475                 vb->state = VIDEOBUF_ERROR;
 476                 spin_unlock_irqrestore(&pcdev->lock, flags);
 477                 RK29CAMERA_TR("Capture image(vb->i:0x%x) which IPP operated is error!\n", vb->i);
 478                 RK29CAMERA_TR("ipp_req.src0.YrgbMst:0x%x ipp_req.src0.CbrMst:0x%x \n", ipp_req.src0.YrgbMst,ipp_req.src0.CbrMst);
 479                 RK29CAMERA_TR("ipp_req.src0.w:0x%x ipp_req.src0.h:0x%x \n",ipp_req.src0.w,ipp_req.src0.h);
 480                 RK29CAMERA_TR("ipp_req.src0.fmt:0x%x\n",ipp_req.src0.fmt);
 481                 RK29CAMERA_TR("ipp_req.dst0.YrgbMst:0x%x ipp_req.dst0.CbrMst:0x%x \n",ipp_req.dst0.YrgbMst,ipp_req.dst0.CbrMst);
 482                 RK29CAMERA_TR("ipp_req.dst0.w:0x%x ipp_req.dst0.h:0x%x \n",ipp_req.dst0.w ,ipp_req.dst0.h);
 483                 RK29CAMERA_TR("ipp_req.dst0.fmt:0x%x\n",ipp_req.dst0.fmt);
 484                 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);
 485             RK29CAMERA_TR("ipp_req.timeout:0x%x ipp_req.flag :0x%x\n",ipp_req.timeout,ipp_req.flag);
 486         }
 487
 488         wake_up(&(camera_work->vb->done));
 489 }
 490 static irqreturn_t rk29_camera_irq(int irq, void *data)
 491 {
 492     struct rk29_camera_dev *pcdev = data;
 493     struct videobuf_buffer *vb;
 494         struct rk29_camera_work *wk;
 495
 496     read_vip_reg(RK29_VIP_INT_STS);    /* clear vip interrupte single  */
 497     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 498     if (read_vip_reg(RK29_VIP_FB_SR) & 0x01) {
 499                 pcdev->fps++;
 500                 if (!pcdev->active)
 501                         goto RK29_CAMERA_IRQ_END;
 502
 503         if (pcdev->frame_inval>0) {
 504             pcdev->frame_inval--;
 505             rk29_videobuf_capture(pcdev->active);
 506             goto RK29_CAMERA_IRQ_END;
 507         } else if (pcdev->frame_inval) {
 508                 RK29CAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 509             pcdev->frame_inval = 0;
 510         }
 511
 512         vb = pcdev->active;
 513                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 514                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 515                 list_del_init(&vb->queue);
 516                 }
 517
 518         pcdev->active = NULL;
 519         if (!list_empty(&pcdev->capture)) {
 520             pcdev->active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 521                         if (pcdev->active) {
 522                                 rk29_videobuf_capture(pcdev->active);
 523                         }
 524         }
 525
 526         if (pcdev->active == NULL) {
 527                         RK29CAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 528         }
 529
 530                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 531                 vb->state = VIDEOBUF_DONE;
 532                 do_gettimeofday(&vb->ts);
 533                 vb->field_count++;
 534                 }
 535
 536                 if ((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height)) {
 537                         wk = pcdev->camera_work + vb->i;
 538                         INIT_WORK(&(wk->work), rk29_camera_capture_process);
 539                         wk->vb = vb;
 540                         wk->pcdev = pcdev;
 541                         queue_work(pcdev->camera_wq, &(wk->work));
 542                 } else {
 543                         wake_up(&vb->done);
 544                 }
 545     }
 546
 547 RK29_CAMERA_IRQ_END:
 548     return IRQ_HANDLED;
 549 }
 550
 551
 552 static void rk29_videobuf_release(struct videobuf_queue *vq,
 553                                   struct videobuf_buffer *vb)
 554 {
 555     struct rk29_buffer *buf = container_of(vb, struct rk29_buffer, vb);
 556     struct soc_camera_device *icd = vq->priv_data;
 557     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 558     struct rk29_camera_dev *pcdev = ici->priv;
 559
 560 #ifdef DEBUG
 561     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 562             vb, vb->baddr, vb->bsize);
 563
 564     switch (vb->state)
 565     {
 566         case VIDEOBUF_ACTIVE:
 567             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 568             break;
 569         case VIDEOBUF_QUEUED:
 570             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 571             break;
 572         case VIDEOBUF_PREPARED:
 573             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 574             break;
 575         default:
 576             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 577             break;
 578     }
 579 #endif
 580         if (vb == pcdev->active) {
 581                 RK29CAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 582                 interruptible_sleep_on_timeout(&vb->done, 100);
 583                 RK29CAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 584         }
 585     rk29_videobuf_free(vq, buf);
 586 }
 587
 588 static struct videobuf_queue_ops rk29_videobuf_ops =
 589 {
 590     .buf_setup      = rk29_videobuf_setup,
 591     .buf_prepare    = rk29_videobuf_prepare,
 592     .buf_queue      = rk29_videobuf_queue,
 593     .buf_release    = rk29_videobuf_release,
 594 };
 595
 596 static void rk29_camera_init_videobuf(struct videobuf_queue *q,
 597                                       struct soc_camera_device *icd)
 598 {
 599     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 600     struct rk29_camera_dev *pcdev = ici->priv;
 601
 602     /* We must pass NULL as dev pointer, then all pci_* dma operations
 603      * transform to normal dma_* ones. */
 604     videobuf_queue_dma_contig_init(q,
 605                                    &rk29_videobuf_ops,
 606                                    ici->v4l2_dev.dev, &pcdev->lock,
 607                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 608                                    V4L2_FIELD_NONE,
 609                                    sizeof(struct rk29_buffer),
 610                                    icd);
 611         pcdev->vb_vidq_ptr = q;         /* ddl@rock-chips.com */
 612 }
 613 static int rk29_camera_activate(struct rk29_camera_dev *pcdev, struct soc_camera_device *icd)
 614 {
 615     unsigned long sensor_bus_flags = SOCAM_MCLK_24MHZ;
 616     struct clk *parent;
 617
 618     RK29CAMERA_DG("%s..%d.. \n",__FUNCTION__,__LINE__);
 619     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
 620                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus ||
 621                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) ||
 622                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
 623
 624         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
 625         goto RK29_CAMERA_ACTIVE_ERR;
 626     }
 627
 628         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
 629                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
 630
 631         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
 632         goto RK29_CAMERA_ACTIVE_ERR;
 633     }
 634
 635         clk_enable(pcdev->aclk_ddr_lcdc);
 636         clk_enable(pcdev->aclk_disp_matrix);
 637
 638         clk_enable(pcdev->hclk_cpu_display);
 639         clk_enable(pcdev->vip_slave);
 640
 641 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 642         clk_enable(pcdev->hclk_disp_matrix);
 643         clk_enable(pcdev->vip_matrix);
 644 #endif
 645
 646         clk_enable(pcdev->vip_input);
 647         clk_enable(pcdev->vip_bus);
 648
 649     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 650         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 651
 652     if (sensor_bus_flags & SOCAM_MCLK_48MHZ) {
 653         parent = clk_get(NULL, "clk48m");
 654         if (!parent || IS_ERR(parent))
 655              goto RK29_CAMERA_ACTIVE_ERR;
 656     } else if (sensor_bus_flags & SOCAM_MCLK_27MHZ) {
 657         parent = clk_get(NULL, "extclk");
 658         if (!parent || IS_ERR(parent))
 659              goto RK29_CAMERA_ACTIVE_ERR;
 660     } else {
 661         parent = clk_get(NULL, "xin24m");
 662         if (!parent || IS_ERR(parent))
 663              goto RK29_CAMERA_ACTIVE_ERR;
 664     }
 665
 666     clk_set_parent(pcdev->vip_out, parent);
 667
 668     clk_enable(pcdev->vip_out);
 669     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_VIP_CLKOUT);
 670     ndelay(10);
 671
 672 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 673         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)|VIP_AHBMASTER);  //VIP Config to AHB
 674         write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)&(~VIP_ACLK_DIV_HCLK_2));   //aclk:hclk = 1:1
 675 #else
 676         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)&(~VIP_AHBMASTER));  //VIP Config to AXI
 677     write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)|VIP_ACLK_DIV_HCLK_2);   //aclk:hclk = 2:1
 678 #endif
 679         ndelay(10);
 680
 681     write_vip_reg(RK29_VIP_RESET, 0x76543210);  /* ddl@rock-chips.com : vip software reset */
 682     udelay(10);
 683
 684     write_vip_reg(RK29_VIP_AHBR_CTRL, 0x07);   /* ddl@rock-chips.com : vip ahb burst 16 */
 685     write_vip_reg(RK29_VIP_INT_MASK, 0x01);    //capture complete interrupt enable
 686     write_vip_reg(RK29_VIP_CRM,  0x00000000);  //Y/CB/CR color modification
 687
 688     return 0;
 689 RK29_CAMERA_ACTIVE_ERR:
 690     return -ENODEV;
 691 }
 692
 693 static void rk29_camera_deactivate(struct rk29_camera_dev *pcdev)
 694 {
 695     //pcdev->active = NULL;
 696
 697     write_vip_reg(RK29_VIP_CTRL, 0);
 698     read_vip_reg(RK29_VIP_INT_STS);             //clear vip interrupte single
 699
 700     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_GPIO1B4);
 701     clk_disable(pcdev->vip_out);
 702
 703         clk_disable(pcdev->vip_input);
 704         clk_disable(pcdev->vip_bus);
 705
 706 #if (CONFIG_RK29_CAM_WORK_BUS==RK29_CAM_AHB)
 707         clk_disable(pcdev->hclk_disp_matrix);
 708         clk_disable(pcdev->vip_matrix);
 709 #endif
 710
 711         clk_disable(pcdev->hclk_cpu_display);
 712         clk_disable(pcdev->vip_slave);
 713
 714         clk_disable(pcdev->aclk_ddr_lcdc);
 715         clk_disable(pcdev->aclk_disp_matrix);
 716     return;
 717 }
 718
 719 /* The following two functions absolutely depend on the fact, that
 720  * there can be only one camera on RK28 quick capture interface */
 721 static int rk29_camera_add_device(struct soc_camera_device *icd)
 722 {
 723     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 724     struct rk29_camera_dev *pcdev = ici->priv;
 725     struct device *control = to_soc_camera_control(icd);
 726     struct v4l2_subdev *sd;
 727     int ret;
 728
 729     mutex_lock(&camera_lock);
 730
 731     if (pcdev->icd) {
 732         ret = -EBUSY;
 733         goto ebusy;
 734     }
 735
 736     dev_info(&icd->dev, "RK29 Camera driver attached to camera%d(%s)\n",
 737              icd->devnum,dev_name(icd->pdev));
 738
 739         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 740     pcdev->active = NULL;
 741     pcdev->icd = NULL;
 742         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 743         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 744      * if app havn't dequeue all videobuf before close camera device;
 745         */
 746     INIT_LIST_HEAD(&pcdev->capture);
 747
 748     ret = rk29_camera_activate(pcdev,icd);
 749     if (ret)
 750         goto ebusy;
 751
 752     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 753     if (control) {
 754         sd = dev_get_drvdata(control);
 755                 v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_IOREQUEST,(void*)pcdev->pdata);
 756         ret = v4l2_subdev_call(sd,core, init, 0);
 757         if (ret)
 758             goto ebusy;
 759     }
 760
 761     pcdev->icd = icd;
 762
 763 ebusy:
 764     mutex_unlock(&camera_lock);
 765
 766     return ret;
 767 }
 768 static void rk29_camera_remove_device(struct soc_camera_device *icd)
 769 {
 770     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 771     struct rk29_camera_dev *pcdev = ici->priv;
 772         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 773
 774         mutex_lock(&camera_lock);
 775     BUG_ON(icd != pcdev->icd);
 776
 777     dev_info(&icd->dev, "RK29 Camera driver detached from camera%d(%s)\n",
 778              icd->devnum,dev_name(icd->pdev));
 779
 780         /* ddl@rock-chips.com: Application will call VIDIOC_STREAMOFF before close device, but
 781            stream may be turn on again before close device, if suspend and resume happened. */
 782         if (read_vip_reg(RK29_VIP_CTRL) & ENABLE_CAPTURE) {
 783                 rk29_camera_s_stream(icd,0);
 784         }
 785
 786     v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
 787         rk29_camera_deactivate(pcdev);
 788
 789         /* ddl@rock-chips.com: Call videobuf_mmap_free here for free the struct video_buffer which malloc in videobuf_alloc */
 790         #if 0
 791         if (pcdev->vb_vidq_ptr) {
 792                 videobuf_mmap_free(pcdev->vb_vidq_ptr);
 793                 pcdev->vb_vidq_ptr = NULL;
 794         }
 795         #else
 796         pcdev->vb_vidq_ptr = NULL;
 797         #endif
 798
 799         if (pcdev->camera_work) {
 800                 kfree(pcdev->camera_work);
 801                 pcdev->camera_work = NULL;
 802                 pcdev->camera_work_count = 0;
 803         }
 804
 805         pcdev->active = NULL;
 806     pcdev->icd = NULL;
 807         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 808         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 809      * if app havn't dequeue all videobuf before close camera device;
 810         */
 811     INIT_LIST_HEAD(&pcdev->capture);
 812
 813         mutex_unlock(&camera_lock);
 814         RK29CAMERA_DG("%s exit\n",__FUNCTION__);
 815
 816         return;
 817 }
 818
 819 static int rk29_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 820 {
 821     unsigned long bus_flags, camera_flags, common_flags;
 822     unsigned int vip_ctrl_val = 0;
 823     int ret = 0;
 824
 825     RK29CAMERA_DG("%s..%d..\n",__FUNCTION__,__LINE__);
 826
 827     bus_flags = RK29_CAM_BUS_PARAM;
 828         if (icd->ops->query_bus_param)
 829         camera_flags = icd->ops->query_bus_param(icd);
 830         else
 831                 camera_flags = 0;
 832
 833     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
 834     if (!common_flags) {
 835         ret = -EINVAL;
 836         goto RK29_CAMERA_SET_BUS_PARAM_END;
 837     }
 838
 839     ret = icd->ops->set_bus_param(icd, common_flags);
 840     if (ret < 0)
 841         goto RK29_CAMERA_SET_BUS_PARAM_END;
 842
 843         if (common_flags & SOCAM_DATAWIDTH_8) {
 844         icd->buswidth = 8;
 845         } else if (common_flags & SOCAM_DATAWIDTH_10) {
 846             icd->buswidth = 10;
 847         }
 848
 849     vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
 850     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
 851         vip_ctrl_val |= NEGATIVE_EDGE;
 852     } else {
 853                 vip_ctrl_val &= ~NEGATIVE_EDGE;
 854     }
 855     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
 856         vip_ctrl_val |= HSY_LOW_ACTIVE;
 857     } else {
 858                 vip_ctrl_val &= ~HSY_LOW_ACTIVE;
 859     }
 860     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
 861         vip_ctrl_val |= VSY_HIGH_ACTIVE;
 862     } else {
 863                 vip_ctrl_val &= ~VSY_HIGH_ACTIVE;
 864     }
 865
 866     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
 867     //vip_ctrl_val |= ENABLE_CAPTURE;
 868
 869     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
 870     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),
 871                 read_grf_reg(GRF_SOC_CON0_Reg)&VIP_AHBMASTER, read_grf_reg(GRF_OS_REG0)&VIP_ACLK_DIV_HCLK_2);
 872
 873 RK29_CAMERA_SET_BUS_PARAM_END:
 874         if (ret)
 875         RK29CAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
 876     return ret;
 877 }
 878
 879 static int rk29_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 880 {
 881     unsigned long bus_flags, camera_flags;
 882     int ret;
 883
 884     bus_flags = RK29_CAM_BUS_PARAM;
 885         if (icd->ops->query_bus_param) {
 886         camera_flags = icd->ops->query_bus_param(icd);
 887         } else {
 888                 camera_flags = 0;
 889         }
 890     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
 891
 892     if (ret < 0)
 893         dev_warn(icd->dev.parent,
 894                          "Flags incompatible: camera %lx, host %lx\n",
 895                          camera_flags, bus_flags);
 896     return ret;
 897 }
 898 static const struct soc_camera_data_format rk29_camera_formats[] = {
 899         {
 900                 .name           = "Planar YUV420 12 bit",
 901                 .depth          = 12,
 902                 .fourcc         = V4L2_PIX_FMT_YUV420,
 903                 .colorspace     = V4L2_COLORSPACE_JPEG,
 904         },{
 905                 .name           = "Planar YUV422 16 bit",
 906                 .depth          = 16,
 907                 .fourcc         = V4L2_PIX_FMT_YUV422P,
 908                 .colorspace     = V4L2_COLORSPACE_JPEG,
 909         },{
 910                 .name           = "Raw Bayer RGB 10 bit",
 911                 .depth          = 16,
 912                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
 913                 .colorspace     = V4L2_COLORSPACE_SRGB,
 914         }
 915 };
 916
 917 static void rk29_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, __u32 cam_pixfmt, struct v4l2_rect *rect)
 918 {
 919         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 920     struct rk29_camera_dev *pcdev = ici->priv;
 921     unsigned int vip_fs = 0,vip_crop = 0;
 922     unsigned int vip_ctrl_val = VIP_SENSOR|ONEFRAME|DISABLE_CAPTURE;
 923
 924     switch (host_pixfmt)
 925     {
 926         case V4L2_PIX_FMT_YUV422P:
 927             vip_ctrl_val |= VIPREGYUV422;
 928                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 929                         pcdev->pixfmt = host_pixfmt;
 930             break;
 931         case V4L2_PIX_FMT_YUV420:
 932             vip_ctrl_val |= VIPREGYUV420;
 933                         if (pcdev->frame_inval != RK29_CAM_FRAME_INVAL_INIT)
 934                                 pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 935                         pcdev->pixfmt = host_pixfmt;
 936             break;
 937                 case V4L2_PIX_FMT_SGRBG10:
 938                         vip_ctrl_val |= (VIP_RAW | VIP_SENSOR | VIP_DATA_LITTLEEND);
 939                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 940                         pcdev->pixfmt = host_pixfmt;
 941                         break;
 942         default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
 943             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & VIPREGYUV422);
 944             break;
 945     }
 946
 947     switch (cam_pixfmt)
 948     {
 949         case V4L2_PIX_FMT_UYVY:
 950             vip_ctrl_val |= SENSOR_UYVY;
 951             break;
 952         case V4L2_PIX_FMT_YUYV:
 953             vip_ctrl_val |= SENSOR_YUYV;
 954             break;
 955         default :
 956             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & SENSOR_YUYV);
 957             break;
 958     }
 959
 960     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 */
 961
 962     read_vip_reg(RK29_VIP_INT_STS);                     /* clear vip interrupte single  */
 963
 964     if (vip_ctrl_val & ONEFRAME)  {
 965         vip_crop = ((rect->left<<16) + rect->top);
 966         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
 967     } else if (vip_ctrl_val & PING_PONG) {
 968         BUG();
 969     }
 970
 971     write_vip_reg(RK29_VIP_CROP, vip_crop);
 972     write_vip_reg(RK29_VIP_FS, vip_fs);
 973
 974     write_vip_reg(RK29_VIP_FB_SR,  0x00000003);
 975
 976         pcdev->host_width = rect->width;
 977         pcdev->host_height = rect->height;
 978
 979     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));
 980         return;
 981 }
 982
 983 static int rk29_camera_get_formats(struct soc_camera_device *icd, int idx,
 984                                   struct soc_camera_format_xlate *xlate)
 985 {
 986     struct device *dev = icd->dev.parent;
 987     int formats = 0, buswidth, ret;
 988
 989     buswidth = 8;
 990
 991     ret = rk29_camera_try_bus_param(icd, buswidth);
 992     if (ret < 0)
 993         return 0;
 994
 995     switch (icd->formats[idx].fourcc) {
 996         case V4L2_PIX_FMT_UYVY:
 997         case V4L2_PIX_FMT_YUYV:
 998             formats++;
 999             if (xlate) {
1000                 xlate->host_fmt = &rk29_camera_formats[0];
1001                 xlate->cam_fmt = icd->formats + idx;
1002                 xlate->buswidth = buswidth;
1003                 xlate++;
1004                 dev_dbg(dev, "Providing format %s using %s\n",
1005                         rk29_camera_formats[0].name,
1006                         icd->formats[idx].name);
1007             }
1008
1009             formats++;
1010             if (xlate) {
1011                 xlate->host_fmt = &rk29_camera_formats[1];
1012                 xlate->cam_fmt = icd->formats + idx;
1013                 xlate->buswidth = buswidth;
1014                 xlate++;
1015                 dev_dbg(dev, "Providing format %s using %s\n",
1016                         rk29_camera_formats[1].name,
1017                         icd->formats[idx].name);
1018             }
1019                         break;
1020                 case V4L2_PIX_FMT_SGRBG10:
1021                         formats++;
1022             if (xlate) {
1023                 xlate->host_fmt = &rk29_camera_formats[2];
1024                 xlate->cam_fmt = icd->formats + idx;
1025                 xlate->buswidth = 10;
1026                 xlate++;
1027                 dev_dbg(dev, "Providing format %s using %s\n",
1028                         rk29_camera_formats[2].name,
1029                         icd->formats[idx].name);
1030             }
1031                         break;
1032         default:
1033             break;
1034     }
1035
1036     return formats;
1037 }
1038
1039 static void rk29_camera_put_formats(struct soc_camera_device *icd)
1040 {
1041         return;
1042 }
1043
1044 static int rk29_camera_set_crop(struct soc_camera_device *icd,
1045                                struct v4l2_crop *a)
1046 {
1047     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1048     struct v4l2_format f;
1049     struct v4l2_pix_format *pix = &f.fmt.pix;
1050     int ret;
1051
1052     f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1053
1054     ret = v4l2_subdev_call(sd, video, g_fmt, &f);
1055     if (ret < 0)
1056         return ret;
1057
1058     if ((pix->width < (a->c.left + a->c.width)) || (pix->height < (a->c.top + a->c.height)))  {
1059
1060         pix->width = a->c.left + a->c.width;
1061         pix->height = a->c.top + a->c.height;
1062
1063         v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1064             &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1065             icd->current_fmt->fourcc == V4L2_PIX_FMT_YUV422P ?4 : 0);
1066
1067         ret = v4l2_subdev_call(sd, video, s_fmt, &f);
1068         if (ret < 0)
1069             return ret;
1070     }
1071
1072     rk29_camera_setup_format(icd, icd->current_fmt->fourcc, pix->pixelformat, &a->c);
1073
1074     icd->user_width = pix->width;
1075     icd->user_height = pix->height;
1076
1077     return 0;
1078 }
1079
1080 static int rk29_camera_set_fmt(struct soc_camera_device *icd,
1081                               struct v4l2_format *f)
1082 {
1083     struct device *dev = icd->dev.parent;
1084     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1085     const struct soc_camera_data_format *cam_fmt = NULL;
1086     const struct soc_camera_format_xlate *xlate = NULL;
1087         struct soc_camera_host *ici =to_soc_camera_host(icd->dev.parent);
1088     struct rk29_camera_dev *pcdev = ici->priv;
1089     struct v4l2_pix_format *pix = &f->fmt.pix;
1090     struct v4l2_format cam_f = *f;
1091     struct v4l2_rect rect;
1092     int ret,usr_w,usr_h;
1093
1094         usr_w = pix->width;
1095         usr_h = pix->height;
1096     RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1097
1098     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1099     if (!xlate) {
1100         dev_err(dev, "Format %x not found\n", pix->pixelformat);
1101         ret = -EINVAL;
1102         goto RK29_CAMERA_SET_FMT_END;
1103     }
1104
1105     cam_fmt = xlate->cam_fmt;
1106
1107     cam_f.fmt.pix.pixelformat = cam_fmt->fourcc;
1108     ret = v4l2_subdev_call(sd, video, s_fmt, &cam_f);
1109     cam_f.fmt.pix.pixelformat = pix->pixelformat;
1110     *pix = cam_f.fmt.pix;
1111         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1112         if ((pix->width != usr_w) || (pix->height != usr_h)) {
1113                 pix->width = usr_w;
1114                 pix->height = usr_h;
1115         }
1116         #endif
1117     icd->sense = NULL;
1118
1119     if (!ret) {
1120         rect.left = 0;
1121         rect.top = 0;
1122         rect.width = cam_f.fmt.pix.width;
1123         rect.height = cam_f.fmt.pix.height;
1124
1125         RK29CAMERA_DG("%s..%s..%s icd width:%d  host width:%d \n",__FUNCTION__,xlate->host_fmt->name, cam_fmt->name,
1126                                    rect.width, pix->width);
1127         rk29_camera_setup_format(icd, pix->pixelformat, cam_fmt->fourcc, &rect);
1128         icd->buswidth = xlate->buswidth;
1129         icd->current_fmt = xlate->host_fmt;
1130
1131                 if (((pcdev->host_width != pcdev->icd->user_width) || (pcdev->host_height != pcdev->icd->user_height))) {
1132                         BUG_ON(pcdev->vipmem_phybase == 0);
1133                 }
1134     }
1135
1136 RK29_CAMERA_SET_FMT_END:
1137         if (ret)
1138         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1139     return ret;
1140 }
1141 static bool rk29_camera_fmt_capturechk(struct v4l2_format *f)
1142 {
1143     bool ret = false;
1144
1145         if ((f->fmt.pix.width == 1024) && (f->fmt.pix.height == 768)) {
1146                 ret = true;
1147         } else if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 1024)) {
1148                 ret = true;
1149         } else if ((f->fmt.pix.width == 1600) && (f->fmt.pix.height == 1200)) {
1150                 ret = true;
1151         } else if ((f->fmt.pix.width == 2048) && (f->fmt.pix.height == 1536)) {
1152                 ret = true;
1153         } else if ((f->fmt.pix.width == 2592) && (f->fmt.pix.height == 1944)) {
1154                 ret = true;
1155         }
1156
1157         if (ret == true)
1158                 RK29CAMERA_DG("%s %dx%d is capture format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1159         return ret;
1160 }
1161 static int rk29_camera_try_fmt(struct soc_camera_device *icd,
1162                                    struct v4l2_format *f)
1163 {
1164     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1165         struct rk29_camera_dev *pcdev = ici->priv;
1166     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1167     const struct soc_camera_format_xlate *xlate;
1168     struct v4l2_pix_format *pix = &f->fmt.pix;
1169     __u32 pixfmt = pix->pixelformat;
1170     enum v4l2_field field;
1171     int ret,usr_w,usr_h;
1172         bool is_capture = rk29_camera_fmt_capturechk(f);
1173         bool vipmem_is_overflow = false;
1174
1175         usr_w = pix->width;
1176         usr_h = pix->height;
1177         RK29CAMERA_DG("%s enter width:%d  height:%d\n",__FUNCTION__,usr_w,usr_h);
1178
1179     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1180     if (!xlate) {
1181         dev_err(ici->v4l2_dev.dev, "Format %x not found\n", pixfmt);
1182         ret = -EINVAL;
1183         goto RK29_CAMERA_TRY_FMT_END;
1184     }
1185    /* limit to rk29 hardware capabilities */
1186     v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1187               &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1188               pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
1189
1190     pix->bytesperline = pix->width * DIV_ROUND_UP(xlate->host_fmt->depth, 8);
1191     pix->sizeimage = pix->height * pix->bytesperline;
1192
1193     /* camera has to see its format, but the user the original one */
1194     pix->pixelformat = xlate->cam_fmt->fourcc;
1195     /* limit to sensor capabilities */
1196     ret = v4l2_subdev_call(sd, video, try_fmt, f);
1197     pix->pixelformat = pixfmt;
1198         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1199         if ((pix->width > usr_w) && (pix->height > usr_h)) {
1200                 if (is_capture) {
1201                         vipmem_is_overflow = (PAGE_ALIGN((pix->width*pix->height*icd->current_fmt->depth+7)>>3) > pcdev->vipmem_size);
1202                 } else {
1203                         /* Assume preview buffer minimum is 4 */
1204                         vipmem_is_overflow = (PAGE_ALIGN((pix->width*pix->height*icd->current_fmt->depth+7)>>3)*4 > pcdev->vipmem_size);
1205                 }
1206                 if (vipmem_is_overflow == false) {
1207                         pix->width = usr_w;
1208                         pix->height = usr_h;
1209                 } else {
1210                         RK29CAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",pix->width,pix->height,usr_w,usr_h);
1211                 }
1212         } else if ((pix->width < usr_w) && (pix->height < usr_h)) {
1213                 if (((usr_w>>1) < pix->width) && ((usr_h>>1) < pix->height)) {
1214                         if (is_capture) {
1215                                 vipmem_is_overflow = (PAGE_ALIGN((pix->width*pix->height*icd->current_fmt->depth+7)>>3) <= pcdev->vipmem_size);
1216                         } else {
1217                                 vipmem_is_overflow = (PAGE_ALIGN((pix->width*pix->height*icd->current_fmt->depth+7)>>3)*4 <= pcdev->vipmem_size);
1218                         }
1219                         if (vipmem_is_overflow == false) {
1220                                 pix->width = usr_w;
1221                                 pix->height = usr_h;
1222                         } else {
1223                                 RK29CAMERA_TR("vipmem for IPP is overflow, This resolution(%dx%d -> %dx%d) is invalidate!\n",pix->width,pix->height,usr_w,usr_h);
1224                         }
1225                 } else {
1226                         RK29CAMERA_TR("The aspect ratio(%dx%d/%dx%d) is bigger than 2 !\n",pix->width,pix->height,usr_w,usr_h);
1227                 }
1228         }
1229         #endif
1230
1231     field = pix->field;
1232
1233     if (field == V4L2_FIELD_ANY) {
1234         pix->field = V4L2_FIELD_NONE;
1235     } else if (field != V4L2_FIELD_NONE) {
1236         dev_err(icd->dev.parent, "Field type %d unsupported.\n", field);
1237         ret = -EINVAL;
1238         goto RK29_CAMERA_TRY_FMT_END;
1239     }
1240
1241 RK29_CAMERA_TRY_FMT_END:
1242         if (ret)
1243         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1244     return ret;
1245 }
1246
1247 static int rk29_camera_reqbufs(struct soc_camera_file *icf,
1248                                struct v4l2_requestbuffers *p)
1249 {
1250     int i;
1251
1252     /* This is for locking debugging only. I removed spinlocks and now I
1253      * check whether .prepare is ever called on a linked buffer, or whether
1254      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1255      * it hadn't triggered */
1256     for (i = 0; i < p->count; i++) {
1257         struct rk29_buffer *buf = container_of(icf->vb_vidq.bufs[i],
1258                                                            struct rk29_buffer, vb);
1259         buf->inwork = 0;
1260         INIT_LIST_HEAD(&buf->vb.queue);
1261     }
1262
1263     return 0;
1264 }
1265
1266 static unsigned int rk29_camera_poll(struct file *file, poll_table *pt)
1267 {
1268     struct soc_camera_file *icf = file->private_data;
1269     struct rk29_buffer *buf;
1270
1271     buf = list_entry(icf->vb_vidq.stream.next, struct rk29_buffer,
1272                      vb.stream);
1273
1274     poll_wait(file, &buf->vb.done, pt);
1275
1276     if (buf->vb.state == VIDEOBUF_DONE ||
1277             buf->vb.state == VIDEOBUF_ERROR)
1278         return POLLIN|POLLRDNORM;
1279
1280     return 0;
1281 }
1282
1283 static int rk29_camera_querycap(struct soc_camera_host *ici,
1284                                 struct v4l2_capability *cap)
1285 {
1286     /* cap->name is set by the firendly caller:-> */
1287     strlcpy(cap->card, rk29_cam_driver_description, sizeof(cap->card));
1288     cap->version = RK29_CAM_VERSION_CODE;
1289     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1290
1291     return 0;
1292 }
1293
1294 static int rk29_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1295 {
1296     struct soc_camera_host *ici =
1297                     to_soc_camera_host(icd->dev.parent);
1298     struct rk29_camera_dev *pcdev = ici->priv;
1299         struct v4l2_subdev *sd;
1300     int ret = 0,tmp;
1301
1302         mutex_lock(&camera_lock);
1303         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1304                 rk29_camera_s_stream(icd, 0);
1305                 sd = soc_camera_to_subdev(icd);
1306                 v4l2_subdev_call(sd, video, s_stream, 0);
1307                 ret = icd->ops->suspend(icd, state);
1308
1309                 pcdev->reginfo_suspend.VipCtrl = read_vip_reg(RK29_VIP_CTRL);
1310                 pcdev->reginfo_suspend.VipCrop = read_vip_reg(RK29_VIP_CROP);
1311                 pcdev->reginfo_suspend.VipFs = read_vip_reg(RK29_VIP_FS);
1312                 pcdev->reginfo_suspend.VipIntMsk = read_vip_reg(RK29_VIP_INT_MASK);
1313                 pcdev->reginfo_suspend.VipCrm = read_vip_reg(RK29_VIP_CRM);
1314
1315                 tmp = pcdev->reginfo_suspend.VipFs>>16;         /* ddl@rock-chips.com */
1316                 tmp += pcdev->reginfo_suspend.VipCrop>>16;
1317                 pcdev->reginfo_suspend.VipFs = (pcdev->reginfo_suspend.VipFs & 0xffff) | (tmp<<16);
1318
1319                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1320                 rk29_camera_deactivate(pcdev);
1321
1322                 RK29CAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1323         } else {
1324                 RK29CAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1325         }
1326         mutex_unlock(&camera_lock);
1327     return ret;
1328 }
1329
1330 static int rk29_camera_resume(struct soc_camera_device *icd)
1331 {
1332     struct soc_camera_host *ici =
1333                     to_soc_camera_host(icd->dev.parent);
1334     struct rk29_camera_dev *pcdev = ici->priv;
1335         struct v4l2_subdev *sd;
1336     int ret = 0;
1337
1338         mutex_lock(&camera_lock);
1339         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1340                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1341                         rk29_camera_activate(pcdev, icd);
1342                         write_vip_reg(RK29_VIP_INT_MASK, pcdev->reginfo_suspend.VipIntMsk);
1343                         write_vip_reg(RK29_VIP_CRM, pcdev->reginfo_suspend.VipCrm);
1344                         write_vip_reg(RK29_VIP_CTRL, pcdev->reginfo_suspend.VipCtrl&~ENABLE_CAPTURE);
1345                         write_vip_reg(RK29_VIP_CROP, pcdev->reginfo_suspend.VipCrop);
1346                         write_vip_reg(RK29_VIP_FS, pcdev->reginfo_suspend.VipFs);
1347
1348                         rk29_videobuf_capture(pcdev->active);
1349                         rk29_camera_s_stream(icd, 1);
1350                         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
1351                 } else {
1352                         RK29CAMERA_TR("Resume fail, vip register recored is invalidate!!\n");
1353                         goto rk29_camera_resume_end;
1354                 }
1355
1356                 ret = icd->ops->resume(icd);
1357                 sd = soc_camera_to_subdev(icd);
1358                 v4l2_subdev_call(sd, video, s_stream, 1);
1359
1360                 RK29CAMERA_DG("%s Enter success\n",__FUNCTION__);
1361         } else {
1362                 RK29CAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1363         }
1364
1365 rk29_camera_resume_end:
1366         mutex_unlock(&camera_lock);
1367     return ret;
1368 }
1369
1370 static void rk29_camera_reinit_work(struct work_struct *work)
1371 {
1372         struct device *control;
1373     struct v4l2_subdev *sd;
1374         struct v4l2_format cam_f;
1375         const struct soc_camera_format_xlate *xlate;
1376         int ret;
1377
1378         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)&(~ENABLE_CAPTURE)));
1379
1380         control = to_soc_camera_control(rk29_camdev_info_ptr->icd);
1381         sd = dev_get_drvdata(control);
1382         ret = v4l2_subdev_call(sd,core, init, 1);
1383
1384         cam_f.fmt.pix.width = rk29_camdev_info_ptr->icd->user_width;
1385         cam_f.fmt.pix.height = rk29_camdev_info_ptr->icd->user_height;
1386         xlate = soc_camera_xlate_by_fourcc(rk29_camdev_info_ptr->icd, rk29_camdev_info_ptr->icd->current_fmt->fourcc);
1387         cam_f.fmt.pix.pixelformat = xlate->cam_fmt->fourcc;
1388         ret |= v4l2_subdev_call(sd, video, s_fmt, &cam_f);
1389
1390         write_vip_reg(RK29_VIP_CTRL, (read_vip_reg(RK29_VIP_CTRL)|ENABLE_CAPTURE));
1391
1392         RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor now! ret:0x%x\n",ret);
1393 }
1394 static enum hrtimer_restart rk29_camera_fps_func(struct hrtimer *timer)
1395 {
1396         RK29CAMERA_DG("rk29_camera_fps_func fps:0x%x\n",rk29_camdev_info_ptr->fps);
1397         if (rk29_camdev_info_ptr->fps < 3) {
1398                 RK29CAMERA_TR("Camera host haven't recevie data from sensor,Reinit sensor delay!\n");
1399                 INIT_WORK(&rk29_camdev_info_ptr->camera_reinit_work, rk29_camera_reinit_work);
1400                 queue_work(rk29_camdev_info_ptr->camera_wq,&(rk29_camdev_info_ptr->camera_reinit_work));
1401         }
1402
1403         return HRTIMER_NORESTART;
1404 }
1405 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable)
1406 {
1407         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1408     struct rk29_camera_dev *pcdev = ici->priv;
1409     int vip_ctrl_val;
1410         int ret;
1411
1412         WARN_ON(pcdev->icd != icd);
1413
1414         vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1415         if (enable) {
1416                 pcdev->fps = 0;
1417                 hrtimer_cancel(&pcdev->fps_timer);
1418                 hrtimer_start(&pcdev->fps_timer,ktime_set(2, 0),HRTIMER_MODE_REL);
1419                 vip_ctrl_val |= ENABLE_CAPTURE;
1420         } else {
1421         vip_ctrl_val &= ~ENABLE_CAPTURE;
1422                 ret = hrtimer_cancel(&pcdev->fps_timer);
1423                 ret |= flush_work(&rk29_camdev_info_ptr->camera_reinit_work);
1424                 RK29CAMERA_DG("STREAM_OFF cancel timer and flush work:0x%x \n", ret);
1425         }
1426         write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1427
1428         RK29CAMERA_DG("%s.. enable : 0x%x \n", __FUNCTION__, enable);
1429         return 0;
1430 }
1431
1432 static struct soc_camera_host_ops rk29_soc_camera_host_ops =
1433 {
1434     .owner              = THIS_MODULE,
1435     .add                = rk29_camera_add_device,
1436     .remove             = rk29_camera_remove_device,
1437     .suspend    = rk29_camera_suspend,
1438     .resume             = rk29_camera_resume,
1439     .set_crop   = rk29_camera_set_crop,
1440     .get_formats        = rk29_camera_get_formats,
1441     .put_formats        = rk29_camera_put_formats,
1442     .set_fmt    = rk29_camera_set_fmt,
1443     .try_fmt    = rk29_camera_try_fmt,
1444     .init_videobuf      = rk29_camera_init_videobuf,
1445     .reqbufs    = rk29_camera_reqbufs,
1446     .poll               = rk29_camera_poll,
1447     .querycap   = rk29_camera_querycap,
1448     .set_bus_param      = rk29_camera_set_bus_param,
1449     .s_stream = rk29_camera_s_stream   /* ddl@rock-chips.com : Add stream control for host */
1450 };
1451 static int rk29_camera_probe(struct platform_device *pdev)
1452 {
1453     struct rk29_camera_dev *pcdev;
1454     struct resource *res;
1455     int irq;
1456     int err = 0;
1457
1458     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1459     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1460     irq = platform_get_irq(pdev, 0);
1461     if (!res || irq < 0) {
1462         err = -ENODEV;
1463         goto exit;
1464     }
1465
1466     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1467     if (!pcdev) {
1468         dev_err(&pdev->dev, "Could not allocate pcdev\n");
1469         err = -ENOMEM;
1470         goto exit_alloc;
1471     }
1472     rk29_camdev_info_ptr = pcdev;
1473
1474     /*config output clk*/
1475         pcdev->aclk_ddr_lcdc = clk_get(&pdev->dev, "aclk_ddr_lcdc");
1476         pcdev->aclk_disp_matrix = clk_get(&pdev->dev, "aclk_disp_matrix");
1477
1478         pcdev->hclk_cpu_display = clk_get(&pdev->dev, "hclk_cpu_display");
1479         pcdev->vip_slave = clk_get(&pdev->dev, "vip_slave");
1480         pcdev->vip_out = clk_get(&pdev->dev,"vip_out");
1481         pcdev->vip_input = clk_get(&pdev->dev,"vip_input");
1482         pcdev->vip_bus = clk_get(&pdev->dev, "vip_bus");
1483
1484         pcdev->hclk_disp_matrix = clk_get(&pdev->dev,"hclk_disp_matrix");
1485         pcdev->vip_matrix = clk_get(&pdev->dev,"vip_matrix");
1486
1487     if (!pcdev->aclk_ddr_lcdc || !pcdev->aclk_disp_matrix ||  !pcdev->hclk_cpu_display ||
1488                 !pcdev->vip_slave || !pcdev->vip_out || !pcdev->vip_input || !pcdev->vip_bus ||
1489                 IS_ERR(pcdev->aclk_ddr_lcdc) || IS_ERR(pcdev->aclk_disp_matrix) ||  IS_ERR(pcdev->hclk_cpu_display) ||
1490                 IS_ERR(pcdev->vip_slave) || IS_ERR(pcdev->vip_out) || IS_ERR(pcdev->vip_input) || IS_ERR(pcdev->vip_bus))  {
1491
1492         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(axi) source\n");
1493         err = -ENOENT;
1494         goto exit_reqmem;
1495     }
1496
1497         if (!pcdev->hclk_disp_matrix || !pcdev->vip_matrix ||
1498                 IS_ERR(pcdev->hclk_disp_matrix) || IS_ERR(pcdev->vip_matrix))  {
1499
1500         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk(ahb) source\n");
1501         err = -ENOENT;
1502         goto exit_reqmem;
1503     }
1504
1505     dev_set_drvdata(&pdev->dev, pcdev);
1506     pcdev->res = res;
1507
1508     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
1509     if (pcdev->pdata && pcdev->pdata->io_init) {
1510         pcdev->pdata->io_init();
1511     }
1512         #ifdef CONFIG_VIDEO_RK29_WORK_IPP
1513         if (pcdev->pdata && (strcmp(pcdev->pdata->meminfo.name,"camera_ipp_mem")==0)) {
1514                 pcdev->vipmem_phybase = pcdev->pdata->meminfo.start;
1515                 pcdev->vipmem_size = pcdev->pdata->meminfo.size;
1516                 RK29CAMERA_DG("\n%s Memory(start:0x%x size:0x%x) for IPP obtain \n",__FUNCTION__, pcdev->pdata->meminfo.start,pcdev->pdata->meminfo.size);
1517         } else {
1518                 RK29CAMERA_TR("\n%s Memory for IPP have not obtain! IPP Function is fail\n",__FUNCTION__);
1519                 pcdev->vipmem_phybase = 0;
1520                 pcdev->vipmem_size = 0;
1521         }
1522         #endif
1523     INIT_LIST_HEAD(&pcdev->capture);
1524     spin_lock_init(&pcdev->lock);
1525
1526     /*
1527      * Request the regions.
1528      */
1529     if (!request_mem_region(res->start, res->end - res->start + 1,
1530                             RK29_CAM_DRV_NAME)) {
1531         err = -EBUSY;
1532         goto exit_reqmem;
1533     }
1534
1535     pcdev->base = ioremap(res->start, res->end - res->start + 1);
1536     if (pcdev->base == NULL) {
1537         dev_err(pcdev->dev, "ioremap() of registers failed\n");
1538         err = -ENXIO;
1539         goto exit_ioremap;
1540     }
1541
1542     pcdev->irq = irq;
1543     pcdev->dev = &pdev->dev;
1544
1545     /* config buffer address */
1546     /* request irq */
1547     err = request_irq(pcdev->irq, rk29_camera_irq, 0, RK29_CAM_DRV_NAME,
1548                       pcdev);
1549     if (err) {
1550         dev_err(pcdev->dev, "Camera interrupt register failed \n");
1551         goto exit_reqirq;
1552     }
1553
1554         pcdev->camera_wq = create_workqueue("camera wq");
1555         if (pcdev->camera_wq == NULL)
1556                 goto exit_free_irq;
1557         INIT_WORK(&pcdev->camera_reinit_work, rk29_camera_reinit_work);
1558
1559     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
1560     pcdev->soc_host.ops         = &rk29_soc_camera_host_ops;
1561     pcdev->soc_host.priv                = pcdev;
1562     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
1563     pcdev->soc_host.nr          = pdev->id;
1564
1565     err = soc_camera_host_register(&pcdev->soc_host);
1566     if (err)
1567         goto exit_free_irq;
1568
1569         hrtimer_init(&pcdev->fps_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1570         pcdev->fps_timer.function = rk29_camera_fps_func;
1571
1572     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1573     return 0;
1574
1575 exit_free_irq:
1576     free_irq(pcdev->irq, pcdev);
1577         if (pcdev->camera_wq) {
1578                 destroy_workqueue(pcdev->camera_wq);
1579                 pcdev->camera_wq = NULL;
1580         }
1581 exit_reqirq:
1582     iounmap(pcdev->base);
1583 exit_ioremap:
1584     release_mem_region(res->start, res->end - res->start + 1);
1585 exit_reqmem:
1586     if (pcdev->aclk_ddr_lcdc) {
1587                 clk_put(pcdev->aclk_ddr_lcdc);
1588                 pcdev->aclk_ddr_lcdc = NULL;
1589     }
1590         if (pcdev->aclk_disp_matrix) {
1591                 clk_put(pcdev->aclk_disp_matrix);
1592                 pcdev->aclk_disp_matrix = NULL;
1593     }
1594         if (pcdev->hclk_cpu_display) {
1595                 clk_put(pcdev->hclk_cpu_display);
1596                 pcdev->hclk_cpu_display = NULL;
1597     }
1598         if (pcdev->vip_slave) {
1599                 clk_put(pcdev->vip_slave);
1600                 pcdev->vip_slave = NULL;
1601     }
1602         if (pcdev->vip_out) {
1603                 clk_put(pcdev->vip_out);
1604                 pcdev->vip_out = NULL;
1605     }
1606         if (pcdev->vip_input) {
1607                 clk_put(pcdev->vip_input);
1608                 pcdev->vip_input = NULL;
1609     }
1610         if (pcdev->vip_bus) {
1611                 clk_put(pcdev->vip_bus);
1612                 pcdev->vip_bus = NULL;
1613     }
1614     if (pcdev->hclk_disp_matrix) {
1615                 clk_put(pcdev->hclk_disp_matrix);
1616                 pcdev->hclk_disp_matrix = NULL;
1617     }
1618         if (pcdev->vip_matrix) {
1619                 clk_put(pcdev->vip_matrix);
1620                 pcdev->vip_matrix = NULL;
1621     }
1622     kfree(pcdev);
1623 exit_alloc:
1624     rk29_camdev_info_ptr = NULL;
1625 exit:
1626     return err;
1627 }
1628
1629 static int __devexit rk29_camera_remove(struct platform_device *pdev)
1630 {
1631     struct rk29_camera_dev *pcdev = platform_get_drvdata(pdev);
1632     struct resource *res;
1633
1634     free_irq(pcdev->irq, pcdev);
1635
1636         if (pcdev->camera_wq) {
1637                 destroy_workqueue(pcdev->camera_wq);
1638                 pcdev->camera_wq = NULL;
1639         }
1640
1641     soc_camera_host_unregister(&pcdev->soc_host);
1642
1643     res = pcdev->res;
1644     release_mem_region(res->start, res->end - res->start + 1);
1645
1646     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
1647         pcdev->pdata->io_deinit(0);
1648                 pcdev->pdata->io_deinit(1);
1649     }
1650
1651     kfree(pcdev);
1652     rk29_camdev_info_ptr = NULL;
1653     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
1654
1655     return 0;
1656 }
1657
1658 static struct platform_driver rk29_camera_driver =
1659 {
1660     .driver     = {
1661         .name   = RK29_CAM_DRV_NAME,
1662     },
1663     .probe              = rk29_camera_probe,
1664     .remove             = __devexit_p(rk29_camera_remove),
1665 };
1666
1667
1668 static int __devinit rk29_camera_init(void)
1669 {
1670     RK29CAMERA_DG("%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1671     return platform_driver_register(&rk29_camera_driver);
1672 }
1673
1674 static void __exit rk29_camera_exit(void)
1675 {
1676     platform_driver_unregister(&rk29_camera_driver);
1677 }
1678
1679 device_initcall_sync(rk29_camera_init);
1680 module_exit(rk29_camera_exit);
1681
1682 MODULE_DESCRIPTION("RK29 Soc Camera Host driver");
1683 MODULE_AUTHOR("ddl <ddl@rock-chips>");
1684 MODULE_LICENSE("GPL");