← Back to branch summary

~ubuntu-branches/ubuntu/precise/linux-lowlatency/precise

« back to all changes in this revision

Viewing changes to drivers/media/video/sh_mobile_ceu_camera.c

  • Committer: Package Import Robot
  • Author(s): Alessio Igor Bogani
  • Date: 2011-10-26 11:13:05 UTC
  • Revision ID: package-import@ubuntu.com-20111026111305-tz023xykf0i6eosh
Tags: upstream-3.2.0
Import upstream version 3.2.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
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/completion.h>
 
21
#include <linux/delay.h>
 
22
#include <linux/dma-mapping.h>
 
23
#include <linux/errno.h>
 
24
#include <linux/fs.h>
 
25
#include <linux/interrupt.h>
 
26
#include <linux/kernel.h>
 
27
#include <linux/mm.h>
 
28
#include <linux/moduleparam.h>
 
29
#include <linux/time.h>
 
30
#include <linux/slab.h>
 
31
#include <linux/device.h>
 
32
#include <linux/platform_device.h>
 
33
#include <linux/videodev2.h>
 
34
#include <linux/pm_runtime.h>
 
35
#include <linux/sched.h>
 
36
 
 
37
#include <media/v4l2-common.h>
 
38
#include <media/v4l2-dev.h>
 
39
#include <media/soc_camera.h>
 
40
#include <media/sh_mobile_ceu.h>
 
41
#include <media/sh_mobile_csi2.h>
 
42
#include <media/videobuf2-dma-contig.h>
 
43
#include <media/v4l2-mediabus.h>
 
44
#include <media/soc_mediabus.h>
 
45
 
 
46
/* register offsets for sh7722 / sh7723 */
 
47
 
 
48
#define CAPSR  0x00 /* Capture start register */
 
49
#define CAPCR  0x04 /* Capture control register */
 
50
#define CAMCR  0x08 /* Capture interface control register */
 
51
#define CMCYR  0x0c /* Capture interface cycle  register */
 
52
#define CAMOR  0x10 /* Capture interface offset register */
 
53
#define CAPWR  0x14 /* Capture interface width register */
 
54
#define CAIFR  0x18 /* Capture interface input format register */
 
55
#define CSTCR  0x20 /* Camera strobe control register (<= sh7722) */
 
56
#define CSECR  0x24 /* Camera strobe emission count register (<= sh7722) */
 
57
#define CRCNTR 0x28 /* CEU register control register */
 
58
#define CRCMPR 0x2c /* CEU register forcible control register */
 
59
#define CFLCR  0x30 /* Capture filter control register */
 
60
#define CFSZR  0x34 /* Capture filter size clip register */
 
61
#define CDWDR  0x38 /* Capture destination width register */
 
62
#define CDAYR  0x3c /* Capture data address Y register */
 
63
#define CDACR  0x40 /* Capture data address C register */
 
64
#define CDBYR  0x44 /* Capture data bottom-field address Y register */
 
65
#define CDBCR  0x48 /* Capture data bottom-field address C register */
 
66
#define CBDSR  0x4c /* Capture bundle destination size register */
 
67
#define CFWCR  0x5c /* Firewall operation control register */
 
68
#define CLFCR  0x60 /* Capture low-pass filter control register */
 
69
#define CDOCR  0x64 /* Capture data output control register */
 
70
#define CDDCR  0x68 /* Capture data complexity level register */
 
71
#define CDDAR  0x6c /* Capture data complexity level address register */
 
72
#define CEIER  0x70 /* Capture event interrupt enable register */
 
73
#define CETCR  0x74 /* Capture event flag clear register */
 
74
#define CSTSR  0x7c /* Capture status register */
 
75
#define CSRTR  0x80 /* Capture software reset register */
 
76
#define CDSSR  0x84 /* Capture data size register */
 
77
#define CDAYR2 0x90 /* Capture data address Y register 2 */
 
78
#define CDACR2 0x94 /* Capture data address C register 2 */
 
79
#define CDBYR2 0x98 /* Capture data bottom-field address Y register 2 */
 
80
#define CDBCR2 0x9c /* Capture data bottom-field address C register 2 */
 
81
 
 
82
#undef DEBUG_GEOMETRY
 
83
#ifdef DEBUG_GEOMETRY
 
84
#define dev_geo dev_info
 
85
#else
 
86
#define dev_geo dev_dbg
 
87
#endif
 
88
 
 
89
/* per video frame buffer */
 
90
struct sh_mobile_ceu_buffer {
 
91
        struct vb2_buffer vb; /* v4l buffer must be first */
 
92
        struct list_head queue;
 
93
};
 
94
 
 
95
struct sh_mobile_ceu_dev {
 
96
        struct soc_camera_host ici;
 
97
        struct soc_camera_device *icd;
 
98
        struct platform_device *csi2_pdev;
 
99
 
 
100
        unsigned int irq;
 
101
        void __iomem *base;
 
102
        size_t video_limit;
 
103
        size_t buf_total;
 
104
 
 
105
        spinlock_t lock;                /* Protects video buffer lists */
 
106
        struct list_head capture;
 
107
        struct vb2_buffer *active;
 
108
        struct vb2_alloc_ctx *alloc_ctx;
 
109
 
 
110
        struct sh_mobile_ceu_info *pdata;
 
111
        struct completion complete;
 
112
 
 
113
        u32 cflcr;
 
114
 
 
115
        enum v4l2_field field;
 
116
        int sequence;
 
117
 
 
118
        unsigned int image_mode:1;
 
119
        unsigned int is_16bit:1;
 
120
        unsigned int frozen:1;
 
121
};
 
122
 
 
123
struct sh_mobile_ceu_cam {
 
124
        /* CEU offsets within the camera output, before the CEU scaler */
 
125
        unsigned int ceu_left;
 
126
        unsigned int ceu_top;
 
127
        /* Client output, as seen by the CEU */
 
128
        unsigned int width;
 
129
        unsigned int height;
 
130
        /*
 
131
         * User window from S_CROP / G_CROP, produced by client cropping and
 
132
         * scaling, CEU scaling and CEU cropping, mapped back onto the client
 
133
         * input window
 
134
         */
 
135
        struct v4l2_rect subrect;
 
136
        /* Camera cropping rectangle */
 
137
        struct v4l2_rect rect;
 
138
        const struct soc_mbus_pixelfmt *extra_fmt;
 
139
        enum v4l2_mbus_pixelcode code;
 
140
};
 
141
 
 
142
static struct sh_mobile_ceu_buffer *to_ceu_vb(struct vb2_buffer *vb)
 
143
{
 
144
        return container_of(vb, struct sh_mobile_ceu_buffer, vb);
 
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->pdev, "soft reset time out\n");
 
186
                return -EIO;
 
187
        }
 
188
 
 
189
        return 0;
 
190
}
 
191
 
 
192
/*
 
193
 *  Videobuf operations
 
194
 */
 
195
 
 
196
/*
 
197
 * .queue_setup() is called to check, whether the driver can accept the
 
198
 *                requested number of buffers and to fill in plane sizes
 
199
 *                for the current frame format if required
 
200
 */
 
201
static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
 
202
                        const struct v4l2_format *fmt,
 
203
                        unsigned int *count, unsigned int *num_planes,
 
204
                        unsigned int sizes[], void *alloc_ctxs[])
 
205
{
 
206
        struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
 
207
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
208
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
209
        int bytes_per_line;
 
210
        unsigned int height;
 
211
 
 
212
        if (fmt) {
 
213
                const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
 
214
                                                                fmt->fmt.pix.pixelformat);
 
215
                if (!xlate)
 
216
                        return -EINVAL;
 
217
                bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
 
218
                                                         xlate->host_fmt);
 
219
                height = fmt->fmt.pix.height;
 
220
        } else {
 
221
                /* Called from VIDIOC_REQBUFS or in compatibility mode */
 
222
                bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 
223
                                                icd->current_fmt->host_fmt);
 
224
                height = icd->user_height;
 
225
        }
 
226
        if (bytes_per_line < 0)
 
227
                return bytes_per_line;
 
228
 
 
229
        sizes[0] = bytes_per_line * height;
 
230
 
 
231
        alloc_ctxs[0] = pcdev->alloc_ctx;
 
232
 
 
233
        if (!vq->num_buffers)
 
234
                pcdev->sequence = 0;
 
235
 
 
236
        if (!*count)
 
237
                *count = 2;
 
238
 
 
239
        /* If *num_planes != 0, we have already verified *count. */
 
240
        if (pcdev->video_limit && !*num_planes) {
 
241
                size_t size = PAGE_ALIGN(sizes[0]) * *count;
 
242
 
 
243
                if (size + pcdev->buf_total > pcdev->video_limit)
 
244
                        *count = (pcdev->video_limit - pcdev->buf_total) /
 
245
                                PAGE_ALIGN(sizes[0]);
 
246
        }
 
247
 
 
248
        *num_planes = 1;
 
249
 
 
250
        dev_dbg(icd->parent, "count=%d, size=%u\n", *count, sizes[0]);
 
251
 
 
252
        return 0;
 
253
}
 
254
 
 
255
#define CEU_CETCR_MAGIC 0x0317f313 /* acknowledge magical interrupt sources */
 
256
#define CEU_CETCR_IGRW (1 << 4) /* prohibited register access interrupt bit */
 
257
#define CEU_CEIER_CPEIE (1 << 0) /* one-frame capture end interrupt */
 
258
#define CEU_CEIER_VBP   (1 << 20) /* vbp error */
 
259
#define CEU_CAPCR_CTNCP (1 << 16) /* continuous capture mode (if set) */
 
260
#define CEU_CEIER_MASK (CEU_CEIER_CPEIE | CEU_CEIER_VBP)
 
261
 
 
262
 
 
263
/*
 
264
 * return value doesn't reflex the success/failure to queue the new buffer,
 
265
 * but rather the status of the previous buffer.
 
266
 */
 
267
static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 
268
{
 
269
        struct soc_camera_device *icd = pcdev->icd;
 
270
        dma_addr_t phys_addr_top, phys_addr_bottom;
 
271
        unsigned long top1, top2;
 
272
        unsigned long bottom1, bottom2;
 
273
        u32 status;
 
274
        bool planar;
 
275
        int ret = 0;
 
276
 
 
277
        /*
 
278
         * The hardware is _very_ picky about this sequence. Especially
 
279
         * the CEU_CETCR_MAGIC value. It seems like we need to acknowledge
 
280
         * several not-so-well documented interrupt sources in CETCR.
 
281
         */
 
282
        ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) & ~CEU_CEIER_MASK);
 
283
        status = ceu_read(pcdev, CETCR);
 
284
        ceu_write(pcdev, CETCR, ~status & CEU_CETCR_MAGIC);
 
285
        if (!pcdev->frozen)
 
286
                ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) | CEU_CEIER_MASK);
 
287
        ceu_write(pcdev, CAPCR, ceu_read(pcdev, CAPCR) & ~CEU_CAPCR_CTNCP);
 
288
        ceu_write(pcdev, CETCR, CEU_CETCR_MAGIC ^ CEU_CETCR_IGRW);
 
289
 
 
290
        /*
 
291
         * When a VBP interrupt occurs, a capture end interrupt does not occur
 
292
         * and the image of that frame is not captured correctly. So, soft reset
 
293
         * is needed here.
 
294
         */
 
295
        if (status & CEU_CEIER_VBP) {
 
296
                sh_mobile_ceu_soft_reset(pcdev);
 
297
                ret = -EIO;
 
298
        }
 
299
 
 
300
        if (pcdev->frozen) {
 
301
                complete(&pcdev->complete);
 
302
                return ret;
 
303
        }
 
304
 
 
305
        if (!pcdev->active)
 
306
                return ret;
 
307
 
 
308
        if (V4L2_FIELD_INTERLACED_BT == pcdev->field) {
 
309
                top1    = CDBYR;
 
310
                top2    = CDBCR;
 
311
                bottom1 = CDAYR;
 
312
                bottom2 = CDACR;
 
313
        } else {
 
314
                top1    = CDAYR;
 
315
                top2    = CDACR;
 
316
                bottom1 = CDBYR;
 
317
                bottom2 = CDBCR;
 
318
        }
 
319
 
 
320
        phys_addr_top = vb2_dma_contig_plane_dma_addr(pcdev->active, 0);
 
321
 
 
322
        switch (icd->current_fmt->host_fmt->fourcc) {
 
323
        case V4L2_PIX_FMT_NV12:
 
324
        case V4L2_PIX_FMT_NV21:
 
325
        case V4L2_PIX_FMT_NV16:
 
326
        case V4L2_PIX_FMT_NV61:
 
327
                planar = true;
 
328
                break;
 
329
        default:
 
330
                planar = false;
 
331
        }
 
332
 
 
333
        ceu_write(pcdev, top1, phys_addr_top);
 
334
        if (V4L2_FIELD_NONE != pcdev->field) {
 
335
                if (planar)
 
336
                        phys_addr_bottom = phys_addr_top + icd->user_width;
 
337
                else
 
338
                        phys_addr_bottom = phys_addr_top +
 
339
                                soc_mbus_bytes_per_line(icd->user_width,
 
340
                                                        icd->current_fmt->host_fmt);
 
341
                ceu_write(pcdev, bottom1, phys_addr_bottom);
 
342
        }
 
343
 
 
344
        if (planar) {
 
345
                phys_addr_top += icd->user_width *
 
346
                        icd->user_height;
 
347
                ceu_write(pcdev, top2, phys_addr_top);
 
348
                if (V4L2_FIELD_NONE != pcdev->field) {
 
349
                        phys_addr_bottom = phys_addr_top + icd->user_width;
 
350
                        ceu_write(pcdev, bottom2, phys_addr_bottom);
 
351
                }
 
352
        }
 
353
 
 
354
        ceu_write(pcdev, CAPSR, 0x1); /* start capture */
 
355
 
 
356
        return ret;
 
357
}
 
358
 
 
359
static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 
360
{
 
361
        struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 
362
 
 
363
        /* Added list head initialization on alloc */
 
364
        WARN(!list_empty(&buf->queue), "Buffer %p on queue!\n", vb);
 
365
 
 
366
        return 0;
 
367
}
 
368
 
 
369
static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 
370
{
 
371
        struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 
372
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
373
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
374
        struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 
375
        unsigned long size;
 
376
        int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 
377
                                                icd->current_fmt->host_fmt);
 
378
 
 
379
        if (bytes_per_line < 0)
 
380
                goto error;
 
381
 
 
382
        size = icd->user_height * bytes_per_line;
 
383
 
 
384
        if (vb2_plane_size(vb, 0) < size) {
 
385
                dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
 
386
                        vb->v4l2_buf.index, vb2_plane_size(vb, 0), size);
 
387
                goto error;
 
388
        }
 
389
 
 
390
        vb2_set_plane_payload(vb, 0, size);
 
391
 
 
392
        dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
 
393
                vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
394
 
 
395
#ifdef DEBUG
 
396
        /*
 
397
         * This can be useful if you want to see if we actually fill
 
398
         * the buffer with something
 
399
         */
 
400
        if (vb2_plane_vaddr(vb, 0))
 
401
                memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
 
402
#endif
 
403
 
 
404
        spin_lock_irq(&pcdev->lock);
 
405
        list_add_tail(&buf->queue, &pcdev->capture);
 
406
 
 
407
        if (!pcdev->active) {
 
408
                /*
 
409
                 * Because there were no active buffer at this moment,
 
410
                 * we are not interested in the return value of
 
411
                 * sh_mobile_ceu_capture here.
 
412
                 */
 
413
                pcdev->active = vb;
 
414
                sh_mobile_ceu_capture(pcdev);
 
415
        }
 
416
        spin_unlock_irq(&pcdev->lock);
 
417
 
 
418
        return;
 
419
 
 
420
error:
 
421
        vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 
422
}
 
423
 
 
424
static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
 
425
{
 
426
        struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 
427
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
428
        struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 
429
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
430
 
 
431
        spin_lock_irq(&pcdev->lock);
 
432
 
 
433
        if (pcdev->active == vb) {
 
434
                /* disable capture (release DMA buffer), reset */
 
435
                ceu_write(pcdev, CAPSR, 1 << 16);
 
436
                pcdev->active = NULL;
 
437
        }
 
438
 
 
439
        /*
 
440
         * Doesn't hurt also if the list is empty, but it hurts, if queuing the
 
441
         * buffer failed, and .buf_init() hasn't been called
 
442
         */
 
443
        if (buf->queue.next)
 
444
                list_del_init(&buf->queue);
 
445
 
 
446
        pcdev->buf_total -= PAGE_ALIGN(vb2_plane_size(vb, 0));
 
447
        dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
 
448
                pcdev->buf_total);
 
449
 
 
450
        spin_unlock_irq(&pcdev->lock);
 
451
}
 
452
 
 
453
static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
 
454
{
 
455
        struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 
456
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
457
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
458
 
 
459
        pcdev->buf_total += PAGE_ALIGN(vb2_plane_size(vb, 0));
 
460
        dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
 
461
                pcdev->buf_total);
 
462
 
 
463
        /* This is for locking debugging only */
 
464
        INIT_LIST_HEAD(&to_ceu_vb(vb)->queue);
 
465
        return 0;
 
466
}
 
467
 
 
468
static int sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
 
469
{
 
470
        struct soc_camera_device *icd = container_of(q, struct soc_camera_device, vb2_vidq);
 
471
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
472
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
473
        struct list_head *buf_head, *tmp;
 
474
 
 
475
        spin_lock_irq(&pcdev->lock);
 
476
 
 
477
        pcdev->active = NULL;
 
478
 
 
479
        list_for_each_safe(buf_head, tmp, &pcdev->capture)
 
480
                list_del_init(buf_head);
 
481
 
 
482
        spin_unlock_irq(&pcdev->lock);
 
483
 
 
484
        return sh_mobile_ceu_soft_reset(pcdev);
 
485
}
 
486
 
 
487
static struct vb2_ops sh_mobile_ceu_videobuf_ops = {
 
488
        .queue_setup    = sh_mobile_ceu_videobuf_setup,
 
489
        .buf_prepare    = sh_mobile_ceu_videobuf_prepare,
 
490
        .buf_queue      = sh_mobile_ceu_videobuf_queue,
 
491
        .buf_cleanup    = sh_mobile_ceu_videobuf_release,
 
492
        .buf_init       = sh_mobile_ceu_videobuf_init,
 
493
        .wait_prepare   = soc_camera_unlock,
 
494
        .wait_finish    = soc_camera_lock,
 
495
        .stop_streaming = sh_mobile_ceu_stop_streaming,
 
496
};
 
497
 
 
498
static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
 
499
{
 
500
        struct sh_mobile_ceu_dev *pcdev = data;
 
501
        struct vb2_buffer *vb;
 
502
        int ret;
 
503
 
 
504
        spin_lock(&pcdev->lock);
 
505
 
 
506
        vb = pcdev->active;
 
507
        if (!vb)
 
508
                /* Stale interrupt from a released buffer */
 
509
                goto out;
 
510
 
 
511
        list_del_init(&to_ceu_vb(vb)->queue);
 
512
 
 
513
        if (!list_empty(&pcdev->capture))
 
514
                pcdev->active = &list_entry(pcdev->capture.next,
 
515
                                            struct sh_mobile_ceu_buffer, queue)->vb;
 
516
        else
 
517
                pcdev->active = NULL;
 
518
 
 
519
        ret = sh_mobile_ceu_capture(pcdev);
 
520
        do_gettimeofday(&vb->v4l2_buf.timestamp);
 
521
        if (!ret) {
 
522
                vb->v4l2_buf.field = pcdev->field;
 
523
                vb->v4l2_buf.sequence = pcdev->sequence++;
 
524
        }
 
525
        vb2_buffer_done(vb, ret < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 
526
 
 
527
out:
 
528
        spin_unlock(&pcdev->lock);
 
529
 
 
530
        return IRQ_HANDLED;
 
531
}
 
532
 
 
533
static struct v4l2_subdev *find_csi2(struct sh_mobile_ceu_dev *pcdev)
 
534
{
 
535
        struct v4l2_subdev *sd;
 
536
 
 
537
        if (!pcdev->csi2_pdev)
 
538
                return NULL;
 
539
 
 
540
        v4l2_device_for_each_subdev(sd, &pcdev->ici.v4l2_dev)
 
541
                if (&pcdev->csi2_pdev->dev == v4l2_get_subdevdata(sd))
 
542
                        return sd;
 
543
 
 
544
        return NULL;
 
545
}
 
546
 
 
547
/* Called with .video_lock held */
 
548
static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
 
549
{
 
550
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
551
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
552
        struct v4l2_subdev *csi2_sd;
 
553
        int ret;
 
554
 
 
555
        if (pcdev->icd)
 
556
                return -EBUSY;
 
557
 
 
558
        dev_info(icd->parent,
 
559
                 "SuperH Mobile CEU driver attached to camera %d\n",
 
560
                 icd->devnum);
 
561
 
 
562
        pm_runtime_get_sync(ici->v4l2_dev.dev);
 
563
 
 
564
        pcdev->buf_total = 0;
 
565
 
 
566
        ret = sh_mobile_ceu_soft_reset(pcdev);
 
567
 
 
568
        csi2_sd = find_csi2(pcdev);
 
569
        if (csi2_sd) {
 
570
                csi2_sd->grp_id = soc_camera_grp_id(icd);
 
571
                v4l2_set_subdev_hostdata(csi2_sd, icd);
 
572
        }
 
573
 
 
574
        ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
 
575
        if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV) {
 
576
                pm_runtime_put_sync(ici->v4l2_dev.dev);
 
577
                return ret;
 
578
        }
 
579
 
 
580
        /*
 
581
         * -ENODEV is special: either csi2_sd == NULL or the CSI-2 driver
 
582
         * has not found this soc-camera device among its clients
 
583
         */
 
584
        if (ret == -ENODEV && csi2_sd)
 
585
                csi2_sd->grp_id = 0;
 
586
        pcdev->icd = icd;
 
587
 
 
588
        return 0;
 
589
}
 
590
 
 
591
/* Called with .video_lock held */
 
592
static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 
593
{
 
594
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
595
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
596
        struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
 
597
 
 
598
        BUG_ON(icd != pcdev->icd);
 
599
 
 
600
        v4l2_subdev_call(csi2_sd, core, s_power, 0);
 
601
        if (csi2_sd)
 
602
                csi2_sd->grp_id = 0;
 
603
        /* disable capture, disable interrupts */
 
604
        ceu_write(pcdev, CEIER, 0);
 
605
        sh_mobile_ceu_soft_reset(pcdev);
 
606
 
 
607
        /* make sure active buffer is canceled */
 
608
        spin_lock_irq(&pcdev->lock);
 
609
        if (pcdev->active) {
 
610
                list_del_init(&to_ceu_vb(pcdev->active)->queue);
 
611
                vb2_buffer_done(pcdev->active, VB2_BUF_STATE_ERROR);
 
612
                pcdev->active = NULL;
 
613
        }
 
614
        spin_unlock_irq(&pcdev->lock);
 
615
 
 
616
        pm_runtime_put_sync(ici->v4l2_dev.dev);
 
617
 
 
618
        dev_info(icd->parent,
 
619
                 "SuperH Mobile CEU driver detached from camera %d\n",
 
620
                 icd->devnum);
 
621
 
 
622
        pcdev->icd = NULL;
 
623
}
 
624
 
 
625
/*
 
626
 * See chapter 29.4.12 "Capture Filter Control Register (CFLCR)"
 
627
 * in SH7722 Hardware Manual
 
628
 */
 
629
static unsigned int size_dst(unsigned int src, unsigned int scale)
 
630
{
 
631
        unsigned int mant_pre = scale >> 12;
 
632
        if (!src || !scale)
 
633
                return src;
 
634
        return ((mant_pre + 2 * (src - 1)) / (2 * mant_pre) - 1) *
 
635
                mant_pre * 4096 / scale + 1;
 
636
}
 
637
 
 
638
static u16 calc_scale(unsigned int src, unsigned int *dst)
 
639
{
 
640
        u16 scale;
 
641
 
 
642
        if (src == *dst)
 
643
                return 0;
 
644
 
 
645
        scale = (src * 4096 / *dst) & ~7;
 
646
 
 
647
        while (scale > 4096 && size_dst(src, scale) < *dst)
 
648
                scale -= 8;
 
649
 
 
650
        *dst = size_dst(src, scale);
 
651
 
 
652
        return scale;
 
653
}
 
654
 
 
655
/* rect is guaranteed to not exceed the scaled camera rectangle */
 
656
static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
 
657
{
 
658
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
659
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
660
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
661
        unsigned int height, width, cdwdr_width, in_width, in_height;
 
662
        unsigned int left_offset, top_offset;
 
663
        u32 camor;
 
664
 
 
665
        dev_geo(icd->parent, "Crop %ux%u@%u:%u\n",
 
666
                icd->user_width, icd->user_height, cam->ceu_left, cam->ceu_top);
 
667
 
 
668
        left_offset     = cam->ceu_left;
 
669
        top_offset      = cam->ceu_top;
 
670
 
 
671
        WARN_ON(icd->user_width & 3 || icd->user_height & 3);
 
672
 
 
673
        width = icd->user_width;
 
674
 
 
675
        if (pcdev->image_mode) {
 
676
                in_width = cam->width;
 
677
                if (!pcdev->is_16bit) {
 
678
                        in_width *= 2;
 
679
                        left_offset *= 2;
 
680
                }
 
681
                cdwdr_width = width;
 
682
        } else {
 
683
                int bytes_per_line = soc_mbus_bytes_per_line(width,
 
684
                                                icd->current_fmt->host_fmt);
 
685
                unsigned int w_factor;
 
686
 
 
687
                switch (icd->current_fmt->host_fmt->packing) {
 
688
                case SOC_MBUS_PACKING_2X8_PADHI:
 
689
                        w_factor = 2;
 
690
                        break;
 
691
                default:
 
692
                        w_factor = 1;
 
693
                }
 
694
 
 
695
                in_width = cam->width * w_factor;
 
696
                left_offset *= w_factor;
 
697
 
 
698
                if (bytes_per_line < 0)
 
699
                        cdwdr_width = width;
 
700
                else
 
701
                        cdwdr_width = bytes_per_line;
 
702
        }
 
703
 
 
704
        height = icd->user_height;
 
705
        in_height = cam->height;
 
706
        if (V4L2_FIELD_NONE != pcdev->field) {
 
707
                height = (height / 2) & ~3;
 
708
                in_height /= 2;
 
709
                top_offset /= 2;
 
710
                cdwdr_width *= 2;
 
711
        }
 
712
 
 
713
        /* CSI2 special configuration */
 
714
        if (pcdev->pdata->csi2) {
 
715
                in_width = ((in_width - 2) * 2);
 
716
                left_offset *= 2;
 
717
        }
 
718
 
 
719
        /* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
 
720
        camor = left_offset | (top_offset << 16);
 
721
 
 
722
        dev_geo(icd->parent,
 
723
                "CAMOR 0x%x, CAPWR 0x%x, CFSZR 0x%x, CDWDR 0x%x\n", camor,
 
724
                (in_height << 16) | in_width, (height << 16) | width,
 
725
                cdwdr_width);
 
726
 
 
727
        ceu_write(pcdev, CAMOR, camor);
 
728
        ceu_write(pcdev, CAPWR, (in_height << 16) | in_width);
 
729
        /* CFSZR clipping is applied _after_ the scaling filter (CFLCR) */
 
730
        ceu_write(pcdev, CFSZR, (height << 16) | width);
 
731
        ceu_write(pcdev, CDWDR, cdwdr_width);
 
732
}
 
733
 
 
734
static u32 capture_save_reset(struct sh_mobile_ceu_dev *pcdev)
 
735
{
 
736
        u32 capsr = ceu_read(pcdev, CAPSR);
 
737
        ceu_write(pcdev, CAPSR, 1 << 16); /* reset, stop capture */
 
738
        return capsr;
 
739
}
 
740
 
 
741
static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 
742
{
 
743
        unsigned long timeout = jiffies + 10 * HZ;
 
744
 
 
745
        /*
 
746
         * Wait until the end of the current frame. It can take a long time,
 
747
         * but if it has been aborted by a CAPSR reset, it shoule exit sooner.
 
748
         */
 
749
        while ((ceu_read(pcdev, CSTSR) & 1) && time_before(jiffies, timeout))
 
750
                msleep(1);
 
751
 
 
752
        if (time_after(jiffies, timeout)) {
 
753
                dev_err(pcdev->ici.v4l2_dev.dev,
 
754
                        "Timeout waiting for frame end! Interface problem?\n");
 
755
                return;
 
756
        }
 
757
 
 
758
        /* Wait until reset clears, this shall not hang... */
 
759
        while (ceu_read(pcdev, CAPSR) & (1 << 16))
 
760
                udelay(10);
 
761
 
 
762
        /* Anything to restore? */
 
763
        if (capsr & ~(1 << 16))
 
764
                ceu_write(pcdev, CAPSR, capsr);
 
765
}
 
766
 
 
767
/* Find the bus subdevice driver, e.g., CSI2 */
 
768
static struct v4l2_subdev *find_bus_subdev(struct sh_mobile_ceu_dev *pcdev,
 
769
                                           struct soc_camera_device *icd)
 
770
{
 
771
        if (pcdev->csi2_pdev) {
 
772
                struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
 
773
                if (csi2_sd && csi2_sd->grp_id == soc_camera_grp_id(icd))
 
774
                        return csi2_sd;
 
775
        }
 
776
 
 
777
        return soc_camera_to_subdev(icd);
 
778
}
 
779
 
 
780
#define CEU_BUS_FLAGS (V4L2_MBUS_MASTER |       \
 
781
                V4L2_MBUS_PCLK_SAMPLE_RISING |  \
 
782
                V4L2_MBUS_HSYNC_ACTIVE_HIGH |   \
 
783
                V4L2_MBUS_HSYNC_ACTIVE_LOW |    \
 
784
                V4L2_MBUS_VSYNC_ACTIVE_HIGH |   \
 
785
                V4L2_MBUS_VSYNC_ACTIVE_LOW |    \
 
786
                V4L2_MBUS_DATA_ACTIVE_HIGH)
 
787
 
 
788
/* Capture is not running, no interrupts, no locking needed */
 
789
static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 
790
                                       __u32 pixfmt)
 
791
{
 
792
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
793
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
794
        struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
 
795
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
796
        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 
797
        unsigned long value, common_flags = CEU_BUS_FLAGS;
 
798
        u32 capsr = capture_save_reset(pcdev);
 
799
        unsigned int yuv_lineskip;
 
800
        int ret;
 
801
 
 
802
        /*
 
803
         * If the client doesn't implement g_mbus_config, we just use our
 
804
         * platform data
 
805
         */
 
806
        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 
807
        if (!ret) {
 
808
                common_flags = soc_mbus_config_compatible(&cfg,
 
809
                                                          common_flags);
 
810
                if (!common_flags)
 
811
                        return -EINVAL;
 
812
        } else if (ret != -ENOIOCTLCMD) {
 
813
                return ret;
 
814
        }
 
815
 
 
816
        /* Make choises, based on platform preferences */
 
817
        if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
 
818
            (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
 
819
                if (pcdev->pdata->flags & SH_CEU_FLAG_HSYNC_LOW)
 
820
                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
 
821
                else
 
822
                        common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
 
823
        }
 
824
 
 
825
        if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
 
826
            (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
 
827
                if (pcdev->pdata->flags & SH_CEU_FLAG_VSYNC_LOW)
 
828
                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
 
829
                else
 
830
                        common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
 
831
        }
 
832
 
 
833
        cfg.flags = common_flags;
 
834
        ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 
835
        if (ret < 0 && ret != -ENOIOCTLCMD)
 
836
                return ret;
 
837
 
 
838
        if (icd->current_fmt->host_fmt->bits_per_sample > 8)
 
839
                pcdev->is_16bit = 1;
 
840
        else
 
841
                pcdev->is_16bit = 0;
 
842
 
 
843
        ceu_write(pcdev, CRCNTR, 0);
 
844
        ceu_write(pcdev, CRCMPR, 0);
 
845
 
 
846
        value = 0x00000010; /* data fetch by default */
 
847
        yuv_lineskip = 0x10;
 
848
 
 
849
        switch (icd->current_fmt->host_fmt->fourcc) {
 
850
        case V4L2_PIX_FMT_NV12:
 
851
        case V4L2_PIX_FMT_NV21:
 
852
                /* convert 4:2:2 -> 4:2:0 */
 
853
                yuv_lineskip = 0; /* skip for NV12/21, no skip for NV16/61 */
 
854
                /* fall-through */
 
855
        case V4L2_PIX_FMT_NV16:
 
856
        case V4L2_PIX_FMT_NV61:
 
857
                switch (cam->code) {
 
858
                case V4L2_MBUS_FMT_UYVY8_2X8:
 
859
                        value = 0x00000000; /* Cb0, Y0, Cr0, Y1 */
 
860
                        break;
 
861
                case V4L2_MBUS_FMT_VYUY8_2X8:
 
862
                        value = 0x00000100; /* Cr0, Y0, Cb0, Y1 */
 
863
                        break;
 
864
                case V4L2_MBUS_FMT_YUYV8_2X8:
 
865
                        value = 0x00000200; /* Y0, Cb0, Y1, Cr0 */
 
866
                        break;
 
867
                case V4L2_MBUS_FMT_YVYU8_2X8:
 
868
                        value = 0x00000300; /* Y0, Cr0, Y1, Cb0 */
 
869
                        break;
 
870
                default:
 
871
                        BUG();
 
872
                }
 
873
        }
 
874
 
 
875
        if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV21 ||
 
876
            icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV61)
 
877
                value ^= 0x00000100; /* swap U, V to change from NV1x->NVx1 */
 
878
 
 
879
        value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
 
880
        value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
 
881
        value |= pcdev->is_16bit ? 1 << 12 : 0;
 
882
 
 
883
        /* CSI2 mode */
 
884
        if (pcdev->pdata->csi2)
 
885
                value |= 3 << 12;
 
886
 
 
887
        ceu_write(pcdev, CAMCR, value);
 
888
 
 
889
        ceu_write(pcdev, CAPCR, 0x00300000);
 
890
 
 
891
        switch (pcdev->field) {
 
892
        case V4L2_FIELD_INTERLACED_TB:
 
893
                value = 0x101;
 
894
                break;
 
895
        case V4L2_FIELD_INTERLACED_BT:
 
896
                value = 0x102;
 
897
                break;
 
898
        default:
 
899
                value = 0;
 
900
                break;
 
901
        }
 
902
        ceu_write(pcdev, CAIFR, value);
 
903
 
 
904
        sh_mobile_ceu_set_rect(icd);
 
905
        mdelay(1);
 
906
 
 
907
        dev_geo(icd->parent, "CFLCR 0x%x\n", pcdev->cflcr);
 
908
        ceu_write(pcdev, CFLCR, pcdev->cflcr);
 
909
 
 
910
        /*
 
911
         * A few words about byte order (observed in Big Endian mode)
 
912
         *
 
913
         * In data fetch mode bytes are received in chunks of 8 bytes.
 
914
         * D0, D1, D2, D3, D4, D5, D6, D7 (D0 received first)
 
915
         *
 
916
         * The data is however by default written to memory in reverse order:
 
917
         * D7, D6, D5, D4, D3, D2, D1, D0 (D7 written to lowest byte)
 
918
         *
 
919
         * The lowest three bits of CDOCR allows us to do swapping,
 
920
         * using 7 we swap the data bytes to match the incoming order:
 
921
         * D0, D1, D2, D3, D4, D5, D6, D7
 
922
         */
 
923
        value = 0x00000007 | yuv_lineskip;
 
924
 
 
925
        ceu_write(pcdev, CDOCR, value);
 
926
        ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
 
927
 
 
928
        dev_dbg(icd->parent, "S_FMT successful for %c%c%c%c %ux%u\n",
 
929
                pixfmt & 0xff, (pixfmt >> 8) & 0xff,
 
930
                (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
 
931
                icd->user_width, icd->user_height);
 
932
 
 
933
        capture_restore(pcdev, capsr);
 
934
 
 
935
        /* not in bundle mode: skip CBDSR, CDAYR2, CDACR2, CDBYR2, CDBCR2 */
 
936
        return 0;
 
937
}
 
938
 
 
939
static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
 
940
                                       unsigned char buswidth)
 
941
{
 
942
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
943
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
944
        struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
 
945
        unsigned long common_flags = CEU_BUS_FLAGS;
 
946
        struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 
947
        int ret;
 
948
 
 
949
        ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 
950
        if (!ret)
 
951
                common_flags = soc_mbus_config_compatible(&cfg,
 
952
                                                          common_flags);
 
953
        else if (ret != -ENOIOCTLCMD)
 
954
                return ret;
 
955
 
 
956
        if (!common_flags || buswidth > 16)
 
957
                return -EINVAL;
 
958
 
 
959
        return 0;
 
960
}
 
961
 
 
962
static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
 
963
        {
 
964
                .fourcc                 = V4L2_PIX_FMT_NV12,
 
965
                .name                   = "NV12",
 
966
                .bits_per_sample        = 8,
 
967
                .packing                = SOC_MBUS_PACKING_1_5X8,
 
968
                .order                  = SOC_MBUS_ORDER_LE,
 
969
        }, {
 
970
                .fourcc                 = V4L2_PIX_FMT_NV21,
 
971
                .name                   = "NV21",
 
972
                .bits_per_sample        = 8,
 
973
                .packing                = SOC_MBUS_PACKING_1_5X8,
 
974
                .order                  = SOC_MBUS_ORDER_LE,
 
975
        }, {
 
976
                .fourcc                 = V4L2_PIX_FMT_NV16,
 
977
                .name                   = "NV16",
 
978
                .bits_per_sample        = 8,
 
979
                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
 
980
                .order                  = SOC_MBUS_ORDER_LE,
 
981
        }, {
 
982
                .fourcc                 = V4L2_PIX_FMT_NV61,
 
983
                .name                   = "NV61",
 
984
                .bits_per_sample        = 8,
 
985
                .packing                = SOC_MBUS_PACKING_2X8_PADHI,
 
986
                .order                  = SOC_MBUS_ORDER_LE,
 
987
        },
 
988
};
 
989
 
 
990
/* This will be corrected as we get more formats */
 
991
static bool sh_mobile_ceu_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 
992
{
 
993
        return  fmt->packing == SOC_MBUS_PACKING_NONE ||
 
994
                (fmt->bits_per_sample == 8 &&
 
995
                 fmt->packing == SOC_MBUS_PACKING_1_5X8) ||
 
996
                (fmt->bits_per_sample == 8 &&
 
997
                 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
 
998
                (fmt->bits_per_sample > 8 &&
 
999
                 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 
1000
}
 
1001
 
 
1002
static int client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect);
 
1003
 
 
1004
static struct soc_camera_device *ctrl_to_icd(struct v4l2_ctrl *ctrl)
 
1005
{
 
1006
        return container_of(ctrl->handler, struct soc_camera_device,
 
1007
                                                        ctrl_handler);
 
1008
}
 
1009
 
 
1010
static int sh_mobile_ceu_s_ctrl(struct v4l2_ctrl *ctrl)
 
1011
{
 
1012
        struct soc_camera_device *icd = ctrl_to_icd(ctrl);
 
1013
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
1014
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
1015
 
 
1016
        switch (ctrl->id) {
 
1017
        case V4L2_CID_SHARPNESS:
 
1018
                switch (icd->current_fmt->host_fmt->fourcc) {
 
1019
                case V4L2_PIX_FMT_NV12:
 
1020
                case V4L2_PIX_FMT_NV21:
 
1021
                case V4L2_PIX_FMT_NV16:
 
1022
                case V4L2_PIX_FMT_NV61:
 
1023
                        ceu_write(pcdev, CLFCR, !ctrl->val);
 
1024
                        return 0;
 
1025
                }
 
1026
                break;
 
1027
        }
 
1028
 
 
1029
        return -EINVAL;
 
1030
}
 
1031
 
 
1032
static const struct v4l2_ctrl_ops sh_mobile_ceu_ctrl_ops = {
 
1033
        .s_ctrl = sh_mobile_ceu_s_ctrl,
 
1034
};
 
1035
 
 
1036
static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int idx,
 
1037
                                     struct soc_camera_format_xlate *xlate)
 
1038
{
 
1039
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1040
        struct device *dev = icd->parent;
 
1041
        struct soc_camera_host *ici = to_soc_camera_host(dev);
 
1042
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
1043
        int ret, k, n;
 
1044
        int formats = 0;
 
1045
        struct sh_mobile_ceu_cam *cam;
 
1046
        enum v4l2_mbus_pixelcode code;
 
1047
        const struct soc_mbus_pixelfmt *fmt;
 
1048
 
 
1049
        ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
 
1050
        if (ret < 0)
 
1051
                /* No more formats */
 
1052
                return 0;
 
1053
 
 
1054
        fmt = soc_mbus_get_fmtdesc(code);
 
1055
        if (!fmt) {
 
1056
                dev_warn(dev, "unsupported format code #%u: %d\n", idx, code);
 
1057
                return 0;
 
1058
        }
 
1059
 
 
1060
        if (!pcdev->pdata->csi2) {
 
1061
                /* Are there any restrictions in the CSI-2 case? */
 
1062
                ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
 
1063
                if (ret < 0)
 
1064
                        return 0;
 
1065
        }
 
1066
 
 
1067
        if (!icd->host_priv) {
 
1068
                struct v4l2_mbus_framefmt mf;
 
1069
                struct v4l2_rect rect;
 
1070
                int shift = 0;
 
1071
 
 
1072
                /* Add our control */
 
1073
                v4l2_ctrl_new_std(&icd->ctrl_handler, &sh_mobile_ceu_ctrl_ops,
 
1074
                                  V4L2_CID_SHARPNESS, 0, 1, 1, 0);
 
1075
                if (icd->ctrl_handler.error)
 
1076
                        return icd->ctrl_handler.error;
 
1077
 
 
1078
                /* FIXME: subwindow is lost between close / open */
 
1079
 
 
1080
                /* Cache current client geometry */
 
1081
                ret = client_g_rect(sd, &rect);
 
1082
                if (ret < 0)
 
1083
                        return ret;
 
1084
 
 
1085
                /* First time */
 
1086
                ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 
1087
                if (ret < 0)
 
1088
                        return ret;
 
1089
 
 
1090
                while ((mf.width > 2560 || mf.height > 1920) && shift < 4) {
 
1091
                        /* Try 2560x1920, 1280x960, 640x480, 320x240 */
 
1092
                        mf.width        = 2560 >> shift;
 
1093
                        mf.height       = 1920 >> shift;
 
1094
                        ret = v4l2_device_call_until_err(sd->v4l2_dev,
 
1095
                                        soc_camera_grp_id(icd), video,
 
1096
                                        s_mbus_fmt, &mf);
 
1097
                        if (ret < 0)
 
1098
                                return ret;
 
1099
                        shift++;
 
1100
                }
 
1101
 
 
1102
                if (shift == 4) {
 
1103
                        dev_err(dev, "Failed to configure the client below %ux%x\n",
 
1104
                                mf.width, mf.height);
 
1105
                        return -EIO;
 
1106
                }
 
1107
 
 
1108
                dev_geo(dev, "camera fmt %ux%u\n", mf.width, mf.height);
 
1109
 
 
1110
                cam = kzalloc(sizeof(*cam), GFP_KERNEL);
 
1111
                if (!cam)
 
1112
                        return -ENOMEM;
 
1113
 
 
1114
                /* We are called with current camera crop, initialise subrect with it */
 
1115
                cam->rect       = rect;
 
1116
                cam->subrect    = rect;
 
1117
 
 
1118
                cam->width      = mf.width;
 
1119
                cam->height     = mf.height;
 
1120
 
 
1121
                icd->host_priv = cam;
 
1122
        } else {
 
1123
                cam = icd->host_priv;
 
1124
        }
 
1125
 
 
1126
        /* Beginning of a pass */
 
1127
        if (!idx)
 
1128
                cam->extra_fmt = NULL;
 
1129
 
 
1130
        switch (code) {
 
1131
        case V4L2_MBUS_FMT_UYVY8_2X8:
 
1132
        case V4L2_MBUS_FMT_VYUY8_2X8:
 
1133
        case V4L2_MBUS_FMT_YUYV8_2X8:
 
1134
        case V4L2_MBUS_FMT_YVYU8_2X8:
 
1135
                if (cam->extra_fmt)
 
1136
                        break;
 
1137
 
 
1138
                /*
 
1139
                 * Our case is simple so far: for any of the above four camera
 
1140
                 * formats we add all our four synthesized NV* formats, so,
 
1141
                 * just marking the device with a single flag suffices. If
 
1142
                 * the format generation rules are more complex, you would have
 
1143
                 * to actually hang your already added / counted formats onto
 
1144
                 * the host_priv pointer and check whether the format you're
 
1145
                 * going to add now is already there.
 
1146
                 */
 
1147
                cam->extra_fmt = sh_mobile_ceu_formats;
 
1148
 
 
1149
                n = ARRAY_SIZE(sh_mobile_ceu_formats);
 
1150
                formats += n;
 
1151
                for (k = 0; xlate && k < n; k++) {
 
1152
                        xlate->host_fmt = &sh_mobile_ceu_formats[k];
 
1153
                        xlate->code     = code;
 
1154
                        xlate++;
 
1155
                        dev_dbg(dev, "Providing format %s using code %d\n",
 
1156
                                sh_mobile_ceu_formats[k].name, code);
 
1157
                }
 
1158
                break;
 
1159
        default:
 
1160
                if (!sh_mobile_ceu_packing_supported(fmt))
 
1161
                        return 0;
 
1162
        }
 
1163
 
 
1164
        /* Generic pass-through */
 
1165
        formats++;
 
1166
        if (xlate) {
 
1167
                xlate->host_fmt = fmt;
 
1168
                xlate->code     = code;
 
1169
                xlate++;
 
1170
                dev_dbg(dev, "Providing format %s in pass-through mode\n",
 
1171
                        fmt->name);
 
1172
        }
 
1173
 
 
1174
        return formats;
 
1175
}
 
1176
 
 
1177
static void sh_mobile_ceu_put_formats(struct soc_camera_device *icd)
 
1178
{
 
1179
        kfree(icd->host_priv);
 
1180
        icd->host_priv = NULL;
 
1181
}
 
1182
 
 
1183
/* Check if any dimension of r1 is smaller than respective one of r2 */
 
1184
static bool is_smaller(struct v4l2_rect *r1, struct v4l2_rect *r2)
 
1185
{
 
1186
        return r1->width < r2->width || r1->height < r2->height;
 
1187
}
 
1188
 
 
1189
/* Check if r1 fails to cover r2 */
 
1190
static bool is_inside(struct v4l2_rect *r1, struct v4l2_rect *r2)
 
1191
{
 
1192
        return r1->left > r2->left || r1->top > r2->top ||
 
1193
                r1->left + r1->width < r2->left + r2->width ||
 
1194
                r1->top + r1->height < r2->top + r2->height;
 
1195
}
 
1196
 
 
1197
static unsigned int scale_down(unsigned int size, unsigned int scale)
 
1198
{
 
1199
        return (size * 4096 + scale / 2) / scale;
 
1200
}
 
1201
 
 
1202
static unsigned int calc_generic_scale(unsigned int input, unsigned int output)
 
1203
{
 
1204
        return (input * 4096 + output / 2) / output;
 
1205
}
 
1206
 
 
1207
/* Get and store current client crop */
 
1208
static int client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
 
1209
{
 
1210
        struct v4l2_crop crop;
 
1211
        struct v4l2_cropcap cap;
 
1212
        int ret;
 
1213
 
 
1214
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
1215
 
 
1216
        ret = v4l2_subdev_call(sd, video, g_crop, &crop);
 
1217
        if (!ret) {
 
1218
                *rect = crop.c;
 
1219
                return ret;
 
1220
        }
 
1221
 
 
1222
        /* Camera driver doesn't support .g_crop(), assume default rectangle */
 
1223
        cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
1224
 
 
1225
        ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 
1226
        if (!ret)
 
1227
                *rect = cap.defrect;
 
1228
 
 
1229
        return ret;
 
1230
}
 
1231
 
 
1232
/* Client crop has changed, update our sub-rectangle to remain within the area */
 
1233
static void update_subrect(struct sh_mobile_ceu_cam *cam)
 
1234
{
 
1235
        struct v4l2_rect *rect = &cam->rect, *subrect = &cam->subrect;
 
1236
 
 
1237
        if (rect->width < subrect->width)
 
1238
                subrect->width = rect->width;
 
1239
 
 
1240
        if (rect->height < subrect->height)
 
1241
                subrect->height = rect->height;
 
1242
 
 
1243
        if (rect->left > subrect->left)
 
1244
                subrect->left = rect->left;
 
1245
        else if (rect->left + rect->width >
 
1246
                 subrect->left + subrect->width)
 
1247
                subrect->left = rect->left + rect->width -
 
1248
                        subrect->width;
 
1249
 
 
1250
        if (rect->top > subrect->top)
 
1251
                subrect->top = rect->top;
 
1252
        else if (rect->top + rect->height >
 
1253
                 subrect->top + subrect->height)
 
1254
                subrect->top = rect->top + rect->height -
 
1255
                        subrect->height;
 
1256
}
 
1257
 
 
1258
/*
 
1259
 * The common for both scaling and cropping iterative approach is:
 
1260
 * 1. try if the client can produce exactly what requested by the user
 
1261
 * 2. if (1) failed, try to double the client image until we get one big enough
 
1262
 * 3. if (2) failed, try to request the maximum image
 
1263
 */
 
1264
static int client_s_crop(struct soc_camera_device *icd, struct v4l2_crop *crop,
 
1265
                         struct v4l2_crop *cam_crop)
 
1266
{
 
1267
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1268
        struct v4l2_rect *rect = &crop->c, *cam_rect = &cam_crop->c;
 
1269
        struct device *dev = sd->v4l2_dev->dev;
 
1270
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1271
        struct v4l2_cropcap cap;
 
1272
        int ret;
 
1273
        unsigned int width, height;
 
1274
 
 
1275
        v4l2_subdev_call(sd, video, s_crop, crop);
 
1276
        ret = client_g_rect(sd, cam_rect);
 
1277
        if (ret < 0)
 
1278
                return ret;
 
1279
 
 
1280
        /*
 
1281
         * Now cam_crop contains the current camera input rectangle, and it must
 
1282
         * be within camera cropcap bounds
 
1283
         */
 
1284
        if (!memcmp(rect, cam_rect, sizeof(*rect))) {
 
1285
                /* Even if camera S_CROP failed, but camera rectangle matches */
 
1286
                dev_dbg(dev, "Camera S_CROP successful for %dx%d@%d:%d\n",
 
1287
                        rect->width, rect->height, rect->left, rect->top);
 
1288
                cam->rect = *cam_rect;
 
1289
                return 0;
 
1290
        }
 
1291
 
 
1292
        /* Try to fix cropping, that camera hasn't managed to set */
 
1293
        dev_geo(dev, "Fix camera S_CROP for %dx%d@%d:%d to %dx%d@%d:%d\n",
 
1294
                cam_rect->width, cam_rect->height,
 
1295
                cam_rect->left, cam_rect->top,
 
1296
                rect->width, rect->height, rect->left, rect->top);
 
1297
 
 
1298
        /* We need sensor maximum rectangle */
 
1299
        ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 
1300
        if (ret < 0)
 
1301
                return ret;
 
1302
 
 
1303
        /* Put user requested rectangle within sensor bounds */
 
1304
        soc_camera_limit_side(&rect->left, &rect->width, cap.bounds.left, 2,
 
1305
                              cap.bounds.width);
 
1306
        soc_camera_limit_side(&rect->top, &rect->height, cap.bounds.top, 4,
 
1307
                              cap.bounds.height);
 
1308
 
 
1309
        /*
 
1310
         * Popular special case - some cameras can only handle fixed sizes like
 
1311
         * QVGA, VGA,... Take care to avoid infinite loop.
 
1312
         */
 
1313
        width = max(cam_rect->width, 2);
 
1314
        height = max(cam_rect->height, 2);
 
1315
 
 
1316
        /*
 
1317
         * Loop as long as sensor is not covering the requested rectangle and
 
1318
         * is still within its bounds
 
1319
         */
 
1320
        while (!ret && (is_smaller(cam_rect, rect) ||
 
1321
                        is_inside(cam_rect, rect)) &&
 
1322
               (cap.bounds.width > width || cap.bounds.height > height)) {
 
1323
 
 
1324
                width *= 2;
 
1325
                height *= 2;
 
1326
 
 
1327
                cam_rect->width = width;
 
1328
                cam_rect->height = height;
 
1329
 
 
1330
                /*
 
1331
                 * We do not know what capabilities the camera has to set up
 
1332
                 * left and top borders. We could try to be smarter in iterating
 
1333
                 * them, e.g., if camera current left is to the right of the
 
1334
                 * target left, set it to the middle point between the current
 
1335
                 * left and minimum left. But that would add too much
 
1336
                 * complexity: we would have to iterate each border separately.
 
1337
                 * Instead we just drop to the left and top bounds.
 
1338
                 */
 
1339
                if (cam_rect->left > rect->left)
 
1340
                        cam_rect->left = cap.bounds.left;
 
1341
 
 
1342
                if (cam_rect->left + cam_rect->width < rect->left + rect->width)
 
1343
                        cam_rect->width = rect->left + rect->width -
 
1344
                                cam_rect->left;
 
1345
 
 
1346
                if (cam_rect->top > rect->top)
 
1347
                        cam_rect->top = cap.bounds.top;
 
1348
 
 
1349
                if (cam_rect->top + cam_rect->height < rect->top + rect->height)
 
1350
                        cam_rect->height = rect->top + rect->height -
 
1351
                                cam_rect->top;
 
1352
 
 
1353
                v4l2_subdev_call(sd, video, s_crop, cam_crop);
 
1354
                ret = client_g_rect(sd, cam_rect);
 
1355
                dev_geo(dev, "Camera S_CROP %d for %dx%d@%d:%d\n", ret,
 
1356
                        cam_rect->width, cam_rect->height,
 
1357
                        cam_rect->left, cam_rect->top);
 
1358
        }
 
1359
 
 
1360
        /* S_CROP must not modify the rectangle */
 
1361
        if (is_smaller(cam_rect, rect) || is_inside(cam_rect, rect)) {
 
1362
                /*
 
1363
                 * The camera failed to configure a suitable cropping,
 
1364
                 * we cannot use the current rectangle, set to max
 
1365
                 */
 
1366
                *cam_rect = cap.bounds;
 
1367
                v4l2_subdev_call(sd, video, s_crop, cam_crop);
 
1368
                ret = client_g_rect(sd, cam_rect);
 
1369
                dev_geo(dev, "Camera S_CROP %d for max %dx%d@%d:%d\n", ret,
 
1370
                        cam_rect->width, cam_rect->height,
 
1371
                        cam_rect->left, cam_rect->top);
 
1372
        }
 
1373
 
 
1374
        if (!ret) {
 
1375
                cam->rect = *cam_rect;
 
1376
                update_subrect(cam);
 
1377
        }
 
1378
 
 
1379
        return ret;
 
1380
}
 
1381
 
 
1382
/* Iterative s_mbus_fmt, also updates cached client crop on success */
 
1383
static int client_s_fmt(struct soc_camera_device *icd,
 
1384
                        struct v4l2_mbus_framefmt *mf, bool ceu_can_scale)
 
1385
{
 
1386
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1387
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1388
        struct device *dev = icd->parent;
 
1389
        unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
 
1390
        unsigned int max_width, max_height;
 
1391
        struct v4l2_cropcap cap;
 
1392
        bool ceu_1to1;
 
1393
        int ret;
 
1394
 
 
1395
        ret = v4l2_device_call_until_err(sd->v4l2_dev,
 
1396
                                         soc_camera_grp_id(icd), video,
 
1397
                                         s_mbus_fmt, mf);
 
1398
        if (ret < 0)
 
1399
                return ret;
 
1400
 
 
1401
        dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
 
1402
 
 
1403
        if (width == mf->width && height == mf->height) {
 
1404
                /* Perfect! The client has done it all. */
 
1405
                ceu_1to1 = true;
 
1406
                goto update_cache;
 
1407
        }
 
1408
 
 
1409
        ceu_1to1 = false;
 
1410
        if (!ceu_can_scale)
 
1411
                goto update_cache;
 
1412
 
 
1413
        cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
1414
 
 
1415
        ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 
1416
        if (ret < 0)
 
1417
                return ret;
 
1418
 
 
1419
        max_width = min(cap.bounds.width, 2560);
 
1420
        max_height = min(cap.bounds.height, 1920);
 
1421
 
 
1422
        /* Camera set a format, but geometry is not precise, try to improve */
 
1423
        tmp_w = mf->width;
 
1424
        tmp_h = mf->height;
 
1425
 
 
1426
        /* width <= max_width && height <= max_height - guaranteed by try_fmt */
 
1427
        while ((width > tmp_w || height > tmp_h) &&
 
1428
               tmp_w < max_width && tmp_h < max_height) {
 
1429
                tmp_w = min(2 * tmp_w, max_width);
 
1430
                tmp_h = min(2 * tmp_h, max_height);
 
1431
                mf->width = tmp_w;
 
1432
                mf->height = tmp_h;
 
1433
                ret = v4l2_device_call_until_err(sd->v4l2_dev,
 
1434
                                        soc_camera_grp_id(icd), video,
 
1435
                                        s_mbus_fmt, mf);
 
1436
                dev_geo(dev, "Camera scaled to %ux%u\n",
 
1437
                        mf->width, mf->height);
 
1438
                if (ret < 0) {
 
1439
                        /* This shouldn't happen */
 
1440
                        dev_err(dev, "Client failed to set format: %d\n", ret);
 
1441
                        return ret;
 
1442
                }
 
1443
        }
 
1444
 
 
1445
update_cache:
 
1446
        /* Update cache */
 
1447
        ret = client_g_rect(sd, &cam->rect);
 
1448
        if (ret < 0)
 
1449
                return ret;
 
1450
 
 
1451
        if (ceu_1to1)
 
1452
                cam->subrect = cam->rect;
 
1453
        else
 
1454
                update_subrect(cam);
 
1455
 
 
1456
        return 0;
 
1457
}
 
1458
 
 
1459
/**
 
1460
 * @width       - on output: user width, mapped back to input
 
1461
 * @height      - on output: user height, mapped back to input
 
1462
 * @mf          - in- / output camera output window
 
1463
 */
 
1464
static int client_scale(struct soc_camera_device *icd,
 
1465
                        struct v4l2_mbus_framefmt *mf,
 
1466
                        unsigned int *width, unsigned int *height,
 
1467
                        bool ceu_can_scale)
 
1468
{
 
1469
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1470
        struct device *dev = icd->parent;
 
1471
        struct v4l2_mbus_framefmt mf_tmp = *mf;
 
1472
        unsigned int scale_h, scale_v;
 
1473
        int ret;
 
1474
 
 
1475
        /*
 
1476
         * 5. Apply iterative camera S_FMT for camera user window (also updates
 
1477
         *    client crop cache and the imaginary sub-rectangle).
 
1478
         */
 
1479
        ret = client_s_fmt(icd, &mf_tmp, ceu_can_scale);
 
1480
        if (ret < 0)
 
1481
                return ret;
 
1482
 
 
1483
        dev_geo(dev, "5: camera scaled to %ux%u\n",
 
1484
                mf_tmp.width, mf_tmp.height);
 
1485
 
 
1486
        /* 6. Retrieve camera output window (g_fmt) */
 
1487
 
 
1488
        /* unneeded - it is already in "mf_tmp" */
 
1489
 
 
1490
        /* 7. Calculate new client scales. */
 
1491
        scale_h = calc_generic_scale(cam->rect.width, mf_tmp.width);
 
1492
        scale_v = calc_generic_scale(cam->rect.height, mf_tmp.height);
 
1493
 
 
1494
        mf->width       = mf_tmp.width;
 
1495
        mf->height      = mf_tmp.height;
 
1496
        mf->colorspace  = mf_tmp.colorspace;
 
1497
 
 
1498
        /*
 
1499
         * 8. Calculate new CEU crop - apply camera scales to previously
 
1500
         *    updated "effective" crop.
 
1501
         */
 
1502
        *width = scale_down(cam->subrect.width, scale_h);
 
1503
        *height = scale_down(cam->subrect.height, scale_v);
 
1504
 
 
1505
        dev_geo(dev, "8: new client sub-window %ux%u\n", *width, *height);
 
1506
 
 
1507
        return 0;
 
1508
}
 
1509
 
 
1510
/*
 
1511
 * CEU can scale and crop, but we don't want to waste bandwidth and kill the
 
1512
 * framerate by always requesting the maximum image from the client. See
 
1513
 * Documentation/video4linux/sh_mobile_ceu_camera.txt for a description of
 
1514
 * scaling and cropping algorithms and for the meaning of referenced here steps.
 
1515
 */
 
1516
static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 
1517
                                  struct v4l2_crop *a)
 
1518
{
 
1519
        struct v4l2_rect *rect = &a->c;
 
1520
        struct device *dev = icd->parent;
 
1521
        struct soc_camera_host *ici = to_soc_camera_host(dev);
 
1522
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
1523
        struct v4l2_crop cam_crop;
 
1524
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1525
        struct v4l2_rect *cam_rect = &cam_crop.c;
 
1526
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1527
        struct v4l2_mbus_framefmt mf;
 
1528
        unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
 
1529
                out_width, out_height;
 
1530
        int interm_width, interm_height;
 
1531
        u32 capsr, cflcr;
 
1532
        int ret;
 
1533
 
 
1534
        dev_geo(dev, "S_CROP(%ux%u@%u:%u)\n", rect->width, rect->height,
 
1535
                rect->left, rect->top);
 
1536
 
 
1537
        /* During camera cropping its output window can change too, stop CEU */
 
1538
        capsr = capture_save_reset(pcdev);
 
1539
        dev_dbg(dev, "CAPSR 0x%x, CFLCR 0x%x\n", capsr, pcdev->cflcr);
 
1540
 
 
1541
        /*
 
1542
         * 1. - 2. Apply iterative camera S_CROP for new input window, read back
 
1543
         * actual camera rectangle.
 
1544
         */
 
1545
        ret = client_s_crop(icd, a, &cam_crop);
 
1546
        if (ret < 0)
 
1547
                return ret;
 
1548
 
 
1549
        dev_geo(dev, "1-2: camera cropped to %ux%u@%u:%u\n",
 
1550
                cam_rect->width, cam_rect->height,
 
1551
                cam_rect->left, cam_rect->top);
 
1552
 
 
1553
        /* On success cam_crop contains current camera crop */
 
1554
 
 
1555
        /* 3. Retrieve camera output window */
 
1556
        ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 
1557
        if (ret < 0)
 
1558
                return ret;
 
1559
 
 
1560
        if (mf.width > 2560 || mf.height > 1920)
 
1561
                return -EINVAL;
 
1562
 
 
1563
        /* 4. Calculate camera scales */
 
1564
        scale_cam_h     = calc_generic_scale(cam_rect->width, mf.width);
 
1565
        scale_cam_v     = calc_generic_scale(cam_rect->height, mf.height);
 
1566
 
 
1567
        /* Calculate intermediate window */
 
1568
        interm_width    = scale_down(rect->width, scale_cam_h);
 
1569
        interm_height   = scale_down(rect->height, scale_cam_v);
 
1570
 
 
1571
        if (interm_width < icd->user_width) {
 
1572
                u32 new_scale_h;
 
1573
 
 
1574
                new_scale_h = calc_generic_scale(rect->width, icd->user_width);
 
1575
 
 
1576
                mf.width = scale_down(cam_rect->width, new_scale_h);
 
1577
        }
 
1578
 
 
1579
        if (interm_height < icd->user_height) {
 
1580
                u32 new_scale_v;
 
1581
 
 
1582
                new_scale_v = calc_generic_scale(rect->height, icd->user_height);
 
1583
 
 
1584
                mf.height = scale_down(cam_rect->height, new_scale_v);
 
1585
        }
 
1586
 
 
1587
        if (interm_width < icd->user_width || interm_height < icd->user_height) {
 
1588
                ret = v4l2_device_call_until_err(sd->v4l2_dev,
 
1589
                                        soc_camera_grp_id(icd), video,
 
1590
                                        s_mbus_fmt, &mf);
 
1591
                if (ret < 0)
 
1592
                        return ret;
 
1593
 
 
1594
                dev_geo(dev, "New camera output %ux%u\n", mf.width, mf.height);
 
1595
                scale_cam_h     = calc_generic_scale(cam_rect->width, mf.width);
 
1596
                scale_cam_v     = calc_generic_scale(cam_rect->height, mf.height);
 
1597
                interm_width    = scale_down(rect->width, scale_cam_h);
 
1598
                interm_height   = scale_down(rect->height, scale_cam_v);
 
1599
        }
 
1600
 
 
1601
        /* Cache camera output window */
 
1602
        cam->width      = mf.width;
 
1603
        cam->height     = mf.height;
 
1604
 
 
1605
        if (pcdev->image_mode) {
 
1606
                out_width       = min(interm_width, icd->user_width);
 
1607
                out_height      = min(interm_height, icd->user_height);
 
1608
        } else {
 
1609
                out_width       = interm_width;
 
1610
                out_height      = interm_height;
 
1611
        }
 
1612
 
 
1613
        /*
 
1614
         * 5. Calculate CEU scales from camera scales from results of (5) and
 
1615
         *    the user window
 
1616
         */
 
1617
        scale_ceu_h     = calc_scale(interm_width, &out_width);
 
1618
        scale_ceu_v     = calc_scale(interm_height, &out_height);
 
1619
 
 
1620
        dev_geo(dev, "5: CEU scales %u:%u\n", scale_ceu_h, scale_ceu_v);
 
1621
 
 
1622
        /* Apply CEU scales. */
 
1623
        cflcr = scale_ceu_h | (scale_ceu_v << 16);
 
1624
        if (cflcr != pcdev->cflcr) {
 
1625
                pcdev->cflcr = cflcr;
 
1626
                ceu_write(pcdev, CFLCR, cflcr);
 
1627
        }
 
1628
 
 
1629
        icd->user_width  = out_width & ~3;
 
1630
        icd->user_height = out_height & ~3;
 
1631
        /* Offsets are applied at the CEU scaling filter input */
 
1632
        cam->ceu_left    = scale_down(rect->left - cam_rect->left, scale_cam_h) & ~1;
 
1633
        cam->ceu_top     = scale_down(rect->top - cam_rect->top, scale_cam_v) & ~1;
 
1634
 
 
1635
        /* 6. Use CEU cropping to crop to the new window. */
 
1636
        sh_mobile_ceu_set_rect(icd);
 
1637
 
 
1638
        cam->subrect = *rect;
 
1639
 
 
1640
        dev_geo(dev, "6: CEU cropped to %ux%u@%u:%u\n",
 
1641
                icd->user_width, icd->user_height,
 
1642
                cam->ceu_left, cam->ceu_top);
 
1643
 
 
1644
        /* Restore capture. The CE bit can be cleared by the hardware */
 
1645
        if (pcdev->active)
 
1646
                capsr |= 1;
 
1647
        capture_restore(pcdev, capsr);
 
1648
 
 
1649
        /* Even if only camera cropping succeeded */
 
1650
        return ret;
 
1651
}
 
1652
 
 
1653
static int sh_mobile_ceu_get_crop(struct soc_camera_device *icd,
 
1654
                                  struct v4l2_crop *a)
 
1655
{
 
1656
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1657
 
 
1658
        a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
1659
        a->c = cam->subrect;
 
1660
 
 
1661
        return 0;
 
1662
}
 
1663
 
 
1664
/*
 
1665
 * Calculate real client output window by applying new scales to the current
 
1666
 * client crop. New scales are calculated from the requested output format and
 
1667
 * CEU crop, mapped backed onto the client input (subrect).
 
1668
 */
 
1669
static void calculate_client_output(struct soc_camera_device *icd,
 
1670
                const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf)
 
1671
{
 
1672
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1673
        struct device *dev = icd->parent;
 
1674
        struct v4l2_rect *cam_subrect = &cam->subrect;
 
1675
        unsigned int scale_v, scale_h;
 
1676
 
 
1677
        if (cam_subrect->width == cam->rect.width &&
 
1678
            cam_subrect->height == cam->rect.height) {
 
1679
                /* No sub-cropping */
 
1680
                mf->width       = pix->width;
 
1681
                mf->height      = pix->height;
 
1682
                return;
 
1683
        }
 
1684
 
 
1685
        /* 1.-2. Current camera scales and subwin - cached. */
 
1686
 
 
1687
        dev_geo(dev, "2: subwin %ux%u@%u:%u\n",
 
1688
                cam_subrect->width, cam_subrect->height,
 
1689
                cam_subrect->left, cam_subrect->top);
 
1690
 
 
1691
        /*
 
1692
         * 3. Calculate new combined scales from input sub-window to requested
 
1693
         *    user window.
 
1694
         */
 
1695
 
 
1696
        /*
 
1697
         * TODO: CEU cannot scale images larger than VGA to smaller than SubQCIF
 
1698
         * (128x96) or larger than VGA
 
1699
         */
 
1700
        scale_h = calc_generic_scale(cam_subrect->width, pix->width);
 
1701
        scale_v = calc_generic_scale(cam_subrect->height, pix->height);
 
1702
 
 
1703
        dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
 
1704
 
 
1705
        /*
 
1706
         * 4. Calculate desired client output window by applying combined scales
 
1707
         *    to client (real) input window.
 
1708
         */
 
1709
        mf->width       = scale_down(cam->rect.width, scale_h);
 
1710
        mf->height      = scale_down(cam->rect.height, scale_v);
 
1711
}
 
1712
 
 
1713
/* Similar to set_crop multistage iterative algorithm */
 
1714
static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
 
1715
                                 struct v4l2_format *f)
 
1716
{
 
1717
        struct device *dev = icd->parent;
 
1718
        struct soc_camera_host *ici = to_soc_camera_host(dev);
 
1719
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
1720
        struct sh_mobile_ceu_cam *cam = icd->host_priv;
 
1721
        struct v4l2_pix_format *pix = &f->fmt.pix;
 
1722
        struct v4l2_mbus_framefmt mf;
 
1723
        __u32 pixfmt = pix->pixelformat;
 
1724
        const struct soc_camera_format_xlate *xlate;
 
1725
        /* Keep Compiler Happy */
 
1726
        unsigned int ceu_sub_width = 0, ceu_sub_height = 0;
 
1727
        u16 scale_v, scale_h;
 
1728
        int ret;
 
1729
        bool image_mode;
 
1730
        enum v4l2_field field;
 
1731
 
 
1732
        switch (pix->field) {
 
1733
        default:
 
1734
                pix->field = V4L2_FIELD_NONE;
 
1735
                /* fall-through */
 
1736
        case V4L2_FIELD_INTERLACED_TB:
 
1737
        case V4L2_FIELD_INTERLACED_BT:
 
1738
        case V4L2_FIELD_NONE:
 
1739
                field = pix->field;
 
1740
                break;
 
1741
        case V4L2_FIELD_INTERLACED:
 
1742
                field = V4L2_FIELD_INTERLACED_TB;
 
1743
                break;
 
1744
        }
 
1745
 
 
1746
        xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 
1747
        if (!xlate) {
 
1748
                dev_warn(dev, "Format %x not found\n", pixfmt);
 
1749
                return -EINVAL;
 
1750
        }
 
1751
 
 
1752
        /* 1.-4. Calculate desired client output geometry */
 
1753
        calculate_client_output(icd, pix, &mf);
 
1754
        mf.field        = pix->field;
 
1755
        mf.colorspace   = pix->colorspace;
 
1756
        mf.code         = xlate->code;
 
1757
 
 
1758
        switch (pixfmt) {
 
1759
        case V4L2_PIX_FMT_NV12:
 
1760
        case V4L2_PIX_FMT_NV21:
 
1761
        case V4L2_PIX_FMT_NV16:
 
1762
        case V4L2_PIX_FMT_NV61:
 
1763
                image_mode = true;
 
1764
                break;
 
1765
        default:
 
1766
                image_mode = false;
 
1767
        }
 
1768
 
 
1769
        dev_geo(dev, "S_FMT(pix=0x%x, fld 0x%x, code 0x%x, %ux%u)\n", pixfmt, mf.field, mf.code,
 
1770
                pix->width, pix->height);
 
1771
 
 
1772
        dev_geo(dev, "4: request camera output %ux%u\n", mf.width, mf.height);
 
1773
 
 
1774
        /* 5. - 9. */
 
1775
        ret = client_scale(icd, &mf, &ceu_sub_width, &ceu_sub_height,
 
1776
                           image_mode && V4L2_FIELD_NONE == field);
 
1777
 
 
1778
        dev_geo(dev, "5-9: client scale return %d\n", ret);
 
1779
 
 
1780
        /* Done with the camera. Now see if we can improve the result */
 
1781
 
 
1782
        dev_geo(dev, "fmt %ux%u, requested %ux%u\n",
 
1783
                mf.width, mf.height, pix->width, pix->height);
 
1784
        if (ret < 0)
 
1785
                return ret;
 
1786
 
 
1787
        if (mf.code != xlate->code)
 
1788
                return -EINVAL;
 
1789
 
 
1790
        /* 9. Prepare CEU crop */
 
1791
        cam->width = mf.width;
 
1792
        cam->height = mf.height;
 
1793
 
 
1794
        /* 10. Use CEU scaling to scale to the requested user window. */
 
1795
 
 
1796
        /* We cannot scale up */
 
1797
        if (pix->width > ceu_sub_width)
 
1798
                ceu_sub_width = pix->width;
 
1799
 
 
1800
        if (pix->height > ceu_sub_height)
 
1801
                ceu_sub_height = pix->height;
 
1802
 
 
1803
        pix->colorspace = mf.colorspace;
 
1804
 
 
1805
        if (image_mode) {
 
1806
                /* Scale pix->{width x height} down to width x height */
 
1807
                scale_h         = calc_scale(ceu_sub_width, &pix->width);
 
1808
                scale_v         = calc_scale(ceu_sub_height, &pix->height);
 
1809
        } else {
 
1810
                pix->width      = ceu_sub_width;
 
1811
                pix->height     = ceu_sub_height;
 
1812
                scale_h         = 0;
 
1813
                scale_v         = 0;
 
1814
        }
 
1815
 
 
1816
        pcdev->cflcr = scale_h | (scale_v << 16);
 
1817
 
 
1818
        /*
 
1819
         * We have calculated CFLCR, the actual configuration will be performed
 
1820
         * in sh_mobile_ceu_set_bus_param()
 
1821
         */
 
1822
 
 
1823
        dev_geo(dev, "10: W: %u : 0x%x = %u, H: %u : 0x%x = %u\n",
 
1824
                ceu_sub_width, scale_h, pix->width,
 
1825
                ceu_sub_height, scale_v, pix->height);
 
1826
 
 
1827
        cam->code               = xlate->code;
 
1828
        icd->current_fmt        = xlate;
 
1829
 
 
1830
        pcdev->field = field;
 
1831
        pcdev->image_mode = image_mode;
 
1832
 
 
1833
        /* CFSZR requirement */
 
1834
        pix->width      &= ~3;
 
1835
        pix->height     &= ~3;
 
1836
 
 
1837
        return 0;
 
1838
}
 
1839
 
 
1840
static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 
1841
                                 struct v4l2_format *f)
 
1842
{
 
1843
        const struct soc_camera_format_xlate *xlate;
 
1844
        struct v4l2_pix_format *pix = &f->fmt.pix;
 
1845
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1846
        struct v4l2_mbus_framefmt mf;
 
1847
        __u32 pixfmt = pix->pixelformat;
 
1848
        int width, height;
 
1849
        int ret;
 
1850
 
 
1851
        dev_geo(icd->parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
 
1852
                 pixfmt, pix->width, pix->height);
 
1853
 
 
1854
        xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 
1855
        if (!xlate) {
 
1856
                dev_warn(icd->parent, "Format %x not found\n", pixfmt);
 
1857
                return -EINVAL;
 
1858
        }
 
1859
 
 
1860
        /* FIXME: calculate using depth and bus width */
 
1861
 
 
1862
        /* CFSZR requires height and width to be 4-pixel aligned */
 
1863
        v4l_bound_align_image(&pix->width, 2, 2560, 2,
 
1864
                              &pix->height, 4, 1920, 2, 0);
 
1865
 
 
1866
        width = pix->width;
 
1867
        height = pix->height;
 
1868
 
 
1869
        /* limit to sensor capabilities */
 
1870
        mf.width        = pix->width;
 
1871
        mf.height       = pix->height;
 
1872
        mf.field        = pix->field;
 
1873
        mf.code         = xlate->code;
 
1874
        mf.colorspace   = pix->colorspace;
 
1875
 
 
1876
        ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
 
1877
                                         video, try_mbus_fmt, &mf);
 
1878
        if (ret < 0)
 
1879
                return ret;
 
1880
 
 
1881
        pix->width      = mf.width;
 
1882
        pix->height     = mf.height;
 
1883
        pix->field      = mf.field;
 
1884
        pix->colorspace = mf.colorspace;
 
1885
 
 
1886
        switch (pixfmt) {
 
1887
        case V4L2_PIX_FMT_NV12:
 
1888
        case V4L2_PIX_FMT_NV21:
 
1889
        case V4L2_PIX_FMT_NV16:
 
1890
        case V4L2_PIX_FMT_NV61:
 
1891
                /* FIXME: check against rect_max after converting soc-camera */
 
1892
                /* We can scale precisely, need a bigger image from camera */
 
1893
                if (pix->width < width || pix->height < height) {
 
1894
                        /*
 
1895
                         * We presume, the sensor behaves sanely, i.e., if
 
1896
                         * requested a bigger rectangle, it will not return a
 
1897
                         * smaller one.
 
1898
                         */
 
1899
                        mf.width = 2560;
 
1900
                        mf.height = 1920;
 
1901
                        ret = v4l2_device_call_until_err(sd->v4l2_dev,
 
1902
                                        soc_camera_grp_id(icd), video,
 
1903
                                        try_mbus_fmt, &mf);
 
1904
                        if (ret < 0) {
 
1905
                                /* Shouldn't actually happen... */
 
1906
                                dev_err(icd->parent,
 
1907
                                        "FIXME: client try_fmt() = %d\n", ret);
 
1908
                                return ret;
 
1909
                        }
 
1910
                }
 
1911
                /* We will scale exactly */
 
1912
                if (mf.width > width)
 
1913
                        pix->width = width;
 
1914
                if (mf.height > height)
 
1915
                        pix->height = height;
 
1916
        }
 
1917
 
 
1918
        pix->width      &= ~3;
 
1919
        pix->height     &= ~3;
 
1920
 
 
1921
        dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
 
1922
                __func__, ret, pix->pixelformat, pix->width, pix->height);
 
1923
 
 
1924
        return ret;
 
1925
}
 
1926
 
 
1927
static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
 
1928
                                      struct v4l2_crop *a)
 
1929
{
 
1930
        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
1931
        struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 
1932
        struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
1933
        u32 out_width = icd->user_width, out_height = icd->user_height;
 
1934
        int ret;
 
1935
 
 
1936
        /* Freeze queue */
 
1937
        pcdev->frozen = 1;
 
1938
        /* Wait for frame */
 
1939
        ret = wait_for_completion_interruptible(&pcdev->complete);
 
1940
        /* Stop the client */
 
1941
        ret = v4l2_subdev_call(sd, video, s_stream, 0);
 
1942
        if (ret < 0)
 
1943
                dev_warn(icd->parent,
 
1944
                         "Client failed to stop the stream: %d\n", ret);
 
1945
        else
 
1946
                /* Do the crop, if it fails, there's nothing more we can do */
 
1947
                sh_mobile_ceu_set_crop(icd, a);
 
1948
 
 
1949
        dev_geo(icd->parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
 
1950
 
 
1951
        if (icd->user_width != out_width || icd->user_height != out_height) {
 
1952
                struct v4l2_format f = {
 
1953
                        .type   = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 
1954
                        .fmt.pix        = {
 
1955
                                .width          = out_width,
 
1956
                                .height         = out_height,
 
1957
                                .pixelformat    = icd->current_fmt->host_fmt->fourcc,
 
1958
                                .field          = pcdev->field,
 
1959
                                .colorspace     = icd->colorspace,
 
1960
                        },
 
1961
                };
 
1962
                ret = sh_mobile_ceu_set_fmt(icd, &f);
 
1963
                if (!ret && (out_width != f.fmt.pix.width ||
 
1964
                             out_height != f.fmt.pix.height))
 
1965
                        ret = -EINVAL;
 
1966
                if (!ret) {
 
1967
                        icd->user_width         = out_width & ~3;
 
1968
                        icd->user_height        = out_height & ~3;
 
1969
                        ret = sh_mobile_ceu_set_bus_param(icd,
 
1970
                                        icd->current_fmt->host_fmt->fourcc);
 
1971
                }
 
1972
        }
 
1973
 
 
1974
        /* Thaw the queue */
 
1975
        pcdev->frozen = 0;
 
1976
        spin_lock_irq(&pcdev->lock);
 
1977
        sh_mobile_ceu_capture(pcdev);
 
1978
        spin_unlock_irq(&pcdev->lock);
 
1979
        /* Start the client */
 
1980
        ret = v4l2_subdev_call(sd, video, s_stream, 1);
 
1981
        return ret;
 
1982
}
 
1983
 
 
1984
static unsigned int sh_mobile_ceu_poll(struct file *file, poll_table *pt)
 
1985
{
 
1986
        struct soc_camera_device *icd = file->private_data;
 
1987
 
 
1988
        return vb2_poll(&icd->vb2_vidq, file, pt);
 
1989
}
 
1990
 
 
1991
static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
 
1992
                                  struct v4l2_capability *cap)
 
1993
{
 
1994
        strlcpy(cap->card, "SuperH_Mobile_CEU", sizeof(cap->card));
 
1995
        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 
1996
        return 0;
 
1997
}
 
1998
 
 
1999
static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
 
2000
                                       struct soc_camera_device *icd)
 
2001
{
 
2002
        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
2003
        q->io_modes = VB2_MMAP | VB2_USERPTR;
 
2004
        q->drv_priv = icd;
 
2005
        q->ops = &sh_mobile_ceu_videobuf_ops;
 
2006
        q->mem_ops = &vb2_dma_contig_memops;
 
2007
        q->buf_struct_size = sizeof(struct sh_mobile_ceu_buffer);
 
2008
 
 
2009
        return vb2_queue_init(q);
 
2010
}
 
2011
 
 
2012
static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
 
2013
        .owner          = THIS_MODULE,
 
2014
        .add            = sh_mobile_ceu_add_device,
 
2015
        .remove         = sh_mobile_ceu_remove_device,
 
2016
        .get_formats    = sh_mobile_ceu_get_formats,
 
2017
        .put_formats    = sh_mobile_ceu_put_formats,
 
2018
        .get_crop       = sh_mobile_ceu_get_crop,
 
2019
        .set_crop       = sh_mobile_ceu_set_crop,
 
2020
        .set_livecrop   = sh_mobile_ceu_set_livecrop,
 
2021
        .set_fmt        = sh_mobile_ceu_set_fmt,
 
2022
        .try_fmt        = sh_mobile_ceu_try_fmt,
 
2023
        .poll           = sh_mobile_ceu_poll,
 
2024
        .querycap       = sh_mobile_ceu_querycap,
 
2025
        .set_bus_param  = sh_mobile_ceu_set_bus_param,
 
2026
        .init_videobuf2 = sh_mobile_ceu_init_videobuf,
 
2027
};
 
2028
 
 
2029
struct bus_wait {
 
2030
        struct notifier_block   notifier;
 
2031
        struct completion       completion;
 
2032
        struct device           *dev;
 
2033
};
 
2034
 
 
2035
static int bus_notify(struct notifier_block *nb,
 
2036
                      unsigned long action, void *data)
 
2037
{
 
2038
        struct device *dev = data;
 
2039
        struct bus_wait *wait = container_of(nb, struct bus_wait, notifier);
 
2040
 
 
2041
        if (wait->dev != dev)
 
2042
                return NOTIFY_DONE;
 
2043
 
 
2044
        switch (action) {
 
2045
        case BUS_NOTIFY_UNBOUND_DRIVER:
 
2046
                /* Protect from module unloading */
 
2047
                wait_for_completion(&wait->completion);
 
2048
                return NOTIFY_OK;
 
2049
        }
 
2050
        return NOTIFY_DONE;
 
2051
}
 
2052
 
 
2053
static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
 
2054
{
 
2055
        struct sh_mobile_ceu_dev *pcdev;
 
2056
        struct resource *res;
 
2057
        void __iomem *base;
 
2058
        unsigned int irq;
 
2059
        int err = 0;
 
2060
        struct bus_wait wait = {
 
2061
                .completion = COMPLETION_INITIALIZER_ONSTACK(wait.completion),
 
2062
                .notifier.notifier_call = bus_notify,
 
2063
        };
 
2064
        struct sh_mobile_ceu_companion *csi2;
 
2065
 
 
2066
        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
2067
        irq = platform_get_irq(pdev, 0);
 
2068
        if (!res || (int)irq <= 0) {
 
2069
                dev_err(&pdev->dev, "Not enough CEU platform resources.\n");
 
2070
                err = -ENODEV;
 
2071
                goto exit;
 
2072
        }
 
2073
 
 
2074
        pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
 
2075
        if (!pcdev) {
 
2076
                dev_err(&pdev->dev, "Could not allocate pcdev\n");
 
2077
                err = -ENOMEM;
 
2078
                goto exit;
 
2079
        }
 
2080
 
 
2081
        INIT_LIST_HEAD(&pcdev->capture);
 
2082
        spin_lock_init(&pcdev->lock);
 
2083
        init_completion(&pcdev->complete);
 
2084
 
 
2085
        pcdev->pdata = pdev->dev.platform_data;
 
2086
        if (!pcdev->pdata) {
 
2087
                err = -EINVAL;
 
2088
                dev_err(&pdev->dev, "CEU platform data not set.\n");
 
2089
                goto exit_kfree;
 
2090
        }
 
2091
 
 
2092
        base = ioremap_nocache(res->start, resource_size(res));
 
2093
        if (!base) {
 
2094
                err = -ENXIO;
 
2095
                dev_err(&pdev->dev, "Unable to ioremap CEU registers.\n");
 
2096
                goto exit_kfree;
 
2097
        }
 
2098
 
 
2099
        pcdev->irq = irq;
 
2100
        pcdev->base = base;
 
2101
        pcdev->video_limit = 0; /* only enabled if second resource exists */
 
2102
 
 
2103
        res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 
2104
        if (res) {
 
2105
                err = dma_declare_coherent_memory(&pdev->dev, res->start,
 
2106
                                                  res->start,
 
2107
                                                  resource_size(res),
 
2108
                                                  DMA_MEMORY_MAP |
 
2109
                                                  DMA_MEMORY_EXCLUSIVE);
 
2110
                if (!err) {
 
2111
                        dev_err(&pdev->dev, "Unable to declare CEU memory.\n");
 
2112
                        err = -ENXIO;
 
2113
                        goto exit_iounmap;
 
2114
                }
 
2115
 
 
2116
                pcdev->video_limit = resource_size(res);
 
2117
        }
 
2118
 
 
2119
        /* request irq */
 
2120
        err = request_irq(pcdev->irq, sh_mobile_ceu_irq, IRQF_DISABLED,
 
2121
                          dev_name(&pdev->dev), pcdev);
 
2122
        if (err) {
 
2123
                dev_err(&pdev->dev, "Unable to register CEU interrupt.\n");
 
2124
                goto exit_release_mem;
 
2125
        }
 
2126
 
 
2127
        pm_suspend_ignore_children(&pdev->dev, true);
 
2128
        pm_runtime_enable(&pdev->dev);
 
2129
        pm_runtime_resume(&pdev->dev);
 
2130
 
 
2131
        pcdev->ici.priv = pcdev;
 
2132
        pcdev->ici.v4l2_dev.dev = &pdev->dev;
 
2133
        pcdev->ici.nr = pdev->id;
 
2134
        pcdev->ici.drv_name = dev_name(&pdev->dev);
 
2135
        pcdev->ici.ops = &sh_mobile_ceu_host_ops;
 
2136
 
 
2137
        pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
 
2138
        if (IS_ERR(pcdev->alloc_ctx)) {
 
2139
                err = PTR_ERR(pcdev->alloc_ctx);
 
2140
                goto exit_free_clk;
 
2141
        }
 
2142
 
 
2143
        err = soc_camera_host_register(&pcdev->ici);
 
2144
        if (err)
 
2145
                goto exit_free_ctx;
 
2146
 
 
2147
        /* CSI2 interfacing */
 
2148
        csi2 = pcdev->pdata->csi2;
 
2149
        if (csi2) {
 
2150
                struct platform_device *csi2_pdev =
 
2151
                        platform_device_alloc("sh-mobile-csi2", csi2->id);
 
2152
                struct sh_csi2_pdata *csi2_pdata = csi2->platform_data;
 
2153
 
 
2154
                if (!csi2_pdev) {
 
2155
                        err = -ENOMEM;
 
2156
                        goto exit_host_unregister;
 
2157
                }
 
2158
 
 
2159
                pcdev->csi2_pdev                = csi2_pdev;
 
2160
 
 
2161
                err = platform_device_add_data(csi2_pdev, csi2_pdata, sizeof(*csi2_pdata));
 
2162
                if (err < 0)
 
2163
                        goto exit_pdev_put;
 
2164
 
 
2165
                csi2_pdata                      = csi2_pdev->dev.platform_data;
 
2166
                csi2_pdata->v4l2_dev            = &pcdev->ici.v4l2_dev;
 
2167
 
 
2168
                csi2_pdev->resource             = csi2->resource;
 
2169
                csi2_pdev->num_resources        = csi2->num_resources;
 
2170
 
 
2171
                err = platform_device_add(csi2_pdev);
 
2172
                if (err < 0)
 
2173
                        goto exit_pdev_put;
 
2174
 
 
2175
                wait.dev = &csi2_pdev->dev;
 
2176
 
 
2177
                err = bus_register_notifier(&platform_bus_type, &wait.notifier);
 
2178
                if (err < 0)
 
2179
                        goto exit_pdev_unregister;
 
2180
 
 
2181
                /*
 
2182
                 * From this point the driver module will not unload, until
 
2183
                 * we complete the completion.
 
2184
                 */
 
2185
 
 
2186
                if (!csi2_pdev->dev.driver) {
 
2187
                        complete(&wait.completion);
 
2188
                        /* Either too late, or probing failed */
 
2189
                        bus_unregister_notifier(&platform_bus_type, &wait.notifier);
 
2190
                        err = -ENXIO;
 
2191
                        goto exit_pdev_unregister;
 
2192
                }
 
2193
 
 
2194
                /*
 
2195
                 * The module is still loaded, in the worst case it is hanging
 
2196
                 * in device release on our completion. So, _now_ dereferencing
 
2197
                 * the "owner" is safe!
 
2198
                 */
 
2199
 
 
2200
                err = try_module_get(csi2_pdev->dev.driver->owner);
 
2201
 
 
2202
                /* Let notifier complete, if it has been locked */
 
2203
                complete(&wait.completion);
 
2204
                bus_unregister_notifier(&platform_bus_type, &wait.notifier);
 
2205
                if (!err) {
 
2206
                        err = -ENODEV;
 
2207
                        goto exit_pdev_unregister;
 
2208
                }
 
2209
        }
 
2210
 
 
2211
        return 0;
 
2212
 
 
2213
exit_pdev_unregister:
 
2214
        platform_device_del(pcdev->csi2_pdev);
 
2215
exit_pdev_put:
 
2216
        pcdev->csi2_pdev->resource = NULL;
 
2217
        platform_device_put(pcdev->csi2_pdev);
 
2218
exit_host_unregister:
 
2219
        soc_camera_host_unregister(&pcdev->ici);
 
2220
exit_free_ctx:
 
2221
        vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 
2222
exit_free_clk:
 
2223
        pm_runtime_disable(&pdev->dev);
 
2224
        free_irq(pcdev->irq, pcdev);
 
2225
exit_release_mem:
 
2226
        if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
 
2227
                dma_release_declared_memory(&pdev->dev);
 
2228
exit_iounmap:
 
2229
        iounmap(base);
 
2230
exit_kfree:
 
2231
        kfree(pcdev);
 
2232
exit:
 
2233
        return err;
 
2234
}
 
2235
 
 
2236
static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
 
2237
{
 
2238
        struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 
2239
        struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
 
2240
                                        struct sh_mobile_ceu_dev, ici);
 
2241
        struct platform_device *csi2_pdev = pcdev->csi2_pdev;
 
2242
 
 
2243
        soc_camera_host_unregister(soc_host);
 
2244
        pm_runtime_disable(&pdev->dev);
 
2245
        free_irq(pcdev->irq, pcdev);
 
2246
        if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
 
2247
                dma_release_declared_memory(&pdev->dev);
 
2248
        iounmap(pcdev->base);
 
2249
        vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 
2250
        if (csi2_pdev && csi2_pdev->dev.driver) {
 
2251
                struct module *csi2_drv = csi2_pdev->dev.driver->owner;
 
2252
                platform_device_del(csi2_pdev);
 
2253
                csi2_pdev->resource = NULL;
 
2254
                platform_device_put(csi2_pdev);
 
2255
                module_put(csi2_drv);
 
2256
        }
 
2257
        kfree(pcdev);
 
2258
 
 
2259
        return 0;
 
2260
}
 
2261
 
 
2262
static int sh_mobile_ceu_runtime_nop(struct device *dev)
 
2263
{
 
2264
        /* Runtime PM callback shared between ->runtime_suspend()
 
2265
         * and ->runtime_resume(). Simply returns success.
 
2266
         *
 
2267
         * This driver re-initializes all registers after
 
2268
         * pm_runtime_get_sync() anyway so there is no need
 
2269
         * to save and restore registers here.
 
2270
         */
 
2271
        return 0;
 
2272
}
 
2273
 
 
2274
static const struct dev_pm_ops sh_mobile_ceu_dev_pm_ops = {
 
2275
        .runtime_suspend = sh_mobile_ceu_runtime_nop,
 
2276
        .runtime_resume = sh_mobile_ceu_runtime_nop,
 
2277
};
 
2278
 
 
2279
static struct platform_driver sh_mobile_ceu_driver = {
 
2280
        .driver         = {
 
2281
                .name   = "sh_mobile_ceu",
 
2282
                .pm     = &sh_mobile_ceu_dev_pm_ops,
 
2283
        },
 
2284
        .probe          = sh_mobile_ceu_probe,
 
2285
        .remove         = __devexit_p(sh_mobile_ceu_remove),
 
2286
};
 
2287
 
 
2288
static int __init sh_mobile_ceu_init(void)
 
2289
{
 
2290
        /* Whatever return code */
 
2291
        request_module("sh_mobile_csi2");
 
2292
        return platform_driver_register(&sh_mobile_ceu_driver);
 
2293
}
 
2294
 
 
2295
static void __exit sh_mobile_ceu_exit(void)
 
2296
{
 
2297
        platform_driver_unregister(&sh_mobile_ceu_driver);
 
2298
}
 
2299
 
 
2300
module_init(sh_mobile_ceu_init);
 
2301
module_exit(sh_mobile_ceu_exit);
 
2302
 
 
2303
MODULE_DESCRIPTION("SuperH Mobile CEU driver");
 
2304
MODULE_AUTHOR("Magnus Damm");
 
2305
MODULE_LICENSE("GPL");
 
2306
MODULE_VERSION("0.0.6");
 
2307
MODULE_ALIAS("platform:sh_mobile_ceu");