Commit | Line | Data |
---|---|---|
392cd78d DS |
1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* | |
3 | * BCM283x / BCM271x Unicam Capture Driver | |
4 | * | |
5 | * Copyright (C) 2017-2020 - Raspberry Pi (Trading) Ltd. | |
6 | * Copyright (C) 2024 - Ideas on Board | |
7 | * | |
8 | * Dave Stevenson <dave.stevenson@raspberrypi.com> | |
9 | * | |
10 | * Based on TI am437x driver by | |
11 | * Benoit Parrot <bparrot@ti.com> | |
12 | * Lad, Prabhakar <prabhakar.csengg@gmail.com> | |
13 | * | |
14 | * and TI CAL camera interface driver by | |
15 | * Benoit Parrot <bparrot@ti.com> | |
16 | * | |
17 | * | |
18 | * There are two camera drivers in the kernel for BCM283x - this one and | |
19 | * bcm2835-camera (currently in staging). | |
20 | * | |
21 | * This driver directly controls the Unicam peripheral - there is no | |
22 | * involvement with the VideoCore firmware. Unicam receives CSI-2 or CCP2 data | |
23 | * and writes it into SDRAM. The only potential processing options are to | |
24 | * repack Bayer data into an alternate format, and applying windowing. The | |
25 | * repacking does not shift the data, so can repack V4L2_PIX_FMT_Sxxxx10P to | |
26 | * V4L2_PIX_FMT_Sxxxx10, or V4L2_PIX_FMT_Sxxxx12P to V4L2_PIX_FMT_Sxxxx12, but | |
27 | * not generically up to V4L2_PIX_FMT_Sxxxx16. Support for windowing may be | |
28 | * added later. | |
29 | * | |
30 | * It should be possible to connect this driver to any sensor with a suitable | |
31 | * output interface and V4L2 subdevice driver. | |
32 | */ | |
33 | ||
34 | #include <linux/clk.h> | |
35 | #include <linux/delay.h> | |
36 | #include <linux/device.h> | |
37 | #include <linux/dma-mapping.h> | |
38 | #include <linux/err.h> | |
39 | #include <linux/interrupt.h> | |
40 | #include <linux/io.h> | |
41 | #include <linux/module.h> | |
42 | #include <linux/of.h> | |
43 | #include <linux/of_device.h> | |
44 | #include <linux/platform_device.h> | |
45 | #include <linux/pm_runtime.h> | |
46 | #include <linux/slab.h> | |
47 | #include <linux/videodev2.h> | |
48 | ||
49 | #include <media/mipi-csi2.h> | |
50 | #include <media/v4l2-async.h> | |
51 | #include <media/v4l2-common.h> | |
52 | #include <media/v4l2-dev.h> | |
53 | #include <media/v4l2-device.h> | |
54 | #include <media/v4l2-event.h> | |
55 | #include <media/v4l2-ioctl.h> | |
56 | #include <media/v4l2-fwnode.h> | |
57 | #include <media/v4l2-mc.h> | |
79390f96 | 58 | #include <media/v4l2-subdev.h> |
392cd78d DS |
59 | #include <media/videobuf2-dma-contig.h> |
60 | ||
61 | #include "bcm2835-unicam-regs.h" | |
62 | ||
63 | #define UNICAM_MODULE_NAME "unicam" | |
64 | ||
65 | /* | |
66 | * Unicam must request a minimum of 250Mhz from the VPU clock. | |
67 | * Otherwise the input FIFOs overrun and cause image corruption. | |
68 | */ | |
69 | #define UNICAM_MIN_VPU_CLOCK_RATE (250 * 1000 * 1000) | |
70 | ||
71 | /* Unicam has an internal DMA alignment constraint of 16 bytes for each line. */ | |
72 | #define UNICAM_DMA_BPL_ALIGNMENT 16 | |
73 | ||
74 | /* | |
75 | * The image stride is stored in a 16 bit register, and needs to be aligned to | |
76 | * the DMA constraint. As the ISP in the same SoC has a 32 bytes alignment | |
77 | * constraint on its input, set the image stride alignment to 32 bytes here as | |
78 | * well to avoid incompatible configurations. | |
79 | */ | |
80 | #define UNICAM_IMAGE_BPL_ALIGNMENT 32 | |
81 | #define UNICAM_IMAGE_MAX_BPL ((1U << 16) - UNICAM_IMAGE_BPL_ALIGNMENT) | |
82 | ||
83 | /* | |
84 | * Max width is therefore determined by the max stride divided by the number of | |
85 | * bits per pixel. Take 32bpp as a worst case. No imposed limit on the height, | |
86 | * so adopt a square image for want of anything better. | |
87 | */ | |
88 | #define UNICAM_IMAGE_MIN_WIDTH 16 | |
89 | #define UNICAM_IMAGE_MIN_HEIGHT 16 | |
90 | #define UNICAM_IMAGE_MAX_WIDTH (UNICAM_IMAGE_MAX_BPL / 4) | |
91 | #define UNICAM_IMAGE_MAX_HEIGHT UNICAM_IMAGE_MAX_WIDTH | |
92 | ||
93 | /* | |
94 | * There's no intrinsic limits on the width and height for embedded data. Use | |
95 | * the same maximum values as for the image, to avoid overflows in the image | |
96 | * size computation. | |
97 | */ | |
98 | #define UNICAM_META_MIN_WIDTH 1 | |
99 | #define UNICAM_META_MIN_HEIGHT 1 | |
100 | #define UNICAM_META_MAX_WIDTH UNICAM_IMAGE_MAX_WIDTH | |
101 | #define UNICAM_META_MAX_HEIGHT UNICAM_IMAGE_MAX_HEIGHT | |
102 | ||
103 | /* | |
104 | * Size of the dummy buffer. Can be any size really, but the DMA | |
105 | * allocation works in units of page sizes. | |
106 | */ | |
107 | #define UNICAM_DUMMY_BUF_SIZE PAGE_SIZE | |
108 | ||
109 | enum unicam_pad { | |
110 | UNICAM_SD_PAD_SINK, | |
111 | UNICAM_SD_PAD_SOURCE_IMAGE, | |
112 | UNICAM_SD_PAD_SOURCE_METADATA, | |
113 | UNICAM_SD_NUM_PADS | |
114 | }; | |
115 | ||
116 | enum unicam_node_type { | |
117 | UNICAM_IMAGE_NODE, | |
118 | UNICAM_METADATA_NODE, | |
119 | UNICAM_MAX_NODES | |
120 | }; | |
121 | ||
122 | /* | |
123 | * struct unicam_format_info - Unicam media bus format information | |
124 | * @fourcc: V4L2 pixel format FCC identifier. 0 if n/a. | |
125 | * @unpacked_fourcc: V4L2 pixel format FCC identifier if the data is expanded | |
126 | * out to 16bpp. 0 if n/a. | |
127 | * @code: V4L2 media bus format code. | |
128 | * @depth: Bits per pixel as delivered from the source. | |
129 | * @csi_dt: CSI data type. | |
130 | * @unpack: PUM value when unpacking to @unpacked_fourcc | |
131 | */ | |
132 | struct unicam_format_info { | |
133 | u32 fourcc; | |
134 | u32 unpacked_fourcc; | |
135 | u32 code; | |
136 | u8 depth; | |
137 | u8 csi_dt; | |
138 | u8 unpack; | |
139 | }; | |
140 | ||
141 | struct unicam_buffer { | |
142 | struct vb2_v4l2_buffer vb; | |
143 | struct list_head list; | |
144 | dma_addr_t dma_addr; | |
145 | unsigned int size; | |
146 | }; | |
147 | ||
148 | static inline struct unicam_buffer *to_unicam_buffer(struct vb2_buffer *vb) | |
149 | { | |
150 | return container_of(vb, struct unicam_buffer, vb.vb2_buf); | |
151 | } | |
152 | ||
153 | struct unicam_node { | |
154 | bool registered; | |
155 | unsigned int id; | |
156 | ||
157 | /* Pointer to the current v4l2_buffer */ | |
158 | struct unicam_buffer *cur_frm; | |
159 | /* Pointer to the next v4l2_buffer */ | |
160 | struct unicam_buffer *next_frm; | |
161 | /* Used to store current pixel format */ | |
162 | struct v4l2_format fmt; | |
163 | /* Buffer queue used in video-buf */ | |
164 | struct vb2_queue buffer_queue; | |
165 | /* Queue of filled frames */ | |
166 | struct list_head dma_queue; | |
167 | /* IRQ lock for DMA queue */ | |
168 | spinlock_t dma_queue_lock; | |
169 | /* Identifies video device for this channel */ | |
170 | struct video_device video_dev; | |
171 | /* Pointer to the parent handle */ | |
172 | struct unicam_device *dev; | |
173 | struct media_pad pad; | |
174 | /* | |
175 | * Dummy buffer intended to be used by unicam | |
176 | * if we have no other queued buffers to swap to. | |
177 | */ | |
178 | struct unicam_buffer dummy_buf; | |
179 | void *dummy_buf_cpu_addr; | |
180 | }; | |
181 | ||
182 | struct unicam_device { | |
183 | struct kref kref; | |
184 | ||
185 | /* peripheral base address */ | |
186 | void __iomem *base; | |
187 | /* clock gating base address */ | |
188 | void __iomem *clk_gate_base; | |
189 | /* lp clock handle */ | |
190 | struct clk *clock; | |
191 | /* vpu clock handle */ | |
192 | struct clk *vpu_clock; | |
193 | /* V4l2 device */ | |
194 | struct v4l2_device v4l2_dev; | |
195 | struct media_device mdev; | |
196 | ||
197 | /* parent device */ | |
198 | struct device *dev; | |
199 | /* subdevice async notifier */ | |
200 | struct v4l2_async_notifier notifier; | |
201 | unsigned int sequence; | |
202 | ||
203 | /* Sensor node */ | |
204 | struct { | |
205 | struct v4l2_subdev *subdev; | |
206 | struct media_pad *pad; | |
207 | } sensor; | |
208 | ||
209 | /* Internal subdev */ | |
210 | struct { | |
211 | struct v4l2_subdev sd; | |
212 | struct media_pad pads[UNICAM_SD_NUM_PADS]; | |
213 | unsigned int enabled_streams; | |
214 | } subdev; | |
215 | ||
216 | enum v4l2_mbus_type bus_type; | |
217 | /* | |
218 | * Stores bus.mipi_csi2.flags for CSI2 sensors, or | |
219 | * bus.mipi_csi1.strobe for CCP2. | |
220 | */ | |
221 | unsigned int bus_flags; | |
222 | unsigned int max_data_lanes; | |
223 | ||
224 | struct { | |
225 | struct media_pipeline pipe; | |
226 | unsigned int num_data_lanes; | |
227 | unsigned int nodes; | |
228 | } pipe; | |
229 | ||
230 | /* Lock used for the video devices of both nodes */ | |
231 | struct mutex lock; | |
232 | struct unicam_node node[UNICAM_MAX_NODES]; | |
233 | }; | |
234 | ||
235 | static inline struct unicam_device * | |
236 | notifier_to_unicam_device(struct v4l2_async_notifier *notifier) | |
237 | { | |
238 | return container_of(notifier, struct unicam_device, notifier); | |
239 | } | |
240 | ||
241 | static inline struct unicam_device * | |
242 | sd_to_unicam_device(struct v4l2_subdev *sd) | |
243 | { | |
244 | return container_of(sd, struct unicam_device, subdev.sd); | |
245 | } | |
246 | ||
247 | static void unicam_release(struct kref *kref) | |
248 | { | |
249 | struct unicam_device *unicam = | |
250 | container_of(kref, struct unicam_device, kref); | |
251 | ||
252 | if (unicam->mdev.dev) | |
253 | media_device_cleanup(&unicam->mdev); | |
254 | ||
255 | mutex_destroy(&unicam->lock); | |
256 | kfree(unicam); | |
257 | } | |
258 | ||
259 | static struct unicam_device *unicam_get(struct unicam_device *unicam) | |
260 | { | |
261 | kref_get(&unicam->kref); | |
262 | ||
263 | return unicam; | |
264 | } | |
265 | ||
266 | static void unicam_put(struct unicam_device *unicam) | |
267 | { | |
268 | kref_put(&unicam->kref, unicam_release); | |
269 | } | |
270 | ||
271 | /* ----------------------------------------------------------------------------- | |
272 | * Misc helper functions | |
273 | */ | |
274 | ||
275 | static inline bool unicam_sd_pad_is_source(u32 pad) | |
276 | { | |
277 | /* Camera RX has 1 sink pad, and N source pads */ | |
278 | return pad != UNICAM_SD_PAD_SINK; | |
279 | } | |
280 | ||
281 | static inline bool is_metadata_node(struct unicam_node *node) | |
282 | { | |
283 | return node->video_dev.device_caps & V4L2_CAP_META_CAPTURE; | |
284 | } | |
285 | ||
286 | static inline bool is_image_node(struct unicam_node *node) | |
287 | { | |
288 | return node->video_dev.device_caps & V4L2_CAP_VIDEO_CAPTURE; | |
289 | } | |
290 | ||
291 | /* ----------------------------------------------------------------------------- | |
292 | * Format data table and helper functions | |
293 | */ | |
294 | ||
295 | static const struct v4l2_mbus_framefmt unicam_default_image_format = { | |
296 | .width = 640, | |
297 | .height = 480, | |
298 | .code = MEDIA_BUS_FMT_UYVY8_1X16, | |
299 | .field = V4L2_FIELD_NONE, | |
300 | .colorspace = V4L2_COLORSPACE_SRGB, | |
301 | .ycbcr_enc = V4L2_YCBCR_ENC_601, | |
302 | .quantization = V4L2_QUANTIZATION_LIM_RANGE, | |
303 | .xfer_func = V4L2_XFER_FUNC_SRGB, | |
304 | .flags = 0, | |
305 | }; | |
306 | ||
307 | static const struct v4l2_mbus_framefmt unicam_default_meta_format = { | |
308 | .width = 640, | |
309 | .height = 2, | |
310 | .code = MEDIA_BUS_FMT_META_8, | |
311 | .field = V4L2_FIELD_NONE, | |
312 | }; | |
313 | ||
314 | static const struct unicam_format_info unicam_image_formats[] = { | |
315 | /* YUV Formats */ | |
316 | { | |
317 | .fourcc = V4L2_PIX_FMT_YUYV, | |
318 | .code = MEDIA_BUS_FMT_YUYV8_1X16, | |
319 | .depth = 16, | |
320 | .csi_dt = MIPI_CSI2_DT_YUV422_8B, | |
321 | }, { | |
322 | .fourcc = V4L2_PIX_FMT_UYVY, | |
323 | .code = MEDIA_BUS_FMT_UYVY8_1X16, | |
324 | .depth = 16, | |
325 | .csi_dt = MIPI_CSI2_DT_YUV422_8B, | |
326 | }, { | |
327 | .fourcc = V4L2_PIX_FMT_YVYU, | |
328 | .code = MEDIA_BUS_FMT_YVYU8_1X16, | |
329 | .depth = 16, | |
330 | .csi_dt = MIPI_CSI2_DT_YUV422_8B, | |
331 | }, { | |
332 | .fourcc = V4L2_PIX_FMT_VYUY, | |
333 | .code = MEDIA_BUS_FMT_VYUY8_1X16, | |
334 | .depth = 16, | |
335 | .csi_dt = MIPI_CSI2_DT_YUV422_8B, | |
336 | }, { | |
337 | /* RGB Formats */ | |
338 | .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */ | |
339 | .code = MEDIA_BUS_FMT_RGB565_1X16, | |
340 | .depth = 16, | |
341 | .csi_dt = MIPI_CSI2_DT_RGB565, | |
342 | }, { | |
343 | .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */ | |
344 | .code = MEDIA_BUS_FMT_RGB888_1X24, | |
345 | .depth = 24, | |
346 | .csi_dt = MIPI_CSI2_DT_RGB888, | |
347 | }, { | |
348 | .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */ | |
349 | .code = MEDIA_BUS_FMT_BGR888_1X24, | |
350 | .depth = 24, | |
351 | .csi_dt = MIPI_CSI2_DT_RGB888, | |
352 | }, { | |
353 | /* Bayer Formats */ | |
354 | .fourcc = V4L2_PIX_FMT_SBGGR8, | |
355 | .code = MEDIA_BUS_FMT_SBGGR8_1X8, | |
356 | .depth = 8, | |
357 | .csi_dt = MIPI_CSI2_DT_RAW8, | |
358 | }, { | |
359 | .fourcc = V4L2_PIX_FMT_SGBRG8, | |
360 | .code = MEDIA_BUS_FMT_SGBRG8_1X8, | |
361 | .depth = 8, | |
362 | .csi_dt = MIPI_CSI2_DT_RAW8, | |
363 | }, { | |
364 | .fourcc = V4L2_PIX_FMT_SGRBG8, | |
365 | .code = MEDIA_BUS_FMT_SGRBG8_1X8, | |
366 | .depth = 8, | |
367 | .csi_dt = MIPI_CSI2_DT_RAW8, | |
368 | }, { | |
369 | .fourcc = V4L2_PIX_FMT_SRGGB8, | |
370 | .code = MEDIA_BUS_FMT_SRGGB8_1X8, | |
371 | .depth = 8, | |
372 | .csi_dt = MIPI_CSI2_DT_RAW8, | |
373 | }, { | |
374 | .fourcc = V4L2_PIX_FMT_SBGGR10P, | |
375 | .unpacked_fourcc = V4L2_PIX_FMT_SBGGR10, | |
376 | .code = MEDIA_BUS_FMT_SBGGR10_1X10, | |
377 | .depth = 10, | |
378 | .csi_dt = MIPI_CSI2_DT_RAW10, | |
379 | .unpack = UNICAM_PUM_UNPACK10, | |
380 | }, { | |
381 | .fourcc = V4L2_PIX_FMT_SGBRG10P, | |
382 | .unpacked_fourcc = V4L2_PIX_FMT_SGBRG10, | |
383 | .code = MEDIA_BUS_FMT_SGBRG10_1X10, | |
384 | .depth = 10, | |
385 | .csi_dt = MIPI_CSI2_DT_RAW10, | |
386 | .unpack = UNICAM_PUM_UNPACK10, | |
387 | }, { | |
388 | .fourcc = V4L2_PIX_FMT_SGRBG10P, | |
389 | .unpacked_fourcc = V4L2_PIX_FMT_SGRBG10, | |
390 | .code = MEDIA_BUS_FMT_SGRBG10_1X10, | |
391 | .depth = 10, | |
392 | .csi_dt = MIPI_CSI2_DT_RAW10, | |
393 | .unpack = UNICAM_PUM_UNPACK10, | |
394 | }, { | |
395 | .fourcc = V4L2_PIX_FMT_SRGGB10P, | |
396 | .unpacked_fourcc = V4L2_PIX_FMT_SRGGB10, | |
397 | .code = MEDIA_BUS_FMT_SRGGB10_1X10, | |
398 | .depth = 10, | |
399 | .csi_dt = MIPI_CSI2_DT_RAW10, | |
400 | .unpack = UNICAM_PUM_UNPACK10, | |
401 | }, { | |
402 | .fourcc = V4L2_PIX_FMT_SBGGR12P, | |
403 | .unpacked_fourcc = V4L2_PIX_FMT_SBGGR12, | |
404 | .code = MEDIA_BUS_FMT_SBGGR12_1X12, | |
405 | .depth = 12, | |
406 | .csi_dt = MIPI_CSI2_DT_RAW12, | |
407 | .unpack = UNICAM_PUM_UNPACK12, | |
408 | }, { | |
409 | .fourcc = V4L2_PIX_FMT_SGBRG12P, | |
410 | .unpacked_fourcc = V4L2_PIX_FMT_SGBRG12, | |
411 | .code = MEDIA_BUS_FMT_SGBRG12_1X12, | |
412 | .depth = 12, | |
413 | .csi_dt = MIPI_CSI2_DT_RAW12, | |
414 | .unpack = UNICAM_PUM_UNPACK12, | |
415 | }, { | |
416 | .fourcc = V4L2_PIX_FMT_SGRBG12P, | |
417 | .unpacked_fourcc = V4L2_PIX_FMT_SGRBG12, | |
418 | .code = MEDIA_BUS_FMT_SGRBG12_1X12, | |
419 | .depth = 12, | |
420 | .csi_dt = MIPI_CSI2_DT_RAW12, | |
421 | .unpack = UNICAM_PUM_UNPACK12, | |
422 | }, { | |
423 | .fourcc = V4L2_PIX_FMT_SRGGB12P, | |
424 | .unpacked_fourcc = V4L2_PIX_FMT_SRGGB12, | |
425 | .code = MEDIA_BUS_FMT_SRGGB12_1X12, | |
426 | .depth = 12, | |
427 | .csi_dt = MIPI_CSI2_DT_RAW12, | |
428 | .unpack = UNICAM_PUM_UNPACK12, | |
429 | }, { | |
430 | .fourcc = V4L2_PIX_FMT_SBGGR14P, | |
431 | .unpacked_fourcc = V4L2_PIX_FMT_SBGGR14, | |
432 | .code = MEDIA_BUS_FMT_SBGGR14_1X14, | |
433 | .depth = 14, | |
434 | .csi_dt = MIPI_CSI2_DT_RAW14, | |
435 | .unpack = UNICAM_PUM_UNPACK14, | |
436 | }, { | |
437 | .fourcc = V4L2_PIX_FMT_SGBRG14P, | |
438 | .unpacked_fourcc = V4L2_PIX_FMT_SGBRG14, | |
439 | .code = MEDIA_BUS_FMT_SGBRG14_1X14, | |
440 | .depth = 14, | |
441 | .csi_dt = MIPI_CSI2_DT_RAW14, | |
442 | .unpack = UNICAM_PUM_UNPACK14, | |
443 | }, { | |
444 | .fourcc = V4L2_PIX_FMT_SGRBG14P, | |
445 | .unpacked_fourcc = V4L2_PIX_FMT_SGRBG14, | |
446 | .code = MEDIA_BUS_FMT_SGRBG14_1X14, | |
447 | .depth = 14, | |
448 | .csi_dt = MIPI_CSI2_DT_RAW14, | |
449 | .unpack = UNICAM_PUM_UNPACK14, | |
450 | }, { | |
451 | .fourcc = V4L2_PIX_FMT_SRGGB14P, | |
452 | .unpacked_fourcc = V4L2_PIX_FMT_SRGGB14, | |
453 | .code = MEDIA_BUS_FMT_SRGGB14_1X14, | |
454 | .depth = 14, | |
455 | .csi_dt = MIPI_CSI2_DT_RAW14, | |
456 | .unpack = UNICAM_PUM_UNPACK14, | |
457 | }, { | |
458 | /* 16 bit Bayer formats could be supported. */ | |
459 | ||
460 | /* Greyscale formats */ | |
461 | .fourcc = V4L2_PIX_FMT_GREY, | |
462 | .code = MEDIA_BUS_FMT_Y8_1X8, | |
463 | .depth = 8, | |
464 | .csi_dt = MIPI_CSI2_DT_RAW8, | |
465 | }, { | |
466 | .fourcc = V4L2_PIX_FMT_Y10P, | |
467 | .unpacked_fourcc = V4L2_PIX_FMT_Y10, | |
468 | .code = MEDIA_BUS_FMT_Y10_1X10, | |
469 | .depth = 10, | |
470 | .csi_dt = MIPI_CSI2_DT_RAW10, | |
471 | .unpack = UNICAM_PUM_UNPACK10, | |
472 | }, { | |
473 | .fourcc = V4L2_PIX_FMT_Y12P, | |
474 | .unpacked_fourcc = V4L2_PIX_FMT_Y12, | |
475 | .code = MEDIA_BUS_FMT_Y12_1X12, | |
476 | .depth = 12, | |
477 | .csi_dt = MIPI_CSI2_DT_RAW12, | |
478 | .unpack = UNICAM_PUM_UNPACK12, | |
479 | }, { | |
480 | .fourcc = V4L2_PIX_FMT_Y14P, | |
481 | .unpacked_fourcc = V4L2_PIX_FMT_Y14, | |
482 | .code = MEDIA_BUS_FMT_Y14_1X14, | |
483 | .depth = 14, | |
484 | .csi_dt = MIPI_CSI2_DT_RAW14, | |
485 | .unpack = UNICAM_PUM_UNPACK14, | |
486 | }, | |
487 | }; | |
488 | ||
489 | static const struct unicam_format_info unicam_meta_formats[] = { | |
490 | { | |
491 | .fourcc = V4L2_META_FMT_GENERIC_8, | |
492 | .code = MEDIA_BUS_FMT_META_8, | |
493 | .depth = 8, | |
494 | }, { | |
495 | .fourcc = V4L2_META_FMT_GENERIC_CSI2_10, | |
496 | .code = MEDIA_BUS_FMT_META_10, | |
497 | .depth = 10, | |
498 | }, { | |
499 | .fourcc = V4L2_META_FMT_GENERIC_CSI2_12, | |
500 | .code = MEDIA_BUS_FMT_META_12, | |
501 | .depth = 12, | |
502 | }, { | |
503 | .fourcc = V4L2_META_FMT_GENERIC_CSI2_14, | |
504 | .code = MEDIA_BUS_FMT_META_14, | |
505 | .depth = 14, | |
506 | }, | |
507 | }; | |
508 | ||
509 | /* Format setup functions */ | |
510 | static const struct unicam_format_info * | |
511 | unicam_find_format_by_code(u32 code, u32 pad) | |
512 | { | |
513 | const struct unicam_format_info *formats; | |
514 | unsigned int num_formats; | |
515 | unsigned int i; | |
516 | ||
517 | if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) { | |
518 | formats = unicam_image_formats; | |
519 | num_formats = ARRAY_SIZE(unicam_image_formats); | |
520 | } else { | |
521 | formats = unicam_meta_formats; | |
522 | num_formats = ARRAY_SIZE(unicam_meta_formats); | |
523 | } | |
524 | ||
525 | for (i = 0; i < num_formats; i++) { | |
526 | if (formats[i].code == code) | |
527 | return &formats[i]; | |
528 | } | |
529 | ||
530 | return NULL; | |
531 | } | |
532 | ||
533 | static const struct unicam_format_info * | |
534 | unicam_find_format_by_fourcc(u32 fourcc, u32 pad) | |
535 | { | |
536 | const struct unicam_format_info *formats; | |
537 | unsigned int num_formats; | |
538 | unsigned int i; | |
539 | ||
540 | if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) { | |
541 | formats = unicam_image_formats; | |
542 | num_formats = ARRAY_SIZE(unicam_image_formats); | |
543 | } else { | |
544 | formats = unicam_meta_formats; | |
545 | num_formats = ARRAY_SIZE(unicam_meta_formats); | |
546 | } | |
547 | ||
548 | for (i = 0; i < num_formats; ++i) { | |
549 | if (formats[i].fourcc == fourcc) | |
550 | return &formats[i]; | |
551 | } | |
552 | ||
553 | return NULL; | |
554 | } | |
555 | ||
556 | static void unicam_calc_image_size_bpl(struct unicam_device *unicam, | |
557 | const struct unicam_format_info *fmtinfo, | |
558 | struct v4l2_pix_format *pix) | |
559 | { | |
560 | u32 min_bpl; | |
561 | ||
562 | v4l_bound_align_image(&pix->width, UNICAM_IMAGE_MIN_WIDTH, | |
563 | UNICAM_IMAGE_MAX_WIDTH, 2, | |
564 | &pix->height, UNICAM_IMAGE_MIN_HEIGHT, | |
565 | UNICAM_IMAGE_MAX_HEIGHT, 0, 0); | |
566 | ||
567 | /* Unpacking always goes to 16bpp */ | |
568 | if (pix->pixelformat == fmtinfo->unpacked_fourcc) | |
569 | min_bpl = pix->width * 2; | |
570 | else | |
571 | min_bpl = pix->width * fmtinfo->depth / 8; | |
572 | min_bpl = ALIGN(min_bpl, UNICAM_IMAGE_BPL_ALIGNMENT); | |
573 | ||
574 | pix->bytesperline = ALIGN(pix->bytesperline, UNICAM_IMAGE_BPL_ALIGNMENT); | |
575 | pix->bytesperline = clamp_t(unsigned int, pix->bytesperline, min_bpl, | |
576 | UNICAM_IMAGE_MAX_BPL); | |
577 | ||
578 | pix->sizeimage = pix->height * pix->bytesperline; | |
579 | } | |
580 | ||
581 | static void unicam_calc_meta_size_bpl(struct unicam_device *unicam, | |
582 | const struct unicam_format_info *fmtinfo, | |
583 | struct v4l2_meta_format *meta) | |
584 | { | |
585 | v4l_bound_align_image(&meta->width, UNICAM_META_MIN_WIDTH, | |
586 | UNICAM_META_MAX_WIDTH, 0, | |
587 | &meta->height, UNICAM_META_MIN_HEIGHT, | |
588 | UNICAM_META_MAX_HEIGHT, 0, 0); | |
589 | ||
590 | meta->bytesperline = ALIGN(meta->width * fmtinfo->depth / 8, | |
591 | UNICAM_DMA_BPL_ALIGNMENT); | |
592 | meta->buffersize = meta->height * meta->bytesperline; | |
593 | } | |
594 | ||
595 | /* ----------------------------------------------------------------------------- | |
596 | * Hardware handling | |
597 | */ | |
598 | ||
599 | static inline void unicam_clk_write(struct unicam_device *unicam, u32 val) | |
600 | { | |
601 | /* Pass the CM_PASSWORD along with the value. */ | |
602 | writel(val | 0x5a000000, unicam->clk_gate_base); | |
603 | } | |
604 | ||
605 | static inline u32 unicam_reg_read(struct unicam_device *unicam, u32 offset) | |
606 | { | |
607 | return readl(unicam->base + offset); | |
608 | } | |
609 | ||
610 | static inline void unicam_reg_write(struct unicam_device *unicam, u32 offset, u32 val) | |
611 | { | |
612 | writel(val, unicam->base + offset); | |
613 | } | |
614 | ||
615 | static inline int unicam_get_field(u32 value, u32 mask) | |
616 | { | |
617 | return (value & mask) >> __ffs(mask); | |
618 | } | |
619 | ||
620 | static inline void unicam_set_field(u32 *valp, u32 field, u32 mask) | |
621 | { | |
622 | u32 val = *valp; | |
623 | ||
624 | val &= ~mask; | |
625 | val |= (field << __ffs(mask)) & mask; | |
626 | *valp = val; | |
627 | } | |
628 | ||
629 | static inline void unicam_reg_write_field(struct unicam_device *unicam, u32 offset, | |
630 | u32 field, u32 mask) | |
631 | { | |
632 | u32 val = unicam_reg_read(unicam, offset); | |
633 | ||
634 | unicam_set_field(&val, field, mask); | |
635 | unicam_reg_write(unicam, offset, val); | |
636 | } | |
637 | ||
638 | static void unicam_wr_dma_addr(struct unicam_node *node, | |
639 | struct unicam_buffer *buf) | |
640 | { | |
641 | dma_addr_t endaddr = buf->dma_addr + buf->size; | |
642 | ||
643 | if (node->id == UNICAM_IMAGE_NODE) { | |
644 | unicam_reg_write(node->dev, UNICAM_IBSA0, buf->dma_addr); | |
645 | unicam_reg_write(node->dev, UNICAM_IBEA0, endaddr); | |
646 | } else { | |
647 | unicam_reg_write(node->dev, UNICAM_DBSA0, buf->dma_addr); | |
648 | unicam_reg_write(node->dev, UNICAM_DBEA0, endaddr); | |
649 | } | |
650 | } | |
651 | ||
652 | static unsigned int unicam_get_lines_done(struct unicam_device *unicam) | |
653 | { | |
654 | struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE]; | |
655 | unsigned int stride = node->fmt.fmt.pix.bytesperline; | |
656 | struct unicam_buffer *frm = node->cur_frm; | |
657 | dma_addr_t cur_addr; | |
658 | ||
659 | if (!frm) | |
660 | return 0; | |
661 | ||
662 | cur_addr = unicam_reg_read(unicam, UNICAM_IBWP); | |
663 | return (unsigned int)(cur_addr - frm->dma_addr) / stride; | |
664 | } | |
665 | ||
666 | static void unicam_schedule_next_buffer(struct unicam_node *node) | |
667 | { | |
668 | struct unicam_buffer *buf; | |
669 | ||
670 | buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list); | |
671 | node->next_frm = buf; | |
672 | list_del(&buf->list); | |
673 | ||
674 | unicam_wr_dma_addr(node, buf); | |
675 | } | |
676 | ||
677 | static void unicam_schedule_dummy_buffer(struct unicam_node *node) | |
678 | { | |
679 | int node_id = is_image_node(node) ? UNICAM_IMAGE_NODE : UNICAM_METADATA_NODE; | |
680 | ||
681 | dev_dbg(node->dev->dev, "Scheduling dummy buffer for node %d\n", node_id); | |
682 | ||
683 | unicam_wr_dma_addr(node, &node->dummy_buf); | |
684 | ||
685 | node->next_frm = NULL; | |
686 | } | |
687 | ||
688 | static void unicam_process_buffer_complete(struct unicam_node *node, | |
689 | unsigned int sequence) | |
690 | { | |
691 | node->cur_frm->vb.field = node->fmt.fmt.pix.field; | |
692 | node->cur_frm->vb.sequence = sequence; | |
693 | ||
694 | vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE); | |
695 | } | |
696 | ||
697 | static void unicam_queue_event_sof(struct unicam_device *unicam) | |
698 | { | |
699 | struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE]; | |
700 | struct v4l2_event event = { | |
701 | .type = V4L2_EVENT_FRAME_SYNC, | |
702 | .u.frame_sync.frame_sequence = unicam->sequence, | |
703 | }; | |
704 | ||
705 | v4l2_event_queue(&node->video_dev, &event); | |
706 | } | |
707 | ||
708 | static irqreturn_t unicam_isr(int irq, void *dev) | |
709 | { | |
710 | struct unicam_device *unicam = dev; | |
711 | unsigned int lines_done = unicam_get_lines_done(dev); | |
712 | unsigned int sequence = unicam->sequence; | |
713 | unsigned int i; | |
714 | u32 ista, sta; | |
715 | bool fe; | |
716 | u64 ts; | |
717 | ||
718 | sta = unicam_reg_read(unicam, UNICAM_STA); | |
719 | /* Write value back to clear the interrupts */ | |
720 | unicam_reg_write(unicam, UNICAM_STA, sta); | |
721 | ||
722 | ista = unicam_reg_read(unicam, UNICAM_ISTA); | |
723 | /* Write value back to clear the interrupts */ | |
724 | unicam_reg_write(unicam, UNICAM_ISTA, ista); | |
725 | ||
726 | dev_dbg(unicam->dev, "ISR: ISTA: 0x%X, STA: 0x%X, sequence %d, lines done %d\n", | |
727 | ista, sta, sequence, lines_done); | |
728 | ||
729 | if (!(sta & (UNICAM_IS | UNICAM_PI0))) | |
730 | return IRQ_HANDLED; | |
731 | ||
732 | /* | |
733 | * Look for either the Frame End interrupt or the Packet Capture status | |
734 | * to signal a frame end. | |
735 | */ | |
736 | fe = ista & UNICAM_FEI || sta & UNICAM_PI0; | |
737 | ||
738 | /* | |
739 | * We must run the frame end handler first. If we have a valid next_frm | |
740 | * and we get a simultaneout FE + FS interrupt, running the FS handler | |
741 | * first would null out the next_frm ptr and we would have lost the | |
742 | * buffer forever. | |
743 | */ | |
744 | if (fe) { | |
745 | /* | |
746 | * Ensure we have swapped buffers already as we can't | |
747 | * stop the peripheral. If no buffer is available, use a | |
748 | * dummy buffer to dump out frames until we get a new buffer | |
749 | * to use. | |
750 | */ | |
751 | for (i = 0; i < ARRAY_SIZE(unicam->node); i++) { | |
752 | struct unicam_node *node = &unicam->node[i]; | |
753 | ||
754 | if (!vb2_start_streaming_called(&node->buffer_queue)) | |
755 | continue; | |
756 | ||
757 | /* | |
758 | * If cur_frm == next_frm, it means we have not had | |
759 | * a chance to swap buffers, likely due to having | |
760 | * multiple interrupts occurring simultaneously (like FE | |
761 | * + FS + LS). In this case, we cannot signal the buffer | |
762 | * as complete, as the HW will reuse that buffer. | |
763 | */ | |
764 | if (node->cur_frm && node->cur_frm != node->next_frm) | |
765 | unicam_process_buffer_complete(node, sequence); | |
766 | node->cur_frm = node->next_frm; | |
767 | } | |
768 | unicam->sequence++; | |
769 | } | |
770 | ||
771 | if (ista & UNICAM_FSI) { | |
772 | /* | |
773 | * Timestamp is to be when the first data byte was captured, | |
774 | * aka frame start. | |
775 | */ | |
776 | ts = ktime_get_ns(); | |
777 | for (i = 0; i < ARRAY_SIZE(unicam->node); i++) { | |
778 | struct unicam_node *node = &unicam->node[i]; | |
779 | ||
780 | if (!vb2_start_streaming_called(&node->buffer_queue)) | |
781 | continue; | |
782 | ||
783 | if (node->cur_frm) | |
784 | node->cur_frm->vb.vb2_buf.timestamp = ts; | |
785 | else | |
786 | dev_dbg(unicam->v4l2_dev.dev, | |
787 | "ISR: [%d] Dropping frame, buffer not available at FS\n", | |
788 | i); | |
789 | /* | |
790 | * Set the next frame output to go to a dummy frame | |
791 | * if we have not managed to obtain another frame | |
792 | * from the queue. | |
793 | */ | |
794 | unicam_schedule_dummy_buffer(node); | |
795 | } | |
796 | ||
797 | unicam_queue_event_sof(unicam); | |
798 | } | |
799 | ||
800 | /* | |
801 | * Cannot swap buffer at frame end, there may be a race condition | |
802 | * where the HW does not actually swap it if the new frame has | |
803 | * already started. | |
804 | */ | |
805 | if (ista & (UNICAM_FSI | UNICAM_LCI) && !fe) { | |
806 | for (i = 0; i < ARRAY_SIZE(unicam->node); i++) { | |
807 | struct unicam_node *node = &unicam->node[i]; | |
808 | ||
809 | if (!vb2_start_streaming_called(&node->buffer_queue)) | |
810 | continue; | |
811 | ||
812 | spin_lock(&node->dma_queue_lock); | |
813 | if (!list_empty(&node->dma_queue) && !node->next_frm) | |
814 | unicam_schedule_next_buffer(node); | |
815 | spin_unlock(&node->dma_queue_lock); | |
816 | } | |
817 | } | |
818 | ||
819 | if (unicam_reg_read(unicam, UNICAM_ICTL) & UNICAM_FCM) { | |
820 | /* Switch out of trigger mode if selected */ | |
821 | unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC); | |
822 | unicam_reg_write_field(unicam, UNICAM_ICTL, 0, UNICAM_FCM); | |
823 | } | |
824 | return IRQ_HANDLED; | |
825 | } | |
826 | ||
827 | static void unicam_set_packing_config(struct unicam_device *unicam, | |
828 | const struct unicam_format_info *fmtinfo) | |
829 | { | |
830 | struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE]; | |
831 | u32 pack, unpack; | |
832 | u32 val; | |
833 | ||
834 | if (node->fmt.fmt.pix.pixelformat == fmtinfo->fourcc) { | |
835 | unpack = UNICAM_PUM_NONE; | |
836 | pack = UNICAM_PPM_NONE; | |
837 | } else { | |
838 | unpack = fmtinfo->unpack; | |
839 | /* Repacking is always to 16bpp */ | |
840 | pack = UNICAM_PPM_PACK16; | |
841 | } | |
842 | ||
843 | val = 0; | |
844 | unicam_set_field(&val, unpack, UNICAM_PUM_MASK); | |
845 | unicam_set_field(&val, pack, UNICAM_PPM_MASK); | |
846 | unicam_reg_write(unicam, UNICAM_IPIPE, val); | |
847 | } | |
848 | ||
849 | static void unicam_cfg_image_id(struct unicam_device *unicam, u8 vc, u8 dt) | |
850 | { | |
851 | if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) { | |
852 | /* CSI2 mode */ | |
853 | unicam_reg_write(unicam, UNICAM_IDI0, (vc << 6) | dt); | |
854 | } else { | |
855 | /* CCP2 mode */ | |
856 | unicam_reg_write(unicam, UNICAM_IDI0, 0x80 | dt); | |
857 | } | |
858 | } | |
859 | ||
860 | static void unicam_enable_ed(struct unicam_device *unicam) | |
861 | { | |
862 | u32 val = unicam_reg_read(unicam, UNICAM_DCS); | |
863 | ||
864 | unicam_set_field(&val, 2, UNICAM_EDL_MASK); | |
865 | /* Do not wrap at the end of the embedded data buffer */ | |
866 | unicam_set_field(&val, 0, UNICAM_DBOB); | |
867 | ||
868 | unicam_reg_write(unicam, UNICAM_DCS, val); | |
869 | } | |
870 | ||
871 | static int unicam_get_image_vc_dt(struct unicam_device *unicam, | |
872 | struct v4l2_subdev_state *state, | |
873 | u8 *vc, u8 *dt) | |
874 | { | |
875 | struct v4l2_mbus_frame_desc fd; | |
876 | u32 stream; | |
877 | int ret; | |
878 | ||
879 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, | |
880 | UNICAM_SD_PAD_SOURCE_IMAGE, | |
881 | 0, NULL, &stream); | |
882 | if (ret) | |
883 | return ret; | |
884 | ||
885 | ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_frame_desc, | |
886 | unicam->sensor.pad->index, &fd); | |
887 | if (ret) | |
888 | return ret; | |
889 | ||
890 | /* Only CSI-2 supports DTs. */ | |
891 | if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) | |
892 | return -EINVAL; | |
893 | ||
894 | for (unsigned int i = 0; i < fd.num_entries; ++i) { | |
895 | const struct v4l2_mbus_frame_desc_entry *fde = &fd.entry[i]; | |
896 | ||
897 | if (fde->stream == stream) { | |
898 | *vc = fde->bus.csi2.vc; | |
899 | *dt = fde->bus.csi2.dt; | |
900 | return 0; | |
901 | } | |
902 | } | |
903 | ||
904 | return -EINVAL; | |
905 | } | |
906 | ||
907 | static void unicam_start_rx(struct unicam_device *unicam, | |
908 | struct v4l2_subdev_state *state) | |
909 | { | |
910 | struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE]; | |
911 | const struct unicam_format_info *fmtinfo; | |
912 | const struct v4l2_mbus_framefmt *fmt; | |
913 | unsigned int line_int_freq; | |
914 | u8 vc, dt; | |
915 | u32 val; | |
916 | int ret; | |
917 | ||
918 | fmt = v4l2_subdev_state_get_format(state, UNICAM_SD_PAD_SOURCE_IMAGE, 0); | |
919 | fmtinfo = unicam_find_format_by_code(fmt->code, | |
920 | UNICAM_SD_PAD_SOURCE_IMAGE); | |
921 | if (WARN_ON(!fmtinfo)) | |
922 | return; | |
923 | ||
924 | /* | |
925 | * Enable lane clocks. The register is structured as follows: | |
926 | * | |
927 | * [9:8] - DAT3 | |
928 | * [7:6] - DAT2 | |
929 | * [5:4] - DAT1 | |
930 | * [3:2] - DAT0 | |
931 | * [1:0] - CLK | |
932 | * | |
933 | * Enabled lane must be set to b01, and disabled lanes to b00. The clock | |
934 | * lane is always enabled. | |
935 | */ | |
936 | val = 0x155 & GENMASK(unicam->pipe.num_data_lanes * 2 + 1, 0); | |
937 | unicam_clk_write(unicam, val); | |
938 | ||
939 | /* Basic init */ | |
940 | unicam_reg_write(unicam, UNICAM_CTRL, UNICAM_MEM); | |
941 | ||
942 | /* Enable analogue control, and leave in reset. */ | |
943 | val = UNICAM_AR; | |
944 | unicam_set_field(&val, 7, UNICAM_CTATADJ_MASK); | |
945 | unicam_set_field(&val, 7, UNICAM_PTATADJ_MASK); | |
946 | unicam_reg_write(unicam, UNICAM_ANA, val); | |
947 | usleep_range(1000, 2000); | |
948 | ||
949 | /* Come out of reset */ | |
950 | unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_AR); | |
951 | ||
952 | /* Peripheral reset */ | |
953 | unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR); | |
954 | unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR); | |
955 | ||
956 | unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE); | |
957 | ||
958 | /* Enable Rx control. */ | |
959 | val = unicam_reg_read(unicam, UNICAM_CTRL); | |
960 | if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) { | |
961 | unicam_set_field(&val, UNICAM_CPM_CSI2, UNICAM_CPM_MASK); | |
962 | unicam_set_field(&val, UNICAM_DCM_STROBE, UNICAM_DCM_MASK); | |
963 | } else { | |
964 | unicam_set_field(&val, UNICAM_CPM_CCP2, UNICAM_CPM_MASK); | |
965 | unicam_set_field(&val, unicam->bus_flags, UNICAM_DCM_MASK); | |
966 | } | |
967 | /* Packet framer timeout */ | |
968 | unicam_set_field(&val, 0xf, UNICAM_PFT_MASK); | |
969 | unicam_set_field(&val, 128, UNICAM_OET_MASK); | |
970 | unicam_reg_write(unicam, UNICAM_CTRL, val); | |
971 | ||
972 | unicam_reg_write(unicam, UNICAM_IHWIN, 0); | |
973 | unicam_reg_write(unicam, UNICAM_IVWIN, 0); | |
974 | ||
975 | /* AXI bus access QoS setup */ | |
976 | val = unicam_reg_read(unicam, UNICAM_PRI); | |
977 | unicam_set_field(&val, 0, UNICAM_BL_MASK); | |
978 | unicam_set_field(&val, 0, UNICAM_BS_MASK); | |
979 | unicam_set_field(&val, 0xe, UNICAM_PP_MASK); | |
980 | unicam_set_field(&val, 8, UNICAM_NP_MASK); | |
981 | unicam_set_field(&val, 2, UNICAM_PT_MASK); | |
982 | unicam_set_field(&val, 1, UNICAM_PE); | |
983 | unicam_reg_write(unicam, UNICAM_PRI, val); | |
984 | ||
985 | unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_DDL); | |
986 | ||
987 | /* Always start in trigger frame capture mode (UNICAM_FCM set) */ | |
988 | val = UNICAM_FSIE | UNICAM_FEIE | UNICAM_FCM | UNICAM_IBOB; | |
989 | line_int_freq = max(fmt->height >> 2, 128); | |
990 | unicam_set_field(&val, line_int_freq, UNICAM_LCIE_MASK); | |
991 | unicam_reg_write(unicam, UNICAM_ICTL, val); | |
992 | unicam_reg_write(unicam, UNICAM_STA, UNICAM_STA_MASK_ALL); | |
993 | unicam_reg_write(unicam, UNICAM_ISTA, UNICAM_ISTA_MASK_ALL); | |
994 | ||
995 | /* tclk_term_en */ | |
996 | unicam_reg_write_field(unicam, UNICAM_CLT, 2, UNICAM_CLT1_MASK); | |
997 | /* tclk_settle */ | |
998 | unicam_reg_write_field(unicam, UNICAM_CLT, 6, UNICAM_CLT2_MASK); | |
999 | /* td_term_en */ | |
1000 | unicam_reg_write_field(unicam, UNICAM_DLT, 2, UNICAM_DLT1_MASK); | |
1001 | /* ths_settle */ | |
1002 | unicam_reg_write_field(unicam, UNICAM_DLT, 6, UNICAM_DLT2_MASK); | |
1003 | /* trx_enable */ | |
1004 | unicam_reg_write_field(unicam, UNICAM_DLT, 0, UNICAM_DLT3_MASK); | |
1005 | ||
1006 | unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_SOE); | |
1007 | ||
1008 | /* Packet compare setup - required to avoid missing frame ends */ | |
1009 | val = 0; | |
1010 | unicam_set_field(&val, 1, UNICAM_PCE); | |
1011 | unicam_set_field(&val, 1, UNICAM_GI); | |
1012 | unicam_set_field(&val, 1, UNICAM_CPH); | |
1013 | unicam_set_field(&val, 0, UNICAM_PCVC_MASK); | |
1014 | unicam_set_field(&val, 1, UNICAM_PCDT_MASK); | |
1015 | unicam_reg_write(unicam, UNICAM_CMP0, val); | |
1016 | ||
1017 | /* Enable clock lane and set up terminations */ | |
1018 | val = 0; | |
1019 | if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) { | |
1020 | /* CSI2 */ | |
1021 | unicam_set_field(&val, 1, UNICAM_CLE); | |
1022 | unicam_set_field(&val, 1, UNICAM_CLLPE); | |
1023 | if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) { | |
1024 | unicam_set_field(&val, 1, UNICAM_CLTRE); | |
1025 | unicam_set_field(&val, 1, UNICAM_CLHSE); | |
1026 | } | |
1027 | } else { | |
1028 | /* CCP2 */ | |
1029 | unicam_set_field(&val, 1, UNICAM_CLE); | |
1030 | unicam_set_field(&val, 1, UNICAM_CLHSE); | |
1031 | unicam_set_field(&val, 1, UNICAM_CLTRE); | |
1032 | } | |
1033 | unicam_reg_write(unicam, UNICAM_CLK, val); | |
1034 | ||
1035 | /* | |
1036 | * Enable required data lanes with appropriate terminations. | |
1037 | * The same value needs to be written to UNICAM_DATn registers for | |
1038 | * the active lanes, and 0 for inactive ones. | |
1039 | */ | |
1040 | val = 0; | |
1041 | if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) { | |
1042 | /* CSI2 */ | |
1043 | unicam_set_field(&val, 1, UNICAM_DLE); | |
1044 | unicam_set_field(&val, 1, UNICAM_DLLPE); | |
1045 | if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) { | |
1046 | unicam_set_field(&val, 1, UNICAM_DLTRE); | |
1047 | unicam_set_field(&val, 1, UNICAM_DLHSE); | |
1048 | } | |
1049 | } else { | |
1050 | /* CCP2 */ | |
1051 | unicam_set_field(&val, 1, UNICAM_DLE); | |
1052 | unicam_set_field(&val, 1, UNICAM_DLHSE); | |
1053 | unicam_set_field(&val, 1, UNICAM_DLTRE); | |
1054 | } | |
1055 | unicam_reg_write(unicam, UNICAM_DAT0, val); | |
1056 | ||
1057 | if (unicam->pipe.num_data_lanes == 1) | |
1058 | val = 0; | |
1059 | unicam_reg_write(unicam, UNICAM_DAT1, val); | |
1060 | ||
1061 | if (unicam->max_data_lanes > 2) { | |
1062 | /* | |
1063 | * Registers UNICAM_DAT2 and UNICAM_DAT3 only valid if the | |
1064 | * instance supports more than 2 data lanes. | |
1065 | */ | |
1066 | if (unicam->pipe.num_data_lanes == 2) | |
1067 | val = 0; | |
1068 | unicam_reg_write(unicam, UNICAM_DAT2, val); | |
1069 | ||
1070 | if (unicam->pipe.num_data_lanes == 3) | |
1071 | val = 0; | |
1072 | unicam_reg_write(unicam, UNICAM_DAT3, val); | |
1073 | } | |
1074 | ||
1075 | unicam_reg_write(unicam, UNICAM_IBLS, | |
1076 | node->fmt.fmt.pix.bytesperline); | |
1077 | unicam_wr_dma_addr(node, node->cur_frm); | |
1078 | unicam_set_packing_config(unicam, fmtinfo); | |
1079 | ||
1080 | ret = unicam_get_image_vc_dt(unicam, state, &vc, &dt); | |
1081 | if (ret) { | |
1082 | /* | |
1083 | * If the source doesn't support frame descriptors, default to | |
1084 | * VC 0 and use the DT corresponding to the format. | |
1085 | */ | |
1086 | vc = 0; | |
1087 | dt = fmtinfo->csi_dt; | |
1088 | } | |
1089 | ||
1090 | unicam_cfg_image_id(unicam, vc, dt); | |
1091 | ||
1092 | val = unicam_reg_read(unicam, UNICAM_MISC); | |
1093 | unicam_set_field(&val, 1, UNICAM_FL0); | |
1094 | unicam_set_field(&val, 1, UNICAM_FL1); | |
1095 | unicam_reg_write(unicam, UNICAM_MISC, val); | |
1096 | ||
1097 | /* Enable peripheral */ | |
1098 | unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPE); | |
1099 | ||
1100 | /* Load image pointers */ | |
1101 | unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_LIP_MASK); | |
1102 | ||
1103 | /* | |
1104 | * Enable trigger only for the first frame to | |
1105 | * sync correctly to the FS from the source. | |
1106 | */ | |
1107 | unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC); | |
1108 | } | |
1109 | ||
1110 | static void unicam_start_metadata(struct unicam_device *unicam) | |
1111 | { | |
1112 | struct unicam_node *node = &unicam->node[UNICAM_METADATA_NODE]; | |
1113 | ||
1114 | unicam_enable_ed(unicam); | |
1115 | unicam_wr_dma_addr(node, node->cur_frm); | |
1116 | unicam_reg_write_field(unicam, UNICAM_DCS, 1, UNICAM_LDP); | |
1117 | } | |
1118 | ||
1119 | static void unicam_disable(struct unicam_device *unicam) | |
1120 | { | |
1121 | /* Analogue lane control disable */ | |
1122 | unicam_reg_write_field(unicam, UNICAM_ANA, 1, UNICAM_DDL); | |
1123 | ||
1124 | /* Stop the output engine */ | |
1125 | unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_SOE); | |
1126 | ||
1127 | /* Disable the data lanes. */ | |
1128 | unicam_reg_write(unicam, UNICAM_DAT0, 0); | |
1129 | unicam_reg_write(unicam, UNICAM_DAT1, 0); | |
1130 | ||
1131 | if (unicam->max_data_lanes > 2) { | |
1132 | unicam_reg_write(unicam, UNICAM_DAT2, 0); | |
1133 | unicam_reg_write(unicam, UNICAM_DAT3, 0); | |
1134 | } | |
1135 | ||
1136 | /* Peripheral reset */ | |
1137 | unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR); | |
1138 | usleep_range(50, 100); | |
1139 | unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR); | |
1140 | ||
1141 | /* Disable peripheral */ | |
1142 | unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE); | |
1143 | ||
1144 | /* Clear ED setup */ | |
1145 | unicam_reg_write(unicam, UNICAM_DCS, 0); | |
1146 | ||
1147 | /* Disable all lane clocks */ | |
1148 | unicam_clk_write(unicam, 0); | |
1149 | } | |
1150 | ||
1151 | /* ----------------------------------------------------------------------------- | |
1152 | * V4L2 subdev operations | |
1153 | */ | |
1154 | ||
1155 | static int __unicam_subdev_set_routing(struct v4l2_subdev *sd, | |
1156 | struct v4l2_subdev_state *state, | |
1157 | struct v4l2_subdev_krouting *routing) | |
1158 | { | |
1159 | struct v4l2_subdev_route *route; | |
1160 | int ret; | |
1161 | ||
1162 | ret = v4l2_subdev_routing_validate(sd, routing, | |
1163 | V4L2_SUBDEV_ROUTING_ONLY_1_TO_1); | |
1164 | if (ret) | |
1165 | return ret; | |
1166 | ||
1167 | ret = v4l2_subdev_set_routing(sd, state, routing); | |
1168 | if (ret) | |
1169 | return ret; | |
1170 | ||
1171 | for_each_active_route(&state->routing, route) { | |
1172 | const struct v4l2_mbus_framefmt *def_fmt; | |
1173 | struct v4l2_mbus_framefmt *fmt; | |
1174 | ||
1175 | if (route->source_pad == UNICAM_SD_PAD_SOURCE_IMAGE) | |
1176 | def_fmt = &unicam_default_image_format; | |
1177 | else | |
1178 | def_fmt = &unicam_default_meta_format; | |
1179 | ||
1180 | fmt = v4l2_subdev_state_get_format(state, route->sink_pad, | |
1181 | route->sink_stream); | |
1182 | *fmt = *def_fmt; | |
1183 | fmt = v4l2_subdev_state_get_format(state, route->source_pad, | |
1184 | route->source_stream); | |
1185 | *fmt = *def_fmt; | |
1186 | } | |
1187 | ||
1188 | return 0; | |
1189 | } | |
1190 | ||
1191 | static int unicam_subdev_init_state(struct v4l2_subdev *sd, | |
1192 | struct v4l2_subdev_state *state) | |
1193 | { | |
1194 | struct v4l2_subdev_route routes[] = { | |
1195 | { | |
1196 | .sink_pad = UNICAM_SD_PAD_SINK, | |
1197 | .sink_stream = 0, | |
1198 | .source_pad = UNICAM_SD_PAD_SOURCE_IMAGE, | |
1199 | .source_stream = 0, | |
1200 | .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE, | |
1201 | }, | |
1202 | }; | |
1203 | ||
1204 | struct v4l2_subdev_krouting routing = { | |
1205 | .len_routes = ARRAY_SIZE(routes), | |
1206 | .num_routes = ARRAY_SIZE(routes), | |
1207 | .routes = routes, | |
1208 | }; | |
1209 | ||
1210 | /* Initialize routing to single route to the fist source pad. */ | |
1211 | return __unicam_subdev_set_routing(sd, state, &routing); | |
1212 | } | |
1213 | ||
1214 | static int unicam_subdev_enum_mbus_code(struct v4l2_subdev *sd, | |
1215 | struct v4l2_subdev_state *state, | |
1216 | struct v4l2_subdev_mbus_code_enum *code) | |
1217 | { | |
1218 | u32 pad, stream; | |
1219 | int ret; | |
1220 | ||
1221 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, | |
1222 | code->pad, code->stream, | |
1223 | &pad, &stream); | |
1224 | if (ret) | |
1225 | return ret; | |
1226 | ||
1227 | if (unicam_sd_pad_is_source(code->pad)) { | |
1228 | /* No transcoding, source and sink codes must match. */ | |
1229 | const struct v4l2_mbus_framefmt *fmt; | |
1230 | ||
1231 | fmt = v4l2_subdev_state_get_format(state, pad, stream); | |
1232 | if (!fmt) | |
1233 | return -EINVAL; | |
1234 | ||
1235 | if (code->index > 0) | |
1236 | return -EINVAL; | |
1237 | ||
1238 | code->code = fmt->code; | |
1239 | } else { | |
1240 | const struct unicam_format_info *formats; | |
1241 | unsigned int num_formats; | |
1242 | ||
1243 | if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) { | |
1244 | formats = unicam_image_formats; | |
1245 | num_formats = ARRAY_SIZE(unicam_image_formats); | |
1246 | } else { | |
1247 | formats = unicam_meta_formats; | |
1248 | num_formats = ARRAY_SIZE(unicam_meta_formats); | |
1249 | } | |
1250 | ||
1251 | if (code->index >= num_formats) | |
1252 | return -EINVAL; | |
1253 | ||
1254 | code->code = formats[code->index].code; | |
1255 | } | |
1256 | ||
1257 | return 0; | |
1258 | } | |
1259 | ||
1260 | static int unicam_subdev_enum_frame_size(struct v4l2_subdev *sd, | |
1261 | struct v4l2_subdev_state *state, | |
1262 | struct v4l2_subdev_frame_size_enum *fse) | |
1263 | { | |
1264 | u32 pad, stream; | |
1265 | int ret; | |
1266 | ||
1267 | if (fse->index > 0) | |
1268 | return -EINVAL; | |
1269 | ||
1270 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, fse->pad, | |
1271 | fse->stream, &pad, | |
1272 | &stream); | |
1273 | if (ret) | |
1274 | return ret; | |
1275 | ||
1276 | if (unicam_sd_pad_is_source(fse->pad)) { | |
1277 | /* No transcoding, source and sink formats must match. */ | |
1278 | const struct v4l2_mbus_framefmt *fmt; | |
1279 | ||
1280 | fmt = v4l2_subdev_state_get_format(state, pad, stream); | |
1281 | if (!fmt) | |
1282 | return -EINVAL; | |
1283 | ||
1284 | if (fse->code != fmt->code) | |
1285 | return -EINVAL; | |
1286 | ||
1287 | fse->min_width = fmt->width; | |
1288 | fse->max_width = fmt->width; | |
1289 | fse->min_height = fmt->height; | |
1290 | fse->max_height = fmt->height; | |
1291 | } else { | |
1292 | const struct unicam_format_info *fmtinfo; | |
1293 | ||
1294 | fmtinfo = unicam_find_format_by_code(fse->code, pad); | |
1295 | if (!fmtinfo) | |
1296 | return -EINVAL; | |
1297 | ||
1298 | if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) { | |
1299 | fse->min_width = UNICAM_IMAGE_MIN_WIDTH; | |
1300 | fse->max_width = UNICAM_IMAGE_MAX_WIDTH; | |
1301 | fse->min_height = UNICAM_IMAGE_MIN_HEIGHT; | |
1302 | fse->max_height = UNICAM_IMAGE_MAX_HEIGHT; | |
1303 | } else { | |
1304 | fse->min_width = UNICAM_META_MIN_WIDTH; | |
1305 | fse->max_width = UNICAM_META_MAX_WIDTH; | |
1306 | fse->min_height = UNICAM_META_MIN_HEIGHT; | |
1307 | fse->max_height = UNICAM_META_MAX_HEIGHT; | |
1308 | } | |
1309 | } | |
1310 | ||
1311 | return 0; | |
1312 | } | |
1313 | ||
1314 | static int unicam_subdev_set_format(struct v4l2_subdev *sd, | |
1315 | struct v4l2_subdev_state *state, | |
1316 | struct v4l2_subdev_format *format) | |
1317 | { | |
1318 | struct unicam_device *unicam = sd_to_unicam_device(sd); | |
1319 | struct v4l2_mbus_framefmt *sink_format, *source_format; | |
1320 | const struct unicam_format_info *fmtinfo; | |
1321 | u32 source_pad, source_stream; | |
1322 | int ret; | |
1323 | ||
1324 | if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && | |
1325 | unicam->subdev.enabled_streams) | |
1326 | return -EBUSY; | |
1327 | ||
1328 | /* No transcoding, source and sink formats must match. */ | |
1329 | if (unicam_sd_pad_is_source(format->pad)) | |
1330 | return v4l2_subdev_get_fmt(sd, state, format); | |
1331 | ||
1332 | /* | |
1333 | * Allowed formats for the stream on the sink pad depend on what source | |
1334 | * pad the stream is routed to. Find the corresponding source pad and | |
1335 | * use it to validate the media bus code. | |
1336 | */ | |
1337 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, | |
1338 | format->pad, format->stream, | |
1339 | &source_pad, &source_stream); | |
1340 | if (ret) | |
1341 | return ret; | |
1342 | ||
1343 | fmtinfo = unicam_find_format_by_code(format->format.code, source_pad); | |
1344 | if (!fmtinfo) { | |
1345 | fmtinfo = source_pad == UNICAM_SD_PAD_SOURCE_IMAGE | |
1346 | ? &unicam_image_formats[0] : &unicam_meta_formats[0]; | |
1347 | format->format.code = fmtinfo->code; | |
1348 | } | |
1349 | ||
1350 | if (source_pad == UNICAM_SD_PAD_SOURCE_IMAGE) { | |
1351 | format->format.width = clamp_t(unsigned int, | |
1352 | format->format.width, | |
1353 | UNICAM_IMAGE_MIN_WIDTH, | |
1354 | UNICAM_IMAGE_MAX_WIDTH); | |
1355 | format->format.height = clamp_t(unsigned int, | |
1356 | format->format.height, | |
1357 | UNICAM_IMAGE_MIN_HEIGHT, | |
1358 | UNICAM_IMAGE_MAX_HEIGHT); | |
1359 | format->format.field = V4L2_FIELD_NONE; | |
1360 | } else { | |
1361 | format->format.width = clamp_t(unsigned int, | |
1362 | format->format.width, | |
1363 | UNICAM_META_MIN_WIDTH, | |
1364 | UNICAM_META_MAX_WIDTH); | |
1365 | format->format.height = clamp_t(unsigned int, | |
1366 | format->format.height, | |
1367 | UNICAM_META_MIN_HEIGHT, | |
1368 | UNICAM_META_MAX_HEIGHT); | |
1369 | format->format.field = V4L2_FIELD_NONE; | |
1370 | ||
1371 | /* Colorspace don't apply to metadata. */ | |
1372 | format->format.colorspace = 0; | |
1373 | format->format.ycbcr_enc = 0; | |
1374 | format->format.quantization = 0; | |
1375 | format->format.xfer_func = 0; | |
1376 | } | |
1377 | ||
1378 | sink_format = v4l2_subdev_state_get_format(state, format->pad, | |
1379 | format->stream); | |
1380 | source_format = v4l2_subdev_state_get_format(state, source_pad, | |
1381 | source_stream); | |
1382 | *sink_format = format->format; | |
1383 | *source_format = format->format; | |
1384 | ||
1385 | return 0; | |
1386 | } | |
1387 | ||
1388 | static int unicam_subdev_set_routing(struct v4l2_subdev *sd, | |
1389 | struct v4l2_subdev_state *state, | |
1390 | enum v4l2_subdev_format_whence which, | |
1391 | struct v4l2_subdev_krouting *routing) | |
1392 | { | |
1393 | struct unicam_device *unicam = sd_to_unicam_device(sd); | |
1394 | ||
1395 | if (which == V4L2_SUBDEV_FORMAT_ACTIVE && unicam->subdev.enabled_streams) | |
1396 | return -EBUSY; | |
1397 | ||
1398 | return __unicam_subdev_set_routing(sd, state, routing); | |
1399 | } | |
1400 | ||
1401 | static int unicam_sd_enable_streams(struct v4l2_subdev *sd, | |
1402 | struct v4l2_subdev_state *state, u32 pad, | |
1403 | u64 streams_mask) | |
1404 | { | |
1405 | struct unicam_device *unicam = sd_to_unicam_device(sd); | |
1406 | u32 other_pad, other_stream; | |
1407 | int ret; | |
1408 | ||
1409 | if (!unicam->subdev.enabled_streams) { | |
1410 | /* Configure and start Unicam. */ | |
1411 | unicam->sequence = 0; | |
1412 | ||
1413 | if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE)) | |
1414 | unicam_start_metadata(unicam); | |
1415 | ||
1416 | unicam_start_rx(unicam, state); | |
1417 | } | |
1418 | ||
1419 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0, | |
1420 | &other_pad, &other_stream); | |
1421 | if (ret) | |
1422 | return ret; | |
1423 | ||
1424 | ret = v4l2_subdev_enable_streams(unicam->sensor.subdev, | |
1425 | unicam->sensor.pad->index, | |
1426 | BIT(other_stream)); | |
1427 | if (ret) { | |
1428 | dev_err(unicam->dev, "stream on failed in subdev\n"); | |
1429 | return ret; | |
1430 | } | |
1431 | ||
1432 | unicam->subdev.enabled_streams |= BIT(other_stream); | |
1433 | ||
1434 | return 0; | |
1435 | } | |
1436 | ||
1437 | static int unicam_sd_disable_streams(struct v4l2_subdev *sd, | |
1438 | struct v4l2_subdev_state *state, u32 pad, | |
1439 | u64 streams_mask) | |
1440 | { | |
1441 | struct unicam_device *unicam = sd_to_unicam_device(sd); | |
1442 | u32 other_pad, other_stream; | |
1443 | int ret; | |
1444 | ||
1445 | ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0, | |
1446 | &other_pad, &other_stream); | |
1447 | if (ret) | |
1448 | return ret; | |
1449 | ||
1450 | v4l2_subdev_disable_streams(unicam->sensor.subdev, | |
1451 | unicam->sensor.pad->index, | |
1452 | BIT(other_stream)); | |
1453 | ||
1454 | unicam->subdev.enabled_streams &= ~BIT(other_stream); | |
1455 | ||
1456 | if (!unicam->subdev.enabled_streams) | |
1457 | unicam_disable(unicam); | |
1458 | ||
1459 | return 0; | |
1460 | } | |
1461 | ||
1462 | static const struct v4l2_subdev_pad_ops unicam_subdev_pad_ops = { | |
1463 | .enum_mbus_code = unicam_subdev_enum_mbus_code, | |
1464 | .enum_frame_size = unicam_subdev_enum_frame_size, | |
1465 | .get_fmt = v4l2_subdev_get_fmt, | |
1466 | .set_fmt = unicam_subdev_set_format, | |
1467 | .set_routing = unicam_subdev_set_routing, | |
1468 | .enable_streams = unicam_sd_enable_streams, | |
1469 | .disable_streams = unicam_sd_disable_streams, | |
1470 | }; | |
1471 | ||
1472 | static const struct v4l2_subdev_ops unicam_subdev_ops = { | |
1473 | .pad = &unicam_subdev_pad_ops, | |
1474 | }; | |
1475 | ||
1476 | static const struct v4l2_subdev_internal_ops unicam_subdev_internal_ops = { | |
1477 | .init_state = unicam_subdev_init_state, | |
1478 | }; | |
1479 | ||
1480 | static const struct media_entity_operations unicam_subdev_media_ops = { | |
1481 | .link_validate = v4l2_subdev_link_validate, | |
1482 | .has_pad_interdep = v4l2_subdev_has_pad_interdep, | |
1483 | }; | |
1484 | ||
1485 | static int unicam_subdev_init(struct unicam_device *unicam) | |
1486 | { | |
1487 | struct v4l2_subdev *sd = &unicam->subdev.sd; | |
1488 | int ret; | |
1489 | ||
1490 | v4l2_subdev_init(sd, &unicam_subdev_ops); | |
1491 | sd->internal_ops = &unicam_subdev_internal_ops; | |
1492 | v4l2_set_subdevdata(sd, unicam); | |
1493 | ||
1494 | sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; | |
1495 | sd->entity.ops = &unicam_subdev_media_ops; | |
1496 | sd->dev = unicam->dev; | |
1497 | sd->owner = THIS_MODULE; | |
1498 | sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS; | |
1499 | ||
1500 | strscpy(sd->name, "unicam", sizeof(sd->name)); | |
1501 | ||
1502 | unicam->subdev.pads[UNICAM_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK; | |
1503 | unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_IMAGE].flags = MEDIA_PAD_FL_SOURCE; | |
1504 | unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_METADATA].flags = MEDIA_PAD_FL_SOURCE; | |
1505 | ||
1506 | ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(unicam->subdev.pads), | |
1507 | unicam->subdev.pads); | |
1508 | if (ret) { | |
1509 | dev_err(unicam->dev, "Failed to initialize media entity: %d\n", | |
1510 | ret); | |
1511 | return ret; | |
1512 | } | |
1513 | ||
1514 | ret = v4l2_subdev_init_finalize(sd); | |
1515 | if (ret) { | |
1516 | dev_err(unicam->dev, "Failed to initialize subdev: %d\n", ret); | |
1517 | goto err_entity; | |
1518 | } | |
1519 | ||
1520 | ret = v4l2_device_register_subdev(&unicam->v4l2_dev, sd); | |
1521 | if (ret) { | |
1522 | dev_err(unicam->dev, "Failed to register subdev: %d\n", ret); | |
1523 | goto err_subdev; | |
1524 | } | |
1525 | ||
1526 | return 0; | |
1527 | ||
1528 | err_subdev: | |
1529 | v4l2_subdev_cleanup(sd); | |
1530 | err_entity: | |
1531 | media_entity_cleanup(&sd->entity); | |
1532 | return ret; | |
1533 | } | |
1534 | ||
1535 | static void unicam_subdev_cleanup(struct unicam_device *unicam) | |
1536 | { | |
1537 | v4l2_subdev_cleanup(&unicam->subdev.sd); | |
1538 | media_entity_cleanup(&unicam->subdev.sd.entity); | |
1539 | } | |
1540 | ||
1541 | /* ----------------------------------------------------------------------------- | |
1542 | * Videobuf2 queue operations | |
1543 | */ | |
1544 | ||
1545 | static int unicam_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers, | |
1546 | unsigned int *nplanes, unsigned int sizes[], | |
1547 | struct device *alloc_devs[]) | |
1548 | { | |
1549 | struct unicam_node *node = vb2_get_drv_priv(vq); | |
1550 | u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage | |
1551 | : node->fmt.fmt.meta.buffersize; | |
1552 | ||
1553 | if (*nplanes) { | |
1554 | if (sizes[0] < size) { | |
1555 | dev_dbg(node->dev->dev, "sizes[0] %i < size %u\n", | |
1556 | sizes[0], size); | |
1557 | return -EINVAL; | |
1558 | } | |
1559 | size = sizes[0]; | |
1560 | } | |
1561 | ||
1562 | *nplanes = 1; | |
1563 | sizes[0] = size; | |
1564 | ||
1565 | return 0; | |
1566 | } | |
1567 | ||
1568 | static int unicam_buffer_prepare(struct vb2_buffer *vb) | |
1569 | { | |
1570 | struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue); | |
1571 | struct unicam_buffer *buf = to_unicam_buffer(vb); | |
1572 | u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage | |
1573 | : node->fmt.fmt.meta.buffersize; | |
1574 | ||
1575 | if (vb2_plane_size(vb, 0) < size) { | |
1576 | dev_dbg(node->dev->dev, | |
1577 | "data will not fit into plane (%lu < %u)\n", | |
1578 | vb2_plane_size(vb, 0), size); | |
1579 | return -EINVAL; | |
1580 | } | |
1581 | ||
1582 | buf->dma_addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0); | |
1583 | buf->size = size; | |
1584 | ||
1585 | vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size); | |
1586 | ||
1587 | return 0; | |
1588 | } | |
1589 | ||
1590 | static void unicam_return_buffers(struct unicam_node *node, | |
1591 | enum vb2_buffer_state state) | |
1592 | { | |
1593 | struct unicam_buffer *buf, *tmp; | |
1594 | ||
1595 | list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) { | |
1596 | list_del(&buf->list); | |
1597 | vb2_buffer_done(&buf->vb.vb2_buf, state); | |
1598 | } | |
1599 | ||
1600 | if (node->cur_frm) | |
1601 | vb2_buffer_done(&node->cur_frm->vb.vb2_buf, | |
1602 | state); | |
1603 | if (node->next_frm && node->cur_frm != node->next_frm) | |
1604 | vb2_buffer_done(&node->next_frm->vb.vb2_buf, | |
1605 | state); | |
1606 | ||
1607 | node->cur_frm = NULL; | |
1608 | node->next_frm = NULL; | |
1609 | } | |
1610 | ||
1611 | static int unicam_num_data_lanes(struct unicam_device *unicam) | |
1612 | { | |
1613 | struct v4l2_mbus_config mbus_config = { 0 }; | |
1614 | unsigned int num_data_lanes; | |
1615 | int ret; | |
1616 | ||
1617 | if (unicam->bus_type != V4L2_MBUS_CSI2_DPHY) | |
1618 | return unicam->max_data_lanes; | |
1619 | ||
1620 | ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_mbus_config, | |
1621 | unicam->sensor.pad->index, &mbus_config); | |
1622 | if (ret == -ENOIOCTLCMD) | |
1623 | return unicam->max_data_lanes; | |
1624 | ||
1625 | if (ret < 0) { | |
1626 | dev_err(unicam->dev, "Failed to get mbus config: %d\n", ret); | |
1627 | return ret; | |
1628 | } | |
1629 | ||
1630 | num_data_lanes = mbus_config.bus.mipi_csi2.num_data_lanes; | |
1631 | ||
1632 | if (num_data_lanes != 1 && num_data_lanes != 2 && num_data_lanes != 4) { | |
1633 | dev_err(unicam->dev, | |
1634 | "Device %s has requested %u data lanes, invalid\n", | |
1635 | unicam->sensor.subdev->name, num_data_lanes); | |
1636 | return -EINVAL; | |
1637 | } | |
1638 | ||
1639 | if (num_data_lanes > unicam->max_data_lanes) { | |
1640 | dev_err(unicam->dev, | |
1641 | "Device %s has requested %u data lanes, >%u configured in DT\n", | |
1642 | unicam->sensor.subdev->name, num_data_lanes, | |
1643 | unicam->max_data_lanes); | |
1644 | return -EINVAL; | |
1645 | } | |
1646 | ||
1647 | return num_data_lanes; | |
1648 | } | |
1649 | ||
1650 | static int unicam_start_streaming(struct vb2_queue *vq, unsigned int count) | |
1651 | { | |
1652 | struct unicam_node *node = vb2_get_drv_priv(vq); | |
1653 | struct unicam_device *unicam = node->dev; | |
1654 | struct unicam_buffer *buf; | |
1655 | struct media_pipeline_pad_iter iter; | |
1656 | struct media_pad *pad; | |
1657 | unsigned long flags; | |
1658 | int ret; | |
1659 | ||
1660 | dev_dbg(unicam->dev, "Starting stream on %s device\n", | |
1661 | is_metadata_node(node) ? "metadata" : "image"); | |
1662 | ||
1663 | /* | |
1664 | * Start the pipeline. This validates all links, and populates the | |
1665 | * pipeline structure. | |
1666 | */ | |
1667 | ret = video_device_pipeline_start(&node->video_dev, &unicam->pipe.pipe); | |
1668 | if (ret < 0) { | |
1669 | dev_dbg(unicam->dev, "Failed to start media pipeline: %d\n", ret); | |
1670 | goto err_buffers; | |
1671 | } | |
1672 | ||
1673 | /* | |
1674 | * Determine which video nodes are included in the pipeline, and get the | |
1675 | * number of data lanes. | |
1676 | */ | |
1677 | if (unicam->pipe.pipe.start_count == 1) { | |
1678 | unicam->pipe.nodes = 0; | |
1679 | ||
1680 | media_pipeline_for_each_pad(&unicam->pipe.pipe, &iter, pad) { | |
1681 | if (pad->entity != &unicam->subdev.sd.entity) | |
1682 | continue; | |
1683 | ||
1684 | if (pad->index == UNICAM_SD_PAD_SOURCE_IMAGE) | |
1685 | unicam->pipe.nodes |= BIT(UNICAM_IMAGE_NODE); | |
1686 | else if (pad->index == UNICAM_SD_PAD_SOURCE_METADATA) | |
1687 | unicam->pipe.nodes |= BIT(UNICAM_METADATA_NODE); | |
1688 | } | |
1689 | ||
1690 | if (!(unicam->pipe.nodes & BIT(UNICAM_IMAGE_NODE))) { | |
1691 | dev_dbg(unicam->dev, | |
1692 | "Pipeline does not include image node\n"); | |
1693 | ret = -EPIPE; | |
1694 | goto err_pipeline; | |
1695 | } | |
1696 | ||
1697 | ret = unicam_num_data_lanes(unicam); | |
1698 | if (ret < 0) | |
1699 | goto err_pipeline; | |
1700 | ||
1701 | unicam->pipe.num_data_lanes = ret; | |
1702 | ||
1703 | dev_dbg(unicam->dev, "Running with %u data lanes, nodes %u\n", | |
1704 | unicam->pipe.num_data_lanes, unicam->pipe.nodes); | |
1705 | } | |
1706 | ||
1707 | /* Arm the node with the first buffer from the DMA queue. */ | |
1708 | spin_lock_irqsave(&node->dma_queue_lock, flags); | |
1709 | buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list); | |
1710 | node->cur_frm = buf; | |
1711 | node->next_frm = buf; | |
1712 | list_del(&buf->list); | |
1713 | spin_unlock_irqrestore(&node->dma_queue_lock, flags); | |
1714 | ||
1715 | /* | |
1716 | * Wait for all the video devices in the pipeline to have been started | |
1717 | * before starting the hardware. In the general case, this would | |
1718 | * prevent capturing multiple streams independently. However, the | |
1719 | * Unicam DMA engines are not generic, they have been designed to | |
1720 | * capture image data and embedded data from the same camera sensor. | |
1721 | * Not only does the main use case not benefit from independent | |
1722 | * capture, it requires proper synchronization of the streams at start | |
1723 | * time. | |
1724 | */ | |
1725 | if (unicam->pipe.pipe.start_count < hweight32(unicam->pipe.nodes)) | |
1726 | return 0; | |
1727 | ||
1728 | ret = pm_runtime_resume_and_get(unicam->dev); | |
1729 | if (ret < 0) { | |
1730 | dev_err(unicam->dev, "PM runtime resume failed: %d\n", ret); | |
1731 | goto err_pipeline; | |
1732 | } | |
1733 | ||
1734 | /* Enable the streams on the source. */ | |
1735 | ret = v4l2_subdev_enable_streams(&unicam->subdev.sd, | |
1736 | UNICAM_SD_PAD_SOURCE_IMAGE, | |
1737 | BIT(0)); | |
1738 | if (ret < 0) { | |
1739 | dev_err(unicam->dev, "stream on failed in subdev\n"); | |
1740 | goto err_pm_put; | |
1741 | } | |
1742 | ||
1743 | if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE)) { | |
1744 | ret = v4l2_subdev_enable_streams(&unicam->subdev.sd, | |
1745 | UNICAM_SD_PAD_SOURCE_METADATA, | |
1746 | BIT(0)); | |
1747 | if (ret < 0) { | |
1748 | dev_err(unicam->dev, "stream on failed in subdev\n"); | |
1749 | goto err_disable_streams; | |
1750 | } | |
1751 | } | |
1752 | ||
1753 | return 0; | |
1754 | ||
1755 | err_disable_streams: | |
1756 | v4l2_subdev_disable_streams(&unicam->subdev.sd, | |
1757 | UNICAM_SD_PAD_SOURCE_IMAGE, BIT(0)); | |
1758 | err_pm_put: | |
1759 | pm_runtime_put_sync(unicam->dev); | |
1760 | err_pipeline: | |
1761 | video_device_pipeline_stop(&node->video_dev); | |
1762 | err_buffers: | |
1763 | unicam_return_buffers(node, VB2_BUF_STATE_QUEUED); | |
1764 | return ret; | |
1765 | } | |
1766 | ||
1767 | static void unicam_stop_streaming(struct vb2_queue *vq) | |
1768 | { | |
1769 | struct unicam_node *node = vb2_get_drv_priv(vq); | |
1770 | struct unicam_device *unicam = node->dev; | |
1771 | ||
1772 | /* Stop the hardware when the first video device gets stopped. */ | |
1773 | if (unicam->pipe.pipe.start_count == hweight32(unicam->pipe.nodes)) { | |
1774 | if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE)) | |
1775 | v4l2_subdev_disable_streams(&unicam->subdev.sd, | |
1776 | UNICAM_SD_PAD_SOURCE_METADATA, | |
1777 | BIT(0)); | |
1778 | ||
1779 | v4l2_subdev_disable_streams(&unicam->subdev.sd, | |
1780 | UNICAM_SD_PAD_SOURCE_IMAGE, | |
1781 | BIT(0)); | |
1782 | ||
1783 | pm_runtime_put(unicam->dev); | |
1784 | } | |
1785 | ||
1786 | video_device_pipeline_stop(&node->video_dev); | |
1787 | ||
1788 | /* Clear all queued buffers for the node */ | |
1789 | unicam_return_buffers(node, VB2_BUF_STATE_ERROR); | |
1790 | } | |
1791 | ||
1792 | static void unicam_buffer_queue(struct vb2_buffer *vb) | |
1793 | { | |
1794 | struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue); | |
1795 | struct unicam_buffer *buf = to_unicam_buffer(vb); | |
1796 | ||
1797 | spin_lock_irq(&node->dma_queue_lock); | |
1798 | list_add_tail(&buf->list, &node->dma_queue); | |
1799 | spin_unlock_irq(&node->dma_queue_lock); | |
1800 | } | |
1801 | ||
1802 | static const struct vb2_ops unicam_video_qops = { | |
1803 | .queue_setup = unicam_queue_setup, | |
1804 | .wait_prepare = vb2_ops_wait_prepare, | |
1805 | .wait_finish = vb2_ops_wait_finish, | |
1806 | .buf_prepare = unicam_buffer_prepare, | |
1807 | .start_streaming = unicam_start_streaming, | |
1808 | .stop_streaming = unicam_stop_streaming, | |
1809 | .buf_queue = unicam_buffer_queue, | |
1810 | }; | |
1811 | ||
1812 | /* ----------------------------------------------------------------------------- | |
1813 | * V4L2 video device operations | |
1814 | */ | |
1815 | ||
1816 | static int unicam_querycap(struct file *file, void *priv, | |
1817 | struct v4l2_capability *cap) | |
1818 | { | |
1819 | strscpy(cap->driver, UNICAM_MODULE_NAME, sizeof(cap->driver)); | |
1820 | strscpy(cap->card, UNICAM_MODULE_NAME, sizeof(cap->card)); | |
1821 | ||
1822 | cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE; | |
1823 | ||
1824 | return 0; | |
1825 | } | |
1826 | ||
1827 | static int unicam_enum_fmt_vid(struct file *file, void *priv, | |
1828 | struct v4l2_fmtdesc *f) | |
1829 | { | |
1830 | unsigned int index; | |
1831 | unsigned int i; | |
1832 | ||
1833 | for (i = 0, index = 0; i < ARRAY_SIZE(unicam_image_formats); i++) { | |
1834 | if (f->mbus_code && unicam_image_formats[i].code != f->mbus_code) | |
1835 | continue; | |
1836 | ||
1837 | if (index == f->index) { | |
1838 | f->pixelformat = unicam_image_formats[i].fourcc; | |
1839 | return 0; | |
1840 | } | |
1841 | ||
1842 | index++; | |
1843 | ||
1844 | if (!unicam_image_formats[i].unpacked_fourcc) | |
1845 | continue; | |
1846 | ||
1847 | if (index == f->index) { | |
1848 | f->pixelformat = unicam_image_formats[i].unpacked_fourcc; | |
1849 | return 0; | |
1850 | } | |
1851 | ||
1852 | index++; | |
1853 | } | |
1854 | ||
1855 | return -EINVAL; | |
1856 | } | |
1857 | ||
1858 | static int unicam_g_fmt_vid(struct file *file, void *priv, | |
1859 | struct v4l2_format *f) | |
1860 | { | |
1861 | struct unicam_node *node = video_drvdata(file); | |
1862 | ||
1863 | *f = node->fmt; | |
1864 | ||
1865 | return 0; | |
1866 | } | |
1867 | ||
1868 | static void __unicam_try_fmt_vid(struct unicam_node *node, | |
1869 | struct v4l2_pix_format *pix) | |
1870 | { | |
1871 | const struct unicam_format_info *fmtinfo; | |
1872 | ||
1873 | /* | |
1874 | * Default to the first format if the requested pixel format code isn't | |
1875 | * supported. | |
1876 | */ | |
1877 | fmtinfo = unicam_find_format_by_fourcc(pix->pixelformat, | |
1878 | UNICAM_SD_PAD_SOURCE_IMAGE); | |
1879 | if (!fmtinfo) { | |
1880 | fmtinfo = &unicam_image_formats[0]; | |
1881 | pix->pixelformat = fmtinfo->fourcc; | |
1882 | } | |
1883 | ||
1884 | unicam_calc_image_size_bpl(node->dev, fmtinfo, pix); | |
1885 | ||
1886 | if (pix->field == V4L2_FIELD_ANY) | |
1887 | pix->field = V4L2_FIELD_NONE; | |
1888 | } | |
1889 | ||
1890 | static int unicam_try_fmt_vid(struct file *file, void *priv, | |
1891 | struct v4l2_format *f) | |
1892 | { | |
1893 | struct unicam_node *node = video_drvdata(file); | |
1894 | ||
1895 | __unicam_try_fmt_vid(node, &f->fmt.pix); | |
1896 | return 0; | |
1897 | } | |
1898 | ||
1899 | static int unicam_s_fmt_vid(struct file *file, void *priv, | |
1900 | struct v4l2_format *f) | |
1901 | { | |
1902 | struct unicam_node *node = video_drvdata(file); | |
1903 | ||
1904 | if (vb2_is_busy(&node->buffer_queue)) | |
1905 | return -EBUSY; | |
1906 | ||
1907 | __unicam_try_fmt_vid(node, &f->fmt.pix); | |
1908 | node->fmt = *f; | |
1909 | ||
1910 | return 0; | |
1911 | } | |
1912 | ||
1913 | static int unicam_enum_fmt_meta(struct file *file, void *priv, | |
1914 | struct v4l2_fmtdesc *f) | |
1915 | { | |
1916 | unsigned int i, index; | |
1917 | ||
1918 | for (i = 0, index = 0; i < ARRAY_SIZE(unicam_meta_formats); i++) { | |
1919 | if (f->mbus_code && unicam_meta_formats[i].code != f->mbus_code) | |
1920 | continue; | |
1921 | ||
1922 | if (index == f->index) { | |
1923 | f->pixelformat = unicam_meta_formats[i].fourcc; | |
1924 | f->type = V4L2_BUF_TYPE_META_CAPTURE; | |
1925 | f->flags = V4L2_FMT_FLAG_META_LINE_BASED; | |
1926 | return 0; | |
1927 | } | |
1928 | ||
1929 | index++; | |
1930 | } | |
1931 | ||
1932 | return -EINVAL; | |
1933 | } | |
1934 | ||
1935 | static int unicam_g_fmt_meta(struct file *file, void *priv, | |
1936 | struct v4l2_format *f) | |
1937 | { | |
1938 | struct unicam_node *node = video_drvdata(file); | |
1939 | ||
1940 | f->fmt.meta = node->fmt.fmt.meta; | |
1941 | ||
1942 | return 0; | |
1943 | } | |
1944 | ||
1945 | static const struct unicam_format_info * | |
1946 | __unicam_try_fmt_meta(struct unicam_node *node, struct v4l2_meta_format *meta) | |
1947 | { | |
1948 | const struct unicam_format_info *fmtinfo; | |
1949 | ||
1950 | /* | |
1951 | * Default to the first format if the requested pixel format code isn't | |
1952 | * supported. | |
1953 | */ | |
1954 | fmtinfo = unicam_find_format_by_fourcc(meta->dataformat, | |
1955 | UNICAM_SD_PAD_SOURCE_METADATA); | |
1956 | if (!fmtinfo) { | |
1957 | fmtinfo = &unicam_meta_formats[0]; | |
1958 | meta->dataformat = fmtinfo->fourcc; | |
1959 | } | |
1960 | ||
1961 | unicam_calc_meta_size_bpl(node->dev, fmtinfo, meta); | |
1962 | ||
1963 | return fmtinfo; | |
1964 | } | |
1965 | ||
1966 | static int unicam_try_fmt_meta(struct file *file, void *priv, | |
1967 | struct v4l2_format *f) | |
1968 | { | |
1969 | struct unicam_node *node = video_drvdata(file); | |
1970 | ||
1971 | __unicam_try_fmt_meta(node, &f->fmt.meta); | |
1972 | return 0; | |
1973 | } | |
1974 | ||
1975 | static int unicam_s_fmt_meta(struct file *file, void *priv, | |
1976 | struct v4l2_format *f) | |
1977 | { | |
1978 | struct unicam_node *node = video_drvdata(file); | |
1979 | ||
1980 | if (vb2_is_busy(&node->buffer_queue)) | |
1981 | return -EBUSY; | |
1982 | ||
1983 | __unicam_try_fmt_meta(node, &f->fmt.meta); | |
1984 | node->fmt = *f; | |
1985 | ||
1986 | return 0; | |
1987 | } | |
1988 | ||
1989 | static int unicam_enum_framesizes(struct file *file, void *fh, | |
1990 | struct v4l2_frmsizeenum *fsize) | |
1991 | { | |
1992 | struct unicam_node *node = video_drvdata(file); | |
1993 | int ret = -EINVAL; | |
1994 | ||
1995 | if (fsize->index > 0) | |
1996 | return ret; | |
1997 | ||
1998 | if (is_image_node(node)) { | |
1999 | if (!unicam_find_format_by_fourcc(fsize->pixel_format, | |
2000 | UNICAM_SD_PAD_SOURCE_IMAGE)) | |
2001 | return ret; | |
2002 | ||
2003 | fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; | |
2004 | fsize->stepwise.min_width = UNICAM_IMAGE_MIN_WIDTH; | |
2005 | fsize->stepwise.max_width = UNICAM_IMAGE_MAX_WIDTH; | |
2006 | fsize->stepwise.step_width = 1; | |
2007 | fsize->stepwise.min_height = UNICAM_IMAGE_MIN_HEIGHT; | |
2008 | fsize->stepwise.max_height = UNICAM_IMAGE_MAX_HEIGHT; | |
2009 | fsize->stepwise.step_height = 1; | |
2010 | } else { | |
2011 | if (!unicam_find_format_by_fourcc(fsize->pixel_format, | |
2012 | UNICAM_SD_PAD_SOURCE_METADATA)) | |
2013 | return ret; | |
2014 | ||
2015 | fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; | |
2016 | fsize->stepwise.min_width = UNICAM_META_MIN_WIDTH; | |
2017 | fsize->stepwise.max_width = UNICAM_META_MAX_WIDTH; | |
2018 | fsize->stepwise.step_width = 1; | |
2019 | fsize->stepwise.min_height = UNICAM_META_MIN_HEIGHT; | |
2020 | fsize->stepwise.max_height = UNICAM_META_MAX_HEIGHT; | |
2021 | fsize->stepwise.step_height = 1; | |
2022 | } | |
2023 | ||
2024 | return 0; | |
2025 | } | |
2026 | ||
2027 | static int unicam_log_status(struct file *file, void *fh) | |
2028 | { | |
2029 | struct unicam_node *node = video_drvdata(file); | |
2030 | struct unicam_device *unicam = node->dev; | |
2031 | u32 reg; | |
2032 | ||
2033 | /* status for sub devices */ | |
2034 | v4l2_device_call_all(&unicam->v4l2_dev, 0, core, log_status); | |
2035 | ||
2036 | dev_info(unicam->dev, "-----Receiver status-----\n"); | |
2037 | dev_info(unicam->dev, "V4L2 width/height: %ux%u\n", | |
2038 | node->fmt.fmt.pix.width, node->fmt.fmt.pix.height); | |
2039 | dev_info(unicam->dev, "V4L2 format: %08x\n", | |
2040 | node->fmt.fmt.pix.pixelformat); | |
2041 | reg = unicam_reg_read(unicam, UNICAM_IPIPE); | |
2042 | dev_info(unicam->dev, "Unpacking/packing: %u / %u\n", | |
2043 | unicam_get_field(reg, UNICAM_PUM_MASK), | |
2044 | unicam_get_field(reg, UNICAM_PPM_MASK)); | |
2045 | dev_info(unicam->dev, "----Live data----\n"); | |
2046 | dev_info(unicam->dev, "Programmed stride: %4u\n", | |
2047 | unicam_reg_read(unicam, UNICAM_IBLS)); | |
2048 | dev_info(unicam->dev, "Detected resolution: %ux%u\n", | |
2049 | unicam_reg_read(unicam, UNICAM_IHSTA), | |
2050 | unicam_reg_read(unicam, UNICAM_IVSTA)); | |
2051 | dev_info(unicam->dev, "Write pointer: %08x\n", | |
2052 | unicam_reg_read(unicam, UNICAM_IBWP)); | |
2053 | ||
2054 | return 0; | |
2055 | } | |
2056 | ||
2057 | static int unicam_subscribe_event(struct v4l2_fh *fh, | |
2058 | const struct v4l2_event_subscription *sub) | |
2059 | { | |
2060 | switch (sub->type) { | |
2061 | case V4L2_EVENT_FRAME_SYNC: | |
2062 | return v4l2_event_subscribe(fh, sub, 2, NULL); | |
2063 | default: | |
2064 | return -EINVAL; | |
2065 | } | |
2066 | } | |
2067 | ||
2068 | static const struct v4l2_ioctl_ops unicam_ioctl_ops = { | |
2069 | .vidioc_querycap = unicam_querycap, | |
2070 | ||
2071 | .vidioc_enum_fmt_vid_cap = unicam_enum_fmt_vid, | |
2072 | .vidioc_g_fmt_vid_cap = unicam_g_fmt_vid, | |
2073 | .vidioc_try_fmt_vid_cap = unicam_try_fmt_vid, | |
2074 | .vidioc_s_fmt_vid_cap = unicam_s_fmt_vid, | |
2075 | ||
2076 | .vidioc_enum_fmt_meta_cap = unicam_enum_fmt_meta, | |
2077 | .vidioc_g_fmt_meta_cap = unicam_g_fmt_meta, | |
2078 | .vidioc_try_fmt_meta_cap = unicam_try_fmt_meta, | |
2079 | .vidioc_s_fmt_meta_cap = unicam_s_fmt_meta, | |
2080 | ||
2081 | .vidioc_enum_framesizes = unicam_enum_framesizes, | |
2082 | ||
2083 | .vidioc_reqbufs = vb2_ioctl_reqbufs, | |
2084 | .vidioc_create_bufs = vb2_ioctl_create_bufs, | |
2085 | .vidioc_prepare_buf = vb2_ioctl_prepare_buf, | |
2086 | .vidioc_querybuf = vb2_ioctl_querybuf, | |
2087 | .vidioc_qbuf = vb2_ioctl_qbuf, | |
2088 | .vidioc_dqbuf = vb2_ioctl_dqbuf, | |
2089 | .vidioc_expbuf = vb2_ioctl_expbuf, | |
2090 | .vidioc_streamon = vb2_ioctl_streamon, | |
2091 | .vidioc_streamoff = vb2_ioctl_streamoff, | |
2092 | ||
2093 | .vidioc_log_status = unicam_log_status, | |
2094 | .vidioc_subscribe_event = unicam_subscribe_event, | |
2095 | .vidioc_unsubscribe_event = v4l2_event_unsubscribe, | |
2096 | }; | |
2097 | ||
2098 | /* unicam capture driver file operations */ | |
2099 | static const struct v4l2_file_operations unicam_fops = { | |
2100 | .owner = THIS_MODULE, | |
2101 | .open = v4l2_fh_open, | |
2102 | .release = vb2_fop_release, | |
2103 | .poll = vb2_fop_poll, | |
2104 | .unlocked_ioctl = video_ioctl2, | |
2105 | .mmap = vb2_fop_mmap, | |
2106 | }; | |
2107 | ||
2108 | static int unicam_video_link_validate(struct media_link *link) | |
2109 | { | |
2110 | struct video_device *vdev = | |
2111 | media_entity_to_video_device(link->sink->entity); | |
2112 | struct v4l2_subdev *sd = | |
2113 | media_entity_to_v4l2_subdev(link->source->entity); | |
2114 | struct unicam_node *node = video_get_drvdata(vdev); | |
2115 | const u32 pad = is_image_node(node) ? UNICAM_SD_PAD_SOURCE_IMAGE | |
2116 | : UNICAM_SD_PAD_SOURCE_METADATA; | |
2117 | const struct v4l2_mbus_framefmt *format; | |
2118 | struct v4l2_subdev_state *state; | |
2119 | int ret = 0; | |
2120 | ||
2121 | state = v4l2_subdev_lock_and_get_active_state(sd); | |
2122 | ||
2123 | format = v4l2_subdev_state_get_format(state, pad, 0); | |
2124 | if (!format) { | |
2125 | ret = -EINVAL; | |
2126 | goto out; | |
2127 | } | |
2128 | ||
2129 | if (is_image_node(node)) { | |
2130 | const struct v4l2_pix_format *fmt = &node->fmt.fmt.pix; | |
2131 | const struct unicam_format_info *fmtinfo; | |
2132 | ||
2133 | fmtinfo = unicam_find_format_by_fourcc(fmt->pixelformat, | |
2134 | UNICAM_SD_PAD_SOURCE_IMAGE); | |
2135 | if (WARN_ON(!fmtinfo)) { | |
2136 | ret = -EPIPE; | |
2137 | goto out; | |
2138 | } | |
2139 | ||
2140 | if (fmtinfo->code != format->code || | |
2141 | fmt->height != format->height || | |
2142 | fmt->width != format->width || | |
2143 | fmt->field != format->field) { | |
2144 | dev_dbg(node->dev->dev, | |
2145 | "image: (%u x %u) 0x%08x %s != (%u x %u) 0x%08x %s\n", | |
2146 | fmt->width, fmt->height, fmtinfo->code, | |
2147 | v4l2_field_names[fmt->field], | |
2148 | format->width, format->height, format->code, | |
2149 | v4l2_field_names[format->field]); | |
2150 | ret = -EPIPE; | |
2151 | } | |
2152 | } else { | |
2153 | const struct v4l2_meta_format *fmt = &node->fmt.fmt.meta; | |
2154 | ||
2155 | const struct unicam_format_info *fmtinfo; | |
2156 | ||
2157 | fmtinfo = unicam_find_format_by_fourcc(fmt->dataformat, | |
2158 | UNICAM_SD_PAD_SOURCE_METADATA); | |
2159 | if (WARN_ON(!fmtinfo)) { | |
2160 | ret = -EPIPE; | |
2161 | goto out; | |
2162 | } | |
2163 | ||
2164 | if (fmtinfo->code != format->code || | |
2165 | fmt->height != format->height || | |
2166 | fmt->width != format->width) { | |
2167 | dev_dbg(node->dev->dev, | |
2168 | "meta: (%u x %u) 0x%04x != (%u x %u) 0x%04x\n", | |
2169 | fmt->width, fmt->height, fmtinfo->code, | |
2170 | format->width, format->height, format->code); | |
2171 | ret = -EPIPE; | |
2172 | } | |
2173 | } | |
2174 | ||
2175 | out: | |
2176 | v4l2_subdev_unlock_state(state); | |
2177 | return ret; | |
2178 | } | |
2179 | ||
2180 | static const struct media_entity_operations unicam_video_media_ops = { | |
2181 | .link_validate = unicam_video_link_validate, | |
2182 | }; | |
2183 | ||
2184 | static void unicam_node_release(struct video_device *vdev) | |
2185 | { | |
2186 | struct unicam_node *node = video_get_drvdata(vdev); | |
2187 | ||
2188 | unicam_put(node->dev); | |
2189 | } | |
2190 | ||
2191 | static void unicam_set_default_format(struct unicam_node *node) | |
2192 | { | |
2193 | if (is_image_node(node)) { | |
2194 | struct v4l2_pix_format *fmt = &node->fmt.fmt.pix; | |
2195 | const struct unicam_format_info *fmtinfo = | |
2196 | &unicam_image_formats[0]; | |
2197 | ||
2198 | node->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
2199 | ||
2200 | v4l2_fill_pix_format(fmt, &unicam_default_image_format); | |
2201 | fmt->pixelformat = fmtinfo->fourcc; | |
2202 | unicam_calc_image_size_bpl(node->dev, fmtinfo, fmt); | |
2203 | } else { | |
2204 | struct v4l2_meta_format *fmt = &node->fmt.fmt.meta; | |
2205 | const struct unicam_format_info *fmtinfo = | |
2206 | &unicam_meta_formats[0]; | |
2207 | ||
2208 | node->fmt.type = V4L2_BUF_TYPE_META_CAPTURE; | |
2209 | ||
2210 | fmt->dataformat = fmtinfo->fourcc; | |
2211 | fmt->width = unicam_default_meta_format.width; | |
2212 | fmt->height = unicam_default_meta_format.height; | |
2213 | unicam_calc_meta_size_bpl(node->dev, fmtinfo, fmt); | |
2214 | } | |
2215 | } | |
2216 | ||
2217 | static int unicam_register_node(struct unicam_device *unicam, | |
2218 | enum unicam_node_type type) | |
2219 | { | |
2220 | const u32 pad_index = type == UNICAM_IMAGE_NODE | |
2221 | ? UNICAM_SD_PAD_SOURCE_IMAGE | |
2222 | : UNICAM_SD_PAD_SOURCE_METADATA; | |
2223 | struct unicam_node *node = &unicam->node[type]; | |
2224 | struct video_device *vdev = &node->video_dev; | |
2225 | struct vb2_queue *q = &node->buffer_queue; | |
2226 | int ret; | |
2227 | ||
2228 | node->dev = unicam_get(unicam); | |
2229 | node->id = type; | |
2230 | ||
2231 | spin_lock_init(&node->dma_queue_lock); | |
2232 | ||
2233 | INIT_LIST_HEAD(&node->dma_queue); | |
2234 | ||
2235 | /* Initialize the videobuf2 queue. */ | |
2236 | q->type = type == UNICAM_IMAGE_NODE ? V4L2_BUF_TYPE_VIDEO_CAPTURE | |
2237 | : V4L2_BUF_TYPE_META_CAPTURE; | |
2238 | q->io_modes = VB2_MMAP | VB2_DMABUF; | |
2239 | q->drv_priv = node; | |
2240 | q->ops = &unicam_video_qops; | |
2241 | q->mem_ops = &vb2_dma_contig_memops; | |
2242 | q->buf_struct_size = sizeof(struct unicam_buffer); | |
2243 | q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; | |
2244 | q->lock = &unicam->lock; | |
2245 | q->min_queued_buffers = 1; | |
2246 | q->dev = unicam->dev; | |
2247 | ||
2248 | ret = vb2_queue_init(q); | |
2249 | if (ret) { | |
2250 | dev_err(unicam->dev, "vb2_queue_init() failed\n"); | |
2251 | goto err_unicam_put; | |
2252 | } | |
2253 | ||
2254 | /* Initialize the video device. */ | |
2255 | vdev->release = unicam_node_release; | |
2256 | vdev->fops = &unicam_fops; | |
2257 | vdev->ioctl_ops = &unicam_ioctl_ops; | |
2258 | vdev->v4l2_dev = &unicam->v4l2_dev; | |
2259 | vdev->vfl_dir = VFL_DIR_RX; | |
2260 | vdev->queue = q; | |
2261 | vdev->lock = &unicam->lock; | |
2262 | vdev->device_caps = type == UNICAM_IMAGE_NODE | |
2263 | ? V4L2_CAP_VIDEO_CAPTURE : V4L2_CAP_META_CAPTURE; | |
2264 | vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_IO_MC; | |
2265 | vdev->entity.ops = &unicam_video_media_ops; | |
2266 | ||
2267 | snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME, | |
2268 | type == UNICAM_IMAGE_NODE ? "image" : "embedded"); | |
2269 | ||
2270 | video_set_drvdata(vdev, node); | |
2271 | ||
2272 | if (type == UNICAM_IMAGE_NODE) | |
2273 | vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT; | |
2274 | ||
2275 | node->pad.flags = MEDIA_PAD_FL_SINK; | |
2276 | ||
2277 | ret = media_entity_pads_init(&vdev->entity, 1, &node->pad); | |
2278 | if (ret) | |
2279 | goto err_unicam_put; | |
2280 | ||
2281 | node->dummy_buf.size = UNICAM_DUMMY_BUF_SIZE; | |
2282 | node->dummy_buf_cpu_addr = dma_alloc_coherent(unicam->dev, | |
2283 | node->dummy_buf.size, | |
2284 | &node->dummy_buf.dma_addr, | |
2285 | GFP_KERNEL); | |
2286 | if (!node->dummy_buf_cpu_addr) { | |
2287 | dev_err(unicam->dev, "Unable to allocate dummy buffer.\n"); | |
2288 | ret = -ENOMEM; | |
2289 | goto err_entity_cleanup; | |
2290 | } | |
2291 | ||
2292 | unicam_set_default_format(node); | |
2293 | ||
2294 | ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); | |
2295 | if (ret) { | |
2296 | dev_err(unicam->dev, "Unable to register video device %s\n", | |
2297 | vdev->name); | |
2298 | goto err_dma_free; | |
2299 | } | |
2300 | ||
2301 | node->registered = true; | |
2302 | ||
2303 | ret = media_create_pad_link(&unicam->subdev.sd.entity, | |
2304 | pad_index, | |
2305 | &node->video_dev.entity, | |
2306 | 0, | |
2307 | MEDIA_LNK_FL_ENABLED | | |
2308 | MEDIA_LNK_FL_IMMUTABLE); | |
2309 | if (ret) { | |
2310 | /* | |
2311 | * No need for cleanup, the caller will unregister the | |
2312 | * video device, which will drop the reference on the | |
2313 | * device and trigger the cleanup. | |
2314 | */ | |
2315 | dev_err(unicam->dev, "Unable to create pad link for %s\n", | |
2316 | unicam->sensor.subdev->name); | |
2317 | return ret; | |
2318 | } | |
2319 | ||
2320 | return 0; | |
2321 | ||
2322 | err_dma_free: | |
2323 | dma_free_coherent(unicam->dev, node->dummy_buf.size, | |
2324 | node->dummy_buf_cpu_addr, | |
2325 | node->dummy_buf.dma_addr); | |
2326 | err_entity_cleanup: | |
2327 | media_entity_cleanup(&vdev->entity); | |
2328 | err_unicam_put: | |
2329 | unicam_put(unicam); | |
2330 | return ret; | |
2331 | } | |
2332 | ||
2333 | static void unicam_unregister_nodes(struct unicam_device *unicam) | |
2334 | { | |
2335 | unsigned int i; | |
2336 | ||
2337 | for (i = 0; i < ARRAY_SIZE(unicam->node); i++) { | |
2338 | struct unicam_node *node = &unicam->node[i]; | |
2339 | ||
2340 | if (node->registered) { | |
2341 | vb2_video_unregister_device(&node->video_dev); | |
2342 | node->registered = false; | |
2343 | } | |
2344 | ||
2345 | if (node->dummy_buf_cpu_addr) | |
2346 | dma_free_coherent(unicam->dev, node->dummy_buf.size, | |
2347 | node->dummy_buf_cpu_addr, | |
2348 | node->dummy_buf.dma_addr); | |
2349 | } | |
2350 | } | |
2351 | ||
2352 | /* ----------------------------------------------------------------------------- | |
2353 | * Power management | |
2354 | */ | |
2355 | ||
2356 | static int unicam_runtime_resume(struct device *dev) | |
2357 | { | |
2358 | struct unicam_device *unicam = dev_get_drvdata(dev); | |
2359 | int ret; | |
2360 | ||
2361 | ret = clk_set_min_rate(unicam->vpu_clock, UNICAM_MIN_VPU_CLOCK_RATE); | |
2362 | if (ret) { | |
2363 | dev_err(unicam->dev, "failed to set up VPU clock\n"); | |
2364 | return ret; | |
2365 | } | |
2366 | ||
2367 | ret = clk_prepare_enable(unicam->vpu_clock); | |
2368 | if (ret) { | |
2369 | dev_err(unicam->dev, "Failed to enable VPU clock: %d\n", ret); | |
2370 | goto err_vpu_clock; | |
2371 | } | |
2372 | ||
2373 | ret = clk_set_rate(unicam->clock, 100 * 1000 * 1000); | |
2374 | if (ret) { | |
2375 | dev_err(unicam->dev, "failed to set up CSI clock\n"); | |
2376 | goto err_vpu_prepare; | |
2377 | } | |
2378 | ||
2379 | ret = clk_prepare_enable(unicam->clock); | |
2380 | if (ret) { | |
2381 | dev_err(unicam->dev, "Failed to enable CSI clock: %d\n", ret); | |
2382 | goto err_vpu_prepare; | |
2383 | } | |
2384 | ||
2385 | return 0; | |
2386 | ||
2387 | err_vpu_prepare: | |
2388 | clk_disable_unprepare(unicam->vpu_clock); | |
2389 | err_vpu_clock: | |
2390 | if (clk_set_min_rate(unicam->vpu_clock, 0)) | |
2391 | dev_err(unicam->dev, "failed to reset the VPU clock\n"); | |
2392 | ||
2393 | return ret; | |
2394 | } | |
2395 | ||
2396 | static int unicam_runtime_suspend(struct device *dev) | |
2397 | { | |
2398 | struct unicam_device *unicam = dev_get_drvdata(dev); | |
2399 | ||
2400 | clk_disable_unprepare(unicam->clock); | |
2401 | ||
2402 | if (clk_set_min_rate(unicam->vpu_clock, 0)) | |
2403 | dev_err(unicam->dev, "failed to reset the VPU clock\n"); | |
2404 | ||
2405 | clk_disable_unprepare(unicam->vpu_clock); | |
2406 | ||
2407 | return 0; | |
2408 | } | |
2409 | ||
2410 | static const struct dev_pm_ops unicam_pm_ops = { | |
2411 | RUNTIME_PM_OPS(unicam_runtime_suspend, unicam_runtime_resume, NULL) | |
2412 | }; | |
2413 | ||
2414 | /* ----------------------------------------------------------------------------- | |
2415 | * V4L2 async notifier | |
2416 | */ | |
2417 | ||
2418 | static int unicam_async_bound(struct v4l2_async_notifier *notifier, | |
2419 | struct v4l2_subdev *subdev, | |
2420 | struct v4l2_async_connection *asc) | |
2421 | { | |
2422 | struct unicam_device *unicam = notifier_to_unicam_device(notifier); | |
2423 | struct media_pad *sink = &unicam->subdev.pads[UNICAM_SD_PAD_SINK]; | |
2424 | struct media_pad *source; | |
2425 | int ret; | |
2426 | ||
2427 | dev_dbg(unicam->dev, "Using sensor %s for capture\n", | |
2428 | subdev->name); | |
2429 | ||
2430 | ret = v4l2_create_fwnode_links_to_pad(subdev, sink, MEDIA_LNK_FL_ENABLED | | |
2431 | MEDIA_LNK_FL_IMMUTABLE); | |
2432 | if (ret) | |
2433 | return ret; | |
2434 | ||
2435 | source = media_pad_remote_pad_unique(sink); | |
b6041c9e | 2436 | if (IS_ERR(source)) { |
392cd78d | 2437 | dev_err(unicam->dev, "No connected sensor pad\n"); |
b6041c9e | 2438 | return PTR_ERR(source); |
392cd78d DS |
2439 | } |
2440 | ||
2441 | unicam->sensor.subdev = subdev; | |
2442 | unicam->sensor.pad = source; | |
2443 | ||
2444 | return 0; | |
2445 | } | |
2446 | ||
2447 | static int unicam_async_complete(struct v4l2_async_notifier *notifier) | |
2448 | { | |
2449 | struct unicam_device *unicam = notifier_to_unicam_device(notifier); | |
2450 | int ret; | |
2451 | ||
2452 | ret = unicam_register_node(unicam, UNICAM_IMAGE_NODE); | |
2453 | if (ret) { | |
2454 | dev_err(unicam->dev, "Unable to register image video device.\n"); | |
2455 | goto unregister; | |
2456 | } | |
2457 | ||
2458 | ret = unicam_register_node(unicam, UNICAM_METADATA_NODE); | |
2459 | if (ret) { | |
2460 | dev_err(unicam->dev, "Unable to register metadata video device.\n"); | |
2461 | goto unregister; | |
2462 | } | |
2463 | ||
2464 | ret = v4l2_device_register_subdev_nodes(&unicam->v4l2_dev); | |
2465 | if (ret) { | |
2466 | dev_err(unicam->dev, "Unable to register subdev nodes.\n"); | |
2467 | goto unregister; | |
2468 | } | |
2469 | ||
2470 | return 0; | |
2471 | ||
2472 | unregister: | |
2473 | unicam_unregister_nodes(unicam); | |
2474 | unicam_put(unicam); | |
2475 | ||
2476 | return ret; | |
2477 | } | |
2478 | ||
2479 | static const struct v4l2_async_notifier_operations unicam_async_ops = { | |
2480 | .bound = unicam_async_bound, | |
2481 | .complete = unicam_async_complete, | |
2482 | }; | |
2483 | ||
2484 | static int unicam_async_nf_init(struct unicam_device *unicam) | |
2485 | { | |
2486 | struct v4l2_fwnode_endpoint ep = { }; | |
2487 | struct fwnode_handle *ep_handle; | |
2488 | struct v4l2_async_connection *asc; | |
2489 | int ret; | |
2490 | ||
2491 | ret = of_property_read_u32(unicam->dev->of_node, "brcm,num-data-lanes", | |
2492 | &unicam->max_data_lanes); | |
2493 | if (ret < 0) { | |
2494 | dev_err(unicam->dev, "Missing %s DT property\n", | |
2495 | "brcm,num-data-lanes"); | |
2496 | return -EINVAL; | |
2497 | } | |
2498 | ||
2499 | /* Get and parse the local endpoint. */ | |
2500 | ep_handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(unicam->dev), 0, 0, | |
2501 | FWNODE_GRAPH_ENDPOINT_NEXT); | |
2502 | if (!ep_handle) { | |
2503 | dev_err(unicam->dev, "No endpoint found\n"); | |
2504 | return -ENODEV; | |
2505 | } | |
2506 | ||
2507 | ret = v4l2_fwnode_endpoint_parse(ep_handle, &ep); | |
2508 | if (ret) { | |
2509 | dev_err(unicam->dev, "Failed to parse endpoint: %d\n", ret); | |
2510 | goto error; | |
2511 | } | |
2512 | ||
2513 | unicam->bus_type = ep.bus_type; | |
2514 | ||
2515 | switch (ep.bus_type) { | |
2516 | case V4L2_MBUS_CSI2_DPHY: { | |
2517 | unsigned int num_data_lanes = ep.bus.mipi_csi2.num_data_lanes; | |
2518 | ||
2519 | if (num_data_lanes != 1 && num_data_lanes != 2 && | |
2520 | num_data_lanes != 4) { | |
2521 | dev_err(unicam->dev, "%u data lanes not supported\n", | |
2522 | num_data_lanes); | |
2523 | ret = -EINVAL; | |
2524 | goto error; | |
2525 | } | |
2526 | ||
2527 | if (num_data_lanes > unicam->max_data_lanes) { | |
2528 | dev_err(unicam->dev, | |
2529 | "Endpoint uses %u data lanes when %u are supported\n", | |
2530 | num_data_lanes, unicam->max_data_lanes); | |
2531 | ret = -EINVAL; | |
2532 | goto error; | |
2533 | } | |
2534 | ||
2535 | unicam->max_data_lanes = num_data_lanes; | |
2536 | unicam->bus_flags = ep.bus.mipi_csi2.flags; | |
2537 | break; | |
2538 | } | |
2539 | ||
2540 | case V4L2_MBUS_CCP2: | |
2541 | unicam->max_data_lanes = 1; | |
2542 | unicam->bus_flags = ep.bus.mipi_csi1.strobe; | |
2543 | break; | |
2544 | ||
2545 | default: | |
2546 | /* Unsupported bus type */ | |
2547 | dev_err(unicam->dev, "Unsupported bus type %u\n", ep.bus_type); | |
2548 | ret = -EINVAL; | |
2549 | goto error; | |
2550 | } | |
2551 | ||
2552 | /* Initialize and register the async notifier. */ | |
2553 | v4l2_async_nf_init(&unicam->notifier, &unicam->v4l2_dev); | |
2554 | ||
2555 | asc = v4l2_async_nf_add_fwnode_remote(&unicam->notifier, ep_handle, | |
2556 | struct v4l2_async_connection); | |
2557 | fwnode_handle_put(ep_handle); | |
2558 | ep_handle = NULL; | |
2559 | ||
2560 | if (IS_ERR(asc)) { | |
2561 | ret = PTR_ERR(asc); | |
2562 | dev_err(unicam->dev, "Failed to add entry to notifier: %d\n", | |
2563 | ret); | |
2564 | goto error; | |
2565 | } | |
2566 | ||
2567 | unicam->notifier.ops = &unicam_async_ops; | |
2568 | ||
2569 | ret = v4l2_async_nf_register(&unicam->notifier); | |
2570 | if (ret) { | |
2571 | dev_err(unicam->dev, "Error registering device notifier: %d\n", | |
2572 | ret); | |
2573 | goto error; | |
2574 | } | |
2575 | ||
2576 | return 0; | |
2577 | ||
2578 | error: | |
2579 | fwnode_handle_put(ep_handle); | |
2580 | return ret; | |
2581 | } | |
2582 | ||
2583 | /* ----------------------------------------------------------------------------- | |
2584 | * Probe & remove | |
2585 | */ | |
2586 | ||
2587 | static int unicam_media_init(struct unicam_device *unicam) | |
2588 | { | |
2589 | int ret; | |
2590 | ||
2591 | unicam->mdev.dev = unicam->dev; | |
2592 | strscpy(unicam->mdev.model, UNICAM_MODULE_NAME, | |
2593 | sizeof(unicam->mdev.model)); | |
2594 | unicam->mdev.hw_revision = 0; | |
2595 | ||
2596 | media_device_init(&unicam->mdev); | |
2597 | ||
2598 | unicam->v4l2_dev.mdev = &unicam->mdev; | |
2599 | ||
2600 | ret = v4l2_device_register(unicam->dev, &unicam->v4l2_dev); | |
2601 | if (ret < 0) { | |
2602 | dev_err(unicam->dev, "Unable to register v4l2 device\n"); | |
2603 | goto err_media_cleanup; | |
2604 | } | |
2605 | ||
2606 | ret = media_device_register(&unicam->mdev); | |
2607 | if (ret < 0) { | |
2608 | dev_err(unicam->dev, | |
2609 | "Unable to register media-controller device\n"); | |
2610 | goto err_v4l2_unregister; | |
2611 | } | |
2612 | ||
2613 | return 0; | |
2614 | ||
2615 | err_v4l2_unregister: | |
2616 | v4l2_device_unregister(&unicam->v4l2_dev); | |
2617 | err_media_cleanup: | |
2618 | media_device_cleanup(&unicam->mdev); | |
2619 | return ret; | |
2620 | } | |
2621 | ||
2622 | static int unicam_probe(struct platform_device *pdev) | |
2623 | { | |
2624 | struct unicam_device *unicam; | |
2625 | int ret; | |
2626 | ||
2627 | unicam = kzalloc(sizeof(*unicam), GFP_KERNEL); | |
2628 | if (!unicam) | |
2629 | return -ENOMEM; | |
2630 | ||
2631 | kref_init(&unicam->kref); | |
2632 | mutex_init(&unicam->lock); | |
2633 | ||
2634 | unicam->dev = &pdev->dev; | |
2635 | platform_set_drvdata(pdev, unicam); | |
2636 | ||
2637 | unicam->base = devm_platform_ioremap_resource_byname(pdev, "unicam"); | |
2638 | if (IS_ERR(unicam->base)) { | |
2639 | ret = PTR_ERR(unicam->base); | |
2640 | goto err_unicam_put; | |
2641 | } | |
2642 | ||
2643 | unicam->clk_gate_base = devm_platform_ioremap_resource_byname(pdev, "cmi"); | |
2644 | if (IS_ERR(unicam->clk_gate_base)) { | |
2645 | ret = PTR_ERR(unicam->clk_gate_base); | |
2646 | goto err_unicam_put; | |
2647 | } | |
2648 | ||
2649 | unicam->clock = devm_clk_get(&pdev->dev, "lp"); | |
2650 | if (IS_ERR(unicam->clock)) { | |
2651 | dev_err(unicam->dev, "Failed to get lp clock\n"); | |
2652 | ret = PTR_ERR(unicam->clock); | |
2653 | goto err_unicam_put; | |
2654 | } | |
2655 | ||
2656 | unicam->vpu_clock = devm_clk_get(&pdev->dev, "vpu"); | |
2657 | if (IS_ERR(unicam->vpu_clock)) { | |
2658 | dev_err(unicam->dev, "Failed to get vpu clock\n"); | |
2659 | ret = PTR_ERR(unicam->vpu_clock); | |
2660 | goto err_unicam_put; | |
2661 | } | |
2662 | ||
2663 | ret = platform_get_irq(pdev, 0); | |
0cc50ced | 2664 | if (ret < 0) |
392cd78d | 2665 | goto err_unicam_put; |
392cd78d DS |
2666 | |
2667 | ret = devm_request_irq(&pdev->dev, ret, unicam_isr, 0, | |
2668 | "unicam_capture0", unicam); | |
2669 | if (ret) { | |
2670 | dev_err(&pdev->dev, "Unable to request interrupt\n"); | |
392cd78d DS |
2671 | goto err_unicam_put; |
2672 | } | |
2673 | ||
2674 | /* Enable the block power domain. */ | |
2675 | pm_runtime_enable(&pdev->dev); | |
2676 | ||
2677 | ret = unicam_media_init(unicam); | |
2678 | if (ret) | |
2679 | goto err_pm_runtime; | |
2680 | ||
2681 | ret = unicam_subdev_init(unicam); | |
2682 | if (ret) | |
2683 | goto err_media_unregister; | |
2684 | ||
2685 | ret = unicam_async_nf_init(unicam); | |
2686 | if (ret) | |
2687 | goto err_subdev_unregister; | |
2688 | ||
2689 | return 0; | |
2690 | ||
2691 | err_subdev_unregister: | |
2692 | unicam_subdev_cleanup(unicam); | |
2693 | err_media_unregister: | |
2694 | media_device_unregister(&unicam->mdev); | |
2695 | err_pm_runtime: | |
2696 | pm_runtime_disable(&pdev->dev); | |
2697 | err_unicam_put: | |
2698 | unicam_put(unicam); | |
2699 | ||
2700 | return ret; | |
2701 | } | |
2702 | ||
abfec2e1 | 2703 | static void unicam_remove(struct platform_device *pdev) |
392cd78d DS |
2704 | { |
2705 | struct unicam_device *unicam = platform_get_drvdata(pdev); | |
2706 | ||
2707 | unicam_unregister_nodes(unicam); | |
2708 | v4l2_device_unregister(&unicam->v4l2_dev); | |
2709 | media_device_unregister(&unicam->mdev); | |
2710 | v4l2_async_nf_unregister(&unicam->notifier); | |
2711 | ||
2712 | unicam_subdev_cleanup(unicam); | |
2713 | ||
2714 | unicam_put(unicam); | |
2715 | ||
2716 | pm_runtime_disable(&pdev->dev); | |
392cd78d DS |
2717 | } |
2718 | ||
2719 | static const struct of_device_id unicam_of_match[] = { | |
2720 | { .compatible = "brcm,bcm2835-unicam", }, | |
2721 | { /* sentinel */ }, | |
2722 | }; | |
2723 | MODULE_DEVICE_TABLE(of, unicam_of_match); | |
2724 | ||
2725 | static struct platform_driver unicam_driver = { | |
2726 | .probe = unicam_probe, | |
abfec2e1 | 2727 | .remove_new = unicam_remove, |
392cd78d DS |
2728 | .driver = { |
2729 | .name = UNICAM_MODULE_NAME, | |
2730 | .pm = pm_ptr(&unicam_pm_ops), | |
44543697 | 2731 | .of_match_table = unicam_of_match, |
392cd78d DS |
2732 | }, |
2733 | }; | |
2734 | ||
2735 | module_platform_driver(unicam_driver); | |
2736 | ||
2737 | MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com>"); | |
2738 | MODULE_DESCRIPTION("BCM2835 Unicam driver"); | |
2739 | MODULE_LICENSE("GPL"); |