SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/media/video/sh_mobile_ceu_camera.c

   1 /*
   2  * V4L2 Driver for SuperH Mobile CEU interface
   3  *
   4  * Copyright (C) 2008 Magnus Damm
   5  *
   6  * Based on V4L2 Driver for PXA camera host - "pxa_camera.c",
   7  *
   8  * Copyright (C) 2006, Sascha Hauer, Pengutronix
   9  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  */
  16
  17 #include <linux/init.h>
  18 #include <linux/module.h>
  19 #include <linux/io.h>
  20 #include <linux/delay.h>
  21 #include <linux/dma-mapping.h>
  22 #include <linux/errno.h>
  23 #include <linux/fs.h>
  24 #include <linux/interrupt.h>
  25 #include <linux/kernel.h>
  26 #include <linux/mm.h>
  27 #include <linux/moduleparam.h>
  28 #include <linux/time.h>
  29 #include <linux/version.h>
  30 #include <linux/device.h>
  31 #include <linux/platform_device.h>
  32 #include <linux/videodev2.h>
  33 #include <linux/pm_runtime.h>
  34 #include <linux/sched.h>
  35
  36 #include <media/v4l2-common.h>
  37 #include <media/v4l2-dev.h>
  38 #include <media/soc_camera.h>
  39 #include <media/sh_mobile_ceu.h>
  40 #include <media/videobuf-dma-contig.h>
  41 #include <media/v4l2-mediabus.h>
  42 #include <media/soc_mediabus.h>
  43
  44 /* register offsets for sh7722 / sh7723 */
  45
  46 #define CAPSR  0x00 /* Capture start register */
  47 #define CAPCR  0x04 /* Capture control register */
  48 #define CAMCR  0x08 /* Capture interface control register */
  49 #define CMCYR  0x0c /* Capture interface cycle  register */
  50 #define CAMOR  0x10 /* Capture interface offset register */
  51 #define CAPWR  0x14 /* Capture interface width register */
  52 #define CAIFR  0x18 /* Capture interface input format register */
  53 #define CSTCR  0x20 /* Camera strobe control register (<= sh7722) */
  54 #define CSECR  0x24 /* Camera strobe emission count register (<= sh7722) */
  55 #define CRCNTR 0x28 /* CEU register control register */
  56 #define CRCMPR 0x2c /* CEU register forcible control register */
  57 #define CFLCR  0x30 /* Capture filter control register */
  58 #define CFSZR  0x34 /* Capture filter size clip register */
  59 #define CDWDR  0x38 /* Capture destination width register */
  60 #define CDAYR  0x3c /* Capture data address Y register */
  61 #define CDACR  0x40 /* Capture data address C register */
  62 #define CDBYR  0x44 /* Capture data bottom-field address Y register */
  63 #define CDBCR  0x48 /* Capture data bottom-field address C register */
  64 #define CBDSR  0x4c /* Capture bundle destination size register */
  65 #define CFWCR  0x5c /* Firewall operation control register */
  66 #define CLFCR  0x60 /* Capture low-pass filter control register */
  67 #define CDOCR  0x64 /* Capture data output control register */
  68 #define CDDCR  0x68 /* Capture data complexity level register */
  69 #define CDDAR  0x6c /* Capture data complexity level address register */
  70 #define CEIER  0x70 /* Capture event interrupt enable register */
  71 #define CETCR  0x74 /* Capture event flag clear register */
  72 #define CSTSR  0x7c /* Capture status register */
  73 #define CSRTR  0x80 /* Capture software reset register */
  74 #define CDSSR  0x84 /* Capture data size register */
  75 #define CDAYR2 0x90 /* Capture data address Y register 2 */
  76 #define CDACR2 0x94 /* Capture data address C register 2 */
  77 #define CDBYR2 0x98 /* Capture data bottom-field address Y register 2 */
  78 #define CDBCR2 0x9c /* Capture data bottom-field address C register 2 */
  79
  80 #undef DEBUG_GEOMETRY
  81 #ifdef DEBUG_GEOMETRY
  82 #define dev_geo dev_info
  83 #else
  84 #define dev_geo dev_dbg
  85 #endif
  86
  87 /* per video frame buffer */
  88 struct sh_mobile_ceu_buffer {
  89         struct videobuf_buffer vb; /* v4l buffer must be first */
  90         enum v4l2_mbus_pixelcode code;
  91 };
  92
  93 struct sh_mobile_ceu_dev {
  94         struct soc_camera_host ici;
  95         struct soc_camera_device *icd;
  96
  97         unsigned int irq;
  98         void __iomem *base;
  99         unsigned long video_limit;
 100
 101         /* lock used to protect videobuf */
 102         spinlock_t lock;
 103         struct list_head capture;
 104         struct videobuf_buffer *active;
 105
 106         struct sh_mobile_ceu_info *pdata;
 107
 108         u32 cflcr;
 109
 110         unsigned int is_interlaced:1;
 111         unsigned int image_mode:1;
 112         unsigned int is_16bit:1;
 113 };
 114
 115 struct sh_mobile_ceu_cam {
 116         struct v4l2_rect ceu_rect;
 117         unsigned int cam_width;
 118         unsigned int cam_height;
 119         const struct soc_mbus_pixelfmt *extra_fmt;
 120         enum v4l2_mbus_pixelcode code;
 121 };
 122
 123 static unsigned long make_bus_param(struct sh_mobile_ceu_dev *pcdev)
 124 {
 125         unsigned long flags;
 126
 127         flags = SOCAM_MASTER |
 128                 SOCAM_PCLK_SAMPLE_RISING |
 129                 SOCAM_HSYNC_ACTIVE_HIGH |
 130                 SOCAM_HSYNC_ACTIVE_LOW |
 131                 SOCAM_VSYNC_ACTIVE_HIGH |
 132                 SOCAM_VSYNC_ACTIVE_LOW |
 133                 SOCAM_DATA_ACTIVE_HIGH;
 134
 135         if (pcdev->pdata->flags & SH_CEU_FLAG_USE_8BIT_BUS)
 136                 flags |= SOCAM_DATAWIDTH_8;
 137
 138         if (pcdev->pdata->flags & SH_CEU_FLAG_USE_16BIT_BUS)
 139                 flags |= SOCAM_DATAWIDTH_16;
 140
 141         if (flags & SOCAM_DATAWIDTH_MASK)
 142                 return flags;
 143
 144         return 0;
 145 }
 146
 147 static void ceu_write(struct sh_mobile_ceu_dev *priv,
 148                       unsigned long reg_offs, u32 data)
 149 {
 150         iowrite32(data, priv->base + reg_offs);
 151 }
 152
 153 static u32 ceu_read(struct sh_mobile_ceu_dev *priv, unsigned long reg_offs)
 154 {
 155         return ioread32(priv->base + reg_offs);
 156 }
 157
 158 static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
 159 {
 160         int i, success = 0;
 161         struct soc_camera_device *icd = pcdev->icd;
 162
 163         ceu_write(pcdev, CAPSR, 1 << 16); /* reset */
 164
 165         /* wait CSTSR.CPTON bit */
 166         for (i = 0; i < 1000; i++) {
 167                 if (!(ceu_read(pcdev, CSTSR) & 1)) {
 168                         success++;
 169                         break;
 170                 }
 171                 udelay(1);
 172         }
 173
 174         /* wait CAPSR.CPKIL bit */
 175         for (i = 0; i < 1000; i++) {
 176                 if (!(ceu_read(pcdev, CAPSR) & (1 << 16))) {
 177                         success++;
 178                         break;
 179                 }
 180                 udelay(1);
 181         }
 182
 183
 184         if (2 != success) {
 185                 dev_warn(&icd->dev, "soft reset time out\n");
 186                 return -EIO;
 187         }
 188
 189         return 0;
 190 }
 191
 192 /*
 193  *  Videobuf operations
 194  */
 195 static int sh_mobile_ceu_videobuf_setup(struct videobuf_queue *vq,
 196                                         unsigned int *count,
 197                                         unsigned int *size)
 198 {
 199         struct soc_camera_device *icd = vq->priv_data;
 200         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 201         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 202         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 203                                                 icd->current_fmt->host_fmt);
 204
 205         if (bytes_per_line < 0)
 206                 return bytes_per_line;
 207
 208         *size = PAGE_ALIGN(bytes_per_line * icd->user_height);
 209
 210         if (0 == *count)
 211                 *count = 2;
 212
 213         if (pcdev->video_limit) {
 214                 while (*size * *count > pcdev->video_limit)
 215                         (*count)--;
 216         }
 217
 218         dev_dbg(icd->dev.parent, "count=%d, size=%d\n", *count, *size);
 219
 220         return 0;
 221 }
 222
 223 static void free_buffer(struct videobuf_queue *vq,
 224                         struct sh_mobile_ceu_buffer *buf)
 225 {
 226         struct soc_camera_device *icd = vq->priv_data;
 227         struct device *dev = icd->dev.parent;
 228
 229         dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 230                 &buf->vb, buf->vb.baddr, buf->vb.bsize);
 231
 232         if (in_interrupt())
 233                 BUG();
 234
 235         videobuf_waiton(&buf->vb, 0, 0);
 236         videobuf_dma_contig_free(vq, &buf->vb);
 237         dev_dbg(dev, "%s freed\n", __func__);
 238         buf->vb.state = VIDEOBUF_NEEDS_INIT;
 239 }
 240
 241 #define CEU_CETCR_MAGIC 0x0317f313 /* acknowledge magical interrupt sources */
 242 #define CEU_CETCR_IGRW (1 << 4) /* prohibited register access interrupt bit */
 243 #define CEU_CEIER_CPEIE (1 << 0) /* one-frame capture end interrupt */
 244 #define CEU_CEIER_VBP   (1 << 20) /* vbp error */
 245 #define CEU_CAPCR_CTNCP (1 << 16) /* continuous capture mode (if set) */
 246 #define CEU_CEIER_MASK (CEU_CEIER_CPEIE | CEU_CEIER_VBP)
 247
 248
 249 /*
 250  * return value doesn't reflex the success/failure to queue the new buffer,
 251  * but rather the status of the previous buffer.
 252  */
 253 static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 254 {
 255         struct soc_camera_device *icd = pcdev->icd;
 256         dma_addr_t phys_addr_top, phys_addr_bottom;
 257         u32 status;
 258         int ret = 0;
 259
 260         /*
 261          * The hardware is _very_ picky about this sequence. Especially
 262          * the CEU_CETCR_MAGIC value. It seems like we need to acknowledge
 263          * several not-so-well documented interrupt sources in CETCR.
 264          */
 265         ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) & ~CEU_CEIER_MASK);
 266         status = ceu_read(pcdev, CETCR);
 267         ceu_write(pcdev, CETCR, ~status & CEU_CETCR_MAGIC);
 268         ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) | CEU_CEIER_MASK);
 269         ceu_write(pcdev, CAPCR, ceu_read(pcdev, CAPCR) & ~CEU_CAPCR_CTNCP);
 270         ceu_write(pcdev, CETCR, CEU_CETCR_MAGIC ^ CEU_CETCR_IGRW);
 271
 272         /*
 273          * When a VBP interrupt occurs, a capture end interrupt does not occur
 274          * and the image of that frame is not captured correctly. So, soft reset
 275          * is needed here.
 276          */
 277         if (status & CEU_CEIER_VBP) {
 278                 sh_mobile_ceu_soft_reset(pcdev);
 279                 ret = -EIO;
 280         }
 281
 282         if (!pcdev->active)
 283                 return ret;
 284
 285         phys_addr_top = videobuf_to_dma_contig(pcdev->active);
 286         ceu_write(pcdev, CDAYR, phys_addr_top);
 287         if (pcdev->is_interlaced) {
 288                 phys_addr_bottom = phys_addr_top + icd->user_width;
 289                 ceu_write(pcdev, CDBYR, phys_addr_bottom);
 290         }
 291
 292         switch (icd->current_fmt->host_fmt->fourcc) {
 293         case V4L2_PIX_FMT_NV12:
 294         case V4L2_PIX_FMT_NV21:
 295         case V4L2_PIX_FMT_NV16:
 296         case V4L2_PIX_FMT_NV61:
 297                 phys_addr_top += icd->user_width *
 298                         icd->user_height;
 299                 ceu_write(pcdev, CDACR, phys_addr_top);
 300                 if (pcdev->is_interlaced) {
 301                         phys_addr_bottom = phys_addr_top +
 302                                 icd->user_width;
 303                         ceu_write(pcdev, CDBCR, phys_addr_bottom);
 304                 }
 305         }
 306
 307         pcdev->active->state = VIDEOBUF_ACTIVE;
 308         ceu_write(pcdev, CAPSR, 0x1); /* start capture */
 309
 310         return ret;
 311 }
 312
 313 static int sh_mobile_ceu_videobuf_prepare(struct videobuf_queue *vq,
 314                                           struct videobuf_buffer *vb,
 315                                           enum v4l2_field field)
 316 {
 317         struct soc_camera_device *icd = vq->priv_data;
 318         struct sh_mobile_ceu_buffer *buf;
 319         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 320                                                 icd->current_fmt->host_fmt);
 321         int ret;
 322
 323         if (bytes_per_line < 0)
 324                 return bytes_per_line;
 325
 326         buf = container_of(vb, struct sh_mobile_ceu_buffer, vb);
 327
 328         dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 329                 vb, vb->baddr, vb->bsize);
 330
 331         /* Added list head initialization on alloc */
 332         WARN_ON(!list_empty(&vb->queue));
 333
 334 #ifdef DEBUG
 335         /*
 336          * This can be useful if you want to see if we actually fill
 337          * the buffer with something
 338          */
 339         memset((void *)vb->baddr, 0xaa, vb->bsize);
 340 #endif
 341
 342         BUG_ON(NULL == icd->current_fmt);
 343
 344         if (buf->code   != icd->current_fmt->code ||
 345             vb->width   != icd->user_width ||
 346             vb->height  != icd->user_height ||
 347             vb->field   != field) {
 348                 buf->code       = icd->current_fmt->code;
 349                 vb->width       = icd->user_width;
 350                 vb->height      = icd->user_height;
 351                 vb->field       = field;
 352                 vb->state       = VIDEOBUF_NEEDS_INIT;
 353         }
 354
 355         vb->size = vb->height * bytes_per_line;
 356         if (0 != vb->baddr && vb->bsize < vb->size) {
 357                 ret = -EINVAL;
 358                 goto out;
 359         }
 360
 361         if (vb->state == VIDEOBUF_NEEDS_INIT) {
 362                 ret = videobuf_iolock(vq, vb, NULL);
 363                 if (ret)
 364                         goto fail;
 365                 vb->state = VIDEOBUF_PREPARED;
 366         }
 367
 368         return 0;
 369 fail:
 370         free_buffer(vq, buf);
 371 out:
 372         return ret;
 373 }
 374
 375 /* Called under spinlock_irqsave(&pcdev->lock, ...) */
 376 static void sh_mobile_ceu_videobuf_queue(struct videobuf_queue *vq,
 377                                          struct videobuf_buffer *vb)
 378 {
 379         struct soc_camera_device *icd = vq->priv_data;
 380         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 381         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 382
 383         dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
 384                 vb, vb->baddr, vb->bsize);
 385
 386         vb->state = VIDEOBUF_QUEUED;
 387         list_add_tail(&vb->queue, &pcdev->capture);
 388
 389         if (!pcdev->active) {
 390                 /*
 391                  * Because there were no active buffer at this moment,
 392                  * we are not interested in the return value of
 393                  * sh_mobile_ceu_capture here.
 394                  */
 395                 pcdev->active = vb;
 396                 sh_mobile_ceu_capture(pcdev);
 397         }
 398 }
 399
 400 static void sh_mobile_ceu_videobuf_release(struct videobuf_queue *vq,
 401                                            struct videobuf_buffer *vb)
 402 {
 403         struct soc_camera_device *icd = vq->priv_data;
 404         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 405         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 406         unsigned long flags;
 407
 408         spin_lock_irqsave(&pcdev->lock, flags);
 409
 410         if (pcdev->active == vb) {
 411                 /* disable capture (release DMA buffer), reset */
 412                 ceu_write(pcdev, CAPSR, 1 << 16);
 413                 pcdev->active = NULL;
 414         }
 415
 416         if ((vb->state == VIDEOBUF_ACTIVE || vb->state == VIDEOBUF_QUEUED) &&
 417             !list_empty(&vb->queue)) {
 418                 vb->state = VIDEOBUF_ERROR;
 419                 list_del_init(&vb->queue);
 420         }
 421
 422         spin_unlock_irqrestore(&pcdev->lock, flags);
 423
 424         free_buffer(vq, container_of(vb, struct sh_mobile_ceu_buffer, vb));
 425 }
 426
 427 static struct videobuf_queue_ops sh_mobile_ceu_videobuf_ops = {
 428         .buf_setup      = sh_mobile_ceu_videobuf_setup,
 429         .buf_prepare    = sh_mobile_ceu_videobuf_prepare,
 430         .buf_queue      = sh_mobile_ceu_videobuf_queue,
 431         .buf_release    = sh_mobile_ceu_videobuf_release,
 432 };
 433
 434 static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
 435 {
 436         struct sh_mobile_ceu_dev *pcdev = data;
 437         struct videobuf_buffer *vb;
 438         unsigned long flags;
 439
 440         spin_lock_irqsave(&pcdev->lock, flags);
 441
 442         vb = pcdev->active;
 443         if (!vb)
 444                 /* Stale interrupt from a released buffer */
 445                 goto out;
 446
 447         list_del_init(&vb->queue);
 448
 449         if (!list_empty(&pcdev->capture))
 450                 pcdev->active = list_entry(pcdev->capture.next,
 451                                            struct videobuf_buffer, queue);
 452         else
 453                 pcdev->active = NULL;
 454
 455         vb->state = (sh_mobile_ceu_capture(pcdev) < 0) ?
 456                 VIDEOBUF_ERROR : VIDEOBUF_DONE;
 457         do_gettimeofday(&vb->ts);
 458         vb->field_count++;
 459         wake_up(&vb->done);
 460
 461 out:
 462         spin_unlock_irqrestore(&pcdev->lock, flags);
 463
 464         return IRQ_HANDLED;
 465 }
 466
 467 /* Called with .video_lock held */
 468 static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 469 {
 470         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 471         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 472         int ret;
 473
 474         if (pcdev->icd)
 475                 return -EBUSY;
 476
 477         dev_info(icd->dev.parent,
 478                  "SuperH Mobile CEU driver attached to camera %d\n",
 479                  icd->devnum);
 480
 481         pm_runtime_get_sync(ici->v4l2_dev.dev);
 482
 483         ret = sh_mobile_ceu_soft_reset(pcdev);
 484         if (!ret)
 485                 pcdev->icd = icd;
 486
 487         return ret;
 488 }
 489
 490 /* Called with .video_lock held */
 491 static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 492 {
 493         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 494         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 495         unsigned long flags;
 496
 497         BUG_ON(icd != pcdev->icd);
 498
 499         /* disable capture, disable interrupts */
 500         ceu_write(pcdev, CEIER, 0);
 501         sh_mobile_ceu_soft_reset(pcdev);
 502
 503         /* make sure active buffer is canceled */
 504         spin_lock_irqsave(&pcdev->lock, flags);
 505         if (pcdev->active) {
 506                 list_del(&pcdev->active->queue);
 507                 pcdev->active->state = VIDEOBUF_ERROR;
 508                 wake_up_all(&pcdev->active->done);
 509                 pcdev->active = NULL;
 510         }
 511         spin_unlock_irqrestore(&pcdev->lock, flags);
 512
 513         pm_runtime_put_sync(ici->v4l2_dev.dev);
 514
 515         dev_info(icd->dev.parent,
 516                  "SuperH Mobile CEU driver detached from camera %d\n",
 517                  icd->devnum);
 518
 519         pcdev->icd = NULL;
 520 }
 521
 522 /*
 523  * See chapter 29.4.12 "Capture Filter Control Register (CFLCR)"
 524  * in SH7722 Hardware Manual
 525  */
 526 static unsigned int size_dst(unsigned int src, unsigned int scale)
 527 {
 528         unsigned int mant_pre = scale >> 12;
 529         if (!src || !scale)
 530                 return src;
 531         return ((mant_pre + 2 * (src - 1)) / (2 * mant_pre) - 1) *
 532                 mant_pre * 4096 / scale + 1;
 533 }
 534
 535 static u16 calc_scale(unsigned int src, unsigned int *dst)
 536 {
 537         u16 scale;
 538
 539         if (src == *dst)
 540                 return 0;
 541
 542         scale = (src * 4096 / *dst) & ~7;
 543
 544         while (scale > 4096 && size_dst(src, scale) < *dst)
 545                 scale -= 8;
 546
 547         *dst = size_dst(src, scale);
 548
 549         return scale;
 550 }
 551
 552 /* rect is guaranteed to not exceed the scaled camera rectangle */
 553 static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd,
 554                                    unsigned int out_width,
 555                                    unsigned int out_height)
 556 {
 557         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 558         struct sh_mobile_ceu_cam *cam = icd->host_priv;
 559         struct v4l2_rect *rect = &cam->ceu_rect;
 560         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 561         unsigned int height, width, cdwdr_width, in_width, in_height;
 562         unsigned int left_offset, top_offset;
 563         u32 camor;
 564
 565         dev_dbg(icd->dev.parent, "Crop %ux%u@%u:%u\n",
 566                 rect->width, rect->height, rect->left, rect->top);
 567
 568         left_offset     = rect->left;
 569         top_offset      = rect->top;
 570
 571         if (pcdev->image_mode) {
 572                 in_width = rect->width;
 573                 if (!pcdev->is_16bit) {
 574                         in_width *= 2;
 575                         left_offset *= 2;
 576                 }
 577                 width = out_width;
 578                 cdwdr_width = out_width;
 579         } else {
 580                 int bytes_per_line = soc_mbus_bytes_per_line(out_width,
 581                                                 icd->current_fmt->host_fmt);
 582                 unsigned int w_factor;
 583
 584                 width = out_width;
 585
 586                 switch (icd->current_fmt->host_fmt->packing) {
 587                 case SOC_MBUS_PACKING_2X8_PADHI:
 588                         w_factor = 2;
 589                         break;
 590                 default:
 591                         w_factor = 1;
 592                 }
 593
 594                 in_width = rect->width * w_factor;
 595                 left_offset = left_offset * w_factor;
 596
 597                 if (bytes_per_line < 0)
 598                         cdwdr_width = out_width;
 599                 else
 600                         cdwdr_width = bytes_per_line;
 601         }
 602
 603         height = out_height;
 604         in_height = rect->height;
 605         if (pcdev->is_interlaced) {
 606                 height /= 2;
 607                 in_height /= 2;
 608                 top_offset /= 2;
 609                 cdwdr_width *= 2;
 610         }
 611
 612         /* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
 613         camor = left_offset | (top_offset << 16);
 614
 615         dev_geo(icd->dev.parent,
 616                 "CAMOR 0x%x, CAPWR 0x%x, CFSZR 0x%x, CDWDR 0x%x\n", camor,
 617                 (in_height << 16) | in_width, (height << 16) | width,
 618                 cdwdr_width);
 619
 620         ceu_write(pcdev, CAMOR, camor);
 621         ceu_write(pcdev, CAPWR, (in_height << 16) | in_width);
 622         ceu_write(pcdev, CFSZR, (height << 16) | width);
 623         ceu_write(pcdev, CDWDR, cdwdr_width);
 624 }
 625
 626 static u32 capture_save_reset(struct sh_mobile_ceu_dev *pcdev)
 627 {
 628         u32 capsr = ceu_read(pcdev, CAPSR);
 629         ceu_write(pcdev, CAPSR, 1 << 16); /* reset, stop capture */
 630         return capsr;
 631 }
 632
 633 static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 634 {
 635         unsigned long timeout = jiffies + 10 * HZ;
 636
 637         /*
 638          * Wait until the end of the current frame. It can take a long time,
 639          * but if it has been aborted by a CAPSR reset, it shoule exit sooner.
 640          */
 641         while ((ceu_read(pcdev, CSTSR) & 1) && time_before(jiffies, timeout))
 642                 msleep(1);
 643
 644         if (time_after(jiffies, timeout)) {
 645                 dev_err(pcdev->ici.v4l2_dev.dev,
 646                         "Timeout waiting for frame end! Interface problem?\n");
 647                 return;
 648         }
 649
 650         /* Wait until reset clears, this shall not hang... */
 651         while (ceu_read(pcdev, CAPSR) & (1 << 16))
 652                 udelay(10);
 653
 654         /* Anything to restore? */
 655         if (capsr & ~(1 << 16))
 656                 ceu_write(pcdev, CAPSR, capsr);
 657 }
 658
 659 static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 660                                        __u32 pixfmt)
 661 {
 662         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 663         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 664         int ret;
 665         unsigned long camera_flags, common_flags, value;
 666         int yuv_lineskip;
 667         struct sh_mobile_ceu_cam *cam = icd->host_priv;
 668         u32 capsr = capture_save_reset(pcdev);
 669
 670         camera_flags = icd->ops->query_bus_param(icd);
 671         common_flags = soc_camera_bus_param_compatible(camera_flags,
 672                                                        make_bus_param(pcdev));
 673         if (!common_flags)
 674                 return -EINVAL;
 675
 676         ret = icd->ops->set_bus_param(icd, common_flags);
 677         if (ret < 0)
 678                 return ret;
 679
 680         switch (common_flags & SOCAM_DATAWIDTH_MASK) {
 681         case SOCAM_DATAWIDTH_8:
 682                 pcdev->is_16bit = 0;
 683                 break;
 684         case SOCAM_DATAWIDTH_16:
 685                 pcdev->is_16bit = 1;
 686                 break;
 687         default:
 688                 return -EINVAL;
 689         }
 690
 691         ceu_write(pcdev, CRCNTR, 0);
 692         ceu_write(pcdev, CRCMPR, 0);
 693
 694         value = 0x00000010; /* data fetch by default */
 695         yuv_lineskip = 0;
 696
 697         switch (icd->current_fmt->host_fmt->fourcc) {
 698         case V4L2_PIX_FMT_NV12:
 699         case V4L2_PIX_FMT_NV21:
 700                 yuv_lineskip = 1; /* skip for NV12/21, no skip for NV16/61 */
 701                 /* fall-through */
 702         case V4L2_PIX_FMT_NV16:
 703         case V4L2_PIX_FMT_NV61:
 704                 switch (cam->code) {
 705                 case V4L2_MBUS_FMT_YUYV8_2X8_BE:
 706                         value = 0x00000000; /* Cb0, Y0, Cr0, Y1 */
 707                         break;
 708                 case V4L2_MBUS_FMT_YVYU8_2X8_BE:
 709                         value = 0x00000100; /* Cr0, Y0, Cb0, Y1 */
 710                         break;
 711                 case V4L2_MBUS_FMT_YUYV8_2X8_LE:
 712                         value = 0x00000200; /* Y0, Cb0, Y1, Cr0 */
 713                         break;
 714                 case V4L2_MBUS_FMT_YVYU8_2X8_LE:
 715                         value = 0x00000300; /* Y0, Cr0, Y1, Cb0 */
 716                         break;
 717                 default:
 718                         BUG();
 719                 }
 720         }
 721
 722         if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV21 ||
 723             icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV61)
 724                 value ^= 0x00000100; /* swap U, V to change from NV1x->NVx1 */
 725
 726         value |= common_flags & SOCAM_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
 727         value |= common_flags & SOCAM_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
 728         value |= pcdev->is_16bit ? 1 << 12 : 0;
 729         ceu_write(pcdev, CAMCR, value);
 730
 731         ceu_write(pcdev, CAPCR, 0x00300000);
 732         ceu_write(pcdev, CAIFR, pcdev->is_interlaced ? 0x101 : 0);
 733
 734         sh_mobile_ceu_set_rect(icd, icd->user_width, icd->user_height);
 735         mdelay(1);
 736
 737         ceu_write(pcdev, CFLCR, pcdev->cflcr);
 738
 739         /*
 740          * A few words about byte order (observed in Big Endian mode)
 741          *
 742          * In data fetch mode bytes are received in chunks of 8 bytes.
 743          * D0, D1, D2, D3, D4, D5, D6, D7 (D0 received first)
 744          *
 745          * The data is however by default written to memory in reverse order:
 746          * D7, D6, D5, D4, D3, D2, D1, D0 (D7 written to lowest byte)
 747          *
 748          * The lowest three bits of CDOCR allows us to do swapping,
 749          * using 7 we swap the data bytes to match the incoming order:
 750          * D0, D1, D2, D3, D4, D5, D6, D7
 751          */
 752         value = 0x00000017;
 753         if (yuv_lineskip)
 754                 value &= ~0x00000010; /* convert 4:2:2 -> 4:2:0 */
 755
 756         ceu_write(pcdev, CDOCR, value);
 757         ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
 758
 759         dev_dbg(icd->dev.parent, "S_FMT successful for %c%c%c%c %ux%u\n",
 760                 pixfmt & 0xff, (pixfmt >> 8) & 0xff,
 761                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
 762                 icd->user_width, icd->user_height);
 763
 764         capture_restore(pcdev, capsr);
 765
 766         /* not in bundle mode: skip CBDSR, CDAYR2, CDACR2, CDBYR2, CDBCR2 */
 767         return 0;
 768 }
 769
 770 static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
 771                                        unsigned char buswidth)
 772 {
 773         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 774         struct sh_mobile_ceu_dev *pcdev = ici->priv;
 775         unsigned long camera_flags, common_flags;
 776
 777         camera_flags = icd->ops->query_bus_param(icd);
 778         common_flags = soc_camera_bus_param_compatible(camera_flags,
 779                                                        make_bus_param(pcdev));
 780         if (!common_flags || buswidth > 16 ||
 781             (buswidth > 8 && !(common_flags & SOCAM_DATAWIDTH_16)))
 782                 return -EINVAL;
 783
 784         return 0;
 785 }
 786
 787 static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
 788         {
 789                 .fourcc                 = V4L2_PIX_FMT_NV12,
 790                 .name                   = "NV12",
 791                 .bits_per_sample        = 12,
 792                 .packing                = SOC_MBUS_PACKING_NONE,
 793                 .order                  = SOC_MBUS_ORDER_LE,
 794         }, {
 795                 .fourcc                 = V4L2_PIX_FMT_NV21,
 796                 .name                   = "NV21",
 797                 .bits_per_sample        = 12,
 798                 .packing                = SOC_MBUS_PACKING_NONE,
 799                 .order                  = SOC_MBUS_ORDER_LE,
 800         }, {
 801                 .fourcc                 = V4L2_PIX_FMT_NV16,
 802                 .name                   = "NV16",
 803                 .bits_per_sample        = 16,
 804                 .packing                = SOC_MBUS_PACKING_NONE,
 805                 .order                  = SOC_MBUS_ORDER_LE,
 806         }, {
 807                 .fourcc                 = V4L2_PIX_FMT_NV61,
 808                 .name                   = "NV61",
 809                 .bits_per_sample        = 16,
 810                 .packing                = SOC_MBUS_PACKING_NONE,
 811                 .order                  = SOC_MBUS_ORDER_LE,
 812         },
 813 };
 814
 815 /* This will be corrected as we get more formats */
 816 static bool sh_mobile_ceu_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 817 {
 818         return  fmt->packing == SOC_MBUS_PACKING_NONE ||
 819                 (fmt->bits_per_sample == 8 &&
 820                  fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
 821                 (fmt->bits_per_sample > 8 &&
 822                  fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 823 }
 824
 825 static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, int idx,
 826                                      struct soc_camera_format_xlate *xlate)
 827 {
 828         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 829         struct device *dev = icd->dev.parent;
 830         int ret, k, n;
 831         int formats = 0;
 832         struct sh_mobile_ceu_cam *cam;
 833         enum v4l2_mbus_pixelcode code;
 834         const struct soc_mbus_pixelfmt *fmt;
 835
 836         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
 837         if (ret < 0)
 838                 /* No more formats */
 839                 return 0;
 840
 841         fmt = soc_mbus_get_fmtdesc(code);
 842         if (!fmt) {
 843                 dev_err(icd->dev.parent,
 844                         "Invalid format code #%d: %d\n", idx, code);
 845                 return -EINVAL;
 846         }
 847
 848         ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
 849         if (ret < 0)
 850                 return 0;
 851
 852         if (!icd->host_priv) {
 853                 cam = kzalloc(sizeof(*cam), GFP_KERNEL);
 854                 if (!cam)
 855                         return -ENOMEM;
 856
 857                 icd->host_priv = cam;
 858         } else {
 859                 cam = icd->host_priv;
 860         }
 861
 862         /* Beginning of a pass */
 863         if (!idx)
 864                 cam->extra_fmt = NULL;
 865
 866         switch (code) {
 867         case V4L2_MBUS_FMT_YUYV8_2X8_BE:
 868         case V4L2_MBUS_FMT_YVYU8_2X8_BE:
 869         case V4L2_MBUS_FMT_YUYV8_2X8_LE:
 870         case V4L2_MBUS_FMT_YVYU8_2X8_LE:
 871                 if (cam->extra_fmt)
 872                         break;
 873
 874                 /*
 875                  * Our case is simple so far: for any of the above four camera
 876                  * formats we add all our four synthesized NV* formats, so,
 877                  * just marking the device with a single flag suffices. If
 878                  * the format generation rules are more complex, you would have
 879                  * to actually hang your already added / counted formats onto
 880                  * the host_priv pointer and check whether the format you're
 881                  * going to add now is already there.
 882                  */
 883                 cam->extra_fmt = sh_mobile_ceu_formats;
 884
 885                 n = ARRAY_SIZE(sh_mobile_ceu_formats);
 886                 formats += n;
 887                 for (k = 0; xlate && k < n; k++) {
 888                         xlate->host_fmt = &sh_mobile_ceu_formats[k];
 889                         xlate->code     = code;
 890                         xlate++;
 891                         dev_dbg(dev, "Providing format %s using code %d\n",
 892                                 sh_mobile_ceu_formats[k].name, code);
 893                 }
 894                 break;
 895         default:
 896                 if (!sh_mobile_ceu_packing_supported(fmt))
 897                         return 0;
 898         }
 899
 900         /* Generic pass-through */
 901         formats++;
 902         if (xlate) {
 903                 xlate->host_fmt = fmt;
 904                 xlate->code     = code;
 905                 xlate++;
 906                 dev_dbg(dev, "Providing format %s in pass-through mode\n",
 907                         xlate->host_fmt->name);
 908         }
 909
 910         return formats;
 911 }
 912
 913 static void sh_mobile_ceu_put_formats(struct soc_camera_device *icd)
 914 {
 915         kfree(icd->host_priv);
 916         icd->host_priv = NULL;
 917 }
 918
 919 /* Check if any dimension of r1 is smaller than respective one of r2 */
 920 static bool is_smaller(struct v4l2_rect *r1, struct v4l2_rect *r2)
 921 {
 922         return r1->width < r2->width || r1->height < r2->height;
 923 }
 924
 925 /* Check if r1 fails to cover r2 */
 926 static bool is_inside(struct v4l2_rect *r1, struct v4l2_rect *r2)
 927 {
 928         return r1->left > r2->left || r1->top > r2->top ||
 929                 r1->left + r1->width < r2->left + r2->width ||
 930                 r1->top + r1->height < r2->top + r2->height;
 931 }
 932
 933 static unsigned int scale_down(unsigned int size, unsigned int scale)
 934 {
 935         return (size * 4096 + scale / 2) / scale;
 936 }
 937
 938 static unsigned int scale_up(unsigned int size, unsigned int scale)
 939 {
 940         return (size * scale + 2048) / 4096;
 941 }
 942
 943 static unsigned int calc_generic_scale(unsigned int input, unsigned int output)
 944 {
 945         return (input * 4096 + output / 2) / output;
 946 }
 947
 948 static int client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
 949 {
 950         struct v4l2_crop crop;
 951         struct v4l2_cropcap cap;
 952         int ret;
 953
 954         crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 955
 956         ret = v4l2_subdev_call(sd, video, g_crop, &crop);
 957         if (!ret) {
 958                 *rect = crop.c;
 959                 return ret;
 960         }
 961
 962         /* Camera driver doesn't support .g_crop(), assume default rectangle */
 963         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 964
 965         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 966         if (ret < 0)
 967                 return ret;
 968
 969         *rect = cap.defrect;
 970
 971         return ret;
 972 }
 973
 974 /*
 975  * The common for both scaling and cropping iterative approach is:
 976  * 1. try if the client can produce exactly what requested by the user
 977  * 2. if (1) failed, try to double the client image until we get one big enough
 978  * 3. if (2) failed, try to request the maximum image
 979  */
 980 static int client_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *crop,
 981                          struct v4l2_crop *cam_crop)
 982 {
 983         struct v4l2_rect *rect = &crop->c, *cam_rect = &cam_crop->c;
 984         struct device *dev = sd->v4l2_dev->dev;
 985         struct v4l2_cropcap cap;
 986         int ret;
 987         unsigned int width, height;
 988
 989         v4l2_subdev_call(sd, video, s_crop, crop);
 990         ret = client_g_rect(sd, cam_rect);
 991         if (ret < 0)
 992                 return ret;
 993
 994         /*
 995          * Now cam_crop contains the current camera input rectangle, and it must
 996          * be within camera cropcap bounds
 997          */
 998         if (!memcmp(rect, cam_rect, sizeof(*rect))) {
 999                 /* Even if camera S_CROP failed, but camera rectangle matches */
1000                 dev_dbg(dev, "Camera S_CROP successful for %ux%u@%u:%u\n",
1001                         rect->width, rect->height, rect->left, rect->top);
1002                 return 0;
1003         }
1004
1005         /* Try to fix cropping, that camera hasn't managed to set */
1006         dev_geo(dev, "Fix camera S_CROP for %ux%u@%u:%u to %ux%u@%u:%u\n",
1007                 cam_rect->width, cam_rect->height,
1008                 cam_rect->left, cam_rect->top,
1009                 rect->width, rect->height, rect->left, rect->top);
1010
1011         /* We need sensor maximum rectangle */
1012         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
1013         if (ret < 0)
1014                 return ret;
1015
1016         soc_camera_limit_side(&rect->left, &rect->width, cap.bounds.left, 2,
1017                               cap.bounds.width);
1018         soc_camera_limit_side(&rect->top, &rect->height, cap.bounds.top, 4,
1019                               cap.bounds.height);
1020
1021         /*
1022          * Popular special case - some cameras can only handle fixed sizes like
1023          * QVGA, VGA,... Take care to avoid infinite loop.
1024          */
1025         width = max(cam_rect->width, 2);
1026         height = max(cam_rect->height, 2);
1027
1028         while (!ret && (is_smaller(cam_rect, rect) ||
1029                         is_inside(cam_rect, rect)) &&
1030                (cap.bounds.width > width || cap.bounds.height > height)) {
1031
1032                 width *= 2;
1033                 height *= 2;
1034
1035                 cam_rect->width = width;
1036                 cam_rect->height = height;
1037
1038                 /*
1039                  * We do not know what capabilities the camera has to set up
1040                  * left and top borders. We could try to be smarter in iterating
1041                  * them, e.g., if camera current left is to the right of the
1042                  * target left, set it to the middle point between the current
1043                  * left and minimum left. But that would add too much
1044                  * complexity: we would have to iterate each border separately.
1045                  */
1046                 if (cam_rect->left > rect->left)
1047                         cam_rect->left = cap.bounds.left;
1048
1049                 if (cam_rect->left + cam_rect->width < rect->left + rect->width)
1050                         cam_rect->width = rect->left + rect->width -
1051                                 cam_rect->left;
1052
1053                 if (cam_rect->top > rect->top)
1054                         cam_rect->top = cap.bounds.top;
1055
1056                 if (cam_rect->top + cam_rect->height < rect->top + rect->height)
1057                         cam_rect->height = rect->top + rect->height -
1058                                 cam_rect->top;
1059
1060                 v4l2_subdev_call(sd, video, s_crop, cam_crop);
1061                 ret = client_g_rect(sd, cam_rect);
1062                 dev_geo(dev, "Camera S_CROP %d for %ux%u@%u:%u\n", ret,
1063                         cam_rect->width, cam_rect->height,
1064                         cam_rect->left, cam_rect->top);
1065         }
1066
1067         /* S_CROP must not modify the rectangle */
1068         if (is_smaller(cam_rect, rect) || is_inside(cam_rect, rect)) {
1069                 /*
1070                  * The camera failed to configure a suitable cropping,
1071                  * we cannot use the current rectangle, set to max
1072                  */
1073                 *cam_rect = cap.bounds;
1074                 v4l2_subdev_call(sd, video, s_crop, cam_crop);
1075                 ret = client_g_rect(sd, cam_rect);
1076                 dev_geo(dev, "Camera S_CROP %d for max %ux%u@%u:%u\n", ret,
1077                         cam_rect->width, cam_rect->height,
1078                         cam_rect->left, cam_rect->top);
1079         }
1080
1081         return ret;
1082 }
1083
1084 static int get_camera_scales(struct v4l2_subdev *sd, struct v4l2_rect *rect,
1085                              unsigned int *scale_h, unsigned int *scale_v)
1086 {
1087         struct v4l2_mbus_framefmt mf;
1088         int ret;
1089
1090         ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1091         if (ret < 0)
1092                 return ret;
1093
1094         *scale_h = calc_generic_scale(rect->width, mf.width);
1095         *scale_v = calc_generic_scale(rect->height, mf.height);
1096
1097         return 0;
1098 }
1099
1100 static int get_camera_subwin(struct soc_camera_device *icd,
1101                              struct v4l2_rect *cam_subrect,
1102                              unsigned int cam_hscale, unsigned int cam_vscale)
1103 {
1104         struct sh_mobile_ceu_cam *cam = icd->host_priv;
1105         struct v4l2_rect *ceu_rect = &cam->ceu_rect;
1106
1107         if (!ceu_rect->width) {
1108                 struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1109                 struct device *dev = icd->dev.parent;
1110                 struct v4l2_mbus_framefmt mf;
1111                 int ret;
1112                 /* First time */
1113
1114                 ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1115                 if (ret < 0)
1116                         return ret;
1117
1118                 dev_geo(dev, "camera fmt %ux%u\n", mf.width, mf.height);
1119
1120                 if (mf.width > 2560) {
1121                         ceu_rect->width  = 2560;
1122                         ceu_rect->left   = (mf.width - 2560) / 2;
1123                 } else {
1124                         ceu_rect->width  = mf.width;
1125                         ceu_rect->left   = 0;
1126                 }
1127
1128                 if (mf.height > 1920) {
1129                         ceu_rect->height = 1920;
1130                         ceu_rect->top    = (mf.height - 1920) / 2;
1131                 } else {
1132                         ceu_rect->height = mf.height;
1133                         ceu_rect->top    = 0;
1134                 }
1135
1136                 dev_geo(dev, "initialised CEU rect %ux%u@%u:%u\n",
1137                         ceu_rect->width, ceu_rect->height,
1138                         ceu_rect->left, ceu_rect->top);
1139         }
1140
1141         cam_subrect->width      = scale_up(ceu_rect->width, cam_hscale);
1142         cam_subrect->left       = scale_up(ceu_rect->left, cam_hscale);
1143         cam_subrect->height     = scale_up(ceu_rect->height, cam_vscale);
1144         cam_subrect->top        = scale_up(ceu_rect->top, cam_vscale);
1145
1146         return 0;
1147 }
1148
1149 static int client_s_fmt(struct soc_camera_device *icd,
1150                         struct v4l2_mbus_framefmt *mf, bool ceu_can_scale)
1151 {
1152         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1153         struct device *dev = icd->dev.parent;
1154         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
1155         unsigned int max_width, max_height;
1156         struct v4l2_cropcap cap;
1157         int ret;
1158
1159         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1160
1161         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
1162         if (ret < 0)
1163                 return ret;
1164
1165         max_width = min(cap.bounds.width, 2560);
1166         max_height = min(cap.bounds.height, 1920);
1167
1168         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, mf);
1169         if (ret < 0)
1170                 return ret;
1171
1172         dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
1173
1174         if ((width == mf->width && height == mf->height) || !ceu_can_scale)
1175                 return 0;
1176
1177         /* Camera set a format, but geometry is not precise, try to improve */
1178         tmp_w = mf->width;
1179         tmp_h = mf->height;
1180
1181         /* width <= max_width && height <= max_height - guaranteed by try_fmt */
1182         while ((width > tmp_w || height > tmp_h) &&
1183                tmp_w < max_width && tmp_h < max_height) {
1184                 tmp_w = min(2 * tmp_w, max_width);
1185                 tmp_h = min(2 * tmp_h, max_height);
1186                 mf->width = tmp_w;
1187                 mf->height = tmp_h;
1188                 ret = v4l2_subdev_call(sd, video, s_mbus_fmt, mf);
1189                 dev_geo(dev, "Camera scaled to %ux%u\n",
1190                         mf->width, mf->height);
1191                 if (ret < 0) {
1192                         /* This shouldn't happen */
1193                         dev_err(dev, "Client failed to set format: %d\n", ret);
1194                         return ret;
1195                 }
1196         }
1197
1198         return 0;
1199 }
1200
1201 /**
1202  * @rect        - camera cropped rectangle
1203  * @sub_rect    - CEU cropped rectangle, mapped back to camera input area
1204  * @ceu_rect    - on output calculated CEU crop rectangle
1205  */
1206 static int client_scale(struct soc_camera_device *icd, struct v4l2_rect *rect,
1207                         struct v4l2_rect *sub_rect, struct v4l2_rect *ceu_rect,
1208                         struct v4l2_mbus_framefmt *mf, bool ceu_can_scale)
1209 {
1210         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1211         struct sh_mobile_ceu_cam *cam = icd->host_priv;
1212         struct device *dev = icd->dev.parent;
1213         struct v4l2_mbus_framefmt mf_tmp = *mf;
1214         unsigned int scale_h, scale_v;
1215         int ret;
1216
1217         /* 5. Apply iterative camera S_FMT for camera user window. */
1218         ret = client_s_fmt(icd, &mf_tmp, ceu_can_scale);
1219         if (ret < 0)
1220                 return ret;
1221
1222         dev_geo(dev, "5: camera scaled to %ux%u\n",
1223                 mf_tmp.width, mf_tmp.height);
1224
1225         /* 6. Retrieve camera output window (g_fmt) */
1226
1227         /* unneeded - it is already in "mf_tmp" */
1228
1229         /* 7. Calculate new camera scales. */
1230         ret = get_camera_scales(sd, rect, &scale_h, &scale_v);
1231         if (ret < 0)
1232                 return ret;
1233
1234         dev_geo(dev, "7: camera scales %u:%u\n", scale_h, scale_v);
1235
1236         cam->cam_width  = mf_tmp.width;
1237         cam->cam_height = mf_tmp.height;
1238         mf->width       = mf_tmp.width;
1239         mf->height      = mf_tmp.height;
1240         mf->colorspace  = mf_tmp.colorspace;
1241
1242         /*
1243          * 8. Calculate new CEU crop - apply camera scales to previously
1244          *    calculated "effective" crop.
1245          */
1246         ceu_rect->left = scale_down(sub_rect->left, scale_h);
1247         ceu_rect->width = scale_down(sub_rect->width, scale_h);
1248         ceu_rect->top = scale_down(sub_rect->top, scale_v);
1249         ceu_rect->height = scale_down(sub_rect->height, scale_v);
1250
1251         dev_geo(dev, "8: new CEU rect %ux%u@%u:%u\n",
1252                 ceu_rect->width, ceu_rect->height,
1253                 ceu_rect->left, ceu_rect->top);
1254
1255         return 0;
1256 }
1257
1258 /* Get combined scales */
1259 static int get_scales(struct soc_camera_device *icd,
1260                       unsigned int *scale_h, unsigned int *scale_v)
1261 {
1262         struct sh_mobile_ceu_cam *cam = icd->host_priv;
1263         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1264         struct v4l2_crop cam_crop;
1265         unsigned int width_in, height_in;
1266         int ret;
1267
1268         cam_crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1269
1270         ret = client_g_rect(sd, &cam_crop.c);
1271         if (ret < 0)
1272                 return ret;
1273
1274         ret = get_camera_scales(sd, &cam_crop.c, scale_h, scale_v);
1275         if (ret < 0)
1276                 return ret;
1277
1278         width_in = scale_up(cam->ceu_rect.width, *scale_h);
1279         height_in = scale_up(cam->ceu_rect.height, *scale_v);
1280
1281         *scale_h = calc_generic_scale(width_in, icd->user_width);
1282         *scale_v = calc_generic_scale(height_in, icd->user_height);
1283
1284         return 0;
1285 }
1286
1287 /*
1288  * CEU can scale and crop, but we don't want to waste bandwidth and kill the
1289  * framerate by always requesting the maximum image from the client. See
1290  * Documentation/video4linux/sh_mobile_camera_ceu.txt for a description of
1291  * scaling and cropping algorithms and for the meaning of referenced here steps.
1292  */
1293 static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
1294                                   struct v4l2_crop *a)
1295 {
1296         struct v4l2_rect *rect = &a->c;
1297         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1298         struct sh_mobile_ceu_dev *pcdev = ici->priv;
1299         struct v4l2_crop cam_crop;
1300         struct sh_mobile_ceu_cam *cam = icd->host_priv;
1301         struct v4l2_rect *cam_rect = &cam_crop.c, *ceu_rect = &cam->ceu_rect;
1302         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1303         struct device *dev = icd->dev.parent;
1304         struct v4l2_mbus_framefmt mf;
1305         unsigned int scale_comb_h, scale_comb_v, scale_ceu_h, scale_ceu_v,
1306                 out_width, out_height;
1307         u32 capsr, cflcr;
1308         int ret;
1309
1310         /* 1. Calculate current combined scales. */
1311         ret = get_scales(icd, &scale_comb_h, &scale_comb_v);
1312         if (ret < 0)
1313                 return ret;
1314
1315         dev_geo(dev, "1: combined scales %u:%u\n", scale_comb_h, scale_comb_v);
1316
1317         /* 2. Apply iterative camera S_CROP for new input window. */
1318         ret = client_s_crop(sd, a, &cam_crop);
1319         if (ret < 0)
1320                 return ret;
1321
1322         dev_geo(dev, "2: camera cropped to %ux%u@%u:%u\n",
1323                 cam_rect->width, cam_rect->height,
1324                 cam_rect->left, cam_rect->top);
1325
1326         /* On success cam_crop contains current camera crop */
1327
1328         /*
1329          * 3. If old combined scales applied to new crop produce an impossible
1330          *    user window, adjust scales to produce nearest possible window.
1331          */
1332         out_width       = scale_down(rect->width, scale_comb_h);
1333         out_height      = scale_down(rect->height, scale_comb_v);
1334
1335         if (out_width > 2560)
1336                 out_width = 2560;
1337         else if (out_width < 2)
1338                 out_width = 2;
1339
1340         if (out_height > 1920)
1341                 out_height = 1920;
1342         else if (out_height < 4)
1343                 out_height = 4;
1344
1345         dev_geo(dev, "3: Adjusted output %ux%u\n", out_width, out_height);
1346
1347         /* 4. Use G_CROP to retrieve actual input window: already in cam_crop */
1348
1349         /*
1350          * 5. Using actual input window and calculated combined scales calculate
1351          *    camera target output window.
1352          */
1353         mf.width        = scale_down(cam_rect->width, scale_comb_h);
1354         mf.height       = scale_down(cam_rect->height, scale_comb_v);
1355
1356         dev_geo(dev, "5: camera target %ux%u\n", mf.width, mf.height);
1357
1358         /* 6. - 9. */
1359         mf.code         = cam->code;
1360         mf.field        = pcdev->is_interlaced ? V4L2_FIELD_INTERLACED :
1361                                                 V4L2_FIELD_NONE;
1362
1363         capsr = capture_save_reset(pcdev);
1364         dev_dbg(dev, "CAPSR 0x%x, CFLCR 0x%x\n", capsr, pcdev->cflcr);
1365
1366         /* Make relative to camera rectangle */
1367         rect->left      -= cam_rect->left;
1368         rect->top       -= cam_rect->top;
1369
1370         ret = client_scale(icd, cam_rect, rect, ceu_rect, &mf,
1371                            pcdev->image_mode && !pcdev->is_interlaced);
1372
1373         dev_geo(dev, "6-9: %d\n", ret);
1374
1375         /* 10. Use CEU cropping to crop to the new window. */
1376         sh_mobile_ceu_set_rect(icd, out_width, out_height);
1377
1378         dev_geo(dev, "10: CEU cropped to %ux%u@%u:%u\n",
1379                 ceu_rect->width, ceu_rect->height,
1380                 ceu_rect->left, ceu_rect->top);
1381
1382         /*
1383          * 11. Calculate CEU scales from camera scales from results of (10) and
1384          *     user window from (3)
1385          */
1386         scale_ceu_h = calc_scale(ceu_rect->width, &out_width);
1387         scale_ceu_v = calc_scale(ceu_rect->height, &out_height);
1388
1389         dev_geo(dev, "11: CEU scales %u:%u\n", scale_ceu_h, scale_ceu_v);
1390
1391         /* 12. Apply CEU scales. */
1392         cflcr = scale_ceu_h | (scale_ceu_v << 16);
1393         if (cflcr != pcdev->cflcr) {
1394                 pcdev->cflcr = cflcr;
1395                 ceu_write(pcdev, CFLCR, cflcr);
1396         }
1397
1398         /* Restore capture */
1399         if (pcdev->active)
1400                 capsr |= 1;
1401         capture_restore(pcdev, capsr);
1402
1403         icd->user_width = out_width;
1404         icd->user_height = out_height;
1405
1406         /* Even if only camera cropping succeeded */
1407         return ret;
1408 }
1409
1410 /* Similar to set_crop multistage iterative algorithm */
1411 static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
1412                                  struct v4l2_format *f)
1413 {
1414         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1415         struct sh_mobile_ceu_dev *pcdev = ici->priv;
1416         struct sh_mobile_ceu_cam *cam = icd->host_priv;
1417         struct v4l2_pix_format *pix = &f->fmt.pix;
1418         struct v4l2_mbus_framefmt mf;
1419         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1420         struct device *dev = icd->dev.parent;
1421         __u32 pixfmt = pix->pixelformat;
1422         const struct soc_camera_format_xlate *xlate;
1423         struct v4l2_crop cam_crop;
1424         struct v4l2_rect *cam_rect = &cam_crop.c, cam_subrect, ceu_rect;
1425         unsigned int scale_cam_h, scale_cam_v;
1426         u16 scale_v, scale_h;
1427         int ret;
1428         bool is_interlaced, image_mode;
1429
1430         switch (pix->field) {
1431         case V4L2_FIELD_INTERLACED:
1432                 is_interlaced = true;
1433                 break;
1434         case V4L2_FIELD_ANY:
1435         default:
1436                 pix->field = V4L2_FIELD_NONE;
1437                 /* fall-through */
1438         case V4L2_FIELD_NONE:
1439                 is_interlaced = false;
1440                 break;
1441         }
1442
1443         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1444         if (!xlate) {
1445                 dev_warn(dev, "Format %x not found\n", pixfmt);
1446                 return -EINVAL;
1447         }
1448
1449         /* 1. Calculate current camera scales. */
1450         cam_crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1451
1452         ret = client_g_rect(sd, cam_rect);
1453         if (ret < 0)
1454                 return ret;
1455
1456         ret = get_camera_scales(sd, cam_rect, &scale_cam_h, &scale_cam_v);
1457         if (ret < 0)
1458                 return ret;
1459
1460         dev_geo(dev, "1: camera scales %u:%u\n", scale_cam_h, scale_cam_v);
1461
1462         /*
1463          * 2. Calculate "effective" input crop (sensor subwindow) - CEU crop
1464          *    scaled back at current camera scales onto input window.
1465          */
1466         ret = get_camera_subwin(icd, &cam_subrect, scale_cam_h, scale_cam_v);
1467         if (ret < 0)
1468                 return ret;
1469
1470         dev_geo(dev, "2: subwin %ux%u@%u:%u\n",
1471                 cam_subrect.width, cam_subrect.height,
1472                 cam_subrect.left, cam_subrect.top);
1473
1474         /*
1475          * 3. Calculate new combined scales from "effective" input window to
1476          *    requested user window.
1477          */
1478         scale_h = calc_generic_scale(cam_subrect.width, pix->width);
1479         scale_v = calc_generic_scale(cam_subrect.height, pix->height);
1480
1481         dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
1482
1483         /*
1484          * 4. Calculate camera output window by applying combined scales to real
1485          *    input window.
1486          */
1487         mf.width        = scale_down(cam_rect->width, scale_h);
1488         mf.height       = scale_down(cam_rect->height, scale_v);
1489         mf.field        = pix->field;
1490         mf.colorspace   = pix->colorspace;
1491         mf.code         = xlate->code;
1492
1493         switch (pixfmt) {
1494         case V4L2_PIX_FMT_NV12:
1495         case V4L2_PIX_FMT_NV21:
1496         case V4L2_PIX_FMT_NV16:
1497         case V4L2_PIX_FMT_NV61:
1498                 image_mode = true;
1499                 break;
1500         default:
1501                 image_mode = false;
1502         }
1503
1504         dev_geo(dev, "4: camera output %ux%u\n", mf.width, mf.height);
1505
1506         /* 5. - 9. */
1507         ret = client_scale(icd, cam_rect, &cam_subrect, &ceu_rect, &mf,
1508                            image_mode && !is_interlaced);
1509
1510         dev_geo(dev, "5-9: client scale %d\n", ret);
1511
1512         /* Done with the camera. Now see if we can improve the result */
1513
1514         dev_dbg(dev, "Camera %d fmt %ux%u, requested %ux%u\n",
1515                 ret, mf.width, mf.height, pix->width, pix->height);
1516         if (ret < 0)
1517                 return ret;
1518
1519         if (mf.code != xlate->code)
1520                 return -EINVAL;
1521
1522         /* 10. Use CEU scaling to scale to the requested user window. */
1523
1524         /* We cannot scale up */
1525         if (pix->width > mf.width)
1526                 pix->width = mf.width;
1527         if (pix->width > ceu_rect.width)
1528                 pix->width = ceu_rect.width;
1529
1530         if (pix->height > mf.height)
1531                 pix->height = mf.height;
1532         if (pix->height > ceu_rect.height)
1533                 pix->height = ceu_rect.height;
1534
1535         pix->colorspace = mf.colorspace;
1536
1537         if (image_mode) {
1538                 /* Scale pix->{width x height} down to width x height */
1539                 scale_h = calc_scale(ceu_rect.width, &pix->width);
1540                 scale_v = calc_scale(ceu_rect.height, &pix->height);
1541
1542                 pcdev->cflcr = scale_h | (scale_v << 16);
1543         } else {
1544                 pix->width = ceu_rect.width;
1545                 pix->height = ceu_rect.height;
1546                 scale_h = scale_v = 0;
1547                 pcdev->cflcr = 0;
1548         }
1549
1550         dev_geo(dev, "10: W: %u : 0x%x = %u, H: %u : 0x%x = %u\n",
1551                 ceu_rect.width, scale_h, pix->width,
1552                 ceu_rect.height, scale_v, pix->height);
1553
1554         cam->code               = xlate->code;
1555         cam->ceu_rect           = ceu_rect;
1556         icd->current_fmt        = xlate;
1557
1558         pcdev->is_interlaced = is_interlaced;
1559         pcdev->image_mode = image_mode;
1560
1561         return 0;
1562 }
1563
1564 static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
1565                                  struct v4l2_format *f)
1566 {
1567         const struct soc_camera_format_xlate *xlate;
1568         struct v4l2_pix_format *pix = &f->fmt.pix;
1569         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1570         struct v4l2_mbus_framefmt mf;
1571         __u32 pixfmt = pix->pixelformat;
1572         int width, height;
1573         int ret;
1574
1575         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1576         if (!xlate) {
1577                 dev_warn(icd->dev.parent, "Format %x not found\n", pixfmt);
1578                 return -EINVAL;
1579         }
1580
1581         /* FIXME: calculate using depth and bus width */
1582
1583         v4l_bound_align_image(&pix->width, 2, 2560, 1,
1584                               &pix->height, 4, 1920, 2, 0);
1585
1586         width = pix->width;
1587         height = pix->height;
1588
1589         pix->bytesperline = soc_mbus_bytes_per_line(width, xlate->host_fmt);
1590         if (pix->bytesperline < 0)
1591                 return pix->bytesperline;
1592         pix->sizeimage = height * pix->bytesperline;
1593
1594         /* limit to sensor capabilities */
1595         mf.width        = pix->width;
1596         mf.height       = pix->height;
1597         mf.field        = pix->field;
1598         mf.code         = xlate->code;
1599         mf.colorspace   = pix->colorspace;
1600
1601         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1602         if (ret < 0)
1603                 return ret;
1604
1605         pix->width      = mf.width;
1606         pix->height     = mf.height;
1607         pix->field      = mf.field;
1608         pix->colorspace = mf.colorspace;
1609
1610         switch (pixfmt) {
1611         case V4L2_PIX_FMT_NV12:
1612         case V4L2_PIX_FMT_NV21:
1613         case V4L2_PIX_FMT_NV16:
1614         case V4L2_PIX_FMT_NV61:
1615                 /* FIXME: check against rect_max after converting soc-camera */
1616                 /* We can scale precisely, need a bigger image from camera */
1617                 if (pix->width < width || pix->height < height) {
1618                         /*
1619                          * We presume, the sensor behaves sanely, i.e., if
1620                          * requested a bigger rectangle, it will not return a
1621                          * smaller one.
1622                          */
1623                         mf.width = 2560;
1624                         mf.height = 1920;
1625                         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1626                         if (ret < 0) {
1627                                 /* Shouldn't actually happen... */
1628                                 dev_err(icd->dev.parent,
1629                                         "FIXME: client try_fmt() = %d\n", ret);
1630                                 return ret;
1631                         }
1632                 }
1633                 /* We will scale exactly */
1634                 if (mf.width > width)
1635                         pix->width = width;
1636                 if (mf.height > height)
1637                         pix->height = height;
1638         }
1639
1640         return ret;
1641 }
1642
1643 static int sh_mobile_ceu_reqbufs(struct soc_camera_file *icf,
1644                                  struct v4l2_requestbuffers *p)
1645 {
1646         int i;
1647
1648         /*
1649          * This is for locking debugging only. I removed spinlocks and now I
1650          * check whether .prepare is ever called on a linked buffer, or whether
1651          * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1652          * it hadn't triggered
1653          */
1654         for (i = 0; i < p->count; i++) {
1655                 struct sh_mobile_ceu_buffer *buf;
1656
1657                 buf = container_of(icf->vb_vidq.bufs[i],
1658                                    struct sh_mobile_ceu_buffer, vb);
1659                 INIT_LIST_HEAD(&buf->vb.queue);
1660         }
1661
1662         return 0;
1663 }
1664
1665 static unsigned int sh_mobile_ceu_poll(struct file *file, poll_table *pt)
1666 {
1667         struct soc_camera_file *icf = file->private_data;
1668         struct sh_mobile_ceu_buffer *buf;
1669
1670         buf = list_entry(icf->vb_vidq.stream.next,
1671                          struct sh_mobile_ceu_buffer, vb.stream);
1672
1673         poll_wait(file, &buf->vb.done, pt);
1674
1675         if (buf->vb.state == VIDEOBUF_DONE ||
1676             buf->vb.state == VIDEOBUF_ERROR)
1677                 return POLLIN|POLLRDNORM;
1678
1679         return 0;
1680 }
1681
1682 static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
1683                                   struct v4l2_capability *cap)
1684 {
1685         strlcpy(cap->card, "SuperH_Mobile_CEU", sizeof(cap->card));
1686         cap->version = KERNEL_VERSION(0, 0, 5);
1687         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1688         return 0;
1689 }
1690
1691 static void sh_mobile_ceu_init_videobuf(struct videobuf_queue *q,
1692                                         struct soc_camera_device *icd)
1693 {
1694         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1695         struct sh_mobile_ceu_dev *pcdev = ici->priv;
1696
1697         videobuf_queue_dma_contig_init(q,
1698                                        &sh_mobile_ceu_videobuf_ops,
1699                                        icd->dev.parent, &pcdev->lock,
1700                                        V4L2_BUF_TYPE_VIDEO_CAPTURE,
1701                                        pcdev->is_interlaced ?
1702                                        V4L2_FIELD_INTERLACED : V4L2_FIELD_NONE,
1703                                        sizeof(struct sh_mobile_ceu_buffer),
1704                                        icd);
1705 }
1706
1707 static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
1708                                   struct v4l2_control *ctrl)
1709 {
1710         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1711         struct sh_mobile_ceu_dev *pcdev = ici->priv;
1712         u32 val;
1713
1714         switch (ctrl->id) {
1715         case V4L2_CID_SHARPNESS:
1716                 val = ceu_read(pcdev, CLFCR);
1717                 ctrl->value = val ^ 1;
1718                 return 0;
1719         }
1720         return -ENOIOCTLCMD;
1721 }
1722
1723 static int sh_mobile_ceu_set_ctrl(struct soc_camera_device *icd,
1724                                   struct v4l2_control *ctrl)
1725 {
1726         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
1727         struct sh_mobile_ceu_dev *pcdev = ici->priv;
1728
1729         switch (ctrl->id) {
1730         case V4L2_CID_SHARPNESS:
1731                 switch (icd->current_fmt->host_fmt->fourcc) {
1732                 case V4L2_PIX_FMT_NV12:
1733                 case V4L2_PIX_FMT_NV21:
1734                 case V4L2_PIX_FMT_NV16:
1735                 case V4L2_PIX_FMT_NV61:
1736                         ceu_write(pcdev, CLFCR, !ctrl->value);
1737                         return 0;
1738                 }
1739                 return -EINVAL;
1740         }
1741         return -ENOIOCTLCMD;
1742 }
1743
1744 static const struct v4l2_queryctrl sh_mobile_ceu_controls[] = {
1745         {
1746                 .id             = V4L2_CID_SHARPNESS,
1747                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
1748                 .name           = "Low-pass filter",
1749                 .minimum        = 0,
1750                 .maximum        = 1,
1751                 .step           = 1,
1752                 .default_value  = 0,
1753         },
1754 };
1755
1756 static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
1757         .owner          = THIS_MODULE,
1758         .add            = sh_mobile_ceu_add_device,
1759         .remove         = sh_mobile_ceu_remove_device,
1760         .get_formats    = sh_mobile_ceu_get_formats,
1761         .put_formats    = sh_mobile_ceu_put_formats,
1762         .set_crop       = sh_mobile_ceu_set_crop,
1763         .set_fmt        = sh_mobile_ceu_set_fmt,
1764         .try_fmt        = sh_mobile_ceu_try_fmt,
1765         .set_ctrl       = sh_mobile_ceu_set_ctrl,
1766         .get_ctrl       = sh_mobile_ceu_get_ctrl,
1767         .reqbufs        = sh_mobile_ceu_reqbufs,
1768         .poll           = sh_mobile_ceu_poll,
1769         .querycap       = sh_mobile_ceu_querycap,
1770         .set_bus_param  = sh_mobile_ceu_set_bus_param,
1771         .init_videobuf  = sh_mobile_ceu_init_videobuf,
1772         .controls       = sh_mobile_ceu_controls,
1773         .num_controls   = ARRAY_SIZE(sh_mobile_ceu_controls),
1774 };
1775
1776 static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
1777 {
1778         struct sh_mobile_ceu_dev *pcdev;
1779         struct resource *res;
1780         void __iomem *base;
1781         unsigned int irq;
1782         int err = 0;
1783
1784         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1785         irq = platform_get_irq(pdev, 0);
1786         if (!res || !irq) {
1787                 dev_err(&pdev->dev, "Not enough CEU platform resources.\n");
1788                 err = -ENODEV;
1789                 goto exit;
1790         }
1791
1792         pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1793         if (!pcdev) {
1794                 dev_err(&pdev->dev, "Could not allocate pcdev\n");
1795                 err = -ENOMEM;
1796                 goto exit;
1797         }
1798
1799         INIT_LIST_HEAD(&pcdev->capture);
1800         spin_lock_init(&pcdev->lock);
1801
1802         pcdev->pdata = pdev->dev.platform_data;
1803         if (!pcdev->pdata) {
1804                 err = -EINVAL;
1805                 dev_err(&pdev->dev, "CEU platform data not set.\n");
1806                 goto exit_kfree;
1807         }
1808
1809         base = ioremap_nocache(res->start, resource_size(res));
1810         if (!base) {
1811                 err = -ENXIO;
1812                 dev_err(&pdev->dev, "Unable to ioremap CEU registers.\n");
1813                 goto exit_kfree;
1814         }
1815
1816         pcdev->irq = irq;
1817         pcdev->base = base;
1818         pcdev->video_limit = 0; /* only enabled if second resource exists */
1819
1820         res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1821         if (res) {
1822                 err = dma_declare_coherent_memory(&pdev->dev, res->start,
1823                                                   res->start,
1824                                                   resource_size(res),
1825                                                   DMA_MEMORY_MAP |
1826                                                   DMA_MEMORY_EXCLUSIVE);
1827                 if (!err) {
1828                         dev_err(&pdev->dev, "Unable to declare CEU memory.\n");
1829                         err = -ENXIO;
1830                         goto exit_iounmap;
1831                 }
1832
1833                 pcdev->video_limit = resource_size(res);
1834         }
1835
1836         /* request irq */
1837         err = request_irq(pcdev->irq, sh_mobile_ceu_irq, IRQF_DISABLED,
1838                           dev_name(&pdev->dev), pcdev);
1839         if (err) {
1840                 dev_err(&pdev->dev, "Unable to register CEU interrupt.\n");
1841                 goto exit_release_mem;
1842         }
1843
1844         pm_suspend_ignore_children(&pdev->dev, true);
1845         pm_runtime_enable(&pdev->dev);
1846         pm_runtime_resume(&pdev->dev);
1847
1848         pcdev->ici.priv = pcdev;
1849         pcdev->ici.v4l2_dev.dev = &pdev->dev;
1850         pcdev->ici.nr = pdev->id;
1851         pcdev->ici.drv_name = dev_name(&pdev->dev);
1852         pcdev->ici.ops = &sh_mobile_ceu_host_ops;
1853
1854         err = soc_camera_host_register(&pcdev->ici);
1855         if (err)
1856                 goto exit_free_clk;
1857
1858         return 0;
1859
1860 exit_free_clk:
1861         pm_runtime_disable(&pdev->dev);
1862         free_irq(pcdev->irq, pcdev);
1863 exit_release_mem:
1864         if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
1865                 dma_release_declared_memory(&pdev->dev);
1866 exit_iounmap:
1867         iounmap(base);
1868 exit_kfree:
1869         kfree(pcdev);
1870 exit:
1871         return err;
1872 }
1873
1874 static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
1875 {
1876         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
1877         struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
1878                                         struct sh_mobile_ceu_dev, ici);
1879
1880         soc_camera_host_unregister(soc_host);
1881         pm_runtime_disable(&pdev->dev);
1882         free_irq(pcdev->irq, pcdev);
1883         if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
1884                 dma_release_declared_memory(&pdev->dev);
1885         iounmap(pcdev->base);
1886         kfree(pcdev);
1887         return 0;
1888 }
1889
1890 static int sh_mobile_ceu_runtime_nop(struct device *dev)
1891 {
1892         /* Runtime PM callback shared between ->runtime_suspend()
1893          * and ->runtime_resume(). Simply returns success.
1894          *
1895          * This driver re-initializes all registers after
1896          * pm_runtime_get_sync() anyway so there is no need
1897          * to save and restore registers here.
1898          */
1899         return 0;
1900 }
1901
1902 static const struct dev_pm_ops sh_mobile_ceu_dev_pm_ops = {
1903         .runtime_suspend = sh_mobile_ceu_runtime_nop,
1904         .runtime_resume = sh_mobile_ceu_runtime_nop,
1905 };
1906
1907 static struct platform_driver sh_mobile_ceu_driver = {
1908         .driver         = {
1909                 .name   = "sh_mobile_ceu",
1910                 .pm     = &sh_mobile_ceu_dev_pm_ops,
1911         },
1912         .probe          = sh_mobile_ceu_probe,
1913         .remove         = __devexit_p(sh_mobile_ceu_remove),
1914 };
1915
1916 static int __init sh_mobile_ceu_init(void)
1917 {
1918         return platform_driver_register(&sh_mobile_ceu_driver);
1919 }
1920
1921 static void __exit sh_mobile_ceu_exit(void)
1922 {
1923         platform_driver_unregister(&sh_mobile_ceu_driver);
1924 }
1925
1926 module_init(sh_mobile_ceu_init);
1927 module_exit(sh_mobile_ceu_exit);
1928
1929 MODULE_DESCRIPTION("SuperH Mobile CEU driver");
1930 MODULE_AUTHOR("Magnus Damm");
1931 MODULE_LICENSE("GPL");
1932 MODULE_ALIAS("platform:sh_mobile_ceu");