media: tegra: add supporting for RAW8/RAW10 input and output
[linux-2.6.git] / drivers / media / video / tegra_v4l2_camera.c
1 /*
2  * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
3  *
4  * This program is free software; you can redistribute it and/or modify it
5  * under the terms and conditions of the GNU General Public License,
6  * version 2, as published by the Free Software Foundation.
7  *
8  * This program is distributed in the hope it will be useful, but WITHOUT
9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
11  * more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
15  */
16
17 #include <linux/kernel.h>
18 #include <linux/module.h>
19 #include <linux/platform_device.h>
20 #include <linux/clk.h>
21 #include <linux/pm_runtime.h>
22 #include <linux/nvhost.h>
23
24 #include <mach/iomap.h>
25
26 #include <media/soc_camera.h>
27 #include <media/soc_mediabus.h>
28 #include <media/videobuf2-dma-nvmap.h>
29 #include <media/tegra_v4l2_camera.h>
30
31 #include "dev.h"
32 #include "bus_client.h"
33 #include "host1x/host1x_syncpt.h"
34
35 #define TEGRA_CAM_DRV_NAME "vi"
36 #define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
37
38 #define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
39 #define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
40
41 #define TEGRA_SYNCPT_RETRY_COUNT                        10
42
43 #define TEGRA_VIP_H_ACTIVE_START                        0x98
44 #define TEGRA_VIP_V_ACTIVE_START                        0x10
45
46 /* SYNCPTs 12-17 are reserved for VI. */
47 #define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
48 #define TEGRA_VI_SYNCPT_CSI                             NVSYNCPT_VI_ISP_3
49
50 /* Tegra CSI-MIPI registers. */
51 #define TEGRA_VI_OUT_1_INCR_SYNCPT                      0x0000
52 #define TEGRA_VI_OUT_1_INCR_SYNCPT_CNTRL                0x0004
53 #define TEGRA_VI_OUT_1_INCR_SYNCPT_ERROR                0x0008
54 #define TEGRA_VI_OUT_2_INCR_SYNCPT                      0x0020
55 #define TEGRA_VI_OUT_2_INCR_SYNCPT_CNTRL                0x0024
56 #define TEGRA_VI_OUT_2_INCR_SYNCPT_ERROR                0x0028
57 #define TEGRA_VI_MISC_INCR_SYNCPT                       0x0040
58 #define TEGRA_VI_MISC_INCR_SYNCPT_CNTRL                 0x0044
59 #define TEGRA_VI_MISC_INCR_SYNCPT_ERROR                 0x0048
60 #define TEGRA_VI_CONT_SYNCPT_OUT_1                      0x0060
61 #define TEGRA_VI_CONT_SYNCPT_OUT_2                      0x0064
62 #define TEGRA_VI_CONT_SYNCPT_VIP_VSYNC                  0x0068
63 #define TEGRA_VI_CONT_SYNCPT_VI2EPP                     0x006c
64 #define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START        0x0070
65 #define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END          0x0074
66 #define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START        0x0078
67 #define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END          0x007c
68 #define TEGRA_VI_CTXSW                                  0x0080
69 #define TEGRA_VI_INTSTATUS                              0x0084
70 #define TEGRA_VI_VI_INPUT_CONTROL                       0x0088
71 #define TEGRA_VI_VI_CORE_CONTROL                        0x008c
72 #define TEGRA_VI_VI_FIRST_OUTPUT_CONTROL                0x0090
73 #define TEGRA_VI_VI_SECOND_OUTPUT_CONTROL               0x0094
74 #define TEGRA_VI_HOST_INPUT_FRAME_SIZE                  0x0098
75 #define TEGRA_VI_HOST_H_ACTIVE                          0x009c
76 #define TEGRA_VI_HOST_V_ACTIVE                          0x00a0
77 #define TEGRA_VI_VIP_H_ACTIVE                           0x00a4
78 #define TEGRA_VI_VIP_V_ACTIVE                           0x00a8
79 #define TEGRA_VI_VI_PEER_CONTROL                        0x00ac
80 #define TEGRA_VI_VI_DMA_SELECT                          0x00b0
81 #define TEGRA_VI_HOST_DMA_WRITE_BUFFER                  0x00b4
82 #define TEGRA_VI_HOST_DMA_BASE_ADDRESS                  0x00b8
83 #define TEGRA_VI_HOST_DMA_WRITE_BUFFER_STATUS           0x00bc
84 #define TEGRA_VI_HOST_DMA_WRITE_PEND_BUFCOUNT           0x00c0
85 #define TEGRA_VI_VB0_START_ADDRESS_FIRST                0x00c4
86 #define TEGRA_VI_VB0_BASE_ADDRESS_FIRST                 0x00c8
87 #define TEGRA_VI_VB0_START_ADDRESS_U                    0x00cc
88 #define TEGRA_VI_VB0_BASE_ADDRESS_U                     0x00d0
89 #define TEGRA_VI_VB0_START_ADDRESS_V                    0x00d4
90 #define TEGRA_VI_VB0_BASE_ADDRESS_V                     0x00d8
91 #define TEGRA_VI_VB_SCRATCH_ADDRESS_UV                  0x00dc
92 #define TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE                0x00e0
93 #define TEGRA_VI_VB0_COUNT_FIRST                        0x00e4
94 #define TEGRA_VI_VB0_SIZE_FIRST                         0x00e8
95 #define TEGRA_VI_VB0_BUFFER_STRIDE_FIRST                0x00ec
96 #define TEGRA_VI_VB0_START_ADDRESS_SECOND               0x00f0
97 #define TEGRA_VI_VB0_BASE_ADDRESS_SECOND                0x00f4
98 #define TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE               0x00f8
99 #define TEGRA_VI_VB0_COUNT_SECOND                       0x00fc
100 #define TEGRA_VI_VB0_SIZE_SECOND                        0x0100
101 #define TEGRA_VI_VB0_BUFFER_STRIDE_SECOND               0x0104
102 #define TEGRA_VI_H_LPF_CONTROL                          0x0108
103 #define TEGRA_VI_H_DOWNSCALE_CONTROL                    0x010c
104 #define TEGRA_VI_V_DOWNSCALE_CONTROL                    0x0110
105 #define TEGRA_VI_CSC_Y                                  0x0114
106 #define TEGRA_VI_CSC_UV_R                               0x0118
107 #define TEGRA_VI_CSC_UV_G                               0x011c
108 #define TEGRA_VI_CSC_UV_B                               0x0120
109 #define TEGRA_VI_CSC_ALPHA                              0x0124
110 #define TEGRA_VI_HOST_VSYNC                             0x0128
111 #define TEGRA_VI_COMMAND                                0x012c
112 #define TEGRA_VI_HOST_FIFO_STATUS                       0x0130
113 #define TEGRA_VI_INTERRUPT_MASK                         0x0134
114 #define TEGRA_VI_INTERRUPT_TYPE_SELECT                  0x0138
115 #define TEGRA_VI_INTERRUPT_POLARITY_SELECT              0x013c
116 #define TEGRA_VI_INTERRUPT_STATUS                       0x0140
117 #define TEGRA_VI_VIP_INPUT_STATUS                       0x0144
118 #define TEGRA_VI_VIDEO_BUFFER_STATUS                    0x0148
119 #define TEGRA_VI_SYNC_OUTPUT                            0x014c
120 #define TEGRA_VI_VVS_OUTPUT_DELAY                       0x0150
121 #define TEGRA_VI_PWM_CONTROL                            0x0154
122 #define TEGRA_VI_PWM_SELECT_PULSE_A                     0x0158
123 #define TEGRA_VI_PWM_SELECT_PULSE_B                     0x015c
124 #define TEGRA_VI_PWM_SELECT_PULSE_C                     0x0160
125 #define TEGRA_VI_PWM_SELECT_PULSE_D                     0x0164
126 #define TEGRA_VI_VI_DATA_INPUT_CONTROL                  0x0168
127 #define TEGRA_VI_PIN_INPUT_ENABLE                       0x016c
128 #define TEGRA_VI_PIN_OUTPUT_ENABLE                      0x0170
129 #define TEGRA_VI_PIN_INVERSION                          0x0174
130 #define TEGRA_VI_PIN_INPUT_DATA                         0x0178
131 #define TEGRA_VI_PIN_OUTPUT_DATA                        0x017c
132 #define TEGRA_VI_PIN_OUTPUT_SELECT                      0x0180
133 #define TEGRA_VI_RAISE_VIP_BUFFER_FIRST_OUTPUT          0x0184
134 #define TEGRA_VI_RAISE_VIP_FRAME_FIRST_OUTPUT           0x0188
135 #define TEGRA_VI_RAISE_VIP_BUFFER_SECOND_OUTPUT         0x018c
136 #define TEGRA_VI_RAISE_VIP_FRAME_SECOND_OUTPUT          0x0190
137 #define TEGRA_VI_RAISE_HOST_FIRST_OUTPUT                0x0194
138 #define TEGRA_VI_RAISE_HOST_SECOND_OUTPUT               0x0198
139 #define TEGRA_VI_RAISE_EPP                              0x019c
140 #define TEGRA_VI_CAMERA_CONTROL                         0x01a0
141 #define TEGRA_VI_VI_ENABLE                              0x01a4
142 #define TEGRA_VI_VI_ENABLE_2                            0x01a8
143 #define TEGRA_VI_VI_RAISE                               0x01ac
144 #define TEGRA_VI_Y_FIFO_WRITE                           0x01b0
145 #define TEGRA_VI_U_FIFO_WRITE                           0x01b4
146 #define TEGRA_VI_V_FIFO_WRITE                           0x01b8
147 #define TEGRA_VI_VI_MCCIF_FIFOCTRL                      0x01bc
148 #define TEGRA_VI_TIMEOUT_WCOAL_VI                       0x01c0
149 #define TEGRA_VI_MCCIF_VIRUV_HP                         0x01c4
150 #define TEGRA_VI_MCCIF_VIWSB_HP                         0x01c8
151 #define TEGRA_VI_MCCIF_VIWU_HP                          0x01cc
152 #define TEGRA_VI_MCCIF_VIWV_HP                          0x01d0
153 #define TEGRA_VI_MCCIF_VIWY_HP                          0x01d4
154 #define TEGRA_VI_CSI_PPA_RAISE_FRAME_START              0x01d8
155 #define TEGRA_VI_CSI_PPA_RAISE_FRAME_END                0x01dc
156 #define TEGRA_VI_CSI_PPB_RAISE_FRAME_START              0x01e0
157 #define TEGRA_VI_CSI_PBB_RAISE_FRAME_END                0x01e4
158 #define TEGRA_VI_CSI_PPA_H_ACTIVE                       0x01e8
159 #define TEGRA_VI_CSI_PPA_V_ACTIVE                       0x01ec
160 #define TEGRA_VI_CSI_PPB_H_ACTIVE                       0x01f0
161 #define TEGRA_VI_CSI_PPB_V_ACTIVE                       0x01f4
162 #define TEGRA_VI_ISP_H_ACTIVE                           0x01f8
163 #define TEGRA_VI_ISP_V_ACTIVE                           0x01fc
164 #define TEGRA_VI_STREAM_1_RESOURCE_DEFINE               0x0200
165 #define TEGRA_VI_STREAM_2_RESOURCE_DEFINE               0x0204
166 #define TEGRA_VI_RAISE_STREAM_1_DONE                    0x0208
167 #define TEGRA_VI_RAISE_STREAM_2_DONE                    0x020c
168 #define TEGRA_VI_TS_MODE                                0x0210
169 #define TEGRA_VI_TS_CONTROL                             0x0214
170 #define TEGRA_VI_TS_PACKET_COUNT                        0x0218
171 #define TEGRA_VI_TS_ERROR_COUNT                         0x021c
172 #define TEGRA_VI_TS_CPU_FLOW_CTL                        0x0220
173 #define TEGRA_VI_VB0_CHROMA_BUFFER_STRIDE_FIRST         0x0224
174 #define TEGRA_VI_VB0_CHROMA_LINE_STRIDE_FIRST           0x0228
175 #define TEGRA_VI_EPP_LINES_PER_BUFFER                   0x022c
176 #define TEGRA_VI_BUFFER_RELEASE_OUTPUT1                 0x0230
177 #define TEGRA_VI_BUFFER_RELEASE_OUTPUT2                 0x0234
178 #define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT1     0x0238
179 #define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT2     0x023c
180 #define TEGRA_VI_TERMINATE_BW_FIRST                     0x0240
181 #define TEGRA_VI_TERMINATE_BW_SECOND                    0x0244
182 #define TEGRA_VI_VB0_FIRST_BUFFER_ADDR_MODE             0x0248
183 #define TEGRA_VI_VB0_SECOND_BUFFER_ADDR_MODE            0x024c
184 #define TEGRA_VI_RESERVE_0                              0x0250
185 #define TEGRA_VI_RESERVE_1                              0x0254
186 #define TEGRA_VI_RESERVE_2                              0x0258
187 #define TEGRA_VI_RESERVE_3                              0x025c
188 #define TEGRA_VI_RESERVE_4                              0x0260
189 #define TEGRA_VI_MCCIF_VIRUV_HYST                       0x0264
190 #define TEGRA_VI_MCCIF_VIWSB_HYST                       0x0268
191 #define TEGRA_VI_MCCIF_VIWU_HYST                        0x026c
192 #define TEGRA_VI_MCCIF_VIWV_HYST                        0x0270
193 #define TEGRA_VI_MCCIF_VIWY_HYST                        0x0274
194
195 #define TEGRA_CSI_VI_INPUT_STREAM_CONTROL               0x0800
196 #define TEGRA_CSI_HOST_INPUT_STREAM_CONTROL             0x0808
197 #define TEGRA_CSI_INPUT_STREAM_A_CONTROL                0x0810
198 #define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0               0x0818
199 #define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1               0x081c
200 #define TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT             0x0820
201 #define TEGRA_CSI_PIXEL_STREAM_A_GAP                    0x0824
202 #define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND              0x0828
203 #define TEGRA_CSI_INPUT_STREAM_B_CONTROL                0x083c
204 #define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0               0x0844
205 #define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1               0x0848
206 #define TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT             0x084c
207 #define TEGRA_CSI_PIXEL_STREAM_B_GAP                    0x0850
208 #define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND              0x0854
209 #define TEGRA_CSI_PHY_CIL_COMMAND                       0x0868
210 #define TEGRA_CSI_PHY_CILA_CONTROL0                     0x086c
211 #define TEGRA_CSI_PHY_CILB_CONTROL0                     0x0870
212 #define TEGRA_CSI_CSI_PIXEL_PARSER_STATUS               0x0878
213 #define TEGRA_CSI_CSI_CIL_STATUS                        0x087c
214 #define TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK       0x0880
215 #define TEGRA_CSI_CSI_CIL_INTERRUPT_MASK                0x0884
216 #define TEGRA_CSI_CSI_READONLY_STATUS                   0x0888
217 #define TEGRA_CSI_ESCAPE_MODE_COMMAND                   0x088c
218 #define TEGRA_CSI_ESCAPE_MODE_DATA                      0x0890
219 #define TEGRA_CSI_CILA_PAD_CONFIG0                      0x0894
220 #define TEGRA_CSI_CILA_PAD_CONFIG1                      0x0898
221 #define TEGRA_CSI_CILB_PAD_CONFIG0                      0x089c
222 #define TEGRA_CSI_CILB_PAD_CONFIG1                      0x08a0
223 #define TEGRA_CSI_CIL_PAD_CONFIG                        0x08a4
224 #define TEGRA_CSI_CILA_MIPI_CAL_CONFIG                  0x08a8
225 #define TEGRA_CSI_CILB_MIPI_CAL_CONFIG                  0x08ac
226 #define TEGRA_CSI_CIL_MIPI_CAL_STATUS                   0x08b0
227 #define TEGRA_CSI_CLKEN_OVERRIDE                        0x08b4
228 #define TEGRA_CSI_DEBUG_CONTROL                         0x08b8
229 #define TEGRA_CSI_DEBUG_COUNTER_0                       0x08bc
230 #define TEGRA_CSI_DEBUG_COUNTER_1                       0x08c0
231 #define TEGRA_CSI_DEBUG_COUNTER_2                       0x08c4
232 #define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME         0x08c8
233 #define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME         0x08cc
234 #define TEGRA_CSI_DSI_MIPI_CAL_CONFIG                   0x08d0
235
236 #define TC_VI_REG_RD(DEV, REG) readl(DEV->vi_base + REG)
237 #define TC_VI_REG_WT(DEV, REG, VAL) writel(VAL, DEV->vi_base + REG)
238
239 #define tegra_camera_port_is_valid(port) \
240         (((port) >= TEGRA_CAMERA_PORT_CSI_A) && \
241          ((port) <= TEGRA_CAMERA_PORT_VIP))
242
243 #define tegra_camera_port_is_csi(port) \
244         (((port) == TEGRA_CAMERA_PORT_CSI_A) || \
245          ((port) == TEGRA_CAMERA_PORT_CSI_B))
246
247 /*
248  * Structures
249  */
250
251 /* buffer for one video frame */
252 struct tegra_buffer {
253         struct vb2_buffer               vb; /* v4l buffer must be first */
254         struct list_head                queue;
255
256         /*
257          * Various buffer addresses shadowed so we don't have to recalculate
258          * per frame.  These are calculated during videobuf_prepare.
259          */
260         dma_addr_t                      buffer_addr;
261         dma_addr_t                      buffer_addr_u;
262         dma_addr_t                      buffer_addr_v;
263         dma_addr_t                      start_addr;
264         dma_addr_t                      start_addr_u;
265         dma_addr_t                      start_addr_v;
266 };
267
268 struct tegra_camera_dev {
269         struct soc_camera_host          ici;
270         struct soc_camera_device        *icd;
271         struct nvhost_device            *ndev;
272         struct tegra_camera_platform_data *pdata;
273
274         struct clk                      *clk_vi;
275         struct clk                      *clk_vi_sensor;
276         struct clk                      *clk_csi;
277         struct clk                      *clk_isp;
278         struct clk                      *clk_csus;
279
280         void __iomem                    *vi_base;
281         spinlock_t                      videobuf_queue_lock;
282         struct list_head                capture;
283         struct vb2_buffer               *active;
284         struct vb2_alloc_ctx            *alloc_ctx;
285         enum v4l2_field                 field;
286         int                             sequence;
287
288         struct work_struct              work;
289         struct mutex                    work_mutex;
290
291         u32                             syncpt_vi;
292         u32                             syncpt_csi;
293
294         /* Debug */
295         int num_frames;
296
297         int output_channel;
298 };
299
300 static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
301         {
302                 .fourcc                 = V4L2_PIX_FMT_UYVY,
303                 .name                   = "YUV422 (UYVY) packed",
304                 .bits_per_sample        = 16,
305                 .packing                = SOC_MBUS_PACKING_NONE,
306                 .order                  = SOC_MBUS_ORDER_LE,
307         },
308         {
309                 .fourcc                 = V4L2_PIX_FMT_VYUY,
310                 .name                   = "YUV422 (VYUY) packed",
311                 .bits_per_sample        = 16,
312                 .packing                = SOC_MBUS_PACKING_NONE,
313                 .order                  = SOC_MBUS_ORDER_LE,
314         },
315         {
316                 .fourcc                 = V4L2_PIX_FMT_YUYV,
317                 .name                   = "YUV422 (YUYV) packed",
318                 .bits_per_sample        = 16,
319                 .packing                = SOC_MBUS_PACKING_NONE,
320                 .order                  = SOC_MBUS_ORDER_LE,
321         },
322         {
323                 .fourcc                 = V4L2_PIX_FMT_YVYU,
324                 .name                   = "YUV422 (YVYU) packed",
325                 .bits_per_sample        = 16,
326                 .packing                = SOC_MBUS_PACKING_NONE,
327                 .order                  = SOC_MBUS_ORDER_LE,
328         },
329         {
330                 .fourcc                 = V4L2_PIX_FMT_YUV420,
331                 .name                   = "YUV420 (YU12) planar",
332                 .bits_per_sample        = 12,
333                 .packing                = SOC_MBUS_PACKING_NONE,
334                 .order                  = SOC_MBUS_ORDER_LE,
335         },
336         {
337                 .fourcc                 = V4L2_PIX_FMT_YVU420,
338                 .name                   = "YVU420 (YV12) planar",
339                 .bits_per_sample        = 12,
340                 .packing                = SOC_MBUS_PACKING_NONE,
341                 .order                  = SOC_MBUS_ORDER_LE,
342         },
343
344         /* For RAW8 and RAW10 output, we always output 16-bit (2 bytes). */
345         {
346                 .fourcc                 = V4L2_PIX_FMT_SGRBG8,
347                 .name                   = "Bayer 8 GRGR.. BGBG..",
348                 .bits_per_sample        = 16,
349                 .packing                = SOC_MBUS_PACKING_EXTEND16,
350                 .order                  = SOC_MBUS_ORDER_LE,
351         },
352         {
353                 .fourcc                 = V4L2_PIX_FMT_SGRBG10,
354                 .name                   = "Bayer 10 GRGR.. BGBG..",
355                 .bits_per_sample        = 16,
356                 .packing                = SOC_MBUS_PACKING_EXTEND16,
357                 .order                  = SOC_MBUS_ORDER_LE,
358         },
359
360 };
361
362 static struct tegra_buffer *to_tegra_vb(struct vb2_buffer *vb)
363 {
364         return container_of(vb, struct tegra_buffer, vb);
365 }
366
367 static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
368 {
369         pcdev->syncpt_csi =
370                 nvhost_syncpt_read_ext(pcdev->ndev,
371                                        TEGRA_VI_SYNCPT_CSI);
372
373         pcdev->syncpt_vi =
374                 nvhost_syncpt_read_ext(pcdev->ndev,
375                                        TEGRA_VI_SYNCPT_VI);
376 }
377
378 static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
379 {
380         nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
381                                    TEGRA_VI_SYNCPT_CSI);
382
383         nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
384                                    TEGRA_VI_SYNCPT_VI);
385 }
386
387 static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
388 {
389         /* CSI A */
390         TC_VI_REG_WT(pcdev, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
391         TC_VI_REG_WT(pcdev, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
392         TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL, 0x00000000);
393         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x00000000);
394         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x00000000);
395         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT, 0x00000000);
396         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00000000);
397         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x00000000);
398         TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL, 0x00000000);
399         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x00000000);
400         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x00000000);
401         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT, 0x00000000);
402         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00000000);
403         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x00000000);
404
405         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00000000);
406         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0, 0x00000000);
407         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0, 0x00000000);
408         TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_STATUS, 0x0);
409         TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_STATUS, 0x0);
410         TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
411         TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_INTERRUPT_MASK, 0x0);
412         TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_READONLY_STATUS, 0x0);
413         TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_COMMAND, 0x0);
414         TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_DATA, 0x0);
415
416
417         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG0, 0x00000000);
418         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG1, 0x00000000);
419         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG0, 0x00000000);
420         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG1, 0x00000000);
421         TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
422         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
423         TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
424         TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
425         TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
426
427         TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_CONTROL, 0x0);
428         TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_0, 0x0);
429         TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_1, 0x0);
430         TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_2, 0x0);
431         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME, 0x0);
432         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME, 0x0);
433 }
434
435 static u32 tegra_camera_header_for_wrong_fmt(struct tegra_camera_dev *pcdev)
436 {
437         struct soc_camera_device *icd = pcdev->icd;
438         const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
439         enum v4l2_mbus_pixelcode input_code = current_fmt->code;
440         u32 hdr;
441
442         switch (input_code) {
443         case V4L2_MBUS_FMT_UYVY8_2X8:
444         case V4L2_MBUS_FMT_VYUY8_2X8:
445         case V4L2_MBUS_FMT_YUYV8_2X8:
446         case V4L2_MBUS_FMT_YVYU8_2X8:
447                 hdr = 30;
448                 break;
449         case V4L2_MBUS_FMT_SGRBG8_1X8:
450                 hdr = 42;
451                 break;
452         case V4L2_MBUS_FMT_SGRBG10_1X10:
453                 hdr = 43;
454                 break;
455         default:
456                 BUG_ON(1);
457         }
458
459         return hdr;
460 }
461
462 static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
463                                              u32 input_control)
464 {
465         struct soc_camera_device *icd = pcdev->icd;
466         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
467                         icd->current_fmt->host_fmt);
468         u32 hdr = tegra_camera_header_for_wrong_fmt(pcdev);
469
470         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
471
472         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL, input_control);
473
474         TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000004);
475         TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000004);
476
477         /* CSI-A H_ACTIVE and V_ACTIVE */
478         TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_H_ACTIVE,
479                      (icd->user_width << 16));
480         TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_V_ACTIVE,
481                      (icd->user_height << 16));
482
483         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1,
484                 0x1); /* Frame # for top field detect for interlaced */
485
486         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT,
487                 bytes_per_line);
488         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00140000);
489
490         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME,
491                 (icd->user_height << 16) |
492                 (0x100 << 4) | /* Wait 0x100 vi clks for timeout */
493                 0x1); /* Enable line timeout */
494
495         /* pad 0s enabled, virtual channel ID 00 */
496         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0,
497                 (0x1 << 16) | /* Output 1 pixel per clock */
498                 (hdr << 8) | /* If hdr shows wrong fmt, use right value */
499                 (0x1 << 7) | /* Check header CRC */
500                 (0x1 << 6) | /* Use word count field in the header */
501                 (0x1 << 5) | /* Look at data identifier byte in hdr */
502                 (0x1 << 4));  /* Expect packet header */
503
504         TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
505                      (0x3f << 16) | /* Skip packet threshold */
506                      (pcdev->pdata->lanes - 1));
507
508         /* Use 0x00000022 for continuous clock mode. */
509         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0,
510                 (pcdev->pdata->continuous_clk << 5) |
511                 0x5); /* Clock settle time */
512
513         TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END,
514                 (0x1 << 8) | /* Enable continuous syncpt */
515                 TEGRA_VI_SYNCPT_CSI);
516
517         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
518
519         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x0000f002);
520 }
521
522 static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
523                                              u32 input_control)
524 {
525         struct soc_camera_device *icd = pcdev->icd;
526         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
527                                                 icd->current_fmt->host_fmt);
528         u32 hdr = tegra_camera_header_for_wrong_fmt(pcdev);
529
530         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
531
532         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL, input_control);
533
534         TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000008);
535         TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000008);
536
537         /* CSI-B H_ACTIVE and V_ACTIVE */
538         TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_H_ACTIVE,
539                 (icd->user_width << 16));
540         TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_V_ACTIVE,
541                 (icd->user_height << 16));
542
543         /* pad 0s enabled, virtual channel ID 00 */
544         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0,
545                 (0x1 << 16) | /* Output 1 pixel per clock */
546                 (hdr << 8) | /* If hdr shows wrong fmt, use right value */
547                 (0x1 << 7) | /* Check header CRC */
548                 (0x1 << 6) | /* Use word count field in the header */
549                 (0x1 << 5) | /* Look at data identifier byte in hdr */
550                 (0x1 << 4) | /* Expect packet header */
551                 0x1); /* Set PPB stream source to CSI B */
552
553         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1,
554                 0x1); /* Frame # for top field detect for interlaced */
555
556         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT,
557                 bytes_per_line);
558         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00140000);
559
560         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME,
561                 (icd->user_height << 16) |
562                 (0x100 << 4) | /* Wait 0x100 vi clks for timeout */
563                 0x1); /* Enable line timeout */
564
565         TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
566                      (0x3f << 16) | /* Skip packet threshold */
567                      (pcdev->pdata->lanes - 1));
568
569         /* Use 0x00000022 for continuous clock mode. */
570         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0,
571                 (pcdev->pdata->continuous_clk << 5) |
572                 0x5); /* Clock settle time */
573
574         TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END,
575                 (0x1 << 8) | /* Enable continuous syncpt */
576                 TEGRA_VI_SYNCPT_CSI);
577
578         TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
579
580         TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x0000f002);
581 }
582
583 static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
584                                              u32 input_control)
585 {
586         struct soc_camera_device *icd = pcdev->icd;
587
588         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
589
590         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
591                 (1 << 27) | /* field detect */
592                 (1 << 25) | /* hsync/vsync decoded from data (BT.656) */
593                 (1 << 1) | /* VIP_INPUT_ENABLE */
594                 input_control);
595
596         TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000000);
597         TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000000);
598
599         /* VIP H_ACTIVE and V_ACTIVE */
600         TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_H_ACTIVE,
601                 (icd->user_width << 16) |
602                 TEGRA_VIP_H_ACTIVE_START);
603         TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_V_ACTIVE,
604                 (icd->user_height << 16) |
605                 TEGRA_VIP_V_ACTIVE_START);
606
607         /*
608          * For VIP, D9..D2 is mapped to the video decoder's P7..P0.
609          * Disable/mask out the other Dn wires.
610          */
611         TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INPUT_ENABLE, 0x000003fc);
612         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_DATA_INPUT_CONTROL, 0x000003fc);
613         TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INVERSION, 0x00000000);
614
615         TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_VIP_VSYNC,
616                 (0x1 << 8) | /* Enable continuous syncpt */
617                 TEGRA_VI_SYNCPT_VI);
618
619         TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL, 0x00000004);
620 }
621
622 static void tegra_camera_capture_output_channel_setup(
623                 struct tegra_camera_dev *pcdev)
624 {
625         struct soc_camera_device *icd = pcdev->icd;
626         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
627                                                 icd->current_fmt->host_fmt);
628         const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
629         u32 output_fourcc = current_fmt->host_fmt->fourcc;
630         u32 output_format, output_control;
631         int port = pcdev->pdata->port;
632
633         switch (output_fourcc) {
634         case V4L2_PIX_FMT_UYVY:
635                 output_format = 0x3; /* Default to YUV422 */
636                 break;
637         case V4L2_PIX_FMT_VYUY:
638                 output_format = (0x1 << 17) | 0x3;
639                 break;
640         case V4L2_PIX_FMT_YUYV:
641                 output_format = (0x2 << 17) | 0x3;
642                 break;
643         case V4L2_PIX_FMT_YVYU:
644                 output_format = (0x3 << 17) | 0x3;
645                 break;
646         case V4L2_PIX_FMT_YUV420:
647         case V4L2_PIX_FMT_YVU420:
648                 output_format = 0x6; /* YUV420 planar */
649                 break;
650         case V4L2_PIX_FMT_SGRBG8:
651         case V4L2_PIX_FMT_SGRBG10:
652                 /* Use second output channel for RAW8/RAW10 */
653                 pcdev->output_channel = 1;
654
655                 if (port == TEGRA_CAMERA_PORT_CSI_A)
656                         output_format = 0x7;
657                 else if (port == TEGRA_CAMERA_PORT_CSI_B)
658                         output_format = 0x8;
659                 else
660                         output_format = 0x9;
661                 break;
662         default:
663                 BUG_ON(1);
664         }
665
666         output_control = (pcdev->pdata->flip_v ? (0x1 << 20) : 0) |
667                         (pcdev->pdata->flip_h ? (0x1 << 19) : 0) |
668                         output_format;
669
670         if (pcdev->output_channel == 0) {
671                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_FIRST_OUTPUT_CONTROL,
672                                 output_control);
673
674                 /*
675                  * Set up frame size.  Bits 31:16 are the number of lines, and
676                  * bits 15:0 are the number of pixels per line.
677                  */
678                 TC_VI_REG_WT(pcdev, TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE,
679                                 (icd->user_height << 16) | icd->user_width);
680
681                 /* First output memory enabled */
682                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
683
684                 /* Set the number of frames in the buffer. */
685                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_FIRST, 0x00000001);
686
687                 /* Set up buffer frame size. */
688                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_FIRST,
689                                 (icd->user_height << 16) | icd->user_width);
690
691                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
692                                 (icd->user_height * bytes_per_line));
693
694                 TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
695                                 (0x1 << 8) | /* Enable continuous syncpt */
696                                 TEGRA_VI_SYNCPT_VI);
697
698                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
699
700         } else if (pcdev->output_channel == 1) {
701                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_SECOND_OUTPUT_CONTROL,
702                                 output_control);
703
704                 TC_VI_REG_WT(pcdev, TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE,
705                                 (icd->user_height << 16) | icd->user_width);
706
707                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
708
709                 /* Set the number of frames in the buffer. */
710                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_SECOND, 0x00000001);
711
712                 /* Set up buffer frame size. */
713                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_SECOND,
714                                 (icd->user_height << 16) | icd->user_width);
715
716                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_SECOND,
717                                 (icd->user_height * bytes_per_line));
718
719                 TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_2,
720                                 (0x1 << 8) | /* Enable continuous syncpt */
721                                 TEGRA_VI_SYNCPT_VI);
722
723                 TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
724         }
725 }
726
727 static void tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
728 {
729         struct soc_camera_device *icd = pcdev->icd;
730         const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
731         enum v4l2_mbus_pixelcode input_code = current_fmt->code;
732         u32 input_control = 0x0;
733         int port = pcdev->pdata->port;
734
735         BUG_ON(!tegra_camera_port_is_valid(port));
736
737         switch (input_code) {
738         case V4L2_MBUS_FMT_UYVY8_2X8:
739                 input_control |= 0x2 << 8;
740                 break;
741         case V4L2_MBUS_FMT_VYUY8_2X8:
742                 input_control |= 0x3 << 8;
743                 break;
744         case V4L2_MBUS_FMT_YUYV8_2X8:
745                 input_control |= 0x0;
746                 break;
747         case V4L2_MBUS_FMT_YVYU8_2X8:
748                 input_control |= 0x1 << 8;
749                 break;
750         case V4L2_MBUS_FMT_SGRBG8_1X8:
751         case V4L2_MBUS_FMT_SGRBG10_1X10:
752                 input_control |= 0x2 << 2;      /* Input Format = Bayer */
753                 break;
754         default:
755                 BUG_ON(1);
756         }
757
758         /*
759          * Set up low pass filter.  Use 0x240 for chromaticity and 0x240
760          * for luminance, which is the default and means not to touch
761          * anything.
762          */
763         TC_VI_REG_WT(pcdev, TEGRA_VI_H_LPF_CONTROL, 0x02400240);
764
765         /* Set up raise-on-edge, so we get an interrupt on end of frame. */
766         TC_VI_REG_WT(pcdev, TEGRA_VI_VI_RAISE, 0x00000001);
767
768         /* Cleanup registers */
769         tegra_camera_capture_clean(pcdev);
770
771         /* Setup registers for CSI-A, CSI-B and VIP inputs */
772         if (port == TEGRA_CAMERA_PORT_CSI_A)
773                 tegra_camera_capture_setup_csi_a(pcdev, input_control);
774         else if (port == TEGRA_CAMERA_PORT_CSI_B)
775                 tegra_camera_capture_setup_csi_b(pcdev, input_control);
776         else
777                 tegra_camera_capture_setup_vip(pcdev, input_control);
778
779         /* Setup registers for output channels */
780         tegra_camera_capture_output_channel_setup(pcdev);
781 }
782
783 static void tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
784                         struct tegra_buffer *buf)
785 {
786         struct soc_camera_device *icd = pcdev->icd;
787
788         switch (icd->current_fmt->host_fmt->fourcc) {
789         case V4L2_PIX_FMT_YUV420:
790         case V4L2_PIX_FMT_YVU420:
791                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_U,
792                              buf->buffer_addr_u);
793                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_U,
794                              buf->start_addr_u);
795
796                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_V,
797                              buf->buffer_addr_v);
798                 TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_V,
799                              buf->start_addr_v);
800
801         case V4L2_PIX_FMT_UYVY:
802         case V4L2_PIX_FMT_VYUY:
803         case V4L2_PIX_FMT_YUYV:
804         case V4L2_PIX_FMT_YVYU:
805         case V4L2_PIX_FMT_SGRBG8:
806         case V4L2_PIX_FMT_SGRBG10:
807                 /* output 1 */
808                 if (!pcdev->output_channel) {
809                         TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_FIRST,
810                                         buf->buffer_addr);
811                         TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_FIRST,
812                                         buf->start_addr);
813                 /* output 2 */
814                 } else if (pcdev->output_channel == 1) {
815                         TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_SECOND,
816                                         buf->buffer_addr);
817                         TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_SECOND,
818                                         buf->start_addr);
819                 }
820         break;
821
822         default:
823                 BUG_ON(1);
824         }
825 }
826
827 static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
828                                       struct tegra_buffer *buf)
829 {
830         int port = pcdev->pdata->port;
831         int err;
832
833         BUG_ON(!tegra_camera_port_is_valid(port));
834
835         pcdev->syncpt_csi++;
836         pcdev->syncpt_vi++;
837
838         tegra_camera_capture_buffer_setup(pcdev, buf);
839
840         if (port == TEGRA_CAMERA_PORT_CSI_A)
841                 TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
842                              0x0000f005);
843         else if (port == TEGRA_CAMERA_PORT_CSI_B)
844                 TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
845                              0x0000f005);
846         else
847                 TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
848                              0x00000001);
849
850         /*
851          * Only wait on CSI frame end syncpt if we're using CSI.  Otherwise,
852          * wait on VIP VSYNC syncpt.
853          */
854         if (tegra_camera_port_is_csi(port))
855                 err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
856                         TEGRA_VI_SYNCPT_CSI,
857                         pcdev->syncpt_csi,
858                         TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
859                         NULL);
860         else
861                 err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
862                         TEGRA_VI_SYNCPT_VI,
863                         pcdev->syncpt_csi,
864                         TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
865                         NULL);
866
867         if (!err)
868                 return 0;
869
870         if (tegra_camera_port_is_csi(port)) {
871                 u32 ppstatus;
872                 u32 cilstatus;
873                 u32 rostatus;
874
875                 dev_err(&pcdev->ndev->dev, "Timeout on CSI syncpt\n");
876                 dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
877                         buf->buffer_addr);
878
879                 ppstatus = TC_VI_REG_RD(pcdev,
880                         TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
881                 cilstatus = TC_VI_REG_RD(pcdev,
882                          TEGRA_CSI_CSI_CIL_STATUS);
883                 rostatus = TC_VI_REG_RD(pcdev,
884                         TEGRA_CSI_CSI_READONLY_STATUS);
885
886                 dev_err(&pcdev->ndev->dev,
887                         "PPSTATUS = 0x%08x, "
888                         "CILSTATUS = 0x%08x, "
889                         "ROSTATUS = 0x%08x\n",
890                         ppstatus, cilstatus, rostatus);
891         } else {
892                 u32 vip_input_status;
893
894                 dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
895                 dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
896                         buf->buffer_addr);
897
898                 vip_input_status = TC_VI_REG_RD(pcdev,
899                         TEGRA_VI_VIP_INPUT_STATUS);
900
901                 dev_err(&pcdev->ndev->dev,
902                         "VIP_INPUT_STATUS = 0x%08x\n",
903                         vip_input_status);
904         }
905
906         return err;
907 }
908
909 static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev)
910 {
911         int port = pcdev->pdata->port;
912         int err;
913
914         BUG_ON(!tegra_camera_port_is_valid(port));
915
916         if (port == TEGRA_CAMERA_PORT_CSI_A)
917                 TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
918                              0x0000f002);
919         else if (port == TEGRA_CAMERA_PORT_CSI_B)
920                 TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
921                              0x0000f002);
922         else
923                 TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
924                              0x00000005);
925
926         if (tegra_camera_port_is_csi(port))
927                 err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
928                         TEGRA_VI_SYNCPT_VI,
929                         pcdev->syncpt_vi,
930                         TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
931                         NULL);
932         else
933                 err = 0;
934
935         if (err) {
936                 u32 buffer_addr;
937                 u32 ppstatus;
938                 u32 cilstatus;
939
940                 dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
941
942                 if (!pcdev->output_channel)
943                         buffer_addr = TC_VI_REG_RD(pcdev,
944                                            TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
945                 else
946                         buffer_addr = TC_VI_REG_RD(pcdev,
947                                            TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
948                 dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
949                         buffer_addr);
950
951                 ppstatus = TC_VI_REG_RD(pcdev,
952                                         TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
953                 cilstatus = TC_VI_REG_RD(pcdev,
954                                          TEGRA_CSI_CSI_CIL_STATUS);
955                 dev_err(&pcdev->ndev->dev,
956                         "PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
957                         ppstatus, cilstatus);
958         }
959
960         return err;
961 }
962
963 static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
964 {
965         struct vb2_buffer *vb;
966         struct tegra_buffer *buf;
967         int retry = TEGRA_SYNCPT_RETRY_COUNT;
968         int port = pcdev->pdata->port;
969         int err;
970
971         if (!pcdev->active)
972                 return 0;
973
974         vb = pcdev->active;
975         buf = to_tegra_vb(vb);
976
977         while (retry) {
978                 err = tegra_camera_capture_start(pcdev, buf);
979                 if (!err)
980                         err = tegra_camera_capture_stop(pcdev);
981
982                 if (err != 0) {
983                         retry--;
984
985                         /* Stop streaming. */
986                         if (port == TEGRA_CAMERA_PORT_CSI_A) {
987                                 TC_VI_REG_WT(pcdev,
988                                              TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
989                                              0x0000f002);
990                                 /* Clear status registers. */
991                                 TC_VI_REG_WT(pcdev,
992                                              TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
993                                              0xffffffff);
994                                 TC_VI_REG_WT(pcdev,
995                                              TEGRA_CSI_CSI_CIL_STATUS,
996                                              0xffffffff);
997                         } else if (port == TEGRA_CAMERA_PORT_CSI_B) {
998                                 TC_VI_REG_WT(pcdev,
999                                              TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
1000                                              0x0000f002);
1001                                 /* Clear status registers. */
1002                                 TC_VI_REG_WT(pcdev,
1003                                              TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
1004                                              0xffffffff);
1005                                 TC_VI_REG_WT(pcdev,
1006                                              TEGRA_CSI_CSI_CIL_STATUS,
1007                                              0xffffffff);
1008                         } else {
1009                                 TC_VI_REG_WT(pcdev,
1010                                              TEGRA_VI_CAMERA_CONTROL,
1011                                              0x00000005);
1012                         }
1013
1014
1015                         tegra_camera_incr_syncpts(pcdev);
1016                         tegra_camera_save_syncpts(pcdev);
1017
1018                         continue;
1019                 }
1020
1021                 break;
1022         }
1023
1024         spin_lock_irq(&pcdev->videobuf_queue_lock);
1025
1026         /*
1027          * If vb->state is VB2_BUF_STATE_ERROR, then the vb has already been
1028          * removed, so we shouldn't remove it again.
1029          */
1030         if (vb->state != VB2_BUF_STATE_ERROR)
1031                 list_del_init(&buf->queue);
1032
1033         if (!list_empty(&pcdev->capture))
1034                 pcdev->active = &list_entry(pcdev->capture.next,
1035                                             struct tegra_buffer, queue)->vb;
1036         else
1037                 pcdev->active = NULL;
1038
1039         do_gettimeofday(&vb->v4l2_buf.timestamp);
1040         vb->v4l2_buf.field = pcdev->field;
1041         vb->v4l2_buf.sequence = pcdev->sequence++;
1042
1043         vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
1044
1045         pcdev->num_frames++;
1046
1047         spin_unlock_irq(&pcdev->videobuf_queue_lock);
1048
1049         return err;
1050 }
1051
1052 static void tegra_camera_work(struct work_struct *work)
1053 {
1054         struct tegra_camera_dev *pcdev =
1055                 container_of(work, struct tegra_camera_dev, work);
1056
1057         mutex_lock(&pcdev->work_mutex);
1058
1059         while (pcdev->active)
1060                 tegra_camera_capture_frame(pcdev);
1061
1062         mutex_unlock(&pcdev->work_mutex);
1063 }
1064
1065 static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
1066 {
1067 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
1068         u32 val;
1069         void __iomem *apb_misc;
1070 #endif
1071
1072         nvhost_module_busy_ext(pcdev->ndev);
1073
1074         /* Turn on relevant clocks. */
1075         clk_enable(pcdev->clk_vi);
1076         clk_enable(pcdev->clk_vi_sensor);
1077         clk_enable(pcdev->clk_csi);
1078         clk_enable(pcdev->clk_isp);
1079         clk_enable(pcdev->clk_csus);
1080
1081 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
1082         apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
1083         val = readl(apb_misc + 0x42c);
1084         writel(val | 0x1, apb_misc + 0x42c);
1085 #endif
1086
1087         /* Save current syncpt values. */
1088         tegra_camera_save_syncpts(pcdev);
1089 }
1090
1091 static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
1092 {
1093         mutex_lock(&pcdev->work_mutex);
1094
1095         /* Cancel active buffer. */
1096         if (pcdev->active) {
1097                 list_del_init(&to_tegra_vb(pcdev->active)->queue);
1098                 vb2_buffer_done(pcdev->active, VB2_BUF_STATE_ERROR);
1099                 pcdev->active = NULL;
1100         }
1101
1102         mutex_unlock(&pcdev->work_mutex);
1103
1104         /* Turn off relevant clocks. */
1105         clk_disable(pcdev->clk_vi);
1106         clk_disable(pcdev->clk_vi_sensor);
1107         clk_disable(pcdev->clk_csi);
1108         clk_disable(pcdev->clk_isp);
1109         clk_disable(pcdev->clk_csus);
1110
1111         nvhost_module_idle_ext(pcdev->ndev);
1112 }
1113
1114 static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
1115                                      struct tegra_buffer *buf)
1116 {
1117         struct soc_camera_device *icd = pcdev->icd;
1118         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
1119                                                 icd->current_fmt->host_fmt);
1120
1121         switch (icd->current_fmt->host_fmt->fourcc) {
1122         case V4L2_PIX_FMT_UYVY:
1123         case V4L2_PIX_FMT_VYUY:
1124         case V4L2_PIX_FMT_YUYV:
1125         case V4L2_PIX_FMT_YVYU:
1126         case V4L2_PIX_FMT_SGRBG8:
1127         case V4L2_PIX_FMT_SGRBG10:
1128                 buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0);
1129                 buf->start_addr = buf->buffer_addr;
1130
1131                 if (pcdev->pdata->flip_v)
1132                         buf->start_addr += bytes_per_line *
1133                                            (icd->user_height-1);
1134
1135                 if (pcdev->pdata->flip_h)
1136                         buf->start_addr += bytes_per_line - 1;
1137
1138                 break;
1139
1140         case V4L2_PIX_FMT_YUV420:
1141         case V4L2_PIX_FMT_YVU420:
1142                 buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0);
1143                 buf->buffer_addr_u = buf->buffer_addr +
1144                                      icd->user_width * icd->user_height;
1145                 buf->buffer_addr_v = buf->buffer_addr_u +
1146                                      (icd->user_width * icd->user_height) / 4;
1147
1148                 /* For YVU420, we swap the locations of the U and V planes. */
1149                 if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
1150                         dma_addr_t temp = buf->buffer_addr_u;
1151                         buf->buffer_addr_u = buf->buffer_addr_v;
1152                         buf->buffer_addr_v = temp;
1153                 }
1154
1155                 buf->start_addr = buf->buffer_addr;
1156                 buf->start_addr_u = buf->buffer_addr_u;
1157                 buf->start_addr_v = buf->buffer_addr_v;
1158
1159                 if (pcdev->pdata->flip_v) {
1160                         buf->start_addr += icd->user_width *
1161                                            (icd->user_height - 1);
1162
1163                         buf->start_addr_u += ((icd->user_width/2) *
1164                                               ((icd->user_height/2) - 1));
1165
1166                         buf->start_addr_v += ((icd->user_width/2) *
1167                                               ((icd->user_height/2) - 1));
1168                 }
1169
1170                 if (pcdev->pdata->flip_h) {
1171                         buf->start_addr += icd->user_width - 1;
1172
1173                         buf->start_addr_u += (icd->user_width/2) - 1;
1174
1175                         buf->start_addr_v += (icd->user_width/2) - 1;
1176                 }
1177
1178                 break;
1179
1180         default:
1181                 BUG_ON(1);
1182         }
1183 }
1184
1185 /*
1186  *  Videobuf operations
1187  */
1188 static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
1189                                        unsigned int *num_buffers,
1190                                        unsigned int *num_planes,
1191                                        unsigned long sizes[],
1192                                        void *alloc_ctxs[])
1193 {
1194         struct soc_camera_device *icd = container_of(vq,
1195                                                      struct soc_camera_device,
1196                                                      vb2_vidq);
1197         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1198         struct tegra_camera_dev *pcdev = ici->priv;
1199         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
1200                                                 icd->current_fmt->host_fmt);
1201
1202         dev_dbg(icd->parent, "In tegra_camera_videobuf_setup()\n");
1203
1204         if (bytes_per_line < 0)
1205                 return bytes_per_line;
1206
1207         *num_planes = 1;
1208
1209         pcdev->sequence = 0;
1210         sizes[0] = bytes_per_line * icd->user_height;
1211         alloc_ctxs[0] = pcdev->alloc_ctx;
1212
1213         if (!*num_buffers)
1214                 *num_buffers = 2;
1215
1216         dev_dbg(icd->parent, "num_buffers=%u, size=%lu\n",
1217                 *num_buffers, sizes[0]);
1218
1219         tegra_camera_capture_setup(pcdev);
1220
1221         dev_dbg(icd->parent, "Finished tegra_camera_videobuf_setup()\n");
1222
1223         return 0;
1224 }
1225
1226 static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
1227 {
1228         struct soc_camera_device *icd = container_of(vb->vb2_queue,
1229                                                      struct soc_camera_device,
1230                                                      vb2_vidq);
1231         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1232         struct tegra_camera_dev *pcdev = ici->priv;
1233         struct tegra_buffer *buf;
1234         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
1235                                                 icd->current_fmt->host_fmt);
1236         unsigned long size;
1237
1238         dev_dbg(icd->parent, "In tegra_camera_videobuf_prepare()\n");
1239
1240         if (bytes_per_line < 0)
1241                 return bytes_per_line;
1242
1243         buf = to_tegra_vb(vb);
1244
1245         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
1246                 vb, vb2_plane_vaddr(vb, 0), vb2_plane_size(vb, 0));
1247
1248 #ifdef PREFILL_BUFFER
1249         /*
1250          * This can be useful if you want to see if we actually fill
1251          * the buffer with something
1252          */
1253         if (vb2_plane_vaddr(vb, 0))
1254                 memset(vb2_plane_vaddr(vb, 0), 0xbd, vb2_plane_size(vb, 0));
1255 #endif
1256
1257         BUG_ON(NULL == icd->current_fmt);
1258
1259         size = icd->user_height * bytes_per_line;
1260
1261         if (vb2_plane_size(vb, 0) < size) {
1262                 dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
1263                         vb2_plane_size(vb, 0), size);
1264                 return -ENOBUFS;
1265         }
1266
1267         vb2_set_plane_payload(vb, 0, size);
1268
1269         tegra_camera_init_buffer(pcdev, buf);
1270
1271         dev_dbg(icd->parent, "Finished tegra_camera_videobuf_prepare()\n");
1272
1273         return 0;
1274 }
1275
1276 static void tegra_camera_videobuf_queue(struct vb2_buffer *vb)
1277 {
1278         struct soc_camera_device *icd = container_of(vb->vb2_queue,
1279                                                      struct soc_camera_device,
1280                                                      vb2_vidq);
1281         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1282         struct tegra_camera_dev *pcdev = ici->priv;
1283         struct tegra_buffer *buf = to_tegra_vb(vb);
1284
1285         dev_dbg(icd->parent, "In tegra_camera_videobuf_queue()\n");
1286
1287         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
1288                 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
1289
1290         spin_lock_irq(&pcdev->videobuf_queue_lock);
1291         list_add_tail(&buf->queue, &pcdev->capture);
1292
1293         if (!pcdev->active) {
1294                 pcdev->active = vb;
1295                 schedule_work(&pcdev->work);
1296         }
1297         spin_unlock_irq(&pcdev->videobuf_queue_lock);
1298
1299         dev_dbg(icd->parent, "Finished tegra_camera_videobuf_queue()\n");
1300 }
1301
1302 static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
1303 {
1304         struct soc_camera_device *icd = container_of(vb->vb2_queue,
1305                                                      struct soc_camera_device,
1306                                                      vb2_vidq);
1307         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1308         struct tegra_buffer *buf = to_tegra_vb(vb);
1309         struct tegra_camera_dev *pcdev = ici->priv;
1310
1311         dev_dbg(icd->parent, "In tegra_camera_videobuf_release()\n");
1312
1313         mutex_lock(&pcdev->work_mutex);
1314
1315         spin_lock_irq(&pcdev->videobuf_queue_lock);
1316
1317         if (pcdev->active == vb)
1318                 pcdev->active = NULL;
1319
1320         /*
1321          * Doesn't hurt also if the list is empty, but it hurts, if queuing the
1322          * buffer failed, and .buf_init() hasn't been called
1323          */
1324         if (buf->queue.next)
1325                 list_del_init(&buf->queue);
1326
1327         spin_unlock_irq(&pcdev->videobuf_queue_lock);
1328
1329         mutex_unlock(&pcdev->work_mutex);
1330
1331         dev_dbg(icd->parent, "Finished tegra_camera_videobuf_release()\n");
1332 }
1333
1334 static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
1335 {
1336         /* This is for locking debugging only */
1337         INIT_LIST_HEAD(&to_tegra_vb(vb)->queue);
1338
1339         return 0;
1340 }
1341
1342 static int tegra_camera_stop_streaming(struct vb2_queue *q)
1343 {
1344         struct soc_camera_device *icd = container_of(q,
1345                                                      struct soc_camera_device,
1346                                                      vb2_vidq);
1347         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1348         struct tegra_camera_dev *pcdev = ici->priv;
1349         struct list_head *buf_head, *tmp;
1350
1351         mutex_lock(&pcdev->work_mutex);
1352
1353         spin_lock_irq(&pcdev->videobuf_queue_lock);
1354
1355         pcdev->active = NULL;
1356
1357         list_for_each_safe(buf_head, tmp, &pcdev->capture)
1358                 list_del_init(buf_head);
1359
1360         spin_unlock_irq(&pcdev->videobuf_queue_lock);
1361
1362         mutex_unlock(&pcdev->work_mutex);
1363
1364         return 0;
1365 }
1366
1367 static struct vb2_ops tegra_camera_videobuf_ops = {
1368         .queue_setup    = tegra_camera_videobuf_setup,
1369         .buf_prepare    = tegra_camera_videobuf_prepare,
1370         .buf_queue      = tegra_camera_videobuf_queue,
1371         .buf_cleanup    = tegra_camera_videobuf_release,
1372         .buf_init       = tegra_camera_videobuf_init,
1373         .wait_prepare   = soc_camera_unlock,
1374         .wait_finish    = soc_camera_lock,
1375         .stop_streaming = tegra_camera_stop_streaming,
1376 };
1377
1378 /*
1379  *  SOC camera host operations
1380  */
1381 static int tegra_camera_init_videobuf(struct vb2_queue *q,
1382                                       struct soc_camera_device *icd)
1383 {
1384         dev_dbg(icd->parent, "In tegra_camera_init_videobuf()\n");
1385
1386         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1387         q->io_modes = VB2_MMAP | VB2_USERPTR;
1388         q->drv_priv = icd;
1389         q->ops = &tegra_camera_videobuf_ops;
1390         q->mem_ops = &vb2_dma_nvmap_memops;
1391         q->buf_struct_size = sizeof(struct tegra_buffer);
1392
1393         dev_dbg(icd->parent, "Finished tegra_camera_init_videobuf()\n");
1394
1395         return vb2_queue_init(q);
1396 }
1397
1398 /*
1399  * Called with .video_lock held
1400  */
1401 static int tegra_camera_add_device(struct soc_camera_device *icd)
1402 {
1403         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1404         struct tegra_camera_dev *pcdev = ici->priv;
1405         int err;
1406
1407         if (pcdev->icd)
1408                 return -EBUSY;
1409
1410         pm_runtime_get_sync(ici->v4l2_dev.dev);
1411
1412         if (pcdev->pdata->enable_camera) {
1413                 err = pcdev->pdata->enable_camera(pcdev->ndev);
1414                 if (IS_ERR_VALUE(err))
1415                         return err;
1416         }
1417
1418         tegra_camera_activate(pcdev);
1419
1420         pcdev->icd = icd;
1421
1422         pcdev->num_frames = 0;
1423         pcdev->output_channel = 0;
1424
1425         dev_dbg(icd->parent, "TEGRA Camera host attached to camera %d\n",
1426                 icd->devnum);
1427
1428         return 0;
1429 }
1430
1431 /* Called with .video_lock held */
1432 static void tegra_camera_remove_device(struct soc_camera_device *icd)
1433 {
1434         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1435         struct tegra_camera_dev *pcdev = ici->priv;
1436
1437         tegra_camera_deactivate(pcdev);
1438
1439         pcdev->icd = NULL;
1440
1441         if (pcdev->pdata->disable_camera)
1442                 pcdev->pdata->disable_camera(pcdev->ndev);
1443
1444         pm_runtime_put_sync(ici->v4l2_dev.dev);
1445
1446         dev_dbg(icd->parent, "Frames captured: %d\n", pcdev->num_frames);
1447
1448         dev_dbg(icd->parent, "TEGRA camera host detached from camera %d\n",
1449                 icd->devnum);
1450 }
1451
1452 static int tegra_camera_set_bus_param(struct soc_camera_device *icd,
1453                                         __u32 pixfmt)
1454 {
1455         return 0;
1456 }
1457
1458 static int tegra_camera_get_formats(struct soc_camera_device *icd,
1459                                     unsigned int idx,
1460                                     struct soc_camera_format_xlate *xlate)
1461 {
1462         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1463         struct device *dev = icd->parent;
1464         int formats = 0;
1465         int ret;
1466         enum v4l2_mbus_pixelcode code;
1467         const struct soc_mbus_pixelfmt *fmt;
1468         int k;
1469
1470         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1471         if (ret != 0)
1472                 /* No more formats */
1473                 return 0;
1474
1475         fmt = soc_mbus_get_fmtdesc(code);
1476         if (!fmt) {
1477                 dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
1478                 return 0;
1479         }
1480
1481         switch (code) {
1482         case V4L2_MBUS_FMT_UYVY8_2X8:
1483         case V4L2_MBUS_FMT_VYUY8_2X8:
1484         case V4L2_MBUS_FMT_YUYV8_2X8:
1485         case V4L2_MBUS_FMT_YVYU8_2X8:
1486         case V4L2_MBUS_FMT_SGRBG8_1X8:
1487         case V4L2_MBUS_FMT_SGRBG10_1X10:
1488                 formats += ARRAY_SIZE(tegra_camera_formats);
1489                 for (k = 0;
1490                      xlate && (k < ARRAY_SIZE(tegra_camera_formats));
1491                      k++) {
1492                         xlate->host_fmt = &tegra_camera_formats[k];
1493                         xlate->code     = code;
1494                         xlate++;
1495
1496                         dev_info(dev, "Providing format %s using code %d\n",
1497                                  tegra_camera_formats[k].name, code);
1498                 }
1499                 break;
1500         default:
1501                 dev_info(dev, "Not supporting %s\n", fmt->name);
1502                 return 0;
1503         }
1504
1505         return formats;
1506 }
1507
1508 static void tegra_camera_put_formats(struct soc_camera_device *icd)
1509 {
1510         kfree(icd->host_priv);
1511         icd->host_priv = NULL;
1512 }
1513
1514 static int tegra_camera_set_fmt(struct soc_camera_device *icd,
1515                               struct v4l2_format *f)
1516 {
1517         struct device *dev = icd->parent;
1518         struct soc_camera_host *ici = to_soc_camera_host(dev);
1519         struct tegra_camera_dev *pcdev = ici->priv;
1520
1521         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1522         const struct soc_camera_format_xlate *xlate = NULL;
1523         struct v4l2_pix_format *pix = &f->fmt.pix;
1524         struct v4l2_mbus_framefmt mf;
1525         int ret;
1526
1527         dev_dbg(dev, "In tegra_camera_set_fmt()\n");
1528
1529         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1530         if (!xlate) {
1531                 dev_warn(dev, "Format %x not found\n", pix->pixelformat);
1532                 return -EINVAL;
1533         }
1534
1535         mf.width        = pix->width;
1536         mf.height       = pix->height;
1537         mf.field        = pix->field;
1538         mf.colorspace   = pix->colorspace;
1539         mf.code         = xlate->code;
1540
1541         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1542         if (IS_ERR_VALUE(ret)) {
1543                 dev_warn(dev, "Failed to configure for format %x\n",
1544                          pix->pixelformat);
1545                 return ret;
1546         }
1547
1548         if (mf.code != xlate->code) {
1549                 dev_warn(dev, "mf.code = %d, xlate->code = %d, mismatch\n",
1550                         mf.code, xlate->code);
1551                 return -EINVAL;
1552         }
1553
1554         icd->user_width         = mf.width;
1555         icd->user_height        = mf.height;
1556         icd->current_fmt        = xlate;
1557
1558         pcdev->field = pix->field;
1559
1560         dev_dbg(dev, "Finished tegra_camera_set_fmt(), returning %d\n", ret);
1561
1562         return ret;
1563 }
1564
1565 static int tegra_camera_try_fmt(struct soc_camera_device *icd,
1566                                 struct v4l2_format *f)
1567 {
1568         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1569         const struct soc_camera_format_xlate *xlate;
1570         struct v4l2_pix_format *pix = &f->fmt.pix;
1571         struct v4l2_mbus_framefmt mf;
1572         __u32 pixfmt = pix->pixelformat;
1573         int ret;
1574
1575         dev_dbg(icd->parent, "In tegra_camera_try_fmt()\n");
1576
1577         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1578         if (!xlate) {
1579                 dev_warn(icd->parent, "Format %x not found\n", pixfmt);
1580                 return -EINVAL;
1581         }
1582
1583         pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1584                                                     xlate->host_fmt);
1585         if (pix->bytesperline < 0)
1586                 return pix->bytesperline;
1587         pix->sizeimage = pix->height * pix->bytesperline;
1588
1589         /* limit to sensor capabilities */
1590         mf.width        = pix->width;
1591         mf.height       = pix->height;
1592         mf.field        = pix->field;
1593         mf.colorspace   = pix->colorspace;
1594         mf.code         = xlate->code;
1595
1596         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1597         if (IS_ERR_VALUE(ret))
1598                 return ret;
1599
1600         pix->width      = mf.width;
1601         pix->height     = mf.height;
1602         pix->colorspace = mf.colorspace;
1603         /*
1604          * width and height could have been changed, therefore update the
1605          * bytesperline and sizeimage here.
1606          */
1607         pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1608                                                     xlate->host_fmt);
1609         pix->sizeimage = pix->height * pix->bytesperline;
1610
1611         switch (mf.field) {
1612         case V4L2_FIELD_ANY:
1613         case V4L2_FIELD_NONE:
1614                 pix->field      = V4L2_FIELD_NONE;
1615                 break;
1616         default:
1617                 /* TODO: support interlaced at least in pass-through mode */
1618                 dev_err(icd->parent, "Field type %d unsupported.\n",
1619                         mf.field);
1620                 return -EINVAL;
1621         }
1622
1623         dev_dbg(icd->parent,
1624                 "Finished tegra_camera_try_fmt(), returning %d\n", ret);
1625
1626         return ret;
1627 }
1628
1629 static int tegra_camera_reqbufs(struct soc_camera_device *icd,
1630                                 struct v4l2_requestbuffers *p)
1631 {
1632         return 0;
1633 }
1634
1635 static unsigned int tegra_camera_poll(struct file *file, poll_table *pt)
1636 {
1637         struct soc_camera_device *icd = file->private_data;
1638
1639         return vb2_poll(&icd->vb2_vidq, file, pt);
1640 }
1641
1642 static int tegra_camera_querycap(struct soc_camera_host *ici,
1643                                  struct v4l2_capability *cap)
1644 {
1645         strlcpy(cap->card, TEGRA_CAM_DRV_NAME, sizeof(cap->card));
1646         cap->version = TEGRA_CAM_VERSION_CODE;
1647         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1648
1649         return 0;
1650 }
1651
1652 static struct soc_camera_host_ops tegra_soc_camera_host_ops = {
1653         .owner          = THIS_MODULE,
1654         .init_videobuf2 = tegra_camera_init_videobuf,
1655         .add            = tegra_camera_add_device,
1656         .remove         = tegra_camera_remove_device,
1657         .set_bus_param  = tegra_camera_set_bus_param,
1658         .get_formats    = tegra_camera_get_formats,
1659         .put_formats    = tegra_camera_put_formats,
1660         .set_fmt        = tegra_camera_set_fmt,
1661         .try_fmt        = tegra_camera_try_fmt,
1662         .reqbufs        = tegra_camera_reqbufs,
1663         .poll           = tegra_camera_poll,
1664         .querycap       = tegra_camera_querycap,
1665 };
1666
1667 static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
1668                                         struct nvhost_device_id *id_table)
1669 {
1670         struct tegra_camera_dev *pcdev;
1671         int err = 0;
1672
1673         pcdev = kzalloc(sizeof(struct tegra_camera_dev), GFP_KERNEL);
1674         if (!pcdev) {
1675                 dev_err(&ndev->dev, "Could not allocate pcdev\n");
1676                 err = -ENOMEM;
1677                 goto exit;
1678         }
1679
1680         pcdev->pdata                    = ndev->dev.platform_data;
1681         pcdev->ndev                     = ndev;
1682
1683         pcdev->ici.priv         = pcdev;
1684         pcdev->ici.v4l2_dev.dev = &ndev->dev;
1685         pcdev->ici.nr           = ndev->id;
1686         pcdev->ici.drv_name     = dev_name(&ndev->dev);
1687         pcdev->ici.ops          = &tegra_soc_camera_host_ops;
1688
1689         INIT_LIST_HEAD(&pcdev->capture);
1690         INIT_WORK(&pcdev->work, tegra_camera_work);
1691         spin_lock_init(&pcdev->videobuf_queue_lock);
1692         mutex_init(&pcdev->work_mutex);
1693
1694         nvhost_set_drvdata(ndev, pcdev);
1695
1696         if (!tegra_camera_port_is_valid(pcdev->pdata->port)) {
1697                 dev_err(&ndev->dev, "Invalid camera port %d in platform data\n",
1698                         pcdev->pdata->port);
1699                 goto exit_free_pcdev;
1700         }
1701
1702         pcdev->clk_vi = clk_get_sys("tegra_camera", "vi");
1703         if (IS_ERR_OR_NULL(pcdev->clk_vi)) {
1704                 dev_err(&ndev->dev, "Failed to get vi clock.\n");
1705                 goto exit_free_pcdev;
1706         }
1707
1708         pcdev->clk_vi_sensor = clk_get_sys("tegra_camera", "vi_sensor");
1709         if (IS_ERR_OR_NULL(pcdev->clk_vi_sensor)) {
1710                 dev_err(&ndev->dev, "Failed to get vi_sensor clock.\n");
1711                 goto exit_put_clk_vi;
1712         }
1713
1714         pcdev->clk_csi = clk_get_sys("tegra_camera", "csi");
1715         if (IS_ERR_OR_NULL(pcdev->clk_csi)) {
1716                 dev_err(&ndev->dev, "Failed to get csi clock.\n");
1717                 goto exit_put_clk_vi_sensor;
1718         }
1719
1720         pcdev->clk_isp = clk_get_sys("tegra_camera", "isp");
1721         if (IS_ERR_OR_NULL(pcdev->clk_isp)) {
1722                 dev_err(&ndev->dev, "Failed to get isp clock.\n");
1723                 goto exit_put_clk_csi;
1724         }
1725
1726         pcdev->clk_csus = clk_get_sys("tegra_camera", "csus");
1727         if (IS_ERR_OR_NULL(pcdev->clk_csus)) {
1728                 dev_err(&ndev->dev, "Failed to get csus clock.\n");
1729                 goto exit_put_clk_isp;
1730         }
1731
1732         clk_set_rate(pcdev->clk_vi, 150000000);
1733         clk_set_rate(pcdev->clk_vi_sensor, 24000000);
1734
1735         err = nvhost_client_device_get_resources(ndev);
1736         if (err)
1737                 goto exit_put_clk_csus;
1738
1739         nvhost_client_device_init(ndev);
1740
1741         pcdev->vi_base = ndev->aperture;
1742
1743         pm_suspend_ignore_children(&ndev->dev, true);
1744         pm_runtime_enable(&ndev->dev);
1745         pm_runtime_resume(&ndev->dev);
1746
1747         pcdev->alloc_ctx = vb2_dma_nvmap_init_ctx(NULL);
1748         if (IS_ERR(pcdev->alloc_ctx)) {
1749                 err = PTR_ERR(pcdev->alloc_ctx);
1750                 goto exit_put_resources;
1751         }
1752
1753         err = soc_camera_host_register(&pcdev->ici);
1754         if (IS_ERR_VALUE(err))
1755                 goto exit_cleanup_alloc_ctx;
1756
1757         dev_notice(&ndev->dev, "Tegra camera driver loaded.\n");
1758
1759         return err;
1760
1761 exit_cleanup_alloc_ctx:
1762         vb2_dma_nvmap_cleanup_ctx(pcdev->alloc_ctx);
1763 exit_put_resources:
1764         pm_runtime_disable(&ndev->dev);
1765         nvhost_client_device_put_resources(ndev);
1766 exit_put_clk_csus:
1767         clk_put(pcdev->clk_csus);
1768 exit_put_clk_isp:
1769         clk_put(pcdev->clk_isp);
1770 exit_put_clk_csi:
1771         clk_put(pcdev->clk_csi);
1772 exit_put_clk_vi_sensor:
1773         clk_put(pcdev->clk_vi_sensor);
1774 exit_put_clk_vi:
1775         clk_put(pcdev->clk_vi);
1776 exit_free_pcdev:
1777         kfree(pcdev);
1778 exit:
1779         return err;
1780 }
1781
1782 static int __devexit tegra_camera_remove(struct nvhost_device *ndev)
1783 {
1784         struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
1785         struct tegra_camera_dev *pcdev = container_of(ici,
1786                                         struct tegra_camera_dev, ici);
1787         struct resource *res;
1788
1789         res = nvhost_get_resource_byname(ndev, IORESOURCE_MEM, "regs");
1790         if (!res)
1791                 return -EBUSY;
1792
1793         soc_camera_host_unregister(ici);
1794
1795         vb2_dma_nvmap_cleanup_ctx(pcdev->alloc_ctx);
1796
1797         pm_runtime_disable(&ndev->dev);
1798
1799         nvhost_client_device_put_resources(ndev);
1800
1801         clk_put(pcdev->clk_csus);
1802         clk_put(pcdev->clk_isp);
1803         clk_put(pcdev->clk_csi);
1804         clk_put(pcdev->clk_vi_sensor);
1805         clk_put(pcdev->clk_vi);
1806
1807         kfree(pcdev);
1808
1809         dev_notice(&ndev->dev, "Tegra camera host driver unloaded\n");
1810
1811         return 0;
1812 }
1813
1814 #ifdef CONFIG_PM_FISH
1815 static int tegra_camera_suspend(struct nvhost_device *ndev, pm_message_t state)
1816 {
1817         struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
1818         struct tegra_camera_dev *pcdev = container_of(ici,
1819                                         struct tegra_camera_dev, ici);
1820
1821         mutex_lock(&pcdev->work_mutex);
1822
1823         /* We only need to do something if a camera sensor is attached. */
1824         if (pcdev->icd) {
1825                 /* Suspend the camera sensor. */
1826                 WARN_ON(!pcdev->icd->ops->suspend);
1827                 pcdev->icd->ops->suspend(pcdev->icd, state);
1828
1829                 /* Power off the camera subsystem. */
1830                 pcdev->pdata->disable_camera(pcdev->ndev);
1831
1832                 nvhost_module_idle_ext(nvhost_get_parent(ndev));
1833         }
1834
1835         return 0;
1836 }
1837
1838 static int tegra_camera_resume(struct nvhost_device *ndev)
1839 {
1840         struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
1841         struct tegra_camera_dev *pcdev = container_of(ici,
1842                                         struct tegra_camera_dev, ici);
1843
1844         /* We only need to do something if a camera sensor is attached. */
1845         if (pcdev->icd) {
1846                 nvhost_module_busy_ext(nvhost_get_parent(ndev));
1847
1848                 /* Power on the camera subsystem. */
1849                 pcdev->pdata->enable_camera(pcdev->ndev);
1850
1851                 /* Resume the camera host. */
1852                 tegra_camera_save_syncpts(pcdev);
1853                 tegra_camera_capture_setup(pcdev);
1854
1855                 /* Resume the camera sensor. */
1856                 WARN_ON(!pcdev->icd->ops->resume);
1857                 pcdev->icd->ops->resume(pcdev->icd);
1858         }
1859
1860         mutex_unlock(&pcdev->work_mutex);
1861
1862         return 0;
1863 }
1864 #endif
1865
1866 static struct nvhost_driver tegra_camera_driver = {
1867         .driver = {
1868                 .name   = TEGRA_CAM_DRV_NAME,
1869                 .owner  = THIS_MODULE,
1870         },
1871         .probe          = tegra_camera_probe,
1872         .remove         = __devexit_p(tegra_camera_remove),
1873 #ifdef CONFIG_PM_FISH
1874         .suspend        = tegra_camera_suspend,
1875         .resume         = tegra_camera_resume,
1876 #endif
1877 };
1878
1879
1880 static int __init tegra_camera_init(void)
1881 {
1882         return nvhost_driver_register(&tegra_camera_driver);
1883 }
1884
1885 static void __exit tegra_camera_exit(void)
1886 {
1887         nvhost_driver_unregister(&tegra_camera_driver);
1888 }
1889
1890 module_init(tegra_camera_init);
1891 module_exit(tegra_camera_exit);
1892
1893 MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
1894 MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
1895 MODULE_LICENSE("GPL v2");
1896 MODULE_ALIAS("nvhost:" TEGRA_CAM_DRV_NAME);