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


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