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