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