drivers/media/video/rk29_camera_pingpong.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
  33 #include <mach/rk29_camera.h>
  34 #include <mach/rk29_iomap.h>
  35 #include <mach/iomux.h>
  36
  37 #include <media/v4l2-common.h>
  38 #include <media/v4l2-dev.h>
  39 #include <media/videobuf-dma-contig.h>
  40 #include <media/soc_camera.h>
  41
  42 // VIP Reg Offset
  43 #define RK29_VIP_AHBR_CTRL                0x00
  44 #define RK29_VIP_INT_MASK                 0x04
  45 #define RK29_VIP_INT_STS                  0x08
  46 #define RK29_VIP_STS                      0x0c
  47 #define RK29_VIP_CTRL                     0x10
  48 #define RK29_VIP_CAPTURE_F1SA_Y           0x14
  49 #define RK29_VIP_CAPTURE_F1SA_UV          0x18
  50 #define RK29_VIP_CAPTURE_F1SA_Cr          0x1c
  51 #define RK29_VIP_CAPTURE_F2SA_Y           0x20
  52 #define RK29_VIP_CAPTURE_F2SA_UV          0x24
  53 #define RK29_VIP_CAPTURE_F2SA_Cr          0x28
  54 #define RK29_VIP_FB_SR                    0x2c
  55 #define RK29_VIP_FS                       0x30
  56 #define RK29_VIP_VIPRESERVED              0x34
  57 #define RK29_VIP_CROP                     0x38
  58 #define RK29_VIP_CRM                      0x3c
  59 #define RK29_VIP_RESET                    0x40
  60 #define RK29_VIP_L_SFT                    0x44
  61
  62 //The key register bit descrition
  63 // VIP_CTRL Reg
  64 #define  DISABLE_CAPTURE              (0x00<<0)
  65 #define  ENABLE_CAPTURE               (0x01<<0)
  66 #define  HSY_HIGH_ACTIVE              (0x00<<1)
  67 #define  HSY_LOW_ACTIVE               (0x01<<1)
  68 #define  VIP_CCIR656                  (0x00<<2)
  69 #define  VIP_SENSOR                   (0x01<<2)
  70 #define  SENSOR_UYVY                  (0x00<<3)
  71 #define  SENSOR_YUYV                  (0x01<<3)
  72 #define  VIP_YUV                      (0x00<<4)
  73 #define  VIP_RAW                      (0x01<<4)
  74 #define  CON_OR_PIN                   (0x00<<5)
  75 #define  ONEFRAME                     (0x01<<5)
  76 #define  VIPREGYUV420                 (0x00<<6)
  77 #define  VIPREGYUV422                 (0x01<<6)
  78 #define  FIELD0_START                 (0x00<<7)
  79 #define  FIELD1_START                 (0x01<<7)
  80 #define  CONTINUOUS                   (0x00<<8)
  81 #define  PING_PONG                    (0x01<<8)
  82 #define  POSITIVE_EDGE                (0x00<<9)
  83 #define  NEGATIVE_EDGE                (0x01<<9)
  84 #define  VIPREGNTSC                   (0x00<<10)
  85 #define  VIPREGPAL                    (0x01<<10)
  86 #define  VIP_DATA_LITTLEEND           (0x00<<11)
  87 #define  VIP_DATA_BIGEND              (0x01<<11)
  88 #define  VSY_LOW_ACTIVE               (0x00<<12)
  89 #define  VSY_HIGH_ACTIVE              (0x01<<12)
  90 #define  VIP_RAWINPUT_BYPASS          (0x00<<13)
  91 #define  VIP_RAWINPUT_POSITIVE_EDGE   (0x01<<13)
  92 #define  VIP_RAWINPUT_NEGATIVE_EDGE   (0x02<<13)
  93
  94 //VIP_FB_SR
  95 #define  VIP_F1_NOREADY               (0x00<<0)
  96 #define  VIP_F1_READY                 (0x01<<0)
  97 #define  VIP_F2_NOREADY               (0x00<<1)
  98 #define  VIP_F2_READY                 (0x01<<1)
  99 #define  VIP_FRAME_NOLOSS             (0x00<<2)
 100 #define  VIP_FRAME_LOSS               (0x01<<2)
 101 #define  VIP_F1_IS_LATEST             (0x00<<3)
 102 #define  VIP_F2_IS_LATEST             (0x01<<3)
 103
 104 // GRF_SOC_CON0 Reg
 105 #define  GRF_SOC_CON0_Reg             0xbc
 106 #define  VIP_AXIMASTER                (0x00<<0)
 107 #define  VIP_AHBMASTER                (0x01<<2)
 108
 109 // GRF_OS_REG0
 110 #define  GRF_OS_REG0                  0xd0
 111 #define  VIP_ACLK_DIV_HCLK_1          (0x00<<0)
 112 #define  VIP_ACLK_DIV_HCLK_2          (0x01<<0)
 113
 114
 115 #define MIN(x,y)   ((x<y) ? x: y)
 116 #define MAX(x,y)    ((x>y) ? x: y)
 117 #define RK29_SENSOR_24MHZ      24           /* MHz */
 118 #define RK29_SENSOR_48MHZ      48
 119
 120 #define write_vip_reg(addr, val)  __raw_writel(val, addr+(rk29_camdev_info_ptr->base))
 121 #define read_vip_reg(addr) __raw_readl(addr+(rk29_camdev_info_ptr->base))
 122 #define mask_vip_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 123
 124 #define write_grf_reg(addr, val)  __raw_writel(val, addr+RK29_GRF_BASE)
 125 #define read_grf_reg(addr) __raw_readl(addr+RK29_GRF_BASE)
 126 #define mask_grf_reg(addr, msk, val)    write_vip_reg(addr, (val)|((~(msk))&read_vip_reg(addr)))
 127
 128 //Configure Macro
 129 #define CONFIG_RK29CAMERA_TR      1
 130 #define CONFIG_RK29CAMERA_DEBUG   1
 131 #if (CONFIG_RK29CAMERA_TR)
 132         #define RK29CAMERA_TR(format, ...)      printk(format, ## __VA_ARGS__)
 133         #if (CONFIG_RK29CAMERA_DEBUG)
 134         #define RK29CAMERA_DG(format, ...)      printk(format, ## __VA_ARGS__)
 135         #else
 136         #define RK29CAMERA_DG(format, ...)
 137         #endif
 138 #else
 139         #define RK29CAMERA_TR(format, ...)
 140         #define RK29CAMERA_DG(format, ...)
 141 #endif
 142
 143 #define RK29_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 1)
 144
 145 /* limit to rk29 hardware capabilities */
 146 #define RK29_CAM_BUS_PARAM   (SOCAM_MASTER |\
 147                 SOCAM_HSYNC_ACTIVE_HIGH |\
 148                 SOCAM_HSYNC_ACTIVE_LOW |\
 149                 SOCAM_VSYNC_ACTIVE_HIGH |\
 150                 SOCAM_VSYNC_ACTIVE_LOW |\
 151                 SOCAM_PCLK_SAMPLE_RISING |\
 152                 SOCAM_PCLK_SAMPLE_FALLING|\
 153                 SOCAM_DATA_ACTIVE_HIGH |\
 154                 SOCAM_DATA_ACTIVE_LOW|\
 155                 SOCAM_DATAWIDTH_8|SOCAM_DATAWIDTH_10|\
 156                 SOCAM_MCLK_24MHZ |SOCAM_MCLK_48MHZ)
 157
 158 #define RK29_CAM_W_MIN        48
 159 #define RK29_CAM_H_MIN        32
 160 #define RK29_CAM_W_MAX        3856            /* ddl@rock-chips.com : 10M Pixel */
 161 #define RK29_CAM_H_MAX        2764
 162 #define RK29_CAM_FRAME_INVAL_INIT 3
 163 #define RK29_CAM_FRAME_INVAL_DC 1             /* ddl@rock-chips.com :  */
 164
 165 // Variable and Function Declare
 166 extern void videobuf_dma_contig_free(struct videobuf_queue *q, struct videobuf_buffer *buf);
 167 extern dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
 168 extern void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 169             struct videobuf_queue_ops *ops,
 170             struct device *dev,
 171             spinlock_t *irqlock,
 172             enum v4l2_buf_type type,
 173             enum v4l2_field field,
 174             unsigned int msize,
 175             void *priv);
 176
 177 /* buffer for one video frame */
 178 struct rk29_buffer
 179 {
 180     /* common v4l buffer stuff -- must be first */
 181     struct videobuf_buffer vb;
 182     const struct soc_camera_data_format        *fmt;
 183     int                 inwork;
 184 };
 185 enum rk29_camera_reg_state
 186 {
 187         Reg_Invalidate,
 188         Reg_Validate
 189 };
 190
 191 struct rk29_camera_reg
 192 {
 193         unsigned int VipCtrl;
 194         unsigned int VipCrop;
 195         unsigned int VipFs;
 196         unsigned int VipIntMsk;
 197         unsigned int VipCrm;
 198         enum rk29_camera_reg_state Inval;
 199 };
 200 struct rk29_camera_dev
 201 {
 202     struct soc_camera_host      soc_host;
 203     struct device               *dev;
 204     /* RK2827x is only supposed to handle one camera on its Quick Capture
 205      * interface. If anyone ever builds hardware to enable more than
 206      * one camera, they will have to modify this driver too */
 207     struct soc_camera_device *icd;
 208     struct clk *clk;
 209     void __iomem *base;
 210     int frame_inval;           /* ddl@rock-chips.com : The first frames is invalidate  */
 211     unsigned int irq;
 212
 213     struct rk29camera_platform_data *pdata;
 214     struct resource             *res;
 215
 216     struct list_head    capture;
 217
 218     spinlock_t          lock;
 219
 220     struct videobuf_buffer      *active1;
 221         struct videobuf_buffer  *active2;
 222         struct videobuf_queue *vb_vidq_ptr;
 223         struct rk29_camera_reg reginfo_suspend;
 224
 225         struct hrtimer frame_timer;
 226 };
 227 static DEFINE_MUTEX(camera_lock);
 228 static const char *rk29_cam_driver_description = "RK29_Camera";
 229 static struct rk29_camera_dev *rk29_camdev_info_ptr;
 230 static int rk29_first;
 231
 232 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable);
 233
 234
 235 /*
 236  *  Videobuf operations
 237  */
 238 static int rk29_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 239                                unsigned int *size)
 240 {
 241     struct soc_camera_device *icd = vq->priv_data;
 242     int bytes_per_pixel = (icd->current_fmt->depth + 7) >> 3;
 243
 244     dev_dbg(&icd->dev, "count=%d, size=%d\n", *count, *size);
 245
 246     /* planar capture requires Y, U and V buffers to be page aligned */
 247     *size = PAGE_ALIGN( icd->user_width * icd->user_height * bytes_per_pixel);                               /* Y pages UV pages, yuv422*/
 248
 249     RK29CAMERA_DG("\n%s..%d.. size = %d\n",__FUNCTION__,__LINE__, *size);
 250
 251     return 0;
 252 }
 253 static void rk29_videobuf_free(struct videobuf_queue *vq, struct rk29_buffer *buf)
 254 {
 255     struct soc_camera_device *icd = vq->priv_data;
 256
 257     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 258             &buf->vb, buf->vb.baddr, buf->vb.bsize);
 259
 260         /* ddl@rock-chips.com: buf_release called soc_camera_streamoff and soc_camera_close*/
 261         if (buf->vb.state == VIDEOBUF_NEEDS_INIT)
 262                 return;
 263
 264     if (in_interrupt())
 265         BUG();
 266
 267     videobuf_dma_contig_free(vq, &buf->vb);
 268     dev_dbg(&icd->dev, "%s freed\n", __func__);
 269     buf->vb.state = VIDEOBUF_NEEDS_INIT;
 270         return;
 271 }
 272 static int rk29_videobuf_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb, enum v4l2_field field)
 273 {
 274     struct soc_camera_device *icd = vq->priv_data;
 275     struct rk29_buffer *buf;
 276     int ret;
 277
 278     buf = container_of(vb, struct rk29_buffer, vb);
 279
 280     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 281             vb, vb->baddr, vb->bsize);
 282
 283     //RK29CAMERA_TR("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 284
 285     /* Added list head initialization on alloc */
 286     WARN_ON(!list_empty(&vb->queue));
 287
 288     /* This can be useful if you want to see if we actually fill
 289      * the buffer with something */
 290     //memset((void *)vb->baddr, 0xaa, vb->bsize);
 291
 292     BUG_ON(NULL == icd->current_fmt);
 293
 294     if (buf->fmt    != icd->current_fmt ||
 295             vb->width   != icd->user_width ||
 296             vb->height  != icd->user_height ||
 297              vb->field   != field) {
 298         buf->fmt    = icd->current_fmt;
 299         vb->width   = icd->user_width;
 300         vb->height  = icd->user_height;
 301         vb->field   = field;
 302         vb->state   = VIDEOBUF_NEEDS_INIT;
 303     }
 304
 305     vb->size = vb->width * vb->height * ((buf->fmt->depth + 7) >> 3) ;          /* ddl@rock-chips.com : fmt->depth is coorect */
 306     if (0 != vb->baddr && vb->bsize < vb->size) {
 307         ret = -EINVAL;
 308         goto out;
 309     }
 310
 311     if (vb->state == VIDEOBUF_NEEDS_INIT) {
 312         ret = videobuf_iolock(vq, vb, NULL);
 313         if (ret) {
 314             goto fail;
 315         }
 316         vb->state = VIDEOBUF_PREPARED;
 317     }
 318     return 0;
 319 fail:
 320     rk29_videobuf_free(vq, buf);
 321 out:
 322     return ret;
 323 }
 324
 325 static inline void rk29_videobuf_capture(struct videobuf_buffer *vb)
 326 {
 327     unsigned int size;
 328         int fb_sr;
 329         struct rk29_camera_dev *pcdev = rk29_camdev_info_ptr;
 330
 331         if (vb) {
 332                 size = vb->width * vb->height;    /* Y pages UV pages, yuv422*/
 333                 fb_sr = read_vip_reg(RK29_VIP_FB_SR);
 334                 if (fb_sr & VIP_F1_READY) {
 335                         write_vip_reg(RK29_VIP_CAPTURE_F1SA_Y, vb->boff);
 336                 write_vip_reg(RK29_VIP_CAPTURE_F1SA_UV, vb->boff + size);
 337                         write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F1_READY));
 338                         pcdev->active1 = vb;
 339                         RK29CAMERA_DG("f1 Start.. vb->boff:0x%x f2 vb->boff:0x%x FB_SR:0x%x, active1:0x%x, active2:0x%x\n",(unsigned int)vb->boff,read_vip_reg(RK29_VIP_CAPTURE_F2SA_Y), read_vip_reg(RK29_VIP_FB_SR)&0x03,
 340                                    (unsigned int)pcdev->active1,(unsigned int)pcdev->active2);
 341                 } else if (fb_sr & VIP_F2_READY) {
 342                         write_vip_reg(RK29_VIP_CAPTURE_F2SA_Y, vb->boff);
 343                 write_vip_reg(RK29_VIP_CAPTURE_F2SA_UV, vb->boff + size);
 344                         write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F2_READY));
 345                         pcdev->active2 = vb;
 346                         RK29CAMERA_DG("f2 Start..vb->boff:0x%x f1 vb->boff:0x%x FB_SR:0x%x, active1:0x%x, active2:0x%x\n",(unsigned int)vb->boff,read_vip_reg(RK29_VIP_CAPTURE_F1SA_Y), read_vip_reg(RK29_VIP_FB_SR)&0x03,
 347                                   (unsigned int)pcdev->active1,(unsigned int)pcdev->active2);
 348         }
 349         }
 350 }
 351 /* Locking: Caller holds q->irqlock */
 352 static void rk29_videobuf_queue(struct videobuf_queue *vq,
 353                                 struct videobuf_buffer *vb)
 354 {
 355     struct soc_camera_device *icd = vq->priv_data;
 356     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 357     struct rk29_camera_dev *pcdev = ici->priv;
 358         unsigned int size;
 359
 360     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 361             vb, vb->baddr, vb->bsize);
 362
 363     vb->state = VIDEOBUF_QUEUED;
 364
 365         if (!list_empty(&pcdev->capture)) {
 366                 list_add_tail(&vb->queue, &pcdev->capture);
 367         } else {
 368                 if (list_entry(pcdev->capture.next, struct videobuf_buffer, queue) != vb)
 369                         list_add_tail(&vb->queue, &pcdev->capture);
 370                 else
 371                         BUG();    /* ddl@rock-chips.com : The same videobuffer queue again */
 372         }
 373
 374     if (!pcdev->active1) {
 375         rk29_videobuf_capture(vb);
 376     }
 377 }
 378 static irqreturn_t rk29_camera_irq(int irq, void *data)
 379 {
 380     struct rk29_camera_dev *pcdev = data;
 381     struct videobuf_buffer *vb=NULL,**nxt_active,**active;
 382         struct list_head *next_head;
 383         int fb_sr,vip_ctrl;
 384
 385     read_vip_reg(RK29_VIP_INT_STS);    /* clear vip interrupte single  */
 386
 387     /* ddl@rock-chps.com : Current VIP is run in One Frame Mode, Frame 1 is validate */
 388         fb_sr = read_vip_reg(RK29_VIP_FB_SR);
 389         vip_ctrl = read_vip_reg(RK29_VIP_CTRL);
 390     if (fb_sr & (VIP_F1_READY|VIP_F2_READY)) {
 391                 #if 0
 392         if (pcdev->frame_inval>0) {
 393                         if (((fb_sr & (VIP_F1_READY|VIP_F2_READY)) == (VIP_F1_READY|VIP_F2_READY)) && (vip_ctrl & PING_PONG)){
 394                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~(VIP_F1_READY|VIP_F2_READY)));
 395                                 pcdev->frame_inval -= 2;
 396                 } else if (fb_sr & VIP_F1_READY) {
 397                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F1_READY));
 398                                 pcdev->frame_inval--;
 399                         } else if ((fb_sr & VIP_F2_READY) && (vip_ctrl & PING_PONG)) {
 400                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F2_READY));
 401                                 pcdev->frame_inval--;
 402                 }
 403
 404                         if (pcdev->frame_inval>=0)
 405                 goto RK29_CAMERA_IRQ_END;
 406         } else if (pcdev->frame_inval) {
 407                 RK29CAMERA_TR("frame_inval : %0x",pcdev->frame_inval);
 408             pcdev->frame_inval = 0;
 409         }
 410
 411
 412                 if (((fb_sr & (VIP_F1_READY|VIP_F2_READY)) == (VIP_F1_READY|VIP_F2_READY)) && (vip_ctrl & PING_PONG)) {
 413                         if ((read_vip_reg(RK29_VIP_FB_SR)&VIP_F2_IS_LATEST) == VIP_F2_IS_LATEST) {
 414                                 vb = pcdev->active2;
 415                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F1_READY));
 416                         } else {
 417                                 vb = pcdev->active1;
 418                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F2_READY));
 419                         }
 420         } else
 421                 #endif
 422
 423                 if (fb_sr & VIP_F1_READY) {
 424                         vb = pcdev->active1;
 425                         if (rk29_first == 0) {
 426                                 rk29_first = 1;
 427                                 list_del_init(&vb->queue);
 428                                 write_vip_reg(RK29_VIP_FB_SR,  read_vip_reg(RK29_VIP_FB_SR)&(~VIP_F1_READY));
 429                                 goto RK29_CAMERA_IRQ_END1;
 430                         }
 431                 } else if ((fb_sr & VIP_F2_READY) && (vip_ctrl & PING_PONG)) {
 432                         vb = pcdev->active2;
 433         }
 434
 435                 if (!vb) {
 436                         goto RK29_CAMERA_IRQ_END;
 437                 }
 438
 439                 /* ddl@rock-chips.com : this vb may be deleted from queue */
 440                 if ((vb->state == VIDEOBUF_QUEUED) || (vb->state == VIDEOBUF_ACTIVE)) {
 441                 list_del_init(&vb->queue);
 442
 443                         vb->state = VIDEOBUF_DONE;
 444                 do_gettimeofday(&vb->ts);
 445                 vb->field_count++;
 446                 }
 447
 448         wake_up(&vb->done);
 449 RK29_CAMERA_IRQ_END1:
 450                 hrtimer_start(&pcdev->frame_timer,
 451                                                 ktime_set(0, 10000),
 452                                                 HRTIMER_MODE_REL);
 453     }
 454
 455 RK29_CAMERA_IRQ_END:
 456     return IRQ_HANDLED;
 457 }
 458
 459
 460 static enum hrtimer_restart rk29_camera_timer_func(struct hrtimer *timer)
 461 {
 462         struct rk29_camera_dev *pcdev = container_of(timer, struct rk29_camera_dev, frame_timer);
 463         struct videobuf_buffer *vb=NULL,**nxt_active;
 464         int fb_sr;
 465
 466         fb_sr = read_vip_reg(RK29_VIP_FB_SR);
 467         if (fb_sr & VIP_F1_READY) {
 468                 nxt_active = &pcdev->active1;
 469         } else if (fb_sr & VIP_F2_READY)  {
 470                 nxt_active = &pcdev->active2;
 471     }
 472
 473         *nxt_active = NULL;
 474     if (!list_empty(&pcdev->capture)) {
 475         *nxt_active = list_entry(pcdev->capture.next, struct videobuf_buffer, queue);
 476                 if (*nxt_active) {
 477                         rk29_videobuf_capture(*nxt_active);
 478                         list_del_init(&(*nxt_active)->queue);
 479                 }
 480     }
 481
 482     if (*nxt_active == NULL) {
 483                 RK29CAMERA_DG("%s video_buf queue is empty!\n",__FUNCTION__);
 484     }
 485
 486         return HRTIMER_NORESTART;
 487 }
 488
 489 static void rk29_videobuf_release(struct videobuf_queue *vq,
 490                                   struct videobuf_buffer *vb)
 491 {
 492     struct rk29_buffer *buf = container_of(vb, struct rk29_buffer, vb);
 493     struct soc_camera_device *icd = vq->priv_data;
 494     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 495     struct rk29_camera_dev *pcdev = ici->priv;
 496
 497 #ifdef DEBUG
 498     dev_dbg(&icd->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 499             vb, vb->baddr, vb->bsize);
 500
 501     switch (vb->state)
 502     {
 503         case VIDEOBUF_ACTIVE:
 504             dev_dbg(&icd->dev, "%s (active)\n", __func__);
 505             break;
 506         case VIDEOBUF_QUEUED:
 507             dev_dbg(&icd->dev, "%s (queued)\n", __func__);
 508             break;
 509         case VIDEOBUF_PREPARED:
 510             dev_dbg(&icd->dev, "%s (prepared)\n", __func__);
 511             break;
 512         default:
 513             dev_dbg(&icd->dev, "%s (unknown)\n", __func__);
 514             break;
 515     }
 516 #endif
 517         if ((vb == pcdev->active1) || (vb == pcdev->active2)) {
 518                 RK29CAMERA_DG("%s Wait for this video buf(0x%x) write finished!\n ",__FUNCTION__,(unsigned int)vb);
 519                 interruptible_sleep_on_timeout(&vb->done, 100);
 520                 RK29CAMERA_DG("%s This video buf(0x%x) write finished, release now!!\n",__FUNCTION__,(unsigned int)vb);
 521         }
 522     rk29_videobuf_free(vq, buf);
 523 }
 524
 525 static struct videobuf_queue_ops rk29_videobuf_ops =
 526 {
 527     .buf_setup      = rk29_videobuf_setup,
 528     .buf_prepare    = rk29_videobuf_prepare,
 529     .buf_queue      = rk29_videobuf_queue,
 530     .buf_release    = rk29_videobuf_release,
 531 };
 532
 533 static void rk29_camera_init_videobuf(struct videobuf_queue *q,
 534                                       struct soc_camera_device *icd)
 535 {
 536     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 537     struct rk29_camera_dev *pcdev = ici->priv;
 538
 539     /* We must pass NULL as dev pointer, then all pci_* dma operations
 540      * transform to normal dma_* ones. */
 541     videobuf_queue_dma_contig_init(q,
 542                                    &rk29_videobuf_ops,
 543                                    ici->v4l2_dev.dev, &pcdev->lock,
 544                                    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 545                                    V4L2_FIELD_NONE,
 546                                    sizeof(struct rk29_buffer),
 547                                    icd);
 548         pcdev->vb_vidq_ptr = q;         /* ddl@rock-chips.com */
 549 }
 550 static int rk29_camera_activate(struct rk29_camera_dev *pcdev, struct soc_camera_device *icd)
 551 {
 552     unsigned long sensor_bus_flags = SOCAM_MCLK_24MHZ;
 553     struct clk *parent;
 554
 555     RK29CAMERA_DG("\n%s..%d.. \n",__FUNCTION__,__LINE__);
 556     if (!pcdev->clk || IS_ERR(pcdev->clk))
 557         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk source\n");
 558
 559     //if (icd->ops->query_bus_param)                                                  /* ddl@rock-chips.com : Query Sensor's xclk */
 560         //sensor_bus_flags = icd->ops->query_bus_param(icd);
 561
 562     if (sensor_bus_flags & SOCAM_MCLK_48MHZ) {
 563         parent = clk_get(NULL, "clk48m");
 564         if (!parent || IS_ERR(parent))
 565              goto RK29_CAMERA_ACTIVE_ERR;
 566     } else if (sensor_bus_flags & SOCAM_MCLK_27MHZ) {
 567         parent = clk_get(NULL, "extclk");
 568         if (!parent || IS_ERR(parent))
 569              goto RK29_CAMERA_ACTIVE_ERR;
 570     } else {
 571         parent = clk_get(NULL, "xin24m");
 572         if (!parent || IS_ERR(parent))
 573              goto RK29_CAMERA_ACTIVE_ERR;
 574     }
 575
 576     clk_set_parent(pcdev->clk, parent);
 577
 578     clk_enable(pcdev->clk);
 579     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_VIP_CLKOUT);
 580     ndelay(10);
 581
 582         //write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)|VIP_AHBMASTER);  //VIP Config to AHB
 583         write_grf_reg(GRF_SOC_CON0_Reg, read_grf_reg(GRF_SOC_CON0_Reg)&(~VIP_AHBMASTER));  //VIP Config to AXI
 584         write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)&(~VIP_ACLK_DIV_HCLK_2));   //aclk:hclk = 1:1
 585         //write_grf_reg(GRF_OS_REG0, read_grf_reg(GRF_OS_REG0)|VIP_ACLK_DIV_HCLK_2);   //aclk:hclk = 2:1
 586
 587     write_vip_reg(RK29_VIP_RESET, 0x76543210);  /* ddl@rock-chips.com : vip software reset */
 588     udelay(10);
 589
 590     write_vip_reg(RK29_VIP_AHBR_CTRL, 0x07);   /* ddl@rock-chips.com : vip ahb burst 16 */
 591     write_vip_reg(RK29_VIP_INT_MASK, 0x01);    //capture complete interrupt enable
 592     write_vip_reg(RK29_VIP_CRM,  0x00000000);  //Y/CB/CR color modification
 593
 594     return 0;
 595 RK29_CAMERA_ACTIVE_ERR:
 596     return -ENODEV;
 597 }
 598
 599 static void rk29_camera_deactivate(struct rk29_camera_dev *pcdev)
 600 {
 601     pcdev->active1 = NULL;
 602         pcdev->active2 = NULL;
 603
 604     write_vip_reg(RK29_VIP_CTRL, 0);
 605     read_vip_reg(RK29_VIP_INT_STS);             //clear vip interrupte single
 606
 607     rk29_mux_api_set(GPIO1B4_VIPCLKOUT_NAME, GPIO1L_GPIO1B4);
 608     clk_disable(pcdev->clk);
 609
 610     return;
 611 }
 612
 613 /* The following two functions absolutely depend on the fact, that
 614  * there can be only one camera on RK28 quick capture interface */
 615 static int rk29_camera_add_device(struct soc_camera_device *icd)
 616 {
 617     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 618     struct rk29_camera_dev *pcdev = ici->priv;
 619     struct device *control = to_soc_camera_control(icd);
 620     struct v4l2_subdev *sd;
 621     int ret;
 622
 623     mutex_lock(&camera_lock);
 624
 625     if (pcdev->icd) {
 626         ret = -EBUSY;
 627         goto ebusy;
 628     }
 629
 630     dev_info(&icd->dev, "RK29 Camera driver attached to camera %d\n",
 631              icd->devnum);
 632
 633         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 634     pcdev->active1 = NULL;
 635         pcdev->active2 = NULL;
 636     pcdev->icd = NULL;
 637         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 638         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 639      * if app havn't dequeue all videobuf before close camera device;
 640         */
 641     INIT_LIST_HEAD(&pcdev->capture);
 642
 643     ret = rk29_camera_activate(pcdev,icd);
 644     if (ret)
 645         goto ebusy;
 646
 647     /* ddl@rock-chips.com : v4l2_subdev is not created when ici->ops->add called in soc_camera_probe  */
 648     if (control) {
 649         sd = dev_get_drvdata(control);
 650         ret = v4l2_subdev_call(sd,core, init, 0);
 651         if (ret)
 652             goto ebusy;
 653     }
 654
 655     pcdev->icd = icd;
 656
 657 ebusy:
 658     mutex_unlock(&camera_lock);
 659
 660     return ret;
 661 }
 662 static void rk29_camera_remove_device(struct soc_camera_device *icd)
 663 {
 664     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 665     struct rk29_camera_dev *pcdev = ici->priv;
 666
 667     BUG_ON(icd != pcdev->icd);
 668
 669     dev_info(&icd->dev, "RK29 Camera driver detached from camera %d\n",
 670              icd->devnum);
 671
 672         rk29_camera_deactivate(pcdev);
 673
 674         /* ddl@rock-chips.com: Call videobuf_mmap_free here for free the struct video_buffer which malloc in videobuf_alloc */
 675         if (pcdev->vb_vidq_ptr) {
 676                 videobuf_mmap_free(pcdev->vb_vidq_ptr);
 677                 pcdev->vb_vidq_ptr = NULL;
 678         }
 679
 680         pcdev->active1 = NULL;
 681         pcdev->active2 = NULL;
 682     pcdev->icd = NULL;
 683         pcdev->reginfo_suspend.Inval = Reg_Invalidate;
 684         /* ddl@rock-chips.com: capture list must be reset, because this list may be not empty,
 685      * if app havn't dequeue all videobuf before close camera device;
 686         */
 687     INIT_LIST_HEAD(&pcdev->capture);
 688
 689         return;
 690 }
 691
 692 static int rk29_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 693 {
 694     unsigned long bus_flags, camera_flags, common_flags;
 695     unsigned int vip_ctrl_val = 0;
 696     int ret = 0;
 697
 698     RK29CAMERA_DG("\n%s..%d..\n",__FUNCTION__,__LINE__);
 699
 700     bus_flags = RK29_CAM_BUS_PARAM;
 701         if (icd->ops->query_bus_param)
 702         camera_flags = icd->ops->query_bus_param(icd);
 703         else
 704                 camera_flags = 0;
 705
 706     common_flags = soc_camera_bus_param_compatible(camera_flags, bus_flags);
 707     if (!common_flags) {
 708         ret = -EINVAL;
 709         goto RK29_CAMERA_SET_BUS_PARAM_END;
 710     }
 711
 712     ret = icd->ops->set_bus_param(icd, common_flags);
 713     if (ret < 0)
 714         goto RK29_CAMERA_SET_BUS_PARAM_END;
 715
 716         if (common_flags & SOCAM_DATAWIDTH_8) {
 717         icd->buswidth = 8;
 718         } else if (common_flags & SOCAM_DATAWIDTH_10) {
 719             icd->buswidth = 10;
 720         }
 721
 722     vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
 723     if (common_flags & SOCAM_PCLK_SAMPLE_FALLING) {
 724         vip_ctrl_val |= NEGATIVE_EDGE;
 725     } else {
 726                 vip_ctrl_val &= ~NEGATIVE_EDGE;
 727     }
 728     if (common_flags & SOCAM_HSYNC_ACTIVE_LOW) {
 729         vip_ctrl_val |= HSY_LOW_ACTIVE;
 730     } else {
 731                 vip_ctrl_val &= ~HSY_LOW_ACTIVE;
 732     }
 733     if (common_flags & SOCAM_VSYNC_ACTIVE_HIGH) {
 734         vip_ctrl_val |= VSY_HIGH_ACTIVE;
 735     } else {
 736                 vip_ctrl_val &= ~VSY_HIGH_ACTIVE;
 737     }
 738
 739     /* ddl@rock-chips.com : Don't enable capture here, enable in stream_on */
 740     //vip_ctrl_val |= ENABLE_CAPTURE;
 741
 742     write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
 743     RK29CAMERA_DG("\n%s..CtrReg=%x  \n",__FUNCTION__,read_vip_reg(RK29_VIP_CTRL));
 744
 745 RK29_CAMERA_SET_BUS_PARAM_END:
 746         if (ret)
 747         RK29CAMERA_TR("\n%s..%d.. ret = %d \n",__FUNCTION__,__LINE__, ret);
 748     return ret;
 749 }
 750
 751 static int rk29_camera_try_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 752 {
 753     unsigned long bus_flags, camera_flags;
 754     int ret;
 755
 756     bus_flags = RK29_CAM_BUS_PARAM;
 757         if (icd->ops->query_bus_param) {
 758         camera_flags = icd->ops->query_bus_param(icd);
 759         } else {
 760                 camera_flags = 0;
 761         }
 762     ret = soc_camera_bus_param_compatible(camera_flags, bus_flags) ;
 763
 764     if (ret < 0)
 765         dev_warn(icd->dev.parent,
 766                          "Flags incompatible: camera %lx, host %lx\n",
 767                          camera_flags, bus_flags);
 768     return ret;
 769 }
 770 static const struct soc_camera_data_format rk29_camera_formats[] = {
 771         {
 772                 .name           = "Planar YUV420 12 bit",
 773                 .depth          = 12,
 774                 .fourcc         = V4L2_PIX_FMT_YUV420,
 775                 .colorspace     = V4L2_COLORSPACE_JPEG,
 776         },{
 777                 .name           = "Planar YUV422 16 bit",
 778                 .depth          = 16,
 779                 .fourcc         = V4L2_PIX_FMT_YUV422P,
 780                 .colorspace     = V4L2_COLORSPACE_JPEG,
 781         },{
 782                 .name           = "Raw Bayer RGB 10 bit",
 783                 .depth          = 16,
 784                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
 785                 .colorspace     = V4L2_COLORSPACE_SRGB,
 786         }
 787 };
 788
 789 static void rk29_camera_setup_format(struct soc_camera_device *icd, __u32 host_pixfmt, __u32 cam_pixfmt, struct v4l2_rect *rect)
 790 {
 791         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 792     struct rk29_camera_dev *pcdev = ici->priv;
 793     unsigned int vip_fs = 0,vip_crop = 0;
 794     unsigned int vip_ctrl_val = VIP_SENSOR|PING_PONG|DISABLE_CAPTURE;
 795
 796     switch (host_pixfmt)
 797     {
 798         case V4L2_PIX_FMT_YUV422P:
 799             vip_ctrl_val |= VIPREGYUV422;
 800                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 801             break;
 802         case V4L2_PIX_FMT_YUV420:
 803             vip_ctrl_val |= VIPREGYUV420;
 804                         if (pcdev->frame_inval != RK29_CAM_FRAME_INVAL_INIT)
 805                                 pcdev->frame_inval = RK29_CAM_FRAME_INVAL_INIT;
 806             break;
 807                 case V4L2_PIX_FMT_SGRBG10:
 808                         vip_ctrl_val |= (VIP_RAW | VIP_SENSOR | VIP_DATA_LITTLEEND);
 809                         pcdev->frame_inval = RK29_CAM_FRAME_INVAL_DC;
 810                         break;
 811         default:                                                                                /* ddl@rock-chips.com : vip output format is hold when pixfmt is invalidate */
 812             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & VIPREGYUV422);
 813             break;
 814     }
 815
 816     switch (cam_pixfmt)
 817     {
 818         case V4L2_PIX_FMT_UYVY:
 819             vip_ctrl_val |= SENSOR_UYVY;
 820             break;
 821         case V4L2_PIX_FMT_YUYV:
 822             vip_ctrl_val |= SENSOR_YUYV;
 823             break;
 824         default :
 825             vip_ctrl_val |= (read_vip_reg(RK29_VIP_CTRL) & SENSOR_YUYV);
 826             break;
 827     }
 828
 829     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 */
 830
 831     read_vip_reg(RK29_VIP_INT_STS);                     /* clear vip interrupte single  */
 832
 833     if (vip_ctrl_val & ONEFRAME)  {
 834         vip_crop = ((rect->left<<16) + rect->top);
 835         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
 836     } else if (vip_ctrl_val & PING_PONG) {
 837         if (rect->left ||rect->top ) {
 838                         RK29CAMERA_DG("\n %s..PingPang not support Crop \n",__FUNCTION__);
 839                         BUG();
 840         }
 841                 vip_crop = 0;
 842         vip_fs  = (((rect->width + rect->left)<<16) + (rect->height+rect->top));
 843     }
 844
 845     write_vip_reg(RK29_VIP_CROP, vip_crop);
 846     write_vip_reg(RK29_VIP_FS, vip_fs);
 847
 848     write_vip_reg(RK29_VIP_FB_SR, (VIP_F2_READY|VIP_F1_READY));
 849
 850     RK29CAMERA_DG("\n%s.. crop:0x%x fs:0x%x CtrlReg:0x%x\n",__FUNCTION__,vip_crop,vip_fs,read_vip_reg(RK29_VIP_CTRL));
 851         return;
 852 }
 853
 854 static int rk29_camera_get_formats(struct soc_camera_device *icd, int idx,
 855                                   struct soc_camera_format_xlate *xlate)
 856 {
 857     struct device *dev = icd->dev.parent;
 858     int formats = 0, buswidth, ret;
 859
 860     buswidth = 8;
 861
 862     ret = rk29_camera_try_bus_param(icd, buswidth);
 863     if (ret < 0)
 864         return 0;
 865
 866     switch (icd->formats[idx].fourcc) {
 867         case V4L2_PIX_FMT_UYVY:
 868         case V4L2_PIX_FMT_YUYV:
 869             formats++;
 870             if (xlate) {
 871                 xlate->host_fmt = &rk29_camera_formats[0];
 872                 xlate->cam_fmt = icd->formats + idx;
 873                 xlate->buswidth = buswidth;
 874                 xlate++;
 875                 dev_dbg(dev, "Providing format %s using %s\n",
 876                         rk29_camera_formats[0].name,
 877                         icd->formats[idx].name);
 878             }
 879
 880             formats++;
 881             if (xlate) {
 882                 xlate->host_fmt = &rk29_camera_formats[1];
 883                 xlate->cam_fmt = icd->formats + idx;
 884                 xlate->buswidth = buswidth;
 885                 xlate++;
 886                 dev_dbg(dev, "Providing format %s using %s\n",
 887                         rk29_camera_formats[1].name,
 888                         icd->formats[idx].name);
 889             }
 890                         break;
 891                 case V4L2_PIX_FMT_SGRBG10:
 892                         formats++;
 893             if (xlate) {
 894                 xlate->host_fmt = &rk29_camera_formats[2];
 895                 xlate->cam_fmt = icd->formats + idx;
 896                 xlate->buswidth = 10;
 897                 xlate++;
 898                 dev_dbg(dev, "Providing format %s using %s\n",
 899                         rk29_camera_formats[2].name,
 900                         icd->formats[idx].name);
 901             }
 902                         break;
 903         default:
 904             break;
 905     }
 906
 907     return formats;
 908 }
 909
 910 static void rk29_camera_put_formats(struct soc_camera_device *icd)
 911 {
 912         return;
 913 }
 914
 915 static int rk29_camera_set_crop(struct soc_camera_device *icd,
 916                                struct v4l2_crop *a)
 917 {
 918     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 919     struct v4l2_format f;
 920     struct v4l2_pix_format *pix = &f.fmt.pix;
 921     int ret;
 922
 923     f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 924
 925     ret = v4l2_subdev_call(sd, video, g_fmt, &f);
 926     if (ret < 0)
 927         return ret;
 928
 929     if ((pix->width < (a->c.left + a->c.width)) || (pix->height < (a->c.top + a->c.height)))  {
 930
 931         pix->width = a->c.left + a->c.width;
 932         pix->height = a->c.top + a->c.height;
 933
 934         v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
 935             &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
 936             icd->current_fmt->fourcc == V4L2_PIX_FMT_YUV422P ?4 : 0);
 937
 938         ret = v4l2_subdev_call(sd, video, s_fmt, &f);
 939         if (ret < 0)
 940             return ret;
 941     }
 942
 943     rk29_camera_setup_format(icd, icd->current_fmt->fourcc, pix->pixelformat, &a->c);
 944
 945     icd->user_width = pix->width;
 946     icd->user_height = pix->height;
 947
 948     return 0;
 949 }
 950
 951 static int rk29_camera_set_fmt(struct soc_camera_device *icd,
 952                               struct v4l2_format *f)
 953 {
 954     struct device *dev = icd->dev.parent;
 955     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 956     const struct soc_camera_data_format *cam_fmt = NULL;
 957     const struct soc_camera_format_xlate *xlate = NULL;
 958     struct v4l2_pix_format *pix = &f->fmt.pix;
 959     struct v4l2_format cam_f = *f;
 960     struct v4l2_rect rect;
 961     int ret;
 962
 963     RK29CAMERA_DG("\n%s..%d..  \n",__FUNCTION__,__LINE__);
 964
 965     xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 966     if (!xlate) {
 967         dev_err(dev, "Format %x not found\n", pix->pixelformat);
 968         ret = -EINVAL;
 969         goto RK29_CAMERA_SET_FMT_END;
 970     }
 971
 972     cam_fmt = xlate->cam_fmt;
 973
 974     cam_f.fmt.pix.pixelformat = cam_fmt->fourcc;
 975     ret = v4l2_subdev_call(sd, video, s_fmt, &cam_f);
 976     cam_f.fmt.pix.pixelformat = pix->pixelformat;
 977     *pix = cam_f.fmt.pix;
 978
 979     icd->sense = NULL;
 980
 981     if (!ret) {
 982         rect.left = 0;
 983         rect.top = 0;
 984         rect.width = pix->width;
 985         rect.height = pix->height;
 986
 987         RK29CAMERA_DG("\n%s..%s..%s \n",__FUNCTION__,xlate->host_fmt->name, cam_fmt->name);
 988         rk29_camera_setup_format(icd, pix->pixelformat, cam_fmt->fourcc, &rect);
 989
 990         icd->buswidth = xlate->buswidth;
 991         icd->current_fmt = xlate->host_fmt;
 992     }
 993
 994 RK29_CAMERA_SET_FMT_END:
 995         if (ret)
 996         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
 997     return ret;
 998 }
 999
1000 static int rk29_camera_try_fmt(struct soc_camera_device *icd,
1001                                    struct v4l2_format *f)
1002 {
1003     struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1004     struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1005     const struct soc_camera_format_xlate *xlate;
1006     struct v4l2_pix_format *pix = &f->fmt.pix;
1007     __u32 pixfmt = pix->pixelformat;
1008     enum v4l2_field field;
1009     int ret;
1010
1011     RK29CAMERA_DG("\n%s..%d.. \n",__FUNCTION__,__LINE__);
1012
1013     xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1014     if (!xlate) {
1015         dev_err(ici->v4l2_dev.dev, "Format %x not found\n", pixfmt);
1016         ret = -EINVAL;
1017         goto RK29_CAMERA_TRY_FMT_END;
1018     }
1019    /* limit to rk29 hardware capabilities */
1020     v4l_bound_align_image(&pix->width, RK29_CAM_W_MIN, RK29_CAM_W_MAX, 1,
1021               &pix->height, RK29_CAM_H_MIN, RK29_CAM_H_MAX, 0,
1022               pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
1023
1024     pix->bytesperline = pix->width * DIV_ROUND_UP(xlate->host_fmt->depth, 8);
1025     pix->sizeimage = pix->height * pix->bytesperline;
1026
1027     /* camera has to see its format, but the user the original one */
1028     pix->pixelformat = xlate->cam_fmt->fourcc;
1029     /* limit to sensor capabilities */
1030     ret = v4l2_subdev_call(sd, video, try_fmt, f);
1031     pix->pixelformat = pixfmt;
1032
1033     field = pix->field;
1034
1035     if (field == V4L2_FIELD_ANY) {
1036         pix->field = V4L2_FIELD_NONE;
1037     } else if (field != V4L2_FIELD_NONE) {
1038         dev_err(icd->dev.parent, "Field type %d unsupported.\n", field);
1039         ret = -EINVAL;
1040         goto RK29_CAMERA_TRY_FMT_END;
1041     }
1042
1043 RK29_CAMERA_TRY_FMT_END:
1044         if (ret)
1045         RK29CAMERA_TR("\n%s..%d.. ret = %d  \n",__FUNCTION__,__LINE__, ret);
1046     return ret;
1047 }
1048
1049 static int rk29_camera_reqbufs(struct soc_camera_file *icf,
1050                                struct v4l2_requestbuffers *p)
1051 {
1052     int i;
1053
1054     /* This is for locking debugging only. I removed spinlocks and now I
1055      * check whether .prepare is ever called on a linked buffer, or whether
1056      * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1057      * it hadn't triggered */
1058     for (i = 0; i < p->count; i++) {
1059         struct rk29_buffer *buf = container_of(icf->vb_vidq.bufs[i],
1060                                                            struct rk29_buffer, vb);
1061         buf->inwork = 0;
1062         INIT_LIST_HEAD(&buf->vb.queue);
1063     }
1064
1065     return 0;
1066 }
1067
1068 static unsigned int rk29_camera_poll(struct file *file, poll_table *pt)
1069 {
1070     struct soc_camera_file *icf = file->private_data;
1071     struct rk29_buffer *buf;
1072
1073     buf = list_entry(icf->vb_vidq.stream.next, struct rk29_buffer,
1074                      vb.stream);
1075
1076     poll_wait(file, &buf->vb.done, pt);
1077
1078     if (buf->vb.state == VIDEOBUF_DONE ||
1079             buf->vb.state == VIDEOBUF_ERROR)
1080         return POLLIN|POLLRDNORM;
1081
1082     return 0;
1083 }
1084
1085 static int rk29_camera_querycap(struct soc_camera_host *ici,
1086                                 struct v4l2_capability *cap)
1087 {
1088     /* cap->name is set by the firendly caller:-> */
1089     strlcpy(cap->card, rk29_cam_driver_description, sizeof(cap->card));
1090     cap->version = RK29_CAM_VERSION_CODE;
1091     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1092
1093     return 0;
1094 }
1095
1096 static int rk29_camera_suspend(struct soc_camera_device *icd, pm_message_t state)
1097 {
1098     struct soc_camera_host *ici =
1099                     to_soc_camera_host(icd->dev.parent);
1100     struct rk29_camera_dev *pcdev = ici->priv;
1101     int ret = 0,tmp;
1102
1103         if ((pcdev->icd == icd) && (icd->ops->suspend)) {
1104                 rk29_camera_s_stream(icd, 0);
1105                 ret = icd->ops->suspend(icd, state);
1106
1107                 pcdev->reginfo_suspend.VipCtrl = read_vip_reg(RK29_VIP_CTRL);
1108                 pcdev->reginfo_suspend.VipCrop = read_vip_reg(RK29_VIP_CROP);
1109                 pcdev->reginfo_suspend.VipFs = read_vip_reg(RK29_VIP_FS);
1110                 pcdev->reginfo_suspend.VipIntMsk = read_vip_reg(RK29_VIP_INT_MASK);
1111                 pcdev->reginfo_suspend.VipCrm = read_vip_reg(RK29_VIP_CRM);
1112
1113                 tmp = pcdev->reginfo_suspend.VipFs>>16;         /* ddl@rock-chips.com */
1114                 tmp += pcdev->reginfo_suspend.VipCrop>>16;
1115                 pcdev->reginfo_suspend.VipFs = (pcdev->reginfo_suspend.VipFs & 0xffff) | (tmp<<16);
1116
1117                 pcdev->reginfo_suspend.Inval = Reg_Validate;
1118                 rk29_camera_deactivate(pcdev);
1119
1120                 RK29CAMERA_DG("%s Enter Success...\n", __FUNCTION__);
1121         } else {
1122                 RK29CAMERA_DG("%s icd has been deattach, don't need enter suspend\n", __FUNCTION__);
1123         }
1124
1125     return ret;
1126 }
1127
1128 static int rk29_camera_resume(struct soc_camera_device *icd)
1129 {
1130     struct soc_camera_host *ici =
1131                     to_soc_camera_host(icd->dev.parent);
1132     struct rk29_camera_dev *pcdev = ici->priv;
1133     int ret = 0;
1134
1135         if ((pcdev->icd == icd) && (icd->ops->resume)) {
1136                 if (pcdev->reginfo_suspend.Inval == Reg_Validate) {
1137                         rk29_camera_activate(pcdev, icd);
1138                         write_vip_reg(RK29_VIP_INT_MASK, pcdev->reginfo_suspend.VipIntMsk);
1139                         write_vip_reg(RK29_VIP_CRM, pcdev->reginfo_suspend.VipCrm);
1140                         write_vip_reg(RK29_VIP_CTRL, pcdev->reginfo_suspend.VipCtrl&~ENABLE_CAPTURE);
1141                         write_vip_reg(RK29_VIP_CROP, pcdev->reginfo_suspend.VipCrop);
1142                         write_vip_reg(RK29_VIP_FS, pcdev->reginfo_suspend.VipFs);
1143                         write_vip_reg(RK29_VIP_FB_SR, (VIP_F2_READY|VIP_F1_READY));
1144
1145                         if (pcdev->active1)
1146                                 rk29_videobuf_capture(pcdev->active1);
1147
1148                         if (pcdev->active2)
1149                                 rk29_videobuf_capture(pcdev->active2);
1150
1151                         rk29_camera_s_stream(icd, 1);
1152                 }
1153                 ret = icd->ops->resume(icd);
1154
1155                 RK29CAMERA_DG("%s Enter success\n",__FUNCTION__);
1156         } else {
1157                 RK29CAMERA_DG("%s icd has been deattach, don't need enter resume\n", __FUNCTION__);
1158         }
1159
1160     return ret;
1161 }
1162
1163 static int rk29_camera_s_stream(struct soc_camera_device *icd, int enable)
1164 {
1165         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1166     struct rk29_camera_dev *pcdev = ici->priv;
1167     int vip_ctrl_val;
1168
1169         WARN_ON(pcdev->icd != icd);
1170
1171         vip_ctrl_val = read_vip_reg(RK29_VIP_CTRL);
1172         if (enable) {
1173                 vip_ctrl_val |= ENABLE_CAPTURE;
1174         } else {
1175         vip_ctrl_val &= ~ENABLE_CAPTURE;
1176         }
1177         write_vip_reg(RK29_VIP_CTRL, vip_ctrl_val);
1178
1179         RK29CAMERA_DG("%s.. enable : %d\n", __FUNCTION__, enable);
1180         return 0;
1181 }
1182
1183 static struct soc_camera_host_ops rk29_soc_camera_host_ops =
1184 {
1185     .owner              = THIS_MODULE,
1186     .add                = rk29_camera_add_device,
1187     .remove             = rk29_camera_remove_device,
1188     .suspend    = rk29_camera_suspend,
1189     .resume             = rk29_camera_resume,
1190     .set_crop   = rk29_camera_set_crop,
1191     .get_formats        = rk29_camera_get_formats,
1192     .put_formats        = rk29_camera_put_formats,
1193     .set_fmt    = rk29_camera_set_fmt,
1194     .try_fmt    = rk29_camera_try_fmt,
1195     .init_videobuf      = rk29_camera_init_videobuf,
1196     .reqbufs    = rk29_camera_reqbufs,
1197     .poll               = rk29_camera_poll,
1198     .querycap   = rk29_camera_querycap,
1199     .set_bus_param      = rk29_camera_set_bus_param,
1200     .s_stream = rk29_camera_s_stream   /* ddl@rock-chips.com : Add stream control for host */
1201 };
1202 static int rk29_camera_probe(struct platform_device *pdev)
1203 {
1204     struct rk29_camera_dev *pcdev;
1205     struct resource *res;
1206     int irq;
1207     int err = 0;
1208
1209     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1210     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1211     irq = platform_get_irq(pdev, 0);
1212     if (!res || irq < 0) {
1213         err = -ENODEV;
1214         goto exit;
1215     }
1216
1217     pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1218     if (!pcdev) {
1219         dev_err(&pdev->dev, "Could not allocate pcdev\n");
1220         err = -ENOMEM;
1221         goto exit_alloc;
1222     }
1223     rk29_camdev_info_ptr = pcdev;
1224
1225     /*config output clk*/
1226     pcdev->clk = clk_get(&pdev->dev, "vip_out");
1227     if (!pcdev->clk || IS_ERR(pcdev->clk))  {
1228         RK29CAMERA_TR(KERN_ERR "failed to get vip_clk source\n");
1229         err = -ENOENT;
1230         goto exit_eclkget;
1231     }
1232
1233     dev_set_drvdata(&pdev->dev, pcdev);
1234     pcdev->res = res;
1235
1236     pcdev->pdata = pdev->dev.platform_data;             /* ddl@rock-chips.com : Request IO in init function */
1237     if (pcdev->pdata && pcdev->pdata->io_init) {
1238         pcdev->pdata->io_init();
1239     }
1240
1241     INIT_LIST_HEAD(&pcdev->capture);
1242     spin_lock_init(&pcdev->lock);
1243
1244     /*
1245      * Request the regions.
1246      */
1247     if (!request_mem_region(res->start, res->end - res->start + 1,
1248                             RK29_CAM_DRV_NAME)) {
1249         err = -EBUSY;
1250         goto exit_reqmem;
1251     }
1252
1253     pcdev->base = ioremap(res->start, res->end - res->start + 1);
1254     if (pcdev->base == NULL) {
1255         dev_err(pcdev->dev, "ioremap() of registers failed\n");
1256         err = -ENXIO;
1257         goto exit_ioremap;
1258     }
1259
1260     pcdev->irq = irq;
1261     pcdev->dev = &pdev->dev;
1262
1263     /* config buffer address */
1264     /* request irq */
1265     err = request_irq(pcdev->irq, rk29_camera_irq, 0, RK29_CAM_DRV_NAME,
1266                       pcdev);
1267     if (err) {
1268         dev_err(pcdev->dev, "Camera interrupt register failed \n");
1269         goto exit_reqirq;
1270     }
1271
1272     pcdev->soc_host.drv_name    = RK29_CAM_DRV_NAME;
1273     pcdev->soc_host.ops         = &rk29_soc_camera_host_ops;
1274     pcdev->soc_host.priv                = pcdev;
1275     pcdev->soc_host.v4l2_dev.dev        = &pdev->dev;
1276     pcdev->soc_host.nr          = pdev->id;
1277
1278     err = soc_camera_host_register(&pcdev->soc_host);
1279     if (err)
1280         goto exit_free_irq;
1281
1282         hrtimer_init(&pcdev->frame_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1283     pcdev->frame_timer.function = rk29_camera_timer_func;
1284
1285     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1286     return 0;
1287
1288 exit_free_irq:
1289     free_irq(pcdev->irq, pcdev);
1290 exit_reqirq:
1291     iounmap(pcdev->base);
1292 exit_ioremap:
1293     release_mem_region(res->start, res->end - res->start + 1);
1294 exit_reqmem:
1295     clk_put(pcdev->clk);
1296 exit_eclkget:
1297     kfree(pcdev);
1298 exit_alloc:
1299     rk29_camdev_info_ptr = NULL;
1300 exit:
1301     return err;
1302 }
1303
1304 static int __devexit rk29_camera_remove(struct platform_device *pdev)
1305 {
1306     struct rk29_camera_dev *pcdev = platform_get_drvdata(pdev);
1307     struct resource *res;
1308
1309     free_irq(pcdev->irq, pcdev);
1310
1311     soc_camera_host_unregister(&pcdev->soc_host);
1312
1313     res = pcdev->res;
1314     release_mem_region(res->start, res->end - res->start + 1);
1315
1316     if (pcdev->pdata && pcdev->pdata->io_deinit) {         /* ddl@rock-chips.com : Free IO in deinit function */
1317         pcdev->pdata->io_deinit();
1318     }
1319
1320     kfree(pcdev);
1321     rk29_camdev_info_ptr = NULL;
1322     dev_info(&pdev->dev, "RK28 Camera driver unloaded\n");
1323
1324     return 0;
1325 }
1326
1327 static struct platform_driver rk29_camera_driver =
1328 {
1329     .driver     = {
1330         .name   = RK29_CAM_DRV_NAME,
1331     },
1332     .probe              = rk29_camera_probe,
1333     .remove             = __devexit_p(rk29_camera_remove),
1334 };
1335
1336
1337 static int __devinit rk29_camera_init(void)
1338 {
1339     RK29CAMERA_DG("\n%s..%s..%d  \n",__FUNCTION__,__FILE__,__LINE__);
1340     return platform_driver_register(&rk29_camera_driver);
1341 }
1342
1343 static void __exit rk29_camera_exit(void)
1344 {
1345     platform_driver_unregister(&rk29_camera_driver);
1346 }
1347
1348 device_initcall_sync(rk29_camera_init);
1349 module_exit(rk29_camera_exit);
1350
1351 MODULE_DESCRIPTION("RK29 Soc Camera Host driver");
1352 MODULE_AUTHOR("ddl <ddl@rock-chips>");
1353 MODULE_LICENSE("GPL");