ARM: OMAP1: DMA: Moving OMAP1 DMA channel definitions to mach-omap1
[muen/linux.git] / drivers / media / platform / soc_camera / 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/platform/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/module.h>
28 #include <linux/platform_device.h>
29 #include <linux/slab.h>
30
31 #include <media/omap1_camera.h>
32 #include <media/soc_camera.h>
33 #include <media/soc_mediabus.h>
34 #include <media/videobuf-dma-contig.h>
35 #include <media/videobuf-dma-sg.h>
36
37 #include <plat/dma.h>
38
39
40 #define DRIVER_NAME             "omap1-camera"
41 #define DRIVER_VERSION          "0.0.2"
42
43 #define OMAP_DMA_CAMERA_IF_RX           20
44
45 /*
46  * ---------------------------------------------------------------------------
47  *  OMAP1 Camera Interface registers
48  * ---------------------------------------------------------------------------
49  */
50
51 #define REG_CTRLCLOCK           0x00
52 #define REG_IT_STATUS           0x04
53 #define REG_MODE                0x08
54 #define REG_STATUS              0x0C
55 #define REG_CAMDATA             0x10
56 #define REG_GPIO                0x14
57 #define REG_PEAK_COUNTER        0x18
58
59 /* CTRLCLOCK bit shifts */
60 #define LCLK_EN                 BIT(7)
61 #define DPLL_EN                 BIT(6)
62 #define MCLK_EN                 BIT(5)
63 #define CAMEXCLK_EN             BIT(4)
64 #define POLCLK                  BIT(3)
65 #define FOSCMOD_SHIFT           0
66 #define FOSCMOD_MASK            (0x7 << FOSCMOD_SHIFT)
67 #define FOSCMOD_12MHz           0x0
68 #define FOSCMOD_6MHz            0x2
69 #define FOSCMOD_9_6MHz          0x4
70 #define FOSCMOD_24MHz           0x5
71 #define FOSCMOD_8MHz            0x6
72
73 /* IT_STATUS bit shifts */
74 #define DATA_TRANSFER           BIT(5)
75 #define FIFO_FULL               BIT(4)
76 #define H_DOWN                  BIT(3)
77 #define H_UP                    BIT(2)
78 #define V_DOWN                  BIT(1)
79 #define V_UP                    BIT(0)
80
81 /* MODE bit shifts */
82 #define RAZ_FIFO                BIT(18)
83 #define EN_FIFO_FULL            BIT(17)
84 #define EN_NIRQ                 BIT(16)
85 #define THRESHOLD_SHIFT         9
86 #define THRESHOLD_MASK          (0x7f << THRESHOLD_SHIFT)
87 #define DMA                     BIT(8)
88 #define EN_H_DOWN               BIT(7)
89 #define EN_H_UP                 BIT(6)
90 #define EN_V_DOWN               BIT(5)
91 #define EN_V_UP                 BIT(4)
92 #define ORDERCAMD               BIT(3)
93
94 #define IRQ_MASK                (EN_V_UP | EN_V_DOWN | EN_H_UP | EN_H_DOWN | \
95                                  EN_NIRQ | EN_FIFO_FULL)
96
97 /* STATUS bit shifts */
98 #define HSTATUS                 BIT(1)
99 #define VSTATUS                 BIT(0)
100
101 /* GPIO bit shifts */
102 #define CAM_RST                 BIT(0)
103
104 /* end of OMAP1 Camera Interface registers */
105
106
107 #define SOCAM_BUS_FLAGS (V4L2_MBUS_MASTER | \
108                         V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
109                         V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
110                         V4L2_MBUS_DATA_ACTIVE_HIGH)
111
112
113 #define FIFO_SIZE               ((THRESHOLD_MASK >> THRESHOLD_SHIFT) + 1)
114 #define FIFO_SHIFT              __fls(FIFO_SIZE)
115
116 #define DMA_BURST_SHIFT         (1 + OMAP_DMA_DATA_BURST_4)
117 #define DMA_BURST_SIZE          (1 << DMA_BURST_SHIFT)
118
119 #define DMA_ELEMENT_SHIFT       OMAP_DMA_DATA_TYPE_S32
120 #define DMA_ELEMENT_SIZE        (1 << DMA_ELEMENT_SHIFT)
121
122 #define DMA_FRAME_SHIFT_CONTIG  (FIFO_SHIFT - 1)
123 #define DMA_FRAME_SHIFT_SG      DMA_BURST_SHIFT
124
125 #define DMA_FRAME_SHIFT(x)      ((x) == OMAP1_CAM_DMA_CONTIG ? \
126                                                 DMA_FRAME_SHIFT_CONTIG : \
127                                                 DMA_FRAME_SHIFT_SG)
128 #define DMA_FRAME_SIZE(x)       (1 << DMA_FRAME_SHIFT(x))
129 #define DMA_SYNC                OMAP_DMA_SYNC_FRAME
130 #define THRESHOLD_LEVEL         DMA_FRAME_SIZE
131
132
133 #define MAX_VIDEO_MEM           4       /* arbitrary video memory limit in MB */
134
135
136 /*
137  * Structures
138  */
139
140 /* buffer for one video frame */
141 struct omap1_cam_buf {
142         struct videobuf_buffer          vb;
143         enum v4l2_mbus_pixelcode        code;
144         int                             inwork;
145         struct scatterlist              *sgbuf;
146         int                             sgcount;
147         int                             bytes_left;
148         enum videobuf_state             result;
149 };
150
151 struct omap1_cam_dev {
152         struct soc_camera_host          soc_host;
153         struct soc_camera_device        *icd;
154         struct clk                      *clk;
155
156         unsigned int                    irq;
157         void __iomem                    *base;
158
159         int                             dma_ch;
160
161         struct omap1_cam_platform_data  *pdata;
162         struct resource                 *res;
163         unsigned long                   pflags;
164         unsigned long                   camexclk;
165
166         struct list_head                capture;
167
168         /* lock used to protect videobuf */
169         spinlock_t                      lock;
170
171         /* Pointers to DMA buffers */
172         struct omap1_cam_buf            *active;
173         struct omap1_cam_buf            *ready;
174
175         enum omap1_cam_vb_mode          vb_mode;
176         int                             (*mmap_mapper)(struct videobuf_queue *q,
177                                                 struct videobuf_buffer *buf,
178                                                 struct vm_area_struct *vma);
179
180         u32                             reg_cache[0];
181 };
182
183
184 static void cam_write(struct omap1_cam_dev *pcdev, u16 reg, u32 val)
185 {
186         pcdev->reg_cache[reg / sizeof(u32)] = val;
187         __raw_writel(val, pcdev->base + reg);
188 }
189
190 static u32 cam_read(struct omap1_cam_dev *pcdev, u16 reg, bool from_cache)
191 {
192         return !from_cache ? __raw_readl(pcdev->base + reg) :
193                         pcdev->reg_cache[reg / sizeof(u32)];
194 }
195
196 #define CAM_READ(pcdev, reg) \
197                 cam_read(pcdev, REG_##reg, false)
198 #define CAM_WRITE(pcdev, reg, val) \
199                 cam_write(pcdev, REG_##reg, val)
200 #define CAM_READ_CACHE(pcdev, reg) \
201                 cam_read(pcdev, REG_##reg, true)
202
203 /*
204  *  Videobuf operations
205  */
206 static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
207                 unsigned int *size)
208 {
209         struct soc_camera_device *icd = vq->priv_data;
210         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
211         struct omap1_cam_dev *pcdev = ici->priv;
212
213         *size = icd->sizeimage;
214
215         if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
216                 *count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
217
218         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
219                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
220
221         dev_dbg(icd->parent,
222                         "%s: count=%d, size=%d\n", __func__, *count, *size);
223
224         return 0;
225 }
226
227 static void free_buffer(struct videobuf_queue *vq, struct omap1_cam_buf *buf,
228                 enum omap1_cam_vb_mode vb_mode)
229 {
230         struct videobuf_buffer *vb = &buf->vb;
231
232         BUG_ON(in_interrupt());
233
234         videobuf_waiton(vq, vb, 0, 0);
235
236         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
237                 videobuf_dma_contig_free(vq, vb);
238         } else {
239                 struct soc_camera_device *icd = vq->priv_data;
240                 struct device *dev = icd->parent;
241                 struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
242
243                 videobuf_dma_unmap(dev, dma);
244                 videobuf_dma_free(dma);
245         }
246
247         vb->state = VIDEOBUF_NEEDS_INIT;
248 }
249
250 static int omap1_videobuf_prepare(struct videobuf_queue *vq,
251                 struct videobuf_buffer *vb, enum v4l2_field field)
252 {
253         struct soc_camera_device *icd = vq->priv_data;
254         struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
255         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
256         struct omap1_cam_dev *pcdev = ici->priv;
257         int ret;
258
259         WARN_ON(!list_empty(&vb->queue));
260
261         BUG_ON(NULL == icd->current_fmt);
262
263         buf->inwork = 1;
264
265         if (buf->code != icd->current_fmt->code || vb->field != field ||
266                         vb->width  != icd->user_width ||
267                         vb->height != icd->user_height) {
268                 buf->code  = icd->current_fmt->code;
269                 vb->width  = icd->user_width;
270                 vb->height = icd->user_height;
271                 vb->field  = field;
272                 vb->state  = VIDEOBUF_NEEDS_INIT;
273         }
274
275         vb->size = icd->sizeimage;
276
277         if (vb->baddr && vb->bsize < vb->size) {
278                 ret = -EINVAL;
279                 goto out;
280         }
281
282         if (vb->state == VIDEOBUF_NEEDS_INIT) {
283                 ret = videobuf_iolock(vq, vb, NULL);
284                 if (ret)
285                         goto fail;
286
287                 vb->state = VIDEOBUF_PREPARED;
288         }
289         buf->inwork = 0;
290
291         return 0;
292 fail:
293         free_buffer(vq, buf, pcdev->vb_mode);
294 out:
295         buf->inwork = 0;
296         return ret;
297 }
298
299 static void set_dma_dest_params(int dma_ch, struct omap1_cam_buf *buf,
300                 enum omap1_cam_vb_mode vb_mode)
301 {
302         dma_addr_t dma_addr;
303         unsigned int block_size;
304
305         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
306                 dma_addr = videobuf_to_dma_contig(&buf->vb);
307                 block_size = buf->vb.size;
308         } else {
309                 if (WARN_ON(!buf->sgbuf)) {
310                         buf->result = VIDEOBUF_ERROR;
311                         return;
312                 }
313                 dma_addr = sg_dma_address(buf->sgbuf);
314                 if (WARN_ON(!dma_addr)) {
315                         buf->sgbuf = NULL;
316                         buf->result = VIDEOBUF_ERROR;
317                         return;
318                 }
319                 block_size = sg_dma_len(buf->sgbuf);
320                 if (WARN_ON(!block_size)) {
321                         buf->sgbuf = NULL;
322                         buf->result = VIDEOBUF_ERROR;
323                         return;
324                 }
325                 if (unlikely(buf->bytes_left < block_size))
326                         block_size = buf->bytes_left;
327                 if (WARN_ON(dma_addr & (DMA_FRAME_SIZE(vb_mode) *
328                                 DMA_ELEMENT_SIZE - 1))) {
329                         dma_addr = ALIGN(dma_addr, DMA_FRAME_SIZE(vb_mode) *
330                                         DMA_ELEMENT_SIZE);
331                         block_size &= ~(DMA_FRAME_SIZE(vb_mode) *
332                                         DMA_ELEMENT_SIZE - 1);
333                 }
334                 buf->bytes_left -= block_size;
335                 buf->sgcount++;
336         }
337
338         omap_set_dma_dest_params(dma_ch,
339                 OMAP_DMA_PORT_EMIFF, OMAP_DMA_AMODE_POST_INC, dma_addr, 0, 0);
340         omap_set_dma_transfer_params(dma_ch,
341                 OMAP_DMA_DATA_TYPE_S32, DMA_FRAME_SIZE(vb_mode),
342                 block_size >> (DMA_FRAME_SHIFT(vb_mode) + DMA_ELEMENT_SHIFT),
343                 DMA_SYNC, 0, 0);
344 }
345
346 static struct omap1_cam_buf *prepare_next_vb(struct omap1_cam_dev *pcdev)
347 {
348         struct omap1_cam_buf *buf;
349
350         /*
351          * If there is already a buffer pointed out by the pcdev->ready,
352          * (re)use it, otherwise try to fetch and configure a new one.
353          */
354         buf = pcdev->ready;
355         if (!buf) {
356                 if (list_empty(&pcdev->capture))
357                         return buf;
358                 buf = list_entry(pcdev->capture.next,
359                                 struct omap1_cam_buf, vb.queue);
360                 buf->vb.state = VIDEOBUF_ACTIVE;
361                 pcdev->ready = buf;
362                 list_del_init(&buf->vb.queue);
363         }
364
365         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
366                 /*
367                  * In CONTIG mode, we can safely enter next buffer parameters
368                  * into the DMA programming register set after the DMA
369                  * has already been activated on the previous buffer
370                  */
371                 set_dma_dest_params(pcdev->dma_ch, buf, pcdev->vb_mode);
372         } else {
373                 /*
374                  * In SG mode, the above is not safe since there are probably
375                  * a bunch of sgbufs from previous sglist still pending.
376                  * Instead, mark the sglist fresh for the upcoming
377                  * try_next_sgbuf().
378                  */
379                 buf->sgbuf = NULL;
380         }
381
382         return buf;
383 }
384
385 static struct scatterlist *try_next_sgbuf(int dma_ch, struct omap1_cam_buf *buf)
386 {
387         struct scatterlist *sgbuf;
388
389         if (likely(buf->sgbuf)) {
390                 /* current sglist is active */
391                 if (unlikely(!buf->bytes_left)) {
392                         /* indicate sglist complete */
393                         sgbuf = NULL;
394                 } else {
395                         /* process next sgbuf */
396                         sgbuf = sg_next(buf->sgbuf);
397                         if (WARN_ON(!sgbuf)) {
398                                 buf->result = VIDEOBUF_ERROR;
399                         } else if (WARN_ON(!sg_dma_len(sgbuf))) {
400                                 sgbuf = NULL;
401                                 buf->result = VIDEOBUF_ERROR;
402                         }
403                 }
404                 buf->sgbuf = sgbuf;
405         } else {
406                 /* sglist is fresh, initialize it before using */
407                 struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
408
409                 sgbuf = dma->sglist;
410                 if (!(WARN_ON(!sgbuf))) {
411                         buf->sgbuf = sgbuf;
412                         buf->sgcount = 0;
413                         buf->bytes_left = buf->vb.size;
414                         buf->result = VIDEOBUF_DONE;
415                 }
416         }
417         if (sgbuf)
418                 /*
419                  * Put our next sgbuf parameters (address, size)
420                  * into the DMA programming register set.
421                  */
422                 set_dma_dest_params(dma_ch, buf, OMAP1_CAM_DMA_SG);
423
424         return sgbuf;
425 }
426
427 static void start_capture(struct omap1_cam_dev *pcdev)
428 {
429         struct omap1_cam_buf *buf = pcdev->active;
430         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
431         u32 mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN;
432
433         if (WARN_ON(!buf))
434                 return;
435
436         /*
437          * Enable start of frame interrupt, which we will use for activating
438          * our end of frame watchdog when capture actually starts.
439          */
440         mode |= EN_V_UP;
441
442         if (unlikely(ctrlclock & LCLK_EN))
443                 /* stop pixel clock before FIFO reset */
444                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
445         /* reset FIFO */
446         CAM_WRITE(pcdev, MODE, mode | RAZ_FIFO);
447
448         omap_start_dma(pcdev->dma_ch);
449
450         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
451                 /*
452                  * In SG mode, it's a good moment for fetching next sgbuf
453                  * from the current sglist and, if available, already putting
454                  * its parameters into the DMA programming register set.
455                  */
456                 try_next_sgbuf(pcdev->dma_ch, buf);
457         }
458
459         /* (re)enable pixel clock */
460         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | LCLK_EN);
461         /* release FIFO reset */
462         CAM_WRITE(pcdev, MODE, mode);
463 }
464
465 static void suspend_capture(struct omap1_cam_dev *pcdev)
466 {
467         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
468
469         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
470         omap_stop_dma(pcdev->dma_ch);
471 }
472
473 static void disable_capture(struct omap1_cam_dev *pcdev)
474 {
475         u32 mode = CAM_READ_CACHE(pcdev, MODE);
476
477         CAM_WRITE(pcdev, MODE, mode & ~(IRQ_MASK | DMA));
478 }
479
480 static void omap1_videobuf_queue(struct videobuf_queue *vq,
481                                                 struct videobuf_buffer *vb)
482 {
483         struct soc_camera_device *icd = vq->priv_data;
484         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
485         struct omap1_cam_dev *pcdev = ici->priv;
486         struct omap1_cam_buf *buf;
487         u32 mode;
488
489         list_add_tail(&vb->queue, &pcdev->capture);
490         vb->state = VIDEOBUF_QUEUED;
491
492         if (pcdev->active) {
493                 /*
494                  * Capture in progress, so don't touch pcdev->ready even if
495                  * empty. Since the transfer of the DMA programming register set
496                  * content to the DMA working register set is done automatically
497                  * by the DMA hardware, this can pretty well happen while we
498                  * are keeping the lock here. Leave fetching it from the queue
499                  * to be done when a next DMA interrupt occures instead.
500                  */
501                 return;
502         }
503
504         WARN_ON(pcdev->ready);
505
506         buf = prepare_next_vb(pcdev);
507         if (WARN_ON(!buf))
508                 return;
509
510         pcdev->active = buf;
511         pcdev->ready = NULL;
512
513         dev_dbg(icd->parent,
514                 "%s: capture not active, setup FIFO, start DMA\n", __func__);
515         mode = CAM_READ_CACHE(pcdev, MODE) & ~THRESHOLD_MASK;
516         mode |= THRESHOLD_LEVEL(pcdev->vb_mode) << THRESHOLD_SHIFT;
517         CAM_WRITE(pcdev, MODE, mode | EN_FIFO_FULL | DMA);
518
519         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
520                 /*
521                  * In SG mode, the above prepare_next_vb() didn't actually
522                  * put anything into the DMA programming register set,
523                  * so we have to do it now, before activating DMA.
524                  */
525                 try_next_sgbuf(pcdev->dma_ch, buf);
526         }
527
528         start_capture(pcdev);
529 }
530
531 static void omap1_videobuf_release(struct videobuf_queue *vq,
532                                  struct videobuf_buffer *vb)
533 {
534         struct omap1_cam_buf *buf =
535                         container_of(vb, struct omap1_cam_buf, vb);
536         struct soc_camera_device *icd = vq->priv_data;
537         struct device *dev = icd->parent;
538         struct soc_camera_host *ici = to_soc_camera_host(dev);
539         struct omap1_cam_dev *pcdev = ici->priv;
540
541         switch (vb->state) {
542         case VIDEOBUF_DONE:
543                 dev_dbg(dev, "%s (done)\n", __func__);
544                 break;
545         case VIDEOBUF_ACTIVE:
546                 dev_dbg(dev, "%s (active)\n", __func__);
547                 break;
548         case VIDEOBUF_QUEUED:
549                 dev_dbg(dev, "%s (queued)\n", __func__);
550                 break;
551         case VIDEOBUF_PREPARED:
552                 dev_dbg(dev, "%s (prepared)\n", __func__);
553                 break;
554         default:
555                 dev_dbg(dev, "%s (unknown %d)\n", __func__, vb->state);
556                 break;
557         }
558
559         free_buffer(vq, buf, pcdev->vb_mode);
560 }
561
562 static void videobuf_done(struct omap1_cam_dev *pcdev,
563                 enum videobuf_state result)
564 {
565         struct omap1_cam_buf *buf = pcdev->active;
566         struct videobuf_buffer *vb;
567         struct device *dev = pcdev->icd->parent;
568
569         if (WARN_ON(!buf)) {
570                 suspend_capture(pcdev);
571                 disable_capture(pcdev);
572                 return;
573         }
574
575         if (result == VIDEOBUF_ERROR)
576                 suspend_capture(pcdev);
577
578         vb = &buf->vb;
579         if (waitqueue_active(&vb->done)) {
580                 if (!pcdev->ready && result != VIDEOBUF_ERROR) {
581                         /*
582                          * No next buffer has been entered into the DMA
583                          * programming register set on time (could be done only
584                          * while the previous DMA interurpt was processed, not
585                          * later), so the last DMA block, be it a whole buffer
586                          * if in CONTIG or its last sgbuf if in SG mode, is
587                          * about to be reused by the just autoreinitialized DMA
588                          * engine, and overwritten with next frame data. Best we
589                          * can do is stopping the capture as soon as possible,
590                          * hopefully before the next frame start.
591                          */
592                         suspend_capture(pcdev);
593                 }
594                 vb->state = result;
595                 do_gettimeofday(&vb->ts);
596                 if (result != VIDEOBUF_ERROR)
597                         vb->field_count++;
598                 wake_up(&vb->done);
599
600                 /* shift in next buffer */
601                 buf = pcdev->ready;
602                 pcdev->active = buf;
603                 pcdev->ready = NULL;
604
605                 if (!buf) {
606                         /*
607                          * No next buffer was ready on time (see above), so
608                          * indicate error condition to force capture restart or
609                          * stop, depending on next buffer already queued or not.
610                          */
611                         result = VIDEOBUF_ERROR;
612                         prepare_next_vb(pcdev);
613
614                         buf = pcdev->ready;
615                         pcdev->active = buf;
616                         pcdev->ready = NULL;
617                 }
618         } else if (pcdev->ready) {
619                 /*
620                  * In both CONTIG and SG mode, the DMA engine has possibly
621                  * been already autoreinitialized with the preprogrammed
622                  * pcdev->ready buffer.  We can either accept this fact
623                  * and just swap the buffers, or provoke an error condition
624                  * and restart capture.  The former seems less intrusive.
625                  */
626                 dev_dbg(dev, "%s: nobody waiting on videobuf, swap with next\n",
627                                 __func__);
628                 pcdev->active = pcdev->ready;
629
630                 if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
631                         /*
632                          * In SG mode, we have to make sure that the buffer we
633                          * are putting back into the pcdev->ready is marked
634                          * fresh.
635                          */
636                         buf->sgbuf = NULL;
637                 }
638                 pcdev->ready = buf;
639
640                 buf = pcdev->active;
641         } else {
642                 /*
643                  * No next buffer has been entered into
644                  * the DMA programming register set on time.
645                  */
646                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
647                         /*
648                          * In CONTIG mode, the DMA engine has already been
649                          * reinitialized with the current buffer. Best we can do
650                          * is not touching it.
651                          */
652                         dev_dbg(dev,
653                                 "%s: nobody waiting on videobuf, reuse it\n",
654                                 __func__);
655                 } else {
656                         /*
657                          * In SG mode, the DMA engine has just been
658                          * autoreinitialized with the last sgbuf from the
659                          * current list. Restart capture in order to transfer
660                          * next frame start into the first sgbuf, not the last
661                          * one.
662                          */
663                         if (result != VIDEOBUF_ERROR) {
664                                 suspend_capture(pcdev);
665                                 result = VIDEOBUF_ERROR;
666                         }
667                 }
668         }
669
670         if (!buf) {
671                 dev_dbg(dev, "%s: no more videobufs, stop capture\n", __func__);
672                 disable_capture(pcdev);
673                 return;
674         }
675
676         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
677                 /*
678                  * In CONTIG mode, the current buffer parameters had already
679                  * been entered into the DMA programming register set while the
680                  * buffer was fetched with prepare_next_vb(), they may have also
681                  * been transferred into the runtime set and already active if
682                  * the DMA still running.
683                  */
684         } else {
685                 /* In SG mode, extra steps are required */
686                 if (result == VIDEOBUF_ERROR)
687                         /* make sure we (re)use sglist from start on error */
688                         buf->sgbuf = NULL;
689
690                 /*
691                  * In any case, enter the next sgbuf parameters into the DMA
692                  * programming register set.  They will be used either during
693                  * nearest DMA autoreinitialization or, in case of an error,
694                  * on DMA startup below.
695                  */
696                 try_next_sgbuf(pcdev->dma_ch, buf);
697         }
698
699         if (result == VIDEOBUF_ERROR) {
700                 dev_dbg(dev, "%s: videobuf error; reset FIFO, restart DMA\n",
701                                 __func__);
702                 start_capture(pcdev);
703                 /*
704                  * In SG mode, the above also resulted in the next sgbuf
705                  * parameters being entered into the DMA programming register
706                  * set, making them ready for next DMA autoreinitialization.
707                  */
708         }
709
710         /*
711          * Finally, try fetching next buffer.
712          * In CONTIG mode, it will also enter it into the DMA programming
713          * register set, making it ready for next DMA autoreinitialization.
714          */
715         prepare_next_vb(pcdev);
716 }
717
718 static void dma_isr(int channel, unsigned short status, void *data)
719 {
720         struct omap1_cam_dev *pcdev = data;
721         struct omap1_cam_buf *buf = pcdev->active;
722         unsigned long flags;
723
724         spin_lock_irqsave(&pcdev->lock, flags);
725
726         if (WARN_ON(!buf)) {
727                 suspend_capture(pcdev);
728                 disable_capture(pcdev);
729                 goto out;
730         }
731
732         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
733                 /*
734                  * In CONTIG mode, assume we have just managed to collect the
735                  * whole frame, hopefully before our end of frame watchdog is
736                  * triggered. Then, all we have to do is disabling the watchdog
737                  * for this frame, and calling videobuf_done() with success
738                  * indicated.
739                  */
740                 CAM_WRITE(pcdev, MODE,
741                                 CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN);
742                 videobuf_done(pcdev, VIDEOBUF_DONE);
743         } else {
744                 /*
745                  * In SG mode, we have to process every sgbuf from the current
746                  * sglist, one after another.
747                  */
748                 if (buf->sgbuf) {
749                         /*
750                          * Current sglist not completed yet, try fetching next
751                          * sgbuf, hopefully putting it into the DMA programming
752                          * register set, making it ready for next DMA
753                          * autoreinitialization.
754                          */
755                         try_next_sgbuf(pcdev->dma_ch, buf);
756                         if (buf->sgbuf)
757                                 goto out;
758
759                         /*
760                          * No more sgbufs left in the current sglist. This
761                          * doesn't mean that the whole videobuffer is already
762                          * complete, but only that the last sgbuf from the
763                          * current sglist is about to be filled. It will be
764                          * ready on next DMA interrupt, signalled with the
765                          * buf->sgbuf set back to NULL.
766                          */
767                         if (buf->result != VIDEOBUF_ERROR) {
768                                 /*
769                                  * Video frame collected without errors so far,
770                                  * we can prepare for collecting a next one
771                                  * as soon as DMA gets autoreinitialized
772                                  * after the current (last) sgbuf is completed.
773                                  */
774                                 buf = prepare_next_vb(pcdev);
775                                 if (!buf)
776                                         goto out;
777
778                                 try_next_sgbuf(pcdev->dma_ch, buf);
779                                 goto out;
780                         }
781                 }
782                 /* end of videobuf */
783                 videobuf_done(pcdev, buf->result);
784         }
785
786 out:
787         spin_unlock_irqrestore(&pcdev->lock, flags);
788 }
789
790 static irqreturn_t cam_isr(int irq, void *data)
791 {
792         struct omap1_cam_dev *pcdev = data;
793         struct device *dev = pcdev->icd->parent;
794         struct omap1_cam_buf *buf = pcdev->active;
795         u32 it_status;
796         unsigned long flags;
797
798         it_status = CAM_READ(pcdev, IT_STATUS);
799         if (!it_status)
800                 return IRQ_NONE;
801
802         spin_lock_irqsave(&pcdev->lock, flags);
803
804         if (WARN_ON(!buf)) {
805                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
806                          __func__, it_status);
807                 suspend_capture(pcdev);
808                 disable_capture(pcdev);
809                 goto out;
810         }
811
812         if (unlikely(it_status & FIFO_FULL)) {
813                 dev_warn(dev, "%s: FIFO overflow\n", __func__);
814
815         } else if (it_status & V_DOWN) {
816                 /* end of video frame watchdog */
817                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
818                         /*
819                          * In CONTIG mode, the watchdog is disabled with
820                          * successful DMA end of block interrupt, and reenabled
821                          * on next frame start. If we get here, there is nothing
822                          * to check, we must be out of sync.
823                          */
824                 } else {
825                         if (buf->sgcount == 2) {
826                                 /*
827                                  * If exactly 2 sgbufs from the next sglist have
828                                  * been programmed into the DMA engine (the
829                                  * first one already transferred into the DMA
830                                  * runtime register set, the second one still
831                                  * in the programming set), then we are in sync.
832                                  */
833                                 goto out;
834                         }
835                 }
836                 dev_notice(dev, "%s: unexpected end of video frame\n",
837                                 __func__);
838
839         } else if (it_status & V_UP) {
840                 u32 mode;
841
842                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
843                         /*
844                          * In CONTIG mode, we need this interrupt every frame
845                          * in oredr to reenable our end of frame watchdog.
846                          */
847                         mode = CAM_READ_CACHE(pcdev, MODE);
848                 } else {
849                         /*
850                          * In SG mode, the below enabled end of frame watchdog
851                          * is kept on permanently, so we can turn this one shot
852                          * setup off.
853                          */
854                         mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_UP;
855                 }
856
857                 if (!(mode & EN_V_DOWN)) {
858                         /* (re)enable end of frame watchdog interrupt */
859                         mode |= EN_V_DOWN;
860                 }
861                 CAM_WRITE(pcdev, MODE, mode);
862                 goto out;
863
864         } else {
865                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
866                                 __func__, it_status);
867                 goto out;
868         }
869
870         videobuf_done(pcdev, VIDEOBUF_ERROR);
871 out:
872         spin_unlock_irqrestore(&pcdev->lock, flags);
873         return IRQ_HANDLED;
874 }
875
876 static struct videobuf_queue_ops omap1_videobuf_ops = {
877         .buf_setup      = omap1_videobuf_setup,
878         .buf_prepare    = omap1_videobuf_prepare,
879         .buf_queue      = omap1_videobuf_queue,
880         .buf_release    = omap1_videobuf_release,
881 };
882
883
884 /*
885  * SOC Camera host operations
886  */
887
888 static void sensor_reset(struct omap1_cam_dev *pcdev, bool reset)
889 {
890         /* apply/release camera sensor reset if requested by platform data */
891         if (pcdev->pflags & OMAP1_CAMERA_RST_HIGH)
892                 CAM_WRITE(pcdev, GPIO, reset);
893         else if (pcdev->pflags & OMAP1_CAMERA_RST_LOW)
894                 CAM_WRITE(pcdev, GPIO, !reset);
895 }
896
897 /*
898  * The following two functions absolutely depend on the fact, that
899  * there can be only one camera on OMAP1 camera sensor interface
900  */
901 static int omap1_cam_add_device(struct soc_camera_device *icd)
902 {
903         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
904         struct omap1_cam_dev *pcdev = ici->priv;
905         u32 ctrlclock;
906
907         if (pcdev->icd)
908                 return -EBUSY;
909
910         clk_enable(pcdev->clk);
911
912         /* setup sensor clock */
913         ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
914         ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
915         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
916
917         ctrlclock &= ~FOSCMOD_MASK;
918         switch (pcdev->camexclk) {
919         case 6000000:
920                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
921                 break;
922         case 8000000:
923                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
924                 break;
925         case 9600000:
926                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
927                 break;
928         case 12000000:
929                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
930                 break;
931         case 24000000:
932                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
933         default:
934                 break;
935         }
936         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
937
938         /* enable internal clock */
939         ctrlclock |= MCLK_EN;
940         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
941
942         sensor_reset(pcdev, false);
943
944         pcdev->icd = icd;
945
946         dev_dbg(icd->parent, "OMAP1 Camera driver attached to camera %d\n",
947                         icd->devnum);
948         return 0;
949 }
950
951 static void omap1_cam_remove_device(struct soc_camera_device *icd)
952 {
953         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
954         struct omap1_cam_dev *pcdev = ici->priv;
955         u32 ctrlclock;
956
957         BUG_ON(icd != pcdev->icd);
958
959         suspend_capture(pcdev);
960         disable_capture(pcdev);
961
962         sensor_reset(pcdev, true);
963
964         /* disable and release system clocks */
965         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
966         ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
967         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
968
969         ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
970         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
971         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
972
973         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
974
975         clk_disable(pcdev->clk);
976
977         pcdev->icd = NULL;
978
979         dev_dbg(icd->parent,
980                 "OMAP1 Camera driver detached from camera %d\n", icd->devnum);
981 }
982
983 /* Duplicate standard formats based on host capability of byte swapping */
984 static const struct soc_mbus_lookup omap1_cam_formats[] = {
985 {
986         .code = V4L2_MBUS_FMT_UYVY8_2X8,
987         .fmt = {
988                 .fourcc                 = V4L2_PIX_FMT_YUYV,
989                 .name                   = "YUYV",
990                 .bits_per_sample        = 8,
991                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
992                 .order                  = SOC_MBUS_ORDER_BE,
993                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
994         },
995 }, {
996         .code = V4L2_MBUS_FMT_VYUY8_2X8,
997         .fmt = {
998                 .fourcc                 = V4L2_PIX_FMT_YVYU,
999                 .name                   = "YVYU",
1000                 .bits_per_sample        = 8,
1001                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1002                 .order                  = SOC_MBUS_ORDER_BE,
1003                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1004         },
1005 }, {
1006         .code = V4L2_MBUS_FMT_YUYV8_2X8,
1007         .fmt = {
1008                 .fourcc                 = V4L2_PIX_FMT_UYVY,
1009                 .name                   = "UYVY",
1010                 .bits_per_sample        = 8,
1011                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1012                 .order                  = SOC_MBUS_ORDER_BE,
1013                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1014         },
1015 }, {
1016         .code = V4L2_MBUS_FMT_YVYU8_2X8,
1017         .fmt = {
1018                 .fourcc                 = V4L2_PIX_FMT_VYUY,
1019                 .name                   = "VYUY",
1020                 .bits_per_sample        = 8,
1021                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1022                 .order                  = SOC_MBUS_ORDER_BE,
1023                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1024         },
1025 }, {
1026         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
1027         .fmt = {
1028                 .fourcc                 = V4L2_PIX_FMT_RGB555,
1029                 .name                   = "RGB555",
1030                 .bits_per_sample        = 8,
1031                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1032                 .order                  = SOC_MBUS_ORDER_BE,
1033                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1034         },
1035 }, {
1036         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
1037         .fmt = {
1038                 .fourcc                 = V4L2_PIX_FMT_RGB555X,
1039                 .name                   = "RGB555X",
1040                 .bits_per_sample        = 8,
1041                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1042                 .order                  = SOC_MBUS_ORDER_BE,
1043                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1044         },
1045 }, {
1046         .code = V4L2_MBUS_FMT_RGB565_2X8_BE,
1047         .fmt = {
1048                 .fourcc                 = V4L2_PIX_FMT_RGB565,
1049                 .name                   = "RGB565",
1050                 .bits_per_sample        = 8,
1051                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1052                 .order                  = SOC_MBUS_ORDER_BE,
1053                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
1054         },
1055 }, {
1056         .code = V4L2_MBUS_FMT_RGB565_2X8_LE,
1057         .fmt = {
1058                 .fourcc                 = V4L2_PIX_FMT_RGB565X,
1059                 .name                   = "RGB565X",
1060                 .bits_per_sample        = 8,
1061                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1062                 .order                  = SOC_MBUS_ORDER_BE,
1063                 .layout                 = SOC_MBUS_LAYOUT_PACKED,
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 {
1440         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1441         struct device *dev = icd->parent;
1442         struct soc_camera_host *ici = to_soc_camera_host(dev);
1443         struct omap1_cam_dev *pcdev = ici->priv;
1444         u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
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 module_platform_driver(omap1_cam_driver);
1716
1717 module_param(sg_mode, bool, 0644);
1718 MODULE_PARM_DESC(sg_mode, "videobuf mode, 0: dma-contig (default), 1: dma-sg");
1719
1720 MODULE_DESCRIPTION("OMAP1 Camera Interface driver");
1721 MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
1722 MODULE_LICENSE("GPL v2");
1723 MODULE_VERSION(DRIVER_VERSION);
1724 MODULE_ALIAS("platform:" DRIVER_NAME);