drivers/media/video/rk29_camera_oneframe.c

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