Merge branch 'fixes-hwmod-regression' of git://git.kernel.org/pub/scm/linux/kernel...
[linux-2.6.git] / drivers / media / video / omap1_camera.c
1 /*
2  * V4L2 SoC Camera driver for OMAP1 Camera Interface
3  *
4  * Copyright (C) 2010, Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>
5  *
6  * Based on V4L2 Driver for i.MXL/i.MXL camera (CSI) host
7  * Copyright (C) 2008, Paulius Zaleckas <paulius.zaleckas@teltonika.lt>
8  * Copyright (C) 2009, Darius Augulis <augulis.darius@gmail.com>
9  *
10  * Based on PXA SoC camera driver
11  * Copyright (C) 2006, Sascha Hauer, Pengutronix
12  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
13  *
14  * Hardware specific bits initialy based on former work by Matt Callow
15  * drivers/media/video/omap/omap1510cam.c
16  * Copyright (C) 2006 Matt Callow
17  *
18  * This program is free software; you can redistribute it and/or modify
19  * it under the terms of the GNU General Public License version 2 as
20  * published by the Free Software Foundation.
21  */
22
23
24 #include <linux/clk.h>
25 #include <linux/dma-mapping.h>
26 #include <linux/interrupt.h>
27 #include <linux/platform_device.h>
28 #include <linux/slab.h>
29
30 #include <media/omap1_camera.h>
31 #include <media/soc_camera.h>
32 #include <media/soc_mediabus.h>
33 #include <media/videobuf-dma-contig.h>
34 #include <media/videobuf-dma-sg.h>
35
36 #include <plat/dma.h>
37
38
39 #define DRIVER_NAME             "omap1-camera"
40 #define DRIVER_VERSION          "0.0.2"
41
42
43 /*
44  * ---------------------------------------------------------------------------
45  *  OMAP1 Camera Interface registers
46  * ---------------------------------------------------------------------------
47  */
48
49 #define REG_CTRLCLOCK           0x00
50 #define REG_IT_STATUS           0x04
51 #define REG_MODE                0x08
52 #define REG_STATUS              0x0C
53 #define REG_CAMDATA             0x10
54 #define REG_GPIO                0x14
55 #define REG_PEAK_COUNTER        0x18
56
57 /* CTRLCLOCK bit shifts */
58 #define LCLK_EN                 BIT(7)
59 #define DPLL_EN                 BIT(6)
60 #define MCLK_EN                 BIT(5)
61 #define CAMEXCLK_EN             BIT(4)
62 #define POLCLK                  BIT(3)
63 #define FOSCMOD_SHIFT           0
64 #define FOSCMOD_MASK            (0x7 << FOSCMOD_SHIFT)
65 #define FOSCMOD_12MHz           0x0
66 #define FOSCMOD_6MHz            0x2
67 #define FOSCMOD_9_6MHz          0x4
68 #define FOSCMOD_24MHz           0x5
69 #define FOSCMOD_8MHz            0x6
70
71 /* IT_STATUS bit shifts */
72 #define DATA_TRANSFER           BIT(5)
73 #define FIFO_FULL               BIT(4)
74 #define H_DOWN                  BIT(3)
75 #define H_UP                    BIT(2)
76 #define V_DOWN                  BIT(1)
77 #define V_UP                    BIT(0)
78
79 /* MODE bit shifts */
80 #define RAZ_FIFO                BIT(18)
81 #define EN_FIFO_FULL            BIT(17)
82 #define EN_NIRQ                 BIT(16)
83 #define THRESHOLD_SHIFT         9
84 #define THRESHOLD_MASK          (0x7f << THRESHOLD_SHIFT)
85 #define DMA                     BIT(8)
86 #define EN_H_DOWN               BIT(7)
87 #define EN_H_UP                 BIT(6)
88 #define EN_V_DOWN               BIT(5)
89 #define EN_V_UP                 BIT(4)
90 #define ORDERCAMD               BIT(3)
91
92 #define IRQ_MASK                (EN_V_UP | EN_V_DOWN | EN_H_UP | EN_H_DOWN | \
93                                  EN_NIRQ | EN_FIFO_FULL)
94
95 /* STATUS bit shifts */
96 #define HSTATUS                 BIT(1)
97 #define VSTATUS                 BIT(0)
98
99 /* GPIO bit shifts */
100 #define CAM_RST                 BIT(0)
101
102 /* end of OMAP1 Camera Interface registers */
103
104
105 #define SOCAM_BUS_FLAGS (V4L2_MBUS_MASTER | \
106                         V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
107                         V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
108                         V4L2_MBUS_DATA_ACTIVE_HIGH)
109
110
111 #define FIFO_SIZE               ((THRESHOLD_MASK >> THRESHOLD_SHIFT) + 1)
112 #define FIFO_SHIFT              __fls(FIFO_SIZE)
113
114 #define DMA_BURST_SHIFT         (1 + OMAP_DMA_DATA_BURST_4)
115 #define DMA_BURST_SIZE          (1 << DMA_BURST_SHIFT)
116
117 #define DMA_ELEMENT_SHIFT       OMAP_DMA_DATA_TYPE_S32
118 #define DMA_ELEMENT_SIZE        (1 << DMA_ELEMENT_SHIFT)
119
120 #define DMA_FRAME_SHIFT_CONTIG  (FIFO_SHIFT - 1)
121 #define DMA_FRAME_SHIFT_SG      DMA_BURST_SHIFT
122
123 #define DMA_FRAME_SHIFT(x)      ((x) == OMAP1_CAM_DMA_CONTIG ? \
124                                                 DMA_FRAME_SHIFT_CONTIG : \
125                                                 DMA_FRAME_SHIFT_SG)
126 #define DMA_FRAME_SIZE(x)       (1 << DMA_FRAME_SHIFT(x))
127 #define DMA_SYNC                OMAP_DMA_SYNC_FRAME
128 #define THRESHOLD_LEVEL         DMA_FRAME_SIZE
129
130
131 #define MAX_VIDEO_MEM           4       /* arbitrary video memory limit in MB */
132
133
134 /*
135  * Structures
136  */
137
138 /* buffer for one video frame */
139 struct omap1_cam_buf {
140         struct videobuf_buffer          vb;
141         enum v4l2_mbus_pixelcode        code;
142         int                             inwork;
143         struct scatterlist              *sgbuf;
144         int                             sgcount;
145         int                             bytes_left;
146         enum videobuf_state             result;
147 };
148
149 struct omap1_cam_dev {
150         struct soc_camera_host          soc_host;
151         struct soc_camera_device        *icd;
152         struct clk                      *clk;
153
154         unsigned int                    irq;
155         void __iomem                    *base;
156
157         int                             dma_ch;
158
159         struct omap1_cam_platform_data  *pdata;
160         struct resource                 *res;
161         unsigned long                   pflags;
162         unsigned long                   camexclk;
163
164         struct list_head                capture;
165
166         /* lock used to protect videobuf */
167         spinlock_t                      lock;
168
169         /* Pointers to DMA buffers */
170         struct omap1_cam_buf            *active;
171         struct omap1_cam_buf            *ready;
172
173         enum omap1_cam_vb_mode          vb_mode;
174         int                             (*mmap_mapper)(struct videobuf_queue *q,
175                                                 struct videobuf_buffer *buf,
176                                                 struct vm_area_struct *vma);
177
178         u32                             reg_cache[0];
179 };
180
181
182 static void cam_write(struct omap1_cam_dev *pcdev, u16 reg, u32 val)
183 {
184         pcdev->reg_cache[reg / sizeof(u32)] = val;
185         __raw_writel(val, pcdev->base + reg);
186 }
187
188 static u32 cam_read(struct omap1_cam_dev *pcdev, u16 reg, bool from_cache)
189 {
190         return !from_cache ? __raw_readl(pcdev->base + reg) :
191                         pcdev->reg_cache[reg / sizeof(u32)];
192 }
193
194 #define CAM_READ(pcdev, reg) \
195                 cam_read(pcdev, REG_##reg, false)
196 #define CAM_WRITE(pcdev, reg, val) \
197                 cam_write(pcdev, REG_##reg, val)
198 #define CAM_READ_CACHE(pcdev, reg) \
199                 cam_read(pcdev, REG_##reg, true)
200
201 /*
202  *  Videobuf operations
203  */
204 static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
205                 unsigned int *size)
206 {
207         struct soc_camera_device *icd = vq->priv_data;
208         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
209                         icd->current_fmt->host_fmt);
210         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
211         struct omap1_cam_dev *pcdev = ici->priv;
212
213         if (bytes_per_line < 0)
214                 return bytes_per_line;
215
216         *size = bytes_per_line * icd->user_height;
217
218         if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
219                 *count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
220
221         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
222                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
223
224         dev_dbg(icd->parent,
225                         "%s: count=%d, size=%d\n", __func__, *count, *size);
226
227         return 0;
228 }
229
230 static void free_buffer(struct videobuf_queue *vq, struct omap1_cam_buf *buf,
231                 enum omap1_cam_vb_mode vb_mode)
232 {
233         struct videobuf_buffer *vb = &buf->vb;
234
235         BUG_ON(in_interrupt());
236
237         videobuf_waiton(vq, vb, 0, 0);
238
239         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
240                 videobuf_dma_contig_free(vq, vb);
241         } else {
242                 struct soc_camera_device *icd = vq->priv_data;
243                 struct device *dev = icd->parent;
244                 struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
245
246                 videobuf_dma_unmap(dev, dma);
247                 videobuf_dma_free(dma);
248         }
249
250         vb->state = VIDEOBUF_NEEDS_INIT;
251 }
252
253 static int omap1_videobuf_prepare(struct videobuf_queue *vq,
254                 struct videobuf_buffer *vb, enum v4l2_field field)
255 {
256         struct soc_camera_device *icd = vq->priv_data;
257         struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
258         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
259                         icd->current_fmt->host_fmt);
260         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
261         struct omap1_cam_dev *pcdev = ici->priv;
262         int ret;
263
264         if (bytes_per_line < 0)
265                 return bytes_per_line;
266
267         WARN_ON(!list_empty(&vb->queue));
268
269         BUG_ON(NULL == icd->current_fmt);
270
271         buf->inwork = 1;
272
273         if (buf->code != icd->current_fmt->code || vb->field != field ||
274                         vb->width  != icd->user_width ||
275                         vb->height != icd->user_height) {
276                 buf->code  = icd->current_fmt->code;
277                 vb->width  = icd->user_width;
278                 vb->height = icd->user_height;
279                 vb->field  = field;
280                 vb->state  = VIDEOBUF_NEEDS_INIT;
281         }
282
283         vb->size = bytes_per_line * vb->height;
284
285         if (vb->baddr && vb->bsize < vb->size) {
286                 ret = -EINVAL;
287                 goto out;
288         }
289
290         if (vb->state == VIDEOBUF_NEEDS_INIT) {
291                 ret = videobuf_iolock(vq, vb, NULL);
292                 if (ret)
293                         goto fail;
294
295                 vb->state = VIDEOBUF_PREPARED;
296         }
297         buf->inwork = 0;
298
299         return 0;
300 fail:
301         free_buffer(vq, buf, pcdev->vb_mode);
302 out:
303         buf->inwork = 0;
304         return ret;
305 }
306
307 static void set_dma_dest_params(int dma_ch, struct omap1_cam_buf *buf,
308                 enum omap1_cam_vb_mode vb_mode)
309 {
310         dma_addr_t dma_addr;
311         unsigned int block_size;
312
313         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
314                 dma_addr = videobuf_to_dma_contig(&buf->vb);
315                 block_size = buf->vb.size;
316         } else {
317                 if (WARN_ON(!buf->sgbuf)) {
318                         buf->result = VIDEOBUF_ERROR;
319                         return;
320                 }
321                 dma_addr = sg_dma_address(buf->sgbuf);
322                 if (WARN_ON(!dma_addr)) {
323                         buf->sgbuf = NULL;
324                         buf->result = VIDEOBUF_ERROR;
325                         return;
326                 }
327                 block_size = sg_dma_len(buf->sgbuf);
328                 if (WARN_ON(!block_size)) {
329                         buf->sgbuf = NULL;
330                         buf->result = VIDEOBUF_ERROR;
331                         return;
332                 }
333                 if (unlikely(buf->bytes_left < block_size))
334                         block_size = buf->bytes_left;
335                 if (WARN_ON(dma_addr & (DMA_FRAME_SIZE(vb_mode) *
336                                 DMA_ELEMENT_SIZE - 1))) {
337                         dma_addr = ALIGN(dma_addr, DMA_FRAME_SIZE(vb_mode) *
338                                         DMA_ELEMENT_SIZE);
339                         block_size &= ~(DMA_FRAME_SIZE(vb_mode) *
340                                         DMA_ELEMENT_SIZE - 1);
341                 }
342                 buf->bytes_left -= block_size;
343                 buf->sgcount++;
344         }
345
346         omap_set_dma_dest_params(dma_ch,
347                 OMAP_DMA_PORT_EMIFF, OMAP_DMA_AMODE_POST_INC, dma_addr, 0, 0);
348         omap_set_dma_transfer_params(dma_ch,
349                 OMAP_DMA_DATA_TYPE_S32, DMA_FRAME_SIZE(vb_mode),
350                 block_size >> (DMA_FRAME_SHIFT(vb_mode) + DMA_ELEMENT_SHIFT),
351                 DMA_SYNC, 0, 0);
352 }
353
354 static struct omap1_cam_buf *prepare_next_vb(struct omap1_cam_dev *pcdev)
355 {
356         struct omap1_cam_buf *buf;
357
358         /*
359          * If there is already a buffer pointed out by the pcdev->ready,
360          * (re)use it, otherwise try to fetch and configure a new one.
361          */
362         buf = pcdev->ready;
363         if (!buf) {
364                 if (list_empty(&pcdev->capture))
365                         return buf;
366                 buf = list_entry(pcdev->capture.next,
367                                 struct omap1_cam_buf, vb.queue);
368                 buf->vb.state = VIDEOBUF_ACTIVE;
369                 pcdev->ready = buf;
370                 list_del_init(&buf->vb.queue);
371         }
372
373         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
374                 /*
375                  * In CONTIG mode, we can safely enter next buffer parameters
376                  * into the DMA programming register set after the DMA
377                  * has already been activated on the previous buffer
378                  */
379                 set_dma_dest_params(pcdev->dma_ch, buf, pcdev->vb_mode);
380         } else {
381                 /*
382                  * In SG mode, the above is not safe since there are probably
383                  * a bunch of sgbufs from previous sglist still pending.
384                  * Instead, mark the sglist fresh for the upcoming
385                  * try_next_sgbuf().
386                  */
387                 buf->sgbuf = NULL;
388         }
389
390         return buf;
391 }
392
393 static struct scatterlist *try_next_sgbuf(int dma_ch, struct omap1_cam_buf *buf)
394 {
395         struct scatterlist *sgbuf;
396
397         if (likely(buf->sgbuf)) {
398                 /* current sglist is active */
399                 if (unlikely(!buf->bytes_left)) {
400                         /* indicate sglist complete */
401                         sgbuf = NULL;
402                 } else {
403                         /* process next sgbuf */
404                         sgbuf = sg_next(buf->sgbuf);
405                         if (WARN_ON(!sgbuf)) {
406                                 buf->result = VIDEOBUF_ERROR;
407                         } else if (WARN_ON(!sg_dma_len(sgbuf))) {
408                                 sgbuf = NULL;
409                                 buf->result = VIDEOBUF_ERROR;
410                         }
411                 }
412                 buf->sgbuf = sgbuf;
413         } else {
414                 /* sglist is fresh, initialize it before using */
415                 struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
416
417                 sgbuf = dma->sglist;
418                 if (!(WARN_ON(!sgbuf))) {
419                         buf->sgbuf = sgbuf;
420                         buf->sgcount = 0;
421                         buf->bytes_left = buf->vb.size;
422                         buf->result = VIDEOBUF_DONE;
423                 }
424         }
425         if (sgbuf)
426                 /*
427                  * Put our next sgbuf parameters (address, size)
428                  * into the DMA programming register set.
429                  */
430                 set_dma_dest_params(dma_ch, buf, OMAP1_CAM_DMA_SG);
431
432         return sgbuf;
433 }
434
435 static void start_capture(struct omap1_cam_dev *pcdev)
436 {
437         struct omap1_cam_buf *buf = pcdev->active;
438         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
439         u32 mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN;
440
441         if (WARN_ON(!buf))
442                 return;
443
444         /*
445          * Enable start of frame interrupt, which we will use for activating
446          * our end of frame watchdog when capture actually starts.
447          */
448         mode |= EN_V_UP;
449
450         if (unlikely(ctrlclock & LCLK_EN))
451                 /* stop pixel clock before FIFO reset */
452                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
453         /* reset FIFO */
454         CAM_WRITE(pcdev, MODE, mode | RAZ_FIFO);
455
456         omap_start_dma(pcdev->dma_ch);
457
458         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
459                 /*
460                  * In SG mode, it's a good moment for fetching next sgbuf
461                  * from the current sglist and, if available, already putting
462                  * its parameters into the DMA programming register set.
463                  */
464                 try_next_sgbuf(pcdev->dma_ch, buf);
465         }
466
467         /* (re)enable pixel clock */
468         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | LCLK_EN);
469         /* release FIFO reset */
470         CAM_WRITE(pcdev, MODE, mode);
471 }
472
473 static void suspend_capture(struct omap1_cam_dev *pcdev)
474 {
475         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
476
477         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
478         omap_stop_dma(pcdev->dma_ch);
479 }
480
481 static void disable_capture(struct omap1_cam_dev *pcdev)
482 {
483         u32 mode = CAM_READ_CACHE(pcdev, MODE);
484
485         CAM_WRITE(pcdev, MODE, mode & ~(IRQ_MASK | DMA));
486 }
487
488 static void omap1_videobuf_queue(struct videobuf_queue *vq,
489                                                 struct videobuf_buffer *vb)
490 {
491         struct soc_camera_device *icd = vq->priv_data;
492         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
493         struct omap1_cam_dev *pcdev = ici->priv;
494         struct omap1_cam_buf *buf;
495         u32 mode;
496
497         list_add_tail(&vb->queue, &pcdev->capture);
498         vb->state = VIDEOBUF_QUEUED;
499
500         if (pcdev->active) {
501                 /*
502                  * Capture in progress, so don't touch pcdev->ready even if
503                  * empty. Since the transfer of the DMA programming register set
504                  * content to the DMA working register set is done automatically
505                  * by the DMA hardware, this can pretty well happen while we
506                  * are keeping the lock here. Leave fetching it from the queue
507                  * to be done when a next DMA interrupt occures instead.
508                  */
509                 return;
510         }
511
512         WARN_ON(pcdev->ready);
513
514         buf = prepare_next_vb(pcdev);
515         if (WARN_ON(!buf))
516                 return;
517
518         pcdev->active = buf;
519         pcdev->ready = NULL;
520
521         dev_dbg(icd->parent,
522                 "%s: capture not active, setup FIFO, start DMA\n", __func__);
523         mode = CAM_READ_CACHE(pcdev, MODE) & ~THRESHOLD_MASK;
524         mode |= THRESHOLD_LEVEL(pcdev->vb_mode) << THRESHOLD_SHIFT;
525         CAM_WRITE(pcdev, MODE, mode | EN_FIFO_FULL | DMA);
526
527         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
528                 /*
529                  * In SG mode, the above prepare_next_vb() didn't actually
530                  * put anything into the DMA programming register set,
531                  * so we have to do it now, before activating DMA.
532                  */
533                 try_next_sgbuf(pcdev->dma_ch, buf);
534         }
535
536         start_capture(pcdev);
537 }
538
539 static void omap1_videobuf_release(struct videobuf_queue *vq,
540                                  struct videobuf_buffer *vb)
541 {
542         struct omap1_cam_buf *buf =
543                         container_of(vb, struct omap1_cam_buf, vb);
544         struct soc_camera_device *icd = vq->priv_data;
545         struct device *dev = icd->parent;
546         struct soc_camera_host *ici = to_soc_camera_host(dev);
547         struct omap1_cam_dev *pcdev = ici->priv;
548
549         switch (vb->state) {
550         case VIDEOBUF_DONE:
551                 dev_dbg(dev, "%s (done)\n", __func__);
552                 break;
553         case VIDEOBUF_ACTIVE:
554                 dev_dbg(dev, "%s (active)\n", __func__);
555                 break;
556         case VIDEOBUF_QUEUED:
557                 dev_dbg(dev, "%s (queued)\n", __func__);
558                 break;
559         case VIDEOBUF_PREPARED:
560                 dev_dbg(dev, "%s (prepared)\n", __func__);
561                 break;
562         default:
563                 dev_dbg(dev, "%s (unknown %d)\n", __func__, vb->state);
564                 break;
565         }
566
567         free_buffer(vq, buf, pcdev->vb_mode);
568 }
569
570 static void videobuf_done(struct omap1_cam_dev *pcdev,
571                 enum videobuf_state result)
572 {
573         struct omap1_cam_buf *buf = pcdev->active;
574         struct videobuf_buffer *vb;
575         struct device *dev = pcdev->icd->parent;
576
577         if (WARN_ON(!buf)) {
578                 suspend_capture(pcdev);
579                 disable_capture(pcdev);
580                 return;
581         }
582
583         if (result == VIDEOBUF_ERROR)
584                 suspend_capture(pcdev);
585
586         vb = &buf->vb;
587         if (waitqueue_active(&vb->done)) {
588                 if (!pcdev->ready && result != VIDEOBUF_ERROR) {
589                         /*
590                          * No next buffer has been entered into the DMA
591                          * programming register set on time (could be done only
592                          * while the previous DMA interurpt was processed, not
593                          * later), so the last DMA block, be it a whole buffer
594                          * if in CONTIG or its last sgbuf if in SG mode, is
595                          * about to be reused by the just autoreinitialized DMA
596                          * engine, and overwritten with next frame data. Best we
597                          * can do is stopping the capture as soon as possible,
598                          * hopefully before the next frame start.
599                          */
600                         suspend_capture(pcdev);
601                 }
602                 vb->state = result;
603                 do_gettimeofday(&vb->ts);
604                 if (result != VIDEOBUF_ERROR)
605                         vb->field_count++;
606                 wake_up(&vb->done);
607
608                 /* shift in next buffer */
609                 buf = pcdev->ready;
610                 pcdev->active = buf;
611                 pcdev->ready = NULL;
612
613                 if (!buf) {
614                         /*
615                          * No next buffer was ready on time (see above), so
616                          * indicate error condition to force capture restart or
617                          * stop, depending on next buffer already queued or not.
618                          */
619                         result = VIDEOBUF_ERROR;
620                         prepare_next_vb(pcdev);
621
622                         buf = pcdev->ready;
623                         pcdev->active = buf;
624                         pcdev->ready = NULL;
625                 }
626         } else if (pcdev->ready) {
627                 /*
628                  * In both CONTIG and SG mode, the DMA engine has possibly
629                  * been already autoreinitialized with the preprogrammed
630                  * pcdev->ready buffer.  We can either accept this fact
631                  * and just swap the buffers, or provoke an error condition
632                  * and restart capture.  The former seems less intrusive.
633                  */
634                 dev_dbg(dev, "%s: nobody waiting on videobuf, swap with next\n",
635                                 __func__);
636                 pcdev->active = pcdev->ready;
637
638                 if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
639                         /*
640                          * In SG mode, we have to make sure that the buffer we
641                          * are putting back into the pcdev->ready is marked
642                          * fresh.
643                          */
644                         buf->sgbuf = NULL;
645                 }
646                 pcdev->ready = buf;
647
648                 buf = pcdev->active;
649         } else {
650                 /*
651                  * No next buffer has been entered into
652                  * the DMA programming register set on time.
653                  */
654                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
655                         /*
656                          * In CONTIG mode, the DMA engine has already been
657                          * reinitialized with the current buffer. Best we can do
658                          * is not touching it.
659                          */
660                         dev_dbg(dev,
661                                 "%s: nobody waiting on videobuf, reuse it\n",
662                                 __func__);
663                 } else {
664                         /*
665                          * In SG mode, the DMA engine has just been
666                          * autoreinitialized with the last sgbuf from the
667                          * current list. Restart capture in order to transfer
668                          * next frame start into the first sgbuf, not the last
669                          * one.
670                          */
671                         if (result != VIDEOBUF_ERROR) {
672                                 suspend_capture(pcdev);
673                                 result = VIDEOBUF_ERROR;
674                         }
675                 }
676         }
677
678         if (!buf) {
679                 dev_dbg(dev, "%s: no more videobufs, stop capture\n", __func__);
680                 disable_capture(pcdev);
681                 return;
682         }
683
684         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
685                 /*
686                  * In CONTIG mode, the current buffer parameters had already
687                  * been entered into the DMA programming register set while the
688                  * buffer was fetched with prepare_next_vb(), they may have also
689                  * been transferred into the runtime set and already active if
690                  * the DMA still running.
691                  */
692         } else {
693                 /* In SG mode, extra steps are required */
694                 if (result == VIDEOBUF_ERROR)
695                         /* make sure we (re)use sglist from start on error */
696                         buf->sgbuf = NULL;
697
698                 /*
699                  * In any case, enter the next sgbuf parameters into the DMA
700                  * programming register set.  They will be used either during
701                  * nearest DMA autoreinitialization or, in case of an error,
702                  * on DMA startup below.
703                  */
704                 try_next_sgbuf(pcdev->dma_ch, buf);
705         }
706
707         if (result == VIDEOBUF_ERROR) {
708                 dev_dbg(dev, "%s: videobuf error; reset FIFO, restart DMA\n",
709                                 __func__);
710                 start_capture(pcdev);
711                 /*
712                  * In SG mode, the above also resulted in the next sgbuf
713                  * parameters being entered into the DMA programming register
714                  * set, making them ready for next DMA autoreinitialization.
715                  */
716         }
717
718         /*
719          * Finally, try fetching next buffer.
720          * In CONTIG mode, it will also enter it into the DMA programming
721          * register set, making it ready for next DMA autoreinitialization.
722          */
723         prepare_next_vb(pcdev);
724 }
725
726 static void dma_isr(int channel, unsigned short status, void *data)
727 {
728         struct omap1_cam_dev *pcdev = data;
729         struct omap1_cam_buf *buf = pcdev->active;
730         unsigned long flags;
731
732         spin_lock_irqsave(&pcdev->lock, flags);
733
734         if (WARN_ON(!buf)) {
735                 suspend_capture(pcdev);
736                 disable_capture(pcdev);
737                 goto out;
738         }
739
740         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
741                 /*
742                  * In CONTIG mode, assume we have just managed to collect the
743                  * whole frame, hopefully before our end of frame watchdog is
744                  * triggered. Then, all we have to do is disabling the watchdog
745                  * for this frame, and calling videobuf_done() with success
746                  * indicated.
747                  */
748                 CAM_WRITE(pcdev, MODE,
749                                 CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN);
750                 videobuf_done(pcdev, VIDEOBUF_DONE);
751         } else {
752                 /*
753                  * In SG mode, we have to process every sgbuf from the current
754                  * sglist, one after another.
755                  */
756                 if (buf->sgbuf) {
757                         /*
758                          * Current sglist not completed yet, try fetching next
759                          * sgbuf, hopefully putting it into the DMA programming
760                          * register set, making it ready for next DMA
761                          * autoreinitialization.
762                          */
763                         try_next_sgbuf(pcdev->dma_ch, buf);
764                         if (buf->sgbuf)
765                                 goto out;
766
767                         /*
768                          * No more sgbufs left in the current sglist. This
769                          * doesn't mean that the whole videobuffer is already
770                          * complete, but only that the last sgbuf from the
771                          * current sglist is about to be filled. It will be
772                          * ready on next DMA interrupt, signalled with the
773                          * buf->sgbuf set back to NULL.
774                          */
775                         if (buf->result != VIDEOBUF_ERROR) {
776                                 /*
777                                  * Video frame collected without errors so far,
778                                  * we can prepare for collecting a next one
779                                  * as soon as DMA gets autoreinitialized
780                                  * after the current (last) sgbuf is completed.
781                                  */
782                                 buf = prepare_next_vb(pcdev);
783                                 if (!buf)
784                                         goto out;
785
786                                 try_next_sgbuf(pcdev->dma_ch, buf);
787                                 goto out;
788                         }
789                 }
790                 /* end of videobuf */
791                 videobuf_done(pcdev, buf->result);
792         }
793
794 out:
795         spin_unlock_irqrestore(&pcdev->lock, flags);
796 }
797
798 static irqreturn_t cam_isr(int irq, void *data)
799 {
800         struct omap1_cam_dev *pcdev = data;
801         struct device *dev = pcdev->icd->parent;
802         struct omap1_cam_buf *buf = pcdev->active;
803         u32 it_status;
804         unsigned long flags;
805
806         it_status = CAM_READ(pcdev, IT_STATUS);
807         if (!it_status)
808                 return IRQ_NONE;
809
810         spin_lock_irqsave(&pcdev->lock, flags);
811
812         if (WARN_ON(!buf)) {
813                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
814                          __func__, it_status);
815                 suspend_capture(pcdev);
816                 disable_capture(pcdev);
817                 goto out;
818         }
819
820         if (unlikely(it_status & FIFO_FULL)) {
821                 dev_warn(dev, "%s: FIFO overflow\n", __func__);
822
823         } else if (it_status & V_DOWN) {
824                 /* end of video frame watchdog */
825                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
826                         /*
827                          * In CONTIG mode, the watchdog is disabled with
828                          * successful DMA end of block interrupt, and reenabled
829                          * on next frame start. If we get here, there is nothing
830                          * to check, we must be out of sync.
831                          */
832                 } else {
833                         if (buf->sgcount == 2) {
834                                 /*
835                                  * If exactly 2 sgbufs from the next sglist have
836                                  * been programmed into the DMA engine (the
837                                  * first one already transferred into the DMA
838                                  * runtime register set, the second one still
839                                  * in the programming set), then we are in sync.
840                                  */
841                                 goto out;
842                         }
843                 }
844                 dev_notice(dev, "%s: unexpected end of video frame\n",
845                                 __func__);
846
847         } else if (it_status & V_UP) {
848                 u32 mode;
849
850                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
851                         /*
852                          * In CONTIG mode, we need this interrupt every frame
853                          * in oredr to reenable our end of frame watchdog.
854                          */
855                         mode = CAM_READ_CACHE(pcdev, MODE);
856                 } else {
857                         /*
858                          * In SG mode, the below enabled end of frame watchdog
859                          * is kept on permanently, so we can turn this one shot
860                          * setup off.
861                          */
862                         mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_UP;
863                 }
864
865                 if (!(mode & EN_V_DOWN)) {
866                         /* (re)enable end of frame watchdog interrupt */
867                         mode |= EN_V_DOWN;
868                 }
869                 CAM_WRITE(pcdev, MODE, mode);
870                 goto out;
871
872         } else {
873                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
874                                 __func__, it_status);
875                 goto out;
876         }
877
878         videobuf_done(pcdev, VIDEOBUF_ERROR);
879 out:
880         spin_unlock_irqrestore(&pcdev->lock, flags);
881         return IRQ_HANDLED;
882 }
883
884 static struct videobuf_queue_ops omap1_videobuf_ops = {
885         .buf_setup      = omap1_videobuf_setup,
886         .buf_prepare    = omap1_videobuf_prepare,
887         .buf_queue      = omap1_videobuf_queue,
888         .buf_release    = omap1_videobuf_release,
889 };
890
891
892 /*
893  * SOC Camera host operations
894  */
895
896 static void sensor_reset(struct omap1_cam_dev *pcdev, bool reset)
897 {
898         /* apply/release camera sensor reset if requested by platform data */
899         if (pcdev->pflags & OMAP1_CAMERA_RST_HIGH)
900                 CAM_WRITE(pcdev, GPIO, reset);
901         else if (pcdev->pflags & OMAP1_CAMERA_RST_LOW)
902                 CAM_WRITE(pcdev, GPIO, !reset);
903 }
904
905 /*
906  * The following two functions absolutely depend on the fact, that
907  * there can be only one camera on OMAP1 camera sensor interface
908  */
909 static int omap1_cam_add_device(struct soc_camera_device *icd)
910 {
911         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
912         struct omap1_cam_dev *pcdev = ici->priv;
913         u32 ctrlclock;
914
915         if (pcdev->icd)
916                 return -EBUSY;
917
918         clk_enable(pcdev->clk);
919
920         /* setup sensor clock */
921         ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
922         ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
923         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
924
925         ctrlclock &= ~FOSCMOD_MASK;
926         switch (pcdev->camexclk) {
927         case 6000000:
928                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
929                 break;
930         case 8000000:
931                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
932                 break;
933         case 9600000:
934                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
935                 break;
936         case 12000000:
937                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
938                 break;
939         case 24000000:
940                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
941         default:
942                 break;
943         }
944         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
945
946         /* enable internal clock */
947         ctrlclock |= MCLK_EN;
948         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
949
950         sensor_reset(pcdev, false);
951
952         pcdev->icd = icd;
953
954         dev_dbg(icd->parent, "OMAP1 Camera driver attached to camera %d\n",
955                         icd->devnum);
956         return 0;
957 }
958
959 static void omap1_cam_remove_device(struct soc_camera_device *icd)
960 {
961         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
962         struct omap1_cam_dev *pcdev = ici->priv;
963         u32 ctrlclock;
964
965         BUG_ON(icd != pcdev->icd);
966
967         suspend_capture(pcdev);
968         disable_capture(pcdev);
969
970         sensor_reset(pcdev, true);
971
972         /* disable and release system clocks */
973         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
974         ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
975         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
976
977         ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
978         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
979         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
980
981         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
982
983         clk_disable(pcdev->clk);
984
985         pcdev->icd = NULL;
986
987         dev_dbg(icd->parent,
988                 "OMAP1 Camera driver detached from camera %d\n", icd->devnum);
989 }
990
991 /* Duplicate standard formats based on host capability of byte swapping */
992 static const struct soc_mbus_lookup omap1_cam_formats[] = {
993 {
994         .code = V4L2_MBUS_FMT_UYVY8_2X8,
995         .fmt = {
996                 .fourcc                 = V4L2_PIX_FMT_YUYV,
997                 .name                   = "YUYV",
998                 .bits_per_sample        = 8,
999                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1000                 .order                  = SOC_MBUS_ORDER_BE,
1001         },
1002 }, {
1003         .code = V4L2_MBUS_FMT_VYUY8_2X8,
1004         .fmt = {
1005                 .fourcc                 = V4L2_PIX_FMT_YVYU,
1006                 .name                   = "YVYU",
1007                 .bits_per_sample        = 8,
1008                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1009                 .order                  = SOC_MBUS_ORDER_BE,
1010         },
1011 }, {
1012         .code = V4L2_MBUS_FMT_YUYV8_2X8,
1013         .fmt = {
1014                 .fourcc                 = V4L2_PIX_FMT_UYVY,
1015                 .name                   = "UYVY",
1016                 .bits_per_sample        = 8,
1017                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1018                 .order                  = SOC_MBUS_ORDER_BE,
1019         },
1020 }, {
1021         .code = V4L2_MBUS_FMT_YVYU8_2X8,
1022         .fmt = {
1023                 .fourcc                 = V4L2_PIX_FMT_VYUY,
1024                 .name                   = "VYUY",
1025                 .bits_per_sample        = 8,
1026                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1027                 .order                  = SOC_MBUS_ORDER_BE,
1028         },
1029 }, {
1030         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
1031         .fmt = {
1032                 .fourcc                 = V4L2_PIX_FMT_RGB555,
1033                 .name                   = "RGB555",
1034                 .bits_per_sample        = 8,
1035                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1036                 .order                  = SOC_MBUS_ORDER_BE,
1037         },
1038 }, {
1039         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
1040         .fmt = {
1041                 .fourcc                 = V4L2_PIX_FMT_RGB555X,
1042                 .name                   = "RGB555X",
1043                 .bits_per_sample        = 8,
1044                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1045                 .order                  = SOC_MBUS_ORDER_BE,
1046         },
1047 }, {
1048         .code = V4L2_MBUS_FMT_RGB565_2X8_BE,
1049         .fmt = {
1050                 .fourcc                 = V4L2_PIX_FMT_RGB565,
1051                 .name                   = "RGB565",
1052                 .bits_per_sample        = 8,
1053                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1054                 .order                  = SOC_MBUS_ORDER_BE,
1055         },
1056 }, {
1057         .code = V4L2_MBUS_FMT_RGB565_2X8_LE,
1058         .fmt = {
1059                 .fourcc                 = V4L2_PIX_FMT_RGB565X,
1060                 .name                   = "RGB565X",
1061                 .bits_per_sample        = 8,
1062                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1063                 .order                  = SOC_MBUS_ORDER_BE,
1064         },
1065 },
1066 };
1067
1068 static int omap1_cam_get_formats(struct soc_camera_device *icd,
1069                 unsigned int idx, struct soc_camera_format_xlate *xlate)
1070 {
1071         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1072         struct device *dev = icd->parent;
1073         int formats = 0, ret;
1074         enum v4l2_mbus_pixelcode code;
1075         const struct soc_mbus_pixelfmt *fmt;
1076
1077         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1078         if (ret < 0)
1079                 /* No more formats */
1080                 return 0;
1081
1082         fmt = soc_mbus_get_fmtdesc(code);
1083         if (!fmt) {
1084                 dev_warn(dev, "%s: unsupported format code #%d: %d\n", __func__,
1085                                 idx, code);
1086                 return 0;
1087         }
1088
1089         /* Check support for the requested bits-per-sample */
1090         if (fmt->bits_per_sample != 8)
1091                 return 0;
1092
1093         switch (code) {
1094         case V4L2_MBUS_FMT_YUYV8_2X8:
1095         case V4L2_MBUS_FMT_YVYU8_2X8:
1096         case V4L2_MBUS_FMT_UYVY8_2X8:
1097         case V4L2_MBUS_FMT_VYUY8_2X8:
1098         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
1099         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
1100         case V4L2_MBUS_FMT_RGB565_2X8_BE:
1101         case V4L2_MBUS_FMT_RGB565_2X8_LE:
1102                 formats++;
1103                 if (xlate) {
1104                         xlate->host_fmt = soc_mbus_find_fmtdesc(code,
1105                                                 omap1_cam_formats,
1106                                                 ARRAY_SIZE(omap1_cam_formats));
1107                         xlate->code     = code;
1108                         xlate++;
1109                         dev_dbg(dev,
1110                                 "%s: providing format %s as byte swapped code #%d\n",
1111                                 __func__, xlate->host_fmt->name, code);
1112                 }
1113         default:
1114                 if (xlate)
1115                         dev_dbg(dev,
1116                                 "%s: providing format %s in pass-through mode\n",
1117                                 __func__, fmt->name);
1118         }
1119         formats++;
1120         if (xlate) {
1121                 xlate->host_fmt = fmt;
1122                 xlate->code     = code;
1123                 xlate++;
1124         }
1125
1126         return formats;
1127 }
1128
1129 static bool is_dma_aligned(s32 bytes_per_line, unsigned int height,
1130                 enum omap1_cam_vb_mode vb_mode)
1131 {
1132         int size = bytes_per_line * height;
1133
1134         return IS_ALIGNED(bytes_per_line, DMA_ELEMENT_SIZE) &&
1135                 IS_ALIGNED(size, DMA_FRAME_SIZE(vb_mode) * DMA_ELEMENT_SIZE);
1136 }
1137
1138 static int dma_align(int *width, int *height,
1139                 const struct soc_mbus_pixelfmt *fmt,
1140                 enum omap1_cam_vb_mode vb_mode, bool enlarge)
1141 {
1142         s32 bytes_per_line = soc_mbus_bytes_per_line(*width, fmt);
1143
1144         if (bytes_per_line < 0)
1145                 return bytes_per_line;
1146
1147         if (!is_dma_aligned(bytes_per_line, *height, vb_mode)) {
1148                 unsigned int pxalign = __fls(bytes_per_line / *width);
1149                 unsigned int salign  = DMA_FRAME_SHIFT(vb_mode) +
1150                                 DMA_ELEMENT_SHIFT - pxalign;
1151                 unsigned int incr    = enlarge << salign;
1152
1153                 v4l_bound_align_image(width, 1, *width + incr, 0,
1154                                 height, 1, *height + incr, 0, salign);
1155                 return 0;
1156         }
1157         return 1;
1158 }
1159
1160 #define subdev_call_with_sense(pcdev, dev, icd, sd, function, args...)               \
1161 ({                                                                                   \
1162         struct soc_camera_sense sense = {                                            \
1163                 .master_clock           = pcdev->camexclk,                           \
1164                 .pixel_clock_max        = 0,                                         \
1165         };                                                                           \
1166         int __ret;                                                                   \
1167                                                                                      \
1168         if (pcdev->pdata)                                                            \
1169                 sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000;           \
1170         icd->sense = &sense;                                                         \
1171         __ret = v4l2_subdev_call(sd, video, function, ##args);                       \
1172         icd->sense = NULL;                                                           \
1173                                                                                      \
1174         if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {                                \
1175                 if (sense.pixel_clock > sense.pixel_clock_max) {                     \
1176                         dev_err(dev,                                                 \
1177                                 "%s: pixel clock %lu set by the camera too high!\n", \
1178                                 __func__, sense.pixel_clock);                        \
1179                         __ret = -EINVAL;                                             \
1180                 }                                                                    \
1181         }                                                                            \
1182         __ret;                                                                       \
1183 })
1184
1185 static int set_mbus_format(struct omap1_cam_dev *pcdev, struct device *dev,
1186                 struct soc_camera_device *icd, struct v4l2_subdev *sd,
1187                 struct v4l2_mbus_framefmt *mf,
1188                 const struct soc_camera_format_xlate *xlate)
1189 {
1190         s32 bytes_per_line;
1191         int ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_mbus_fmt, mf);
1192
1193         if (ret < 0) {
1194                 dev_err(dev, "%s: s_mbus_fmt failed\n", __func__);
1195                 return ret;
1196         }
1197
1198         if (mf->code != xlate->code) {
1199                 dev_err(dev, "%s: unexpected pixel code change\n", __func__);
1200                 return -EINVAL;
1201         }
1202
1203         bytes_per_line = soc_mbus_bytes_per_line(mf->width, xlate->host_fmt);
1204         if (bytes_per_line < 0) {
1205                 dev_err(dev, "%s: soc_mbus_bytes_per_line() failed\n",
1206                                 __func__);
1207                 return bytes_per_line;
1208         }
1209
1210         if (!is_dma_aligned(bytes_per_line, mf->height, pcdev->vb_mode)) {
1211                 dev_err(dev, "%s: resulting geometry %ux%u not DMA aligned\n",
1212                                 __func__, mf->width, mf->height);
1213                 return -EINVAL;
1214         }
1215         return 0;
1216 }
1217
1218 static int omap1_cam_set_crop(struct soc_camera_device *icd,
1219                                struct v4l2_crop *crop)
1220 {
1221         struct v4l2_rect *rect = &crop->c;
1222         const struct soc_camera_format_xlate *xlate = icd->current_fmt;
1223         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1224         struct device *dev = icd->parent;
1225         struct soc_camera_host *ici = to_soc_camera_host(dev);
1226         struct omap1_cam_dev *pcdev = ici->priv;
1227         struct v4l2_mbus_framefmt mf;
1228         int ret;
1229
1230         ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_crop, crop);
1231         if (ret < 0) {
1232                 dev_warn(dev, "%s: failed to crop to %ux%u@%u:%u\n", __func__,
1233                          rect->width, rect->height, rect->left, rect->top);
1234                 return ret;
1235         }
1236
1237         ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1238         if (ret < 0) {
1239                 dev_warn(dev, "%s: failed to fetch current format\n", __func__);
1240                 return ret;
1241         }
1242
1243         ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
1244                         false);
1245         if (ret < 0) {
1246                 dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
1247                                 __func__, mf.width, mf.height,
1248                                 xlate->host_fmt->name);
1249                 return ret;
1250         }
1251
1252         if (!ret) {
1253                 /* sensor returned geometry not DMA aligned, trying to fix */
1254                 ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
1255                 if (ret < 0) {
1256                         dev_err(dev, "%s: failed to set format\n", __func__);
1257                         return ret;
1258                 }
1259         }
1260
1261         icd->user_width  = mf.width;
1262         icd->user_height = mf.height;
1263
1264         return 0;
1265 }
1266
1267 static int omap1_cam_set_fmt(struct soc_camera_device *icd,
1268                               struct v4l2_format *f)
1269 {
1270         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1271         const struct soc_camera_format_xlate *xlate;
1272         struct device *dev = icd->parent;
1273         struct soc_camera_host *ici = to_soc_camera_host(dev);
1274         struct omap1_cam_dev *pcdev = ici->priv;
1275         struct v4l2_pix_format *pix = &f->fmt.pix;
1276         struct v4l2_mbus_framefmt mf;
1277         int ret;
1278
1279         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1280         if (!xlate) {
1281                 dev_warn(dev, "%s: format %#x not found\n", __func__,
1282                                 pix->pixelformat);
1283                 return -EINVAL;
1284         }
1285
1286         mf.width        = pix->width;
1287         mf.height       = pix->height;
1288         mf.field        = pix->field;
1289         mf.colorspace   = pix->colorspace;
1290         mf.code         = xlate->code;
1291
1292         ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
1293                         true);
1294         if (ret < 0) {
1295                 dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
1296                                 __func__, pix->width, pix->height,
1297                                 xlate->host_fmt->name);
1298                 return ret;
1299         }
1300
1301         ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
1302         if (ret < 0) {
1303                 dev_err(dev, "%s: failed to set format\n", __func__);
1304                 return ret;
1305         }
1306
1307         pix->width       = mf.width;
1308         pix->height      = mf.height;
1309         pix->field       = mf.field;
1310         pix->colorspace  = mf.colorspace;
1311         icd->current_fmt = xlate;
1312
1313         return 0;
1314 }
1315
1316 static int omap1_cam_try_fmt(struct soc_camera_device *icd,
1317                               struct v4l2_format *f)
1318 {
1319         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1320         const struct soc_camera_format_xlate *xlate;
1321         struct v4l2_pix_format *pix = &f->fmt.pix;
1322         struct v4l2_mbus_framefmt mf;
1323         int ret;
1324         /* TODO: limit to mx1 hardware capabilities */
1325
1326         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1327         if (!xlate) {
1328                 dev_warn(icd->parent, "Format %#x not found\n",
1329                          pix->pixelformat);
1330                 return -EINVAL;
1331         }
1332
1333         mf.width        = pix->width;
1334         mf.height       = pix->height;
1335         mf.field        = pix->field;
1336         mf.colorspace   = pix->colorspace;
1337         mf.code         = xlate->code;
1338
1339         /* limit to sensor capabilities */
1340         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1341         if (ret < 0)
1342                 return ret;
1343
1344         pix->width      = mf.width;
1345         pix->height     = mf.height;
1346         pix->field      = mf.field;
1347         pix->colorspace = mf.colorspace;
1348
1349         return 0;
1350 }
1351
1352 static bool sg_mode;
1353
1354 /*
1355  * Local mmap_mapper wrapper,
1356  * used for detecting videobuf-dma-contig buffer allocation failures
1357  * and switching to videobuf-dma-sg automatically for future attempts.
1358  */
1359 static int omap1_cam_mmap_mapper(struct videobuf_queue *q,
1360                                   struct videobuf_buffer *buf,
1361                                   struct vm_area_struct *vma)
1362 {
1363         struct soc_camera_device *icd = q->priv_data;
1364         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1365         struct omap1_cam_dev *pcdev = ici->priv;
1366         int ret;
1367
1368         ret = pcdev->mmap_mapper(q, buf, vma);
1369
1370         if (ret == -ENOMEM)
1371                 sg_mode = true;
1372
1373         return ret;
1374 }
1375
1376 static void omap1_cam_init_videobuf(struct videobuf_queue *q,
1377                                      struct soc_camera_device *icd)
1378 {
1379         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1380         struct omap1_cam_dev *pcdev = ici->priv;
1381
1382         if (!sg_mode)
1383                 videobuf_queue_dma_contig_init(q, &omap1_videobuf_ops,
1384                                 icd->parent, &pcdev->lock,
1385                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
1386                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
1387         else
1388                 videobuf_queue_sg_init(q, &omap1_videobuf_ops,
1389                                 icd->parent, &pcdev->lock,
1390                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
1391                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
1392
1393         /* use videobuf mode (auto)selected with the module parameter */
1394         pcdev->vb_mode = sg_mode ? OMAP1_CAM_DMA_SG : OMAP1_CAM_DMA_CONTIG;
1395
1396         /*
1397          * Ensure we substitute the videobuf-dma-contig version of the
1398          * mmap_mapper() callback with our own wrapper, used for switching
1399          * automatically to videobuf-dma-sg on buffer allocation failure.
1400          */
1401         if (!sg_mode && q->int_ops->mmap_mapper != omap1_cam_mmap_mapper) {
1402                 pcdev->mmap_mapper = q->int_ops->mmap_mapper;
1403                 q->int_ops->mmap_mapper = omap1_cam_mmap_mapper;
1404         }
1405 }
1406
1407 static int omap1_cam_reqbufs(struct soc_camera_device *icd,
1408                               struct v4l2_requestbuffers *p)
1409 {
1410         int i;
1411
1412         /*
1413          * This is for locking debugging only. I removed spinlocks and now I
1414          * check whether .prepare is ever called on a linked buffer, or whether
1415          * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1416          * it hadn't triggered
1417          */
1418         for (i = 0; i < p->count; i++) {
1419                 struct omap1_cam_buf *buf = container_of(icd->vb_vidq.bufs[i],
1420                                                       struct omap1_cam_buf, vb);
1421                 buf->inwork = 0;
1422                 INIT_LIST_HEAD(&buf->vb.queue);
1423         }
1424
1425         return 0;
1426 }
1427
1428 static int omap1_cam_querycap(struct soc_camera_host *ici,
1429                                struct v4l2_capability *cap)
1430 {
1431         /* cap->name is set by the friendly caller:-> */
1432         strlcpy(cap->card, "OMAP1 Camera", sizeof(cap->card));
1433         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1434
1435         return 0;
1436 }
1437
1438 static int omap1_cam_set_bus_param(struct soc_camera_device *icd,
1439                 __u32 pixfmt)
1440 {
1441         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1442         struct device *dev = icd->parent;
1443         struct soc_camera_host *ici = to_soc_camera_host(dev);
1444         struct omap1_cam_dev *pcdev = ici->priv;
1445         const struct soc_camera_format_xlate *xlate;
1446         const struct soc_mbus_pixelfmt *fmt;
1447         struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
1448         unsigned long common_flags;
1449         u32 ctrlclock, mode;
1450         int ret;
1451
1452         ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
1453         if (!ret) {
1454                 common_flags = soc_mbus_config_compatible(&cfg, SOCAM_BUS_FLAGS);
1455                 if (!common_flags) {
1456                         dev_warn(dev,
1457                                  "Flags incompatible: camera 0x%x, host 0x%x\n",
1458                                  cfg.flags, SOCAM_BUS_FLAGS);
1459                         return -EINVAL;
1460                 }
1461         } else if (ret != -ENOIOCTLCMD) {
1462                 return ret;
1463         } else {
1464                 common_flags = SOCAM_BUS_FLAGS;
1465         }
1466
1467         /* Make choices, possibly based on platform configuration */
1468         if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
1469                         (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
1470                 if (!pcdev->pdata ||
1471                                 pcdev->pdata->flags & OMAP1_CAMERA_LCLK_RISING)
1472                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
1473                 else
1474                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
1475         }
1476
1477         cfg.flags = common_flags;
1478         ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
1479         if (ret < 0 && ret != -ENOIOCTLCMD) {
1480                 dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
1481                         common_flags, ret);
1482                 return ret;
1483         }
1484
1485         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
1486         if (ctrlclock & LCLK_EN)
1487                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
1488
1489         if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) {
1490                 dev_dbg(dev, "CTRLCLOCK_REG |= POLCLK\n");
1491                 ctrlclock |= POLCLK;
1492         } else {
1493                 dev_dbg(dev, "CTRLCLOCK_REG &= ~POLCLK\n");
1494                 ctrlclock &= ~POLCLK;
1495         }
1496         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
1497
1498         if (ctrlclock & LCLK_EN)
1499                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
1500
1501         /* select bus endianess */
1502         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1503         fmt = xlate->host_fmt;
1504
1505         mode = CAM_READ(pcdev, MODE) & ~(RAZ_FIFO | IRQ_MASK | DMA);
1506         if (fmt->order == SOC_MBUS_ORDER_LE) {
1507                 dev_dbg(dev, "MODE_REG &= ~ORDERCAMD\n");
1508                 CAM_WRITE(pcdev, MODE, mode & ~ORDERCAMD);
1509         } else {
1510                 dev_dbg(dev, "MODE_REG |= ORDERCAMD\n");
1511                 CAM_WRITE(pcdev, MODE, mode | ORDERCAMD);
1512         }
1513
1514         return 0;
1515 }
1516
1517 static unsigned int omap1_cam_poll(struct file *file, poll_table *pt)
1518 {
1519         struct soc_camera_device *icd = file->private_data;
1520         struct omap1_cam_buf *buf;
1521
1522         buf = list_entry(icd->vb_vidq.stream.next, struct omap1_cam_buf,
1523                          vb.stream);
1524
1525         poll_wait(file, &buf->vb.done, pt);
1526
1527         if (buf->vb.state == VIDEOBUF_DONE ||
1528             buf->vb.state == VIDEOBUF_ERROR)
1529                 return POLLIN | POLLRDNORM;
1530
1531         return 0;
1532 }
1533
1534 static struct soc_camera_host_ops omap1_host_ops = {
1535         .owner          = THIS_MODULE,
1536         .add            = omap1_cam_add_device,
1537         .remove         = omap1_cam_remove_device,
1538         .get_formats    = omap1_cam_get_formats,
1539         .set_crop       = omap1_cam_set_crop,
1540         .set_fmt        = omap1_cam_set_fmt,
1541         .try_fmt        = omap1_cam_try_fmt,
1542         .init_videobuf  = omap1_cam_init_videobuf,
1543         .reqbufs        = omap1_cam_reqbufs,
1544         .querycap       = omap1_cam_querycap,
1545         .set_bus_param  = omap1_cam_set_bus_param,
1546         .poll           = omap1_cam_poll,
1547 };
1548
1549 static int __init omap1_cam_probe(struct platform_device *pdev)
1550 {
1551         struct omap1_cam_dev *pcdev;
1552         struct resource *res;
1553         struct clk *clk;
1554         void __iomem *base;
1555         unsigned int irq;
1556         int err = 0;
1557
1558         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1559         irq = platform_get_irq(pdev, 0);
1560         if (!res || (int)irq <= 0) {
1561                 err = -ENODEV;
1562                 goto exit;
1563         }
1564
1565         clk = clk_get(&pdev->dev, "armper_ck");
1566         if (IS_ERR(clk)) {
1567                 err = PTR_ERR(clk);
1568                 goto exit;
1569         }
1570
1571         pcdev = kzalloc(sizeof(*pcdev) + resource_size(res), GFP_KERNEL);
1572         if (!pcdev) {
1573                 dev_err(&pdev->dev, "Could not allocate pcdev\n");
1574                 err = -ENOMEM;
1575                 goto exit_put_clk;
1576         }
1577
1578         pcdev->res = res;
1579         pcdev->clk = clk;
1580
1581         pcdev->pdata = pdev->dev.platform_data;
1582         if (pcdev->pdata) {
1583                 pcdev->pflags = pcdev->pdata->flags;
1584                 pcdev->camexclk = pcdev->pdata->camexclk_khz * 1000;
1585         }
1586
1587         switch (pcdev->camexclk) {
1588         case 6000000:
1589         case 8000000:
1590         case 9600000:
1591         case 12000000:
1592         case 24000000:
1593                 break;
1594         default:
1595                 /* pcdev->camexclk != 0 => pcdev->pdata != NULL */
1596                 dev_warn(&pdev->dev,
1597                                 "Incorrect sensor clock frequency %ld kHz, "
1598                                 "should be one of 0, 6, 8, 9.6, 12 or 24 MHz, "
1599                                 "please correct your platform data\n",
1600                                 pcdev->pdata->camexclk_khz);
1601                 pcdev->camexclk = 0;
1602         case 0:
1603                 dev_info(&pdev->dev, "Not providing sensor clock\n");
1604         }
1605
1606         INIT_LIST_HEAD(&pcdev->capture);
1607         spin_lock_init(&pcdev->lock);
1608
1609         /*
1610          * Request the region.
1611          */
1612         if (!request_mem_region(res->start, resource_size(res), DRIVER_NAME)) {
1613                 err = -EBUSY;
1614                 goto exit_kfree;
1615         }
1616
1617         base = ioremap(res->start, resource_size(res));
1618         if (!base) {
1619                 err = -ENOMEM;
1620                 goto exit_release;
1621         }
1622         pcdev->irq = irq;
1623         pcdev->base = base;
1624
1625         sensor_reset(pcdev, true);
1626
1627         err = omap_request_dma(OMAP_DMA_CAMERA_IF_RX, DRIVER_NAME,
1628                         dma_isr, (void *)pcdev, &pcdev->dma_ch);
1629         if (err < 0) {
1630                 dev_err(&pdev->dev, "Can't request DMA for OMAP1 Camera\n");
1631                 err = -EBUSY;
1632                 goto exit_iounmap;
1633         }
1634         dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_ch);
1635
1636         /* preconfigure DMA */
1637         omap_set_dma_src_params(pcdev->dma_ch, OMAP_DMA_PORT_TIPB,
1638                         OMAP_DMA_AMODE_CONSTANT, res->start + REG_CAMDATA,
1639                         0, 0);
1640         omap_set_dma_dest_burst_mode(pcdev->dma_ch, OMAP_DMA_DATA_BURST_4);
1641         /* setup DMA autoinitialization */
1642         omap_dma_link_lch(pcdev->dma_ch, pcdev->dma_ch);
1643
1644         err = request_irq(pcdev->irq, cam_isr, 0, DRIVER_NAME, pcdev);
1645         if (err) {
1646                 dev_err(&pdev->dev, "Camera interrupt register failed\n");
1647                 goto exit_free_dma;
1648         }
1649
1650         pcdev->soc_host.drv_name        = DRIVER_NAME;
1651         pcdev->soc_host.ops             = &omap1_host_ops;
1652         pcdev->soc_host.priv            = pcdev;
1653         pcdev->soc_host.v4l2_dev.dev    = &pdev->dev;
1654         pcdev->soc_host.nr              = pdev->id;
1655
1656         err = soc_camera_host_register(&pcdev->soc_host);
1657         if (err)
1658                 goto exit_free_irq;
1659
1660         dev_info(&pdev->dev, "OMAP1 Camera Interface driver loaded\n");
1661
1662         return 0;
1663
1664 exit_free_irq:
1665         free_irq(pcdev->irq, pcdev);
1666 exit_free_dma:
1667         omap_free_dma(pcdev->dma_ch);
1668 exit_iounmap:
1669         iounmap(base);
1670 exit_release:
1671         release_mem_region(res->start, resource_size(res));
1672 exit_kfree:
1673         kfree(pcdev);
1674 exit_put_clk:
1675         clk_put(clk);
1676 exit:
1677         return err;
1678 }
1679
1680 static int __exit omap1_cam_remove(struct platform_device *pdev)
1681 {
1682         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
1683         struct omap1_cam_dev *pcdev = container_of(soc_host,
1684                                         struct omap1_cam_dev, soc_host);
1685         struct resource *res;
1686
1687         free_irq(pcdev->irq, pcdev);
1688
1689         omap_free_dma(pcdev->dma_ch);
1690
1691         soc_camera_host_unregister(soc_host);
1692
1693         iounmap(pcdev->base);
1694
1695         res = pcdev->res;
1696         release_mem_region(res->start, resource_size(res));
1697
1698         clk_put(pcdev->clk);
1699
1700         kfree(pcdev);
1701
1702         dev_info(&pdev->dev, "OMAP1 Camera Interface driver unloaded\n");
1703
1704         return 0;
1705 }
1706
1707 static struct platform_driver omap1_cam_driver = {
1708         .driver         = {
1709                 .name   = DRIVER_NAME,
1710         },
1711         .probe          = omap1_cam_probe,
1712         .remove         = __exit_p(omap1_cam_remove),
1713 };
1714
1715 static int __init omap1_cam_init(void)
1716 {
1717         return platform_driver_register(&omap1_cam_driver);
1718 }
1719 module_init(omap1_cam_init);
1720
1721 static void __exit omap1_cam_exit(void)
1722 {
1723         platform_driver_unregister(&omap1_cam_driver);
1724 }
1725 module_exit(omap1_cam_exit);
1726
1727 module_param(sg_mode, bool, 0644);
1728 MODULE_PARM_DESC(sg_mode, "videobuf mode, 0: dma-contig (default), 1: dma-sg");
1729
1730 MODULE_DESCRIPTION("OMAP1 Camera Interface driver");
1731 MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
1732 MODULE_LICENSE("GPL v2");
1733 MODULE_VERSION(DRIVER_VERSION);
1734 MODULE_ALIAS("platform:" DRIVER_NAME);