projects / openwrt / staging / pepe2k.git / blob
? search:


8f383ece2b516d039a8627944d894e75b2c3c0f0
[openwrt/staging/pepe2k.git] /
1 From 5f6585179d2779f98e83d1cedf780b51aef1e924 Mon Sep 17 00:00:00 2001
2 From: Naushir Patuck <naush@raspberrypi.com>
3 Date: Mon, 4 May 2020 12:25:41 +0300
4 Subject: [PATCH] media: bcm2835-unicam: Driver for CCP2/CSI2 camera
5 interface
6
7 Add a driver for the Unicam camera receiver block on BCM283x processors.
8 Compared to the bcm2835-camera driver present in staging, this driver
9 handles the Unicam block only (CSI-2 receiver), and doesn't depend on
10 the VC4 firmware running on the VPU.
11
12 The commit is made up of a series of changes cherry-picked from the
13 rpi-5.4.y branch of https://github.com/raspberrypi/linux/ with
14 additional enhancements, forward-ported to the mainline kernel.
15
16 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
17 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
18 Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
19 Reported-by: kbuild test robot <lkp@intel.com>
20 ---
21 MAINTAINERS | 2 +-
22 drivers/media/platform/bcm2835/Kconfig | 21 +
23 drivers/media/platform/bcm2835/Makefile | 3 +
24 .../media/platform/bcm2835/bcm2835-unicam.c | 2827 +++++++++++++++++
25 .../media/platform/bcm2835/vc4-regs-unicam.h | 253 ++
26 5 files changed, 3105 insertions(+), 1 deletion(-)
27 create mode 100644 drivers/media/platform/bcm2835/Kconfig
28 create mode 100644 drivers/media/platform/bcm2835/Makefile
29 create mode 100644 drivers/media/platform/bcm2835/bcm2835-unicam.c
30 create mode 100644 drivers/media/platform/bcm2835/vc4-regs-unicam.h
31
32 --- a/MAINTAINERS
33 +++ b/MAINTAINERS
34 @@ -4042,7 +4042,7 @@ M: Dave Stevenson <dave.stevenson@raspbe
35 L: linux-media@vger.kernel.org
36 S: Maintained
37 F: drivers/media/platform/bcm2835/
38 -F: Documentation/devicetree/bindings/media/bcm2835-unicam.txt
39 +F: Documentation/devicetree/bindings/media/brcm,bcm2835-unicam.yaml
40
41 BROADCOM BCM47XX MIPS ARCHITECTURE
42 M: Hauke Mehrtens <hauke@hauke-m.de>
43 --- /dev/null
44 +++ b/drivers/media/platform/bcm2835/Kconfig
45 @@ -0,0 +1,21 @@
46 +# Broadcom VideoCore4 V4L2 camera support
47 +
48 +config VIDEO_BCM2835_UNICAM
49 + tristate "Broadcom BCM283x/BCM271x Unicam video capture driver"
50 + depends on VIDEO_DEV
51 + depends on ARCH_BCM2835 || COMPILE_TEST
52 + select VIDEO_V4L2_SUBDEV_API
53 + select MEDIA_CONTROLLER
54 + select VIDEOBUF2_DMA_CONTIG
55 + select V4L2_FWNODE
56 + help
57 + Say Y here to enable support for the BCM283x/BCM271x CSI-2 receiver.
58 + This is a V4L2 driver that controls the CSI-2 receiver directly,
59 + independently from the VC4 firmware.
60 + This driver is mutually exclusive with the use of bcm2835-camera. The
61 + firmware will disable all access to the peripheral from within the
62 + firmware if it finds a DT node using it, and bcm2835-camera will
63 + therefore fail to probe.
64 +
65 + To compile this driver as a module, choose M here. The module will be
66 + called bcm2835-unicam.
67 --- /dev/null
68 +++ b/drivers/media/platform/bcm2835/Makefile
69 @@ -0,0 +1,3 @@
70 +# Makefile for BCM2835 Unicam driver
71 +
72 +obj-$(CONFIG_VIDEO_BCM2835_UNICAM) += bcm2835-unicam.o
73 --- /dev/null
74 +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
75 @@ -0,0 +1,2827 @@
76 +// SPDX-License-Identifier: GPL-2.0-only
77 +/*
78 + * BCM2835 Unicam Capture Driver
79 + *
80 + * Copyright (C) 2017-2020 - Raspberry Pi (Trading) Ltd.
81 + *
82 + * Dave Stevenson <dave.stevenson@raspberrypi.com>
83 + *
84 + * Based on TI am437x driver by
85 + * Benoit Parrot <bparrot@ti.com>
86 + * Lad, Prabhakar <prabhakar.csengg@gmail.com>
87 + *
88 + * and TI CAL camera interface driver by
89 + * Benoit Parrot <bparrot@ti.com>
90 + *
91 + *
92 + * There are two camera drivers in the kernel for BCM283x - this one
93 + * and bcm2835-camera (currently in staging).
94 + *
95 + * This driver directly controls the Unicam peripheral - there is no
96 + * involvement with the VideoCore firmware. Unicam receives CSI-2 or
97 + * CCP2 data and writes it into SDRAM.
98 + * The only potential processing options are to repack Bayer data into an
99 + * alternate format, and applying windowing.
100 + * The repacking does not shift the data, so can repack V4L2_PIX_FMT_Sxxxx10P
101 + * to V4L2_PIX_FMT_Sxxxx10, or V4L2_PIX_FMT_Sxxxx12P to V4L2_PIX_FMT_Sxxxx12,
102 + * but not generically up to V4L2_PIX_FMT_Sxxxx16. The driver will add both
103 + * formats where the relevant formats are defined, and will automatically
104 + * configure the repacking as required.
105 + * Support for windowing may be added later.
106 + *
107 + * It should be possible to connect this driver to any sensor with a
108 + * suitable output interface and V4L2 subdevice driver.
109 + *
110 + * bcm2835-camera uses the VideoCore firmware to control the sensor,
111 + * Unicam, ISP, and all tuner control loops. Fully processed frames are
112 + * delivered to the driver by the firmware. It only has sensor drivers
113 + * for Omnivision OV5647, and Sony IMX219 sensors.
114 + *
115 + * The two drivers are mutually exclusive for the same Unicam instance.
116 + * The VideoCore firmware checks the device tree configuration during boot.
117 + * If it finds device tree nodes called csi0 or csi1 it will block the
118 + * firmware from accessing the peripheral, and bcm2835-camera will
119 + * not be able to stream data.
120 + */
121 +
122 +#include <linux/clk.h>
123 +#include <linux/delay.h>
124 +#include <linux/device.h>
125 +#include <linux/dma-mapping.h>
126 +#include <linux/err.h>
127 +#include <linux/init.h>
128 +#include <linux/interrupt.h>
129 +#include <linux/io.h>
130 +#include <linux/module.h>
131 +#include <linux/of_device.h>
132 +#include <linux/of_graph.h>
133 +#include <linux/pinctrl/consumer.h>
134 +#include <linux/platform_device.h>
135 +#include <linux/pm_runtime.h>
136 +#include <linux/slab.h>
137 +#include <linux/uaccess.h>
138 +#include <linux/videodev2.h>
139 +
140 +#include <media/v4l2-common.h>
141 +#include <media/v4l2-ctrls.h>
142 +#include <media/v4l2-dev.h>
143 +#include <media/v4l2-device.h>
144 +#include <media/v4l2-dv-timings.h>
145 +#include <media/v4l2-event.h>
146 +#include <media/v4l2-ioctl.h>
147 +#include <media/v4l2-fwnode.h>
148 +#include <media/videobuf2-dma-contig.h>
149 +
150 +#include <media/v4l2-async.h>
151 +
152 +#include "vc4-regs-unicam.h"
153 +
154 +#define UNICAM_MODULE_NAME "unicam"
155 +#define UNICAM_VERSION "0.1.0"
156 +
157 +static int debug;
158 +module_param(debug, int, 0644);
159 +MODULE_PARM_DESC(debug, "Debug level 0-3");
160 +
161 +#define unicam_dbg(level, dev, fmt, arg...) \
162 + v4l2_dbg(level, debug, &(dev)->v4l2_dev, fmt, ##arg)
163 +#define unicam_info(dev, fmt, arg...) \
164 + v4l2_info(&(dev)->v4l2_dev, fmt, ##arg)
165 +#define unicam_err(dev, fmt, arg...) \
166 + v4l2_err(&(dev)->v4l2_dev, fmt, ##arg)
167 +
168 +/*
169 + * To protect against a dodgy sensor driver never returning an error from
170 + * enum_mbus_code, set a maximum index value to be used.
171 + */
172 +#define MAX_ENUM_MBUS_CODE 128
173 +
174 +/*
175 + * Stride is a 16 bit register, but also has to be a multiple of 32.
176 + */
177 +#define BPL_ALIGNMENT 32
178 +#define MAX_BYTESPERLINE ((1 << 16) - BPL_ALIGNMENT)
179 +/*
180 + * Max width is therefore determined by the max stride divided by
181 + * the number of bits per pixel. Take 32bpp as a
182 + * worst case.
183 + * No imposed limit on the height, so adopt a square image for want
184 + * of anything better.
185 + */
186 +#define MAX_WIDTH (MAX_BYTESPERLINE / 4)
187 +#define MAX_HEIGHT MAX_WIDTH
188 +/* Define a nominal minimum image size */
189 +#define MIN_WIDTH 16
190 +#define MIN_HEIGHT 16
191 +/* Default size of the embedded buffer */
192 +#define UNICAM_EMBEDDED_SIZE 8192
193 +
194 +/*
195 + * Size of the dummy buffer. Can be any size really, but the DMA
196 + * allocation works in units of page sizes.
197 + */
198 +#define DUMMY_BUF_SIZE (PAGE_SIZE)
199 +
200 +enum pad_types {
201 + IMAGE_PAD,
202 + METADATA_PAD,
203 + MAX_NODES
204 +};
205 +
206 +/*
207 + * struct unicam_fmt - Unicam media bus format information
208 + * @pixelformat: V4L2 pixel format FCC identifier. 0 if n/a.
209 + * @repacked_fourcc: V4L2 pixel format FCC identifier if the data is expanded
210 + * out to 16bpp. 0 if n/a.
211 + * @code: V4L2 media bus format code.
212 + * @depth: Bits per pixel as delivered from the source.
213 + * @csi_dt: CSI data type.
214 + * @check_variants: Flag to denote that there are multiple mediabus formats
215 + * still in the list that could match this V4L2 format.
216 + */
217 +struct unicam_fmt {
218 + u32 fourcc;
219 + u32 repacked_fourcc;
220 + u32 code;
221 + u8 depth;
222 + u8 csi_dt;
223 + u8 check_variants;
224 +};
225 +
226 +static const struct unicam_fmt formats[] = {
227 + /* YUV Formats */
228 + {
229 + .fourcc = V4L2_PIX_FMT_YUYV,
230 + .code = MEDIA_BUS_FMT_YUYV8_2X8,
231 + .depth = 16,
232 + .csi_dt = 0x1e,
233 + .check_variants = 1,
234 + }, {
235 + .fourcc = V4L2_PIX_FMT_UYVY,
236 + .code = MEDIA_BUS_FMT_UYVY8_2X8,
237 + .depth = 16,
238 + .csi_dt = 0x1e,
239 + .check_variants = 1,
240 + }, {
241 + .fourcc = V4L2_PIX_FMT_YVYU,
242 + .code = MEDIA_BUS_FMT_YVYU8_2X8,
243 + .depth = 16,
244 + .csi_dt = 0x1e,
245 + .check_variants = 1,
246 + }, {
247 + .fourcc = V4L2_PIX_FMT_VYUY,
248 + .code = MEDIA_BUS_FMT_VYUY8_2X8,
249 + .depth = 16,
250 + .csi_dt = 0x1e,
251 + .check_variants = 1,
252 + }, {
253 + .fourcc = V4L2_PIX_FMT_YUYV,
254 + .code = MEDIA_BUS_FMT_YUYV8_1X16,
255 + .depth = 16,
256 + .csi_dt = 0x1e,
257 + }, {
258 + .fourcc = V4L2_PIX_FMT_UYVY,
259 + .code = MEDIA_BUS_FMT_UYVY8_1X16,
260 + .depth = 16,
261 + .csi_dt = 0x1e,
262 + }, {
263 + .fourcc = V4L2_PIX_FMT_YVYU,
264 + .code = MEDIA_BUS_FMT_YVYU8_1X16,
265 + .depth = 16,
266 + .csi_dt = 0x1e,
267 + }, {
268 + .fourcc = V4L2_PIX_FMT_VYUY,
269 + .code = MEDIA_BUS_FMT_VYUY8_1X16,
270 + .depth = 16,
271 + .csi_dt = 0x1e,
272 + }, {
273 + /* RGB Formats */
274 + .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
275 + .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
276 + .depth = 16,
277 + .csi_dt = 0x22,
278 + }, {
279 + .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
280 + .code = MEDIA_BUS_FMT_RGB565_2X8_BE,
281 + .depth = 16,
282 + .csi_dt = 0x22
283 + }, {
284 + .fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
285 + .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
286 + .depth = 16,
287 + .csi_dt = 0x21,
288 + }, {
289 + .fourcc = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
290 + .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
291 + .depth = 16,
292 + .csi_dt = 0x21,
293 + }, {
294 + .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */
295 + .code = MEDIA_BUS_FMT_RGB888_1X24,
296 + .depth = 24,
297 + .csi_dt = 0x24,
298 + }, {
299 + .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */
300 + .code = MEDIA_BUS_FMT_BGR888_1X24,
301 + .depth = 24,
302 + .csi_dt = 0x24,
303 + }, {
304 + .fourcc = V4L2_PIX_FMT_RGB32, /* argb */
305 + .code = MEDIA_BUS_FMT_ARGB8888_1X32,
306 + .depth = 32,
307 + .csi_dt = 0x0,
308 + }, {
309 + /* Bayer Formats */
310 + .fourcc = V4L2_PIX_FMT_SBGGR8,
311 + .code = MEDIA_BUS_FMT_SBGGR8_1X8,
312 + .depth = 8,
313 + .csi_dt = 0x2a,
314 + }, {
315 + .fourcc = V4L2_PIX_FMT_SGBRG8,
316 + .code = MEDIA_BUS_FMT_SGBRG8_1X8,
317 + .depth = 8,
318 + .csi_dt = 0x2a,
319 + }, {
320 + .fourcc = V4L2_PIX_FMT_SGRBG8,
321 + .code = MEDIA_BUS_FMT_SGRBG8_1X8,
322 + .depth = 8,
323 + .csi_dt = 0x2a,
324 + }, {
325 + .fourcc = V4L2_PIX_FMT_SRGGB8,
326 + .code = MEDIA_BUS_FMT_SRGGB8_1X8,
327 + .depth = 8,
328 + .csi_dt = 0x2a,
329 + }, {
330 + .fourcc = V4L2_PIX_FMT_SBGGR10P,
331 + .repacked_fourcc = V4L2_PIX_FMT_SBGGR10,
332 + .code = MEDIA_BUS_FMT_SBGGR10_1X10,
333 + .depth = 10,
334 + .csi_dt = 0x2b,
335 + }, {
336 + .fourcc = V4L2_PIX_FMT_SGBRG10P,
337 + .repacked_fourcc = V4L2_PIX_FMT_SGBRG10,
338 + .code = MEDIA_BUS_FMT_SGBRG10_1X10,
339 + .depth = 10,
340 + .csi_dt = 0x2b,
341 + }, {
342 + .fourcc = V4L2_PIX_FMT_SGRBG10P,
343 + .repacked_fourcc = V4L2_PIX_FMT_SGRBG10,
344 + .code = MEDIA_BUS_FMT_SGRBG10_1X10,
345 + .depth = 10,
346 + .csi_dt = 0x2b,
347 + }, {
348 + .fourcc = V4L2_PIX_FMT_SRGGB10P,
349 + .repacked_fourcc = V4L2_PIX_FMT_SRGGB10,
350 + .code = MEDIA_BUS_FMT_SRGGB10_1X10,
351 + .depth = 10,
352 + .csi_dt = 0x2b,
353 + }, {
354 + .fourcc = V4L2_PIX_FMT_SBGGR12P,
355 + .repacked_fourcc = V4L2_PIX_FMT_SBGGR12,
356 + .code = MEDIA_BUS_FMT_SBGGR12_1X12,
357 + .depth = 12,
358 + .csi_dt = 0x2c,
359 + }, {
360 + .fourcc = V4L2_PIX_FMT_SGBRG12P,
361 + .repacked_fourcc = V4L2_PIX_FMT_SGBRG12,
362 + .code = MEDIA_BUS_FMT_SGBRG12_1X12,
363 + .depth = 12,
364 + .csi_dt = 0x2c,
365 + }, {
366 + .fourcc = V4L2_PIX_FMT_SGRBG12P,
367 + .repacked_fourcc = V4L2_PIX_FMT_SGRBG12,
368 + .code = MEDIA_BUS_FMT_SGRBG12_1X12,
369 + .depth = 12,
370 + .csi_dt = 0x2c,
371 + }, {
372 + .fourcc = V4L2_PIX_FMT_SRGGB12P,
373 + .repacked_fourcc = V4L2_PIX_FMT_SRGGB12,
374 + .code = MEDIA_BUS_FMT_SRGGB12_1X12,
375 + .depth = 12,
376 + .csi_dt = 0x2c,
377 + }, {
378 + .fourcc = V4L2_PIX_FMT_SBGGR14P,
379 + .code = MEDIA_BUS_FMT_SBGGR14_1X14,
380 + .depth = 14,
381 + .csi_dt = 0x2d,
382 + }, {
383 + .fourcc = V4L2_PIX_FMT_SGBRG14P,
384 + .code = MEDIA_BUS_FMT_SGBRG14_1X14,
385 + .depth = 14,
386 + .csi_dt = 0x2d,
387 + }, {
388 + .fourcc = V4L2_PIX_FMT_SGRBG14P,
389 + .code = MEDIA_BUS_FMT_SGRBG14_1X14,
390 + .depth = 14,
391 + .csi_dt = 0x2d,
392 + }, {
393 + .fourcc = V4L2_PIX_FMT_SRGGB14P,
394 + .code = MEDIA_BUS_FMT_SRGGB14_1X14,
395 + .depth = 14,
396 + .csi_dt = 0x2d,
397 + }, {
398 + /*
399 + * 16 bit Bayer formats could be supported, but there is no CSI2
400 + * data_type defined for raw 16, and no sensors that produce it at
401 + * present.
402 + */
403 +
404 + /* Greyscale formats */
405 + .fourcc = V4L2_PIX_FMT_GREY,
406 + .code = MEDIA_BUS_FMT_Y8_1X8,
407 + .depth = 8,
408 + .csi_dt = 0x2a,
409 + }, {
410 + .fourcc = V4L2_PIX_FMT_Y10P,
411 + .repacked_fourcc = V4L2_PIX_FMT_Y10,
412 + .code = MEDIA_BUS_FMT_Y10_1X10,
413 + .depth = 10,
414 + .csi_dt = 0x2b,
415 + }, {
416 + /* NB There is no packed V4L2 fourcc for this format. */
417 + .repacked_fourcc = V4L2_PIX_FMT_Y12,
418 + .code = MEDIA_BUS_FMT_Y12_1X12,
419 + .depth = 12,
420 + .csi_dt = 0x2c,
421 + },
422 + /* Embedded data format */
423 + {
424 + .fourcc = V4L2_META_FMT_SENSOR_DATA,
425 + .code = MEDIA_BUS_FMT_SENSOR_DATA,
426 + .depth = 8,
427 + }
428 +};
429 +
430 +struct unicam_buffer {
431 + struct vb2_v4l2_buffer vb;
432 + struct list_head list;
433 +};
434 +
435 +static inline struct unicam_buffer *to_unicam_buffer(struct vb2_buffer *vb)
436 +{
437 + return container_of(vb, struct unicam_buffer, vb.vb2_buf);
438 +}
439 +
440 +struct unicam_node {
441 + bool registered;
442 + int open;
443 + bool streaming;
444 + unsigned int pad_id;
445 + /* Pointer pointing to current v4l2_buffer */
446 + struct unicam_buffer *cur_frm;
447 + /* Pointer pointing to next v4l2_buffer */
448 + struct unicam_buffer *next_frm;
449 + /* video capture */
450 + const struct unicam_fmt *fmt;
451 + /* Used to store current pixel format */
452 + struct v4l2_format v_fmt;
453 + /* Used to store current mbus frame format */
454 + struct v4l2_mbus_framefmt m_fmt;
455 + /* Buffer queue used in video-buf */
456 + struct vb2_queue buffer_queue;
457 + /* Queue of filled frames */
458 + struct list_head dma_queue;
459 + /* IRQ lock for DMA queue */
460 + spinlock_t dma_queue_lock;
461 + /* lock used to access this structure */
462 + struct mutex lock;
463 + /* Identifies video device for this channel */
464 + struct video_device video_dev;
465 + /* Pointer to the parent handle */
466 + struct unicam_device *dev;
467 + struct media_pad pad;
468 + unsigned int embedded_lines;
469 + /*
470 + * Dummy buffer intended to be used by unicam
471 + * if we have no other queued buffers to swap to.
472 + */
473 + void *dummy_buf_cpu_addr;
474 + dma_addr_t dummy_buf_dma_addr;
475 +};
476 +
477 +struct unicam_device {
478 + struct kref kref;
479 +
480 + /* V4l2 specific parameters */
481 + struct v4l2_async_subdev asd;
482 +
483 + /* peripheral base address */
484 + void __iomem *base;
485 + /* clock gating base address */
486 + void __iomem *clk_gate_base;
487 + /* clock handle */
488 + struct clk *clock;
489 + /* V4l2 device */
490 + struct v4l2_device v4l2_dev;
491 + struct media_device mdev;
492 +
493 + /* parent device */
494 + struct platform_device *pdev;
495 + /* subdevice async Notifier */
496 + struct v4l2_async_notifier notifier;
497 + unsigned int sequence;
498 +
499 + /* ptr to sub device */
500 + struct v4l2_subdev *sensor;
501 + /* Pad config for the sensor */
502 + struct v4l2_subdev_pad_config *sensor_config;
503 +
504 + enum v4l2_mbus_type bus_type;
505 + /*
506 + * Stores bus.mipi_csi2.flags for CSI2 sensors, or
507 + * bus.mipi_csi1.strobe for CCP2.
508 + */
509 + unsigned int bus_flags;
510 + unsigned int max_data_lanes;
511 + unsigned int active_data_lanes;
512 + bool sensor_embedded_data;
513 +
514 + struct unicam_node node[MAX_NODES];
515 + struct v4l2_ctrl_handler ctrl_handler;
516 +};
517 +
518 +static inline struct unicam_device *
519 +to_unicam_device(struct v4l2_device *v4l2_dev)
520 +{
521 + return container_of(v4l2_dev, struct unicam_device, v4l2_dev);
522 +}
523 +
524 +/* Hardware access */
525 +static inline void clk_write(struct unicam_device *dev, u32 val)
526 +{
527 + writel(val | 0x5a000000, dev->clk_gate_base);
528 +}
529 +
530 +static inline u32 reg_read(struct unicam_device *dev, u32 offset)
531 +{
532 + return readl(dev->base + offset);
533 +}
534 +
535 +static inline void reg_write(struct unicam_device *dev, u32 offset, u32 val)
536 +{
537 + writel(val, dev->base + offset);
538 +}
539 +
540 +static inline int get_field(u32 value, u32 mask)
541 +{
542 + return (value & mask) >> __ffs(mask);
543 +}
544 +
545 +static inline void set_field(u32 *valp, u32 field, u32 mask)
546 +{
547 + u32 val = *valp;
548 +
549 + val &= ~mask;
550 + val |= (field << __ffs(mask)) & mask;
551 + *valp = val;
552 +}
553 +
554 +static inline u32 reg_read_field(struct unicam_device *dev, u32 offset,
555 + u32 mask)
556 +{
557 + return get_field(reg_read(dev, offset), mask);
558 +}
559 +
560 +static inline void reg_write_field(struct unicam_device *dev, u32 offset,
561 + u32 field, u32 mask)
562 +{
563 + u32 val = reg_read(dev, offset);
564 +
565 + set_field(&val, field, mask);
566 + reg_write(dev, offset, val);
567 +}
568 +
569 +/* Power management functions */
570 +static inline int unicam_runtime_get(struct unicam_device *dev)
571 +{
572 + return pm_runtime_get_sync(&dev->pdev->dev);
573 +}
574 +
575 +static inline void unicam_runtime_put(struct unicam_device *dev)
576 +{
577 + pm_runtime_put_sync(&dev->pdev->dev);
578 +}
579 +
580 +/* Format setup functions */
581 +static const struct unicam_fmt *find_format_by_code(u32 code)
582 +{
583 + unsigned int i;
584 +
585 + for (i = 0; i < ARRAY_SIZE(formats); i++) {
586 + if (formats[i].code == code)
587 + return &formats[i];
588 + }
589 +
590 + return NULL;
591 +}
592 +
593 +static int check_mbus_format(struct unicam_device *dev,
594 + const struct unicam_fmt *format)
595 +{
596 + unsigned int i;
597 + int ret = 0;
598 +
599 + for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) {
600 + struct v4l2_subdev_mbus_code_enum mbus_code = {
601 + .index = i,
602 + .pad = IMAGE_PAD,
603 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
604 + };
605 +
606 + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
607 + NULL, &mbus_code);
608 +
609 + if (!ret && mbus_code.code == format->code)
610 + return 1;
611 + }
612 +
613 + return 0;
614 +}
615 +
616 +static const struct unicam_fmt *find_format_by_pix(struct unicam_device *dev,
617 + u32 pixelformat)
618 +{
619 + unsigned int i;
620 +
621 + for (i = 0; i < ARRAY_SIZE(formats); i++) {
622 + if (formats[i].fourcc == pixelformat ||
623 + formats[i].repacked_fourcc == pixelformat) {
624 + if (formats[i].check_variants &&
625 + !check_mbus_format(dev, &formats[i]))
626 + continue;
627 + return &formats[i];
628 + }
629 + }
630 +
631 + return NULL;
632 +}
633 +
634 +static inline unsigned int bytes_per_line(u32 width,
635 + const struct unicam_fmt *fmt,
636 + u32 v4l2_fourcc)
637 +{
638 + if (v4l2_fourcc == fmt->repacked_fourcc)
639 + /* Repacking always goes to 16bpp */
640 + return ALIGN(width << 1, BPL_ALIGNMENT);
641 + else
642 + return ALIGN((width * fmt->depth) >> 3, BPL_ALIGNMENT);
643 +}
644 +
645 +static int __subdev_get_format(struct unicam_device *dev,
646 + struct v4l2_mbus_framefmt *fmt, int pad_id)
647 +{
648 + struct v4l2_subdev_format sd_fmt = {
649 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
650 + .pad = pad_id
651 + };
652 + int ret;
653 +
654 + ret = v4l2_subdev_call(dev->sensor, pad, get_fmt, dev->sensor_config,
655 + &sd_fmt);
656 + if (ret < 0)
657 + return ret;
658 +
659 + *fmt = sd_fmt.format;
660 +
661 + unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__,
662 + fmt->width, fmt->height, fmt->code);
663 +
664 + return 0;
665 +}
666 +
667 +static int __subdev_set_format(struct unicam_device *dev,
668 + struct v4l2_mbus_framefmt *fmt, int pad_id)
669 +{
670 + struct v4l2_subdev_format sd_fmt = {
671 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
672 + .pad = pad_id
673 + };
674 + int ret;
675 +
676 + sd_fmt.format = *fmt;
677 +
678 + ret = v4l2_subdev_call(dev->sensor, pad, set_fmt, dev->sensor_config,
679 + &sd_fmt);
680 + if (ret < 0)
681 + return ret;
682 +
683 + *fmt = sd_fmt.format;
684 +
685 + if (pad_id == IMAGE_PAD)
686 + unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__, fmt->width,
687 + fmt->height, fmt->code);
688 + else
689 + unicam_dbg(1, dev, "%s Embedded data code:%04x\n", __func__,
690 + sd_fmt.format.code);
691 +
692 + return 0;
693 +}
694 +
695 +static int unicam_calc_format_size_bpl(struct unicam_device *dev,
696 + const struct unicam_fmt *fmt,
697 + struct v4l2_format *f)
698 +{
699 + unsigned int min_bytesperline;
700 +
701 + v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, MAX_WIDTH, 2,
702 + &f->fmt.pix.height, MIN_HEIGHT, MAX_HEIGHT, 0,
703 + 0);
704 +
705 + min_bytesperline = bytes_per_line(f->fmt.pix.width, fmt,
706 + f->fmt.pix.pixelformat);
707 +
708 + if (f->fmt.pix.bytesperline > min_bytesperline &&
709 + f->fmt.pix.bytesperline <= MAX_BYTESPERLINE)
710 + f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.bytesperline,
711 + BPL_ALIGNMENT);
712 + else
713 + f->fmt.pix.bytesperline = min_bytesperline;
714 +
715 + f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
716 +
717 + unicam_dbg(3, dev, "%s: fourcc: %08X size: %dx%d bpl:%d img_size:%d\n",
718 + __func__,
719 + f->fmt.pix.pixelformat,
720 + f->fmt.pix.width, f->fmt.pix.height,
721 + f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
722 +
723 + return 0;
724 +}
725 +
726 +static int unicam_reset_format(struct unicam_node *node)
727 +{
728 + struct unicam_device *dev = node->dev;
729 + struct v4l2_mbus_framefmt mbus_fmt;
730 + int ret;
731 +
732 + if (dev->sensor_embedded_data || node->pad_id != METADATA_PAD) {
733 + ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id);
734 + if (ret) {
735 + unicam_err(dev, "Failed to get_format - ret %d\n", ret);
736 + return ret;
737 + }
738 +
739 + if (mbus_fmt.code != node->fmt->code) {
740 + unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n",
741 + node->fmt->code, mbus_fmt.code);
742 + return ret;
743 + }
744 + }
745 +
746 + if (node->pad_id == IMAGE_PAD) {
747 + v4l2_fill_pix_format(&node->v_fmt.fmt.pix, &mbus_fmt);
748 + node->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
749 + unicam_calc_format_size_bpl(dev, node->fmt, &node->v_fmt);
750 + } else {
751 + node->v_fmt.type = V4L2_BUF_TYPE_META_CAPTURE;
752 + node->v_fmt.fmt.meta.dataformat = V4L2_META_FMT_SENSOR_DATA;
753 + if (dev->sensor_embedded_data) {
754 + node->v_fmt.fmt.meta.buffersize =
755 + mbus_fmt.width * mbus_fmt.height;
756 + node->embedded_lines = mbus_fmt.height;
757 + } else {
758 + node->v_fmt.fmt.meta.buffersize = UNICAM_EMBEDDED_SIZE;
759 + node->embedded_lines = 1;
760 + }
761 + }
762 +
763 + node->m_fmt = mbus_fmt;
764 + return 0;
765 +}
766 +
767 +static void unicam_wr_dma_addr(struct unicam_device *dev, dma_addr_t dmaaddr,
768 + unsigned int buffer_size, int pad_id)
769 +{
770 + dma_addr_t endaddr = dmaaddr + buffer_size;
771 +
772 + /*
773 + * dmaaddr and endaddr should be a 32-bit address with the top two bits
774 + * set to 0x3 to signify uncached access through the Videocore memory
775 + * controller.
776 + */
777 + WARN_ON((dmaaddr >> 30) != 0x3 || (endaddr >> 30) != 0x3);
778 +
779 + if (pad_id == IMAGE_PAD) {
780 + reg_write(dev, UNICAM_IBSA0, dmaaddr);
781 + reg_write(dev, UNICAM_IBEA0, endaddr);
782 + } else {
783 + reg_write(dev, UNICAM_DBSA0, dmaaddr);
784 + reg_write(dev, UNICAM_DBEA0, endaddr);
785 + }
786 +}
787 +
788 +static inline unsigned int unicam_get_lines_done(struct unicam_device *dev)
789 +{
790 + dma_addr_t start_addr, cur_addr;
791 + unsigned int stride = dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline;
792 + struct unicam_buffer *frm = dev->node[IMAGE_PAD].cur_frm;
793 +
794 + if (!frm)
795 + return 0;
796 +
797 + start_addr = vb2_dma_contig_plane_dma_addr(&frm->vb.vb2_buf, 0);
798 + cur_addr = reg_read(dev, UNICAM_IBWP);
799 + return (unsigned int)(cur_addr - start_addr) / stride;
800 +}
801 +
802 +static inline void unicam_schedule_next_buffer(struct unicam_node *node)
803 +{
804 + struct unicam_device *dev = node->dev;
805 + struct unicam_buffer *buf;
806 + unsigned int size;
807 + dma_addr_t addr;
808 +
809 + buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list);
810 + node->next_frm = buf;
811 + list_del(&buf->list);
812 +
813 + addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
814 + size = (node->pad_id == IMAGE_PAD) ?
815 + node->v_fmt.fmt.pix.sizeimage :
816 + node->v_fmt.fmt.meta.buffersize;
817 +
818 + unicam_wr_dma_addr(dev, addr, size, node->pad_id);
819 +}
820 +
821 +static inline void unicam_schedule_dummy_buffer(struct unicam_node *node)
822 +{
823 + struct unicam_device *dev = node->dev;
824 +
825 + unicam_dbg(3, dev, "Scheduling dummy buffer for node %d\n",
826 + node->pad_id);
827 +
828 + unicam_wr_dma_addr(dev, node->dummy_buf_dma_addr, DUMMY_BUF_SIZE,
829 + node->pad_id);
830 + node->next_frm = NULL;
831 +}
832 +
833 +static inline void unicam_process_buffer_complete(struct unicam_node *node,
834 + unsigned int sequence)
835 +{
836 + node->cur_frm->vb.field = node->m_fmt.field;
837 + node->cur_frm->vb.sequence = sequence;
838 +
839 + vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
840 +}
841 +
842 +static bool unicam_all_nodes_streaming(struct unicam_device *dev)
843 +{
844 + bool ret;
845 +
846 + ret = dev->node[IMAGE_PAD].open && dev->node[IMAGE_PAD].streaming;
847 + ret &= !dev->node[METADATA_PAD].open ||
848 + dev->node[METADATA_PAD].streaming;
849 + return ret;
850 +}
851 +
852 +static bool unicam_all_nodes_disabled(struct unicam_device *dev)
853 +{
854 + return !dev->node[IMAGE_PAD].streaming &&
855 + !dev->node[METADATA_PAD].streaming;
856 +}
857 +
858 +static void unicam_queue_event_sof(struct unicam_device *unicam)
859 +{
860 + struct v4l2_event event = {
861 + .type = V4L2_EVENT_FRAME_SYNC,
862 + .u.frame_sync.frame_sequence = unicam->sequence,
863 + };
864 +
865 + v4l2_event_queue(&unicam->node[IMAGE_PAD].video_dev, &event);
866 +}
867 +
868 +/*
869 + * unicam_isr : ISR handler for unicam capture
870 + * @irq: irq number
871 + * @dev_id: dev_id ptr
872 + *
873 + * It changes status of the captured buffer, takes next buffer from the queue
874 + * and sets its address in unicam registers
875 + */
876 +static irqreturn_t unicam_isr(int irq, void *dev)
877 +{
878 + struct unicam_device *unicam = dev;
879 + unsigned int lines_done = unicam_get_lines_done(dev);
880 + unsigned int sequence = unicam->sequence;
881 + unsigned int i;
882 + u32 ista, sta;
883 + u64 ts;
884 +
885 + /*
886 + * Don't service interrupts if not streaming.
887 + * Avoids issues if the VPU should enable the
888 + * peripheral without the kernel knowing (that
889 + * shouldn't happen, but causes issues if it does).
890 + */
891 + if (unicam_all_nodes_disabled(unicam))
892 + return IRQ_NONE;
893 +
894 + sta = reg_read(unicam, UNICAM_STA);
895 + /* Write value back to clear the interrupts */
896 + reg_write(unicam, UNICAM_STA, sta);
897 +
898 + ista = reg_read(unicam, UNICAM_ISTA);
899 + /* Write value back to clear the interrupts */
900 + reg_write(unicam, UNICAM_ISTA, ista);
901 +
902 + unicam_dbg(3, unicam, "ISR: ISTA: 0x%X, STA: 0x%X, sequence %d, lines done %d",
903 + ista, sta, sequence, lines_done);
904 +
905 + if (!(sta & (UNICAM_IS | UNICAM_PI0)))
906 + return IRQ_HANDLED;
907 +
908 + /*
909 + * We must run the frame end handler first. If we have a valid next_frm
910 + * and we get a simultaneout FE + FS interrupt, running the FS handler
911 + * first would null out the next_frm ptr and we would have lost the
912 + * buffer forever.
913 + */
914 + if (ista & UNICAM_FEI || sta & UNICAM_PI0) {
915 + /*
916 + * Ensure we have swapped buffers already as we can't
917 + * stop the peripheral. If no buffer is available, use a
918 + * dummy buffer to dump out frames until we get a new buffer
919 + * to use.
920 + */
921 + for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
922 + if (!unicam->node[i].streaming)
923 + continue;
924 +
925 + if (unicam->node[i].cur_frm)
926 + unicam_process_buffer_complete(&unicam->node[i],
927 + sequence);
928 + unicam->node[i].cur_frm = unicam->node[i].next_frm;
929 + }
930 + unicam->sequence++;
931 + }
932 +
933 + if (ista & UNICAM_FSI) {
934 + /*
935 + * Timestamp is to be when the first data byte was captured,
936 + * aka frame start.
937 + */
938 + ts = ktime_get_ns();
939 + for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
940 + if (!unicam->node[i].streaming)
941 + continue;
942 +
943 + if (unicam->node[i].cur_frm)
944 + unicam->node[i].cur_frm->vb.vb2_buf.timestamp =
945 + ts;
946 + /*
947 + * Set the next frame output to go to a dummy frame
948 + * if we have not managed to obtain another frame
949 + * from the queue.
950 + */
951 + unicam_schedule_dummy_buffer(&unicam->node[i]);
952 + }
953 +
954 + unicam_queue_event_sof(unicam);
955 + }
956 +
957 + /*
958 + * Cannot swap buffer at frame end, there may be a race condition
959 + * where the HW does not actually swap it if the new frame has
960 + * already started.
961 + */
962 + if (ista & (UNICAM_FSI | UNICAM_LCI) && !(ista & UNICAM_FEI)) {
963 + for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
964 + if (!unicam->node[i].streaming)
965 + continue;
966 +
967 + spin_lock(&unicam->node[i].dma_queue_lock);
968 + if (!list_empty(&unicam->node[i].dma_queue) &&
969 + !unicam->node[i].next_frm)
970 + unicam_schedule_next_buffer(&unicam->node[i]);
971 + spin_unlock(&unicam->node[i].dma_queue_lock);
972 + }
973 + }
974 +
975 + if (reg_read(unicam, UNICAM_ICTL) & UNICAM_FCM) {
976 + /* Switch out of trigger mode if selected */
977 + reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC);
978 + reg_write_field(unicam, UNICAM_ICTL, 0, UNICAM_FCM);
979 + }
980 + return IRQ_HANDLED;
981 +}
982 +
983 +static int unicam_querycap(struct file *file, void *priv,
984 + struct v4l2_capability *cap)
985 +{
986 + struct unicam_node *node = video_drvdata(file);
987 + struct unicam_device *dev = node->dev;
988 +
989 + strlcpy(cap->driver, UNICAM_MODULE_NAME, sizeof(cap->driver));
990 + strlcpy(cap->card, UNICAM_MODULE_NAME, sizeof(cap->card));
991 +
992 + snprintf(cap->bus_info, sizeof(cap->bus_info),
993 + "platform:%s", dev_name(&dev->pdev->dev));
994 +
995 + cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE;
996 +
997 + return 0;
998 +}
999 +
1000 +static int unicam_enum_fmt_vid_cap(struct file *file, void *priv,
1001 + struct v4l2_fmtdesc *f)
1002 +{
1003 + struct unicam_node *node = video_drvdata(file);
1004 + struct unicam_device *dev = node->dev;
1005 + unsigned int index = 0;
1006 + unsigned int i;
1007 + int ret = 0;
1008 +
1009 + if (node->pad_id != IMAGE_PAD)
1010 + return -EINVAL;
1011 +
1012 + for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) {
1013 + struct v4l2_subdev_mbus_code_enum mbus_code = {
1014 + .index = i,
1015 + .pad = IMAGE_PAD,
1016 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1017 + };
1018 + const struct unicam_fmt *fmt;
1019 +
1020 + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
1021 + NULL, &mbus_code);
1022 + if (ret < 0) {
1023 + unicam_dbg(2, dev,
1024 + "subdev->enum_mbus_code idx %d returned %d - index invalid\n",
1025 + i, ret);
1026 + return -EINVAL;
1027 + }
1028 +
1029 + fmt = find_format_by_code(mbus_code.code);
1030 + if (fmt) {
1031 + if (fmt->fourcc) {
1032 + if (index == f->index) {
1033 + f->pixelformat = fmt->fourcc;
1034 + break;
1035 + }
1036 + index++;
1037 + }
1038 + if (fmt->repacked_fourcc) {
1039 + if (index == f->index) {
1040 + f->pixelformat = fmt->repacked_fourcc;
1041 + break;
1042 + }
1043 + index++;
1044 + }
1045 + }
1046 + }
1047 +
1048 + return 0;
1049 +}
1050 +
1051 +static int unicam_g_fmt_vid_cap(struct file *file, void *priv,
1052 + struct v4l2_format *f)
1053 +{
1054 + struct v4l2_mbus_framefmt mbus_fmt = {0};
1055 + struct unicam_node *node = video_drvdata(file);
1056 + struct unicam_device *dev = node->dev;
1057 + const struct unicam_fmt *fmt = NULL;
1058 + int ret;
1059 +
1060 + if (node->pad_id != IMAGE_PAD)
1061 + return -EINVAL;
1062 +
1063 + /*
1064 + * If a flip has occurred in the sensor, the fmt code might have
1065 + * changed. So we will need to re-fetch the format from the subdevice.
1066 + */
1067 + ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id);
1068 + if (ret)
1069 + return -EINVAL;
1070 +
1071 + /* Find the V4L2 format from mbus code. We must match a known format. */
1072 + fmt = find_format_by_code(mbus_fmt.code);
1073 + if (!fmt)
1074 + return -EINVAL;
1075 +
1076 + node->fmt = fmt;
1077 + node->v_fmt.fmt.pix.pixelformat = fmt->fourcc;
1078 + *f = node->v_fmt;
1079 +
1080 + return 0;
1081 +}
1082 +
1083 +static
1084 +const struct unicam_fmt *get_first_supported_format(struct unicam_device *dev)
1085 +{
1086 + struct v4l2_subdev_mbus_code_enum mbus_code;
1087 + const struct unicam_fmt *fmt = NULL;
1088 + unsigned int i;
1089 + int ret;
1090 +
1091 + for (i = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++i) {
1092 + memset(&mbus_code, 0, sizeof(mbus_code));
1093 + mbus_code.index = i;
1094 + mbus_code.pad = IMAGE_PAD;
1095 + mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1096 +
1097 + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
1098 + &mbus_code);
1099 + if (ret < 0) {
1100 + unicam_dbg(2, dev,
1101 + "subdev->enum_mbus_code idx %u returned %d - continue\n",
1102 + i, ret);
1103 + continue;
1104 + }
1105 +
1106 + unicam_dbg(2, dev, "subdev %s: code: 0x%08x idx: %u\n",
1107 + dev->sensor->name, mbus_code.code, i);
1108 +
1109 + fmt = find_format_by_code(mbus_code.code);
1110 + unicam_dbg(2, dev, "fmt 0x%08x returned as %p, V4L2 FOURCC 0x%08x, csi_dt 0x%02x\n",
1111 + mbus_code.code, fmt, fmt ? fmt->fourcc : 0,
1112 + fmt ? fmt->csi_dt : 0);
1113 + if (fmt)
1114 + return fmt;
1115 + }
1116 +
1117 + return NULL;
1118 +}
1119 +
1120 +static int unicam_try_fmt_vid_cap(struct file *file, void *priv,
1121 + struct v4l2_format *f)
1122 +{
1123 + struct unicam_node *node = video_drvdata(file);
1124 + struct unicam_device *dev = node->dev;
1125 + struct v4l2_subdev_format sd_fmt = {
1126 + .which = V4L2_SUBDEV_FORMAT_TRY,
1127 + .pad = IMAGE_PAD
1128 + };
1129 + struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
1130 + const struct unicam_fmt *fmt;
1131 + int ret;
1132 +
1133 + if (node->pad_id != IMAGE_PAD)
1134 + return -EINVAL;
1135 +
1136 + fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
1137 + if (!fmt) {
1138 + /*
1139 + * Pixel format not supported by unicam. Choose the first
1140 + * supported format, and let the sensor choose something else.
1141 + */
1142 + unicam_dbg(3, dev, "Fourcc format (0x%08x) not found. Use first format.\n",
1143 + f->fmt.pix.pixelformat);
1144 +
1145 + fmt = &formats[0];
1146 + f->fmt.pix.pixelformat = fmt->fourcc;
1147 + }
1148 +
1149 + v4l2_fill_mbus_format(mbus_fmt, &f->fmt.pix, fmt->code);
1150 + /*
1151 + * No support for receiving interlaced video, so never
1152 + * request it from the sensor subdev.
1153 + */
1154 + mbus_fmt->field = V4L2_FIELD_NONE;
1155 +
1156 + ret = v4l2_subdev_call(dev->sensor, pad, set_fmt, dev->sensor_config,
1157 + &sd_fmt);
1158 + if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
1159 + return ret;
1160 +
1161 + if (mbus_fmt->field != V4L2_FIELD_NONE)
1162 + unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
1163 +
1164 + v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
1165 + if (mbus_fmt->code != fmt->code) {
1166 + /* Sensor has returned an alternate format */
1167 + fmt = find_format_by_code(mbus_fmt->code);
1168 + if (!fmt) {
1169 + /*
1170 + * The alternate format is one unicam can't support.
1171 + * Find the first format that is supported by both, and
1172 + * then set that.
1173 + */
1174 + fmt = get_first_supported_format(dev);
1175 + mbus_fmt->code = fmt->code;
1176 +
1177 + ret = v4l2_subdev_call(dev->sensor, pad, set_fmt,
1178 + dev->sensor_config, &sd_fmt);
1179 + if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
1180 + return ret;
1181 +
1182 + if (mbus_fmt->field != V4L2_FIELD_NONE)
1183 + unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
1184 +
1185 + v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
1186 +
1187 + if (mbus_fmt->code != fmt->code) {
1188 + /*
1189 + * We've set a format that the sensor reports
1190 + * as being supported, but it refuses to set it.
1191 + * Not much else we can do.
1192 + * Assume that the sensor driver may accept the
1193 + * format when it is set (rather than tried).
1194 + */
1195 + unicam_err(dev, "Sensor won't accept default format, and Unicam can't support sensor default\n");
1196 + }
1197 + }
1198 +
1199 + if (fmt->fourcc)
1200 + f->fmt.pix.pixelformat = fmt->fourcc;
1201 + else
1202 + f->fmt.pix.pixelformat = fmt->repacked_fourcc;
1203 + }
1204 +
1205 + return unicam_calc_format_size_bpl(dev, fmt, f);
1206 +}
1207 +
1208 +static int unicam_s_fmt_vid_cap(struct file *file, void *priv,
1209 + struct v4l2_format *f)
1210 +{
1211 + struct unicam_node *node = video_drvdata(file);
1212 + struct unicam_device *dev = node->dev;
1213 + struct vb2_queue *q = &node->buffer_queue;
1214 + struct v4l2_mbus_framefmt mbus_fmt = {0};
1215 + const struct unicam_fmt *fmt;
1216 + int ret;
1217 +
1218 + if (vb2_is_busy(q))
1219 + return -EBUSY;
1220 +
1221 + ret = unicam_try_fmt_vid_cap(file, priv, f);
1222 + if (ret < 0)
1223 + return ret;
1224 +
1225 + fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
1226 + if (!fmt) {
1227 + /*
1228 + * Unknown pixel format - adopt a default.
1229 + * This shouldn't happen as try_fmt should have resolved any
1230 + * issues first.
1231 + */
1232 + fmt = get_first_supported_format(dev);
1233 + if (!fmt)
1234 + /*
1235 + * It shouldn't be possible to get here with no
1236 + * supported formats
1237 + */
1238 + return -EINVAL;
1239 + f->fmt.pix.pixelformat = fmt->fourcc;
1240 + return -EINVAL;
1241 + }
1242 +
1243 + v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code);
1244 +
1245 + ret = __subdev_set_format(dev, &mbus_fmt, node->pad_id);
1246 + if (ret) {
1247 + unicam_dbg(3, dev, "%s __subdev_set_format failed %d\n",
1248 + __func__, ret);
1249 + return ret;
1250 + }
1251 +
1252 + /* Just double check nothing has gone wrong */
1253 + if (mbus_fmt.code != fmt->code) {
1254 + unicam_dbg(3, dev,
1255 + "%s subdev changed format on us, this should not happen\n",
1256 + __func__);
1257 + return -EINVAL;
1258 + }
1259 +
1260 + node->fmt = fmt;
1261 + node->v_fmt.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
1262 + node->v_fmt.fmt.pix.bytesperline = f->fmt.pix.bytesperline;
1263 + unicam_reset_format(node);
1264 +
1265 + unicam_dbg(3, dev,
1266 + "%s %dx%d, mbus_fmt 0x%08X, V4L2 pix 0x%08X.\n",
1267 + __func__, node->v_fmt.fmt.pix.width,
1268 + node->v_fmt.fmt.pix.height, mbus_fmt.code,
1269 + node->v_fmt.fmt.pix.pixelformat);
1270 +
1271 + *f = node->v_fmt;
1272 +
1273 + return 0;
1274 +}
1275 +
1276 +static int unicam_enum_fmt_meta_cap(struct file *file, void *priv,
1277 + struct v4l2_fmtdesc *f)
1278 +{
1279 + struct unicam_node *node = video_drvdata(file);
1280 + struct unicam_device *dev = node->dev;
1281 + const struct unicam_fmt *fmt;
1282 + u32 code;
1283 + int ret = 0;
1284 +
1285 + if (node->pad_id != METADATA_PAD || f->index != 0)
1286 + return -EINVAL;
1287 +
1288 + if (dev->sensor_embedded_data) {
1289 + struct v4l2_subdev_mbus_code_enum mbus_code = {
1290 + .index = f->index,
1291 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1292 + .pad = METADATA_PAD,
1293 + };
1294 +
1295 + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
1296 + &mbus_code);
1297 + if (ret < 0) {
1298 + unicam_dbg(2, dev,
1299 + "subdev->enum_mbus_code idx 0 returned %d - index invalid\n",
1300 + ret);
1301 + return -EINVAL;
1302 + }
1303 +
1304 + code = mbus_code.code;
1305 + } else {
1306 + code = MEDIA_BUS_FMT_SENSOR_DATA;
1307 + }
1308 +
1309 + fmt = find_format_by_code(code);
1310 + if (fmt)
1311 + f->pixelformat = fmt->fourcc;
1312 +
1313 + return 0;
1314 +}
1315 +
1316 +static int unicam_g_fmt_meta_cap(struct file *file, void *priv,
1317 + struct v4l2_format *f)
1318 +{
1319 + struct unicam_node *node = video_drvdata(file);
1320 +
1321 + if (node->pad_id != METADATA_PAD)
1322 + return -EINVAL;
1323 +
1324 + *f = node->v_fmt;
1325 +
1326 + return 0;
1327 +}
1328 +
1329 +static int unicam_queue_setup(struct vb2_queue *vq,
1330 + unsigned int *nbuffers,
1331 + unsigned int *nplanes,
1332 + unsigned int sizes[],
1333 + struct device *alloc_devs[])
1334 +{
1335 + struct unicam_node *node = vb2_get_drv_priv(vq);
1336 + struct unicam_device *dev = node->dev;
1337 + unsigned int size = node->pad_id == IMAGE_PAD ?
1338 + node->v_fmt.fmt.pix.sizeimage :
1339 + node->v_fmt.fmt.meta.buffersize;
1340 +
1341 + if (vq->num_buffers + *nbuffers < 3)
1342 + *nbuffers = 3 - vq->num_buffers;
1343 +
1344 + if (*nplanes) {
1345 + if (sizes[0] < size) {
1346 + unicam_err(dev, "sizes[0] %i < size %u\n", sizes[0],
1347 + size);
1348 + return -EINVAL;
1349 + }
1350 + size = sizes[0];
1351 + }
1352 +
1353 + *nplanes = 1;
1354 + sizes[0] = size;
1355 +
1356 + return 0;
1357 +}
1358 +
1359 +static int unicam_buffer_prepare(struct vb2_buffer *vb)
1360 +{
1361 + struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
1362 + struct unicam_device *dev = node->dev;
1363 + struct unicam_buffer *buf = to_unicam_buffer(vb);
1364 + unsigned long size;
1365 +
1366 + if (WARN_ON(!node->fmt))
1367 + return -EINVAL;
1368 +
1369 + size = node->pad_id == IMAGE_PAD ? node->v_fmt.fmt.pix.sizeimage :
1370 + node->v_fmt.fmt.meta.buffersize;
1371 + if (vb2_plane_size(vb, 0) < size) {
1372 + unicam_err(dev, "data will not fit into plane (%lu < %lu)\n",
1373 + vb2_plane_size(vb, 0), size);
1374 + return -EINVAL;
1375 + }
1376 +
1377 + vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
1378 + return 0;
1379 +}
1380 +
1381 +static void unicam_buffer_queue(struct vb2_buffer *vb)
1382 +{
1383 + struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
1384 + struct unicam_buffer *buf = to_unicam_buffer(vb);
1385 + unsigned long flags;
1386 +
1387 + spin_lock_irqsave(&node->dma_queue_lock, flags);
1388 + list_add_tail(&buf->list, &node->dma_queue);
1389 + spin_unlock_irqrestore(&node->dma_queue_lock, flags);
1390 +}
1391 +
1392 +static void unicam_set_packing_config(struct unicam_device *dev)
1393 +{
1394 + u32 pack, unpack;
1395 + u32 val;
1396 +
1397 + if (dev->node[IMAGE_PAD].v_fmt.fmt.pix.pixelformat ==
1398 + dev->node[IMAGE_PAD].fmt->fourcc) {
1399 + unpack = UNICAM_PUM_NONE;
1400 + pack = UNICAM_PPM_NONE;
1401 + } else {
1402 + switch (dev->node[IMAGE_PAD].fmt->depth) {
1403 + case 8:
1404 + unpack = UNICAM_PUM_UNPACK8;
1405 + break;
1406 + case 10:
1407 + unpack = UNICAM_PUM_UNPACK10;
1408 + break;
1409 + case 12:
1410 + unpack = UNICAM_PUM_UNPACK12;
1411 + break;
1412 + case 14:
1413 + unpack = UNICAM_PUM_UNPACK14;
1414 + break;
1415 + case 16:
1416 + unpack = UNICAM_PUM_UNPACK16;
1417 + break;
1418 + default:
1419 + unpack = UNICAM_PUM_NONE;
1420 + break;
1421 + }
1422 +
1423 + /* Repacking is always to 16bpp */
1424 + pack = UNICAM_PPM_PACK16;
1425 + }
1426 +
1427 + val = 0;
1428 + set_field(&val, unpack, UNICAM_PUM_MASK);
1429 + set_field(&val, pack, UNICAM_PPM_MASK);
1430 + reg_write(dev, UNICAM_IPIPE, val);
1431 +}
1432 +
1433 +static void unicam_cfg_image_id(struct unicam_device *dev)
1434 +{
1435 + if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
1436 + /* CSI2 mode, hardcode VC 0 for now. */
1437 + reg_write(dev, UNICAM_IDI0,
1438 + (0 << 6) | dev->node[IMAGE_PAD].fmt->csi_dt);
1439 + } else {
1440 + /* CCP2 mode */
1441 + reg_write(dev, UNICAM_IDI0,
1442 + 0x80 | dev->node[IMAGE_PAD].fmt->csi_dt);
1443 + }
1444 +}
1445 +
1446 +static void unicam_enable_ed(struct unicam_device *dev)
1447 +{
1448 + u32 val = reg_read(dev, UNICAM_DCS);
1449 +
1450 + set_field(&val, 2, UNICAM_EDL_MASK);
1451 + /* Do not wrap at the end of the embedded data buffer */
1452 + set_field(&val, 0, UNICAM_DBOB);
1453 +
1454 + reg_write(dev, UNICAM_DCS, val);
1455 +}
1456 +
1457 +static void unicam_start_rx(struct unicam_device *dev, dma_addr_t *addr)
1458 +{
1459 + int line_int_freq = dev->node[IMAGE_PAD].v_fmt.fmt.pix.height >> 2;
1460 + unsigned int size, i;
1461 + u32 val;
1462 +
1463 + if (line_int_freq < 128)
1464 + line_int_freq = 128;
1465 +
1466 + /* Enable lane clocks */
1467 + val = 1;
1468 + for (i = 0; i < dev->active_data_lanes; i++)
1469 + val = val << 2 | 1;
1470 + clk_write(dev, val);
1471 +
1472 + /* Basic init */
1473 + reg_write(dev, UNICAM_CTRL, UNICAM_MEM);
1474 +
1475 + /* Enable analogue control, and leave in reset. */
1476 + val = UNICAM_AR;
1477 + set_field(&val, 7, UNICAM_CTATADJ_MASK);
1478 + set_field(&val, 7, UNICAM_PTATADJ_MASK);
1479 + reg_write(dev, UNICAM_ANA, val);
1480 + usleep_range(1000, 2000);
1481 +
1482 + /* Come out of reset */
1483 + reg_write_field(dev, UNICAM_ANA, 0, UNICAM_AR);
1484 +
1485 + /* Peripheral reset */
1486 + reg_write_field(dev, UNICAM_CTRL, 1, UNICAM_CPR);
1487 + reg_write_field(dev, UNICAM_CTRL, 0, UNICAM_CPR);
1488 +
1489 + reg_write_field(dev, UNICAM_CTRL, 0, UNICAM_CPE);
1490 +
1491 + /* Enable Rx control. */
1492 + val = reg_read(dev, UNICAM_CTRL);
1493 + if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
1494 + set_field(&val, UNICAM_CPM_CSI2, UNICAM_CPM_MASK);
1495 + set_field(&val, UNICAM_DCM_STROBE, UNICAM_DCM_MASK);
1496 + } else {
1497 + set_field(&val, UNICAM_CPM_CCP2, UNICAM_CPM_MASK);
1498 + set_field(&val, dev->bus_flags, UNICAM_DCM_MASK);
1499 + }
1500 + /* Packet framer timeout */
1501 + set_field(&val, 0xf, UNICAM_PFT_MASK);
1502 + set_field(&val, 128, UNICAM_OET_MASK);
1503 + reg_write(dev, UNICAM_CTRL, val);
1504 +
1505 + reg_write(dev, UNICAM_IHWIN, 0);
1506 + reg_write(dev, UNICAM_IVWIN, 0);
1507 +
1508 + /* AXI bus access QoS setup */
1509 + val = reg_read(dev, UNICAM_PRI);
1510 + set_field(&val, 0, UNICAM_BL_MASK);
1511 + set_field(&val, 0, UNICAM_BS_MASK);
1512 + set_field(&val, 0xe, UNICAM_PP_MASK);
1513 + set_field(&val, 8, UNICAM_NP_MASK);
1514 + set_field(&val, 2, UNICAM_PT_MASK);
1515 + set_field(&val, 1, UNICAM_PE);
1516 + reg_write(dev, UNICAM_PRI, val);
1517 +
1518 + reg_write_field(dev, UNICAM_ANA, 0, UNICAM_DDL);
1519 +
1520 + /* Always start in trigger frame capture mode (UNICAM_FCM set) */
1521 + val = UNICAM_FSIE | UNICAM_FEIE | UNICAM_FCM | UNICAM_IBOB;
1522 + set_field(&val, line_int_freq, UNICAM_LCIE_MASK);
1523 + reg_write(dev, UNICAM_ICTL, val);
1524 + reg_write(dev, UNICAM_STA, UNICAM_STA_MASK_ALL);
1525 + reg_write(dev, UNICAM_ISTA, UNICAM_ISTA_MASK_ALL);
1526 +
1527 + /* tclk_term_en */
1528 + reg_write_field(dev, UNICAM_CLT, 2, UNICAM_CLT1_MASK);
1529 + /* tclk_settle */
1530 + reg_write_field(dev, UNICAM_CLT, 6, UNICAM_CLT2_MASK);
1531 + /* td_term_en */
1532 + reg_write_field(dev, UNICAM_DLT, 2, UNICAM_DLT1_MASK);
1533 + /* ths_settle */
1534 + reg_write_field(dev, UNICAM_DLT, 6, UNICAM_DLT2_MASK);
1535 + /* trx_enable */
1536 + reg_write_field(dev, UNICAM_DLT, 0, UNICAM_DLT3_MASK);
1537 +
1538 + reg_write_field(dev, UNICAM_CTRL, 0, UNICAM_SOE);
1539 +
1540 + /* Packet compare setup - required to avoid missing frame ends */
1541 + val = 0;
1542 + set_field(&val, 1, UNICAM_PCE);
1543 + set_field(&val, 1, UNICAM_GI);
1544 + set_field(&val, 1, UNICAM_CPH);
1545 + set_field(&val, 0, UNICAM_PCVC_MASK);
1546 + set_field(&val, 1, UNICAM_PCDT_MASK);
1547 + reg_write(dev, UNICAM_CMP0, val);
1548 +
1549 + /* Enable clock lane and set up terminations */
1550 + val = 0;
1551 + if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
1552 + /* CSI2 */
1553 + set_field(&val, 1, UNICAM_CLE);
1554 + set_field(&val, 1, UNICAM_CLLPE);
1555 + if (dev->bus_flags & V4L2_MBUS_CSI2_CONTINUOUS_CLOCK) {
1556 + set_field(&val, 1, UNICAM_CLTRE);
1557 + set_field(&val, 1, UNICAM_CLHSE);
1558 + }
1559 + } else {
1560 + /* CCP2 */
1561 + set_field(&val, 1, UNICAM_CLE);
1562 + set_field(&val, 1, UNICAM_CLHSE);
1563 + set_field(&val, 1, UNICAM_CLTRE);
1564 + }
1565 + reg_write(dev, UNICAM_CLK, val);
1566 +
1567 + /*
1568 + * Enable required data lanes with appropriate terminations.
1569 + * The same value needs to be written to UNICAM_DATn registers for
1570 + * the active lanes, and 0 for inactive ones.
1571 + */
1572 + val = 0;
1573 + if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
1574 + /* CSI2 */
1575 + set_field(&val, 1, UNICAM_DLE);
1576 + set_field(&val, 1, UNICAM_DLLPE);
1577 + if (dev->bus_flags & V4L2_MBUS_CSI2_CONTINUOUS_CLOCK) {
1578 + set_field(&val, 1, UNICAM_DLTRE);
1579 + set_field(&val, 1, UNICAM_DLHSE);
1580 + }
1581 + } else {
1582 + /* CCP2 */
1583 + set_field(&val, 1, UNICAM_DLE);
1584 + set_field(&val, 1, UNICAM_DLHSE);
1585 + set_field(&val, 1, UNICAM_DLTRE);
1586 + }
1587 + reg_write(dev, UNICAM_DAT0, val);
1588 +
1589 + if (dev->active_data_lanes == 1)
1590 + val = 0;
1591 + reg_write(dev, UNICAM_DAT1, val);
1592 +
1593 + if (dev->max_data_lanes > 2) {
1594 + /*
1595 + * Registers UNICAM_DAT2 and UNICAM_DAT3 only valid if the
1596 + * instance supports more than 2 data lanes.
1597 + */
1598 + if (dev->active_data_lanes == 2)
1599 + val = 0;
1600 + reg_write(dev, UNICAM_DAT2, val);
1601 +
1602 + if (dev->active_data_lanes == 3)
1603 + val = 0;
1604 + reg_write(dev, UNICAM_DAT3, val);
1605 + }
1606 +
1607 + reg_write(dev, UNICAM_IBLS,
1608 + dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline);
1609 + size = dev->node[IMAGE_PAD].v_fmt.fmt.pix.sizeimage;
1610 + unicam_wr_dma_addr(dev, addr[IMAGE_PAD], size, IMAGE_PAD);
1611 + unicam_set_packing_config(dev);
1612 + unicam_cfg_image_id(dev);
1613 +
1614 + val = reg_read(dev, UNICAM_MISC);
1615 + set_field(&val, 1, UNICAM_FL0);
1616 + set_field(&val, 1, UNICAM_FL1);
1617 + reg_write(dev, UNICAM_MISC, val);
1618 +
1619 + if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data) {
1620 + size = dev->node[METADATA_PAD].v_fmt.fmt.meta.buffersize;
1621 + unicam_enable_ed(dev);
1622 + unicam_wr_dma_addr(dev, addr[METADATA_PAD], size, METADATA_PAD);
1623 + }
1624 +
1625 + /* Enable peripheral */
1626 + reg_write_field(dev, UNICAM_CTRL, 1, UNICAM_CPE);
1627 +
1628 + /* Load image pointers */
1629 + reg_write_field(dev, UNICAM_ICTL, 1, UNICAM_LIP_MASK);
1630 +
1631 + /* Load embedded data buffer pointers if needed */
1632 + if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data)
1633 + reg_write_field(dev, UNICAM_DCS, 1, UNICAM_LDP);
1634 +
1635 + /*
1636 + * Enable trigger only for the first frame to
1637 + * sync correctly to the FS from the source.
1638 + */
1639 + reg_write_field(dev, UNICAM_ICTL, 1, UNICAM_TFC);
1640 +}
1641 +
1642 +static void unicam_disable(struct unicam_device *dev)
1643 +{
1644 + /* Analogue lane control disable */
1645 + reg_write_field(dev, UNICAM_ANA, 1, UNICAM_DDL);
1646 +
1647 + /* Stop the output engine */
1648 + reg_write_field(dev, UNICAM_CTRL, 1, UNICAM_SOE);
1649 +
1650 + /* Disable the data lanes. */
1651 + reg_write(dev, UNICAM_DAT0, 0);
1652 + reg_write(dev, UNICAM_DAT1, 0);
1653 +
1654 + if (dev->max_data_lanes > 2) {
1655 + reg_write(dev, UNICAM_DAT2, 0);
1656 + reg_write(dev, UNICAM_DAT3, 0);
1657 + }
1658 +
1659 + /* Peripheral reset */
1660 + reg_write_field(dev, UNICAM_CTRL, 1, UNICAM_CPR);
1661 + usleep_range(50, 100);
1662 + reg_write_field(dev, UNICAM_CTRL, 0, UNICAM_CPR);
1663 +
1664 + /* Disable peripheral */
1665 + reg_write_field(dev, UNICAM_CTRL, 0, UNICAM_CPE);
1666 +
1667 + /* Clear ED setup */
1668 + reg_write(dev, UNICAM_DCS, 0);
1669 +
1670 + /* Disable all lane clocks */
1671 + clk_write(dev, 0);
1672 +}
1673 +
1674 +static void unicam_return_buffers(struct unicam_node *node)
1675 +{
1676 + struct unicam_buffer *buf, *tmp;
1677 + unsigned long flags;
1678 +
1679 + spin_lock_irqsave(&node->dma_queue_lock, flags);
1680 + list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) {
1681 + list_del(&buf->list);
1682 + vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
1683 + }
1684 +
1685 + if (node->cur_frm)
1686 + vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
1687 + VB2_BUF_STATE_ERROR);
1688 + if (node->next_frm && node->cur_frm != node->next_frm)
1689 + vb2_buffer_done(&node->next_frm->vb.vb2_buf,
1690 + VB2_BUF_STATE_ERROR);
1691 +
1692 + node->cur_frm = NULL;
1693 + node->next_frm = NULL;
1694 + spin_unlock_irqrestore(&node->dma_queue_lock, flags);
1695 +}
1696 +
1697 +static int unicam_start_streaming(struct vb2_queue *vq, unsigned int count)
1698 +{
1699 + struct unicam_node *node = vb2_get_drv_priv(vq);
1700 + struct unicam_device *dev = node->dev;
1701 + dma_addr_t buffer_addr[MAX_NODES] = { 0 };
1702 + unsigned long flags;
1703 + unsigned int i;
1704 + int ret;
1705 +
1706 + node->streaming = true;
1707 + if (!unicam_all_nodes_streaming(dev)) {
1708 + unicam_dbg(3, dev, "Not all nodes are streaming yet.");
1709 + return 0;
1710 + }
1711 +
1712 + dev->sequence = 0;
1713 + ret = unicam_runtime_get(dev);
1714 + if (ret < 0) {
1715 + unicam_dbg(3, dev, "unicam_runtime_get failed\n");
1716 + goto err_streaming;
1717 + }
1718 +
1719 + /*
1720 + * TODO: Retrieve the number of active data lanes from the connected
1721 + * subdevice.
1722 + */
1723 + dev->active_data_lanes = dev->max_data_lanes;
1724 +
1725 + ret = clk_set_rate(dev->clock, 100 * 1000 * 1000);
1726 + if (ret) {
1727 + unicam_err(dev, "failed to set up clock\n");
1728 + goto err_pm_put;
1729 + }
1730 +
1731 + ret = clk_prepare_enable(dev->clock);
1732 + if (ret) {
1733 + unicam_err(dev, "Failed to enable CSI clock: %d\n", ret);
1734 + goto err_pm_put;
1735 + }
1736 +
1737 + for (i = 0; i < ARRAY_SIZE(dev->node); i++) {
1738 + struct unicam_buffer *buf;
1739 +
1740 + if (!dev->node[i].streaming)
1741 + continue;
1742 +
1743 + spin_lock_irqsave(&dev->node[i].dma_queue_lock, flags);
1744 + buf = list_first_entry(&dev->node[i].dma_queue,
1745 + struct unicam_buffer, list);
1746 + dev->node[i].cur_frm = buf;
1747 + dev->node[i].next_frm = buf;
1748 + list_del(&buf->list);
1749 + spin_unlock_irqrestore(&dev->node[i].dma_queue_lock, flags);
1750 +
1751 + buffer_addr[i] =
1752 + vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
1753 + }
1754 +
1755 + unicam_start_rx(dev, buffer_addr);
1756 +
1757 + ret = v4l2_subdev_call(dev->sensor, video, s_stream, 1);
1758 + if (ret < 0) {
1759 + unicam_err(dev, "stream on failed in subdev\n");
1760 + goto err_disable_unicam;
1761 + }
1762 +
1763 + return 0;
1764 +
1765 +err_disable_unicam:
1766 + unicam_disable(dev);
1767 + clk_disable_unprepare(dev->clock);
1768 +err_pm_put:
1769 + unicam_runtime_put(dev);
1770 +err_streaming:
1771 + unicam_return_buffers(node);
1772 + node->streaming = false;
1773 +
1774 + return ret;
1775 +}
1776 +
1777 +static void unicam_stop_streaming(struct vb2_queue *vq)
1778 +{
1779 + struct unicam_node *node = vb2_get_drv_priv(vq);
1780 + struct unicam_device *dev = node->dev;
1781 +
1782 + node->streaming = false;
1783 +
1784 + if (node->pad_id == IMAGE_PAD) {
1785 + /*
1786 + * Stop streaming the sensor and disable the peripheral.
1787 + * We cannot continue streaming embedded data with the
1788 + * image pad disabled.
1789 + */
1790 + if (v4l2_subdev_call(dev->sensor, video, s_stream, 0) < 0)
1791 + unicam_err(dev, "stream off failed in subdev\n");
1792 +
1793 + unicam_disable(dev);
1794 + clk_disable_unprepare(dev->clock);
1795 + unicam_runtime_put(dev);
1796 +
1797 + } else if (node->pad_id == METADATA_PAD) {
1798 + /*
1799 + * Allow the hardware to spin in the dummy buffer.
1800 + * This is only really needed if the embedded data pad is
1801 + * disabled before the image pad.
1802 + */
1803 + unicam_wr_dma_addr(dev, node->dummy_buf_dma_addr,
1804 + DUMMY_BUF_SIZE, METADATA_PAD);
1805 + }
1806 +
1807 + /* Clear all queued buffers for the node */
1808 + unicam_return_buffers(node);
1809 +}
1810 +
1811 +static int unicam_enum_input(struct file *file, void *priv,
1812 + struct v4l2_input *inp)
1813 +{
1814 + struct unicam_node *node = video_drvdata(file);
1815 + struct unicam_device *dev = node->dev;
1816 +
1817 + if (inp->index != 0)
1818 + return -EINVAL;
1819 +
1820 + inp->type = V4L2_INPUT_TYPE_CAMERA;
1821 + if (v4l2_subdev_has_op(dev->sensor, video, s_dv_timings)) {
1822 + inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
1823 + inp->std = 0;
1824 + } else if (v4l2_subdev_has_op(dev->sensor, video, s_std)) {
1825 + inp->capabilities = V4L2_IN_CAP_STD;
1826 + if (v4l2_subdev_call(dev->sensor, video, g_tvnorms, &inp->std)
1827 + < 0)
1828 + inp->std = V4L2_STD_ALL;
1829 + } else {
1830 + inp->capabilities = 0;
1831 + inp->std = 0;
1832 + }
1833 + sprintf(inp->name, "Camera 0");
1834 + return 0;
1835 +}
1836 +
1837 +static int unicam_g_input(struct file *file, void *priv, unsigned int *i)
1838 +{
1839 + *i = 0;
1840 +
1841 + return 0;
1842 +}
1843 +
1844 +static int unicam_s_input(struct file *file, void *priv, unsigned int i)
1845 +{
1846 + /*
1847 + * FIXME: Ideally we would like to be able to query the source
1848 + * subdevice for information over the input connectors it supports,
1849 + * and map that through in to a call to video_ops->s_routing.
1850 + * There is no infrastructure support for defining that within
1851 + * devicetree at present. Until that is implemented we can't
1852 + * map a user physical connector number to s_routing input number.
1853 + */
1854 + if (i > 0)
1855 + return -EINVAL;
1856 +
1857 + return 0;
1858 +}
1859 +
1860 +static int unicam_querystd(struct file *file, void *priv,
1861 + v4l2_std_id *std)
1862 +{
1863 + struct unicam_node *node = video_drvdata(file);
1864 + struct unicam_device *dev = node->dev;
1865 +
1866 + return v4l2_subdev_call(dev->sensor, video, querystd, std);
1867 +}
1868 +
1869 +static int unicam_g_std(struct file *file, void *priv, v4l2_std_id *std)
1870 +{
1871 + struct unicam_node *node = video_drvdata(file);
1872 + struct unicam_device *dev = node->dev;
1873 +
1874 + return v4l2_subdev_call(dev->sensor, video, g_std, std);
1875 +}
1876 +
1877 +static int unicam_s_std(struct file *file, void *priv, v4l2_std_id std)
1878 +{
1879 + struct unicam_node *node = video_drvdata(file);
1880 + struct unicam_device *dev = node->dev;
1881 + int ret;
1882 + v4l2_std_id current_std;
1883 +
1884 + ret = v4l2_subdev_call(dev->sensor, video, g_std, &current_std);
1885 + if (ret)
1886 + return ret;
1887 +
1888 + if (std == current_std)
1889 + return 0;
1890 +
1891 + if (vb2_is_busy(&node->buffer_queue))
1892 + return -EBUSY;
1893 +
1894 + ret = v4l2_subdev_call(dev->sensor, video, s_std, std);
1895 +
1896 + /* Force recomputation of bytesperline */
1897 + node->v_fmt.fmt.pix.bytesperline = 0;
1898 +
1899 + unicam_reset_format(node);
1900 +
1901 + return ret;
1902 +}
1903 +
1904 +static int unicam_s_edid(struct file *file, void *priv, struct v4l2_edid *edid)
1905 +{
1906 + struct unicam_node *node = video_drvdata(file);
1907 + struct unicam_device *dev = node->dev;
1908 +
1909 + return v4l2_subdev_call(dev->sensor, pad, set_edid, edid);
1910 +}
1911 +
1912 +static int unicam_g_edid(struct file *file, void *priv, struct v4l2_edid *edid)
1913 +{
1914 + struct unicam_node *node = video_drvdata(file);
1915 + struct unicam_device *dev = node->dev;
1916 +
1917 + return v4l2_subdev_call(dev->sensor, pad, get_edid, edid);
1918 +}
1919 +
1920 +static int unicam_s_selection(struct file *file, void *priv,
1921 + struct v4l2_selection *sel)
1922 +{
1923 + struct unicam_node *node = video_drvdata(file);
1924 + struct unicam_device *dev = node->dev;
1925 + struct v4l2_subdev_selection sdsel = {
1926 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1927 + .target = sel->target,
1928 + .flags = sel->flags,
1929 + .r = sel->r,
1930 + };
1931 +
1932 + return v4l2_subdev_call(dev->sensor, pad, set_selection, NULL, &sdsel);
1933 +}
1934 +
1935 +static int unicam_g_selection(struct file *file, void *priv,
1936 + struct v4l2_selection *sel)
1937 +{
1938 + struct unicam_node *node = video_drvdata(file);
1939 + struct unicam_device *dev = node->dev;
1940 + struct v4l2_subdev_selection sdsel = {
1941 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1942 + .target = sel->target,
1943 + };
1944 + int ret;
1945 +
1946 + ret = v4l2_subdev_call(dev->sensor, pad, get_selection, NULL, &sdsel);
1947 + if (!ret)
1948 + sel->r = sdsel.r;
1949 +
1950 + return ret;
1951 +}
1952 +
1953 +static int unicam_enum_framesizes(struct file *file, void *priv,
1954 + struct v4l2_frmsizeenum *fsize)
1955 +{
1956 + struct unicam_node *node = video_drvdata(file);
1957 + struct unicam_device *dev = node->dev;
1958 + const struct unicam_fmt *fmt;
1959 + struct v4l2_subdev_frame_size_enum fse;
1960 + int ret;
1961 +
1962 + /* check for valid format */
1963 + fmt = find_format_by_pix(dev, fsize->pixel_format);
1964 + if (!fmt) {
1965 + unicam_dbg(3, dev, "Invalid pixel code: %x\n",
1966 + fsize->pixel_format);
1967 + return -EINVAL;
1968 + }
1969 + fse.code = fmt->code;
1970 +
1971 + fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1972 + fse.index = fsize->index;
1973 + fse.pad = node->pad_id;
1974 +
1975 + ret = v4l2_subdev_call(dev->sensor, pad, enum_frame_size, NULL, &fse);
1976 + if (ret)
1977 + return ret;
1978 +
1979 + unicam_dbg(1, dev, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
1980 + __func__, fse.index, fse.code, fse.min_width, fse.max_width,
1981 + fse.min_height, fse.max_height);
1982 +
1983 + fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1984 + fsize->discrete.width = fse.max_width;
1985 + fsize->discrete.height = fse.max_height;
1986 +
1987 + return 0;
1988 +}
1989 +
1990 +static int unicam_enum_frameintervals(struct file *file, void *priv,
1991 + struct v4l2_frmivalenum *fival)
1992 +{
1993 + struct unicam_node *node = video_drvdata(file);
1994 + struct unicam_device *dev = node->dev;
1995 + const struct unicam_fmt *fmt;
1996 + struct v4l2_subdev_frame_interval_enum fie = {
1997 + .index = fival->index,
1998 + .width = fival->width,
1999 + .height = fival->height,
2000 + .which = V4L2_SUBDEV_FORMAT_ACTIVE,
2001 + };
2002 + int ret;
2003 +
2004 + fmt = find_format_by_pix(dev, fival->pixel_format);
2005 + if (!fmt)
2006 + return -EINVAL;
2007 +
2008 + fie.code = fmt->code;
2009 + ret = v4l2_subdev_call(dev->sensor, pad, enum_frame_interval,
2010 + NULL, &fie);
2011 + if (ret)
2012 + return ret;
2013 +
2014 + fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
2015 + fival->discrete = fie.interval;
2016 +
2017 + return 0;
2018 +}
2019 +
2020 +static int unicam_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
2021 +{
2022 + struct unicam_node *node = video_drvdata(file);
2023 + struct unicam_device *dev = node->dev;
2024 +
2025 + return v4l2_g_parm_cap(video_devdata(file), dev->sensor, a);
2026 +}
2027 +
2028 +static int unicam_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
2029 +{
2030 + struct unicam_node *node = video_drvdata(file);
2031 + struct unicam_device *dev = node->dev;
2032 +
2033 + return v4l2_s_parm_cap(video_devdata(file), dev->sensor, a);
2034 +}
2035 +
2036 +static int unicam_g_dv_timings(struct file *file, void *priv,
2037 + struct v4l2_dv_timings *timings)
2038 +{
2039 + struct unicam_node *node = video_drvdata(file);
2040 + struct unicam_device *dev = node->dev;
2041 +
2042 + return v4l2_subdev_call(dev->sensor, video, g_dv_timings, timings);
2043 +}
2044 +
2045 +static int unicam_s_dv_timings(struct file *file, void *priv,
2046 + struct v4l2_dv_timings *timings)
2047 +{
2048 + struct unicam_node *node = video_drvdata(file);
2049 + struct unicam_device *dev = node->dev;
2050 + struct v4l2_dv_timings current_timings;
2051 + int ret;
2052 +
2053 + ret = v4l2_subdev_call(dev->sensor, video, g_dv_timings,
2054 + &current_timings);
2055 +
2056 + if (v4l2_match_dv_timings(timings, &current_timings, 0, false))
2057 + return 0;
2058 +
2059 + if (vb2_is_busy(&node->buffer_queue))
2060 + return -EBUSY;
2061 +
2062 + ret = v4l2_subdev_call(dev->sensor, video, s_dv_timings, timings);
2063 +
2064 + /* Force recomputation of bytesperline */
2065 + node->v_fmt.fmt.pix.bytesperline = 0;
2066 +
2067 + unicam_reset_format(node);
2068 +
2069 + return ret;
2070 +}
2071 +
2072 +static int unicam_query_dv_timings(struct file *file, void *priv,
2073 + struct v4l2_dv_timings *timings)
2074 +{
2075 + struct unicam_node *node = video_drvdata(file);
2076 + struct unicam_device *dev = node->dev;
2077 +
2078 + return v4l2_subdev_call(dev->sensor, video, query_dv_timings, timings);
2079 +}
2080 +
2081 +static int unicam_enum_dv_timings(struct file *file, void *priv,
2082 + struct v4l2_enum_dv_timings *timings)
2083 +{
2084 + struct unicam_node *node = video_drvdata(file);
2085 + struct unicam_device *dev = node->dev;
2086 +
2087 + return v4l2_subdev_call(dev->sensor, pad, enum_dv_timings, timings);
2088 +}
2089 +
2090 +static int unicam_dv_timings_cap(struct file *file, void *priv,
2091 + struct v4l2_dv_timings_cap *cap)
2092 +{
2093 + struct unicam_node *node = video_drvdata(file);
2094 + struct unicam_device *dev = node->dev;
2095 +
2096 + return v4l2_subdev_call(dev->sensor, pad, dv_timings_cap, cap);
2097 +}
2098 +
2099 +static int unicam_subscribe_event(struct v4l2_fh *fh,
2100 + const struct v4l2_event_subscription *sub)
2101 +{
2102 + switch (sub->type) {
2103 + case V4L2_EVENT_FRAME_SYNC:
2104 + return v4l2_event_subscribe(fh, sub, 2, NULL);
2105 + case V4L2_EVENT_SOURCE_CHANGE:
2106 + return v4l2_event_subscribe(fh, sub, 4, NULL);
2107 + }
2108 +
2109 + return v4l2_ctrl_subscribe_event(fh, sub);
2110 +}
2111 +
2112 +static int unicam_log_status(struct file *file, void *fh)
2113 +{
2114 + struct unicam_node *node = video_drvdata(file);
2115 + struct unicam_device *dev = node->dev;
2116 + u32 reg;
2117 +
2118 + /* status for sub devices */
2119 + v4l2_device_call_all(&dev->v4l2_dev, 0, core, log_status);
2120 +
2121 + unicam_info(dev, "-----Receiver status-----\n");
2122 + unicam_info(dev, "V4L2 width/height: %ux%u\n",
2123 + node->v_fmt.fmt.pix.width, node->v_fmt.fmt.pix.height);
2124 + unicam_info(dev, "Mediabus format: %08x\n", node->fmt->code);
2125 + unicam_info(dev, "V4L2 format: %08x\n",
2126 + node->v_fmt.fmt.pix.pixelformat);
2127 + reg = reg_read(dev, UNICAM_IPIPE);
2128 + unicam_info(dev, "Unpacking/packing: %u / %u\n",
2129 + get_field(reg, UNICAM_PUM_MASK),
2130 + get_field(reg, UNICAM_PPM_MASK));
2131 + unicam_info(dev, "----Live data----\n");
2132 + unicam_info(dev, "Programmed stride: %4u\n",
2133 + reg_read(dev, UNICAM_IBLS));
2134 + unicam_info(dev, "Detected resolution: %ux%u\n",
2135 + reg_read(dev, UNICAM_IHSTA),
2136 + reg_read(dev, UNICAM_IVSTA));
2137 + unicam_info(dev, "Write pointer: %08x\n",
2138 + reg_read(dev, UNICAM_IBWP));
2139 +
2140 + return 0;
2141 +}
2142 +
2143 +static void unicam_notify(struct v4l2_subdev *sd,
2144 + unsigned int notification, void *arg)
2145 +{
2146 + struct unicam_device *dev = to_unicam_device(sd->v4l2_dev);
2147 +
2148 + switch (notification) {
2149 + case V4L2_DEVICE_NOTIFY_EVENT:
2150 + v4l2_event_queue(&dev->node[IMAGE_PAD].video_dev, arg);
2151 + break;
2152 + default:
2153 + break;
2154 + }
2155 +}
2156 +
2157 +static const struct vb2_ops unicam_video_qops = {
2158 + .wait_prepare = vb2_ops_wait_prepare,
2159 + .wait_finish = vb2_ops_wait_finish,
2160 + .queue_setup = unicam_queue_setup,
2161 + .buf_prepare = unicam_buffer_prepare,
2162 + .buf_queue = unicam_buffer_queue,
2163 + .start_streaming = unicam_start_streaming,
2164 + .stop_streaming = unicam_stop_streaming,
2165 +};
2166 +
2167 +/*
2168 + * unicam_v4l2_open : This function is based on the v4l2_fh_open helper
2169 + * function. It has been augmented to handle sensor subdevice power management,
2170 + */
2171 +static int unicam_v4l2_open(struct file *file)
2172 +{
2173 + struct unicam_node *node = video_drvdata(file);
2174 + struct unicam_device *dev = node->dev;
2175 + int ret;
2176 +
2177 + mutex_lock(&node->lock);
2178 +
2179 + ret = v4l2_fh_open(file);
2180 + if (ret) {
2181 + unicam_err(dev, "v4l2_fh_open failed\n");
2182 + goto unlock;
2183 + }
2184 +
2185 + node->open++;
2186 +
2187 + if (!v4l2_fh_is_singular_file(file))
2188 + goto unlock;
2189 +
2190 + ret = v4l2_subdev_call(dev->sensor, core, s_power, 1);
2191 + if (ret < 0 && ret != -ENOIOCTLCMD) {
2192 + v4l2_fh_release(file);
2193 + node->open--;
2194 + goto unlock;
2195 + }
2196 +
2197 + ret = 0;
2198 +
2199 +unlock:
2200 + mutex_unlock(&node->lock);
2201 + return ret;
2202 +}
2203 +
2204 +static int unicam_v4l2_release(struct file *file)
2205 +{
2206 + struct unicam_node *node = video_drvdata(file);
2207 + struct unicam_device *dev = node->dev;
2208 + struct v4l2_subdev *sd = dev->sensor;
2209 + bool fh_singular;
2210 + int ret;
2211 +
2212 + mutex_lock(&node->lock);
2213 +
2214 + fh_singular = v4l2_fh_is_singular_file(file);
2215 +
2216 + ret = _vb2_fop_release(file, NULL);
2217 +
2218 + if (fh_singular)
2219 + v4l2_subdev_call(sd, core, s_power, 0);
2220 +
2221 + node->open--;
2222 + mutex_unlock(&node->lock);
2223 +
2224 + return ret;
2225 +}
2226 +
2227 +/* unicam capture driver file operations */
2228 +static const struct v4l2_file_operations unicam_fops = {
2229 + .owner = THIS_MODULE,
2230 + .open = unicam_v4l2_open,
2231 + .release = unicam_v4l2_release,
2232 + .read = vb2_fop_read,
2233 + .poll = vb2_fop_poll,
2234 + .unlocked_ioctl = video_ioctl2,
2235 + .mmap = vb2_fop_mmap,
2236 +};
2237 +
2238 +/* unicam capture ioctl operations */
2239 +static const struct v4l2_ioctl_ops unicam_ioctl_ops = {
2240 + .vidioc_querycap = unicam_querycap,
2241 + .vidioc_enum_fmt_vid_cap = unicam_enum_fmt_vid_cap,
2242 + .vidioc_g_fmt_vid_cap = unicam_g_fmt_vid_cap,
2243 + .vidioc_s_fmt_vid_cap = unicam_s_fmt_vid_cap,
2244 + .vidioc_try_fmt_vid_cap = unicam_try_fmt_vid_cap,
2245 +
2246 + .vidioc_enum_fmt_meta_cap = unicam_enum_fmt_meta_cap,
2247 + .vidioc_g_fmt_meta_cap = unicam_g_fmt_meta_cap,
2248 + .vidioc_s_fmt_meta_cap = unicam_g_fmt_meta_cap,
2249 + .vidioc_try_fmt_meta_cap = unicam_g_fmt_meta_cap,
2250 +
2251 + .vidioc_enum_input = unicam_enum_input,
2252 + .vidioc_g_input = unicam_g_input,
2253 + .vidioc_s_input = unicam_s_input,
2254 +
2255 + .vidioc_querystd = unicam_querystd,
2256 + .vidioc_s_std = unicam_s_std,
2257 + .vidioc_g_std = unicam_g_std,
2258 +
2259 + .vidioc_g_edid = unicam_g_edid,
2260 + .vidioc_s_edid = unicam_s_edid,
2261 +
2262 + .vidioc_enum_framesizes = unicam_enum_framesizes,
2263 + .vidioc_enum_frameintervals = unicam_enum_frameintervals,
2264 +
2265 + .vidioc_g_selection = unicam_g_selection,
2266 + .vidioc_s_selection = unicam_s_selection,
2267 +
2268 + .vidioc_g_parm = unicam_g_parm,
2269 + .vidioc_s_parm = unicam_s_parm,
2270 +
2271 + .vidioc_s_dv_timings = unicam_s_dv_timings,
2272 + .vidioc_g_dv_timings = unicam_g_dv_timings,
2273 + .vidioc_query_dv_timings = unicam_query_dv_timings,
2274 + .vidioc_enum_dv_timings = unicam_enum_dv_timings,
2275 + .vidioc_dv_timings_cap = unicam_dv_timings_cap,
2276 +
2277 + .vidioc_reqbufs = vb2_ioctl_reqbufs,
2278 + .vidioc_create_bufs = vb2_ioctl_create_bufs,
2279 + .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
2280 + .vidioc_querybuf = vb2_ioctl_querybuf,
2281 + .vidioc_qbuf = vb2_ioctl_qbuf,
2282 + .vidioc_dqbuf = vb2_ioctl_dqbuf,
2283 + .vidioc_expbuf = vb2_ioctl_expbuf,
2284 + .vidioc_streamon = vb2_ioctl_streamon,
2285 + .vidioc_streamoff = vb2_ioctl_streamoff,
2286 +
2287 + .vidioc_log_status = unicam_log_status,
2288 + .vidioc_subscribe_event = unicam_subscribe_event,
2289 + .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
2290 +};
2291 +
2292 +static int
2293 +unicam_async_bound(struct v4l2_async_notifier *notifier,
2294 + struct v4l2_subdev *subdev,
2295 + struct v4l2_async_subdev *asd)
2296 +{
2297 + struct unicam_device *unicam = to_unicam_device(notifier->v4l2_dev);
2298 +
2299 + if (unicam->sensor) {
2300 + unicam_info(unicam, "Rejecting subdev %s (Already set!!)",
2301 + subdev->name);
2302 + return 0;
2303 + }
2304 +
2305 + unicam->sensor = subdev;
2306 + unicam_dbg(1, unicam, "Using sensor %s for capture\n", subdev->name);
2307 +
2308 + return 0;
2309 +}
2310 +
2311 +static void unicam_release(struct kref *kref)
2312 +{
2313 + struct unicam_device *unicam =
2314 + container_of(kref, struct unicam_device, kref);
2315 +
2316 + v4l2_ctrl_handler_free(&unicam->ctrl_handler);
2317 + media_device_cleanup(&unicam->mdev);
2318 +
2319 + if (unicam->sensor_config)
2320 + v4l2_subdev_free_pad_config(unicam->sensor_config);
2321 +
2322 + kfree(unicam);
2323 +}
2324 +
2325 +static void unicam_put(struct unicam_device *unicam)
2326 +{
2327 + kref_put(&unicam->kref, unicam_release);
2328 +}
2329 +
2330 +static void unicam_get(struct unicam_device *unicam)
2331 +{
2332 + kref_get(&unicam->kref);
2333 +}
2334 +
2335 +static void unicam_node_release(struct video_device *vdev)
2336 +{
2337 + struct unicam_node *node = video_get_drvdata(vdev);
2338 +
2339 + unicam_put(node->dev);
2340 +}
2341 +
2342 +static int register_node(struct unicam_device *unicam, struct unicam_node *node,
2343 + enum v4l2_buf_type type, int pad_id)
2344 +{
2345 + struct video_device *vdev;
2346 + struct vb2_queue *q;
2347 + struct v4l2_mbus_framefmt mbus_fmt = {0};
2348 + const struct unicam_fmt *fmt;
2349 + int ret;
2350 +
2351 + if (pad_id == IMAGE_PAD) {
2352 + ret = __subdev_get_format(unicam, &mbus_fmt, pad_id);
2353 + if (ret) {
2354 + unicam_err(unicam, "Failed to get_format - ret %d\n",
2355 + ret);
2356 + return ret;
2357 + }
2358 +
2359 + fmt = find_format_by_code(mbus_fmt.code);
2360 + if (!fmt) {
2361 + /*
2362 + * Find the first format that the sensor and unicam both
2363 + * support
2364 + */
2365 + fmt = get_first_supported_format(unicam);
2366 +
2367 + if (!fmt)
2368 + /* No compatible formats */
2369 + return -EINVAL;
2370 +
2371 + mbus_fmt.code = fmt->code;
2372 + ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
2373 + if (ret)
2374 + return -EINVAL;
2375 + }
2376 + if (mbus_fmt.field != V4L2_FIELD_NONE) {
2377 + /* Interlaced not supported - disable it now. */
2378 + mbus_fmt.field = V4L2_FIELD_NONE;
2379 + ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
2380 + if (ret)
2381 + return -EINVAL;
2382 + }
2383 +
2384 + node->v_fmt.fmt.pix.pixelformat = fmt->fourcc ? fmt->fourcc
2385 + : fmt->repacked_fourcc;
2386 + } else {
2387 + /* Fix this node format as embedded data. */
2388 + fmt = find_format_by_code(MEDIA_BUS_FMT_SENSOR_DATA);
2389 + node->v_fmt.fmt.meta.dataformat = fmt->fourcc;
2390 + }
2391 +
2392 + node->dev = unicam;
2393 + node->pad_id = pad_id;
2394 + node->fmt = fmt;
2395 +
2396 + /* Read current subdev format */
2397 + unicam_reset_format(node);
2398 +
2399 + if (v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
2400 + v4l2_std_id tvnorms;
2401 +
2402 + if (WARN_ON(!v4l2_subdev_has_op(unicam->sensor, video,
2403 + g_tvnorms)))
2404 + /*
2405 + * Subdevice should not advertise s_std but not
2406 + * g_tvnorms
2407 + */
2408 + return -EINVAL;
2409 +
2410 + ret = v4l2_subdev_call(unicam->sensor, video,
2411 + g_tvnorms, &tvnorms);
2412 + if (WARN_ON(ret))
2413 + return -EINVAL;
2414 + node->video_dev.tvnorms |= tvnorms;
2415 + }
2416 +
2417 + spin_lock_init(&node->dma_queue_lock);
2418 + mutex_init(&node->lock);
2419 +
2420 + vdev = &node->video_dev;
2421 + if (pad_id == IMAGE_PAD) {
2422 + /* Add controls from the subdevice */
2423 + ret = v4l2_ctrl_add_handler(&unicam->ctrl_handler,
2424 + unicam->sensor->ctrl_handler, NULL,
2425 + true);
2426 + if (ret < 0)
2427 + return ret;
2428 +
2429 + /*
2430 + * If the sensor subdevice has any controls, associate the node
2431 + * with the ctrl handler to allow access from userland.
2432 + */
2433 + if (!list_empty(&unicam->ctrl_handler.ctrls))
2434 + vdev->ctrl_handler = &unicam->ctrl_handler;
2435 + }
2436 +
2437 + q = &node->buffer_queue;
2438 + q->type = type;
2439 + q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
2440 + q->drv_priv = node;
2441 + q->ops = &unicam_video_qops;
2442 + q->mem_ops = &vb2_dma_contig_memops;
2443 + q->buf_struct_size = sizeof(struct unicam_buffer);
2444 + q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
2445 + q->lock = &node->lock;
2446 + q->min_buffers_needed = 2;
2447 + q->dev = &unicam->pdev->dev;
2448 +
2449 + ret = vb2_queue_init(q);
2450 + if (ret) {
2451 + unicam_err(unicam, "vb2_queue_init() failed\n");
2452 + return ret;
2453 + }
2454 +
2455 + INIT_LIST_HEAD(&node->dma_queue);
2456 +
2457 + vdev->release = unicam_node_release;
2458 + vdev->fops = &unicam_fops;
2459 + vdev->ioctl_ops = &unicam_ioctl_ops;
2460 + vdev->v4l2_dev = &unicam->v4l2_dev;
2461 + vdev->vfl_dir = VFL_DIR_RX;
2462 + vdev->queue = q;
2463 + vdev->lock = &node->lock;
2464 + vdev->device_caps = (pad_id == IMAGE_PAD) ?
2465 + (V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING) :
2466 + (V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING);
2467 +
2468 + /* Define the device names */
2469 + snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME,
2470 + pad_id == IMAGE_PAD ? "image" : "embedded");
2471 +
2472 + video_set_drvdata(vdev, node);
2473 + if (pad_id == IMAGE_PAD)
2474 + vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
2475 + node->pad.flags = MEDIA_PAD_FL_SINK;
2476 + media_entity_pads_init(&vdev->entity, 1, &node->pad);
2477 +
2478 + node->dummy_buf_cpu_addr = dma_alloc_coherent(&unicam->pdev->dev,
2479 + DUMMY_BUF_SIZE,
2480 + &node->dummy_buf_dma_addr,
2481 + GFP_KERNEL);
2482 + if (!node->dummy_buf_cpu_addr) {
2483 + unicam_err(unicam, "Unable to allocate dummy buffer.\n");
2484 + return -ENOMEM;
2485 + }
2486 +
2487 + if (pad_id == METADATA_PAD) {
2488 + v4l2_disable_ioctl(vdev, VIDIOC_DQEVENT);
2489 + v4l2_disable_ioctl(vdev, VIDIOC_SUBSCRIBE_EVENT);
2490 + v4l2_disable_ioctl(vdev, VIDIOC_UNSUBSCRIBE_EVENT);
2491 + }
2492 + if (pad_id == METADATA_PAD ||
2493 + !v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
2494 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_STD);
2495 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_STD);
2496 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUMSTD);
2497 + }
2498 + if (pad_id == METADATA_PAD ||
2499 + !v4l2_subdev_has_op(unicam->sensor, video, querystd))
2500 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERYSTD);
2501 + if (pad_id == METADATA_PAD ||
2502 + !v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) {
2503 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_EDID);
2504 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_EDID);
2505 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_DV_TIMINGS_CAP);
2506 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_DV_TIMINGS);
2507 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_DV_TIMINGS);
2508 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_DV_TIMINGS);
2509 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERY_DV_TIMINGS);
2510 + }
2511 + if (pad_id == METADATA_PAD ||
2512 + !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval))
2513 + v4l2_disable_ioctl(&node->video_dev,
2514 + VIDIOC_ENUM_FRAMEINTERVALS);
2515 + if (pad_id == METADATA_PAD ||
2516 + !v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval))
2517 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_PARM);
2518 + if (pad_id == METADATA_PAD ||
2519 + !v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval))
2520 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_PARM);
2521 +
2522 + if (pad_id == METADATA_PAD ||
2523 + !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size))
2524 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMESIZES);
2525 +
2526 + if (node->pad_id == METADATA_PAD ||
2527 + !v4l2_subdev_has_op(unicam->sensor, pad, set_selection))
2528 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_SELECTION);
2529 +
2530 + if (node->pad_id == METADATA_PAD ||
2531 + !v4l2_subdev_has_op(unicam->sensor, pad, get_selection))
2532 + v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_SELECTION);
2533 +
2534 + ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
2535 + if (ret) {
2536 + unicam_err(unicam, "Unable to register video device %s\n",
2537 + vdev->name);
2538 + return ret;
2539 + }
2540 +
2541 + /*
2542 + * Acquire a reference to unicam, which will be released when the video
2543 + * device will be unregistered and userspace will have closed all open
2544 + * file handles.
2545 + */
2546 + unicam_get(unicam);
2547 + node->registered = true;
2548 +
2549 + if (pad_id != METADATA_PAD || unicam->sensor_embedded_data) {
2550 + ret = media_create_pad_link(&unicam->sensor->entity, pad_id,
2551 + &node->video_dev.entity, 0,
2552 + MEDIA_LNK_FL_ENABLED |
2553 + MEDIA_LNK_FL_IMMUTABLE);
2554 + if (ret)
2555 + unicam_err(unicam, "Unable to create pad link for %s\n",
2556 + vdev->name);
2557 + }
2558 +
2559 + return ret;
2560 +}
2561 +
2562 +static void unregister_nodes(struct unicam_device *unicam)
2563 +{
2564 + unsigned int i;
2565 +
2566 + for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
2567 + struct unicam_node *node = &unicam->node[i];
2568 +
2569 + if (node->dummy_buf_cpu_addr) {
2570 + dma_free_coherent(&unicam->pdev->dev, DUMMY_BUF_SIZE,
2571 + node->dummy_buf_cpu_addr,
2572 + node->dummy_buf_dma_addr);
2573 + }
2574 +
2575 + if (node->registered) {
2576 + node->registered = false;
2577 + video_unregister_device(&node->video_dev);
2578 + }
2579 + }
2580 +}
2581 +
2582 +static int unicam_probe_complete(struct unicam_device *unicam)
2583 +{
2584 + int ret;
2585 +
2586 + unicam->v4l2_dev.notify = unicam_notify;
2587 +
2588 + unicam->sensor_config = v4l2_subdev_alloc_pad_config(unicam->sensor);
2589 + if (!unicam->sensor_config)
2590 + return -ENOMEM;
2591 +
2592 + unicam->sensor_embedded_data = (unicam->sensor->entity.num_pads >= 2);
2593 +
2594 + ret = register_node(unicam, &unicam->node[IMAGE_PAD],
2595 + V4L2_BUF_TYPE_VIDEO_CAPTURE, IMAGE_PAD);
2596 + if (ret) {
2597 + unicam_err(unicam, "Unable to register image video device.\n");
2598 + goto unregister;
2599 + }
2600 +
2601 + ret = register_node(unicam, &unicam->node[METADATA_PAD],
2602 + V4L2_BUF_TYPE_META_CAPTURE, METADATA_PAD);
2603 + if (ret) {
2604 + unicam_err(unicam, "Unable to register metadata video device.\n");
2605 + goto unregister;
2606 + }
2607 +
2608 + ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev);
2609 + if (ret) {
2610 + unicam_err(unicam, "Unable to register subdev nodes.\n");
2611 + goto unregister;
2612 + }
2613 +
2614 + /*
2615 + * Release the initial reference, all references are now owned by the
2616 + * video devices.
2617 + */
2618 + unicam_put(unicam);
2619 + return 0;
2620 +
2621 +unregister:
2622 + unregister_nodes(unicam);
2623 + unicam_put(unicam);
2624 +
2625 + return ret;
2626 +}
2627 +
2628 +static int unicam_async_complete(struct v4l2_async_notifier *notifier)
2629 +{
2630 + struct unicam_device *unicam = to_unicam_device(notifier->v4l2_dev);
2631 +
2632 + return unicam_probe_complete(unicam);
2633 +}
2634 +
2635 +static const struct v4l2_async_notifier_operations unicam_async_ops = {
2636 + .bound = unicam_async_bound,
2637 + .complete = unicam_async_complete,
2638 +};
2639 +
2640 +static int of_unicam_connect_subdevs(struct unicam_device *dev)
2641 +{
2642 + struct platform_device *pdev = dev->pdev;
2643 + struct v4l2_fwnode_endpoint ep = { 0 };
2644 + struct device_node *ep_node;
2645 + struct device_node *sensor_node;
2646 + unsigned int lane;
2647 + int ret = -EINVAL;
2648 +
2649 + if (of_property_read_u32(pdev->dev.of_node, "brcm,num-data-lanes",
2650 + &dev->max_data_lanes) < 0) {
2651 + unicam_err(dev, "number of data lanes not set\n");
2652 + return -EINVAL;
2653 + }
2654 +
2655 + /* Get the local endpoint and remote device. */
2656 + ep_node = of_graph_get_next_endpoint(pdev->dev.of_node, NULL);
2657 + if (!ep_node) {
2658 + unicam_dbg(3, dev, "can't get next endpoint\n");
2659 + return -EINVAL;
2660 + }
2661 +
2662 + unicam_dbg(3, dev, "ep_node is %pOF\n", ep_node);
2663 +
2664 + sensor_node = of_graph_get_remote_port_parent(ep_node);
2665 + if (!sensor_node) {
2666 + unicam_dbg(3, dev, "can't get remote parent\n");
2667 + goto cleanup_exit;
2668 + }
2669 +
2670 + unicam_dbg(1, dev, "found subdevice %pOF\n", sensor_node);
2671 +
2672 + /* Parse the local endpoint and validate its configuration. */
2673 + v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
2674 +
2675 + unicam_dbg(3, dev, "parsed local endpoint, bus_type %u\n",
2676 + ep.bus_type);
2677 +
2678 + dev->bus_type = ep.bus_type;
2679 +
2680 + switch (ep.bus_type) {
2681 + case V4L2_MBUS_CSI2_DPHY:
2682 + switch (ep.bus.mipi_csi2.num_data_lanes) {
2683 + case 1:
2684 + case 2:
2685 + case 4:
2686 + break;
2687 +
2688 + default:
2689 + unicam_err(dev, "subdevice %pOF: %u data lanes not supported\n",
2690 + sensor_node,
2691 + ep.bus.mipi_csi2.num_data_lanes);
2692 + goto cleanup_exit;
2693 + }
2694 +
2695 + for (lane = 0; lane < ep.bus.mipi_csi2.num_data_lanes; lane++) {
2696 + if (ep.bus.mipi_csi2.data_lanes[lane] != lane + 1) {
2697 + unicam_err(dev, "subdevice %pOF: data lanes reordering not supported\n",
2698 + sensor_node);
2699 + goto cleanup_exit;
2700 + }
2701 + }
2702 +
2703 + if (ep.bus.mipi_csi2.num_data_lanes > dev->max_data_lanes) {
2704 + unicam_err(dev, "subdevice requires %u data lanes when %u are supported\n",
2705 + ep.bus.mipi_csi2.num_data_lanes,
2706 + dev->max_data_lanes);
2707 + }
2708 +
2709 + dev->max_data_lanes = ep.bus.mipi_csi2.num_data_lanes;
2710 + dev->bus_flags = ep.bus.mipi_csi2.flags;
2711 +
2712 + break;
2713 +
2714 + case V4L2_MBUS_CCP2:
2715 + if (ep.bus.mipi_csi1.clock_lane != 0 ||
2716 + ep.bus.mipi_csi1.data_lane != 1) {
2717 + unicam_err(dev, "subdevice %pOF: unsupported lanes configuration\n",
2718 + sensor_node);
2719 + goto cleanup_exit;
2720 + }
2721 +
2722 + dev->max_data_lanes = 1;
2723 + dev->bus_flags = ep.bus.mipi_csi1.strobe;
2724 + break;
2725 +
2726 + default:
2727 + /* Unsupported bus type */
2728 + unicam_err(dev, "subdevice %pOF: unsupported bus type %u\n",
2729 + sensor_node, ep.bus_type);
2730 + goto cleanup_exit;
2731 + }
2732 +
2733 + unicam_dbg(3, dev, "subdevice %pOF: %s bus, %u data lanes, flags=0x%08x\n",
2734 + sensor_node,
2735 + dev->bus_type == V4L2_MBUS_CSI2_DPHY ? "CSI-2" : "CCP2",
2736 + dev->max_data_lanes, dev->bus_flags);
2737 +
2738 + /* Initialize and register the async notifier. */
2739 + v4l2_async_nf_init(&dev->notifier);
2740 + dev->notifier.ops = &unicam_async_ops;
2741 +
2742 + dev->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
2743 + dev->asd.match.fwnode = of_fwnode_handle(sensor_node);
2744 + ret = __v4l2_async_nf_add_subdev(&dev->notifier, &dev->asd);
2745 + if (ret) {
2746 + unicam_err(dev, "Error adding subdevice: %d\n", ret);
2747 + goto cleanup_exit;
2748 + }
2749 +
2750 + ret = v4l2_async_nf_register(&dev->v4l2_dev, &dev->notifier);
2751 + if (ret) {
2752 + unicam_err(dev, "Error registering async notifier: %d\n", ret);
2753 + ret = -EINVAL;
2754 + }
2755 +
2756 +cleanup_exit:
2757 + of_node_put(sensor_node);
2758 + of_node_put(ep_node);
2759 +
2760 + return ret;
2761 +}
2762 +
2763 +static int unicam_probe(struct platform_device *pdev)
2764 +{
2765 + struct unicam_device *unicam;
2766 + int ret;
2767 +
2768 + unicam = kzalloc(sizeof(*unicam), GFP_KERNEL);
2769 + if (!unicam)
2770 + return -ENOMEM;
2771 +
2772 + kref_init(&unicam->kref);
2773 + unicam->pdev = pdev;
2774 +
2775 + unicam->base = devm_platform_ioremap_resource(pdev, 0);
2776 + if (IS_ERR(unicam->base)) {
2777 + unicam_err(unicam, "Failed to get main io block\n");
2778 + ret = PTR_ERR(unicam->base);
2779 + goto err_unicam_put;
2780 + }
2781 +
2782 + unicam->clk_gate_base = devm_platform_ioremap_resource(pdev, 1);
2783 + if (IS_ERR(unicam->clk_gate_base)) {
2784 + unicam_err(unicam, "Failed to get 2nd io block\n");
2785 + ret = PTR_ERR(unicam->clk_gate_base);
2786 + goto err_unicam_put;
2787 + }
2788 +
2789 + unicam->clock = devm_clk_get(&pdev->dev, "lp");
2790 + if (IS_ERR(unicam->clock)) {
2791 + unicam_err(unicam, "Failed to get clock\n");
2792 + ret = PTR_ERR(unicam->clock);
2793 + goto err_unicam_put;
2794 + }
2795 +
2796 + ret = platform_get_irq(pdev, 0);
2797 + if (ret <= 0) {
2798 + dev_err(&pdev->dev, "No IRQ resource\n");
2799 + ret = -EINVAL;
2800 + goto err_unicam_put;
2801 + }
2802 +
2803 + ret = devm_request_irq(&pdev->dev, ret, unicam_isr, 0,
2804 + "unicam_capture0", unicam);
2805 + if (ret) {
2806 + dev_err(&pdev->dev, "Unable to request interrupt\n");
2807 + ret = -EINVAL;
2808 + goto err_unicam_put;
2809 + }
2810 +
2811 + unicam->mdev.dev = &pdev->dev;
2812 + strscpy(unicam->mdev.model, UNICAM_MODULE_NAME,
2813 + sizeof(unicam->mdev.model));
2814 + strscpy(unicam->mdev.serial, "", sizeof(unicam->mdev.serial));
2815 + snprintf(unicam->mdev.bus_info, sizeof(unicam->mdev.bus_info),
2816 + "platform:%s", dev_name(&pdev->dev));
2817 + unicam->mdev.hw_revision = 0;
2818 +
2819 + media_device_init(&unicam->mdev);
2820 +
2821 + unicam->v4l2_dev.mdev = &unicam->mdev;
2822 +
2823 + ret = v4l2_device_register(&pdev->dev, &unicam->v4l2_dev);
2824 + if (ret) {
2825 + unicam_err(unicam,
2826 + "Unable to register v4l2 device.\n");
2827 + goto err_unicam_put;
2828 + }
2829 +
2830 + ret = media_device_register(&unicam->mdev);
2831 + if (ret < 0) {
2832 + unicam_err(unicam,
2833 + "Unable to register media-controller device.\n");
2834 + goto err_v4l2_unregister;
2835 + }
2836 +
2837 + /* Reserve space for the controls */
2838 + ret = v4l2_ctrl_handler_init(&unicam->ctrl_handler, 16);
2839 + if (ret < 0)
2840 + goto err_media_unregister;
2841 +
2842 + /* set the driver data in platform device */
2843 + platform_set_drvdata(pdev, unicam);
2844 +
2845 + ret = of_unicam_connect_subdevs(unicam);
2846 + if (ret) {
2847 + dev_err(&pdev->dev, "Failed to connect subdevs\n");
2848 + goto err_media_unregister;
2849 + }
2850 +
2851 + /* Enable the block power domain */
2852 + pm_runtime_enable(&pdev->dev);
2853 +
2854 + return 0;
2855 +
2856 +err_media_unregister:
2857 + media_device_unregister(&unicam->mdev);
2858 +err_v4l2_unregister:
2859 + v4l2_device_unregister(&unicam->v4l2_dev);
2860 +err_unicam_put:
2861 + unicam_put(unicam);
2862 +
2863 + return ret;
2864 +}
2865 +
2866 +static int unicam_remove(struct platform_device *pdev)
2867 +{
2868 + struct unicam_device *unicam = platform_get_drvdata(pdev);
2869 +
2870 + unicam_dbg(2, unicam, "%s\n", __func__);
2871 +
2872 + v4l2_async_nf_unregister(&unicam->notifier);
2873 + v4l2_device_unregister(&unicam->v4l2_dev);
2874 + media_device_unregister(&unicam->mdev);
2875 + unregister_nodes(unicam);
2876 +
2877 + pm_runtime_disable(&pdev->dev);
2878 +
2879 + return 0;
2880 +}
2881 +
2882 +static const struct of_device_id unicam_of_match[] = {
2883 + { .compatible = "brcm,bcm2835-unicam", },
2884 + { /* sentinel */ },
2885 +};
2886 +MODULE_DEVICE_TABLE(of, unicam_of_match);
2887 +
2888 +static struct platform_driver unicam_driver = {
2889 + .probe = unicam_probe,
2890 + .remove = unicam_remove,
2891 + .driver = {
2892 + .name = UNICAM_MODULE_NAME,
2893 + .of_match_table = of_match_ptr(unicam_of_match),
2894 + },
2895 +};
2896 +
2897 +module_platform_driver(unicam_driver);
2898 +
2899 +MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com>");
2900 +MODULE_DESCRIPTION("BCM2835 Unicam driver");
2901 +MODULE_LICENSE("GPL");
2902 +MODULE_VERSION(UNICAM_VERSION);
2903 --- /dev/null
2904 +++ b/drivers/media/platform/bcm2835/vc4-regs-unicam.h
2905 @@ -0,0 +1,253 @@
2906 +/* SPDX-License-Identifier: GPL-2.0-only */
2907 +
2908 +/*
2909 + * Copyright (C) 2017-2020 Raspberry Pi Trading.
2910 + * Dave Stevenson <dave.stevenson@raspberrypi.com>
2911 + */
2912 +
2913 +#ifndef VC4_REGS_UNICAM_H
2914 +#define VC4_REGS_UNICAM_H
2915 +
2916 +/*
2917 + * The following values are taken from files found within the code drop
2918 + * made by Broadcom for the BCM21553 Graphics Driver, predominantly in
2919 + * brcm_usrlib/dag/vmcsx/vcinclude/hardware_vc4.h.
2920 + * They have been modified to be only the register offset.
2921 + */
2922 +#define UNICAM_CTRL 0x000
2923 +#define UNICAM_STA 0x004
2924 +#define UNICAM_ANA 0x008
2925 +#define UNICAM_PRI 0x00c
2926 +#define UNICAM_CLK 0x010
2927 +#define UNICAM_CLT 0x014
2928 +#define UNICAM_DAT0 0x018
2929 +#define UNICAM_DAT1 0x01c
2930 +#define UNICAM_DAT2 0x020
2931 +#define UNICAM_DAT3 0x024
2932 +#define UNICAM_DLT 0x028
2933 +#define UNICAM_CMP0 0x02c
2934 +#define UNICAM_CMP1 0x030
2935 +#define UNICAM_CAP0 0x034
2936 +#define UNICAM_CAP1 0x038
2937 +#define UNICAM_ICTL 0x100
2938 +#define UNICAM_ISTA 0x104
2939 +#define UNICAM_IDI0 0x108
2940 +#define UNICAM_IPIPE 0x10c
2941 +#define UNICAM_IBSA0 0x110
2942 +#define UNICAM_IBEA0 0x114
2943 +#define UNICAM_IBLS 0x118
2944 +#define UNICAM_IBWP 0x11c
2945 +#define UNICAM_IHWIN 0x120
2946 +#define UNICAM_IHSTA 0x124
2947 +#define UNICAM_IVWIN 0x128
2948 +#define UNICAM_IVSTA 0x12c
2949 +#define UNICAM_ICC 0x130
2950 +#define UNICAM_ICS 0x134
2951 +#define UNICAM_IDC 0x138
2952 +#define UNICAM_IDPO 0x13c
2953 +#define UNICAM_IDCA 0x140
2954 +#define UNICAM_IDCD 0x144
2955 +#define UNICAM_IDS 0x148
2956 +#define UNICAM_DCS 0x200
2957 +#define UNICAM_DBSA0 0x204
2958 +#define UNICAM_DBEA0 0x208
2959 +#define UNICAM_DBWP 0x20c
2960 +#define UNICAM_DBCTL 0x300
2961 +#define UNICAM_IBSA1 0x304
2962 +#define UNICAM_IBEA1 0x308
2963 +#define UNICAM_IDI1 0x30c
2964 +#define UNICAM_DBSA1 0x310
2965 +#define UNICAM_DBEA1 0x314
2966 +#define UNICAM_MISC 0x400
2967 +
2968 +/*
2969 + * The following bitmasks are from the kernel released by Broadcom
2970 + * for Android - https://android.googlesource.com/kernel/bcm/
2971 + * The Rhea, Hawaii, and Java chips all contain the same VideoCore4
2972 + * Unicam block as BCM2835, as defined in eg
2973 + * arch/arm/mach-rhea/include/mach/rdb_A0/brcm_rdb_cam.h and similar.
2974 + * Values reworked to use the kernel BIT and GENMASK macros.
2975 + *
2976 + * Some of the bit mnenomics have been amended to match the datasheet.
2977 + */
2978 +/* UNICAM_CTRL Register */
2979 +#define UNICAM_CPE BIT(0)
2980 +#define UNICAM_MEM BIT(1)
2981 +#define UNICAM_CPR BIT(2)
2982 +#define UNICAM_CPM_MASK GENMASK(3, 3)
2983 +#define UNICAM_CPM_CSI2 0
2984 +#define UNICAM_CPM_CCP2 1
2985 +#define UNICAM_SOE BIT(4)
2986 +#define UNICAM_DCM_MASK GENMASK(5, 5)
2987 +#define UNICAM_DCM_STROBE 0
2988 +#define UNICAM_DCM_DATA 1
2989 +#define UNICAM_SLS BIT(6)
2990 +#define UNICAM_PFT_MASK GENMASK(11, 8)
2991 +#define UNICAM_OET_MASK GENMASK(20, 12)
2992 +
2993 +/* UNICAM_STA Register */
2994 +#define UNICAM_SYN BIT(0)
2995 +#define UNICAM_CS BIT(1)
2996 +#define UNICAM_SBE BIT(2)
2997 +#define UNICAM_PBE BIT(3)
2998 +#define UNICAM_HOE BIT(4)
2999 +#define UNICAM_PLE BIT(5)
3000 +#define UNICAM_SSC BIT(6)
3001 +#define UNICAM_CRCE BIT(7)
3002 +#define UNICAM_OES BIT(8)
3003 +#define UNICAM_IFO BIT(9)
3004 +#define UNICAM_OFO BIT(10)
3005 +#define UNICAM_BFO BIT(11)
3006 +#define UNICAM_DL BIT(12)
3007 +#define UNICAM_PS BIT(13)
3008 +#define UNICAM_IS BIT(14)
3009 +#define UNICAM_PI0 BIT(15)
3010 +#define UNICAM_PI1 BIT(16)
3011 +#define UNICAM_FSI_S BIT(17)
3012 +#define UNICAM_FEI_S BIT(18)
3013 +#define UNICAM_LCI_S BIT(19)
3014 +#define UNICAM_BUF0_RDY BIT(20)
3015 +#define UNICAM_BUF0_NO BIT(21)
3016 +#define UNICAM_BUF1_RDY BIT(22)
3017 +#define UNICAM_BUF1_NO BIT(23)
3018 +#define UNICAM_DI BIT(24)
3019 +
3020 +#define UNICAM_STA_MASK_ALL \
3021 + (UNICAM_DL + \
3022 + UNICAM_SBE + \
3023 + UNICAM_PBE + \
3024 + UNICAM_HOE + \
3025 + UNICAM_PLE + \
3026 + UNICAM_SSC + \
3027 + UNICAM_CRCE + \
3028 + UNICAM_IFO + \
3029 + UNICAM_OFO + \
3030 + UNICAM_PS + \
3031 + UNICAM_PI0 + \
3032 + UNICAM_PI1)
3033 +
3034 +/* UNICAM_ANA Register */
3035 +#define UNICAM_APD BIT(0)
3036 +#define UNICAM_BPD BIT(1)
3037 +#define UNICAM_AR BIT(2)
3038 +#define UNICAM_DDL BIT(3)
3039 +#define UNICAM_CTATADJ_MASK GENMASK(7, 4)
3040 +#define UNICAM_PTATADJ_MASK GENMASK(11, 8)
3041 +
3042 +/* UNICAM_PRI Register */
3043 +#define UNICAM_PE BIT(0)
3044 +#define UNICAM_PT_MASK GENMASK(2, 1)
3045 +#define UNICAM_NP_MASK GENMASK(7, 4)
3046 +#define UNICAM_PP_MASK GENMASK(11, 8)
3047 +#define UNICAM_BS_MASK GENMASK(15, 12)
3048 +#define UNICAM_BL_MASK GENMASK(17, 16)
3049 +
3050 +/* UNICAM_CLK Register */
3051 +#define UNICAM_CLE BIT(0)
3052 +#define UNICAM_CLPD BIT(1)
3053 +#define UNICAM_CLLPE BIT(2)
3054 +#define UNICAM_CLHSE BIT(3)
3055 +#define UNICAM_CLTRE BIT(4)
3056 +#define UNICAM_CLAC_MASK GENMASK(8, 5)
3057 +#define UNICAM_CLSTE BIT(29)
3058 +
3059 +/* UNICAM_CLT Register */
3060 +#define UNICAM_CLT1_MASK GENMASK(7, 0)
3061 +#define UNICAM_CLT2_MASK GENMASK(15, 8)
3062 +
3063 +/* UNICAM_DATn Registers */
3064 +#define UNICAM_DLE BIT(0)
3065 +#define UNICAM_DLPD BIT(1)
3066 +#define UNICAM_DLLPE BIT(2)
3067 +#define UNICAM_DLHSE BIT(3)
3068 +#define UNICAM_DLTRE BIT(4)
3069 +#define UNICAM_DLSM BIT(5)
3070 +#define UNICAM_DLFO BIT(28)
3071 +#define UNICAM_DLSTE BIT(29)
3072 +
3073 +#define UNICAM_DAT_MASK_ALL (UNICAM_DLSTE + UNICAM_DLFO)
3074 +
3075 +/* UNICAM_DLT Register */
3076 +#define UNICAM_DLT1_MASK GENMASK(7, 0)
3077 +#define UNICAM_DLT2_MASK GENMASK(15, 8)
3078 +#define UNICAM_DLT3_MASK GENMASK(23, 16)
3079 +
3080 +/* UNICAM_ICTL Register */
3081 +#define UNICAM_FSIE BIT(0)
3082 +#define UNICAM_FEIE BIT(1)
3083 +#define UNICAM_IBOB BIT(2)
3084 +#define UNICAM_FCM BIT(3)
3085 +#define UNICAM_TFC BIT(4)
3086 +#define UNICAM_LIP_MASK GENMASK(6, 5)
3087 +#define UNICAM_LCIE_MASK GENMASK(28, 16)
3088 +
3089 +/* UNICAM_IDI0/1 Register */
3090 +#define UNICAM_ID0_MASK GENMASK(7, 0)
3091 +#define UNICAM_ID1_MASK GENMASK(15, 8)
3092 +#define UNICAM_ID2_MASK GENMASK(23, 16)
3093 +#define UNICAM_ID3_MASK GENMASK(31, 24)
3094 +
3095 +/* UNICAM_ISTA Register */
3096 +#define UNICAM_FSI BIT(0)
3097 +#define UNICAM_FEI BIT(1)
3098 +#define UNICAM_LCI BIT(2)
3099 +
3100 +#define UNICAM_ISTA_MASK_ALL (UNICAM_FSI + UNICAM_FEI + UNICAM_LCI)
3101 +
3102 +/* UNICAM_IPIPE Register */
3103 +#define UNICAM_PUM_MASK GENMASK(2, 0)
3104 + /* Unpacking modes */
3105 + #define UNICAM_PUM_NONE 0
3106 + #define UNICAM_PUM_UNPACK6 1
3107 + #define UNICAM_PUM_UNPACK7 2
3108 + #define UNICAM_PUM_UNPACK8 3
3109 + #define UNICAM_PUM_UNPACK10 4
3110 + #define UNICAM_PUM_UNPACK12 5
3111 + #define UNICAM_PUM_UNPACK14 6
3112 + #define UNICAM_PUM_UNPACK16 7
3113 +#define UNICAM_DDM_MASK GENMASK(6, 3)
3114 +#define UNICAM_PPM_MASK GENMASK(9, 7)
3115 + /* Packing modes */
3116 + #define UNICAM_PPM_NONE 0
3117 + #define UNICAM_PPM_PACK8 1
3118 + #define UNICAM_PPM_PACK10 2
3119 + #define UNICAM_PPM_PACK12 3
3120 + #define UNICAM_PPM_PACK14 4
3121 + #define UNICAM_PPM_PACK16 5
3122 +#define UNICAM_DEM_MASK GENMASK(11, 10)
3123 +#define UNICAM_DEBL_MASK GENMASK(14, 12)
3124 +#define UNICAM_ICM_MASK GENMASK(16, 15)
3125 +#define UNICAM_IDM_MASK GENMASK(17, 17)
3126 +
3127 +/* UNICAM_ICC Register */
3128 +#define UNICAM_ICFL_MASK GENMASK(4, 0)
3129 +#define UNICAM_ICFH_MASK GENMASK(9, 5)
3130 +#define UNICAM_ICST_MASK GENMASK(12, 10)
3131 +#define UNICAM_ICLT_MASK GENMASK(15, 13)
3132 +#define UNICAM_ICLL_MASK GENMASK(31, 16)
3133 +
3134 +/* UNICAM_DCS Register */
3135 +#define UNICAM_DIE BIT(0)
3136 +#define UNICAM_DIM BIT(1)
3137 +#define UNICAM_DBOB BIT(3)
3138 +#define UNICAM_FDE BIT(4)
3139 +#define UNICAM_LDP BIT(5)
3140 +#define UNICAM_EDL_MASK GENMASK(15, 8)
3141 +
3142 +/* UNICAM_DBCTL Register */
3143 +#define UNICAM_DBEN BIT(0)
3144 +#define UNICAM_BUF0_IE BIT(1)
3145 +#define UNICAM_BUF1_IE BIT(2)
3146 +
3147 +/* UNICAM_CMP[0,1] register */
3148 +#define UNICAM_PCE BIT(31)
3149 +#define UNICAM_GI BIT(9)
3150 +#define UNICAM_CPH BIT(8)
3151 +#define UNICAM_PCVC_MASK GENMASK(7, 6)
3152 +#define UNICAM_PCDT_MASK GENMASK(5, 0)
3153 +
3154 +/* UNICAM_MISC register */
3155 +#define UNICAM_FL0 BIT(6)
3156 +#define UNICAM_FL1 BIT(9)
3157 +
3158 +#endif
Staging tree of Piotr Dymacz